正向运动学

初始化指定要运动的move_group（arm，gripper）
设置每个move_group距离目标的允许误差值
设置move_group的初始位置
使用move_group中每个joint的旋转角度作为目标值

执行动作

class MoveItFkDemo:def __init__(self) -> None:# 初始化move_group的APImoveit_commander.roscpp_initialize(sys.argv)# 初始化ROS节点rospy.init_node('moveit_fk_demo', anonymous=True)# 初始化需要使用move group控制的机械臂中的arm grouparm = moveit_commander.MoveGroupCommander('arm')# 初始化需要使用move group控制的机械臂中的gripper groupgripper = moveit_commander.MoveGroupCommander('gripper')# 设置机械臂和夹爪的允许误差值arm.set_goal_joint_tolerance(0.001)gripper.set_goal_joint_tolerance(0.001)# 控制机械臂先回到初始化位置arm.set_named_target('home')arm.go()rospy.sleep(2)# 设置夹爪的目标位置，并控制夹爪运动gripper.set_joint_value_target([0.01])gripper.go()rospy.sleep(1)# 设置机械臂的目标位置，使用六轴的位置数据进行描述（单位：弧度）joint_positions = [-0.0867, -1.274, 0.02832, 0.0820, -1.273, -0.003]arm.set_joint_value_target(joint_positions)# 控制机械臂完成运动arm.go()rospy.sleep(1)# 关闭并退出moveitmoveit_commander.roscpp_shutdown()moveit_commander.os._exit(0)

逆向运动学

初始化指定要运动的move_group（arm，gripper）
获取机械臂末端link
设置需要到达的目标的参考系
设置每个move_group距离目标的允许误差值，速度加速度，是否允许重新规划
设置move_group的初始位置
设置当前的状态为运动初始状态
设置末端link目标位姿，使用“target_pose = PoseStamped()“指定目标值为坐标

plan & execute

class MoveItIkDemo:def __init__(self):# 初始化move_group的APImoveit_commander.roscpp_initialize(sys.argv)# 初始化ROS节点rospy.init_node('moveit_ik_demo')# 初始化需要使用move group控制的机械臂中的arm grouparm = 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.01)# 设置允许的最大速度和加速度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_frametarget_pose.header.stamp = rospy.Time.now()     target_pose.pose.position.x = 0.2593target_pose.pose.position.y = 0.0636target_pose.pose.position.z = 0.1787target_pose.pose.orientation.x = 0.70692target_pose.pose.orientation.y = 0.0target_pose.pose.orientation.z = 0.0target_pose.pose.orientation.w = 0.70729# 设置机器臂当前的状态作为运动初始状态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)# 控制机械臂回到初始化位置arm.set_named_target('home')arm.go()# 关闭并退出moveitmoveit_commander.roscpp_shutdown()moveit_commander.os._exit(0)

笛卡尔空间规划

关节空间
- 对于一个具有n个自由度的的操作臂来说，它的所有连杆位置可由一组n个关节变量来确定。这样的一组变量通常被称为n x 1的关节矢量。所有关节矢量组成的空间称为关节空间。
笛卡尔空间
- 当位置是在空间相互正交的轴上的测量，且姿态是按照空间描述章节中任一一种规定测量的时候，我们称这个空间为笛卡尔空间，有时也称为任务空间或者操作空间。简单地理解成在空间直角坐标系。
- 可以应用于在空间中机械臂末端规划直线

初始化指定要运动的move_group（arm，gripper）
获取机械臂末端link
设置需要到达的目标的参考系
设置每个move_group距离目标的允许误差值，速度加速度，是否允许重新规划
设置move_group的初始位置
设置当前的状态为运动初始状态
设置一系列的终端姿态waypoints
计算路径，返回waypoints覆盖率 1表示所有waypoints都可覆盖

执行plan

