ROS2系列教程：Moveit2进阶（抓取/OMPL/自定义规划）

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('pick_simple')
8
    group = MoveGroupCommander('arm_group', node)
9

10
    # 1. 回到 home
11
    group.set_named_target('home')
12
    group.go(wait=True)
13

14
    # 2. 预抓取：物体在 (0.35, 0.0, 0.15)，先到上方 0.08m
15
    group.set_pose_target([0.35, 0.0, 0.23, 0, 0, 0], 'tool0')  # xyz + rpy(度)
16
    group.go(wait=True)
17

18
    # 3. 下探到抓取高度
19
    group.set_pose_target([0.35, 0.0, 0.15, 0, 0, 0], 'tool0')
20
    group.go(wait=True)
21
    print('到达抓取位置——在这里闭合夹爪（真机）/ 模拟中跳过')
22

23
    # 4. 抬升
24
    group.set_pose_target([0.35, 0.0, 0.23, 0, 0, 0], 'tool0')
25
    group.go(wait=True)
26

27
    # 5. 回到 home
28
    group.set_named_target('home')
29
    group.go(wait=True)
30
    print('完成一次简化抓取')
31

32
    node.destroy_node()
33
    rclpy.shutdown()
34

35
if __name__ == '__main__':
36
    main()

1
<library path="libmy_planner">
2
  <class name="my_planner/MyPlanner"
3
         type="my_planner::MyPlanner"
4
         base_class_type="planning_interface::PlannerManager">
5
    <description>自定义直线规划器示例</description>
6
  </class>
7
</library>

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('waypoints')
8
    group = MoveGroupCommander('arm_group', node)
9

10
    # 路径点（相对 base_link 的 xyz，姿态默认）
11
    points = [
12
        [0.30, 0.0, 0.25],
13
        [0.30, 0.20, 0.25],
14
        [0.20, 0.20, 0.30],
15
        [0.30, 0.0, 0.25],   # 回到起点
16
    ]
17

18
    group.set_named_target('home')
19
    group.go(wait=True)
20

21
    for i, p in enumerate(points):
22
        group.set_position_target(p)
23
        ok = group.go(wait=True)
24
        if not ok:
25
            print(f'第 {i} 个点规划/执行失败，中断')
26
            break
27
        print(f'已到达第 {i+1} 个点: {p}')
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
from geometry_msgs.msg import Pose
5

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

11
    # 从当前位姿出发，沿 x 正方向画直线
12
    waypoints = []
13
    for i in range(11):           # 11 个插值点
14
        p = group.get_current_pose().pose
15
        p.position.x += i * 0.02  # 每步 2cm
16
        waypoints.append(p)
17

18
    plan, fraction = group.compute_cartesian_path(
19
        waypoints, eef_step=0.01, jump_threshold=0.0)
20
    print(f'笛卡尔路径完成度: {fraction:.1%}')
21

22
    if fraction > 0.9:
23
        group.execute(plan, wait=True)
24
    else:
25
        print('路径被阻塞，未执行')
26

27
    node.destroy_node()
28
    rclpy.shutdown()
29

30
if __name__ == '__main__':
31
    main()

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

5
class PickPlaceFSM:
6
    """简易状态机：HOME → PRE_GRASP → GRASP → LIFT → PLACE → BACK"""
7
    def __init__(self, node):
8
        self.group = MoveGroupCommander('arm_group', node)
9
        self.scene = PlanningSceneInterface(node)
10
        self.state = 'HOME'
11
        self.pick_pos = [0.35, 0.0, 0.15]
12
        self.place_pos = [0.35, 0.30, 0.15]
13
        self.lift_h = 0.08   # 抬升高度
14

15
    def move_to(self, target, wait=True):
16
        if len(target) == 3:
17
            self.group.set_position_target(target)
18
        else:
19
            self.group.set_pose_target(target, 'tool0')
20
        return self.group.go(wait=wait)
21

22
    def run(self):
23
        while rclpy.ok():
24
            if self.state == 'HOME':
25
                self.group.set_named_target('home')
26
                self.group.go(wait=True)
27
                self.state = 'PRE_GRASP'
28
            elif self.state == 'PRE_GRASP':
29
                if self.move_to([self.pick_pos[0], self.pick_pos[1],
30
                                 self.pick_pos[2] + self.lift_h]):
31
                    self.state = 'GRASP'
32
            elif self.state == 'GRASP':
33
                if self.move_to(self.pick_pos):
34
                    print('>>> 闭合夹爪（此处接真机/仿真夹爪指令）')
35
                    self.state = 'LIFT'
36
            elif self.state == 'LIFT':
37
                if self.move_to([self.pick_pos[0], self.pick_pos[1],
38
                                 self.pick_pos[2] + self.lift_h]):
39
                    self.state = 'PLACE'
40
            elif self.state == 'PLACE':
41
                if self.move_to([self.place_pos[0], self.place_pos[1],
42
                                 self.place_pos[2] + self.lift_h]):
43
                    self.state = 'PUT_DOWN'
44
            elif self.state == 'PUT_DOWN':
45
                if self.move_to(self.place_pos):
46
                    print('>>> 张开夹爪')
47
                    self.state = 'BACK'
48
            elif self.state == 'BACK':
49
                self.move_to([self.place_pos[0], self.place_pos[1],
50
                              self.place_pos[2] + self.lift_h])
51
                self.group.set_named_target('home')
52
                self.group.go(wait=True)
53
                print('任务完成！')
54
                break
55

56
def main():
57
    rclpy.init()
58
    node = rclpy.create_node('pick_place_fsm')
59
    fsm = PickPlaceFSM(node)
60
    fsm.run()
61
    node.destroy_node()
62
    rclpy.shutdown()
63

64
if __name__ == '__main__':
65
    main()