睿尔曼超轻量仿人机械臂之在ROS里引用正逆解函数库

一、机械臂运动学正逆解说明

机械臂运动学就是根据未端执行器与所选参考坐标系之间的几何关系，确定末端执行器的空间位置和姿态与各关节变量之间的数学关系。包括正运动学 (Forward Kinematics)和逆运动学 (Inverse Kinematics)两部分。

机械臂运动分为关节空间运动和笛卡尔空间运动2种方式，2种运动方式分别对应机械臂运动学算法的正解和逆解，在不同的应用场景下可以使用不同的运动方式。

两种运动方式之间的不同可总结为以下几点：

表示方式不同。关节空间运动发给机械臂的是机械臂各个关节的角度，从而控制机械臂运动到某个位置；笛卡尔空间运动发给机械臂的是在笛卡尔空间种机械臂末端的位置和姿态（简称为位姿），目前大多数机械臂用来表达位姿使用(x,y,z,rx,ry,rz)，其中姿态表达为欧拉角；
控制方式不同。在关节空间中，通过控制各个关节的角度实现控制机械臂运动；在笛卡尔空间中，通过控制机械臂末端的位置和姿态实现控制机械臂运动；
运动规划不同。关节空间运动规划是对每个关节进行规划，计算每个关节的插补点；笛卡尔空间运动规划是在笛卡尔空间规划末端轨迹，然后每个插补点再去逆解，计算对应的每个关节的角度。
控制自由度不同。使用笛卡尔空间运动时，机械臂可以由N个自由度组成，只需确保机械臂的末端到达所发位姿即可；关节空间运动时，需要根据机械臂的实际自由度发送相对应数量的关节角度；

睿尔曼六轴机械臂关节运动可使用机械臂示教器、JSON协议、API等多种方式进行控制。

二、RM-65B ROS包架构简析

三、系统环境要求

经过睿尔曼测试的操作系统版本： Ubuntu 20.04

经过睿尔曼测试的ROS版本： ROS1 Noetic

未经过测试的操作系统版本与ROS版本，用户根据需求，可自行尝试可行性

四、在ROS里引用正逆解函数库

1、创建一个功能包或者进入现有ROS功能包，新建"include"以及"lib"文件夹（如果不存在）。

2、将API中提供的头文件复制到"include"文件夹，将"Linux"文件夹中的对应版本的so库文件复制到"lib"文件夹。

3、在ROS功能包中的CMakeLists.txt文件中，添加对引用库的依赖

include_directories(

include

${catkin_INCLUDE_DIRS}

)

link_directories(

${catkin_LIB_DIRS} lib

)

4、编译功能包

5、引用头文件，在ROS代码中使用API库中的算法接口：

extern "C"{

#include "rm_base.h"

}

#include <math.h>

#include <ros/ros.h>

#include <rm_msgs/MoveJ.h>

#include <rm_msgs/MoveJ_P.h>

#include <rm_msgs/Plan_State.h>

using namespace std;

// 接收到订阅的机械臂执行状态消息后，会进入消息回调函数

void planStateCallback(const rm_msgs::Plan_State::ConstPtr& msg)

{

// 将接收到的消息打印出来，显示机械臂是否完成运动

if(msg->state)

{

ROS_INFO("*******Plan State OK");

} else {

ROS_INFO("*******Plan State Fail");

}

int main(int argc, char **argv)

{

// 初始化ROS节点

ros::init(argc, argv, "kinematics_demo");

// 创建节点句柄

ros::NodeHandle nh;

// 声明spinner对象，参数2表示并发线程数，默认处理全局Callback队列

ros::AsyncSpinner spin(2);

// 启动两个spinner线程并发执行可用回调

spin.start();

// 空间规划指令Publisher

ros::Publisher moveJ_P_pub = nh.advertise<rm_msgs::MoveJ_P>("/rm_driver/MoveJ_P_Cmd", 10);

// 订阅机械臂执行状态话题

ros::Subscriber planState_sub = nh.subscribe("/rm_driver/Plan_State", 10, planStateCallback);

//初始化

int dmode = ARM_65;

RobotType rbt_type = RM65B;

Algo_Init_Sys_Data(dmode,rbt_type);

//设置安装角度

Algo_Set_Angle(-90,90,0);

ros::Duration(2.0).sleep();

//joint记录机械臂的目标角度

float joint[6] = {81.001717,104.540276,-104.602264,-158.27413,78.476189,77.162445};

float q_out[6];

//调用运动学正解api，输入为目标位置关节角度，输出为目标位置机械比位姿（位置+欧拉角形式）

Pose now = Algo_Forward_Kinematics(joint);

ROS_INFO("Now Flange: %f, %f, %f, %f, %f, %f\n",now.position.x,now.position.y,now.position.z,now.quaternion.x,now.quaternion.y,now.quaternion.z,now.quaternion.w);

ros::waitForShutdown();

return 0;

}

// //将目标位姿通过moveJ_P发送给机械臂，控制机械臂运动到目标位姿

rm_msgs::MoveJ_P moveJ_P_target;

moveJ_P_target.Pose.position.x = now.position.x;

moveJ_P_target.Pose.position.y = now.position.y;

moveJ_P_target.Pose.position.z = now.position.z;

moveJ_P_target.Pose.orientation.x = now.quaternion.x;

moveJ_P_target.Pose.orientation.y = now.quaternion.y;

moveJ_P_target.Pose.orientation.z = now.quaternion.z;

moveJ_P_target.Pose.orientation.w= now.quaternion.w;