Dofbot机械臂从零部署笔记（4）——ROS之Moveit下实现实机逆向运动学规划

本节接上节，实现逆向运动学规划。
本节源代码位于（ /home/jetson/dofbot_ws/src/dofbot_moveit/scripts）
我们将 02_motion_plan.py 复制到上面的scripts文件夹下。

编译代码

02_motion_plan.py 复制到上面的scripts文件夹后，检查一下是否开启运行权限（py文件右键properties→permission→Execute打勾）。
回工作空间目录catkin_make一下就可以待运行了（其实melodic不catkin_make也可以，强迫症犯了）。

逆向运动学规划例子

需要将源码中初始化机械臂时传入的“dofbot”改成我们在之前moveit规划时设置的规划组“xiaok”

代码和运行效果

#!/usr/bin/env python
# coding: utf-8
import rospy
from math import pi
from time import sleep
from geometry_msgs.msg import Pose
from moveit_commander.move_group import MoveGroupCommander
from tf.transformations import quaternion_from_euler

# 角度转弧度
DE2RA = pi / 180

if __name__ == '__main__':
    # 初始化节点
    rospy.init_node("dofbot_motion_plan_py")
    # 初始化机械臂
    dofbot = MoveGroupCommander("xiaok")
    # 当运动规划失败后，允许重新规划
    dofbot.allow_replanning(True)
    dofbot.set_planning_time(5)
    # 尝试规划的次数
    dofbot.set_num_planning_attempts(10)
    # 设置允许目标位置误差
    dofbot.set_goal_position_tolerance(0.01)
    # 设置允许目标姿态误差
    dofbot.set_goal_orientation_tolerance(0.01)
    # 设置允许目标误差
    dofbot.set_goal_tolerance(0.01)
    # 设置最大速度
    dofbot.set_max_velocity_scaling_factor(1.0)
    # 设置最大加速度
    dofbot.set_max_acceleration_scaling_factor(1.0)
    # 设置"down"为目标点
    dofbot.set_named_target("down")
    dofbot.go()
    sleep(0.5)
    # 创建位姿实例
    pos = Pose()
    # 设置具体的位置
    pos.position.x = 0.0
    pos.position.y = 0.0597016
    pos.position.z = 0.168051
    # RPY的单位是角度值
    roll = -140.0
    pitch = 0.0
    yaw = 0.0
    # RPY转四元素
    q = quaternion_from_euler(roll * DE2RA, pitch * DE2RA, yaw * DE2RA)
    # pos.orientation.x = 0.940132
    # pos.orientation.y = -0.000217502
    # pos.orientation.z = 0.000375234
    # pos.orientation.w = -0.340811
    pos.orientation.x = q[0]
    pos.orientation.y = q[1]
    pos.orientation.z = q[2]
    pos.orientation.w = q[3]
    # 设置目标点
    dofbot.set_pose_target(pos)
    # 多次执行,提高成功率
    for i in range(5):
        # 运动规划
        plan = dofbot.plan()
        if len(plan.joint_trajectory.points) != 0:
            print ("plan success")
            # 规划成功后运行
            dofbot.execute(plan)
            break
        else:
            print ("plan error")

运行，分别打开demo和rviz，最后运行该程序：

roslaunch xiaok_moveit_config demo.launch

rosrun rviz rviz

rosrun xiaok_moveit_config 02_motion_plan.py

可以看到机械臂先来到之前设定好的“down”位置后，通过Plan，Execute到了指定的空间位置和角度。
我们可以看rivz，Display中打开MotionPlanning→Planned Path→Links→Link5，观察规划运动完后Positon和Orientation的值，应该和代码中我们预设的位姿值相近。

关于老是规划失败

我们在代码里设了5次循环，可以看到失败率是挺高的，第4次才成功规划并运行到指定位置。其实很有可能一直不成功。
猜测原因：
1.可能和默认的运动学算法 KDL 有关，这个算法求逆向运动学的特点就是计算精确，但是效率低下，失败率高。可以更换算法试试，不过网上说更换成TRAC-IK我个人感觉没什么差别。
2.可能和Jetson Nano的CPU性能有关，可以试着调整一下moveit中的规划相关的参数。比如：
dofbot.set_planning_time(10) 给它调成10秒。
规划时间给增多一点，实测也有几率plan成功。
3. set_goal_tolerance(0.03) 允许目标误差设得大一点。
然后for循环range()给个20次，多试几次。

