41 #include <boost/bind.hpp> 48 std::vector<kinematic_constraints::JointConstraint> jc;
49 for (std::size_t i = 0; i < constr.joint_constraints.size(); ++i)
52 if (j.configure(constr.joint_constraints[i]))
56 return jc.empty() ?
false :
configure(jc);
65 ROS_ERROR_NAMED(
"constraint_samplers",
"NULL group specified for constraint sampler");
71 std::map<std::string, JointInfo> bound_data;
72 bool some_valid_constraint =
false;
73 for (std::size_t i = 0; i < jc.size(); ++i)
82 some_valid_constraint =
true;
86 std::map<std::string, JointInfo>::iterator it = bound_data.find(jc[i].getJointVariableName());
87 if (it != bound_data.end())
92 std::max(joint_bounds.
min_position_, jc[i].getDesiredJointPosition() - jc[i].getJointToleranceBelow()),
93 std::min(joint_bounds.
max_position_, jc[i].getDesiredJointPosition() + jc[i].getJointToleranceAbove()));
95 ROS_DEBUG_NAMED(
"constraint_samplers",
"Bounds for %s JointConstraint are %g %g",
100 std::stringstream cs;
103 "The constraints for joint '%s' are such that " 104 "there are no possible values for the joint: min_bound: %g, max_bound: %g. Failing.\n",
109 bound_data[jc[i].getJointVariableName()] = ji;
112 if (!some_valid_constraint)
114 ROS_WARN_NAMED(
"constraint_samplers",
"No valid joint constraints");
118 for (std::map<std::string, JointInfo>::iterator it = bound_data.begin(); it != bound_data.end(); ++it)
123 for (std::size_t i = 0; i < joints.size(); ++i)
124 if (bound_data.find(joints[i]->getName()) == bound_data.end() && joints[i]->getVariableCount() > 0 &&
125 joints[i]->getMimic() ==
nullptr)
128 const std::vector<std::string>& vars = joints[i]->getVariableNames();
131 bool all_found =
true;
132 for (std::size_t j = 0; j < vars.size(); ++j)
133 if (bound_data.find(vars[j]) == bound_data.end())
156 ROS_WARN_NAMED(
"constraint_samplers",
"JointConstraintSampler not configured, won't sample");
161 std::vector<double> v;
162 for (std::size_t i = 0; i <
unbounded_.size(); ++i)
166 for (std::size_t j = 0; j < v.size(); ++j)
171 for (std::size_t i = 0; i <
bounds_.size(); ++i)
182 return sample(state, state, max_attempts);
224 const kinematic_constraints::OrientationConstraintPtr& oc)
234 transform_ik_ =
false;
235 eef_to_ik_tip_transform_ = Eigen::Affine3d::Identity();
236 need_eef_to_ik_tip_transform_ =
false;
249 ROS_WARN_NAMED(
"constraint_samplers",
"No enabled constraints in sampling pose");
254 ik_timeout_ = jmg_->getDefaultIKTimeout();
255 if (sampling_pose_.position_constraint_ && sampling_pose_.orientation_constraint_)
256 if (sampling_pose_.position_constraint_->getLinkModel()->getName() !=
257 sampling_pose_.orientation_constraint_->getLinkModel()->getName())
260 "Position and orientation constraints need to be specified for the same link " 261 "in order to use IK-based sampling");
265 if (sampling_pose_.position_constraint_ && sampling_pose_.position_constraint_->mobileReferenceFrame())
266 frame_depends_.push_back(sampling_pose_.position_constraint_->getReferenceFrame());
267 if (sampling_pose_.orientation_constraint_ && sampling_pose_.orientation_constraint_->mobileReferenceFrame())
268 frame_depends_.push_back(sampling_pose_.orientation_constraint_->getReferenceFrame());
269 kb_ = jmg_->getSolverInstance();
272 ROS_WARN_NAMED(
"constraint_samplers",
"No solver instance in setup");
276 is_valid_ = loadIKSolver();
282 for (std::size_t p = 0; p < constr.position_constraints.size(); ++p)
283 for (std::size_t o = 0; o < constr.orientation_constraints.size(); ++o)
284 if (constr.position_constraints[p].link_name == constr.orientation_constraints[o].link_name)
286 kinematic_constraints::PositionConstraintPtr pc(
288 kinematic_constraints::OrientationConstraintPtr oc(
290 if (pc->configure(constr.position_constraints[p], scene_->getTransforms()) &&
291 oc->configure(constr.orientation_constraints[o], scene_->getTransforms()))
295 for (std::size_t p = 0; p < constr.position_constraints.size(); ++p)
297 kinematic_constraints::PositionConstraintPtr pc(
299 if (pc->configure(constr.position_constraints[p], scene_->getTransforms()))
303 for (std::size_t o = 0; o < constr.orientation_constraints.size(); ++o)
305 kinematic_constraints::OrientationConstraintPtr oc(
307 if (oc->configure(constr.orientation_constraints[o], scene_->getTransforms()))
316 if (sampling_pose_.position_constraint_)
318 const std::vector<bodies::BodyPtr>& b = sampling_pose_.position_constraint_->getConstraintRegions();
320 for (std::size_t i = 0; i < b.size(); ++i)
321 vol += b[i]->computeVolume();
326 if (sampling_pose_.orientation_constraint_)
327 v *= sampling_pose_.orientation_constraint_->getXAxisTolerance() *
328 sampling_pose_.orientation_constraint_->getYAxisTolerance() *
329 sampling_pose_.orientation_constraint_->getZAxisTolerance();
335 if (sampling_pose_.orientation_constraint_)
336 return sampling_pose_.orientation_constraint_->getLinkModel()->getName();
337 return sampling_pose_.position_constraint_->getLinkModel()->getName();
349 ik_frame_ = kb_->getBaseFrame();
351 if (!ik_frame_.empty() && ik_frame_[0] ==
'/')
352 ik_frame_.erase(ik_frame_.begin());
354 if (!jmg_->getParentModel().hasLinkModel(ik_frame_))
357 "The IK solver expects requests in frame '%s' but this frame is not known to the sampler. " 358 "Ignoring transformation (IK may fail)",
360 transform_ik_ =
false;
364 bool wrong_link =
false;
365 if (sampling_pose_.position_constraint_)
372 for (moveit::core::LinkTransformMap::const_iterator it = fixed_links.begin(); it != fixed_links.end(); ++it)
375 eef_to_ik_tip_transform_ = it->second;
376 need_eef_to_ik_tip_transform_ =
true;
383 if (!wrong_link && sampling_pose_.orientation_constraint_)
390 for (moveit::core::LinkTransformMap::const_iterator it = fixed_links.begin(); it != fixed_links.end(); ++it)
393 eef_to_ik_tip_transform_ = it->second;
394 need_eef_to_ik_tip_transform_ =
true;
404 "IK cannot be performed for link '%s'. The solver can report IK solutions for link '%s'.",
405 sampling_pose_.position_constraint_ ?
406 sampling_pose_.position_constraint_->getLinkModel()->getName().c_str() :
407 sampling_pose_.orientation_constraint_->getLinkModel()->getName().c_str(),
408 kb_->getTipFrame().c_str());
415 unsigned int max_attempts)
421 "IKConstraintSampler received dirty robot state, but valid transforms are required. " 426 if (sampling_pose_.position_constraint_)
428 const std::vector<bodies::BodyPtr>& b = sampling_pose_.position_constraint_->getConstraintRegions();
432 std::size_t k = random_number_generator_.uniformInteger(0, b.size() - 1);
433 for (std::size_t i = 0; i < b.size(); ++i)
434 if (b[(i + k) % b.size()]->samplePointInside(random_number_generator_, max_attempts, pos))
441 ROS_ERROR_NAMED(
"constraint_samplers",
"Unable to sample a point inside the constraint region");
447 ROS_ERROR_NAMED(
"constraint_samplers",
"Unable to sample a point inside the constraint region. " 448 "Constraint region is empty when it should not be.");
454 if (sampling_pose_.position_constraint_->mobileReferenceFrame())
455 pos = ks.
getFrameTransform(sampling_pose_.position_constraint_->getReferenceFrame()) * pos;
462 pos = tempState.
getGlobalLinkTransform(sampling_pose_.orientation_constraint_->getLinkModel()).translation();
465 if (sampling_pose_.orientation_constraint_)
469 2.0 * (random_number_generator_.uniform01() - 0.5) *
470 (sampling_pose_.orientation_constraint_->getXAxisTolerance() - std::numeric_limits<double>::epsilon());
472 2.0 * (random_number_generator_.uniform01() - 0.5) *
473 (sampling_pose_.orientation_constraint_->getYAxisTolerance() - std::numeric_limits<double>::epsilon());
475 2.0 * (random_number_generator_.uniform01() - 0.5) *
476 (sampling_pose_.orientation_constraint_->getZAxisTolerance() - std::numeric_limits<double>::epsilon());
477 Eigen::Affine3d
diff(Eigen::AngleAxisd(angle_x, Eigen::Vector3d::UnitX()) *
478 Eigen::AngleAxisd(angle_y, Eigen::Vector3d::UnitY()) *
479 Eigen::AngleAxisd(angle_z, Eigen::Vector3d::UnitZ()));
480 Eigen::Affine3d reqr(sampling_pose_.orientation_constraint_->getDesiredRotationMatrix() * diff.linear());
481 quat = Eigen::Quaterniond(reqr.linear());
485 if (sampling_pose_.orientation_constraint_->mobileReferenceFrame())
487 const Eigen::Affine3d& t = ks.
getFrameTransform(sampling_pose_.orientation_constraint_->getReferenceFrame());
488 Eigen::Affine3d rt(t.linear() * quat.toRotationMatrix());
489 quat = Eigen::Quaterniond(rt.linear());
496 random_number_generator_.quaternion(q);
497 quat = Eigen::Quaterniond(q[3], q[0], q[1], q[2]);
501 if (sampling_pose_.position_constraint_ && sampling_pose_.position_constraint_->hasLinkOffset())
503 pos = pos - quat.toRotationMatrix() * sampling_pose_.position_constraint_->getLinkOffset();
512 const geometry_msgs::Pose& ,
const std::vector<double>& ik_sol,
513 moveit_msgs::MoveItErrorCodes& error_code)
516 std::vector<double> solution(bij.size());
517 for (std::size_t i = 0; i < bij.size(); ++i)
518 solution[i] = ik_sol[bij[i]];
519 if (constraint(state, jmg, &solution[0]))
520 error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
527 unsigned int max_attempts)
529 return sampleHelper(state, reference_state, max_attempts,
false);
533 unsigned int max_attempts,
bool project)
537 ROS_WARN_NAMED(
"constraint_samplers",
"IKConstraintSampler not configured, won't sample");
542 if (group_state_validity_callback_)
543 adapted_ik_validity_callback =
544 boost::bind(&samplingIkCallbackFnAdapter, &state, jmg_, group_state_validity_callback_, _1, _2, _3);
546 for (
unsigned int a = 0; a < max_attempts; ++a)
549 Eigen::Vector3d point;
550 Eigen::Quaterniond quat;
551 if (!samplePose(point, quat, reference_state, max_attempts))
554 ROS_INFO_NAMED(
"constraint_samplers",
"IK constraint sampler was unable to produce a pose to run IK for");
563 Eigen::Affine3d ikq(Eigen::Translation3d(point) * quat.toRotationMatrix());
564 ikq = reference_state.
getFrameTransform(ik_frame_).inverse(Eigen::Isometry) * ikq;
565 point = ikq.translation();
566 quat = Eigen::Quaterniond(ikq.linear());
569 if (need_eef_to_ik_tip_transform_)
572 Eigen::Affine3d ikq(Eigen::Translation3d(point) * quat.toRotationMatrix());
573 ikq = ikq * eef_to_ik_tip_transform_;
574 point = ikq.translation();
575 quat = Eigen::Quaterniond(ikq.linear());
578 geometry_msgs::Pose ik_query;
579 ik_query.position.x = point.x();
580 ik_query.position.y = point.y();
581 ik_query.position.z = point.z();
582 ik_query.orientation.x = quat.x();
583 ik_query.orientation.y = quat.y();
584 ik_query.orientation.z = quat.z();
585 ik_query.orientation.w = quat.w();
587 if (callIK(ik_query, adapted_ik_validity_callback, ik_timeout_, state, project && a == 0))
595 return sampleHelper(state, state, max_attempts,
true);
601 return (!sampling_pose_.orientation_constraint_ ||
602 sampling_pose_.orientation_constraint_->decide(state, verbose_).satisfied) &&
603 (!sampling_pose_.position_constraint_ ||
604 sampling_pose_.position_constraint_->decide(state, verbose_).satisfied);
611 const std::vector<unsigned int>& ik_joint_bijection = jmg_->getKinematicsSolverJointBijection();
612 std::vector<double> seed(ik_joint_bijection.size(), 0.0);
613 std::vector<double> vals;
619 jmg_->getVariableRandomPositions(random_number_generator_, vals);
621 assert(vals.size() == ik_joint_bijection.size());
622 for (std::size_t i = 0; i < ik_joint_bijection.size(); ++i)
623 seed[i] = vals[ik_joint_bijection[i]];
625 std::vector<double> ik_sol;
626 moveit_msgs::MoveItErrorCodes error;
628 if (adapted_ik_validity_callback ?
629 kb_->searchPositionIK(ik_query, seed, timeout, ik_sol, adapted_ik_validity_callback, error) :
630 kb_->searchPositionIK(ik_query, seed, timeout, ik_sol, error))
632 assert(ik_sol.size() == ik_joint_bijection.size());
633 std::vector<double> solution(ik_joint_bijection.size());
634 for (std::size_t i = 0; i < ik_joint_bijection.size(); ++i)
635 solution[ik_joint_bijection[i]] = ik_sol[i];
643 error.val != moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE &&
645 ROS_ERROR_NAMED(
"constraint_samplers",
"IK solver failed with error %d", error.val);
const std::string & getName() const
The name of this link.
const std::string & getName() const
Get the name of the joint.
bool hasJointModel(const std::string &joint) const
Check if a joint is part of this group.
std::vector< unsigned int > uindex_
The index of the unbounded joints in the joint state vector.
A structure for potentially holding a position constraint and an orientation constraint for use durin...
#define ROS_INFO_NAMED(name,...)
int getVariableGroupIndex(const std::string &variable) const
Get the index of a variable within the group. Return -1 on error.
Class for constraints on the orientation of a link.
bool callIK(const geometry_msgs::Pose &ik_query, const kinematics::KinematicsBase::IKCallbackFn &adapted_ik_validity_callback, double timeout, robot_state::RobotState &state, bool use_as_seed)
Actually calls IK on the given pose, generating a random seed state.
#define ROS_WARN_NAMED(name,...)
const Eigen::Affine3d & getGlobalLinkTransform(const std::string &link_name)
virtual bool configure(const moveit_msgs::Constraints &constr)
Configures the IK constraint given a constraints message.
void potentiallyAdjustMinMaxBounds(double min, double max)
Function that adjusts the joints only if they are more restrictive. This means that the min limit is ...
std::vector< double > values_
Values associated with this group to avoid continuously reallocating.
bool validate(robot_state::RobotState &state) const
Class for handling single DOF joint constraints.
random_numbers::RandomNumberGenerator random_number_generator_
Random number generator used to sample.
IMETHOD Vector diff(const Vector &p_w_a, const Vector &p_w_b, double dt=1)
double getSamplingVolume() const
Gets the volume associated with the position and orientation constraints.
virtual void clear()
Clears all data from the constraint.
const Eigen::Affine3d & getFrameTransform(const std::string &id)
Get the transformation matrix from the model frame to the frame identified by id. ...
ROSCPP_DECL bool validate(const std::string &name, std::string &error)
IKSamplingPose()
Empty constructor.
virtual bool sample(robot_state::RobotState &state, const robot_state::RobotState &reference_state, unsigned int max_attempts)
Produces an IK sample.
bool loadIKSolver()
Performs checks and sets various internal values associated with the IK solver.
kinematic_constraints::OrientationConstraintPtr orientation_constraint_
Holds the orientation constraint for sampling.
virtual void clear()
Clears all data from the constraint.
static const int NO_IK_SOLUTION
Representation and evaluation of kinematic constraints.
std::map< const LinkModel *, Eigen::Affine3d, std::less< const LinkModel * >, Eigen::aligned_allocator< std::pair< const LinkModel *const, Eigen::Affine3d > > > LinkTransformMap
Map from link model instances to Eigen transforms.
virtual bool project(robot_state::RobotState &state, unsigned int max_attempts)
Project a sample given the constraints, updating the joint state group. This function allows the para...
void copyJointGroupPositions(const std::string &joint_group_name, std::vector< double > &gstate) const
For a given group, copy the position values of the variables that make up the group into another loca...
virtual void clear()
Clears all data from the constraint.
unsigned int getVariableCount() const
Get the number of variables that describe this joint group. This includes variables necessary for mim...
const std::vector< unsigned int > & getKinematicsSolverJointBijection() const
Return the mapping between the order of the joints in this group and the order of the joints in the k...
The constraint samplers namespace contains a number of methods for generating samples based on a cons...
#define ROS_DEBUG_NAMED(name,...)
bool sampleHelper(robot_state::RobotState &state, const robot_state::RobotState &reference_state, unsigned int max_attempts, bool project)
void update(bool force=false)
Update all transforms.
virtual bool configure(const moveit_msgs::Constraints &constr)
Configures a joint constraint given a Constraints message.
std::vector< JointInfo > bounds_
The bounds for any joint with bounds that are more restrictive than the joint limits.
const std::string & getLinkName() const
Gets the link name associated with this sampler.
bool dirtyLinkTransforms() const
double uniformReal(double lower_bound, double upper_bound)
boost::function< void(const geometry_msgs::Pose &ik_pose, const std::vector< double > &ik_solution, moveit_msgs::MoveItErrorCodes &error_code)> IKCallbackFn
The signature for a callback that can compute IK.
A joint from the robot. Models the transform that this joint applies in the kinematic chain...
const std::vector< const JointModel * > & getJointModels() const
Get all the joints in this group (including fixed and mimic joints).
Class for constraints on the XYZ position of a link.
const robot_model::JointModelGroup *const jmg_
Holds the joint model group associated with this constraint.
virtual bool sample(robot_state::RobotState &state, const robot_state::RobotState &ks, unsigned int max_attempts)
Samples given the constraints, populating state. This function allows the parameter max_attempts to b...
An internal structure used for maintaining constraints on a particular joint.
void setJointGroupPositions(const std::string &joint_group_name, const double *gstate)
Given positions for the variables that make up a group, in the order found in the group (including va...
void setToRandomPositions()
Set all joints to random values. Values will be within default bounds.
static const int TIMED_OUT
bool is_valid_
Holds the value for validity.
std::vector< const robot_model::JointModel * > unbounded_
The joints that are not bounded except by joint limits.
#define ROS_ERROR_NAMED(name,...)
const LinkTransformMap & getAssociatedFixedTransforms() const
Get the set of links that are attached to this one via fixed transforms.
Representation of a robot's state. This includes position, velocity, acceleration and effort...
A link from the robot. Contains the constant transform applied to the link and its geometry...
kinematic_constraints::PositionConstraintPtr position_constraint_
Holds the position constraint for sampling.
virtual bool project(robot_state::RobotState &state, unsigned int max_attempts)
Project a sample given the constraints, updating the joint state group. This function allows the para...
const VariableBounds & getVariableBounds(const std::string &variable) const
Get the bounds for a variable. Throw an exception if the variable was not found.
boost::function< bool(RobotState *robot_state, const JointModelGroup *joint_group, const double *joint_group_variable_values)> GroupStateValidityCallbackFn
Signature for functions that can verify that if the group joint_group in robot_state is set to joint_...
planning_scene::PlanningSceneConstPtr scene_
Holds the planning scene.
bool samplePose(Eigen::Vector3d &pos, Eigen::Quaterniond &quat, const robot_state::RobotState &ks, unsigned int max_attempts)
Returns a pose that falls within the constraint regions.