Python中利用SimpleActionClient()执行轨迹控制

在Python中，可以使用SimpleActionClient来执行轨迹控制。SimpleActionClient是roslibpy库中的一个类，它允许用户向运动控制器发送轨迹动作请求，并接收运动控制器的响应。

下面是一个使用SimpleActionClient执行轨迹控制的示例：

import rospy
from roslibpy import SimpleActionClient
from control_msgs.msg import FollowJointTrajectoryAction, FollowJointTrajectoryGoal
from trajectory_msgs.msg import JointTrajectoryPoint

# 初始化节点
rospy.init_node('traj_control_example')

# 创建SimpleActionClient
client = SimpleActionClient('/joint_trajectory_action', FollowJointTrajectoryAction)

# 等待服务器响应
client.wait_for_server()

# 创建轨迹控制目标
goal = FollowJointTrajectoryGoal()

# 设置目标轨迹的关节名称
goal.trajectory.joint_names = ['joint1', 'joint2']

# 创建轨迹点
point1 = JointTrajectoryPoint()
point1.positions = [0, 0]  # 设置      个轨迹点的关节位置
point1.time_from_start = rospy.Duration(1)  # 设置      个轨迹点的运动时间

point2 = JointTrajectoryPoint()
point2.positions = [1, 1]  # 设置第二个轨迹点的关节位置
point2.time_from_start = rospy.Duration(2)  # 设置第二个轨迹点的运动时间

# 将轨迹点添加到目标轨迹中
goal.trajectory.points = [point1, point2]

# 发送轨迹控制目标给运动控制器
client.send_goal(goal)

# 等待运动控制器执行轨迹
client.wait_for_result()

# 获取运动控制器的执行结果
result = client.get_result()

# 打印执行结果
print(result)

在这个示例中，先初始化一个ros节点，然后创建一个SimpleActionClient对象来连接轨迹控制的动作服务器。接下来，我们设置一个相应的运动轨迹的目标，包括定义关节名称、设置轨迹点以及关节位置和运动时间。

然后，我们使用send_goal()方法将目标发送给运动控制器，然后使用wait_for_result()方法等待运动控制器执行轨迹。最后，使用get_result()方法获取运动控制器的执行结果，并打印出来。

请确保在运行这个示例之前，相应的运动控制器和动作服务器已经启动并正确配置。

这个示例只是一个基本的演示，可根据实际需求进行修改和扩展。希望这个例子能帮助到你！