Dofbot机械臂从零部署笔记（5补完）——新开发环境下复现前面的例子

新开发环境配置完成后，可以顺便过一遍之前的例子。

创建URDF模型

步骤同前

配置MoveIt

默认没安装moveit，安装一下：
安装

sudo apt-get install ros-noetic-moveit

启动

roslaunch moveit_setup_assistant setup_assistant.launch

步骤同前

之后可启动：

roslaunch xiaok_moveit_config demo.launch

常见报错：
①报错：
Unable to connect to move_group action server ‘move_group‘ within allotted time (30s)
模型加载时间过长，导致rviz启动有问题。
解决方法：等模型完全加载显示可以轨迹规划时，点击rviz左下角的 Reset 即可。

②启动实时控制互动界面：joint_state_publisher_gui
打开 launch/demo.launch：
<arg name="use_gui" default="false" />改成<arg name="use_gui" default="true" />，回工作空间catkin_make一下再启动。

简化模型

上面的报错①的根本原因是模型文件太大，碰撞点太多太复杂，可以替换一下简化版的模型。
简化模型：
用 home/dofbot_ws/src/dofbot_info/meshes下的文件（文件较小，碰撞点信息也相对少）替换 home/xiaok_ws/src/xiaok_description/meshes下的建模文件。

例子（2）——操作实机实时控制机械臂每个关节转动

①打开

roslaunch xiaok_moveit_config demo.launch

②打开VSCode，选择一个新的文件夹作为工作目录，新建文件（选第二个 Alt+Ctrl+N），选择Jupyter Notebook。

将如下代码复制进notebook，启动，选择解释器Python3.8。
运行，即可实现实时控制：

import rospy
import Arm_Lib
from math import pi
from sensor_msgs.msg import JointState

# 弧度转角度
RA2DE = 180 / pi


def topic(msg):
    # 如果不是该话题的数据直接返回
    if not isinstance(msg, JointState): return
    # 定义关节角度容器,最后一个是夹爪的角度,默认夹爪不动为90.
    joints = [0.0, 0.0, 0.0, 0.0, 0.0, 90.0]
    # 将接收到的弧度[-1.57,1.57]转换成角度[0,180]
    for i in range(5): joints[i] = (msg.position[i] * RA2DE) + 90
    # 调驱动函数
    sbus.Arm_serial_servo_write6_array(joints, 100)


if __name__ == '__main__':
    sbus = Arm_Lib.Arm_Device()
    # ROS节点初始化
    rospy.init_node("ros_dofbot")
    # 创建一个订阅者
    subscriber = rospy.Subscriber("/joint_states", JointState, topic)
    # 设置循环的频率
    rate = rospy.Rate(2)
    # 按照循环频率延时
    rospy.spin()

例子（3）——Moveit下实现顺向运动学规划+实机同步运动

①打开

roslaunch xiaok_moveit_config demo.launch

②打开VSCode，新建一个Jupyter Notebook。
代码如下，启动时选择解释器Python3.8。
注意选择python3起一个新的线程使用的是 import _thread

from time import sleep
import rospy
from moveit_commander.move_group import MoveGroupCommander
import Arm_Lib
from math import pi
from sensor_msgs.msg import JointState
import _thread,time

# 弧度转角度
RA2DE = 180 / pi

def topic(msg):
    # 如果不是该话题的数据直接返回
    if not isinstance(msg, JointState): return
    # 定义关节角度容器,最后一个是夹爪的角度,默认夹爪不动为90.
    joints = [0.0, 0.0, 0.0, 0.0, 0.0, 90.0]
    # 将接收到的弧度[-1.57,1.57]转换成角度[0,180]
    for i in range(5): joints[i] = (msg.position[i] * RA2DE) + 90
    # 调驱动函数
    sbus.Arm_serial_servo_write6_array(joints, 100)

def command_thread():
    while (1):
        # 设置随机目标点
        dofbot.set_random_target()
        # 开始运动
        dofbot.go()
        sleep(0.5)

if __name__ == '__main__':
    # 初始化节点
    rospy.init_node("dofbot_set_move")
    # 初始化机械臂
    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)

    sbus = Arm_Lib.Arm_Device()

    # 创建一个订阅者
    subscriber = rospy.Subscriber("/joint_states", JointState, topic)
    # 设置循环的频率
    rate = rospy.Rate(2)

    _thread.start_new_thread(command_thread, ())    

    # 按照循环频率延时
    rospy.spin()

例子（4）——Moveit下实现逆向运动学规划

①打开

roslaunch xiaok_moveit_config demo.launch

②打开VSCode，新建一个Jupyter Notebook。
代码如下，启动时选择解释器Python3.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 = 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")

最后，这个逆向运动学的失败率让人抓狂，我已经准备使用替代方案，即自己使用一些开源项目的代码做六关节角度逆解。毕竟我们需要跑实机做设计。