40 #include <moveit/robot_model/robot_model.h> 41 #include <moveit/planning_scene/planning_scene.h> 42 #include <Eigen/Geometry.h> 43 #include <eigen_conversions/eigen_msg.h> 44 #include <boost/bind.hpp> 49 const std::string& group_name)
51 if (!kinematic_model->hasJointModelGroup(group_name))
53 ROS_ERROR_NAMED(
"kinematics_constraint_aware",
"The group %s does not exist", group_name.c_str());
67 ROS_DEBUG_NAMED(
"kinematics_constraint_aware",
"No kinematics solver instance defined for group %s",
69 bool is_solvable_group =
true;
72 const std::vector<std::string> sub_groups_names =
joint_model_group_->getSubgroupNames();
73 for (std::size_t i = 0; i < sub_groups_names.size(); ++i)
75 if (!
kinematic_model_->getJointModelGroup(sub_groups_names[i])->getSolverInstance())
77 is_solvable_group =
false;
81 if (is_solvable_group)
83 ROS_DEBUG_NAMED(
"kinematics_constraint_aware",
"Group %s is a group for which we can solve IK",
96 ROS_INFO_NAMED(
"kinematics_constraint_aware",
"No solver allocated for group %s", group_name.c_str());
109 ROS_ERROR_NAMED(
"kinematics_constraint_aware",
"This solver has not been constructed properly");
115 ROS_ERROR_NAMED(
"kinematics_constraint_aware",
"Planning scene must be allocated");
121 response.
solution_.reset(
new robot_state::RobotState(planning_scene->getCurrentState()));
124 ros::WallTime start_time = ros::WallTime::now();
132 robot_state::RobotState kinematic_state = *request.
robot_state_;
153 ROS_ERROR_NAMED(
"kinematics_constraint_aware",
"Could not find IK solver for link %s for group %s",
161 EigenSTL::vector_Affine3d goals =
164 robot_state::StateValidityCallbackFn constraint_callback_fn =
170 result = kinematic_state.getJointStateGroup(
group_name_)
171 ->setFromIK(goals, ik_link_names,
ik_attempts_, request.
timeout_.toSec(), constraint_callback_fn);
176 ik_link_names.empty() ?
180 ->setFromIK(goals[0], ik_link_names[0],
ik_attempts_, request.
timeout_.toSec(), constraint_callback_fn);
185 std::vector<double> solution_values;
186 kinematic_state.getJointStateGroup(
group_name_)->getVariableValues(solution_values);
201 robot_state::JointStateGroup* joint_state_group,
202 const std::vector<double>& joint_group_variable_values)
const 204 joint_state_group->setVariableValues(joint_group_variable_values);
209 collision_detection::CollisionRequest collision_request;
210 collision_detection::CollisionResult collision_result;
211 collision_request.group_name = request.
group_name_;
212 planning_scene->checkCollision(collision_request, collision_result, *joint_state_group->getRobotState());
213 if (collision_result.collision)
215 ROS_DEBUG_NAMED(
"kinematics_constraint_aware",
"IK solution is in collision");
224 kinematic_constraints::ConstraintEvaluationResult constraint_result;
227 if (!constraint_result.satisfied)
229 ROS_DEBUG_NAMED(
"kinematics_constraint_aware",
"IK solution violates constraints");
240 ROS_DEBUG_NAMED(
"kinematics_constraint_aware",
"IK solution violates user specified constraints");
250 const moveit_msgs::GetConstraintAwarePositionIK::Request& request,
251 moveit_msgs::GetConstraintAwarePositionIK::Response& response)
const 255 ROS_ERROR_NAMED(
"kinematics_constraint_aware",
"This solver has not been constructed properly");
261 ROS_ERROR_NAMED(
"kinematics_constraint_aware",
"Planning scene must be allocated");
270 response.error_code = kinematics_response.
error_code_;
274 bool result =
getIK(planning_scene, kinematics_request, kinematics_response);
275 response.error_code = kinematics_response.
error_code_;
276 kinematics_response.
solution_->getStateValues(response.solution.joint_state);
281 const planning_scene::PlanningSceneConstPtr& planning_scene,
282 const moveit_msgs::GetConstraintAwarePositionIK::Request& request,
288 ROS_ERROR_NAMED(
"kinematics_constraint_aware",
"This kinematics solver does not support requests for group: %s",
289 request.ik_request.group_name.c_str());
294 if (!request.ik_request.pose_stamped_vector.empty() &&
297 ROS_ERROR_NAMED(
"kinematics_constraint_aware",
298 "Number of poses in request: %d must match number of sub groups %d in this group",
304 if (!request.ik_request.ik_link_names.empty() && request.ik_request.ik_link_names.size() !=
sub_groups_names_.size())
306 ROS_ERROR_NAMED(
"kinematics_constraint_aware",
307 "Number of ik_link_names in request: " 308 "%d must match number of sub groups %d in this group or must be zero",
314 if (request.ik_request.ik_link_names.empty() && request.ik_request.ik_link_name !=
"")
315 kinematics_request.
ik_link_names_.push_back(request.ik_request.ik_link_name);
317 kinematics_request.
ik_link_names_ = request.ik_request.ik_link_names;
319 if (request.ik_request.pose_stamped_vector.empty())
324 kinematics_request.
robot_state_.reset(
new robot_state::RobotState(planning_scene->getCurrentState()));
325 kinematics_request.
robot_state_->setStateValues(request.ik_request.robot_state.joint_state);
327 new kinematic_constraints::KinematicConstraintSet(
kinematic_model_, planning_scene->getTransforms()));
328 kinematics_request.
constraints_->add(request.constraints);
329 kinematics_request.
timeout_ = request.ik_request.timeout;
330 kinematics_request.
group_name_ = request.ik_request.group_name;
333 kinematics_response.
solution_.reset(
new robot_state::RobotState(planning_scene->getCurrentState()));
339 const planning_scene::PlanningSceneConstPtr& planning_scene,
const robot_state::RobotState& kinematic_state,
340 const std::vector<geometry_msgs::PoseStamped>& poses,
const std::string& target_frame)
const 342 Eigen::Affine3d eigen_pose, eigen_pose_2;
343 EigenSTL::vector_Affine3d result(poses.size());
344 bool target_frame_is_root_frame = (target_frame == kinematic_state.getRobotModel()->getModelFrame());
345 for (std::size_t i = 0; i < poses.size(); ++i)
347 geometry_msgs::Pose pose = poses[i].pose;
348 tf::poseMsgToEigen(pose, eigen_pose_2);
349 planning_scene->getTransforms()->transformPose(kinematic_state, poses[i].header.frame_id, eigen_pose_2, eigen_pose);
350 if (!target_frame_is_root_frame)
352 eigen_pose_2 = planning_scene->getTransforms()->getTransform(kinematic_state, target_frame);
353 eigen_pose = eigen_pose_2.inverse(Eigen::Isometry) * eigen_pose;
355 result[i] = eigen_pose;
361 const planning_scene::PlanningSceneConstPtr& planning_scene,
const robot_state::RobotState& kinematic_state,
362 const geometry_msgs::Pose& pose,
const std::string& link_name,
unsigned int sub_group_index)
const 364 geometry_msgs::Pose result;
365 Eigen::Affine3d eigen_pose_in, eigen_pose_link, eigen_pose_tip;
366 std::string tip_name =
368 tf::poseMsgToEigen(pose, eigen_pose_in);
369 eigen_pose_link = planning_scene->getTransforms()->getTransform(kinematic_state, link_name);
370 eigen_pose_tip = planning_scene->getTransforms()->getTransform(kinematic_state, tip_name);
371 eigen_pose_in = eigen_pose_in * (eigen_pose_link.inverse(Eigen::Isometry) * eigen_pose_tip);
372 tf::poseEigenToMsg(eigen_pose_in, result);
robot_model::RobotModelConstPtr kinematic_model_
unsigned int ik_attempts_
robot_state::RobotStatePtr solution_
std::vector< std::string > sub_groups_names_
bool check_for_collisions_
EigenSTL::vector_Affine3d transformPoses(const planning_scene::PlanningSceneConstPtr &planning_scene, const robot_state::RobotState &kinematic_state, const std::vector< geometry_msgs::PoseStamped > &poses, const std::string &target_frame) const
std::vector< geometry_msgs::PoseStamped > pose_stamped_vector_
std::vector< std::string > ik_link_names_
moveit_msgs::MoveItErrorCodes error_code_
bool convertServiceRequest(const planning_scene::PlanningSceneConstPtr &planning_scene, const moveit_msgs::GetConstraintAwarePositionIK::Request &request, kinematics_constraint_aware::KinematicsRequest &kinematics_request, kinematics_constraint_aware::KinematicsResponse &kinematics_response) const
const robot_model::JointModelGroup * joint_model_group_
bool getIK(const planning_scene::PlanningSceneConstPtr &planning_scene, const kinematics_constraint_aware::KinematicsRequest &request, kinematics_constraint_aware::KinematicsResponse &response) const
Solve the planning problem.
bool validityCallbackFn(const planning_scene::PlanningSceneConstPtr &planning_scene, const kinematics_constraint_aware::KinematicsRequest &request, kinematics_constraint_aware::KinematicsResponse &response, robot_state::JointStateGroup *joint_state_group, const std::vector< double > &joint_group_variable_values) const
std::vector< kinematic_constraints::ConstraintEvaluationResult > constraint_eval_results_
kinematic_constraints::KinematicConstraintSetPtr constraints_
geometry_msgs::Pose getTipFramePose(const planning_scene::PlanningSceneConstPtr &planning_scene, const robot_state::RobotState &kinematic_state, const geometry_msgs::Pose &pose, const std::string &link_name, unsigned int sub_group_index) const
robot_state::StateValidityCallbackFn constraint_callback_
robot_state::RobotStatePtr robot_state_
KinematicsConstraintAware(const robot_model::RobotModelConstPtr &kinematic_model, const std::string &group_name)
Default constructor.