- DOF(Degree Of Freedom)。自由度。
Ros用moveit_msgs::RobotTrajectory封装发向机器人的路径控制，对当中每个路点，表示它的有关节状态(positions)、相对于起始路点的时间戳(time_from_start)等。
C:\Windows\System32>rosmsg show moveit_msgs/RobotTrajectory trajectory_msgs/JointTrajectory joint_trajectory std_msgs/Header header uint32 seq time stamp string frame_id string[] joint_names trajectory_msgs/JointTrajectoryPoint[] points float64[] positions float64[] velocities float64[] accelerations float64[] effort duration time_from_start trajectory_msgs/MultiDOFJointTrajectory multi_dof_joint_trajectory std_msgs/Header header uint32 seq time stamp string frame_id string[] joint_names trajectory_msgs/MultiDOFJointTrajectoryPoint[] points geometry_msgs/Transform[] transforms geometry_msgs/Vector3 translation float64 x float64 y float64 z geometry_msgs/Quaternion rotation float64 x float64 y float64 z float64 w geometry_msgs/Twist[] velocities geometry_msgs/Vector3 linear float64 x float64 y float64 z geometry_msgs/Vector3 angular float64 x float64 y float64 z geometry_msgs/Twist[] accelerations geometry_msgs/Vector3 linear float64 x float64 y float64 z geometry_msgs/Vector3 angular float64 x float64 y float64 z duration time_from_start
moveit_msgs::RobotTrajectory就是trajectory_msgs::JointTrajectory和trajectory_msgs::MultiDOFJointTrajector的struct，JointTrajectory发向机器人中那些单自由度关节，MultiDOFJointTrajector则是多自由度关节。
一、robot_trajectory::RobotTrajectory转成moveit_msgs::RobotTrajectory
- @trajectory[OUT]。要转换出的moveit_msgs::RobotTrajectory类型的路径轨迹。
- @joint_filter[IN]。额外的关节值筛选器。当不是空时，存储时认为有效的关节名称，像a、b、c。路径中要关节需要是它们中的一种，它会认为有效，才会加入要生成的路径。默认空。
void RobotTrajectory::getRobotTrajectoryMsg(moveit_msgs::RobotTrajectory& trajectory,
const std::vector<std::string>& joint_filter) const
{
trajectory = moveit_msgs::RobotTrajectory();
if (waypoints_.empty())
return;
const std::vector<const moveit::core::JointModel*>& jnts =
group_ ? group_->getActiveJointModels() : robot_model_->getActiveJointModels();
std::vector<const moveit::core::JointModel*> onedof;
std::vector<const moveit::core::JointModel*> mdof;
trajectory.joint_trajectory.joint_names.clear();
trajectory.multi_dof_joint_trajectory.joint_names.clear();
for (const moveit::core::JointModel* active_joint : jnts)
{
// only consider joints listed in joint_filter
if (!joint_filter.empty() &&
std::find(joint_filter.begin(), joint_filter.end(), active_joint->getName()) == joint_filter.end())
continue;
if (active_joint->getVariableCount() == 1)
{
trajectory.joint_trajectory.joint_names.push_back(active_joint->getName());
onedof.push_back(active_joint);
}
else
{
trajectory.multi_dof_joint_trajectory.joint_names.push_back(active_joint->getName());
mdof.push_back(active_joint);
}
}
if (!onedof.empty())
{
trajectory.joint_trajectory.header.frame_id = robot_model_->getModelFrame();
trajectory.joint_trajectory.header.stamp = ros::Time(0);
trajectory.joint_trajectory.points.resize(waypoints_.size());
}
if (!mdof.empty())
{
trajectory.multi_dof_joint_trajectory.header.frame_id = robot_model_->getModelFrame();
trajectory.multi_dof_joint_trajectory.header.stamp = ros::Time(0);
trajectory.multi_dof_joint_trajectory.points.resize(waypoints_.size());
}
onedof：存储着此个规划组中单自由度关节信息。示例中有3个。
mdof：存储着此此个规划组多自由度关节信息。示例中有0个。
static const ros::Duration ZERO_DURATION(0.0);
double total_time = 0.0;
for (std::size_t i = 0; i < waypoints_.size(); ++i)
{
if (duration_from_previous_.size() > i)
total_time += duration_from_previous_[i];
if (!onedof.empty())
{
trajectory.joint_trajectory.points[i].positions.resize(onedof.size());
trajectory.joint_trajectory.points[i].velocities.reserve(onedof.size());
for (std::size_t j = 0; j < onedof.size(); ++j)
{
trajectory.joint_trajectory.points[i].positions[j] =
waypoints_[i]->getVariablePosition(onedof[j]->getFirstVariableIndex());
// if we have velocities/accelerations/effort, copy those too
if (waypoints_[i]->hasVelocities())
trajectory.joint_trajectory.points[i].velocities.push_back(
waypoints_[i]->getVariableVelocity(onedof[j]->getFirstVariableIndex()));
if (waypoints_[i]->hasAccelerations())
trajectory.joint_trajectory.points[i].accelerations.push_back(
waypoints_[i]->getVariableAcceleration(onedof[j]->getFirstVariableIndex()));
if (waypoints_[i]->hasEffort())
trajectory.joint_trajectory.points[i].effort.push_back(
waypoints_[i]->getVariableEffort(onedof[j]->getFirstVariableIndex()));
}
// clear velocities if we have an incomplete specification
if (trajectory.joint_trajectory.points[i].velocities.size() != onedof.size())
trajectory.joint_trajectory.points[i].velocities.clear();
// clear accelerations if we have an incomplete specification
if (trajectory.joint_trajectory.points[i].accelerations.size() != onedof.size())
trajectory.joint_trajectory.points[i].accelerations.clear();
// clear effort if we have an incomplete specification
if (trajectory.joint_trajectory.points[i].effort.size() != onedof.size())
trajectory.joint_trajectory.points[i].effort.clear();
if (duration_from_previous_.size() > i)
trajectory.joint_trajectory.points[i].time_from_start = ros::Duration(total_time);
else
trajectory.joint_trajectory.points[i].time_from_start = ZERO_DURATION;
SDL_Log("[%i/%i]{onedof}, duration_from_previous_[%i/%i]: %.5f, total_time: %.5f, time_from_start: %.5f, positions: %s",
(int)i, (int)waypoints_.size() - 1, (int)i, (int)duration_from_previous_.size() - 1, duration_from_previous_[i], total_time,
trajectory.joint_trajectory.points[i].time_from_start.toSec(), vector_double_DebugString(trajectory.joint_trajectory.points[i].positions).c_str());为理解，增加了SDL_log，底下有对图1的一次运行时输出。
}
if (!mdof.empty())
{
trajectory.multi_dof_joint_trajectory.points[i].transforms.resize(mdof.size());
for (std::size_t j = 0; j < mdof.size(); ++j)
{
geometry_msgs::TransformStamped ts = tf2::eigenToTransform(waypoints_[i]->getJointTransform(mdof[j]));
trajectory.multi_dof_joint_trajectory.points[i].transforms[j] = ts.transform;
// TODO: currently only checking for planar multi DOF joints / need to add check for floating
if (waypoints_[i]->hasVelocities() && (mdof[j]->getType() == moveit::core::JointModel::JointType::PLANAR))
{
const std::vector<std::string> names = mdof[j]->getVariableNames();
const double* velocities = waypoints_[i]->getJointVelocities(mdof[j]);
geometry_msgs::Twist point_velocity;
for (std::size_t k = 0; k < names.size(); ++k)
{
if (names[k].find("/x") != std::string::npos)
{
point_velocity.linear.x = velocities[k];
}
else if (names[k].find("/y") != std::string::npos)
{
point_velocity.linear.y = velocities[k];
}
else if (names[k].find("/z") != std::string::npos)
{
point_velocity.linear.z = velocities[k];
}
else if (names[k].find("/theta") != std::string::npos)
{
point_velocity.angular.z = velocities[k];
}
}
trajectory.multi_dof_joint_trajectory.points[i].velocities.push_back(point_velocity);
}
}
if (duration_from_previous_.size() > i)
trajectory.multi_dof_joint_trajectory.points[i].time_from_start = ros::Duration(total_time);
else
trajectory.multi_dof_joint_trajectory.points[i].time_from_start = ZERO_DURATION;
}
}
}a、b、c三个单自由度关节，图1显示了此时相关变量值。
[0/41]{onedof}, duration_from_previous_[0/41]: 0, total_time: 0, time_from_start: 0, positions: (0, 0, 0)
[1/41]{onedof}, duration_from_previous_[1/41]: 0.02009, total_time: 0.02009, time_from_start: 0.02009, positions: (4.71548e-05, 5.65878e-05, -6.60183e-05)
[2/41]{onedof}, duration_from_previous_[2/41]: 0.18117, total_time: 0.20127, time_from_start: 0.20127, positions: (0.0128202, 0.0153849, -0.0179488)
[3/41]{onedof}, duration_from_previous_[3/41]: 0.07927, total_time: 0.28054, time_from_start: 0.28054, positions: (0.0256405, 0.0307697, -0.0358975)
[4/41]{onedof}, duration_from_previous_[4/41]: 0.06426, total_time: 0.34481, time_from_start: 0.34481, positions: (0.0384607, 0.0461546, -0.0538463)
[5/41]{onedof}, duration_from_previous_[5/41]: 0.07131, total_time: 0.41612, time_from_start: 0.41612, positions: (0.051281, 0.0615394, -0.0717951)
...
[37/41]{onedof}, duration_from_previous_[37/41]: 0.07131, total_time: 2.98264, time_from_start: 2.98264, positions: (0.461529, 0.553855, -0.646156)
[38/41]{onedof}, duration_from_previous_[38/41]: 0.06426, total_time: 3.04690, time_from_start: 3.04690, positions: (0.474349, 0.56924, -0.664105)
[39/41]{onedof}, duration_from_previous_[39/41]: 0.07927, total_time: 3.12618, time_from_start: 3.12618, positions: (0.487169, 0.584625, -0.682053)
[40/41]{onedof}, duration_from_previous_[40/41]: 0.18117, total_time: 3.30735, time_from_start: 3.30735, positions: (0.499942, 0.599953, -0.699936)
[41/41]{onedof}, duration_from_previous_[41/41]: 0.02009, total_time: 3.32745, time_from_start: 3.32745, positions: (0.499989, 0.600009, -0.700002)对生成的time_from_start，值等于total_time，
二、moveit_msgs::RobotTrajectory发向机器人
moveit四大插件之一的控制器(MoveItControllerManager)负责把moveit_msgs::RobotTrajectory发向机器人。这里以默认的控制器插件MoveItFakeControllerManager为例，它支持三种方式，先择当中的ViaPointController。
- ViaPointController。每个点有个time_from_start字段，指示该point要发送到机器人的时刻。选择这种时，时间必须已到这个时刻，才把原值为作为js发送向关节。
void ViaPointController::execTrajectory(const moveit_msgs::RobotTrajectory& t)
{
ROS_INFO("Fake execution of trajectory");
sensor_msgs::JointState js;
js.header = t.joint_trajectory.header;
js.name = t.joint_trajectory.joint_names;
// publish joint states for all intermediate via points of the trajectory
// no further interpolation
int at = 0;
ros::Time start_time = ros::Time::now();start_time是发送零路点的真实时刻，“ros::Time::now() - start_time”指现在离零路点真实过了多少时间，“理论应该相隔的时间-实际时间相隔了时间”wait_time就是希望再等待时间。
for (std::vector<trajectory_msgs::JointTrajectoryPoint>::const_iterator via = t.joint_trajectory.points.begin(),
end = t.joint_trajectory.points.end();
!cancelled() && via != end; ++via)
{
js.position = via->positions;
js.velocity = via->velocities;
js.effort = via->effort;
ros::Duration wait_time = via->time_from_start - (ros::Time::now() - start_time);
if (wait_time.toSec() > std::numeric_limits<float>::epsilon())
{
ROS_INFO("Fake execution: waiting %0.5fs for next via point, %ld remaining", wait_time.toSec(), end - via);
wait_time.sleep();
}
js.header.stamp = ros::Time::now();
ROS_INFO("[%i/%i]via->time_from_start: %.5f, pre pub_.publish(%s)",
at ++, (int)t.joint_trajectory.points.size() - 1, via->time_from_start.toSec(), vector_double_DebugString(js.position).c_str());
pub_.publish(js);
}
ROS_DEBUG("Fake execution of trajectory: done");
}执行示例后的输出日志。
[1653911981.900536500]: Fake execution of trajectory [1653911981.900794100]: [0/41]via->time_from_start: 0.00000, pre pub_.publish((0, 0, 0)) [1653911981.900984700]: Fake execution: waiting 0.01990s for next via point, 41 remaining [1653911981.921961500]: [1/41]via->time_from_start: 0.02010, pre pub_.publish((4.71548e-05, 5.65878e-05, -6.60183e-05)) [1653911981.922242000]: Fake execution: waiting 0.17981s for next via point, 40 remaining [1653911982.095091900]: scene update 2.09506 robot stamp: 2.09449 [1653911982.102515300]: [2/41]via->time_from_start: 0.20127, pre pub_.publish((0.0128202, 0.0153849, -0.0179488)) [1653911982.102970500]: Fake execution: waiting 0.07838s for next via point, 39 remaining [1653911982.183268800]: [3/41]via->time_from_start: 0.28055, pre pub_.publish((0.0256405, 0.0307697, -0.0358975)) [1653911982.183629300]: Fake execution: waiting 0.06197s for next via point, 38 remaining [1653911982.246102600]: [4/41]via->time_from_start: 0.34481, pre pub_.publish((0.0384607, 0.0461546, -0.0538463)) [1653911982.246325800]: Fake execution: waiting 0.07059s for next via point, 37 remaining [1653911982.296290700]: scene update 2.29626 robot stamp: 2.29493 [1653911982.317920100]: [5/41]via->time_from_start: 0.41613, pre pub_.publish((0.051281, 0.0615394, -0.0717951)) [1653911982.318311000]: Fake execution: waiting 0.08092s for next via point, 36 remaining ... [1653911984.813651100]: Fake execution: waiting 0.06977s for next via point, 5 remaining [1653911984.884144800]: [37/41]via->time_from_start: 2.98264, pre pub_.publish((0.461529, 0.553855, -0.646156)) [1653911984.884487100]: Fake execution: waiting 0.06320s for next via point, 4 remaining [1653911984.949513200]: [38/41]via->time_from_start: 3.04691, pre pub_.publish((0.474349, 0.56924, -0.664105)) [1653911984.949882000]: Fake execution: waiting 0.07708s for next via point, 3 remaining [1653911985.028742400]: [39/41]via->time_from_start: 3.12618, pre pub_.publish((0.487169, 0.584625, -0.682053)) [1653911985.029164100]: Fake execution: waiting 0.17897s for next via point, 2 remaining [1653911985.051992700]: scene update 5.05192 robot stamp: 4.99531 [1653911985.209224100]: [40/41]via->time_from_start: 3.30736, pre pub_.publish((0.499942, 0.599953, -0.699936)) [1653911985.209493500]: Fake execution: waiting 0.01874s for next via point, 1 remaining [1653911985.229174200]: [41/41]via->time_from_start: 3.32745, pre pub_.publish((0.499989, 0.600009, -0.700002))
按规则，最后一个路点[41]发布时刻应该约等于发布[0]路点时刻(1653911981.900794100)加上路点[41]的time_from_start(3.32745)。