MoveIT和KDL中进行机械臂位置和姿态插值
机械臂轨迹规划中，可以使用直线和圆弧规划，不同规划方式对应不同的计算方法。在MoveIT中，moveit_planners/pilz_industrial_motion_planner/src/下保存规划方法，trajectory_generator_lin.cpp对应线性轨迹规划，trajectory_generator_circ.cpp对应圆弧规划。trajectory_functions.cpp文件中通过generateJointTrajectory()调用KDL插值方法
- 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));
}
- 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;
}
- 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;
}