38#include <rclcpp/logger.hpp>
39#include <rclcpp/logging.hpp>
57 std::vector<kinematic_constraints::JointConstraint> jc;
58 for (
const moveit_msgs::msg::JointConstraint& joint_constraint : constr.joint_constraints)
65 return jc.empty() ? false :
configure(jc);
74 RCLCPP_ERROR(getLogger(),
"nullptr group specified for constraint sampler");
80 std::map<std::string, JointInfo> bound_data;
81 bool some_valid_constraint =
false;
84 if (!joint_constraint.enabled())
91 some_valid_constraint =
true;
95 std::map<std::string, JointInfo>::iterator it = bound_data.find(joint_constraint.getJointVariableName());
96 if (it != bound_data.end())
106 joint_constraint.getDesiredJointPosition() - joint_constraint.getJointToleranceBelow()),
108 joint_constraint.getDesiredJointPosition() + joint_constraint.getJointToleranceAbove()));
110 RCLCPP_DEBUG(getLogger(),
"Bounds for %s JointConstraint are %g %g",
115 std::stringstream cs;
116 joint_constraint.print(cs);
117 RCLCPP_ERROR(getLogger(),
118 "The constraints for joint '%s' are such that "
119 "there are no possible values for the joint: min_bound: %g, max_bound: %g. Failing.\n",
124 bound_data[joint_constraint.getJointVariableName()] = ji;
127 if (!some_valid_constraint)
129 RCLCPP_WARN(getLogger(),
"No valid joint constraints");
133 for (std::pair<const std::string, JointInfo>& it : bound_data)
140 if (bound_data.find(joint->getName()) == bound_data.end() && joint->getVariableCount() > 0 &&
141 joint->getMimic() ==
nullptr)
144 const std::vector<std::string>& vars = joint->getVariableNames();
147 bool all_found =
true;
148 for (
const std::string& var : vars)
150 if (bound_data.find(var) == bound_data.end())
175 RCLCPP_WARN(getLogger(),
"JointConstraintSampler not configured, won't sample");
180 std::vector<double> v;
181 for (std::size_t i = 0; i <
unbounded_.size(); ++i)
185 for (std::size_t j = 0; j < v.size(); ++j)
238 const kinematic_constraints::OrientationConstraintPtr& oc)
239 : position_constraint_(pc), orientation_constraint_(oc)
263 RCLCPP_WARN(getLogger(),
"No enabled constraints in sampling pose");
274 RCLCPP_ERROR(getLogger(),
"Position and orientation constraints need to be specified for the same link "
275 "in order to use IK-based sampling");
287 RCLCPP_WARN(getLogger(),
"No solver instance in setup");
297 for (std::size_t p = 0; p < constr.position_constraints.size(); ++p)
299 for (std::size_t o = 0; o < constr.orientation_constraints.size(); ++o)
301 if (constr.position_constraints[p].link_name == constr.orientation_constraints[o].link_name)
303 kinematic_constraints::PositionConstraintPtr pc(
305 kinematic_constraints::OrientationConstraintPtr oc(
307 if (pc->configure(constr.position_constraints[p],
scene_->getTransforms()) &&
308 oc->configure(constr.orientation_constraints[o],
scene_->getTransforms()))
314 for (
const moveit_msgs::msg::PositionConstraint& position_constraint : constr.position_constraints)
316 kinematic_constraints::PositionConstraintPtr pc(
318 if (pc->configure(position_constraint,
scene_->getTransforms()))
322 for (
const moveit_msgs::msg::OrientationConstraint& orientation_constraint : constr.orientation_constraints)
324 kinematic_constraints::OrientationConstraintPtr oc(
326 if (oc->configure(orientation_constraint,
scene_->getTransforms()))
337 const std::vector<bodies::BodyPtr>& constraint_regions =
340 for (
const bodies::BodyPtr& constraint_region : constraint_regions)
341 vol += constraint_region->computeVolume();
342 if (!constraint_regions.empty())
366 RCLCPP_ERROR(getLogger(),
"No IK solver");
379 RCLCPP_ERROR(getLogger(),
380 "The IK solver expects requests in frame '%s' but this frame is not known to the sampler. "
381 "Ignoring transformation (IK may fail)",
388 bool wrong_link =
false;
396 for (
const std::pair<const moveit::core::LinkModel* const, Eigen::Isometry3d>& fixed_link : fixed_links)
416 for (
const std::pair<const moveit::core::LinkModel* const, Eigen::Isometry3d>& fixed_link : fixed_links)
431 RCLCPP_ERROR(getLogger(),
"IK cannot be performed for link '%s'. The solver can report IK solutions for link '%s'.",
435 kb_->getTipFrame().c_str());
442 unsigned int max_attempts)
447 RCLCPP_ERROR(getLogger(),
"IKConstraintSampler received dirty robot state, but valid transforms are required. "
459 for (std::size_t i = 0; i < b.size(); ++i)
469 RCLCPP_ERROR(getLogger(),
"Unable to sample a point inside the constraint region");
475 RCLCPP_ERROR(getLogger(),
"Unable to sample a point inside the constraint region. "
476 "Constraint region is empty when it should not be.");
506 Eigen::Isometry3d diff;
508 moveit_msgs::msg::OrientationConstraint::XYZ_EULER_ANGLES)
510 diff = Eigen::Isometry3d(Eigen::AngleAxisd(angle_x, Eigen::Vector3d::UnitX()) *
511 Eigen::AngleAxisd(angle_y, Eigen::Vector3d::UnitY()) *
512 Eigen::AngleAxisd(angle_z, Eigen::Vector3d::UnitZ()));
515 moveit_msgs::msg::OrientationConstraint::ROTATION_VECTOR)
517 Eigen::Vector3d rotation_vector(angle_x, angle_y, angle_z);
518 diff = Eigen::Isometry3d(Eigen::AngleAxisd(rotation_vector.norm(), rotation_vector.normalized()));
523 RCLCPP_ERROR(getLogger(),
"The parameterization type for the orientation constraints is invalid.");
529 quat = Eigen::Quaterniond(reqr.linear());
538 Eigen::Isometry3d rt(t.linear() * quat);
539 quat = Eigen::Quaterniond(rt.linear());
547 quat = Eigen::Quaterniond(q[3], q[0], q[1], q[2]);
564 const std::vector<double>& ik_sol, moveit_msgs::msg::MoveItErrorCodes& error_code)
567 std::vector<double> solution(bij.size());
568 for (std::size_t i = 0; i < bij.size(); ++i)
569 solution[i] = ik_sol[bij[i]];
570 if (constraint(state, jmg, &solution[0]))
572 error_code.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS;
576 error_code.val = moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION;
582 unsigned int max_attempts)
584 return sampleHelper(state, reference_state, max_attempts);
588 unsigned int max_attempts)
592 RCLCPP_WARN(getLogger(),
"IKConstraintSampler not configured, won't sample");
599 adapted_ik_validity_callback = [
this, state_ptr = &state](
const geometry_msgs::msg::Pose&,
600 const std::vector<double>& joints,
601 moveit_msgs::msg::MoveItErrorCodes& error_code) {
606 for (
unsigned int a = 0; a < max_attempts; ++a)
609 Eigen::Vector3d point;
610 Eigen::Quaterniond quat;
611 if (!
samplePose(point, quat, reference_state, max_attempts))
614 RCLCPP_INFO(getLogger(),
"IK constraint sampler was unable to produce a pose to run IK for");
623 Eigen::Isometry3d ikq(Eigen::Translation3d(point) * quat);
626 point = ikq.translation();
627 quat = Eigen::Quaterniond(ikq.linear());
633 Eigen::Isometry3d ikq(Eigen::Translation3d(point) * quat);
635 point = ikq.translation();
636 quat = Eigen::Quaterniond(ikq.linear());
639 geometry_msgs::msg::Pose ik_query;
640 ik_query.position.x = point.x();
641 ik_query.position.y = point.y();
642 ik_query.position.z = point.z();
643 ik_query.orientation.x = quat.x();
644 ik_query.orientation.y = quat.y();
645 ik_query.orientation.z = quat.z();
646 ik_query.orientation.w = quat.w();
668 std::vector<double> seed(ik_joint_bijection.size(), 0.0);
669 std::vector<double> vals;
681 assert(vals.size() == ik_joint_bijection.size());
682 for (std::size_t i = 0; i < ik_joint_bijection.size(); ++i)
683 seed[i] = vals[ik_joint_bijection[i]];
685 std::vector<double> ik_sol;
686 moveit_msgs::msg::MoveItErrorCodes error;
688 if (adapted_ik_validity_callback ?
689 kb_->searchPositionIK(ik_query, seed, timeout, ik_sol, adapted_ik_validity_callback, error) :
690 kb_->searchPositionIK(ik_query, seed, timeout, ik_sol, error))
692 assert(ik_sol.size() == ik_joint_bijection.size());
693 std::vector<double> solution(ik_joint_bijection.size());
694 for (std::size_t i = 0; i < ik_joint_bijection.size(); ++i)
695 solution[ik_joint_bijection[i]] = ik_sol[i];
702 if (error.val != moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION &&
703 error.val != moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE &&
704 error.val != moveit_msgs::msg::MoveItErrorCodes::TIMED_OUT)
706 RCLCPP_ERROR(getLogger(),
"IK solver failed with error %d", error.val);
710 RCLCPP_INFO(getLogger(),
"IK failed");
bool is_valid_
Holds the value for validity.
std::vector< std::string > frame_depends_
Holds the set of frames that must exist in the reference state to allow samples to be drawn.
const moveit::core::JointModelGroup *const jmg_
Holds the joint model group associated with this constraint.
virtual void clear()
Clears all data from the constraint.
moveit::core::GroupStateValidityCallbackFn group_state_validity_callback_
Holds the callback for state validity.
bool verbose_
True if verbosity is on.
planning_scene::PlanningSceneConstPtr scene_
Holds the planning scene.
bool configure(const moveit_msgs::msg::Constraints &constr) override
Configures the IK constraint given a constraints message.
double ik_timeout_
Holds the timeout associated with IK.
bool loadIKSolver()
Performs checks and sets various internal values associated with the IK solver.
const std::string & getLinkName() const
Gets the link name associated with this sampler.
bool validate(moveit::core::RobotState &state) const
IKSamplingPose sampling_pose_
Holder for the pose used for sampling.
bool callIK(const geometry_msgs::msg::Pose &ik_query, const kinematics::KinematicsBase::IKCallbackFn &adapted_ik_validity_callback, double timeout, moveit::core::RobotState &state, bool use_as_seed)
Actually calls IK on the given pose, generating a random seed state.
double getSamplingVolume() const
Gets the volume associated with the position and orientation constraints.
Eigen::Isometry3d eef_to_ik_tip_transform_
Holds the transformation from end effector to IK tip frame.
bool need_eef_to_ik_tip_transform_
True if the tip frame of the inverse kinematic is different than the frame of the end effector.
kinematics::KinematicsBaseConstPtr kb_
Holds the kinematics solver.
void clear() override
Clears all data from the constraint.
bool sampleHelper(moveit::core::RobotState &state, const moveit::core::RobotState &reference_state, unsigned int max_attempts)
std::string ik_frame_
Holds the base from of the IK solver.
random_numbers::RandomNumberGenerator random_number_generator_
Random generator used by the sampler.
bool transform_ik_
True if the frame associated with the kinematic model is different than the base frame of the IK solv...
bool sample(moveit::core::RobotState &state, const moveit::core::RobotState &reference_state, unsigned int max_attempts) override
Produces an IK sample.
bool samplePose(Eigen::Vector3d &pos, Eigen::Quaterniond &quat, const moveit::core::RobotState &ks, unsigned int max_attempts)
Returns a pose that falls within the constraint regions.
std::vector< unsigned int > uindex_
The index of the unbounded joints in the joint state vector.
void clear() override
Clears all data from the constraint.
random_numbers::RandomNumberGenerator random_number_generator_
Random number generator used to sample.
std::vector< const moveit::core::JointModel * > unbounded_
The joints that are not bounded except by joint limits.
bool configure(const moveit_msgs::msg::Constraints &constr) override
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.
bool sample(moveit::core::RobotState &state, const moveit::core::RobotState &ks, unsigned int max_attempts) override
Samples given the constraints, populating state. This function allows the parameter max_attempts to b...
std::vector< double > values_
Values associated with this group to avoid continuously reallocating.
Class for handling single DOF joint constraints.
bool configure(const moveit_msgs::msg::JointConstraint &jc)
Configure the constraint based on a moveit_msgs::msg::JointConstraint.
Class for constraints on the orientation of a link.
Class for constraints on the XYZ position of a link.
std::function< void(const geometry_msgs::msg::Pose &, const std::vector< double > &, moveit_msgs::msg::MoveItErrorCodes &)> IKCallbackFn
Signature for a callback to validate an IK solution. Typically used for collision checking.
const RobotModel & getParentModel() const
Get the kinematic model this group is part of.
double getDefaultIKTimeout() const
Get the default IK timeout.
void getVariableRandomPositions(random_numbers::RandomNumberGenerator &rng, double *values) const
Compute random values for the state of the joint group.
const kinematics::KinematicsBaseConstPtr getSolverInstance() const
bool hasJointModel(const std::string &joint) const
Check if a joint is part of this group.
const std::vector< size_t > & getKinematicsSolverJointBijection() const
Return the mapping between the order of the joints in this group and the order of the joints in the k...
unsigned int getVariableCount() const
Get the number of variables that describe this joint group. This includes variables necessary for mim...
int getVariableGroupIndex(const std::string &variable) const
Get the index of a variable within the group. Return -1 on error.
const std::vector< const JointModel * > & getJointModels() const
Get all the joints in this group (including fixed and mimic joints).
A joint from the robot. Models the transform that this joint applies in the kinematic chain....
const std::string & getName() const
Get the name of the joint.
const VariableBounds & getVariableBounds(const std::string &variable) const
Get the bounds for a variable. Throw an exception if the variable was not found.
A link from the robot. Contains the constant transform applied to the link and its geometry.
const std::string & getName() const
The name of this link.
const LinkTransformMap & getAssociatedFixedTransforms() const
Get the set of links that are attached to this one via fixed transforms. The returned transforms are ...
bool hasLinkModel(const std::string &name) const
Check if a link exists. Return true if it does.
const std::string & getModelFrame() const
Get the frame in which the transforms for this model are computed (when using a RobotState)....
Representation of a robot's state. This includes position, velocity, acceleration and effort.
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 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...
const Eigen::Isometry3d & getFrameTransform(const std::string &frame_id, bool *frame_found=nullptr)
Get the transformation matrix from the model frame (root of model) to the frame identified by frame_i...
void setToRandomPositions()
Set all joints to random values. Values will be within default bounds.
bool dirtyLinkTransforms() const
void update(bool force=false)
Update all transforms.
const Eigen::Isometry3d & getGlobalLinkTransform(const std::string &link_name)
Get the link transform w.r.t. the root link (model frame) of the RobotModel. This is typically the ro...
rclcpp::Logger getLogger()
The constraint samplers namespace contains a number of methods for generating samples based on a cons...
Representation and evaluation of kinematic constraints.
std::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_...
std::map< const LinkModel *, Eigen::Isometry3d, std::less< const LinkModel * >, Eigen::aligned_allocator< std::pair< const LinkModel *const, Eigen::Isometry3d > > > LinkTransformMap
Map from link model instances to Eigen transforms.
rclcpp::Logger getLogger(const std::string &name)
Creates a namespaced logger.
A structure for potentially holding a position constraint and an orientation constraint for use durin...
IKSamplingPose()
Empty constructor.
kinematic_constraints::PositionConstraintPtr position_constraint_
Holds the position constraint for sampling.
kinematic_constraints::OrientationConstraintPtr orientation_constraint_
Holds the orientation constraint for sampling.
An internal structure used for maintaining constraints on a particular joint.
void potentiallyAdjustMinMaxBounds(double min, double max)
Function that adjusts the joints only if they are more restrictive. This means that the min limit is ...