机械臂逆解算

机械臂解算

1、内容说明

本课程实现机械臂的正运动学计算、逆运动学计算。正运动学是由机械臂的每个舵机的角度值计算末端执行的位姿，而逆运动学则是由末端的位姿来计算在每个舵机的角度值，两者在进行三维空间夹取的时候有着至关重要的作用。由正运动学算法，可以得知当前机械臂末端的位姿，在进行坐标系转换的时候，我们需要知道这个值；由逆运动学算法，可以计算机械臂末端要到达目标位姿，机械臂的每个舵机的角度是多少，在进行夹取前，需要调用这个算法去计算舵机值，然后控制舵机运动到夹取的姿态。

本节内容需要在终端中输入指令，这里根据主板类型来选择打开终端。本节课程以树莓派5为例。树莓派和Jetson-Nano主板，需要在宿主机中打开终端，然后输入进入docker容器的指令，进入到docker容器后的终端里边输入本节课程提及的指令，宿主机进入docker容器的教程可以参考本产品教程【0.说明书和安装步骤】中的【进入小车Dockder（Jetson-Nano和树莓派5用户看这里）】的内容。

Orin主板直接打开终端，输入本节课程提及的指令即可。

2、程序启动

在终端中，输入以下命令启动，


ros2 run arm_kin kin_srv

启动后，终端输入ros2 node list来查看节点列表，

/kinemarics_arm是启动正逆解算的节点，终端输入ros2 node info /kinemarics_arm 来查询节点信息，

如上图所示，/kinemarics_arm节点提供了一个服务/get_kinemarics，服务的类型是arm_interface/srv/ArmKinemarics，终端输入ros2 interface show arm_interface/srv/ArmKinemarics查看这个服务数据包含哪些内容，


xxxxxxxxxx
float64 tar_x
float64 tar_y
float64 tar_z
float64 roll
float64 pitch
float64 yaw
float64 cur_joint1
float64 cur_joint2
float64 cur_joint3
float64 cur_joint4
float64 cur_joint5
float64 cur_joint6
string  kin_name
---
float64 joint1
float64 joint2
float64 joint3
float64 joint4
float64 joint5
float64 joint6
float64 x
float64 y
float64 z
float64 roll
float64 pitch
float64 yaw

---把数据分为上下两部分，上边的是request，下边的response。request部分如下，


xxxxxxxxxx
#机械臂末端位姿x坐标，单位是米
float64 tar_x 
#机械臂末端位姿y坐标，单位是米
float64 tar_y
#机械臂末端位姿z坐标，单位是米
float64 tar_z
#机械臂末端位姿roll值 绕X轴旋转值，单位是弧度
float64 roll
#机械臂末端位姿pitch值 绕y轴旋转值，单位是弧度
float64 pitch
#机械臂末端位姿yaw值 绕z轴旋转值，单位是弧度
float64 yaw
#当前1号舵机值，单位是度
float64 cur_joint1
#当前2号舵机值，单位是度
float64 cur_joint2
#当前3号舵机值，单位是度
float64 cur_joint3
#当前4号舵机值，单位是度
float64 cur_joint4
#当前5号舵机值，单位是度
float64 cur_joint5
#当前6号舵机值，单位是度
float64 cur_joint6
#解算类型：ik表示逆运动学解算，fk表示正运动学解算
string  kin_name

response部分如下，


xxxxxxxxxx
#1号舵机角度
float64 joint1
#2号舵机角度
float64 joint2
#3号舵机角度
float64 joint3
#4号舵机角度
float64 joint4
#5号舵机角度
float64 joint5
#6号舵机角度
float64 joint6
#机械臂末端位姿x坐标
float64 x
#机械臂末端位姿坐标
float64 y
#机械臂末端位姿z坐标
float64 z
#机械臂末端位姿roll值 绕X轴旋转值，单位是弧度
float64 roll
#机械臂末端位姿pitch值 绕Y轴旋转值，单位是弧度
float64 pitch
#机械臂末端位姿yaw值 绕Z轴旋转值，单位是弧度
float64 yaw

