小惠珠哦

这个屌丝很懒，什么也没留下！

热门标签

机器人操作系统ROS（九）：MoveIt！与机械臂控制_ros moveit

作者：小惠珠哦 | 2024-07-22 11:15:30

踩

ros moveit

机器人操作系统ROS（九）：MoveIt！与机械臂控制

MoveIt！简介
MoveIt！系统架构
MoveIt！编程与机械臂控制
Pick and Place示例

MoveIt！简介

MoveIt！是在2012年在PR2与多个机械臂功能包的基础上集成得到的ROS软件包，它为开发者提供了一个易于使用的集成化开发平台，由一系列移动操作的功能包组成，包括运动规划、操作控制、3D感知、运动学、控制与导航算法等，且提供友好的GUI，可以广泛应用于工业、商业、研发和其他领域。

使用MoveIt！实现机械臂控制的四个步骤：

组装：在控制之前需要有机器人，可以是真实的机械臂，也可以是仿真的机械臂，但都要创建完整的机器人URDF模型。
配置：使用MoveIt！控制机械臂之前，需要根据机器人的URDF模型，再使用Setup Assistant工具完成自碰撞矩阵、规划组、终端夹具等配置，配置完成后生成一个ROS功能包。
驱动：使用ArbotiX或者ros_control功能包中的控制器插件，实现对机械臂关节的驱动。插件的使用方法一般分为两步：首先创建插件的YAML配置文件，然后通过launch文件启动插件并加载配置参数。
控制：MoveIt！提供了C++、Python、rviz插件等接口，可以实现机器人关节空间和工作空间下的运动规划，规划过程中会综合考虑场景信息，并实现自主避障的优化控制。

关于MoveIt！安装与使用的更多内容可见教程。

MoveIt！系统架构

move_group是MoveIt！的核心节点，可以综合其他独立的功能组件作为用户提供ROS中的动作指令和服务。move_group本身并不具备丰富的功能，主要完成各功能包、插件的集成。它通过话题（Topic）的方式接收机器人发布的传感器信息、关节状态信息，以及机器人的TF坐标变换；它通过服务（Service）的方式运动规划器和运动学求解器通信；它还通过动作（Action）的方式向机器人控制器发送规划结果，并接受机器人执行情况的反馈；另外，还需要ROS参数服务器提供机器人的运动学参数，这些参数可根据机器人的URDF模型生成（SRDF和配置文件）。
在这里插入图片描述

MoveIt！编程与机械臂控制

MoveIt！的move_group提供了丰富的C++和Python的编程API，可以帮助我们完成更多运动控制的相关功能，接下来主要以Python API为例，介绍MoveIt！的编程方法。

关节空间规划

关节空间运动是以关节角度为控制量的机器人运动。机械臂关节空间的规划不需要考虑机器人终端的姿态。
使用如下命令实现MArm关节空间下的运动测试：

roslaunch marm_planning arm_planning.launch
rosrun marm_planning moveit_fk_demo.py
1
2

marm_planning/scripts/moveit_fk_demo.py的源码如下：

import rospy, sys
import moveit_commander

class MoveItFkDemo:
    def __init__(self):
        # 初始化move_group的API
        moveit_commander.roscpp_initialize(sys.argv)

        # 初始化ROS节点
        rospy.init_node('moveit_fk_demo', anonymous=True)
 
        # 初始化需要使用move group控制的机械臂中的arm group
        arm = moveit_commander.MoveGroupCommander('manipulator')
        
        # 设置机械臂运动的允许误差值
        arm.set_goal_joint_tolerance(0.001)

        # 设置允许的最大速度和加速度
        arm.set_max_acceleration_scaling_factor(0.5)
        arm.set_max_velocity_scaling_factor(0.5)
        
        # 控制机械臂先回到初始化位置
        arm.set_named_target('home')
        arm.go()
        rospy.sleep(1)
         
        # 设置机械臂的目标位置，使用六轴的位置数据进行描述（单位：弧度）
        joint_positions = [0.391410, -0.676384, -0.376217, 0.0, 1.052834, 0.454125]
        arm.set_joint_value_target(joint_positions)
                 
        # 控制机械臂完成运动
        arm.go()
        rospy.sleep(1)

        # 控制机械臂先回到初始化位置
        arm.set_named_target('home')
        arm.go()
        rospy.sleep(1)
        
        # 关闭并退出moveit
        moveit_commander.roscpp_shutdown()
        moveit_commander.os._exit(0)

