ROS2系列教程：Moveit2仿真与运动规划（基础）

1
ros2 node list
2
# /move_group
3
# /robot_state_publisher
4
# /rviz2

1
#!/usr/bin/env python3
2
import rclpy
3
from moveit_commander import MoveGroupCommander, PlanningSceneInterface
4

5
def main():
6
    rclpy.init()
7
    node = rclpy.create_node('check_group')
8
    # 1. 连接规划组
9
    group = MoveGroupCommander('arm_group', node)
10
    # 2. 打印当前状态
11
    print('当前关节角度：', group.get_current_joint_values())
12
    print('当前末端位姿：\n', group.get_current_pose().pose)
13
    print('规划组关节列表：', group.get_active_joints())
14
    print('支持的运动学求解器：', group.get_interface_description())
15
    node.destroy_node()
16
    rclpy.shutdown()
17

18
if __name__ == '__main__':
19
    main()

1
#!/usr/bin/env python3
2
import rclpy
3
from moveit_commander import MoveGroupCommander
4
from geometry_msgs.msg import Pose
5

6
def main():
7
    rclpy.init()
8
    node = rclpy.create_node('plan_pose')
9
    group = MoveGroupCommander('arm_group', node)
10

11
    # 1. 构造目标位姿：正前方 0.3m 处，tool0 竖直向下
12
    target = Pose()
13
    target.position.x = 0.30
14
    target.position.y = 0.0
15
    target.position.z = 0.25
16
    target.orientation.w = 1.0   # 单位四元数 = 无旋转
17

18
    # 2. 设置目标并规划
19
    group.set_pose_target(target, 'tool0')
20
    plan = group.plan()          # 返回计划对象（含轨迹）
21

22
    # 3. 检查结果
23
    if plan.joint_trajectory.points:
24
        print('规划成功！轨迹点数：', len(plan.joint_trajectory.points))
25
        first = plan.joint_trajectory.points[0]
26
        last = plan.joint_trajectory.points[-1]
27
        print('起点关节角：', first.positions)
28
        print('终点关节角：', last.positions)
29
    else:
30
        print('规划失败：目标不可达或无解')
31

32
    # 4. 显示在 RViz 里（只规划不执行）
33
    group.set_goal_tolerance(0.01)
34
    node.destroy_node()
35
    rclpy.shutdown()
36

37
if __name__ == '__main__':
38
    main()

1
#!/usr/bin/env python3
2
import rclpy
3
from moveit_commander import MoveGroupCommander
4

5
def main():
6
    rclpy.init()
7
    node = rclpy.create_node('plan_execute')
8
    group = MoveGroupCommander('arm_group', node)
9

10
    # 1. 回到 home 位形
11
    group.set_named_target('home')   # 来自 SRDF 里的 group_state
12
    group.go(wait=True)
13
    group.clear_pose_targets()
14

15
    # 2. 指定位姿并规划执行
16
    group.set_position_target([0.30, 0.0, 0.25])  # 只约束位置
17
    plan = group.plan()
18
    if getattr(plan, 'joint_trajectory', None) and plan.joint_trajectory.points:
19
        print('开始执行……')
20
        group.execute(plan, wait=True)
21
        print('执行完成！')
22
    else:
23
        print('规划失败')
24

25
    # 3. 回到 home
26
    group.set_named_target('home')
27
    group.go(wait=True)
28

29
    node.destroy_node()
30
    rclpy.shutdown()
31

32
if __name__ == '__main__':
33
    main()

1
#!/usr/bin/env python3
2
import rclpy
3
from moveit_commander import MoveGroupCommander
4

5
def main():
6
    rclpy.init()
7
    node = rclpy.create_node('plan_joints')
8
    group = MoveGroupCommander('arm_group', node)
9

10
    # 1. 直接在关节空间设目标（3 个活动关节）
11
    group.set_joint_value_target([0.5, -0.8, 1.2])
12

13
    # 2. 慢速规划：速度/加速度都缩到 30%
14
    group.set_max_velocity_scaling_factor(0.3)
15
    group.set_max_acceleration_scaling_factor(0.3)
16

17
    # 3. 规划并执行
18
    success, traj = group.plan() if hasattr(group.plan(), '__len__') else (True, group.plan())
19
    group.go(wait=True)
20
    print('到达关节目标，当前关节角：', group.get_current_joint_values())
21

22
    node.destroy_node()
23
    rclpy.shutdown()
24

25
if __name__ == '__main__':
26
    main()

1
#include <rclcpp/rclcpp.hpp>
2
#include <moveit/move_group_interface/move_group_interface.h>
3

4
int main(int argc, char** argv) {
5
  rclcpp::init(argc, argv);
6
  auto node = std::make_shared<rclcpp::Node>("arm_planner");
7
  auto executor = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
8
  executor->add_node(node);
9
  std::thread spin([&executor]() { executor->spin(); });
10

11
  using moveit::planning_interface::MoveGroupInterface;
12
  // 1. 连接规划组
13
  MoveGroupInterface group(node, "arm_group");
14

15
  // 2. 设置目标位姿
16
  geometry_msgs::msg::Pose target;
17
  target.position.x = 0.30;
18
  target.position.y = 0.0;
19
  target.position.z = 0.25;
20
  target.orientation.w = 1.0;
21
  group.setPoseTarget(target, "tool0");
22

23
  // 3. 规划
24
  moveit::planning_interface::MoveGroupInterface::Plan plan;
25
  bool ok = (group.plan(plan) == moveit::core::MoveItErrorCode::SUCCESS);
26
  if (!ok) {
27
    RCLCPP_ERROR(node->get_logger(), "规划失败");
28
  } else {
29
    RCLCPP_INFO(node->get_logger(), "规划成功，轨迹点数=%zu",
30
                plan.trajectory_.joint_trajectory.points.size());
31
    // 4. 执行
32
    group.execute(plan);
33
    RCLCPP_INFO(node->get_logger(), "执行完成");
34
  }
35

36
  // 5. 回到 home
37
  group.setNamedTarget("home");
38
  group.move();
39

40
  spin.join();
41
  rclcpp::shutdown();
42
  return 0;
43
}

1
#!/usr/bin/env python3
2
import rclpy
3
from moveit_commander import PlanningSceneInterface
4
from shape_msgs.msg import SolidPrimitive
5
from geometry_msgs.msg import Pose
6

7
def main():
8
    rclpy.init()
9
    node = rclpy.create_node('add_obstacle')
10
    scene = PlanningSceneInterface(node)
11
    # 在机械臂正前方 0.35m 放一个 0.1m 的立方体
12
    box = {'box': (0.08, 0.08, 0.08)}
13
    scene.add_box('obstacle', box, Pose(
14
        position=[0.30, 0.0, 0.15]))
15
    node.get_logger().info('已添加障碍物 obstacle')
16
    rclpy.spin_once(node, timeout_sec=0.5)
17
    rclpy.shutdown()
18

19
if __name__ == '__main__':
20
    main()