2.1、调用fk

我们调用fk来计算下：当机械臂向上伸直的时候，机械臂末端的位姿是多少。首先我们输入以下指令让机械臂向上伸直，在成功连接agent后，终端输入以下指令，


xxxxxxxxxx
ros2 topic pub /arm6_joints arm_msgs/msg/ArmJoints {"joint1: 90,joint2: 90,joint3: 90,joint4: 90,joint5: 90,joint6: 90,time: 1500"} --once

运行后，机械臂会向上伸直，接着，我们在终端输入以下指令，调用fk服务，


xxxxxxxxxx
ros2 service call /get_kinemarics arm_interface/srv/ArmKinemarics  "{tar_x: 0.0, tar_y: 0.0, tar_z: 0.0, roll: 0.0, pitch: 0.0, yaw: 0.0, cur_joint1: 90.0, cur_joint2: 90.0, cur_joint3: 90.0, cur_joint4: 90.0, cur_joint5: 90.0, cur_joint6: 90.0, kin_name: 'fk'}"

这里输入的值有cur_joint1到cur_joint6的值，我们都是输入90.0，还有就是kin_name的值，我们输入'fk'，表示调用fk-正解服务，终端会回复如下图的以下的内容，

查看response部分：


xxxxxxxxxx
response:
arm_interface.srv.ArmKinemarics_Response(joint1=0.0, joint2=0.0, joint3=0.0, joint4=0.0, joint5=0.0, joint6=0.0, x=0.03141308752246765, y=0.00020942581836905875, z=0.5517500187814817, roll=1.5728637148906415, pitch=-1.5707324948694676, yaw=-1.5728927150075942)

我们只需要关心后边的x、y、z、roll、pitch和yaw的值。这里表示的是机械臂末端的位姿坐标，表示的是，在世界坐标系下，机械臂末端的位置，与base_link（0，0，0）为基准点，当机械臂伸直向上的时候，xyz和rpy的值为x=0.03141308752246765, y=0.00020942581836905875, z=0.5517500187814817, roll=1.5728637148906415, pitch=-1.5707324948694676, yaw=-1.5728927150075942，这里在虚拟机启动urdf显示查看下，虚拟机终端输入以下指令启动urdf显示，


xxxxxxxxxx
ros2 launch yahboom_M3Pro_description display_launch.py

如下图所示，使用TF插件来查看Gripping的位姿，xyz的坐标值与response的值相差无几，rpy的值则需要通过四元数转rpy来获取。

2.2、调用ik

我们调用fk来计算下：当机械臂，机械臂末端的位姿是x=0.03141308752246765, y=0.00020942581836905875, z=0.5517500187814817, roll=1.5728637148906415, pitch=-1.5707324948694676, yaw=-1.5728927150075942的时候，每个舵机的是多少。其实这里就是反推回去了，理论上结果应该是六个舵机的值都是90.0。终端输入以下指令，


xxxxxxxxxx
ros2 service call /get_kinemarics arm_interface/srv/ArmKinemarics  "{tar_x: 0.03141308752246765, tar_y: 0.00020942581836905875, tar_z: 0.5517500187814817, roll: 1.5728637148906415, pitch: -1.5707324948694676, yaw: -1.5728927150075942, cur_joint1: 0.0, cur_joint2: 0.0, cur_joint3: 0.0, cur_joint4: 0.0, cur_joint5: 0.0, cur_joint6: 0.0, kin_name: 'ik'}"

这里输入的值是xyz和rpy，cur_joint1-cur_joint6我们使用默认的就行，结果如下图所示，

最后返回的response值是如下，


xxxxxxxxxx
arm_interface.srv.ArmKinemarics_Response(joint1=90.0, joint2=90.0, joint3=90.0, joint4=90.0, joint5=90.0, joint6=0.0, x=0.0, y=0.0, z=0.0, roll=0.0, pitch=0.0, yaw=0.0)