if __name__ == "__main__":
    try:
        MoveItFkDemo()
    except rospy.ROSInterruptException:
        pass
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

工作空间规划

工作空间规划是通过机器人终端的三维坐标位置和姿态给定，在运动规划时使用逆向运动学求解各轴位置。
使用如下命令实现MArm工作空间下的运动测试：

roslaunch marm_planning arm_planning.launch
rosrun marm_planning moveit_ik_demo.py
1
2

marm_planning/scripts/moveit_ik_demo.py的源码如下：

import rospy, sys
import moveit_commander
from geometry_msgs.msg import PoseStamped, Pose


class MoveItIkDemo:
    def __init__(self):
        # 初始化move_group的API
        moveit_commander.roscpp_initialize(sys.argv)
        
        # 初始化ROS节点
        rospy.init_node('moveit_ik_demo')
                
        # 初始化需要使用move group控制的机械臂中的arm group
        arm = moveit_commander.MoveGroupCommander('manipulator')
                
        # 获取终端link的名称
        end_effector_link = arm.get_end_effector_link()
                        
        # 设置目标位置所使用的参考坐标系
        reference_frame = 'base_link'
        arm.set_pose_reference_frame(reference_frame)
                
        # 当运动规划失败后，允许重新规划
        arm.allow_replanning(True)
        
        # 设置位置(单位：米)和姿态（单位：弧度）的允许误差
        arm.set_goal_position_tolerance(0.001)
        arm.set_goal_orientation_tolerance(0.001)
       
        # 设置允许的最大速度和加速度
        arm.set_max_acceleration_scaling_factor(0.5)
        arm.set_max_velocity_scaling_factor(0.5)

        # 控制机械臂先回到初始化位置
        arm.set_named_target('home')
        arm.go()
        rospy.sleep(1)
               
        # 设置机械臂工作空间中的目标位姿，位置使用x、y、z坐标描述，
        # 姿态使用四元数描述，基于base_link坐标系
        target_pose = PoseStamped()
        target_pose.header.frame_id = reference_frame
        target_pose.header.stamp = rospy.Time.now()     
        target_pose.pose.position.x = 0.2593
        target_pose.pose.position.y = 0.0636
        target_pose.pose.position.z = 0.1787
        target_pose.pose.orientation.x = 0.911822
        target_pose.pose.orientation.y = -0.0269758
        target_pose.pose.orientation.z = 0.285694
        target_pose.pose.orientation.w = -0.293653
        
        # 设置机器臂当前的状态作为运动初始状态
        arm.set_start_state_to_current_state()
        
        # 设置机械臂终端运动的目标位姿
        arm.set_pose_target(target_pose, end_effector_link)
        
        # 规划运动路径
        traj = arm.plan()
        
        # 按照规划的运动路径控制机械臂运动
        arm.execute(traj)
        rospy.sleep(1)

        # 控制机械臂终端向右移动5cm
        arm.shift_pose_target(1, -0.05, end_effector_link)
        arm.go()
        rospy.sleep(1)
  
        # 控制机械臂终端反向旋转90度
        arm.shift_pose_target(3, -1.57, end_effector_link)
        arm.go()
        rospy.sleep(1)
           
        # 控制机械臂回到初始化位置
        arm.set_named_target('home')
        arm.go()

        # 关闭并退出moveit
        moveit_commander.roscpp_shutdown()
        moveit_commander.os._exit(0)

if __name__ == "__main__":
    MoveItIkDemo()
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
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85

笛卡尔运动规划

工作空间的运动规划并没有对机器人终端轨迹有任何约束，目标位姿给定后，可以通过运动学反解获得关节空间下的各轴位置（即角度），接下来的规划和运动仍然在关节空间下完成。笛卡尔运动规划不仅考虑机械臂的起始位姿，还考虑了运动过程中的位姿约束。
使用如下命令实现MArm笛卡尔运动规划测试：

roslaunch marm_planning arm_planning_with_trail.launch
# 带路径约束的运动规划
rosrun marm_planning moveit_cartesian_demo.py _cartesian:=True
# 不带路径约束的运动规划
rosrun marm_planning moveit_cartesian_demo.py _cartesian:=False
1
2
3
4
5

marm_planning/scripts/moveit_cartesian_demo.py的源码如下：