class MoveItCartesianDemo:def __init__(self):# 初始化move_group的APImoveit_commander.roscpp_initialize(sys.argv)# 初始化ROS节点rospy.init_node('moveit_cartesian_demo', anonymous=True)# 是否需要使用笛卡尔空间的运动规划cartesian = rospy.get_param('~cartesian', True)# 初始化需要使用move group控制的机械臂中的arm grouparm = MoveGroupCommander('arm')# 当运动规划失败后，允许重新规划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).poseprint("start_pose",start_pose)# 初始化路点列表waypoints = []# 将初始位姿加入路点列表if cartesian:waypoints.append(start_pose)# 设置路点数据，并加入路点列表wpose = deepcopy(start_pose)wpose.position.z -= 0.2if cartesian:waypoints.append(deepcopy(wpose))else:arm.set_pose_target(wpose)arm.go()rospy.sleep(1)wpose.position.x += 0.15if cartesian:waypoints.append(deepcopy(wpose))else:arm.set_pose_target(wpose)arm.go()rospy.sleep(1)wpose.position.y += 0.1if cartesian:waypoints.append(deepcopy(wpose))else:arm.set_pose_target(wpose)arm.go()rospy.sleep(1)wpose.position.x -= 0.15wpose.position.y -= 0.1if cartesian:waypoints.append(deepcopy(wpose))else:arm.set_pose_target(wpose)arm.go()rospy.sleep(1)print("waypoints",waypoints)if cartesian:fraction = 0.0   #路径规划覆盖率maxtries = 100   #最大尝试规划次数attempts = 0     #已经尝试规划次数# 设置机器臂当前的状态作为运动初始状态arm.set_start_state_to_current_state()# 尝试规划一条笛卡尔空间下的路径，依次通过所有路点while fraction < 0.5 and attempts < maxtries:(plan, fraction) = arm.compute_cartesian_path (waypoints,   # waypoint poses，路点列表0.01,        # eef_step，终端步进值0.0,         # jump_threshold，跳跃阈值True)        # avoid_collisions，避障规划# print("plan",plan)print("fraction",fraction)# 尝试次数累加attempts += 1# 打印运动规划进程if attempts % 10 == 0:rospy.loginfo("Still trying after " + str(attempts) + " attempts...")# 如果路径规划成功（覆盖率100%）,则开始控制机械臂运动if fraction >= 0.5: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)# 关闭并退出moveitmoveit_commander.roscpp_shutdown()moveit_commander.os._exit(0)

pick & place & 避障

创建场景

初始化场景对象和创建一个场景发布者
设置场景物体的id并移除场景中之前程序可能残留的物体
设置场景中物体的形状大小颜色和相对参考坐标系的位置坐标并发布

# 初始化场景对象scene = PlanningSceneInterface()# 创建一个发布场景变化信息的发布者self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10)# 设置场景物体的名称 table_id = 'table'box1_id = 'box1'box2_id = 'box2'target_id = 'target'# 移除场景中之前运行残留的物体scene.remove_world_object(table_id)scene.remove_world_object(box1_id)scene.remove_world_object(box2_id)scene.remove_world_object(target_id)# 移除场景中之前与机器臂绑定的物体scene.remove_attached_object(GRIPPER_FRAME, target_id)  rospy.sleep(1)# 设置桌面的高度table_ground = 0.2# 设置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_FRAMEtable_pose.pose.position.x = 0.35table_pose.pose.position.y = 0.0table_pose.pose.position.z = table_ground + table_size[2] / 2.0table_pose.pose.orientation.w = 1.0scene.add_box(table_id, table_pose, table_size)box1_pose = PoseStamped()box1_pose.header.frame_id = REFERENCE_FRAMEbox1_pose.pose.position.x = 0.31box1_pose.pose.position.y = -0.1box1_pose.pose.position.z = table_ground + table_size[2] + box1_size[2] / 2.0box1_pose.pose.orientation.w = 1.0   scene.add_box(box1_id, box1_pose, box1_size)box2_pose = PoseStamped()box2_pose.header.frame_id = REFERENCE_FRAMEbox2_pose.pose.position.x = 0.29box2_pose.pose.position.y = 0.13box2_pose.pose.position.z = table_ground + table_size[2] + box2_size[2] / 2.0box2_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)# 设置目标物体的尺寸target_size = [0.02, 0.01, 0.12]# 设置目标物体的位置，位于桌面之上两个盒子之间target_pose = PoseStamped()target_pose.header.frame_id = REFERENCE_FRAMEtarget_pose.pose.position.x = 0.32target_pose.pose.position.y = 0.0target_pose.pose.position.z = table_ground + table_size[2] + target_size[2] / 2.0target_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)# 将场景中的颜色设置发布self.sendColors()# 设置支持的表面arm.set_support_surface_name(table_id)

