40 #include <bio_ik/robot_info.h> 42 #include <geometric_shapes/shapes.h> 44 #include <moveit/collision_detection/collision_common.h> 45 #include <moveit/collision_detection_fcl/collision_common.h> 47 #include <bio_ik/goal.h> 55 bool ros_params_initrd;
56 std::vector<int> joint_usage;
57 std::vector<ssize_t> link_tip_indices;
58 std::vector<double> minimal_displacement_factors;
59 std::vector<double> joint_transmission_goal_temp, joint_transmission_goal_temp2;
60 moveit::core::RobotModelConstPtr robot_model;
61 const moveit::core::JointModelGroup* joint_model_group;
64 double dpos, drot, dtwist;
65 #if (MOVEIT_FCL_VERSION < FCL_VERSION_CHECK(0, 6, 0)) 68 std::vector<Vector3> vertices;
69 std::vector<fcl::Vec3f> points;
70 std::vector<int> polygons;
71 std::vector<fcl::Vec3f> plane_normals;
72 std::vector<double> plane_dis;
73 collision_detection::FCLGeometryConstPtr geometry;
75 std::vector<std::vector<size_t>> edges;
80 std::vector<std::shared_ptr<CollisionShape>> shapes;
86 std::vector<CollisionLink> collision_links;
88 size_t addTipLink(
const moveit::core::LinkModel* link_model);
129 GoalContext goal_context;
132 std::vector<double> initial_guess;
133 std::vector<size_t> active_variables;
134 std::vector<size_t> tip_link_indices;
135 std::vector<GoalInfo> goals;
136 std::vector<GoalInfo> secondary_goals;
138 void initialize(moveit::core::RobotModelConstPtr robot_model,
const moveit::core::JointModelGroup* joint_model_group,
const IKParams& params,
const std::vector<const Goal*>& goals2,
const BioIKKinematicsQueryOptions* options);
140 double computeGoalFitness(
GoalInfo& goal,
const Frame* tip_frames,
const double* active_variable_positions);
141 double computeGoalFitness(std::vector<GoalInfo>& goals,
const Frame* tip_frames,
const double* active_variable_positions);
142 bool checkSolutionActiveVariables(
const std::vector<Frame>& tip_frames,
const double* active_variable_positions);