MoveIT和KDL中进行机械臂位置和姿态插值

机械臂轨迹规划中，可以使用直线和圆弧规划，不同规划方式对应不同的计算方法。在MoveIT中，moveit_planners/pilz_industrial_motion_planner/src/下保存规划方法，trajectory_generator_lin.cpp对应线性轨迹规划，trajectory_generator_circ.cpp对应圆弧规划。trajectory_functions.cpp文件中通过generateJointTrajectory()调用KDL插值方法

1. trajectory_generator_lin.cpp通过plan进行路径规划

void TrajectoryGeneratorLIN::plan(const planning_scene::PlanningSceneConstPtr& scene,
                                  const planning_interface::MotionPlanRequest& req, const MotionPlanInfo& plan_info,
                                  const double& sampling_time, trajectory_msgs::JointTrajectory& joint_trajectory)
{
  // create Cartesian path for lin
  std::unique_ptr<KDL::Path> path(setPathLIN(plan_info.start_pose, plan_info.goal_pose));

  // create velocity profile
  std::unique_ptr<KDL::VelocityProfile> vp(
      cartesianTrapVelocityProfile(req.max_velocity_scaling_factor, req.max_acceleration_scaling_factor, path));

  // combine path and velocity profile into Cartesian trajectory
  // with the third parameter set to false, KDL::Trajectory_Segment does not
  // take
  // the ownship of Path and Velocity Profile
  KDL::Trajectory_Segment cart_trajectory(path.get(), vp.get(), false);

  moveit_msgs::MoveItErrorCodes error_code;
  // sample the Cartesian trajectory and compute joint trajectory using inverse
  // kinematics
  if (!generateJointTrajectory(scene, planner_limits_.getJointLimitContainer(), cart_trajectory, plan_info.group_name,
                               plan_info.link_name, plan_info.start_joint_position, sampling_time, joint_trajectory,
                               error_code))
  {
    std::ostringstream os;
    os << "Failed to generate valid joint trajectory from the Cartesian path";
    throw LinTrajectoryConversionFailure(os.str(), error_code.val);
  }
}
//生成KDL线路径
std::unique_ptr<KDL::Path> TrajectoryGeneratorLIN::setPathLIN(const Eigen::Affine3d& start_pose,
                                                              const Eigen::Affine3d& goal_pose) const
{
  ROS_DEBUG("Set Cartesian path for LIN command.");

  KDL::Frame kdl_start_pose, kdl_goal_pose;
  tf2::fromMsg(tf2::toMsg(start_pose), kdl_start_pose);
  tf2::fromMsg(tf2::toMsg(goal_pose), kdl_goal_pose);
  //根据最大移动速度和旋转速度计算等效半径
  double eqradius = planner_limits_.getCartesianLimits().getMaxTranslationalVelocity() /
                    planner_limits_.getCartesianLimits().getMaxRotationalVelocity();
  /*插值器，在这个类中计算pos值,orocos_kinematics_dynamics/orocos_kdl/src/path_line.cpp
  Frame Path_Line::Pos(double s) const  {
	return Frame(orient->Pos(s*scalerot),V_base_start + V_start_end*s*scalelin );
	}
 */
  KDL::RotationalInterpolation* rot_interpo = new KDL::RotationalInterpolation_SingleAxis();

  return std::unique_ptr<KDL::Path>(new KDL::Path_Line(kdl_start_pose, kdl_goal_pose, rot_interpo, eqradius, true));
}

2. trajectory_generator_circ.cpp进行路径规划

