- 生成轨迹中points点数至少“1+路点数”，1是机器人当前状态。此时每个路点都没有分步长。
- 笛卡儿规划要用运动学求器解，用不到路径规划器。
示例
[0/2]{p: (0.281330, -0.000289, 0.296321), M: (-0.418335i+0.258038j+-0.736527k+0.464692)}
[1/2]{p: (0.381415, -0.000289, 0.118198), M: (-0.737963i+0.340484j+-0.526792k+0.248938)}
[2/2]{p: (0.298734, -0.000288, 0.293430), M: (-0.277421i+0.271752j+-0.650418k+0.652796)}
一、MoveGroupCartesianPathService::computeService
bool MoveGroupCartesianPathService::computeService(moveit_msgs::GetCartesianPath::Request& req,
moveit_msgs::GetCartesianPath::Response& res)
{
ROS_INFO_NAMED(getName(), "Received request to compute Cartesian path");
context_->planning_scene_monitor_->updateFrameTransforms();
moveit::core::RobotState start_state =
planning_scene_monitor::LockedPlanningSceneRO(context_->planning_scene_monitor_)->getCurrentState();
moveit::core::robotStateMsgToRobotState(req.start_state, start_state);
if (const moveit::core::JointModelGroup* jmg = start_state.getJointModelGroup(req.group_name))
{
std::string link_name = req.link_name;
if (link_name.empty() && !jmg->getLinkModelNames().empty())
link_name = jmg->getLinkModelNames().back();
bool ok = true;
EigenSTL::vector_Isometry3d waypoints(req.waypoints.size());
const std::string& default_frame = context_->planning_scene_monitor_->getRobotModel()->getModelFrame();
bool no_transform = req.header.frame_id.empty() ||
moveit::core::Transforms::sameFrame(req.header.frame_id, default_frame) ||
moveit::core::Transforms::sameFrame(req.header.frame_id, link_name);
for (std::size_t i = 0; i < req.waypoints.size(); ++i)
{
if (no_transform)
tf2::fromMsg(req.waypoints[i], waypoints[i]);位姿变换[1/5]tf2::fromMsg(req.waypoints[i], waypoints[i])。位姿类型从geometry_msgs::Pose转换为Isometry3d，值不变。
waypoints[1]:{p(0.381415, -0.000289, 0.118198) quat(0.737963i+-0.340484j+0.526792k+-0.248938)}
0.21312 -0.240252 0.947026
-0.764808 -0.644199 0.00868566
0.607987 -0.726145 -0.321038
else
{
geometry_msgs::PoseStamped p;
p.header = req.header;
p.pose = req.waypoints[i];
if (performTransform(p, default_frame))
tf2::fromMsg(p.pose, waypoints[i]);
else
{
ROS_ERROR_NAMED(getName(), "Error encountered transforming waypoints to frame '%s'", default_frame.c_str());
ok = false;
break;
}
}
}
if (ok)
{
if (req.max_step < std::numeric_limits<double>::epsilon())
{
ROS_ERROR_NAMED(getName(), "Maximum step to take between consecutive configrations along Cartesian path"
"was not specified (this value needs to be > 0)");
res.error_code.val = moveit_msgs::MoveItErrorCodes::FAILURE;
}
else
{
if (!waypoints.empty())
{
moveit::core::GroupStateValidityCallbackFn constraint_fn;
std::unique_ptr<planning_scene_monitor::LockedPlanningSceneRO> ls;
std::unique_ptr<kinematic_constraints::KinematicConstraintSet> kset;
if (req.avoid_collisions || !moveit::core::isEmpty(req.path_constraints))
{
ls = std::make_unique<planning_scene_monitor::LockedPlanningSceneRO>(context_->planning_scene_monitor_);
kset = std::make_unique<kinematic_constraints::KinematicConstraintSet>((*ls)->getRobotModel());
kset->add(req.path_constraints, (*ls)->getTransforms());
constraint_fn = std::bind(
&isStateValid,
req.avoid_collisions ? static_cast<const planning_scene::PlanningSceneConstPtr&>(*ls).get() : nullptr,
kset->empty() ? nullptr : kset.get(), std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3);req.avoid_collisions一般非0，因而会进这入口。创建了指向isStateValid的函数指针constraint_fn，它的调用时机是在运动学求解器逆解一个位姿成功后。除运动学约束，isStateValid还会把逆解出的关节状态设置向start_pose。
} bool global_frame = !moveit::core::Transforms::sameFrame(link_name, req.header.frame_id); ROS_INFO_NAMED(getName(), "Attempting to follow %u waypoints for link '%s' using a step of %lf m " "and jump threshold %lf (in %s reference frame)", (unsigned int)waypoints.size(), link_name.c_str(), req.max_step, req.jump_threshold, global_frame ? "global" : "link"); std::vector<moveit::core::RobotStatePtr> traj; res.fraction = moveit::core::CartesianInterpolator::computeCartesianPath( &start_state, jmg, traj, start_state.getLinkModel(link_name), waypoints, global_frame, moveit::core::MaxEEFStep(req.max_step), moveit::core::JumpThreshold(req.jump_threshold), constraint_fn);
traj存储着所有路点的关节状态，4个。
moveit::core::robotStateToRobotStateMsg(start_state, res.start_state);
robot_trajectory::RobotTrajectory rt(context_->planning_scene_monitor_->getRobotModel(), req.group_name);
for (const moveit::core::RobotStatePtr& traj_state : traj)
rt.addSuffixWayPoint(traj_state, 0.0);
// time trajectory
// \todo optionally compute timing to move the eef with constant speed
trajectory_processing::IterativeParabolicTimeParameterization time_param;
time_param.computeTimeStamps(rt, 1.0);
rt.getRobotTrajectoryMsg(res.solution);
ROS_INFO_NAMED(getName(), "Computed Cartesian path with %u points (followed %lf%% of requested trajectory)",
(unsigned int)traj.size(), res.fraction * 100.0);
if (display_computed_paths_ && rt.getWayPointCount() > 0)
{
moveit_msgs::DisplayTrajectory disp;
disp.model_id = context_->planning_scene_monitor_->getRobotModel()->getName();
disp.trajectory.resize(1, res.solution);
moveit::core::robotStateToRobotStateMsg(rt.getFirstWayPoint(), disp.trajectory_start);
display_path_.publish(disp);
}
}
res.error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
}
}
else
res.error_code.val = moveit_msgs::MoveItErrorCodes::FRAME_TRANSFORM_FAILURE;
}
else
res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
return true;
}
二、外层CartesianInterpolator::computeCartesianPath
double CartesianInterpolator::computeCartesianPath(RobotState* start_state, const JointModelGroup* group,
std::vector<RobotStatePtr>& traj, const LinkModel* link,
const EigenSTL::vector_Isometry3d& waypoints,
bool global_reference_frame, const MaxEEFStep& max_step,
const JumpThreshold& jump_threshold,
const GroupStateValidityCallbackFn& validCallback,
const kinematics::KinematicsQueryOptions& options)
{
double percentage_solved = 0.0;
for (std::size_t i = 0; i < waypoints.size(); ++i)
{
// Don't test joint space jumps for every waypoint, test them later on the whole trajectory.
static const JumpThreshold NO_JOINT_SPACE_JUMP_TEST;
std::vector<RobotStatePtr> waypoint_traj;
double wp_percentage_solved =
computeCartesianPath(start_state, group, waypoint_traj, link, waypoints[i], global_reference_frame, max_step,
NO_JOINT_SPACE_JUMP_TEST, validCallback, options);waypoint_traj存储着此轮路点逆解出的位姿，一旦成功，最小长度是2。[0]总是start_state。
wp_percentage_solved。当前路点求解了的百分比。范围[0, 1]。
percentage_solved。累计求解了的百分比。范围[0, 1]。
if (fabs(wp_percentage_solved - 1.0) < std::numeric_limits<double>::epsilon())
{此个路点中所有位姿都逆解成功。
percentage_solved = (double)(i + 1) / (double)waypoints.size();
std::vector<RobotStatePtr>::iterator start = waypoint_traj.begin();
if (i > 0 && !waypoint_traj.empty())
std::advance(start, 1);
traj.insert(traj.end(), start, waypoint_traj.end());
}
else
{此个路点有位姿逆解失败。
percentage_solved += wp_percentage_solved / (double)waypoints.size(); std::vector<RobotStatePtr>::iterator start = waypoint_traj.begin(); if (i > 0 && !waypoint_traj.empty()) std::advance(start, 1); traj.insert(traj.end(), start, waypoint_traj.end()); break; } }
if1, wp_percentage_solved: 1.000000, percentage_solved: 0.333333, waypoint_traj.size: 2
if1, wp_percentage_solved: 1.000000, percentage_solved: 0.666666, waypoint_traj.size: 2
if1, wp_percentage_solved: 1.000000, percentage_solved: 1.000000, waypoint_traj.size: 2
percentage_solved *= checkJointSpaceJump(group, traj, jump_threshold); return percentage_solved; }
三、内层CartesianInterpolator::computeCartesianPath
double CartesianInterpolator::computeCartesianPath(RobotState* start_state, const JointModelGroup* group,
std::vector<RobotStatePtr>& traj, const LinkModel* link,
const Eigen::Isometry3d& target, bool global_reference_frame,
const MaxEEFStep& max_step, const JumpThreshold& jump_threshold,
const GroupStateValidityCallbackFn& validCallback,
const kinematics::KinematicsQueryOptions& options)
{
const std::vector<const JointModel*>& cjnt = group->getContinuousJointModels();
// make sure that continuous joints wrap
for (const JointModel* joint : cjnt)
start_state->enforceBounds(joint);
// this is the Cartesian pose we start from, and we move in the direction indicated
// getGlobalLinkTransform() returns a valid isometry by contract
Eigen::Isometry3d start_pose = start_state->getGlobalLinkTransform(link); // valid isometry第一个路点的start_pose是机器人当前位姿，后续路点的start_pose是上一个路点的位姿。
ASSERT_ISOMETRY(target) // unsanitized input, could contain a non-isometry // the target can be in the local reference frame (in which case we rotate it) Eigen::Isometry3d rotated_target = global_reference_frame ? target : start_pose * target; // valid isometry
start_pose：机器人当前正处于的位姿。start_state: {position_: -0.56,0.79,-0.45}
start_pose: {p(0.281330, -0.000289, 0.296320) quat(0.418337i+-0.258039j+0.736526k+-0.464691)}
-0.218111 0.468621 0.85605
-0.900411 -0.434954 0.00868973
0.376415 -0.768901 0.51682
target：上面由tf2::fromMsg(req.waypoints[i], waypoints[i])算出的waypoints[i]。
target: {p(0.381415, -0.000289, 0.118198) quat(0.737963i+-0.340484j+0.526792k+-0.248938)}
0.21312 -0.240252 0.947026
-0.764808 -0.644199 0.00868566
0.607987 -0.726145 -0.321038
Eigen::Isometry3d rotated_target = global_reference_frame ? target : start_pose * target;
bool global_frame = !moveit::core::Transforms::sameFrame(link_name, req.header.frame_id)。global_reference_frame表示target是否是规划组末端link坐标系。false表示是末端link坐标系，像grasping_frame。那target存储的是相对于start_pose的相对位姿，否则target存储的是绝对位姿。
位姿变换[2/5]由于global_reference_frame是true，rotated_target直接取自target。
rotated_target: {p(0.381415, -0.000289, 0.118198) quat(0.737963i+-0.340484j+0.526792k+-0.248938)}
0.21312 -0.240252 0.947026
-0.764808 -0.644199 0.00868566
0.607987 -0.726145 -0.321038
Eigen::Quaterniond start_quaternion(start_pose.linear());
Eigen::Quaterniond target_quaternion(rotated_target.linear());
if (max_step.translation <= 0.0 && max_step.rotation <= 0.0)
{
ROS_ERROR_NAMED(LOGNAME,
"Invalid MaxEEFStep passed into computeCartesianPath. Both the MaxEEFStep.rotation and "
"MaxEEFStep.translation components must be non-negative and at least one component must be "
"greater than zero");
return 0.0;
}
double rotation_distance = start_quaternion.angularDistance(target_quaternion);
double translation_distance = (rotated_target.translation() - start_pose.translation()).norm();
// decide how many steps we will need for this trajectory
std::size_t translation_steps = 0;
if (max_step.translation > 0.0)
translation_steps = floor(translation_distance / max_step.translation);
std::size_t rotation_steps = 0;
if (max_step.rotation > 0.0)
rotation_steps = floor(rotation_distance / max_step.rotation);
// If we are testing for relative jumps, we always want at least MIN_STEPS_FOR_JUMP_THRESH steps
std::size_t steps = std::max(translation_steps, rotation_steps) + 1;
if (jump_threshold.factor > 0 && steps < MIN_STEPS_FOR_JUMP_THRESH)
steps = MIN_STEPS_FOR_JUMP_THRESH;translation_steps是位置上分出的段数，rotation_steps是角度上分出的段数，它们较大值就是此个在(start_pose, 路点)这个位姿范围内要分出的段数。
// To limit absolute joint-space jumps, we pass consistency limits to the IK solver
std::vector<double> consistency_limits;
if (jump_threshold.prismatic > 0 || jump_threshold.revolute > 0)
for (const JointModel* jm : group->getActiveJointModels())
{
double limit;
switch (jm->getType())
{
case JointModel::REVOLUTE:
limit = jump_threshold.revolute;
break;
case JointModel::PRISMATIC:
limit = jump_threshold.prismatic;
break;
default:
limit = 0.0;
}
if (limit == 0.0)
limit = jm->getMaximumExtent();
consistency_limits.push_back(limit);
}
traj.clear();
traj.push_back(std::make_shared<moveit::core::RobotState>(*start_state));由路点生成的traj，[0]肯定是start_state。
double last_valid_percentage = 0.0;
for (std::size_t i = 1; i <= steps; ++i)
{
double percentage = (double)i / (double)steps;
Eigen::Isometry3d pose(start_quaternion.slerp(percentage, target_quaternion));位姿变换[3/5]pose。得到按比例角度，位置保持0。
pose: {p(0, 0, 0) quat(0.737963i+-0.340484j+0.526792k+-0.248938)}
0.21312 -0.240252 0.947026
-0.764808 -0.644199 0.00868566
0.607987 -0.726145 -0.321038
pose.translation() = percentage * rotated_target.translation() + (1 - percentage) * start_pose.translation();
位姿变换[4/5]pose。加上按比例位置，至此形成一个完整的按比例位姿。
pose: {p(0.381415, -0.000289, 0.118198) quat(0.737963i+-0.340484j+0.526792k+-0.248938)}
0.21312 -0.240252 0.947026
-0.764808 -0.644199 0.00868566
0.607987 -0.726145 -0.321038
// Explicitly use a single IK attempt only: We want a smooth trajectory. // Random seeding (of additional attempts) would probably create IK jumps. if (start_state->setFromIK(group, pose, link->getName(), consistency_limits, 0.0, validCallback, options)) traj.push_back(std::make_shared<moveit::core::RobotState>(*start_state));
setFromIK执行依次执行1）求pose的逆解。2）调用setJointGroupPositions，求出的逆解存储在start_state的position_。
traj.push_back(...)把state_state添加到traj，这同时把求出的逆解加到了traj。
percentage指是一但完成此个i位姿，那已完成的百分比。对于特例，当i==steps，percentage是100%。一量i逆解失败，那成功的就是上次的last_valid_percentage。
else break; last_valid_percentage = percentage; } last_valid_percentage *= checkJointSpaceJump(group, traj, jump_threshold); return last_valid_percentage; }
四、RobotState::setToIKSolverFrame
bool RobotState::setToIKSolverFrame(Eigen::Isometry3d& pose, const std::string& ik_frame)
{
// Bring the pose to the frame of the IK solver
if (!Transforms::sameFrame(ik_frame, robot_model_->getModelFrame()))
{
const LinkModel* link_model =
getLinkModel((!ik_frame.empty() && ik_frame[0] == '/') ? ik_frame.substr(1) : ik_frame);
if (!link_model)
{
ROS_ERROR_STREAM_NAMED(LOGNAME, "IK frame '" << ik_frame << "' does not exist.");
return false;
}
pose = getGlobalLinkTransform(link_model).inverse() * pose;右侧pose就此[4/5]时生成的pose，是base_link坐标系下的位姿。变量ik_frame值是arm_joint，getGlobalLinkTransform(link_model)返回base_link <-- arm_joint的变换矩阵。位姿变换[5/5]pose是乘法算出的arm_joint下位姿。由于是要从base_link算出arm_joint，要用变换矩阵的逆。
getGlobalLinkTransform(link_model): {p(0.117869, 0, 0.078854) quat(0.274591i+0.274592j+0.651614k+0.651611)}
-3.67321e-06 -0.698398 0.71571
1 -2.56536e-06 2.62895e-06
-2.60902e-15 0.71571 0.698398
位姿变换[5/5]pose是乘法算出的arm_joint下位姿。此个pose就是要送入运动学求解器要逆解的位姿。
pose: {p(-0.000289, -0.155900, 0.216099) quat(0.182704i+-0.489721j+0.801609k+0.290197)}
-0.764809 -0.644198 0.00868218
0.286302 -0.351916 -0.891171
0.577147 -0.67909 0.453584
} return true; }