ROS2 Moveit2

基于Moveit2的人形机器人双臂规划3：Moveit2规划接口

Posted by banwf on 2025-12-21
Estimated Reading Time 9 Minutes
Words 1.9k In Total
Viewed Times

基于Moveit2的人形机器人双臂规划3：Moveit2规划接口

Moveit2的接口主要是两个：MoveGroupInterface和PlanningSceneInterface，其中MoveGroupInterfacePtr是规划接口，用于指定规划参数和发布规划指令，PlanningSceneInterface是场景接口，用于查询机器人模型和添加障碍物信息。

1.MoveGroup

MoveGroup是moveit2的核心接口，可以给规划组设置各种参数，并提供了三种规划方式：给定末端执行器pose规划，给定目标关节角规划，笛卡尔空间规划，后面会详细介绍。

1.1 创建MoveGroup

在配置moveit时我们确定了规划组名称，根据规划组名称创建MoveGroup对象
这里使用指针定义，其中"left_arm"等为规划组名称

1
2
3
4
5
6
7
moveit::planning_interface::MoveGroupInterfacePtr both_move_group_;
moveit::planning_interface::MoveGroupInterfacePtr left_move_group_;
moveit::planning_interface::MoveGroupInterfacePtr right_move_group_;

both_move_group_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>(shared_from_this(), "both_arms");
left_move_group_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>(shared_from_this(), "left_arm");
right_move_group_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>(shared_from_this(), "right_arm");

1.2 参数设置

具体参数可以参考Moveit2官方文档，以下介绍本项目用到的参数：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
// 设置规划管道和规划器
move_group->setPlanningPipelineId("isaac_ros_cumotion");
move_group->setPlannerId("cuMotion");
// 获取当前关节值
move_group->getCurrentJointValues();
// 设置参考坐标系
move_group->setPoseReferenceFrame(reference_frame);
// 设置目标坐标系
move_group->setEndEffectorLink(to_frame);
// 设置最大速度和加速度
move_group->setMaxVelocityScalingFactor(0.5);
move_group->setMaxAccelerationScalingFactor(0.5);
// 获取名称
move_group->getName()
// 设置起始状态为当前状态
move_group->setStartStateToCurrentState()
// 获取末端名称
move_group->getEndEffectorLink()
// 设置参考约束条件
move_group->setPathConstraints(ocm_constraints);
move_group->setGoalPositionTolerance(0.01);
move_group->setGoalOrientationTolerance(0.01);
// 清除参考约束条件
move_group->clearPathConstraints()

1.3 根据末端位姿规划

Moveit提供根据末端位姿规划路线，会自动求逆解并规划路线，末端位姿格式为geometry_msgs::msg::PoseStamped target_pose
使用move_group->setPoseTarget接口

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
move_group->setPoseReferenceFrame(reference_frame);
move_group->setEndEffectorLink(to_frame);
move_group->setPoseTarget(target_pose, to_frame);
moveit::planning_interface::MoveGroupInterface::Plan my_plan;
RCLCPP_INFO(this->get_logger(),"Begin plan to target_pose.");
bool success = (move_group->plan(my_plan) 
                == moveit::core::MoveItErrorCode::SUCCESS);
if (success){
    RCLCPP_INFO(this->get_logger(),"Move to target_pose successful.");
    move_group->execute(my_plan);
    move_group->clearPathConstraints();
    return true;
} else {
    RCLCPP_ERROR(this->get_logger(),"Planning failed to target_pose.");
    move_group->clearPathConstraints();
    return false;
}

还可以限制末端位姿的旋转，可以实现在路径中末端某些轴保持不动
在plan之前使用move_group->setPathConstraints

1
2
3
4
5
6
7
8
9
10
11
12
13
14
moveit_msgs::msg::OrientationConstraint ocm;
ocm.link_name = to_frame; // 设置约束应用于的链接
ocm.header.frame_id = "torso_link"; // 设置参考坐标系为 "base_link"
ocm.orientation = current_pose2.orientation; // 设置目标方向
ocm.absolute_x_axis_tolerance = 0.15; // 设置 X 轴容差
ocm.absolute_y_axis_tolerance = 0.15; // 设置 Y 轴容差
ocm.absolute_z_axis_tolerance = 1.57; // 设置 Z 轴容差
ocm.weight = 1.0; // 设置约束的权重

// 创建一个 Constraints 对象并添加 OrientationConstraint
moveit_msgs::msg::Constraints ocm_constraints;
ocm_constraints.orientation_constraints.push_back(ocm);
// 应用路径约束
move_group->setPathConstraints(ocm_constraints);

1.4 根据关节角度规划

使用move_group->setJointValueTarget接口进行规划

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
const moveit::core::JointModelGroup* joint_model_group_;
    
