笛卡尔空间运动

笛卡尔路径约束，路径点之间的路径形状是一条直线

moveit_cartesian_demo.py

解析Python文件

1、导入相关库

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

rospy: ROS的Python客户端库，用于与ROS通信。

moveit_commander: MoveIt!库的Python接口，提供了控制机械臂的功能。

MoveGroupCommander: 这是 MoveIt! 中用于控制机械臂的核心类。

Pose: 用于定义位置和姿态的数据结构。

deepcopy: 用于复制对象，确保在修改后不影响原始对象。

2、初始化类

class MoveItCartesianDemo:
def __init__(self):
    moveit_commander.roscpp_initialize(sys.argv)
    rospy.init_node('moveit_cartesian_demo', anonymous=True)
    cartesian = rospy.get_param('~cartesian', True)
    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)
    end_effector_link = arm.get_end_effector_link()

moveit_commander.roscpp_initialize(sys.argv): 初始化 MoveIt! 库。
rospy.init_node(‘moveit_cartesian_demo’, anonymous=True): 初始化ROS节点。
cartesian = rospy.get_param(’~cartesian’, True): 获取ROS参数（是否使用笛卡尔空间路径规划）。
MoveGroupCommander(‘manipulator’): 初始化控制机械臂（名为 manipulator）的 MoveGroupCommander 对象。
设置一些控制参数：
allow_replanning(True): 如果运动规划失败，允许重新规划。
set_pose_reference_frame(‘base_link’): 设置坐标系参考框架为 base_link。
set_goal_position_tolerance(0.001) 和 set_goal_orientation_tolerance(0.001): 设置位置和姿态的容差。
set_max_acceleration_scaling_factor(0.5) 和 set_max_velocity_scaling_factor(0.5): 设置机械臂的最大加速度和速度的比例。
end_effector_link = arm.get_end_effector_link(): 获取机械臂末端执行器的链接名称。

3、控制机械臂返回初始位置

arm.set_named_target('home')
arm.go()
rospy.sleep(1)

set_named_target(‘home’): 机械臂回到名为 home 的预设位置。
arm.go(): 执行运动。
rospy.sleep(1): 暂停1秒，确保机械臂完成运动。

4、获取当前位姿，并构建笛卡尔路径

start_pose = arm.get_current_pose(end_effector_link).pose
waypoints = []
if cartesian:
waypoints.append(start_pose)

获取机械臂当前末端执行器的位姿（位置和姿态）。
如果使用笛卡尔路径，初始化路径点列表 waypoints 并将当前位姿添加到列表中。

5、定义一系列路点（目标位置）

wpose = deepcopy(start_pose)
wpose.position.z -= 0.2
if cartesian:
waypoints.append(deepcopy(wpose))
else:
arm.set_pose_target(wpose)
arm.go()
rospy.sleep(1)

wpose.position.z -= 0.2: 修改目标位姿，向下移动0.2米。
如果使用笛卡尔路径，将修改后的位姿添加到路径点列表中。
如果不使用笛卡尔路径，直接将位姿设置为目标位置，并执行运动。
接下来的几行代码类似，定义了不同的路径点，并决定是否使用笛卡尔路径或普通路径控制机械臂。

6、笛卡尔路径规划

if cartesian:
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,
        eef_step=0.01,
    )
    attempts += 1
    if attempts % 10 == 0:
        rospy.loginfo("Still trying after " + str(attempts) + " attempts...")
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.")

如果 cartesian 为 True，程序将使用笛卡尔空间进行路径规划。
arm.set_start_state_to_current_state(): 设置当前状态作为起始状态。
arm.compute_cartesian_path(waypoints, eef_step=0.01): 计算笛卡尔路径，eef_step 是每次移动的步长。
通过循环和最大尝试次数限制（maxtries）来尝试生成完整的路径。
如果路径规划成功，执行路径，否则输出失败信息。

7、回到初始化位置并关闭程序

arm.set_named_target('home')
arm.go()
rospy.sleep(1)
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)

将机械臂返回到初始化位置 home。
moveit_commander.roscpp_shutdown(): 关闭 MoveIt! 控制。
moveit_commander.os._exit(0): 正常退出程序。

总结

这段代码演示了如何通过 MoveIt! 实现机械臂的控制和路径规划。它包含了：

初始化 MoveIt! 和 ROS。

设置机械臂的目标、容差、速度等参数。

支持笛卡尔空间路径规划，通过多个路点规划路径。

尝试规划并执行路径，如果失败会重新尝试。

最后，控制机械臂回到初始位置并退出。

代码演示

roslaunch probot_anno_moveit_config demo.launch
rosrun probot_demo moveit_cartesian_demo.py _cartesian:=True

rosrun probot_demo moveit_cartesian_demo.py _cartesian:=False