import rospy, sys
import moveit_commander
from moveit_commander import MoveGroupCommander
from geometry_msgs.msg import Pose
from copy import deepcopy

class MoveItCartesianDemo:
    def __init__(self):
        # 初始化move_group的API
        moveit_commander.roscpp_initialize(sys.argv)

        # 初始化ROS节点
        rospy.init_node('moveit_cartesian_demo', anonymous=True)
                        
        # 初始化需要使用move group控制的机械臂中的arm group
        arm = MoveGroupCommander('manipulator')
        
        # 当运动规划失败后，允许重新规划
        arm.allow_replanning(True)
        
        # 设置目标位置所使用的参考坐标系
        arm.set_pose_reference_frame('base_link')
                
        # 设置位置(单位：米)和姿态（单位：弧度）的允许误差
        arm.set_goal_position_tolerance(0.001)
        arm.set_goal_orientation_tolerance(0.001)
        
        # 设置允许的最大速度和加速度
        arm.set_max_acceleration_scaling_factor(0.5)
        arm.set_max_velocity_scaling_factor(0.5)
        
        # 获取终端link的名称
        end_effector_link = arm.get_end_effector_link()

        # 控制机械臂先回到初始化位置
        arm.set_named_target('home')
        arm.go()
        rospy.sleep(1)
                                               
        # 获取当前位姿数据最为机械臂运动的起始位姿
        start_pose = arm.get_current_pose(end_effector_link).pose

        print start_pose
                
        # 初始化路点列表
        waypoints = []
                
        # 将初始位姿加入路点列表
        waypoints.append(start_pose)
            
        # 设置路点数据，并加入路点列表
        wpose = deepcopy(start_pose)
        wpose.position.z -= 0.2
        waypoints.append(deepcopy(wpose))

        wpose.position.x += 0.1
        waypoints.append(deepcopy(wpose))
        
        wpose.position.y += 0.1
        waypoints.append(deepcopy(wpose))

        fraction = 0.0   #路径规划覆盖率
        maxtries = 100   #最大尝试规划次数
        attempts = 0     #已经尝试规划次数
        
        # 设置机器臂当前的状态作为运动初始状态
        arm.set_start_state_to_current_state()
 
        # 尝试规划一条笛卡尔空间下的路径，依次通过所有路点
        while fraction < 1.0 and attempts < maxtries:
            (plan, fraction) = arm.compute_cartesian_path (
                                    waypoints,   # waypoint poses，路点列表
                                    0.01,        # eef_step，终端步进值
                                    0.0,         # jump_threshold，跳跃阈值
                                    True)        # avoid_collisions，避障规划
            
            # 尝试次数累加
            attempts += 1
            
            # 打印运动规划进程
            if attempts % 10 == 0:
                rospy.loginfo("Still trying after " + str(attempts) + " attempts...")
                     
        # 如果路径规划成功（覆盖率100%）,则开始控制机械臂运动
        if fraction == 1.0:
            rospy.loginfo("Path computed successfully. Moving the arm.")
            arm.execute(plan)
            rospy.loginfo("Path execution complete.")
        # 如果路径规划失败，则打印失败信息
        else:
            rospy.loginfo("Path planning failed with only " + str(fraction) + " success after " + str(maxtries) + " attempts.")  

        rospy.sleep(1)

        # 控制机械臂先回到初始化位置
        arm.set_named_target('home')
        arm.go()
        rospy.sleep(1)
        
        # 关闭并退出moveit
        moveit_commander.roscpp_shutdown()
        moveit_commander.os._exit(0)

if __name__ == "__main__":
    try:
        MoveItCartesianDemo()
    except rospy.ROSInterruptException:
        pass
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
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108

避障规划

机器人在工作空间内运动规划过程中存在障碍物时，需要进行避障规划。

使用如下命令实现MArm避障规划测试：

roslaunch marm_planning arm_planning.launch
rosrun marm_planning moveit_obstacles_demo.py
1
2

marm_planning/scripts/moveit_obstacles_demo.py的源码如下：

import rospy, sys
import moveit_commander
from moveit_commander import MoveGroupCommander, PlanningSceneInterface
from moveit_msgs.msg import  PlanningScene, ObjectColor
from geometry_msgs.msg import PoseStamped, Pose