void TrajectoryGeneratorCIRC::plan(const planning_scene::PlanningSceneConstPtr& scene,
                                   const planning_interface::MotionPlanRequest& req, const MotionPlanInfo& plan_info,
                                   const double& sampling_time, trajectory_msgs::JointTrajectory& joint_trajectory)
{
  std::unique_ptr<KDL::Path> cart_path(setPathCIRC(plan_info));
  std::unique_ptr<KDL::VelocityProfile> vel_profile(
      cartesianTrapVelocityProfile(req.max_velocity_scaling_factor, req.max_acceleration_scaling_factor, cart_path));

  // combine path and velocity profile into Cartesian trajectory
  // with the third parameter set to false, KDL::Trajectory_Segment does not
  // take
  // the ownship of Path and Velocity Profile
  KDL::Trajectory_Segment cart_trajectory(cart_path.get(), vel_profile.get(), false);

  moveit_msgs::MoveItErrorCodes error_code;
  // sample the Cartesian trajectory and compute joint trajectory using inverse
  // kinematics
  if (!generateJointTrajectory(scene, planner_limits_.getJointLimitContainer(), cart_trajectory, plan_info.group_name,
                               plan_info.link_name, plan_info.start_joint_position, sampling_time, joint_trajectory,
                               error_code))
  {
    throw CircTrajectoryConversionFailure("Failed to generate valid joint trajectory from the Cartesian path",
                                          error_code.val);
  }
}
//生成圆弧轨迹
std::unique_ptr<KDL::Path> TrajectoryGeneratorCIRC::setPathCIRC(const MotionPlanInfo& info) const
{
  ROS_DEBUG("Set Cartesian path for CIRC command.");

  KDL::Frame start_pose, goal_pose;
  tf2::fromMsg(tf2::toMsg(info.start_pose), start_pose);
  tf2::fromMsg(tf2::toMsg(info.goal_pose), goal_pose);

  const auto& eigen_path_point = info.circ_path_point.second;
  const KDL::Vector path_point{ eigen_path_point.x(), eigen_path_point.y(), eigen_path_point.z() };

  // pass the ratio of translational by rotational velocity as equivalent radius
  // to get a trajectory with rotational speed, if no (or very little)
  // translational distance
  // The KDL::Path implementation chooses the motion with the longer duration
  // (translation vs. rotation)
  // and uses eqradius as scaling factor between the distances.
  double eqradius = planner_limits_.getCartesianLimits().getMaxTranslationalVelocity() /
                    planner_limits_.getCartesianLimits().getMaxRotationalVelocity();

  try
  {
    if (info.circ_path_point.first == "center")
    {
      return PathCircleGenerator::circleFromCenter(start_pose, goal_pose, path_point, eqradius);
    }
    else  // if (info.circ_path_point.first == "interim")
    {
      return PathCircleGenerator::circleFromInterim(start_pose, goal_pose, path_point, eqradius);
    }
  }
  catch (KDL::Error_MotionPlanning_Circle_No_Plane& e)
  {
    std::ostringstream os;
    os << "Failed to create path object for circle." << e.Description();
    throw CircleNoPlane(os.str());
  }
  catch (KDL::Error_MotionPlanning_Circle_ToSmall& e)
  {
    std::ostringstream os;
    os << "Failed to create path object for circle." << e.Description();
    throw CircleToSmall(os.str());
  }
  catch (ErrorMotionPlanningCenterPointDifferentRadius& e)
  {
    std::ostringstream os;
    os << "Failed to create path object for circle." << e.Description();
    throw CenterPointDifferentRadius(os.str());
  }

  return nullptr;
}

3. trajectory_functions.cpp中通过逆运动学计算出各个关节角度、速度、、加速度、时间戳等信息

