KUKA youbot机械臂与Moveit工具包（2）

这里利用在ROS by example volume2中学到的知识，将它转化到youbot上，这里我将记录一些关于学习moveit中的正反解以及利用第三方求解器（主要是IKFast和youbot_arm_kinematics_moveit::KinematicsPlugin）的一些过程和体会，希望对大家有所帮助。

1. 利用Moveit的API求解正运动学

roslaunch ik_follow_joint_trajectory youbot_with_gripper.launch sim:=true
roslaunch youbot_moveit_config move_group.launch
rosrun rviz rviz
rosrun ik_follow_joint_trajectory moveit_fk_demo_youbot.py

第二个launch文件是由我们之前的Moveit assistant向导帮我们生成的，它负责启动一些让我们能利用API对robot进行控制的node和service。这里我们的主要文件是moveit_fk_demo_youbot.py.代码如下

#!/usr/bin/env python
import rospy, sys
import moveit_commander

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 right_arm move group
right_arm = moveit_commander.MoveGroupCommander('right_arm')

# Get the name of the end-effector link
end_effector_link = right_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
right_arm.set_goal_joint_tolerance(0.001)
right_gripper.set_goal_joint_tolerance(0.001)

# Start the arm target in "resting" pose stored in the SRDF file
right_arm.set_named_target('straight_forward')

# Plan a trajectory to the goal configuration
traj = right_arm.plan()

# Execute the planned trajectory
right_arm.execute(traj)

# Pause for a moment
rospy.sleep(1)

# Set target joint values for the arm: joints are in the order they appear in
# the kinematic tree.
joint_positions = [2.9, 0.08, -2.6, 0.11, 0.12]

# Set the arm's goal configuration to the be the joint positions
right_arm.set_joint_value_target(joint_positions)

# Plan and execute the motion
right_arm.go()
rospy.sleep(1)

# Save this configuration for later
right_arm.remember_joint_values('saved_config', joint_positions)

# Set the arm target to the named "straight_out" pose stored in the SRDF file
right_arm.set_named_target('straight_forward')

# Plan and execute the motion
right_arm.go()
rospy.sleep(1)

# Set the goal configuration to the named configuration saved earlier
right_arm.set_named_target('saved_config')

# Plan and execute the motion
right_arm.go()
rospy.sleep(1)

# Return the arm to the named "resting" pose stored in the SRDF file
right_arm.set_named_target('resting')
right_arm.go()
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

node 图如下图所示，数据量有点大，不过我们关注一下主要的地方即可（图片另存为，或者使用Ctrl+滚轮，缩放网页）。



其实，这里的moeit的fk就已经不知不觉中解决了我们之前的那个四个数据点往复摆动的问题（之前的那篇action里是用c++写的action client)，这里也是定义了一些点，规划，然后调用go命令，用moveit（python API）实现。

视频地址仿真视频

点击打开链接

实际动作

点击打开链接

2. 运动学反解

2.1 关于KDL和IK Fast


你也许会疑问，都没有看到一些计算公式，怎么正解就求出来了，怎么还可以求反解，之所以能够这样，秘密全在我们加载Moveit时所使用的URDF文件中，真可以说URDF写的好，一切没烦恼。如果你做过URDF tutorails的话，地址http://wiki.ros.org/urdf/Tutorials 你也看到URDF中会存在一些Joint和Link之间的平移和旋转（另外说个题外话，用Solidworks画出机器人模型，是可以用插件导出URDF文件ros.org/wiki/sw_urdf_exporter），这个道理就相当于我们利用传统的D-H建模时各个关节的旋转和平移变换，另外包括ROS内部的TF
tree，它其实也是基于这个URDF实现的。

其实，学习这个Moveit package我遇到的最大问题还是在反解这里，看到by example作者的反解仿真，各种绕障碍，各种顺畅，心里那个痒痒。这里的求解器我用的是第三方的，我没有用那个默认的KDL和IK Fast主要是因为花了很多时间仍然没有解决问题，暂时没有找到头绪。

默认的KDL是因为经常求解不出来，造成timeout



上面的那两个错误我没有理他，因为我是基于手臂做的逆解，它提示模型文件中Urdf中没有找到叫做'base_l_wheel_joint'的关节。

IKFast因为要基于dae文件，计算IK Fast求解器再聚合到Moveit中，但是在计算它的求解器时，没有求解成功，更不用说后面的聚合到moveit中了，这里参考的部分是ROS By Example volume2的P379页，另外提一句，6自由度机器人参数是：transform6d，但是5自由度机器人的参数是：translationdirection5d





