关节空间运动

点到点运动：不需要在笛卡尔空间规划运动轨迹，机器人各个关节运动不需要联动

一、正向运动学

先解析一下需要运行的Python文件

moveit_fk_demo.py

1、文件头部（告诉系统用 Python3 解释器来执行此文件。）

#!/usr/bin/env python3

2、导入模块

import rospy,sys
import moveit_commander

rospy：ROS提供的Python库，用于节点管理、发布/订阅消息等。
sys:Python标准库，用来处理命令行参数。
moveit_commander：MoveIt! 的Python API，用于控制机械臂运动。

3、定义类MoveItFkDemo

class MoveItFkDemo:

定义了一个名为 MoveItFkDemo 的类，封装整个机械臂运动流程。

4、初始化函数 init

def __init__(self):

当类实例化时，这段逻辑会自动运行，做一系列初始化设置。

(1) 初始化 MoveIt

moveit_commander.roscpp_initialize(sys.argv)

初始化 MoveIt! 的 Python 接口。
sys.argv 是启动脚本时的参数列表，ROS需要这个来处理一些系统参数。

(2) 初始化 ROS 节点

rospy.init_node('moveit_fk_demo', anonymous=True)

初始化一个 ROS 节点，名字叫 moveit_fk_demo。
anonymous=True 保证每次运行时节点名唯一，避免冲突。

(3) 定义操作的机械臂组

arm = moveit_commander.MoveGroupCommander('manipulator')

创建一个操作对象 arm，对应 MoveIt 配置文件中叫 ‘manipulator’ 的 group。

(4) 设置参数

arm.set_goal_joint_tolerance(0.001)
arm.set_max_acceleration_scaling_factor(0.5)
arm.set_max_velocity_scaling_factor(0.5)

设定关节目标允许的误差：1毫米（非常精确）。
最大加速度和最大速度分别缩小为50%，保证运动更平稳，不至于太快导致机械臂震动。

(5) 回到初始位置 “home”

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

set_named_target(‘home’)：设置一个预先定义好的姿态（home位）。
go()：开始运动到目标。
rospy.sleep(1)：等待一秒，确保动作完成后再继续。

(6) 设置具体关节角度目标

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)

joint_positions 是一个长度为 6 的列表，对应机械臂6个关节的角度值（单位是弧度 rad）。

set_joint_value_target：将这些角度作为目标。

go()：控制机械臂运动到新位置。

sleep(1)：等1秒，等待完成。

(7) 再次回到 “home”

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

和之前一样，运动回初始姿态。

(8) 退出清理

moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)

roscpp_shutdown()：释放资源，关闭 MoveIt。

os._exit(0)：强制退出 Python 程序。

6、程序入口

if __name__ == "__main__":
try:
    MoveItFkDemo()
except rospy.ROSInterruptException:
    pass

if name == “main”：保证这段代码作为脚本直接运行时才执行。

try-except 捕捉 ROS 中断异常（比如按 Ctrl+C 退出时）。

核心流程：

初始化 MoveIt 和 ROS 节点

创建操作机械臂的接口

控制机械臂回到 home

控制机械臂移动到一个指定姿态

再回 home

释放资源退出

代码执行

roslaunch probot_anno_moveit_config demo.launch

rosrun probot_demo moveit_fk_demo.py

可以看见机械臂从home位置运动到目标位置后又回到home位置

二、逆向运动学

先解析一下需要运行的Python文件

moveit_ik_demo.py

1、文件头部

#!/usr/bin/env python3

指示操作系统使用 Python 3 来执行这个脚本。

2、导入必要的模块

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

rospy：是 ROS 的 Python 库，用于编写 ROS 节点。

sys：Python 的系统模块，通常用来获取命令行参数等。

moveit_commander：MoveIt! 的 Python 接口库，用于控制机器人。

PoseStamped 和 Pose：geometry_msgs 中的消息类型，用于表示位姿（位置和姿态）。

3、MoveItIkDemo 类

class MoveItIkDemo:
def __init__(self):

定义了一个 MoveItIkDemo 类，用于示范 MoveIt! 逆向运动学（IK）控制。

4、初始化和设置 MoveIt! API

moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_ik_demo')

moveit_commander.roscpp_initialize(sys.argv)：初始化 MoveIt! 的 C++ API。

rospy.init_node(‘moveit_ik_demo’)：初始化一个名为 moveit_ik_demo 的 ROS 节点。

5、控制机械臂

arm = moveit_commander.MoveGroupCommander('manipulator')

MoveGroupCommander 用于控制一个指定的机器人组（在此为 manipulator）。manipulator 是一个 MoveIt! 配置中指定的组名称，表示机械臂的运动控制。

end_effector_link = arm.get_end_effector_link()

获取机械臂末端执行器（即末端工具）链接的名称。这个名称通常会用于设置末端执行器的目标位姿。

reference_frame = 'base_link'
arm.set_pose_reference_frame(reference_frame)

设置参考坐标系为 base_link，即机械臂的基座坐标系。

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)

设置运动的最大加速度和最大速度的缩放因子。值越小，速度和加速度越慢。

6、机械臂到达初始化位置

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

7、设置目标位姿并进行运动

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.70692
target_pose.pose.orientation.y = 0.0
target_pose.pose.orientation.z = 0.0
target_pose.pose.orientation.w = 0.70729

创建目标位姿 target_pose，使用 PoseStamped 消息类型，其中 frame_id 设置为参考坐标系 base_link，并设置目标位置和姿态（使用四元数）。

arm.set_start_state_to_current_state()
arm.set_pose_target(target_pose, end_effector_link)

set_start_state_to_current_state()：将机械臂的当前状态设置为起始状态。

set_pose_target(target_pose, end_effector_link)：设置目标位姿为 target_pose，并指定末端执行器链接。

8、规划和执行运动

traj = arm.plan()
arm.execute(traj)
rospy.sleep(1)

arm.plan()：计算从当前状态到目标状态的运动规划。

arm.execute(traj)：执行计算出的运动轨迹。

9、回到初始化位置并退出

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

10、程序入口

if __name__ == "__main__":
MoveItIkDemo()

这是 Python 中的标准入口点。当文件作为脚本执行时，将创建 MoveItIkDemo 类的实例，启动机械臂运动。

代码执行

roslaunch probot_anno_moveit_config demo.launch

rosrun probot_demo moveit_fk_demo.py