添加arbotix关节控制器
以下内容,注意文件的路径
1.新建一个文件arm.yaml。arm_moveit_config/config/arm.yaml
joints: {
joint1: {id: 1, max_angle: 180, min_angle: -180, max_speed: 90},
joint2: {id: 2, max_angle: 134.6, min_angle: -108.9, max_speed: 90},
joint3: {id: 3, max_angle: 86, min_angle: -166, max_speed: 90},
joint4: {id: 4, max_angle: 180, min_angle: -180, max_speed: 90},
joint5: {id: 5, max_angle: 237.2, min_angle: -68.8, max_speed: 90},
joint6: {id: 6, max_angle: 180, min_angle: -180, max_speed: 90},
}
controllers: {
arm_controller: {type: follow_controller, joints: [joint1, joint2, joint3, joint4, joint5, joint6], action_name: arm_controller/follow_joint_trajectory, onboard: False }
}
2.继续新建一个文件fake_arm.launch。
arm_moveit_config/launch/fake_arm.launch
<!-- 配置moveit关节控制器 p322 1 -->
<launch>
<!-- 不使用仿真时间 -->
<param name="/use_sim_time" value="false" />
<param name="robot_description" textfile="$(find arm)/urdf/arm.urdf" />
<!-- 启动arbotix driver-->
<arg name="sim" default="true" />
<node name="arbotix" pkg="arbotix_python" type="arbotix_driver" output="screen">
<rosparam file="$(find arm_moveit_config)/config/arm.yaml" command="load" />
<param name="sim" value="true"/>
</node>
<node pkg="robot_state_publisher" type="robot_state_publisher" name="rob_st_pub" />
<!-- 启动rviz -->
<!-- 第一次打开rviz使用,需要配置,保存一下就可以用下面这个 -->
<node name="rviz" pkg="rviz" type="rviz" args="rviz" />
<!-- 配置保存后,把上面的注释,下面这个注释打开 -->
<!-- <node name="rviz" pkg="rviz" type="rviz" args="-d $(find arm_moveit_config)/urdf/fake_arm_urdf.rviz" required="true" /> -->
</launch>
3.继续新建一个文件夹urdf。
arm_moveit_config/urdf
一会打开rviz配置信息,保存在这里。
4.继续新建一个文件夹scripts。
arm_moveit_config/scripts 。
在scripts文件夹下面新建文件 trajectory_demo.py
arm_moveit_config/scripts/trajectory_demo.py
#!/usr/bin/env python
# -*- coding: utf-8 -*-
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)
# 机械臂中joint的命名
arm_joints = ['joint1',
'joint2',
'joint3',
'joint4',
'joint5',
'joint6']
if reset:
# 如果需要回到初始化位置,需要将目标位置设置为初始化位置的六轴角度
arm_goal = [0, 0, 0, 0, 0, 0]
else:
# 如果不需要回初始化位置,则设置目标位置的六轴角度
arm_goal = [0.5, 0.8, 1, 1.2,-2,-0.9]
# 连接机械臂轨迹规划的trajectory action server
rospy.loginfo('等待手臂轨迹控制器...')
arm_client = actionlib.SimpleActionClient('arm_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
arm_client.wait_for_server()
rospy.loginfo('联系中...')
# 使用设置的目标位置创建一条轨迹数据
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('将手臂移动到目标位置 ....')
# 创建一个轨迹目标的空对象
arm_goal = FollowJointTrajectoryGoal()
# 将之前创建好的轨迹数据加入轨迹目标对象中
arm_goal.trajectory = arm_trajectory
# 设置执行时间的允许误差值
arm_goal.goal_time_tolerance = rospy.Duration(0.0)
# 将轨迹目标发送到action server进行处理,实现机械臂的运动控制
arm_client.send_goal(arm_goal)
# 等待机械臂运动结束
arm_client.wait_for_result(rospy.Duration(5.0))
rospy.loginfo('....完成')
if __name__ == '__main__':
try:
TrajectoryDemo()
except rospy.ROSInterruptException:
pass
- 运行
机器人在RVIZ中显示
roslaunch arm_moveit_config fake_arm.launch
启动rviz没有显示,需要配置一下
模型就能正常显示了按“ctrl+shift+s”保存一下我们刚才的设置,选择刚才新建的urdf文件下
fake_arm_urdf.rviz
把原来的rviz注释,打开下面的注释。
机器人开始移动到目标位置
rosrun arm_moveit_config trajectory_demo.py _reset:=False
机器人从目标位置回到原始位置
rosrun arm_moveit_config trajectory_demo.py _reset:=True
[rosrun] Couldn't find executable named trajectory_demo.py below /home/hh/ws_moveit/src/mechanical_arm/arm_moveit_config
[rosrun] Found the following, but they're either not files,
[rosrun] or not executable:
[rosrun] /home/hh/ws_moveit/src/mechanical_arm/arm_moveit_config/scripts/trajectory_demo.py
在文件的上一级目录下打开终端
chmod +x trajectory_demo.py