使用moveit的运动规划接口不能进行速度设置的问题(2022.11.25更新解决方案)

103 阅读 0 评论 68 点赞

我是靠谱客的博主魁梧钢笔，这篇文章主要介绍使用moveit的运动规划接口不能进行速度设置的问题(2022.11.25更新解决方案)，现在分享给大家，希望可以做个参考。

问题分析

关于笛卡尔运动不能调速问题的分析：

官方源码：

planning_interface: move_group_interface.cpp Source File (ros.org)

在规划plan和移动move函数里都有：（先对目标goal进行参数配置，然后调用move_action_client_执行）

复制代码

1
2
3
4
moveit_msgs::MoveGroupGoal goal;
constructGoal(goal);
...// 对goal的一系列设置
move_action_client_->sendGoal(goal);

而执行函数execute，是对轨迹的直接执行，使用的是execute_action_client_

复制代码

1
2
3
4
5
execute(const moveit_msgs::RobotTrajectory& trajectory, bool wait){
    moveit_msgs::ExecuteTrajectoryGoal goal;
    goal.trajectory = trajectory;
    execute_action_client_->sendGoal(goal);
}

而constructGoal(goal);这个函数里是有速度和加速度的参数设置的

复制代码

1
2
3
4
5
6
void constructMotionPlanRequest(moveit_msgs::MotionPlanRequest& request){
    ...
    request.max_velocity_scaling_factor = max_velocity_scaling_factor_;
     request.max_acceleration_scaling_factor =                      max_acceleration_scaling_factor_;
    ...
}

而在笛卡尔路径规划的函数里面，req的一系列设置里面，是没有对轨迹进行速度和加速度的设置的

复制代码

1
2
3
4
5
6
7
8
9
10
double computeCartesianPath(const std::vector<geometry_msgs::Pose>& waypoints,...,moveit_msgs::RobotTrajectory& msg){
     moveit_msgs::GetCartesianPath::Request req;
     moveit_msgs::GetCartesianPath::Response res;
     ... //    req 的一系列设置
     
    cartesian_path_service_.call(req, res);
    ... //如果规划成功
    msg = res.solution;
    ...        
}

复制代码

1
2
3
4
5
6
7
8
9
10
    // req的一系列设置：
     req.group_name = opt_.group_name_;
     req.header.frame_id = getPoseReferenceFrame();
     req.header.stamp = ros::Time::now();
     req.waypoints = waypoints;
     req.max_step = step;
     req.jump_threshold = jump_threshold;
     req.path_constraints = path_constraints;
     req.avoid_collisions = avoid_collisions;
     req.link_name = getEndEffectorLink();

但很奇怪的是，在官方的指导例程里，有这样的一段：

ros-planning/moveit_tutorials at melodic-devel (github.com)

(在doc/move_group_interface/src/move_group_interface_tutorial.cpp里，大约从246开始到291行结束都是笛卡尔运动规划的例子)

复制代码

1
2
3
4
5
6
// Cartesian motions are frequently needed to be slower for actions such as approach and retreat
  // grasp motions. Here we demonstrate how to reduce the speed of the robot arm via a scaling factor
  // of the maxiumum speed of each joint. Note this is not the speed of the end effector point.
  move_group.setMaxVelocityScalingFactor(0.1);
  ...
  move_group.computeCartesianPath(waypoints,eef_step,jump_threshold,    trajectory);

很奇怪，通过setMaxVelocityScalingFactor这个函数调用之后只是设置了max_velocity_scaling_factor这个变量的值，而后续调用的computeCartesianPath这个函数并不会对设置的速度最大值进行任何调用。

由于moveit官方的笛卡尔运动规划接口中，只规划和计算了轨迹点，所以导致了程序调用的笛卡尔运动的规划不能进行调速。而关节运动、回零、直接设置目标点（即调用plan或者move进行运动规划，而不是笛卡尔路径规划）等，都可以进行调速。

解决方案：

1.对computeCartesianPath生成的轨迹进行处理，加多项式插补算法等，重新自己做轨迹

解决思路：

1. 轨迹点获取

规划后生成的轨迹是存在trajectory里面的，可以尝试把里面的东西取出来

复制代码

1
2
3
4
5
6
7
8
9
10
11
fraction = arm.computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory);

...


for(auto it : trajectory.joint_trajectory.points){
            for(auto posit : it.positions){
                std::cout << posit << ", ";
            }
            std::cout << std::endl;
        }

（其实这步也没什么用，我只是想看看这个点）

2. 分析rviz里的相关代码

