经过一周的纠结,这里终于做好了,还是基础太薄.所以非常有必要总结一下。moveit作为一个很好的机械臂路径规划工具，大大降低了机械臂的开发的难度，很多功能都可以在模拟环境下测试运行，如前面博客中讲到的，但要让真实的机械臂能够按照moveit规划好的路径动起来，就需要开发连接机械臂和moveit的驱动代码。
1.驱动的原理
上图为通讯原理
首先，moveit把计算的结果通过Ros action的方式发送给driver，driver调用Ros_arduino_bridge的servo_write 服务发送各个关节舵机的控制指令给Arduino uno控制板
其次，同时Driver也要通过调用Ros_arduino_bridge的servo_read服务读取各个关节的舵机状态，通过joint_state消息的方式发送给moveit，供moveit进行路径规划计算。
2.关于硬件连接
主控制板:arduino uno r3, 16路pwm控制板.IIC接口
Gnd------------------Gnd
+5V------------------VCC
A4-------------------SDA
A5-------------------SCL
3.Arduino程序修改
没有基础控制器,只是响使用它来控制舵机，编辑ROSArduinoBridge sketch，在文件的最前面，将这两行：
#define USE_BASE
//#undef USE_BASE
改成这样:
//#define USE_BASE
#undef USE_BASE
NOTE:还需要将文件encoder_driver.ino中的这一行注释掉：
#include "MegaEncoderCounter.h"
编译并上传代码
4.创建控制器配置文件six_arm_controllers.yaml
在moveit assistant产生的配置文件目录的config子目录下，创建配置文件six_arm_controllers.yaml，此文件告诉moveit将与哪个的action通讯.
six_arm_controllers.yaml代码如下:
controller_list:
# 控制器的ros topic——arm_controller/follow_joint_trajectory
- name: arm_controller
action_ns: follow_joint_trajectory
type: FollowJointTrajectory//在driver中要声明的action service类型
default: true
joints:
- arm_base_to_arm_round_joint_stevo0
- shoulder_2_to_arm_joint_stevo1
- big_arm_round_to_joint_stevo2
- arm_joint_stevo2_to_arm_joint_stevo3
- wrist_to_arm_joint_stevo4
- arm_joint_stevo4_to_arm_joint_stevo5
这里为机械臂定义了控制器arm_controller,告诉moveit机械臂通讯的topic是arm_controller/follow_joint_trajectory.
5 机械臂控制的FollowController
扩展ros_arduino_bridge的功能来实现FollowController，在ros_arduino_python目录下创建joints.py和six_arm_follow_controller.py。
joint.py
封装了关节舵机控制的代码，主要功能函数：
__init__类初始化代码
setCurrentPosition(self):通过调用/arduino/servo_write服务发送舵机的控制命令，把舵机的位置设置为self.position
setCurrentPosition(self, position):
通过调用/arduino/servo_write服务发送舵机的控制命令，把舵机的位置设置为moveit给出的舵机位置
mapToServoPosition(self,position):将moveit给出的舵机位置，转换为机械臂舵机实际对应的位置，此功能需要标定后，才能准确控制机械臂的位置。
#!/usr/bin/env python
# Copyright (c) 2017-2018 diego.
# All right reserved.
#
## @file joints.py the jonit clase support functions for joints.
## @brief Joints hold current values.
import roslib
import rospy
from ros_arduino_msgs.srv import *
from math import pi as PI, degrees, radians
class Joint:
## @brief Constructs a Joint instance.
##
## @param servoNum The servo id for this joint.
##
## @param name The joint name.
##
## @param name The servo control range.
def __init__(self, name, servoNum, max, min, servo_max, servo_min, inverse):
self.name = name//关节名称
self.servoNum=servoNum//对应的舵机编号
self.max=max//舵机最大角度：0
self.min=min//舵机最小角度：180
self.servo_max=servo_max
self.servo_min=servo_min
self.inverse=inverse
self.position = 0.0
self.velocity = 0.0
## @brief Set the current position.
def setCurrentPosition(self):
rospy.wait_for_service('/arduino/servo_write')
try:
servo_write=rospy.ServiceProxy('/arduino/servo_write',ServoWrite)
servo_write(self.servoNum,self.position)
except rospy.ServiceException, e:
print "Service call failed: %s"%e
## @brief Set the current position.
##
## @param position The current position.
def setCurrentPosition(self, position):
rospy.wait_for_service('/arduino/servo_write')
try:
#if self.servoNum==2:
servo_write=rospy.ServiceProxy('/arduino/servo_write',ServoWrite)
rospy.loginfo("%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%")
rospy.loginfo("before mapping--- the servo id:"+str(self.servoNum)+" position is "+str(position))
self.mapToServoPosition(position)
rospy.loginfo("after mapping---- the servo id:"+str(self.servoNum)+" self.position is "+str(self.position))
servo_write(self.servoNum,radians(self.position))
except rospy.ServiceException, e:
print "Service call failed: %s"%e
## @brief map to Servo Position.
##
## @param position The current position.
def mapToServoPosition(self,position):
per=(position-self.min)/(self.max-self.min)
if not self.inverse:
self.position=self.servo_min+per*(self.servo_max-self.servo_min)
else:
self.position=self.servo_max-per*(self.servo_max-self.servo_min)
six_arm_follow_controller.py
机械臂控制器类，接收moveit的控制命令，转换为关节舵机的实际控制指令，驱动机械臂的运动。
#!/usr/bin/env python
# Copyright (c) 2017-2018 williamar.
# All right reserved.
import rospy, actionlib
from control_msgs.msg import FollowJointTrajectoryAction
from trajectory_msgs.msg import JointTrajectory
from diagnostic_msgs.msg import *
from math import pi as PI, degrees, radians
from joints import Joint
class FollowController:
# 此类是驱动的核心代码
def __init__(self, name):
self.name = name
rospy.init_node(self.name)
# 初始化化ros note，设置节点名称，刷新频率为50hz
# rates
self.rate = 50.0
# 初始化机械臂的关节，并把关节放入joints列表中，方便后续操作
# Arm jonits
self.arm_base_to_arm_round_joint_stevo0=Joint('arm_base_to_arm_round_joint_stevo0',0,1.5797,-1.5707,130,0,False)
self.shoulder_2_to_arm_joint_stevo1=Joint('shoulder_2_to_arm_joint_stevo1',1,1.5707,-0.1899,115,45,False)
self.big_arm_round_to_joint_stevo2=Joint('big_arm_round_to_joint_stevo2',2,2.5891,1,100,20,False)
self.arm_joint_stevo2_to_arm_joint_stevo3=Joint('arm_joint_stevo2_to_arm_joint_stevo3',3,1.5707,-1.5707,130,0,False)
self.wrist_to_arm_joint_stevo4=Joint('wrist_to_arm_joint_stevo4',4,1.5707,-1.5707,130,0,False)
self.arm_joint_stevo4_to_arm_joint_stevo5=Joint('arm_joint_stevo4_to_arm_joint_stevo5',5,1.5707,-1.5707,130,0,True)
self.joints=[self.arm_base_to_arm_round_joint_stevo0,
self.shoulder_2_to_arm_joint_stevo1,
self.big_arm_round_to_joint_stevo2,
self.arm_joint_stevo2_to_arm_joint_stevo3,
self.wrist_to_arm_joint_stevo4,
self.arm_joint_stevo4_to_arm_joint_stevo5]
# action server
self.server = actionlib.SimpleActionServer('arm_controller/follow_joint_trajectory', FollowJointTrajectoryAction, execute_cb=self.actionCb, auto_start=False)
self.server.start()
rospy.spin()
rospy.loginfo("Started FollowController")
def actionCb(self, goal):
print("****************actionCb*********************")
rospy.loginfo(self.name + ": Action goal recieved.")
traj = goal.trajectory
if set(self.joints) != set(traj.joint_names):
for j in self.joints:
if j.name not in traj.joint_names:
msg = "Trajectory joint names does not match action controlled joints." + str(traj.joint_names)
rospy.logerr(msg)
self.server.set_aborted(text=msg)
return
rospy.logwarn("Extra joints in trajectory")
if not traj.points:
msg = "Trajectory empy."
rospy.logerr(msg)
self.server.set_aborted(text=msg)
return
try:
indexes = [traj.joint_names.index(joint.name) for joint in self.joints]
except ValueError as val:
msg = "Trajectory invalid."
rospy.logerr(msg)
self.server.set_aborted(text=msg)
return
if self.executeTrajectory(traj):
self.server.set_succeeded()
else:
self.server.set_aborted(text="Execution failed.")
rospy.loginfo(self.name + ": Done.")
def executeTrajectory(self, traj):
rospy.loginfo("Executing trajectory")
rospy.logdebug(traj)
# carry out trajectory
try:
indexes = [traj.joint_names.index(joint.name) for joint in self.joints]
except ValueError as val:
rospy.logerr("Invalid joint in trajectory.")
return False
# get starting timestamp, MoveIt uses 0, need to fill in
start = traj.header.stamp
if start.secs == 0 and start.nsecs == 0:
start = rospy.Time.now()
r = rospy.Rate(self.rate)
for point in traj.points:
desired = [ point.positions[k] for k in indexes ]
for i in indexes:
#self.joints[i].position=desired[i]
self.joints[i].setCurrentPosition(desired[i])
while rospy.Time.now() + rospy.Duration(0.01) < start:
rospy.sleep(0.01)
return True
if __name__=='__main__':
try:
rospy.loginfo("start followController...")
FollowController('follow_Controller')
except rospy.ROSInterruptException:
rospy.loginfo('Failed to start followController...')
6 launch启动文件
moveit assistant会生成一个launch文件夹，修改后测试.
修改six_arm_moveit_controller_manager.launch.xml
此文件是moveit assistant自动生成的，但其中内容是空的，我增加如下内容，告诉moveit，启动Controller的配置文件位置
<launch>
<!-- Set the param that trajectory_execution_manager needs to find the controller plugin -->
<arg name="moveit_controller_manager" default="moveit_simple_controller_manager/MoveItSimpleControllerManager" />
<param name="moveit_controller_manager" value="$(arg moveit_controller_manager)"/>
<!-- load controller_list -->
<rosparam file="$(find six_arm_moveit_config)/config/six_arm_controllers.yaml"/>
</launch>
创建six_arm_demo.launch文件
在moveit的launch文件夹下创建six_arm_demo.launch文件，并添加如下内容
<launch>
<master auto="start"/>
<node name="arduino" pkg="ros_arduino_python" type="arduino_node.py" output="screen">
<rosparam file="$(find ros_arduino_python)/config/my_arduino_params.yaml" command="load" />
</node>
<!-- By default, we are not in debug mode -->
<arg name="debug" default="false" />
<!-- Load the URDF, SRDF and other .yaml configuration files on the param server -->
<include file="$(find six_arm_moveit_config)/launch/planning_context.launch">
<arg name="load_robot_description" value="true"/>
</include>
<node name="arduino_follow_controller" pkg="ros_arduino_python" type="six_arm_follow_controller.py" output="screen">
</node>
<!-- If needed, broadcast static tf for robot root -->
<node pkg="tf" type="static_transform_publisher" name="virtual_joint_broadcaster_0" args="0 0 0 0 0 0 world_frame base_link 100" />
<!-- We do not have a robot connected, so publish fake joint states -->
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
<param name="/use_gui" value="false"/>
<rosparam param="/source_list">[/move_group/fake_controller_joint_states]</rosparam>
</node>
<!-- Run the main MoveIt executable without trajectory execution (we do not have controllers configured by default) -->
<include file="$(find six_arm_moveit_config)/launch/move_group.launch">
</include>
</launch>
这里主要是作为一个测试使用，所以joint_states使用了假数据，在调用joint_state_publisher时我们把source list设置为/move_group/fake_controller_joint_states
执行如下代码来启动moveit
roslaunch six_arm_moveit_config six_arm_demo.launch
这时候我们可以通过控制台输入moveit命令来控制机械臂，我们也可以用代码来调用moveit接口来控制机械臂，下面我们来写一段测试代码。
moveit控制测试代码
在ros_arduino_python目录下创建diego_moveit_test.py，代码如下：
#!/usr/bin/env python
import rospy, sys
import moveit_commander
from control_msgs.msg import GripperCommand
class MoveItDemo:
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
# Initialize the ROS node
rospy.init_node('moveit_demo', anonymous=True)
# Connect to the arm move group
arm = moveit_commander.MoveGroupCommander('arm')
# Get the name of the end-effector link
end_effector_link = arm.get_end_effector_link()
# Display the name of the end_effector link
rospy.loginfo("The end effector link is: " + str(end_effector_link))
# Set a small tolerance on joint angles
arm.set_goal_joint_tolerance(0.001)
# Start the arm target in "resting" pose stored in the SRDF file
arm.set_named_target('arm_default_pose')
# Plan a trajectory to the goal configuration
traj = arm.plan()
# Execute the planned trajectory
arm.execute(traj)
# Pause for a moment
rospy.sleep(1)
# Cleanly shut down MoveIt
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
if __name__ == "__main__":
try:
MoveItDemo()
except rospy.ROSInterruptException:
pass
在测试代码中，我们连接到group arm，并命令机械臂运行到姿势 arm_default_pose，运行如下命令启动测试代码。
rosrun ros_arduino_python six_arm_moveit_test.py
启动后可以看到机械臂马上响应到arm_default_pose的位置。
7 附录
启动命令记录:
roslaunch six_arm_moveit_config demo.launch
新终端:
roslaunch ros_arduino_python arduino.launch
roslaunch ros_arduino_python six_arm_follow_controller.launch
roslaunch six_arm_moveit_config six_arm_demo.launch
rosrun ros_arduino_python six_arm_moveit_test.py
3988
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
