- 对目标状态器中的constraint_samplers::ConstraintSampler采样器对象，在位姿规划创建的派生类是IKConstraintSampler。
- PositionConstraint封装了某个link上位置(xyz字段)方面的约束。它的configure方法以moveit_msgs::PositionConstraint为参数，设置该对象的各字段。constraint_region_类型是std::vector<bodies::BodyPtr>，bodies::Body是基类，派生类像Sphere。Sphere中的center_、radiusU_分别存储着client要求的在urdf的model_frame_坐标系的位置、误差阀值。
- OrientationConstraint封装了某个link上角度（rpy）方面的约束。它的configure方法以moveit_msgs::OrientationConstraint为参数，设置该对象的各字段。desired_rotation_matrix_是client要求角度在urdf的model_frame_坐标系的旋转矩阵，像base_link。desired_rotation_matrix_inv_则是对应的逆旋转矩阵。
- IKConstraintSampler::sample(...)负责采样一个State。分三步：1）调用samplePose(...)，从(PositionConstraint, OrientationConstraint)生成一个位姿，它是client要求的位姿加上随机产生的一个误差，这个位姿属于urdf的model_frame_坐标系，记为ikq。2）把ikq变换到ik_frame_坐标系下的位姿，针对示例，就是arm_link坐标系下的位姿，记为ik_query。3）callIK以ik_query为参数，算出对应的各关节状态，当中要使用三大插件中的运行学求解器。
- 不少运动学求解器会用牛顿迭代法，像KDL、trac_ik，这时需要一个作为迭代初值的初始关节状态seed。这是通过调用getVariableRandomPositions随机产生一个不会超出各关节值范围的关节状态。
arm_joint关节坐标系arm_joint_tf。 <origin xyz="0.11787 0 0.078855" rpy="0.79764 2.5278E-15 1.5708" /> ------ -3.67321e-06 -0.698398 0.71571 0.11787 1 -2.56536e-06 2.62895e-06 0 -2.60902e-15 0.71571 0.698398 0.078855 0 0 0 1 希望设置到的arm_link坐标系位姿arm_link_pose。 xyz(-0.00028, -0.15590, 0.21609) rpy(-0.98192, -0.61523, 2.78340) ------ -0.76481 -0.644197 0.0086852 -0.00028 0.2863 -0.351918 -0.891171 -0.1559 0.577146 -0.67909 0.453584 0.21609 0 0 0 1 希望设置到的base_link坐标系位姿。base_link_pose = arm_joint_tf * arm_link_pose xyz(0.381408, -0.000279032, 0.118193), rpy(1.15452, -2.48807, 1.84256) ------ 0.213121 -0.24025 0.947027 0.381408 -0.764809 -0.644198 0.00868868 -0.000279032 0.607985 -0.726147 -0.321038 0.118193 0 0 0 1 quad: 0.73796i + -0.34048j + 0.52679k + -0.24894
一、ActionServerThread线程：生成IKConstraintSampler类型的constraint_sampler_
1.1 ConstraintSamplerManager::selectDefaultSampler
在“目标状态器”有说过，是在selectDefaultSampler创建constraint_samplers::ConstraintSampler对象，针对位姿规划，创建的派生类是IKConstraintSampler。这函数较复杂，这里只抄和位姿规划相关的。
constraint_samplers::ConstraintSamplerPtr
constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(const planning_scene::PlanningSceneConstPtr& scene,
const std::string& group_name,
const moveit_msgs::Constraints& constr)
{
const moveit::core::JointModelGroup* jmg = scene->getRobotModel()->getJointModelGroup(group_name);
...
std::vector<ConstraintSamplerPtr> samplers;
if (joint_sampler) // Start making a union of constraint samplers
samplers.push_back(joint_sampler);
// read the ik allocators, if any
const moveit::core::JointModelGroup::KinematicsSolver& ik_alloc = jmg->getGroupKinematics().first;
const moveit::core::JointModelGroup::KinematicsSolverMap& ik_subgroup_alloc = jmg->getGroupKinematics().second;
// if we have a means of computing complete states for the group using IK, then we try to see if any IK constraints
// should be used
if (ik_alloc)
{位姿规划时，ik_alloc是true。
// keep track of which links we constrained
std::map<std::string, IKConstraintSamplerPtr> used_l;
// if we have position and/or orientation constraints on links that we can perform IK for,
// we will use a sampleable goal region that employs IK to sample goals;
// if there are multiple constraints for the same link, we keep the one with the smallest
// volume for sampling
for (std::size_t p = 0; p < constr.position_constraints.size(); ++p)
for (std::size_t o = 0; o < constr.orientation_constraints.size(); ++o)
if (constr.position_constraints[p].link_name == constr.orientation_constraints[o].link_name)
{
kinematic_constraints::PositionConstraintPtr pc(
new kinematic_constraints::PositionConstraint(scene->getRobotModel()));
kinematic_constraints::OrientationConstraintPtr oc(
new kinematic_constraints::OrientationConstraint(scene->getRobotModel()));
if (pc->configure(constr.position_constraints[p], scene->getTransforms()) &&
oc->configure(constr.orientation_constraints[o], scene->getTransforms()))
{
IKConstraintSamplerPtr iks(new IKConstraintSampler(scene, jmg->getName()));
if (iks->configure(IKSamplingPose(pc, oc)))
{
bool use = true;
// Check if there already is a constraint on this link
if (used_l.find(constr.position_constraints[p].link_name) != used_l.end())
// If there is, check if the previous one has a smaller volume for sampling
if (used_l[constr.position_constraints[p].link_name]->getSamplingVolume() < iks->getSamplingVolume())
use = false; // only use new constraint if it has a smaller sampling volume
if (use)
{
// assign the link to a new constraint sampler
used_l[constr.position_constraints[p].link_name] = iks;
ROS_DEBUG_NAMED("constraint_samplers",
"Allocated an IK-based sampler for group '%s' "
"satisfying position and orientation constraints on link '%s'",
jmg->getName().c_str(), constr.position_constraints[p].link_name.c_str());
对示例，used_l只生成一条记录：used_l["grasping_frame"] = iks。
}
}
}
}
// keep track of links constrained with a full pose
std::map<std::string, IKConstraintSamplerPtr> used_l_full_pose = used_l;
for (const moveit_msgs::PositionConstraint& position_constraint : constr.position_constraints)
{
// if we are constraining this link with a full pose, we do not attempt to constrain it with a position constraint
// only
if (used_l_full_pose.find(position_constraint.link_name) != used_l_full_pose.end())
continue;对大多情况，client会给出配对的位置、角度约束，像示例。上面这个if会返回true，这个for其实没有操作。
以下是这段逻辑，是client给的约束中有“孤立”的位置约束，即该位置约束没有对应的角度约束。这时在生成的位姿约束时，强制给角度部分赋值0。
...
}
}
for (const moveit_msgs::OrientationConstraint& orientation_constraint : constr.orientation_constraints)
{
// if we are constraining this link with a full pose, we do not attempt to constrain it with an orientation
// constraint only
if (used_l_full_pose.find(orientation_constraint.link_name) != used_l_full_pose.end())
continue;对大多情况，client会给出配对的位置、角度约束，像示例。上面这个if会返回true，这个for其实没有操作。
以下是这段逻辑，是client给的约束中有“孤立”的角度约束，即该角度约束没有对象的位置约束。这时在生成的位姿约束时，强制给位置部分赋值0。
...
}
if (used_l.size() == 1)
{
if (samplers.empty())
return used_l.begin()->second;单目标，而且这个目标就是位姿类型，像示例。在这入口返回。
else
{
samplers.push_back(used_l.begin()->second);
return ConstraintSamplerPtr(new UnionConstraintSampler(scene, jmg->getName(), samplers));
}
}
...
}
...
}IKSamplingPose包含了位置和角度约束，使用在Ik采样期间。
struct IKSamplingPose
{
// 存储着位置约束数据。
kinematic_constraints::PositionConstraintPtr position_constraint_;
// 存储着角度约束数据。
kinematic_constraints::OrientationConstraintPtr orientation_constraint_;
};1.2 PositionConstraint
PositionConstraint封装了某个link上位置(xyz字段)方面的约束。位置区域(constraint_region_)被指定为由一个或多个形状（实体基元(solid primitives)或网格(meshes)）组成的受限区域块。位姿信息(constraint_region_pose_)将使用pc.constraint_region.primitive_poses进行解释。此外，还可以指定在受约束位置中指定目标偏移量(offset_)。对PositionConstraint，虚操作getType()返回POSITION_CONSTRAINT。
class PositionConstraint : public KinematicConstraint
{
protected:
Eigen::Vector3d offset_; /**< \brief The target offset */
bool has_offset_; /**< \brief Whether the offset is substantially different than 0.0 */
std::vector<bodies::BodyPtr> constraint_region_; /**< \brief The constraint region vector */
/** \brief The constraint region pose vector. All isometries are guaranteed to be valid. */
EigenSTL::vector_Isometry3d constraint_region_pose_;
bool mobile_frame_; /**< \brief Whether or not a mobile frame is employed*/
std::string constraint_frame_id_; /**< \brief The constraint frame id */
const moveit::core::LinkModel* link_model_; /**< \brief The link model constraint subject */
};PositionConstraint会存储在两个地方，一是KinematicConstraintSet中的kinematic_constraints_，以及position_constraints_。KinematicConstraintSet自身存储在ModelBasedPlanningContext中的goal_constraints_。二是IKConstraintSampler中的sampling_pose_.position_constraint_。
bool PositionConstraint::configure(const moveit_msgs::PositionConstraint& pc, const moveit::core::Transforms& tf)
{
// clearing before we configure to get rid of any old data
clear();
link_model_ = robot_model_->getLinkModel(pc.link_name);
...
offset_ = Eigen::Vector3d(pc.target_point_offset.x, pc.target_point_offset.y, pc.target_point_offset.z);
has_offset_ = offset_.squaredNorm() > std::numeric_limits<double>::epsilon();
if (tf.isFixedFrame(pc.header.frame_id))
{client指定位姿时，会同时指定位姿在哪个坐标系，如果这个坐标系在transforms_map_，isFixedFrame返回true。对base_link，要没意外会返回true。
constraint_frame_id_ = tf.getTargetFrame();
mobile_frame_ = false;
}
else
{
constraint_frame_id_ = pc.header.frame_id;
mobile_frame_ = true;
}
// load primitive shapes, first clearing any we already have
for (std::size_t i = 0; i < pc.constraint_region.primitives.size(); ++i)
{
std::unique_ptr<shapes::Shape> shape(shapes::constructShapeFromMsg(pc.constraint_region.primitives[i]));
if (shape)
{
if (pc.constraint_region.primitive_poses.size() <= i)
{
ROS_WARN_NAMED("kinematic_constraints", "Constraint region message does not contain enough primitive poses");
continue;
}
Eigen::Isometry3d t;
tf2::fromMsg(pc.constraint_region.primitive_poses[i], t);pc.constraint_region.primitive_poses[i]存储着client要求的位姿，其实有效的只有位置，角度固定是0。参考图2，position是client要求的角度，orientation是0度。tf2::fromMsg把position、orientation给成一个变化矩阵t。
ASSERT_ISOMETRY(t) // unsanitized input, could contain a non-isometry constraint_region_pose_.push_back(t); if (!mobile_frame_) tf.transformPose(pc.header.frame_id, constraint_region_pose_.back(), constraint_region_pose_.back());
把frame_id坐标系下的位姿转换到urdf的model_frame_坐标系下，第一个constraint_region_pose_.back()是作为输入，第二个是作为输出。示例中model_frame_就是base_link，因而输入值就是赋值到输出。
const bodies::BodyPtr body(bodies::createEmptyBodyFromShapeType(shape->type)); body->setDimensionsDirty(shape.get()); body->setPoseDirty(constraint_region_pose_.back()); body->updateInternalData();
setPoseDirty把constraint_region_pose_.back()赋给值成员pose_。>updateInternalData则把pose_求解出center_。
constraint_region_.push_back(body);
}
else
ROS_WARN_NAMED("kinematic_constraints", "Could not construct primitive shape %zu", i);
}
// load meshes
for (std::size_t i = 0; i < pc.constraint_region.meshes.size(); ++i)
{逻辑类似处理pc.constraint_region.primitives，由于meshes较耗资源，实际可能不大使用。
...
}
if (pc.weight <= std::numeric_limits<double>::epsilon())
{
ROS_WARN_NAMED("kinematic_constraints",
"The weight on position constraint for link '%s' is near zero. Setting to 1.0.",
pc.link_name.c_str());
constraint_weight_ = 1.0;
}
else
constraint_weight_ = pc.weight;
图2左侧是输入，moveit_msgs::PositionConstraint类型的pc。右侧是PositionConstraint配置后的结果，constraint_region_中的center_类型是Eigen::Vector3d，存储着client要求的位置。
return !constraint_region_.empty(); }
namespace bodies
该命名空间内的类允许快速判断给定点是否在对象内部。当从机器人内部移除点时（例如，当机器人看到其手臂时），此功能非常有用。在PositionConstraint，要用bodies中的Sphere(从Body派生)，调用它的samplePointInside(...)生成一个随机数，随机数限制在一个以center_为球心、radiusU_为半径的球体内。
1.3 OrientationConstraint
OrientationConstraint封装了某个link上角度（rpy）方面的约束。约束以四元数的形式指定，还有一个X、Y和Z轴的误差阀值(absolute_x/y/z_axis_tolerance_)。旋转差基于XYZ欧拉角公式（固有旋转）或作为旋转向量计算。这取决于“参数化”(Parameterization_type_)类型。对OrientationConstraint，虚操作getType()返回ORIENTATION_CONSTRAINT。
class OrientationConstraint : public KinematicConstraint
{
protected:
const moveit::core::LinkModel* link_model_; /**< \brief The target link model */
// 希望的旋转矩阵。getDesiredRotationMatrix()返回该值。
Eigen::Matrix3d desired_rotation_matrix_; /**< \brief The desired rotation matrix in the tf frame. Guaranteed to
* be valid rotation matrix. */
Eigen::Matrix3d desired_rotation_matrix_inv_; /**< \brief The inverse of the desired rotation matrix, precomputed for
* efficiency. Guaranteed to be valid rotation matrix. */
std::string desired_rotation_frame_id_; /**< \brief The target frame of the transform tree */
bool mobile_frame_; /**< \brief Whether or not the header frame is mobile or fixed */
int parameterization_type_; /**< \brief Parameterization type for orientation tolerance. */
// absolute_x/y/z_axis_tolerance_：0.001。误差。
double absolute_x_axis_tolerance_, absolute_y_axis_tolerance_,
absolute_z_axis_tolerance_; /**< \brief Storage for the tolerances */
};OrientationConstraint会存储在两个地方，一是KinematicConstraintSet中的kinematic_constraints_，以及orientation_constraints_。KinematicConstraintSet自身存储在ModelBasedPlanningContext中的goal_constraints_。二是IKConstraintSampler中的sampling_pose_.orientation_constraint_。
bool OrientationConstraint::configure(const moveit_msgs::OrientationConstraint& oc, const moveit::core::Transforms& tf)
{
// clearing out any old data
clear();
link_model_ = robot_model_->getLinkModel(oc.link_name);
...
Eigen::Quaterniond q;
tf2::fromMsg(oc.orientation, q);geometry_msgs::Quaternion类型的四元数oc.orientation转换成Eigen::Quaterniond类型的四元数。q = 0.737963i + -0.340484j + 0.526792k + -0.24894
if (fabs(q.norm() - 1.0) > 1e-3)
{
ROS_WARN_NAMED("kinematic_constraints",
"Orientation constraint for link '%s' is probably incorrect: %f, %f, %f, "
"%f. Assuming identity instead.",
oc.link_name.c_str(), oc.orientation.x, oc.orientation.y, oc.orientation.z, oc.orientation.w);
q = Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0);
}
if (oc.header.frame_id.empty())
ROS_WARN_NAMED("kinematic_constraints", "No frame specified for position constraint on link '%s'!",
oc.link_name.c_str());
if (tf.isFixedFrame(oc.header.frame_id))
{
tf.transformQuaternion(oc.header.frame_id, q, q);
desired_rotation_frame_id_ = tf.getTargetFrame();
desired_rotation_matrix_ = Eigen::Matrix3d(q);
desired_rotation_matrix_inv_ = desired_rotation_matrix_.transpose();
mobile_frame_ = false;把四元数q转换到urdf的model_frame_坐标系。oc.header.frame_id是base_link，model_frame_坐标系也是base_link，既然是同一个坐标系，q值不会变。
desired_rotation_matrix_ =
0.213121 -0.24025 0.947027
-0.764809 -0.644198 0.00868868
0.607985 -0.726147 -0.321038
q = 0.737963i + -0.340484j + 0.526792k + -0.24894
desired_rotation_matrix_就是base_link坐标系下的旋转矩阵，desired_rotation_matrix_inv_则是desired_rotation_matrix_的转置矩阵。(疑问：旋转矩阵的转置矩阵一定等于逆矩阵吗？——好像是)
desired_rotation_matrix_inv_ =
0.213121 -0.764809 0.607985
-0.24025 -0.644198 -0.726147
0.947027 0.00868868 -0.321038
}
else
{
desired_rotation_frame_id_ = oc.header.frame_id;
desired_rotation_matrix_ = Eigen::Matrix3d(q);
mobile_frame_ = true;
}
std::stringstream matrix_str;
matrix_str << desired_rotation_matrix_;
ROS_DEBUG_NAMED("kinematic_constraints", "The desired rotation matrix for link '%s' in frame %s is:\n%s",
oc.link_name.c_str(), desired_rotation_frame_id_.c_str(), matrix_str.str().c_str());
if (oc.weight <= std::numeric_limits<double>::epsilon())
{
ROS_WARN_NAMED("kinematic_constraints",
"The weight on position constraint for link '%s' is near zero. Setting to 1.0.",
oc.link_name.c_str());
constraint_weight_ = 1.0;
}
else
{
constraint_weight_ = oc.weight;
}
parameterization_type_ = oc.parameterization;后面代码：1）检查parameterization_type_有效性。2）给三个误差阀值赋值：absolute_x/y/z_axis_tolerance_
...
图3左侧是输入，moveit_msgs::OrientationConstraint类型的oc。右侧是OrientationConstraint配置后的结果，desired_rotation_matrix_是client要求角度在urdf的model_frame_坐标系的旋转矩阵，像base_link。desired_rotation_matrix_inv_则是对应的逆旋转矩阵。
return link_model_ != nullptr; }
二、goalSamplingThread线程：采样出State，添加到States_
之前已说，goalSamplingThread线程采样State可分为两个步骤。
- 私有采样器constraint_sampler_调用sample(...)采样出一个状态，放在work_state.position_。
- checkStateValidity把work_state.position_更新到new_goal。于是sample(...)生成采样值就是要放入目标状态器的状态。
针对位姿规划，私有采样器是IKConstraintSampler，第一步就是调用IKConstraintSampler::sample。
bool IKConstraintSampler::sample(moveit::core::RobotState& state, const moveit::core::RobotState& reference_state,
unsigned int max_attempts)
{
return sampleHelper(state, reference_state, max_attempts, false);
}sample逻辑很简单，直调用调用sampleHelper。
2.1 IKConstraintSampler::sampleHelper
要是成功，一次sample会采出一个State，该值会到state.position_。
bool IKConstraintSampler::sampleHelper(moveit::core::RobotState& state, const moveit::core::RobotState& reference_state,
unsigned int max_attempts, bool project)
{
...
for (unsigned int a = 0; a < max_attempts; ++a) {牛顿迭代初值是随机的，不排除这一次不能成功逆解，下一次却可以。max_attemps值是4。
// sample a point in the constraint region
Eigen::Vector3d point;
Eigen::Quaterniond quat; // quat is normalized by contract
if (!samplePose(point, quat, reference_state, max_attempts))
{samplePos采样出一个在base_link坐标系下的位姿(point+quat)。这个位姿是client希望的位姿，加上一个在误差范围内的随机值。
point = (0.381426, -0.000289836, 0.118188)。增加的位置随机值：(0.000018, -0.000010804, -0.000005)
quat = 0.738112i + -0.340157j + 0.526907k + -0.248703。增加的角度随机值：(-0.000001906, 0.0000106588, -0.000891617)
if (verbose_)
ROS_INFO_NAMED("constraint_samplers", "IK constraint sampler was unable to produce a pose to run IK for");
return false;
}
// we now have the transform we wish to perform IK for, in the planning frame
if (transform_ik_)
{
// we need to convert this transform to the frame expected by the IK solver
// both the planning frame and the frame for the IK are assumed to be robot links
Eigen::Isometry3d ikq(Eigen::Translation3d(point) * quat); // valid isometry by construction
// getFrameTransform() returns a valid isometry by contract
ikq = reference_state.getFrameTransform(ik_frame_).inverse() * ikq; // valid isometry * valid isometry
point = ikq.translation();
quat = Eigen::Quaterniond(ikq.linear()); // ikq is isometry, so quat is normalized第一次ikq。把samplePose生成的point、quat组成一个变化矩阵。
ikq =
0.213325 -0.240062 0.947028 0.381426
-0.764235 -0.64488 0.0086793 -0.000289836
0.608636 -0.725604 -0.321033 0.118188
0 0 0 1
第二次ikq。第一次ikq是在base_link坐标系下位姿，运动学求解器要求的是ik_frame_下的位姿，此时ik_frame_值是arm_link，也就说第二次ikq是arm_link坐标系下的位姿。由于reference_state.getFrameTransform(ik_frame_)得到的位姿是arm_link到base_link坐标系，所以从base_link算到arm_link，要用它的逆。
ik_frame_: arm_Link, reference_state.getFrameTransform(ik_frame_) =
-3.67321e-06 -0.698398 0.71571 0.11787
1 -2.56536e-06 2.62895e-06 0
-2.60902e-15 0.71571 0.698398 0.078855
0 0 0 1
reference_state.getFrameTransform(ik_frame_).inverse() =
-3.67321e-06 1 -2.60902e-15 4.32961e-07
-0.698398 -2.56536e-06 0.71571 0.0258828
0.71571 2.62895e-06 0.698398 -0.139433
0 0 0 1
第二次ikq。同是arm_link坐标系，它的值就是arm_link_pose加上随机偏差。
-0.764236 -0.644879 0.00867582 -0.000290804
0.286623 -0.351661 -0.891169 -0.155916
0.577747 -0.678576 0.453589 0.2161
0 0 0 1
quat = 0.182918i + -0.489639j + 0.801481k + 0.290556
此时的point是client希望的在arm_link坐标系位姿的位置部分，quat则是角度。
}
if (need_eef_to_ik_tip_transform_)
{need_eef_to_ik_tip_transform_默认false。
// After sampling the pose needs to be transformed to the ik chain tip Eigen::Isometry3d ikq(Eigen::Translation3d(point) * quat); // valid isometry by construction ikq = ikq * eef_to_ik_tip_transform_; // eef_to_ik_tip_transform_ is valid isometry (checked in loadIKSolver()) point = ikq.translation(); quat = Eigen::Quaterniond(ikq.linear()); // ikq is isometry, so quat is normalized } geometry_msgs::Pose ik_query; ik_query.position.x = point.x(); ik_query.position.y = point.y(); ik_query.position.z = point.z(); ik_query.orientation.x = quat.x(); ik_query.orientation.y = quat.y(); ik_query.orientation.z = quat.z(); ik_query.orientation.w = quat.w(); if (callIK(ik_query, adapted_ik_validity_callback, ik_timeout_, state, project && a == 0)) return true;
ik_query是client希望的在arm_link坐标系位姿。callIK以该位姿为参数，算出对应的各关节状态，当中要使用三大插件中的运行学求解器。对第5个参数，由于参数project是false，callIK得到的会总是false。
} return false; }
2.2 IKConstraintSampler::samplePose
samplePos采样出一个在base_link坐标系下的位姿(point+quat)。这个位姿是client希望的位姿，加上一个在误差范围内的随机值。算出来后，位置部分放在参数pos，角度部分放在quat。
- @pos[OUT]：存储位姿中的位置。
- @quat[OUT]：存储位姿中的旋转。
bool IKConstraintSampler::samplePose(Eigen::Vector3d& pos, Eigen::Quaterniond& quat, const moveit::core::RobotState& ks,
unsigned int max_attempts)
{
...
if (sampling_pose_.position_constraint_)
{
const std::vector<bodies::BodyPtr>& b = sampling_pose_.position_constraint_->getConstraintRegions();
if (!b.empty())
{
bool found = false;
std::size_t k = random_number_generator_.uniformInteger(0, b.size() - 1);
for (std::size_t i = 0; i < b.size(); ++i)
if (b[(i + k) % b.size()]->samplePointInside(random_number_generator_, max_attempts, pos))
{计算位置pos。这个位置是用samplePointInside生成的一个随机数，随机数限制在一个以center_为球心、radiusU_为半径的球体内。结果存放在result。
found = true;
break;
}
...
}
...
// if this constraint is with respect a mobile frame, we need to convert this rotation to the root frame of the
// model
if (sampling_pose_.position_constraint_->mobileReferenceFrame())
pos = ks.getFrameTransform(sampling_pose_.position_constraint_->getReferenceFrame()) * pos;
}
...
if (sampling_pose_.orientation_constraint_)
{
// sample a rotation matrix within the allowed bounds
double angle_x =
2.0 * (random_number_generator_.uniform01() - 0.5) *
(sampling_pose_.orientation_constraint_->getXAxisTolerance() - std::numeric_limits<double>::epsilon());
double angle_y =
2.0 * (random_number_generator_.uniform01() - 0.5) *
(sampling_pose_.orientation_constraint_->getYAxisTolerance() - std::numeric_limits<double>::epsilon());
double angle_z =
2.0 * (random_number_generator_.uniform01() - 0.5) *
(sampling_pose_.orientation_constraint_->getZAxisTolerance() - std::numeric_limits<double>::epsilon());angle_x、angle_y、angle_z是随机产生的值，值限制在范围absolute_x/y/z_axis_tolerance_。举个例子，absolute_x_axis_tolerance_是0.001，那angle_x是范围(-0.001, 0.001)中的某个值。angle_x、angle_y、angle_z：(-0.00023890645336354434, 0.00021065886132414432, -0.00089161701500396003)
Eigen::Isometry3d diff;
if (sampling_pose_.orientation_constraint_->getParameterizationType() ==
moveit_msgs::OrientationConstraint::XYZ_EULER_ANGLES)
{
diff = Eigen::Isometry3d(Eigen::AngleAxisd(angle_x, Eigen::Vector3d::UnitX()) *
Eigen::AngleAxisd(angle_y, Eigen::Vector3d::UnitY()) *
Eigen::AngleAxisd(angle_z, Eigen::Vector3d::UnitZ()));类似rpy="0.79764 2.5278E-15 1.5708"定义的“origin”，type类型是XYZ_EULER_ANGLES。生成的diff是误差角度。
}
else if (sampling_pose_.orientation_constraint_->getParameterizationType() ==
moveit_msgs::OrientationConstraint::ROTATION_VECTOR)
{
Eigen::Vector3d rotation_vector(angle_x, angle_y, angle_z);
diff = Eigen::Isometry3d(Eigen::AngleAxisd(rotation_vector.norm(), rotation_vector.normalized()));
}
else
{
/* The parameterization type should be validated in configure, so this should never happen. */
ROS_ERROR_STREAM_NAMED("default_constraint_samplers",
"The parameterization type for the orientation constraints is invalid.");
}
// diff is isometry by construction
// getDesiredRotationMatrix() returns a valid rotation matrix by contract
// reqr has thus to be a valid isometry
Eigen::Isometry3d reqr(sampling_pose_.orientation_constraint_->getDesiredRotationMatrix() * diff.linear());
quat = Eigen::Quaterniond(reqr.linear()); // reqr is isometry, so quat has to be normalized在上面乘法中，getDesiredRotationMatrix()值是desired_rotation_matrix_，就是client设置的角度；diff.linear()返回随机误差中左上角的旋转矩阵。它们相乘等价两个角度“相加”。也就是说，reqr存储着client设置角度“加上”随机误差角度，格式是变换矩阵。reqr.linear()得到旋转矩阵，quat是以这旋转矩阵得到的四元数，也就是说quat就是以元四数表格表示的client希望角度“加上”随机误差角度。
偏差旋转矩阵diff.linear() =
1 0.000891617 1.06588e-05
-0.000891617 1 1.906e-06
-1.06571e-05 -1.9155e-06 1
(base_link坐标系)期望位姿旋转矩阵getDesiredRotationMatrix() =
0.213121 -0.24025 0.947027
-0.764809 -0.644198 0.00868868
0.607985 -0.726147 -0.321038
期望位姿+偏差旋转矩阵reqr.linear() =
0.213325 -0.240062 0.947028
-0.764235 -0.64488 0.0086793
0.608636 -0.725604 -0.321033
quat = 0.738112i + -0.340157j + 0.526907k + -0.248703
// if this constraint is with respect a mobile frame, we need to convert this rotation to the root frame of the
// model
if (sampling_pose_.orientation_constraint_->mobileReferenceFrame())
{
// getFrameTransform() returns a valid isometry by contract
const Eigen::Isometry3d& t = ks.getFrameTransform(sampling_pose_.orientation_constraint_->getReferenceFrame());
// rt is isometry by construction
Eigen::Isometry3d rt(t.linear() * quat);
quat = Eigen::Quaterniond(rt.linear()); // rt is isometry, so quat has to be normalized
}
}
...
// if there is an offset, we need to undo the induced rotation in the sampled transform origin (point)
if (sampling_pose_.position_constraint_ && sampling_pose_.position_constraint_->hasLinkOffset())
// the rotation matrix that corresponds to the desired orientation
pos = pos - quat * sampling_pose_.position_constraint_->getLinkOffset();
return true;
}2.3 Sphere::samplePointInside
samplePointInside生成一个随机数，随机数限制在一个以center_为球心、radiusU_为半径的球体内。结果存放在result。
@result[OUT]：存储着随机生成的位置。
bool bodies::Sphere::samplePointInside(random_numbers::RandomNumberGenerator& rng, unsigned int max_attempts,
Eigen::Vector3d& result) const
{
for (unsigned int i = 0; i < max_attempts; ++i)
{
const double minX = center_.x() - radiusU_;
const double maxX = center_.x() + radiusU_;
const double minY = center_.y() - radiusU_;
const double maxY = center_.y() + radiusU_;
const double minZ = center_.z() - radiusU_;
const double maxZ = center_.z() + radiusU_;
// we are sampling in a box; the probability of success after 20 attempts is 99.99996% given the ratio of box volume
// to sphere volume
for (int j = 0; j < 20; ++j)
{
result = Eigen::Vector3d(rng.uniformReal(minX, maxX), rng.uniformReal(minY, maxY), rng.uniformReal(minZ, maxZ));
if (containsPoint(result))
return true;containsPoint逻辑就是“(center_ - result).squaredNorm() <= radiusU_*radiusU_”。
} } return false; }
2.4 IKConstraintSampler::callIK
要是成功，由位姿ik_query反向求解出的关节状态会更新到state.position_。
bool IKConstraintSampler::callIK(const geometry_msgs::Pose& ik_query,
const kinematics::KinematicsBase::IKCallbackFn& adapted_ik_validity_callback,
double timeout, moveit::core::RobotState& state, bool use_as_seed)
{
const std::vector<unsigned int>& ik_joint_bijection = jmg_->getKinematicsSolverJointBijection();
std::vector<double> seed(ik_joint_bijection.size(), 0.0);
std::vector<double> vals;
if (use_as_seed)
state.copyJointGroupPositions(jmg_, vals);
else
// sample a seed value
jmg_->getVariableRandomPositions(random_number_generator_, vals);不少运动学求解器会用牛顿迭代法，像KDL、trac_ik，这时需要一个作为迭代初值的初始关节状态，vals就是这个值。由于传给sampleHelper的project固定是false，所以use_as_seed值是false，是调用getVariableRandomPositions随机产生一个关节状态，当然它会保证不会超出([-1.57, 1.57], [0, 3.14], [-1.57, 1.57])范围，像(0.27, 0.72, 0.25)。顺便说下，如果use_as_seed=true，那会把机器人目前的关节状态作为迭代法的初始状态。
assert(vals.size() == ik_joint_bijection.size()); for (std::size_t i = 0; i < ik_joint_bijection.size(); ++i) seed[i] = vals[ik_joint_bijection[i]];
ik_joint_bijection大都就是(0, 1, 2)，因而seed就是vals的原样复制。
std::vector<double> ik_sol;
moveit_msgs::MoveItErrorCodes error;
if (adapted_ik_validity_callback ?
kb_->searchPositionIK(ik_query, seed, timeout, ik_sol, adapted_ik_validity_callback, error) :
kb_->searchPositionIK(ik_query, seed, timeout, ik_sol, error))
{kb_就是运动学求解器插件，调用它的searchPositionIK由(ik_query, seed)求解出关节状态，结果存放在ik_sol。
assert(ik_sol.size() == ik_joint_bijection.size()); std::vector<double> solution(ik_joint_bijection.size()); for (std::size_t i = 0; i < ik_joint_bijection.size(); ++i) solution[ik_joint_bijection[i]] = ik_sol[i]; state.setJointGroupPositions(jmg_, solution);
ik_sol存储着由位姿求解出的关节状态，把它更新到state.position_。更新方法和关节规划时一样，参考“JointConstraintSampler::sample”，也用state.setJointGroupPositions。
return validate(state); } ... return false; }
至此一个和client要求有偏差的目标位姿被反向求解到了“同值”关节状态。和关节规划一样，目标关节状态更新到了state.position_。