在官方的rviz里面，如果勾选了笛卡尔路径规划那个选项，再拖动执行，会发现界面里设置的速度和加速度因子是有效的，（那就可以去看看源码里面是怎么调用笛卡尔空间路径规划的）

在源码文件的 moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp 里面，有这么一个函数:

复制代码

bool MotionPlanningFrame::computeCartesianPlan()
{
  rclcpp::Time start = rclcpp::Clock().now();
  // get goal pose
  moveit::core::RobotState goal = *planning_display_->getQueryGoalState();
  std::vector<geometry_msgs::msg::Pose> waypoints;
  const std::string& link_name = move_group_->getEndEffectorLink();
  const moveit::core::LinkModel* link = move_group_->getRobotModel()->getLinkModel(link_name);
  if (!link)
  {
    RCLCPP_ERROR_STREAM(LOGGER, "Failed to determine unique end-effector link: " << link_name);
    return false;
  }
  waypoints.push_back(tf2::toMsg(goal.getGlobalLinkTransform(link)));

// setup default params
  double cart_step_size = 0.01;
  double cart_jump_thresh = 0.0;
  bool avoid_collisions = true;

// compute trajectory
  moveit_msgs::msg::RobotTrajectory trajectory;
  double fraction =
      move_group_->computeCartesianPath(waypoints, cart_step_size, cart_jump_thresh, trajectory, avoid_collisions);

if (fraction >= 1.0)
  {
    RCLCPP_INFO(LOGGER, "Achieved %f %% of Cartesian path", fraction * 100.);

// Compute time parameterization to also provide velocities
    // https://groups.google.com/forum/#!topic/moveit-users/MOoFxy2exT4
    robot_trajectory::RobotTrajectory rt(move_group_->getRobotModel(), move_group_->getName());
    rt.setRobotTrajectoryMsg(*move_group_->getCurrentState(), trajectory);
    trajectory_processing::IterativeParabolicTimeParameterization iptp;
    bool success =
        iptp.computeTimeStamps(rt, ui_->velocity_scaling_factor->value(), ui_->acceleration_scaling_factor->value());
    RCLCPP_INFO(LOGGER, "Computing time stamps %s", success ? "SUCCEDED" : "FAILED");

// Store trajectory in current_plan_
    current_plan_.reset(new moveit::planning_interface::MoveGroupInterface::Plan());
    rt.getRobotTrajectoryMsg(current_plan_->trajectory_);
    current_plan_->planning_time_ = (rclcpp::Clock().now() - start).seconds();
    return success;
  }
  return false;
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
bool MotionPlanningFrame::computeCartesianPlan()
{
  rclcpp::Time start = rclcpp::Clock().now();
  // get goal pose
  moveit::core::RobotState goal = *planning_display_->getQueryGoalState();
  std::vector<geometry_msgs::msg::Pose> waypoints;
  const std::string& link_name = move_group_->getEndEffectorLink();
  const moveit::core::LinkModel* link = move_group_->getRobotModel()->getLinkModel(link_name);
  if (!link)
  {
    RCLCPP_ERROR_STREAM(LOGGER, "Failed to determine unique end-effector link: " << link_name);
    return false;
  }
  waypoints.push_back(tf2::toMsg(goal.getGlobalLinkTransform(link)));

  // setup default params
  double cart_step_size = 0.01;
  double cart_jump_thresh = 0.0;
  bool avoid_collisions = true;

  // compute trajectory
  moveit_msgs::msg::RobotTrajectory trajectory;
  double fraction =
      move_group_->computeCartesianPath(waypoints, cart_step_size, cart_jump_thresh, trajectory, avoid_collisions);

  if (fraction >= 1.0)
  {
    RCLCPP_INFO(LOGGER, "Achieved %f %% of Cartesian path", fraction * 100.);

    // Compute time parameterization to also provide velocities
    // https://groups.google.com/forum/#!topic/moveit-users/MOoFxy2exT4
    robot_trajectory::RobotTrajectory rt(move_group_->getRobotModel(), move_group_->getName());
    rt.setRobotTrajectoryMsg(*move_group_->getCurrentState(), trajectory);
    trajectory_processing::IterativeParabolicTimeParameterization iptp;
    bool success =
        iptp.computeTimeStamps(rt, ui_->velocity_scaling_factor->value(), ui_->acceleration_scaling_factor->value());
    RCLCPP_INFO(LOGGER, "Computing time stamps %s", success ? "SUCCEDED" : "FAILED");

    // Store trajectory in current_plan_
    current_plan_.reset(new moveit::planning_interface::MoveGroupInterface::Plan());
    rt.getRobotTrajectoryMsg(current_plan_->trajectory_);
    current_plan_->planning_time_ = (rclcpp::Clock().now() - start).seconds();
    return success;
  }
  return false;
}

可以看到，它不是单纯地调用 computeCartesianPath 这个函数，下面还额外做了些操作。大概是1. 先实例化了一个 robottrajectory rt，来获得要规划的模型

2. 再示例化了一个轨迹处理的，简称为 iptp 的东西

3. 然后用iptp 里的 computeTimeStamps 功能，入参是速度约束和加速度约束这些

4. 执行rt.getRobotTrajectoryMsg这个函数

秘密可能就在 computeTimeStamps 这个函数里，点进去看看

复制代码

bool IterativeParabolicTimeParameterization::computeTimeStamps(robot_trajectory::RobotTrajectory& trajectory,
                                                               const double max_velocity_scaling_factor,
                                                               const double max_acceleration_scaling_factor) const
{
  if (trajectory.empty())
    return true;

const moveit::core::JointModelGroup* group = trajectory.getGroup();
  if (!group)
  {
    RCLCPP_ERROR(LOGGER, "It looks like the planner did not set "
                         "the group the plan was computed for");
    return false;
  }

// this lib does not actually work properly when angles wrap around, so we need to unwind the path first
  trajectory.unwind();

const int num_points = trajectory.getWayPointCount();
  std::vector<double> time_diff(num_points - 1, 0.0);  // the time difference between adjacent points

applyVelocityConstraints(trajectory, time_diff, max_velocity_scaling_factor);
  applyAccelerationConstraints(trajectory, time_diff, max_acceleration_scaling_factor);

updateTrajectory(trajectory, time_diff);
  return true;
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
bool IterativeParabolicTimeParameterization::computeTimeStamps(robot_trajectory::RobotTrajectory& trajectory,
                                                               const double max_velocity_scaling_factor,
                                                               const double max_acceleration_scaling_factor) const
{
  if (trajectory.empty())
    return true;

  const moveit::core::JointModelGroup* group = trajectory.getGroup();
  if (!group)
  {
    RCLCPP_ERROR(LOGGER, "It looks like the planner did not set "
                         "the group the plan was computed for");
    return false;
  }

  // this lib does not actually work properly when angles wrap around, so we need to unwind the path first
  trajectory.unwind();

  const int num_points = trajectory.getWayPointCount();
  std::vector<double> time_diff(num_points - 1, 0.0);  // the time difference between adjacent points

  applyVelocityConstraints(trajectory, time_diff, max_velocity_scaling_factor);
  applyAccelerationConstraints(trajectory, time_diff, max_acceleration_scaling_factor);

  updateTrajectory(trajectory, time_diff);
  return true;
}

看到这里面有两个函数，用来约束速度和加速度。！！！就是它

那iptp是什么？

具体可以看 trajectory_processing 这个功能包

复制代码

1
2
3
4
  src/iterative_time_parameterization.cpp
  src/iterative_spline_parameterization.cpp
  src/trajectory_tools.cpp
  src/time_optimal_trajectory_generation.cpp

除了iptp，还有ispt、totg这些，都是轨迹处理的东西

3. 实现方法

1. 一种是直接类似于 rviz的做法，增加一下这四行

复制代码

1
2
3
4
5
6
7
8
9
10
#include <moveit/robot_trajectory/robot_trajectory.h>
#include <moveit/trajectory_processing/iterative_time_parameterization.h>        


        robot_trajectory::RobotTrajectory rt(arm.getRobotModel(), arm.getName());
        rt.setRobotTrajectoryMsg(*arm.getCurrentState(), trajectory);
        trajectory_processing::IterativeParabolicTimeParameterization iptp;
        iptp.computeTimeStamps(rt, 0.01, 0.01);
        rt.getRobotTrajectoryMsg(trajectory);

其中的 0.01 ，0.01 是设置的最大速度和最大加速度因子。（该方法已经验证可行）

2. 另一种是直接设置

注意：这个方法我没验证过，只是觉得理论可行

复制代码

1
2
3
4
5
6
7
8
9
10
11
12
#include <moveit/robot_trajectory/robot_trajectory.h>
#include <moveit/trajectory_processing/iterative_time_parameterization.h>


  // this lib does not actually work properly when angles wrap around, so we need to unwind the path first
  trajectory.unwind();

  const int num_points = trajectory.getWayPointCount();
  std::vector<double> time_diff(num_points - 1, 0.0);  // the time difference between adjacent points

  applyVelocityConstraints(trajectory, time_diff, max_velocity_scaling_factor);
  applyAccelerationConstraints(trajectory, time_diff, max_acceleration_scaling_factor);