class MoveItObstaclesDemo:
    def __init__(self):
        # 初始化move_group的API
        moveit_commander.roscpp_initialize(sys.argv)
        
        # 初始化ROS节点
        rospy.init_node('moveit_obstacles_demo')
        
        # 初始化场景对象
        scene = PlanningSceneInterface()
        
        # 创建一个发布场景变化信息的发布者
        self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=5)
        
        # 创建一个存储物体颜色的字典对象
        self.colors = dict()
        
        # 等待场景准备就绪
        rospy.sleep(1)
                        
        # 初始化需要使用move group控制的机械臂中的arm group
        arm = MoveGroupCommander('arm')
        
        # 获取终端link的名称
        end_effector_link = arm.get_end_effector_link()
        
        # 设置位置(单位：米)和姿态（单位：弧度）的允许误差
        arm.set_goal_position_tolerance(0.01)
        arm.set_goal_orientation_tolerance(0.05)
       
        # 当运动规划失败后，允许重新规划
        arm.allow_replanning(True)
        
        # 设置目标位置所使用的参考坐标系
        reference_frame = 'base_link'
        arm.set_pose_reference_frame(reference_frame)
        
        # 设置每次运动规划的时间限制：5s
        arm.set_planning_time(5)
        
        # 设置场景物体的名称
        table_id = 'table'
        box1_id = 'box1'
        box2_id = 'box2'
        
        # 移除场景中之前运行残留的物体
        scene.remove_world_object(table_id)
        scene.remove_world_object(box1_id)
        scene.remove_world_object(box2_id)    
        rospy.sleep(1)
        
        # 控制机械臂先回到初始化位置
        arm.set_named_target('home')
        arm.go()
        rospy.sleep(2)
        
        # 设置桌面的高度
        table_ground = 0.25
        
        # 设置table、box1和box2的三维尺寸
        table_size = [0.2, 0.7, 0.01]
        box1_size = [0.1, 0.05, 0.05]
        box2_size = [0.05, 0.05, 0.15]
        
        # 将三个物体加入场景当中
        table_pose = PoseStamped()
        table_pose.header.frame_id = reference_frame
        table_pose.pose.position.x = 0.26
        table_pose.pose.position.y = 0.0
        table_pose.pose.position.z = table_ground + table_size[2] / 2.0
        table_pose.pose.orientation.w = 1.0
        scene.add_box(table_id, table_pose, table_size)
        
        box1_pose = PoseStamped()
        box1_pose.header.frame_id = reference_frame
        box1_pose.pose.position.x = 0.21
        box1_pose.pose.position.y = -0.1
        box1_pose.pose.position.z = table_ground + table_size[2] + box1_size[2] / 2.0
        box1_pose.pose.orientation.w = 1.0   
        scene.add_box(box1_id, box1_pose, box1_size)
        
        box2_pose = PoseStamped()
        box2_pose.header.frame_id = reference_frame
        box2_pose.pose.position.x = 0.19
        box2_pose.pose.position.y = 0.15
        box2_pose.pose.position.z = table_ground + table_size[2] + box2_size[2] / 2.0
        box2_pose.pose.orientation.w = 1.0   
        scene.add_box(box2_id, box2_pose, box2_size)
        
        # 将桌子设置成红色，两个box设置成橙色
        self.setColor(table_id, 0.8, 0, 0, 1.0)
        self.setColor(box1_id, 0.8, 0.4, 0, 1.0)
        self.setColor(box2_id, 0.8, 0.4, 0, 1.0)
        
        # 将场景中的颜色设置发布
        self.sendColors()    
        
        # 设置机械臂的运动目标位置，位于桌面之上两个盒子之间
        target_pose = PoseStamped()
        target_pose.header.frame_id = reference_frame
        target_pose.pose.position.x = 0.2
        target_pose.pose.position.y = 0.0
        target_pose.pose.position.z = table_pose.pose.position.z + table_size[2] + 0.05
        target_pose.pose.orientation.w = 1.0
        
        # 控制机械臂运动到目标位置
        arm.set_pose_target(target_pose, end_effector_link)
        arm.go()
        rospy.sleep(2)

        # 设置机械臂的运动目标位置，进行避障规划
        target_pose2 = PoseStamped()
        target_pose2.header.frame_id = reference_frame
        target_pose2.pose.position.x = 0.2
        target_pose2.pose.position.y = -0.25
        target_pose2.pose.position.z = table_pose.pose.position.z + table_size[2] + 0.05
        target_pose2.pose.orientation.w = 1.0
        
        # 控制机械臂运动到目标位置
        arm.set_pose_target(target_pose2, end_effector_link)
        arm.go()
        rospy.sleep(2)
        
        # 控制机械臂回到初始化位置
        arm.set_named_target('home')
        arm.go()
        
        # 关闭并退出moveit
        moveit_commander.roscpp_shutdown()
        moveit_commander.os._exit(0)
        
    # 设置场景物体的颜色
    def setColor(self, name, r, g, b, a = 0.9):
        # 初始化moveit颜色对象
        color = ObjectColor()
        
        # 设置颜色值
        color.id = name
        color.color.r = r
        color.color.g = g
        color.color.b = b
        color.color.a = a
        
        # 更新颜色字典
        self.colors[name] = color

    # 将颜色设置发送并应用到moveit场景当中
    def sendColors(self):
        # 初始化规划场景对象
        p = PlanningScene()

        # 需要设置规划场景是否有差异     
        p.is_diff = True
        
        # 从颜色字典中取出颜色设置
        for color in self.colors.values():
            p.object_colors.append(color)
        
        # 发布场景物体颜色设置
        self.scene_pub.publish(p)

