1. pick and place

本博客讲介绍一个更加复杂的机器人应用： pick and place
简单来讲，这个应用就是让机器人用夹爪将工作空间内的某个物体夹起来，然后将该物体放到目标位置

类似的功能在机器人的实际生产应用中使用非常广泛
例如码垛、搬运、挑拣等工作

Movelt!已经为这类应用准备了丰富的接口，无论是夹具控制还是运动控制，都可以快速搭建功能原型

接下来将快速组建一个简单的pick and place应用
在这个应用例程中，假设已知物体的位置，Movelt!需要控制机器人去抓取物体并放到指定位置

2. 启动机械臂

第一步，启动arm_planning.launch文件

$ roslaunch hharm_planning arm_planning.launch

3. 创建抓取的目标物体

场景和 ROS笔记（36） 避障运动规划 类似
只需要用相同的方法增加一个用来抓取的目标物体，并将其设置为与其他物体都不同的黄色

# 设置目标物体的尺寸
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)

4. 设置目标物体的放置位置

然后创建一个 place ，并准备放置目标物体的位置

 # 设置一个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

4. 设置抓取姿态

生成抓取姿态

# 将目标位置设置为机器人的抓取目标位置
grasp_pose = target_pose
        
# 生成抓取姿态
grasps = self.make_grasps(grasp_pose, [target_id])

# 将抓取姿态发布，可以在rviz中显示
for grasp in grasps:
    self.gripper_pose_pub.publish(grasp.grasp_pose)
    rospy.sleep(0.2)

在抓取之前，需要确认目标位姿是否存在正确的抓取姿态
在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

make_grasps函数通过pich角度的变化得到不同的抓取姿态

5. 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)

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

6. place

如果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("Place attempt: " +  str(n_attempts))
        for place in places:
            result = arm.place(target_id, place)
            if result == MoveItErrorCodes.SUCCESS:
                break
        rospy.sleep(0.2)
        
    if result != MoveItErrorCodes.SUCCESS:
        rospy.loginfo("Place operation failed after " + str(n_attempts) + " attempts.")
else:
    rospy.loginfo("Pick operation failed after " + str(n_attempts) + " attempts.")

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.015]
    
    # 定义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

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

7. 启动抓取和放置

运行moveit_pick_and_place_demo.py文件启动抓取和放置

$ rosrun hharm_planning moveit_pick_and_place_demo.py

可以看到，在之前避障规划的场景上两个盒子之间又加入了一个用于抓取的目标物体
机器人首先运行到目标物体附近进行pick操作
抓取成功后机器人完成运动并开始place操作，将物体放置到指定位置

界面中夹爪上的坐标轴表示机械臂终端夹爪的抓取姿态
如果 pick 或 place 多次尝试依然失败
可能是由于机械臂抓取姿态的运动学逆解无法求解或者规划的路径会与场景物体发生碰撞
可以尝试重启例程，或者修改场景的相对位置，或者修改srdf文件修改碰撞的配置

参考：

ROS官方wiki
古月居

相关推荐：

ROS笔记（36） 避障运动规划
ROS笔记（35） 笛卡尔运动规划
ROS笔记（34） 工作空间规划
ROS笔记（33） 关节空间规划
ROS笔记（32） MoveIt!关节控制器

谢谢！