pick & place

初始化指定要运动的move_group（arm，gripper）
获取机械臂末端link
设置需要到达的目标的参考系
设置每个move_group距离目标的允许误差值，速度加速度，是否允许重新规划
设置move_group的初始位置
设置当前的状态为运动初始状态

设置pick

初始化抓取姿态对象
创建夹爪张开闭合的姿态

   # 创建夹爪的姿态数据JointTrajectorydef make_gripper_posture(self, joint_positions):# 初始化夹爪的关节运动轨迹t = JointTrajectory()# 设置夹爪的关节名称t.joint_names = ['finger_joint1']# 初始化关节轨迹点tp = JointTrajectoryPoint()# 将输入的关节位置作为一个目标轨迹点tp.positions = joint_positions# 设置夹爪的力度tp.effort = [1.0]# 设置运动时间tp.time_from_start = rospy.Duration(1.0)# 将目标轨迹点加入到运动轨迹中t.points.append(tp)# 返回夹爪的关节运动轨迹return t

设置期望的夹爪靠近、撤离目标的参数

    # 使用给定向量创建夹爪的translation结构def make_gripper_translation(self, min_dist, desired, vector):# 初始化translation对象g = GripperTranslation()# 设机械臂末端的方向向量g.direction.vector.x = vector[0]g.direction.vector.y = vector[1]g.direction.vector.z = vector[2]# 设置参考坐标系g.direction.header.frame_id = GRIPPER_FRAME# 设置最小和期望的距离g.min_distance = min_distg.desired_distance = desiredreturn g

设置抓取姿态
设置末端抓取时可供调整的pitch姿态

        # 设置抓取姿态g.grasp_pose = initial_pose_stamped# 需要尝试改变姿态的数据列表# 抓取时末端pitch姿态pitch_vals = [0, 0.1, -0.1, 0.2, -0.2, 0.3, -0.3]# pitch_vals = [1,2,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

返回抓取姿态列表并发布（发布非必要）
调用arm.pick，并传入抓取目标的物体id和抓取姿态列表

place

定义放置的位置
设置放置位置x,y方向的可偏移的位置和pitch，yaw
遍历每一个可偏移位置并生成不同的位置点并返回

   # 创建一个允许的放置姿态列表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 + xplace.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

执行arm.place(target_id, place)

完整代码