bool pilz_industrial_motion_planner::generateJointTrajectory(
    const planning_scene::PlanningSceneConstPtr& scene,
    const pilz_industrial_motion_planner::JointLimitsContainer& joint_limits, const KDL::Trajectory& trajectory,
    const std::string& group_name, const std::string& link_name,
    const std::map<std::string, double>& initial_joint_position, const double& sampling_time,
    trajectory_msgs::JointTrajectory& joint_trajectory, moveit_msgs::MoveItErrorCodes& error_code,
    bool check_self_collision)
{
  ROS_DEBUG("Generate joint trajectory from a Cartesian trajectory.");

  const moveit::core::RobotModelConstPtr& robot_model = scene->getRobotModel();
  ros::Time generation_begin = ros::Time::now();

  // generate the time samples
  const double epsilon = 10e-06;  // avoid adding the last time sample twice
  std::vector<double> time_samples;
  for (double t_sample = 0.0; t_sample < trajectory.Duration() - epsilon; t_sample += sampling_time)
  {
    time_samples.push_back(t_sample);
  }
  time_samples.push_back(trajectory.Duration());

  // sample the trajectory and solve the inverse kinematics
  Eigen::Isometry3d pose_sample;
  std::map<std::string, double> ik_solution_last, ik_solution, joint_velocity_last;
  ik_solution_last = initial_joint_position;
  for (const auto& item : ik_solution_last)
  {
    joint_velocity_last[item.first] = 0.0;
  }
  //ik_solution存储逆运算结果
  for (std::vector<double>::const_iterator time_iter = time_samples.begin(); time_iter != time_samples.end();
       ++time_iter)
  {
    tf2::fromMsg(tf2::toMsg(trajectory.Pos(*time_iter)), pose_sample);

    if (!computePoseIK(scene, group_name, link_name, pose_sample, robot_model->getModelFrame(), ik_solution_last,
                       ik_solution, check_self_collision))
    {
      ROS_ERROR("Failed to compute inverse kinematics solution for sampled "
                "Cartesian pose.");
      error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
      joint_trajectory.points.clear();
      return false;
    }

    // check the joint limits
    double duration_current_sample = sampling_time;
    // last interval can be shorter than the sampling time
    if (time_iter == (time_samples.end() - 1) && time_samples.size() > 1)
    {
      duration_current_sample = *time_iter - *(time_iter - 1);
    }
    if (time_samples.size() == 1)
    {
      duration_current_sample = *time_iter;
    }

    // skip the first sample with zero time from start for limits checking
    if (time_iter != time_samples.begin() &&
        !verifySampleJointLimits(ik_solution_last, joint_velocity_last, ik_solution, sampling_time,
                                 duration_current_sample, joint_limits))
    {
      ROS_ERROR_STREAM("Inverse kinematics solution at "
                       << *time_iter << "s violates the joint velocity/acceleration/deceleration limits.");
      error_code.val = moveit_msgs::MoveItErrorCodes::PLANNING_FAILED;
      joint_trajectory.points.clear();
      return false;
    }

    // fill the point with joint values
    trajectory_msgs::JointTrajectoryPoint point;

    // set joint names
    joint_trajectory.joint_names.clear();
    for (const auto& start_joint : initial_joint_position)
    {
      joint_trajectory.joint_names.push_back(start_joint.first);
    }

    point.time_from_start = ros::Duration(*time_iter);
    //填充各关节值
    for (const auto& joint_name : joint_trajectory.joint_names)
    {
      point.positions.push_back(ik_solution.at(joint_name));

      if (time_iter != time_samples.begin() && time_iter != time_samples.end() - 1)
      {
        double joint_velocity =
            (ik_solution.at(joint_name) - ik_solution_last.at(joint_name)) / duration_current_sample;
        point.velocities.push_back(joint_velocity);
        point.accelerations.push_back((joint_velocity - joint_velocity_last.at(joint_name)) /
                                      (duration_current_sample + sampling_time) * 2);
        joint_velocity_last[joint_name] = joint_velocity;
      }
      else
      {
        point.velocities.push_back(0.);
        point.accelerations.push_back(0.);
        joint_velocity_last[joint_name] = 0.;
      }
    }

    // update joint trajectory
    joint_trajectory.points.push_back(point);
    ik_solution_last = ik_solution;
  }

  error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
  double duration_ms = (ros::Time::now() - generation_begin).toSec() * 1000;
  ROS_DEBUG_STREAM("Generate trajectory (N-Points: " << joint_trajectory.points.size() << ") took " << duration_ms
                                                     << " ms | " << duration_ms / joint_trajectory.points.size()
                                                     << " ms per Point");

  return true;
}