Arbotix关节轨迹控制器的使用
import rospy
import actionlib
from control_msgs.msg import FollowJointTrajectoryAction, FollowJointTrajectoryGoal
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
首先，我们需要导入一些和轨迹相关的消息和动作类型。我们已知FollowJointTrajectoryAction动作和FollowJointTrajectoryGoal消息。这些对象的关键组成部分是用于关节位置、速度、加速度和受到的作用力来定义关节轨迹。因此我们还要导入JointTrajectory和JointTrajectoryPoint两种消息类型。
reset = rospy.get_param('~reset', False)
sync = rospy.get_param('~sync', True)
reset参数允许我们将机械臂或其他规划组移回到其起始位置。sync(synchronize)参数确定我们时同时运行多个规划组的轨迹还是先后运行。我这里只有一个规划组。
arm_joints = ['shoulder_pan_joint',
'shoulder_lift_joint',
'elbow_joint',
'wrist_1_joint',
'wrist_2_joint',
'wrist_3_joint']
这里包含你将要规划的规划组中包含的关节名称。在之后指定轨迹目标时会用到。
if reset:
arm_goal = [0, 0, 0, 0, 0, 0]
else:
arm_goal = [-0.3, -2.0, -1.0, 0.8, 1.0, -0.7]
我们使用关节位置来定义目标位置。关节位置的顺序需要和你之前定义的关节名一一对应。如果命令行里面参数reset设置为true，则目标位置就会被设置回空档位置。
arm_client = actionlib.SimpleActionClient('six_manipulator_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
arm_client.wait_for_server()
创建一个简单的动作客户端，它会连接到关节轨迹动作服务器。这个控制器的命名空间是用action_name参数定义在arbotix控制器的配置文件中。我的文件位置为（myur_dynamixels/config/arbotix/myur_arbotix.yaml），内容如下图所示。
arm_trajectory = JointTrajectory()
arm_trajectory.joint_names = arm_joints
arm_trajectory.points.append(JointTrajectoryPoint())
arm_trajectory.points[0].positions = arm_goal
arm_trajectory.points[0].velocities = [0.0 for i in arm_joints]
arm_trajectory.points[0].accelerations = [0.0 for i in arm_joints]
arm_trajectory.points[0].time_from_start = rospy.Duration(3.0)
实际上，仅用一个点（关节配置）来创建机械臂的轨迹目标，也就是之前在arm_goal 变量里保存的目标位置。换言之，是用轨迹的结束点来定义这段轨迹。可以这么做的原因时，轨迹服务器会在开始配置和我们发送给它的目标配置之间进行插值，添加过度状态的关节配置。
上面创建了一个空的JointTrajectory对象。把机械臂关节名称一一列举并存放在arm_joints变量里，之后填入这条轨迹的关节名称内。由于一个关节轨迹包含一个轨迹点的序列，所以我们创建一个空的JointTrajectoryPoint，之后往里面太难如目标配置。
我们将目标配置设置为我们存放在轨迹点列表里序号为0的位置，设置此时每个关节的速度和加速度为0，因为这是轨迹的终点，机械臂会停下来。
time_from_start设置为3.0，意思是轨迹在开始动作后的0秒时经过这个点，由于这是轨迹的端点，意味着我们希望整条轨迹大约用3秒时间执行完。
arm_goal = FollowJointTrajectoryGoal()
arm_goal.trajectory = arm_trajectory
arm_goal.goal_time_tolerance = rospy.Duration(0.0)
arm_client.send_goal(arm_goal)
if not sync:
arm_client.wait_for_result(rospy.Duration(5.0))
我们创建一个FollowJointTrajectoryGoal消息。然后设置目标的轨迹为刚才创建的机械臂轨迹，如果声明要轨迹按时完成，则设置goal.time.tolerance为0，如果机械臂到达目标位置晚一些也可以接受，可以增加限度。
使用arm_client.send_goal()方法发送实际的轨迹目标到动作服务器。
下面是完整的代码：
import rospy
import actionlib
from control_msgs.msg import FollowJointTrajectoryAction, FollowJointTrajectoryGoal
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
class TrajectoryDemo():
def __init__(self):
rospy.init_node('trajectory_demo')
reset = rospy.get_param('~reset', False)
sync = rospy.get_param('~sync', True)
arm_joints = ['shoulder_pan_joint',
'shoulder_lift_joint',
'elbow_joint',
'wrist_1_joint',
'wrist_2_joint',
'wrist_3_joint']
if reset:
arm_goal = [0, 0, 0, 0, 0, 0]
else:
arm_goal = [-0.3, -2.0, -1.0, 0.8, 1.0, -0.7]
rospy.loginfo('Waiting for myur trajectory controller...')
arm_client = actionlib.SimpleActionClient('six_manipulator_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
arm_client.wait_for_server()
rospy.loginfo('...connected.')
arm_trajectory = JointTrajectory()
arm_trajectory.joint_names = arm_joints
arm_trajectory.points.append(JointTrajectoryPoint())
arm_trajectory.points[0].positions = arm_goal
arm_trajectory.points[0].velocities = [0.0 for i in arm_joints]
arm_trajectory.points[0].accelerations = [0.0 for i in arm_joints]
arm_trajectory.points[0].time_from_start = rospy.Duration(3.0)
rospy.loginfo('Moving the arm to goal position...')
arm_goal = FollowJointTrajectoryGoal()
arm_goal.trajectory = arm_trajectory
arm_goal.goal_time_tolerance = rospy.Duration(0.0)
arm_client.send_goal(arm_goal)
if not sync:
arm_client.wait_for_result(rospy.Duration(5.0))
rospy.loginfo('...done')
if __name__ == '__main__':
try:
TrajectoryDemo()
except rospy.ROSInterruptException:
pass
相关知识
个性化步态轨迹规划及机器人辅助控制研究
路灯控制器
秋冬保养好你的关节 留意关节的非正常响声（附按摩手法）
数字灯光控制器
花园自动灌溉控制器的制作教程
东方2路数字光源控制器
农作物综合控制器 有没有农作物管理的软件
迷迭香的抗炎消肿和减轻关节疼痛功效
迷迭香的抗炎作用和缓解关节疼痛功效
自动灌溉控制器及灌溉系统
网址: Arbotix关节轨迹控制器的使用 https://m.huajiangbk.com/newsview1141735.html
| 上一篇: 解析XML和封装XML的基本用法 |
下一篇: python的axis=0 or |