GROUP_NAME_ARM = 'arm'
GROUP_NAME_GRIPPER = 'gripper'GRIPPER_FRAME = 'grasping_frame'GRIPPER_OPEN = [0.004]
GRIPPER_CLOSED = [0.01]REFERENCE_FRAME = 'base_link'class MoveItPickAndPlaceDemo:def __init__(self):# 初始化move_group的APImoveit_commander.roscpp_initialize(sys.argv)# 初始化ROS节点rospy.init_node('moveit_pick_and_place_demo')# 初始化场景对象scene = PlanningSceneInterface()# 创建一个发布场景变化信息的发布者self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10)# 创建一个发布抓取姿态的发布者self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped, queue_size=10)# 创建一个存储物体颜色的字典对象self.colors = dict()# 初始化需要使用move group控制的机械臂中的arm grouparm = MoveGroupCommander(GROUP_NAME_ARM)# 初始化需要使用move group控制的机械臂中的gripper groupgripper = MoveGroupCommander(GROUP_NAME_GRIPPER)# 获取终端link的名称end_effector_link = arm.get_end_effector_link()# 设置位置(单位：米)和姿态（单位：弧度）的允许误差arm.set_goal_position_tolerance(0.05)arm.set_goal_orientation_tolerance(0.1)# 当运动规划失败后，允许重新规划arm.allow_replanning(True)# 设置目标位置所使用的参考坐标系arm.set_pose_reference_frame(REFERENCE_FRAME)# 设置每次运动规划的时间限制：5sarm.set_planning_time(5)# 设置pick和place阶段的最大尝试次数max_pick_attempts = 5max_place_attempts = 5rospy.sleep(2)# 设置场景物体的名称 table_id = 'table'box1_id = 'box1'box2_id = 'box2'target_id = 'target'# 移除场景中之前运行残留的物体scene.remove_world_object(table_id)scene.remove_world_object(box1_id)scene.remove_world_object(box2_id)scene.remove_world_object(target_id)# 移除场景中之前与机器臂绑定的物体scene.remove_attached_object(GRIPPER_FRAME, target_id)  rospy.sleep(1)# 控制机械臂先回到初始化位置arm.set_named_target('home')arm.go()# 控制夹爪张开gripper.set_joint_value_target(GRIPPER_OPEN)gripper.go()rospy.sleep(1)# 设置桌面的高度table_ground = 0.2# 设置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_FRAMEtable_pose.pose.position.x = 0.35table_pose.pose.position.y = 0.0table_pose.pose.position.z = table_ground + table_size[2] / 2.0table_pose.pose.orientation.w = 1.0scene.add_box(table_id, table_pose, table_size)box1_pose = PoseStamped()box1_pose.header.frame_id = REFERENCE_FRAMEbox1_pose.pose.position.x = 0.31box1_pose.pose.position.y = -0.1box1_pose.pose.position.z = table_ground + table_size[2] + box1_size[2] / 2.0box1_pose.pose.orientation.w = 1.0   scene.add_box(box1_id, box1_pose, box1_size)box2_pose = PoseStamped()box2_pose.header.frame_id = REFERENCE_FRAMEbox2_pose.pose.position.x = 0.29box2_pose.pose.position.y = 0.13box2_pose.pose.position.z = table_ground + table_size[2] + box2_size[2] / 2.0box2_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)# 设置目标物体的尺寸target_size = [0.02, 0.01, 0.12]# 设置目标物体的位置，位于桌面之上两个盒子之间target_pose = PoseStamped()target_pose.header.frame_id = REFERENCE_FRAMEtarget_pose.pose.position.x = 0.32target_pose.pose.position.y = 0.0target_pose.pose.position.z = table_ground + table_size[2] + target_size[2] / 2.0target_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)# 将场景中的颜色设置发布self.sendColors()# 设置支持的外观arm.set_support_surface_name(table_id)# 设置一个place阶段需要放置物体的目标位置place_pose = PoseStamped()place_pose.header.frame_id = REFERENCE_FRAMEplace_pose.pose.position.x = 0.32place_pose.pose.position.y = -0.2place_pose.pose.position.z = table_ground + table_size[2] + target_size[2] / 2.0place_pose.pose.orientation.w = 1.0# 将目标位置设置为机器人的抓取目标位置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)# 追踪抓取成功与否，以及抓取的尝试次数result = Nonen_attempts = 0# 重复尝试抓取，直道成功或者超多最大尝试次数while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts:n_attempts += 1rospy.loginfo("Pick attempt: " +  str(n_attempts))result = arm.pick(target_id, grasps)rospy.sleep(0.2)# 如果pick成功，则进入place阶段 if result == MoveItErrorCodes.SUCCESS:result = Nonen_attempts = 0# 生成放置姿态places = self.make_places(place_pose)# 重复尝试放置，直道成功或者超多最大尝试次数while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts:n_attempts += 1rospy.loginfo("Place attempt: " +  str(n_attempts))for place in places:result = arm.place(target_id, place)if result == MoveItErrorCodes.SUCCESS:breakrospy.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.")# 控制机械臂回到初始化位置arm.set_named_target('home')arm.go()# 控制夹爪回到张开的状态gripper.set_joint_value_target(GRIPPER_OPEN)gripper.go()rospy.sleep(1)# 关闭并退出moveitmoveit_commander.roscpp_shutdown()moveit_commander.os._exit(0)# 创建夹爪的姿态数据JointTrajectorydef make_gripper_posture(self, joint_positions):# 初始化夹爪的关节运动轨迹t = JointTrajectory()# 设置夹爪的关节名称t.joint_names = ['finger_joint1']# 初始化关节轨迹点tp = JointTrajectoryPoint()# 将输入的关节位置作为一个目标轨迹点tp.positions = joint_positions# 设置夹爪的力度tp.effort = [1.0]# 设置运动时间tp.time_from_start = rospy.Duration(1.0)# 将目标轨迹点加入到运动轨迹中t.points.append(tp)# 返回夹爪的关节运动轨迹return t# 使用给定向量创建夹爪的translation结构def make_gripper_translation(self, min_dist, desired, vector):# 初始化translation对象g = GripperTranslation()# 设置方向向量g.direction.vector.x = vector[0]g.direction.vector.y = vector[1]g.direction.vector.z = vector[2]# 设置参考坐标系g.direction.header.frame_id = GRIPPER_FRAME# 设置最小和期望的距离g.min_distance = min_distg.desired_distance = desiredreturn g# 创建一个允许的的抓取姿态列表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姿态pitch_vals = [0, 0.1, -0.1, 0.2, -0.2, 0.3, -0.3]# pitch_vals = [1,2,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# 创建一个允许的放置姿态列表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 + xplace.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# 设置场景物体的颜色def setColor(self, name, r, g, b, a = 0.9):# 初始化moveit颜色对象color = ObjectColor()# 设置颜色值color.id = namecolor.color.r = rcolor.color.g = gcolor.color.b = bcolor.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)