如果有知道其他原因的小伙伴还请指教。

逆向运动学Moveit编程步骤，代码修正

程序2大致的步骤和之前程序1一样大致为：
・创建 Move group 对象
・设置目标位姿
・修改参数
・执行运动规划

但是这个步骤并不具体细致，落实到逆向运动学，我们没有设定以哪个末端link来定位目标位姿Pose，我们也没有指定以哪个参考坐标系进行运动，而是让程序默认去识别。

所以我们对程序进行修改：
我们把Pose()类换成PoseStamped()类，PoseStamped包含了位姿更多的信息。类名对应了ROS中的消息名，我们通过rosmsg show来查看它们俩的区别：
可以看到PoseStamped包含了一个head头信息，包括frame_id(以哪个坐标系定义位姿)和stamp(时间戳)。

同时我们通过get_end_effector_link()方法获取终端Link名称，set_pose_reference_frame()方法置目标位置所使用的参考坐标系。
再在set_pose_target(target_pose, end_effector_link)传入PoseStamped和终端Link名。
具体修正代码 02_motion_plan2.py 见下：

#!/usr/bin/env python
# coding: utf-8
import rospy
from math import pi
from time import sleep
from geometry_msgs.msg import PoseStamped, Pose
from moveit_commander.move_group import MoveGroupCommander
from tf.transformations import quaternion_from_euler

# 角度转弧度
DE2RA = pi / 180

if __name__ == '__main__':
    # 初始化节点
    rospy.init_node("dofbot_motion_plan_py")
    # 初始化机械臂
    dofbot = MoveGroupCommander("xiaok")

    # 获取终端link的名称
    end_effector_link = dofbot.get_end_effector_link()
    print(end_effector_link)
    # 设置目标位置所使用的参考坐标系
    reference_frame = 'base_link'
    dofbot.set_pose_reference_frame(reference_frame)

    # 当运动规划失败后，允许重新规划
    dofbot.allow_replanning(True)
    dofbot.set_planning_time(5)
    # 尝试规划的次数
    dofbot.set_num_planning_attempts(10)
    # 设置允许目标位置误差
    dofbot.set_goal_position_tolerance(0.01)
    # 设置允许目标姿态误差
    dofbot.set_goal_orientation_tolerance(0.01)
    # 设置允许目标误差
    dofbot.set_goal_tolerance(0.01)
    # 设置最大速度
    dofbot.set_max_velocity_scaling_factor(1.0)
    # 设置最大加速度
    dofbot.set_max_acceleration_scaling_factor(1.0)
    # 设置"down"为目标点
    dofbot.set_named_target("down")
    dofbot.go()
    sleep(0.5)

    # 创建位姿实例
    target_pose = PoseStamped()
    target_pose.header.frame_id = reference_frame
    target_pose.header.stamp = rospy.Time.now()
    # 设置具体的位置
    target_pose.pose.position.x = 0.0
    target_pose.pose.position.y = 0.0597016
    target_pose.pose.position.z = 0.168051
    # RPY的单位是角度值
    roll = -140.0
    pitch = 0.0
    yaw = 0.0
    # RPY转四元素
    q = quaternion_from_euler(roll * DE2RA, pitch * DE2RA, yaw * DE2RA)
    print(q)
    target_pose.pose.orientation.x = q[0]
    target_pose.pose.orientation.y = q[1]
    target_pose.pose.orientation.z = q[2]
    target_pose.pose.orientation.w = q[3]

    # 设置机器臂当前的状态作为运动初始状态
    dofbot.set_start_state_to_current_state()

    # 设置目标点
    dofbot.set_pose_target(target_pose, end_effector_link)

    # 多次执行,提高成功率
    for i in range(5):
        # 运动规划
        traj = dofbot.plan()
        if len(traj.joint_trajectory.points) != 0:
            print ("plan success")
            # 规划成功后运行
            dofbot.execute(traj)
            break
        else:
            print ("plan error")

总结一下步骤：
・创建 Move group 对象
・获取终端Link名称
・设置目标位姿对应的参考坐标系，设定起始、终止位姿
・执行运动规划

因为规划失败率高，耗时长，实机同步省略（实现思路和订阅Joint State一样）。