if __name__ == "__main__":
    try:
        MoveItObstaclesDemo()
    except KeyboardInterrupt:
        raise
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
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172

Pick and Place示例

此节演示了一个简单的机器人pick and place应用，这个应用就是让机器人用夹爪将工作空间内的某个物体夹起来，然后将该物体放到目标位置。类似的功能在机器人的实际生产应用中使用非常广泛，例如码垛、搬运、挑拣等工作。

在这个应用例程中，假设已知物体的位置，MoveIt！需要控制机器人去抓取物体并放置到指定位置。

使用如下命令实现pick and place例程：

roslaunch marm_planning arm_planning.launch
rosrun marm_planning moveit_pick_and_place_demo.py
1
2

创建抓取的目标物体
创建一个用来抓取的目标物体，并将其设置颜色，代码如下：

target_size = [0.02,0.01,0.12]
target_pose = PoseStamped() 
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = 0.32
target_pose.pose.position.y = 0.0
target_pose.pose.position.z = table_ground + table_size[2] + target_size[2]/2.0
target_pose.pose.orientation.w =1.0

scene.add_box(target_id, target_pose, target_size)

self.setColor(target_id, 0.9, 0.9, 0, 1.0)
1
2
3
4
5
6
7
8
9
10
11

设置目标物体的放置位置
创建一个place，并准备放置目标物体的位置，代码如下：

place_pose = PoseStamped()
place_pose.header.frame_id = REFERENCE_FRAME
place_pose.pose.position.x = 0.32
place_pose.pose.position.y = -0.2
place_pose.pose.position.z = table_ground + table_size[2] + target_size[2]/2.0
place_pose.pose.orientation.w = 1.0
1
2
3
4
5
6

生成抓取姿态
生成抓取姿态的代码如下：

grasp_pose = target_pose
grasps = self.make_grasps(grasp_pose, [target_id]) 
for grasp in grasps: 
	self.gripper_pose_pub.publish(grasp.grasp_pose)
	rospy.sleep(0.2)
1
2
3
4
5

在抓去之前，需要确认目标位姿是否存在正确的抓取姿态。在pick时，目标物体的抓取位置就是物体摆放的位置，然后使用make_grasps()函数生成抓取姿态的列表，并将抓取姿态的消息发布，显示在rviz中，代码如下：