（ROS）Moveit编程示例相关推荐

python做运动控制_ROS探索总结-61.MoveIt!编程驾驭机械臂运动控制
ROS探索总结-61.MoveIt!编程驾驭机械臂运动控制说明: 介绍MoveIt!编程驾驭机械臂运动控制正文本讲我们将从以下四个部分进行讲解. 首先来回顾下MoveIt!编程接口的框架. Mo ...
六轴机械臂下位机（arduino）+上位机（ROS+Moveit）---（一）机械臂硬件
六轴机械臂下位机(arduino)+上位机(ROS+Moveit)---(一)机械臂硬件机械部分机械臂制作时的注意点!!!(坑) 零件的3D打印控制器接线问题机械部分六轴机械臂在工业领域的运 ...
Moveit编程——moveit 编程技巧笔记——圆弧轨迹规划+修改轨迹
moveit 编程技巧笔记--圆弧轨迹规划+修改轨迹 1 笛卡尔空间圆弧轨迹规划 2 轨迹重定义(修改moveit生成的轨迹数据) 1 笛卡尔空间圆弧轨迹规划之前学习过笛卡尔空间下轨迹规划API:( ...
python控制机械臂6轴_基于Firmata协议的ROS Moveit六轴机械臂设计
ROS(Robot Operating System,开源机器人操作系统)是目前较为火热的一个一个机器人开发者平台.依赖于强大的资源库和开发者社区,ROS可以说是风靡全球. 在机器人开发的领域,机械臂 ...
ROS moveit 机械臂笛卡尔空间运动
机械臂moveit编程(python) 机械臂在笛卡尔空间的运动只能走点到点的直线运动,通过将位姿点加入waypoints中,机械臂会一次按照waypoints中的唯一依次沿直线运动到下一个点. 程序 ...
ROS moveit 机械臂避障运动规划
机械臂moveit编程(python) moveit默认使用的运动规划库OMPL支持臂章规划,这里选用RRT算法,使用move group中的PlanningSceneInterface()添加障碍物 ...
C语言与OpenCL的编程示例比较
C语言与OpenCL的编程示例比较 OpenCL支持数据并行,任务并行编程,同时支持两种模式的混合.对于同步 OpenCL支持同一工作组内工作项的同步和命令队列中处于同一个上下文中的命令的同步. 在 ...
Win32 API 多线程编程示例 - 窗口版
网上有一些Win32 多线程编程示例,多是控制台版本:下面哥作一个窗口版本: /*-------------------------------------------------bobo, 2020 ...
Linux IPv6 UDP套接字编程示例
udp ipv6套接字编程和ipv4接口类似,参数略有不同,流程都包括创建套接字.绑定地址.发送等. 下面是一个udp ipv6 demo, 包括创建ipv6套接字.绑定地址和发送数据等. 首先先在l ...

（ROS）Moveit编程示例

文章目录

正向运动学

逆向运动学

笛卡尔空间规划

pick & place & 避障

创建场景

pick & place

（ROS）Moveit编程示例相关推荐

最新文章

热门文章