这里只需要关注joint1-joint5的值，因为机械臂的末端是Gripping，连接到的是5号舵机，6号的舵机的值不在逆解算的范围，所以这里获取到的值[90.0,90.0,90.0,90.0,90.0]，与机械臂当前的姿态下的每个舵机值一样，可认为逆解算出来的结果是正确的。

3、核心代码解析

程序代码路径：

树莓派5和Jetson-Nano主板
程序代码在运行的docker中。docker中的路径为 /root/yahboomcar_ws/src/arm_kin/src/kin_srv.cpp
Orin主板
程序代码路径为/home/jetson/yahboomcar_ws/src/arm_kin/src/kin_srv.cpp

主函数main，


xxxxxxxxxx
int main(int argc,char **argv)
{
    rclcpp::init(argc, argv);
    rcutils_logging_set_logger_level("kdl_parser", RCUTILS_LOG_SEVERITY_ERROR);
    auto node = rclcpp::Node::make_shared("kinemarics_arm");
    //创建一个服务，服务名称是get_kinemarics，服务回调函数为handle_service
    auto service = node->create_service<arm_interface::srv::ArmKinemarics>("get_kinemarics", handle_service);
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;   
}

服务回调函数handle_service，


x
void handle_service(
  const std::shared_ptr<arm_interface::srv::ArmKinemarics::Request> request,
  std::shared_ptr<arm_interface::srv::ArmKinemarics::Response> response)
{
  cout<<"-----------------"<<endl;
  cout<<request->kin_name<<endl;
  
      if (request->kin_name == "fk") {
        double joints[]{request->cur_joint1, request->cur_joint2, request->cur_joint3, request->cur_joint4,
                        request->cur_joint5,request->cur_joint6};
        // 定义目标关节角容器
        vector<double> initjoints;
        // 定义位姿容器
        vector<double> initpos;
        // 目标关节角度单位转换,由角度转换成弧度
        for (int i = 0; i < 6; ++i) initjoints.push_back((joints[i] - 90) * DE2RA);
        //调用fk，获取目标位姿initpos
        arm_getFK(urdf_file, initjoints, initpos);
        response->x = initpos.at(0);
        response->y = initpos.at(1);
        response->z = initpos.at(2);
        response->roll = initpos.at(3);
        response->pitch = initpos.at(4);
        response->yaw = initpos.at(5);
        cout<<"-----------------"<<endl;
    }
    
    if (request->kin_name == "ik") {
        // 抓取的位姿
        double Roll = request->roll ;
        double Pitch = request->pitch;
        double Yaw = request->yaw ;
        double x=request->tar_x;
        double y=request->tar_y;
        double z=request->tar_z;
        // 末端位置(单位: m)
        double xyz[]{x, y, z};
        cout << x << y << z << endl;
        // 末端姿态(单位: 弧度)
        //double rpy[]{Roll * DE2RA, Pitch * DE2RA, Yaw * DE2RA};
        double rpy[]{Roll , Pitch, Yaw };
        // 创建输出角度容器
        vector<double> outjoints;
        // 创建末端位置容器
        vector<double> targetXYZ;
        // 创建末端姿态容器
        vector<double> targetRPY;
        for (int k = 0; k < 3; ++k) targetXYZ.push_back(xyz[k]);
        for (int l = 0; l < 3; ++l) targetRPY.push_back(rpy[l]);
        // //调用fk，获取目标舵机角度outjoints
        arm_getIK(urdf_file, targetXYZ, targetRPY, outjoints);
        // 打印反解结果
        for (int i = 0; i < 5; i++) cout << (outjoints.at(i) * RA2DE) + 90 << ",";
        cout << endl;
        a++;
        response->joint1 = (outjoints.at(0) * RA2DE) + 90;
        response->joint2 = (outjoints.at(1) * RA2DE) + 90;
        response->joint3 = (outjoints.at(2) * RA2DE) + 90;
        response->joint4 = (outjoints.at(3) * RA2DE) + 90;
        response->joint5 = (outjoints.at(4) * RA2DE) + 90;
        cout<<"-----------------"<<endl;
    }
}