def make_grasps(self, initial_pose_stamped, allowed_touch_objects):
	# 初始化抓取姿态对象 
	g = Grasp()
	# 创建夹爪张开、闭合的姿态 
	g.pre_grasp_posture = self.make_gripper_posture(GRIPPER_OPEN)
	g.grasp_posture = self.make_gripper_posture(GRIPPER_CLOSED)
	# 设置期望的夹爪靠近、撤离目标的参数
	g.pre_grasp_approach = self.make_gripper_translation(0.01, 0.1, [1.0, 0.0, 0.0])
	g.post_grasp_retreat = self.make_gripper_translation(0.1, 0.15, [0.0, -1.0, 1.0])
	# 设置抓取姿态
	g.grasp_pose = initial_pose_stamped 
	# 需要尝试改变姿态的数据列表
	pitch_vals = [0, 0.1, -0.1, 0.2, -0.2, 0.3, -0.3]
	yaw_vals = [0]
	# 抓取姿态的列表
	grasps = []
	# 改变姿态，生成抓取动作 
	for y in yaw_vals:
		for p in pitch_vals:
			# 欧拉角到四元数的转换
			q = quaternion _ from _ euler (0, p , y )
			# 设置抓取的姿态
			g.grasp_pose.pose.orientation.x = q[0]
			g.grasp_pose.pose.orientation.y = q[1]
			g.grasp_pose.pose.orientation.z = q[2]
			g.grasp_pose.pose.orientation.w = q[3]
			# 设置抓取的唯一 id 号
			g.id = str(len(grasps))
			# 设置允许接触的物体
			g.allowed_touch_objects = allowed_touch_objects
			# 将本次规划的抓取放入抓取列表中
			grasps.append(deepcopy(g))
	# 返回抓取列表
	return grasps
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

make_grasps()函数通过pitch角度的变化得到不同的抓取姿态。
pick
设置pick时的代码如下：

while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts:
	n_attempts += 1
	rospy.loginfo("Pick attempt: " + str(n_attempts))
	result = arm.pick(target_id, grasps)
	rospy.sleep(0.2)
1
2
3
4
5

接下来机器人就可以尝试 pick 目标物体了。针对不同的抓取姿态，如果无法求解运动学逆解或者规划轨迹会发生碰撞，pick 的运动规划就会出错，因此例程设置重新尝试规划的次数。如果规划成功，则 pick()会控制机器人按照规划轨迹运动。

place
如果 pick 阶段的运动控制没有问题，那我们的工作已经完成了一半。接下来的任务是 place ，将目标物体放置到指定位置，代码如下：

if result == MoveItErrorCodes.SUCCESS:
	result = None
	n_attempts = 0
	# 生成放置姿态 
	places = self.make_places(place_pose)
	# 重复尝试放置，直到成功或者最多尝试次数
	while result != MoveItErrorCodes.SUCCESS and n _ attempts < max_place_attempts:
		n _ attempts += 1
		rospy.loginfo("Placeattempt: " + str(n_attempts))
		for place in places:
			result = arm.place(target_id, place)
			if result == MoveItErrorCodes.SUCCESS:
				break
		rospy.sleep(0.2)
1
2
3
4
5
6
7
8
9
10
11
12
13
14

place 与 pick 的原理是一致的，同样需要使用 make_places() 生成一个可能的夹爪放置姿态列表。然后根据这些可能的放置姿态尝试规划 place 操作的轨迹，规划成功后就可以控制机器人运动了，代码如下：

def make_places(self, init_pose)
	# 初始化放置抓取物体的位置
	place = PoseStamped()
	# 设置放置抓取物体的位置
	place = init_pose
	# 定义 x 方向上用于尝试放置物体的偏移参数
	x_vals = [0, 0.005, 0.01, 0.015, -0.005, -0.01, -0.0151]
	# 定义 y 方向上用于尝试放置物体的偏移参数
	y_vals = [0, 0.005, 0.01, 0.015, -0.005, -0.01, -0.015]
	pitch_vals = [0]
	# 定义用于尝试放置物体的偏航角参数
	yaw_vals = [0]
	# 定义放置物体的姿态列表
	places = []
	# 生成每个角度和偏移方向上的抓取姿态
	for y in yaw_vals:
		for p in pitch_vals:
			for y in y_vals:
				for x in x_vals:
					place.pose.position.x = init_pose.pose.position.x + x
					place.pose.position.y = init_pose.pose.position.y + y
					# 欧拉角到四元数的转换
					q = quaternion_from_euler(0, p, y)
					# 欧拉角到四元数的转换
					place.pose.orientation.x = q[0] 
					place.pose.orientation.y = q[1] 
					place.pose.orientation.z = q[2] 
					place.pose.orientation.w = q[3]
					# 将该放置姿态加入列表
					places.append(deepcopy(place))
	# 返回放置物体的姿态列表
	return places
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

make_places() 和 make_grasps() 的实现原理相同，都是通过设定的方向偏移、旋转列表生成多个可能的终端姿态

通过以上例程，我们就实现了一个简单的 pick and place 应用。

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/小惠珠哦/article/detail/864893