这里你也可以发现一些线索，IK Fast之所以能够快，它首先截取了一定精度，而非像由urdf直接生成而来的dae文件中那样长达十几位的小数，我们知道计算机相比处理整数，处理小数计算是十分困难的，这也是嵌入式里面常常提到硬件浮点和软件浮点（题外话了）。第二个注意上副图里，求解器的计算用到了一些算法，也就是说经过二次封装了，只可惜我们这里没有求解出来，按照by example作者说的，用上IK Fast求解器，可以提升好几倍的计算速度。这里不是说这种办法不行，在这上花了两三天时间，我没找到解决办法，所以只有暂时转移目标了，有兴趣的可以基于自己用到的机器人去试试。

2.2 关于youbot_arm_kinematics_moveit::Kinematics_Plugin
这个求解器是我在Github上搜索得到的，其实当时也是抱着试一试的心态去用的，比起上面两个，这个起码还是可用的

https://github.com/svenschneider/youbot-manipulation

另外也有一个repository也挺厉害的，之前看过他们的论文，大工程。

https://github.com/WPI-RAIL

http://wiki.ros.org/carl_interactive_manipulation?distro=indigo

在那个youbot_manipulation 工程中，我们编译过后会得到关于plugin的一些文件，但是你也要注意作者的那个Moveit_config

，因为我们之前利用过向导生成了config文件，为了避免冲突我们先看看作者是怎么设置Group和end effector的，我们待会儿基于作者的来修改自己的config以保证能够用上

https://github.com/svenschneider/youbot-manipulation/blob/hydro/youbot_moveit/config/youbot.srdf

<group name="arm_1">
<joint name="arm_joint_1" />
<joint name="arm_joint_2" />
<joint name="arm_joint_3" />
<joint name="arm_joint_4" />
<joint name="arm_joint_5" />
</group>
<group name="arm_1_gripper">
<joint name="gripper_finger_joint_l" />
<joint name="gripper_finger_joint_r" />
</group>

作者有两个Group分别是手臂和夹子，但是他加入的全部的是运动关节，也就是不存在那些固定的，比如作为父关节的arm_joint_0和夹子的gripper_finger_joint_palm.

<end_effector name="arm_1_gripper" parent_link="arm_link_5" group="arm_1_gripper" parent_group="arm_1" />
<virtual_joint name="odom" type="fixed" parent_frame="odom" child_link="base_footprint" />

第二句里面的type，还记得在上一篇moveit配置向导的时候，我的Virtual Joint吗？因为，我看到这里用的fixed，于是没有按照by example的，由planar改为了fixed

再来看看它的controller

https://github.com/svenschneider/youbot-manipulation/blob/hydro/youbot_moveit/config/controllers.yaml

controller_list:
- name: arm_1/arm_controller
action_ns: follow_joint_trajectory
type: FollowJointTrajectory
default: true
joints:
- arm_joint_1
- arm_joint_2
- arm_joint_3
- arm_joint_4
- arm_joint_5

直接就是5个关节，也就是说，夹子我们不能控制。by example作者能够控制是因为Arbotix有个GripperCommandAction，然而我们youbot的driver起动后的topic中是没有这个action的，这个地方我们加入TODO List，后面看能否自己找其他办法实现。

2.2 启动仿真及youbot实际运动

roslaunch ik_follow_joint_trajectory youbot_with_gripper.launch sim:=true
roslaunch youbot_moveit_config move_group.launch
rosrun rviz rviz
rosrun ik_follow_joint_trajectory moveit_ik_demo_youbot.py

这里的程序是基于by example P323页修改的，给他一个末端的姿态点。我因为怕自己给的点跑到工作空间外，这里给的是手臂伸直时的末端姿态点，注意下图左边，arm_link_5的position和oriention，将它给到moveit_ik_demo_youbot.py中就行了



视频地址点击打开链接

3. 总结

在这里使用moveit也不是说完全没有问题，比如那些个关于找不到底座轮子的ERROR信息，我没有涉及底部的轮子，我猜想是自己当时在moveit向导中，用的是整个机器人，后面会尝试用只含手臂的urdf；
明明那个youbot_arm_kinematics_moveit::Kinematics_Plugin已经加载了，但是它还是会提示说找不到，这个地方我已经在github上给作者留言了；
在向导的选折下拉框中还有两个求解器，PR2和service的那两个，不太明白，还需进一步深入；

最重要的是我们在控制的时候，夹子没有加入到运行的动作中去。