const std::string& group_name = move_group->getName();
RCLCPP_INFO(this->get_logger(), "group_name = %s", group_name.c_str());
joint_model_group_ = move_group->getCurrentState()->getJointModelGroup(group_name);

size_t joint_count = joint_model_group_->getVariableCount();
if (target_joint_positions.size() != joint_count)
{
RCLCPP_ERROR(this->get_logger(), "Target joint positions size (%zu) does not match joint count (%zu).", 
                target_joint_positions.size(), joint_count);
return false;
}

move_group->setJointValueTarget(target_joint_positions);

// 规划和执行
moveit::planning_interface::MoveGroupInterface::Plan plan;
bool success = (move_group->plan(plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);

if (success)
{
    RCLCPP_INFO(this->get_logger(), "Plan successful! Executing...");
    auto execute_result = move_group->execute(plan);
    if (execute_result != moveit::planning_interface::MoveItErrorCode::SUCCESS)
    {
        RCLCPP_ERROR(this->get_logger(), "Execution failed with error code: %d", execute_result.val);
        return false;
    }
}
else
{
    RCLCPP_ERROR(this->get_logger(), "Plan failed!");
    return false;
}

1.5 笛卡尔空间规划

通过指定目标点，在笛卡尔空间内规划路径，可实现直线运动、曲线运动，是moveit2中自由度最高的规划方式，可以自己确定轨迹，接口为move_group->computeCartesianPath，主要参数为

  • std::vector<geometry_msgs::msg::Pose>& waypoints，途径点列表，moveit会根据这些途径点进行插值，依次求逆解并生成关节角度列表进行执行；
  • double current_eef_step插值步长，步长越短插值的点越多；
  • double jump_threshold跳变阈值，一般设为0.0，不允许跳变

特别需要注意的是，笛卡尔空间插值只会对位置进行插值，如果途径点列表中相邻两点的位姿发生变化过大就会导致路径生成失败，如果要途径点位姿变化，需要自己手动进行球面插值，在之后的文章中会提到如何实现。

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
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
try {
    // 2. 设置起始状态
    move_group->setStartStateToCurrentState();
    constexpr int MAX_RETRIES = 5;
    constexpr double INITIAL_EEF_STEP = 0.01;
    constexpr double STEP_DECAY_FACTOR = 0.5;
    double current_eef_step = INITIAL_EEF_STEP;
    double jump_threshold = 0.0;
    double max_vel_scaling = 0.25;
    double max_accel_scaling = 0.25;
    bool success = false;
    int attempt = 0;
    moveit_msgs::msg::RobotTrajectory trajectory_msg;
    // 3. 计算笛卡尔路径
    do {
        
        double fraction = move_group->computeCartesianPath(
            waypoints,
            current_eef_step,  // 动态调整步长
            jump_threshold,
            trajectory_msg,
            true,
            nullptr
        );

        if(fraction >= 0.85) {
            // 路径规划成功
            success = true;
            break;
        } else {
            // 自适应调整策略
            current_eef_step *= STEP_DECAY_FACTOR;  // 降低步长提高精度
            RCLCPP_WARN(get_logger(), "Attempt %d failed (%.1f%%), retrying with eef_step=%.3f", 
                    attempt+1, fraction*100, current_eef_step);
        }
    } while (++attempt < MAX_RETRIES);
    if(success == false){
    RCLCPP_ERROR(this->get_logger(), "compute failed");
    return false;
    }

    // 5. 轨迹时间参数化
    robot_trajectory::RobotTrajectory trajectory(
        move_group->getCurrentState()->getRobotModel(),
        move_group->getName()
    );
    trajectory.setRobotTrajectoryMsg(*move_group->getCurrentState(), trajectory_msg);

    trajectory_processing::IterativeParabolicTimeParameterization time_param;
    if(!time_param.computeTimeStamps(trajectory, max_vel_scaling, max_accel_scaling)) {
        RCLCPP_ERROR(this->get_logger(), "Time parameterization failed");
        return false;
    }


    // 7. 执行轨迹
    moveit::planning_interface::MoveGroupInterface::Plan plan;
    plan.trajectory_ = trajectory_msg;
    
    if(move_group->execute(plan) == moveit::core::MoveItErrorCode::SUCCESS) {
        return true;
    } else {
        RCLCPP_ERROR(this->get_logger(), "Execute failed");
        return false;
    }

} catch (const std::exception& e) {
    RCLCPP_ERROR(this->get_logger(), e.what());
    return false;
}

2.PlanningScene

通过场景规划接口我们可以给场景中添加或者删除障碍物，机械臂规划时会自动避开这些障碍物
接口类型为moveit::planning_interface::PlanningSceneInterface
主要使用3个接口：

  • planning_scene_interface_.applyCollisionObjects({box}) 添加障碍物
  • planning_scene_interface_.removeCollisionObjects({box.id}) 删除障碍物
  • planning_scene_interface_.applyAttachedCollisionObject(attached_object); 将障碍物附着在机械臂末端一起移动

2.1 创建并添加障碍物

moveit中障碍物有三种类型，分别是长方体、圆柱体和球体，创建和添加步骤如下，此代码是从我的服务中摘出来的，所以参数是从req获取，请根据实际情况修改

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
// 创建碰撞物体
moveit_msgs::msg::CollisionObject collision_object;
collision_object.id = req->obstacle_id;
collision_object.header.frame_id = "torso_link";
// 设置形状
shape_msgs::msg::SolidPrimitive primitive;
switch (req->shape_type) {
    case control_interfaces::srv::ManageObstacle::Request::BOX:
    RCLCPP_INFO(this->get_logger(), "box");
    primitive.type = primitive.BOX;
    primitive.dimensions.resize(3); // 必须预先确定尺寸
    primitive.dimensions[0] = req->dimensions[0]; // x
    primitive.dimensions[1] = req->dimensions[1]; // y
    primitive.dimensions[2] = req->dimensions[2]; // z
    break;

    case control_interfaces::srv::ManageObstacle::Request::SPHERE:
    primitive.type = primitive.SPHERE;
    primitive.dimensions.resize(1);
    primitive.dimensions[0] = req->dimensions[0];
    break;

    case control_interfaces::srv::ManageObstacle::Request::CYLINDER:
    primitive.type = primitive.CYLINDER;
    primitive.dimensions.resize(2);
    primitive.dimensions[0] = req->dimensions[0];  // 半径
    primitive.dimensions[1] = req->dimensions[1];  // 高度
    break;
}
// 设置位置
geometry_msgs::msg::Pose pose;
pose = req->center;
// 添加操作
collision_object.primitives.push_back(primitive);
collision_object.primitive_poses.push_back(pose);
collision_object.operation = moveit_msgs::msg::CollisionObject::ADD;
// 应用更新
planning_scene_interface_.applyCollisionObject(collision_object);

2.2 将障碍物附着在末端

创建一个moveit_msgs::msg::AttachedCollisionObject attached_object并把障碍物添加到attached_object中，并修改碰撞矩阵，设置不考虑末端和障碍物之间的碰撞

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
moveit_msgs::msg::AttachedCollisionObject attached_object;
attached_object.link_name = LoR + "_tcp_1";
attached_object.object.id = LoR + "box";
attached_object.object.operation = attached_object.object.ADD;
planning_scene_interface_.applyCollisionObjects({box});
planning_scene_interface_.applyAttachedCollisionObject(attached_object);
// auto planning_scene_monitor = std::make_shared<planning_scene_monitor::PlanningSceneMonitor>("robot_description");
planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor(
    new planning_scene_monitor::PlanningSceneMonitor(shared_from_this(), "robot_description", "planning_scene_monitor"));
planning_scene_monitor::LockedPlanningSceneRW ls(planning_scene_monitor);
collision_detection::AllowedCollisionMatrix& acm = ls->getAllowedCollisionMatrixNonConst();
// planning_scene_monitor->clearOctomap();
if (LoR == "left")
{
acm.setEntry("leftbox", "left_robotiq_85_left_finger_tip_link", true);
acm.setEntry("leftbox", "left_robotiq_85_right_finger_tip_link", true);
acm.setEntry("leftbox", "left_robotiq_85_base_link", true);
acm.setEntry("leftbox", "left_robotiq_85_left_inner_knuckle_link", true);
acm.setEntry("leftbox", "left_robotiq_85_right_inner_knuckle_link", true);
// acm.setEntry("leftbox", "octomap", true);
left_attached = true;
} else {
acm.setEntry("rightbox", "right_robotiq_85_left_finger_tip_link", true);
acm.setEntry("rightbox", "right_robotiq_85_right_finger_tip_link", true);
acm.setEntry("rightbox", "right_robotiq_85_base_link", true);
acm.setEntry("rightbox", "right_robotiq_85_left_inner_knuckle_link", true);
acm.setEntry("rightbox", "right_robotiq_85_right_inner_knuckle_link", true);
// acm.setEntry("rightbox", "octomap", true);    
right_attached = true;
}
moveit_msgs::msg::PlanningScene diff_scene;
ls->getPlanningSceneDiffMsg(diff_scene);

planning_scene_interface_.applyPlanningScene(diff_scene);

如果您喜欢此博客或发现它对您有用，则欢迎对此发表评论。 也欢迎您共享此博客，以便更多人可以参与。 如果博客中使用的图像侵犯了您的版权，请与作者联系以将其删除。 谢谢 ！

特色标签
ROS2 Moveit2
链友
ESC