¶ 卓尔-ARM机械臂教程
¶ 机械臂通讯架构
卓尔ARM机器人提供以太网和 WLAN 连接。在卓尔ARM机器人中，机械臂与 Orin Nano 主控通过网线连接到路由器形成一个局域网，用户可以通过局域网连接到机器人进行控制。
机械臂的控制方式有两种：一种是通过乐白机器人提供的网页端 SDK 进行控制，另一种是通过 ROS 控制。在 ROS 控制中，用户可以通过 ROS 控制机械臂的运动，也可以通过 ROS 控制机械臂的传感器。
网页端SDK，是在连接到机器人路由器辐射出的局域网后，通过浏览器打开‘192.168.0.50’这个IP地址来访问。
操作指南如下链接：
** 机械臂Moveit规划的代码在arm_demo功能包下。**
ROS端控制，则是通过ROS的Moveit！架构来控制机械臂的运动。
注意事项：机械臂使用 ROS 控制时，需要在网页端 SDK 里确认六个关节的角度均在 URDF 关节的有效范围内，URDF 的关节有效范围可以根据实际情况自行调整，ROS 里 URDF 的运动范围需要小于等于网页端 SDK 的运动范围，在笛卡尔路径规划中，如遇到机械臂出现‘绕大圈’的情况，可以将特定关节的运动范围调小。URDF 文件路径：lebai_lm3_support/urdf/s300_lm3.xacro。
使用ROS控制机械臂，需要启动下面的launch文件：
roslaunch lebai_lm3_moveit_config real_robot.launch
注意启动前要确认机械臂的电源已经打开，可ping通机械臂，并且在网页端操作正常。
如果一切正常，可以看到如下的结果：
这时，就可以使用卓尔-ARM的机械臂进行Movit规划了。
Moveit 是 ROS 中一个用于运动规划的软件框架，可以用于机器人的运动规划、逆运动学求解、碰撞检测等功能。在 Moveit 中，我们可以通过设置目标位姿，来规划机械臂的运动轨迹。
使用Moveit控制机械臂进行规划与控制，需要声明一个 MoveGroupInterface 类型的对象，然后通过这个对象来控制机械臂的运动。
moveit::planning_interface::MoveGroupInterface::Plan arm_plan;
卓尔-ARM的机械臂规划组为‘manipulator’
而由于，机械臂规划控制，是针对末端执行机构的位置和姿态，因此，需要设置末端执行器的名称。
卓尔-ARM的机械臂末端执行器的名称为‘lebai_gripper’
我们将此类封装在了arm_demo的‘lebai_demo.h’中，可以在后续代码中调用。
¶ 机械臂正运动学控制
机械臂正解，给定机械臂各个关节的角度，计算出机器人末端在空间中的位置和姿态，给定了各个角度值之后，直接调用 moveit 中的 setJointValueTarget()函数设置目标关节角度值，再通过 move()函数执行。
封装代码如下所示：
void moveJ(double j1,double j2,double j3,double j4,double j5,double j6)
{
std::vector<double> joint={j1,j2,j3,j4,j5,j6};//机械臂每个关节旋转角度
manipulator->setJointValueTarget(joint);//设置目标点关节值
manipulator->move();//移动
}
调用代码如下所示：
#include "arm_demo/lebai_demo.h"
int main(int argc, char **argv)
{
ros::init(argc, argv, "arm_fk_demo"); //初始化ros节点
ros::AsyncSpinner spinner(1); //异步启停，开启指定数量的Spinner线程并发执行Callback队列中的可用回调。
spinner.start();
LeBai lebai("manipulator","lebai_gripper");
lebai.set_MaxVelocity_Accelerate(1,0.5);//设置运动速度，如需更快的速度可在lebai_lm3_support/urdf/s300_lm3.xacro 文件修改各个jointx_vel_limit的值
lebai.move_Look();
lebai.moveJ(-1.01, -2.14, -1.72, -0.77, 1.9, 3.68);
sleep(1);
lebai.move_Look();
ros::shutdown();
}
可以看到，我们先控制机械臂运动到预设的‘Look’位置内，然后调用 moveJ 函数，传入六个关节的角度值，控制机械臂运动到指定的位置。，然后回到‘Look’位置。
¶ 机械臂逆运动学控制
机械臂逆解即机械臂逆运动学，通过给定机械臂末端的位置和姿态，求解出对应的机械臂各个关节需要旋转的角度值。
moveit！封装的函数如下所示：
void moveP(double x,double y,double z)//逆解，相对base_link的xyz位置，以起始点姿态作为目标点姿态
{
geometry_msgs::Pose target_pose;
target_pose = this->getCurrentPose(end_effector_link).pose;
target_pose.position.x = x;
target_pose.position.y = y;
target_pose.position.z = z;
this->setStartStateToCurrentState();
this->setPoseTarget(target_pose);
moveit::core::MoveItErrorCode success = this->plan(arm_plan);
ROS_INFO("move_p:%s", success ? "SUCCESS" : "FAILED");
if (success) {
this->execute(arm_plan);
sleep(1);
}
}
void moveP(double x,double y,double z,double qx,double qy,double qz,double qw)//逆解，相对base_link的xyz位置，末端姿态
{
geometry_msgs::Pose target_pose;
target_pose.position.x = x;
target_pose.position.y = y;
target_pose.position.z = z;
target_pose.orientation.x = qx;
target_pose.orientation.y = qy;
target_pose.orientation.z = qz;
target_pose.orientation.w = qw;
manipulator->setStartStateToCurrentState();//设置当前状态为起始点
manipulator->setPoseTarget(target_pose);//设置目标位姿
moveit::planning_interface::MoveGroupInterface::Plan plan;
moveit::planning_interface::MoveItErrorCode success =
manipulator->plan(plan);
ROS_INFO("move_p:%s", success ? "SUCCESS" : "FAILED");
if (success) {
manipulator->execute(plan);//如果规划成功就执行
sleep(1);
}
}
调用的代码如下所示：
#include "arm_demo/lebai_demo.h"
int main(int argc, char **argv)
{
ros::init(argc, argv, "arm_fk_demo"); //初始化ros节点
ros::AsyncSpinner spinner(1); //异步启停，开启指定数量的Spinner线程并发执行Callback队列中的可用回调。
spinner.start();
LeBai lebai("manipulator","lebai_gripper");
lebai.set_MaxVelocity_Accelerate(1,0.5);//设置运动速度，如需更快的速度可在lebai_lm3_support/urdf/s300_lm3.xacro 文件修改各个jointx_vel_limit的值
lebai.move_Look();
lebai.moveP(0.45, 0, 0.3);
sleep(1);
lebai.move_Look();
ros::shutdown();
}
先运动到预设的‘Look’位置，然后调用 moveP 函数，传入末端的位置和姿态，控制机械臂运动到指定的位置。
注意，函数参数 x,y,z,qx,qy,qz,qw 为机械臂末端相对底盘 base_link 的位姿。
¶ 笛卡尔路径规划
当机械臂在进行逆解运动时，机械臂末端会选择一条最短的路径去运动，如果这条路径上有障碍物，那么就可能与障碍物发生碰撞，为了避开障碍物，我们可以通过添加路点的方式让机械臂在到达终点的过程中经过我们给定的路点实现绕过障碍物的效果，每两个路点间的运动为直线运动。
封装代码如下：
geometry_msgs::Pose start_pose = arm.getCurrentPose(end_effector_link).pose;
//先获取机械臂末端当前位姿
std::vector<geometry_msgs::Pose> waypoints;
//创建路点容器
start_pose.position.x+=0.05;
waypoints.push_back(start_pose);
//A 点加入路点列表
start_pose.position.y+=0.05;
waypoints.push_back(start_pose);
//B 点加入路点列表
start_pose.position.x-=0.05;
waypoints.push_back(start_pose);
//C 点加入路点列表
start_pose.position.y-=0.05;
waypoints.push_back(start_pose);
// 笛卡尔路径规划
moveit_msgs::RobotTrajectory trajectory;
const double jump_threshold = 0.0; //跳跃值
const double eef_step = 0.01; //步进值
double fraction = 0.0; //返回 1 表示规划成功
int maxtries = 100; //最大尝试规划次数
int attempts = 0; //已经尝试规划次数
while(fraction < 1.0 && attempts < maxtries)
{
fraction = arm.computeCartesianPath(waypoints, eef_step, jump_threshold,trajectory);
attempts++;
if(attempts % 10 == 0)
ROS_INFO("Still trying after %d attempts...", attempts);
}
if(fraction == 1)
{
ROS_INFO("Path computed successfully. Moving the arm.");
// 运动规划数据
moveit::planning_interface::MoveGroupInterface::Plan plan;
plan.trajectory_ = trajectory;
// 执行
arm.execute(plan);
sleep(1);
}
else
{
ROS_INFO("Path planning failed with only %0.6f success after %d attempts.",fraction, maxtries);
}
调用代码如下：
#include "arm_demo/lebai_demo.h"
int main(int argc, char **argv)
{
ros::init(argc, argv, "cartesian_node"); //初始化ros节点
ros::AsyncSpinner spinner(1); //异步启停，开启指定数量的Spinner线程并发执行Callback队列中的可用回调。
spinner.start();
LeBai lebai("manipulator","lebai_gripper");
lebai.set_MaxVelocity_Accelerate(1,0.5);//设置运动速度，如需更快的速度可在lebai_lm3_support/urdf/s300_lm3.xacro 文件修改各个jointx_vel_limit的值
lebai.move_Look();
geometry_msgs::Pose pose;
std::vector<geometry_msgs::Pose> poses;
pose.position.x=0.47;
pose.position.y=0;
pose.position.z=0.2;
pose.orientation.x=0;
pose.orientation.y=0;
pose.orientation.z=0;
pose.orientation.w=1;
poses.push_back(pose);
pose.position.y=0.2;
poses.push_back(pose);
pose.position.y=0;
pose.position.z=0.3;
poses.push_back(pose);
pose.position.y=-0.2;
pose.position.z=0.2;
poses.push_back(pose);
pose.position.y=0;
poses.push_back(pose);
lebai.moveCartesian(poses);
ros::shutdown();
}
可以看到，在上面的代码中，我们控制机械臂末端先去往Look，然后通过 moveCartesian 函数，传入一系列的路点，控制机械臂运动到指定的位置。
¶ 圆弧路径规划
在 Moveit 中，并没有直接的圆弧运动的相关 API，我们需要利用上一节的笛卡尔路径规划来走出圆弧轨迹。首先通过圆的轨迹方程，计算得到一系列圆上的路径点，然后使用 computeCartesianPath 函数完成所有路径点之间的直线轨迹规划，再 execute 执行运动。
封装函数如下：
float a= start_pose.position.x;
float b = start_pose.position.y;
for(float th=0.0; th<6.28; th=th+0.01)
{
start_pose.position.x = a + (radius - radius * cos(th));
start_pose.position.y = b + radius * sin(th);
waypoints.push_back(start_pose);
}
调用函数如下：
#include "arm_demo/lebai_demo.h"
int main(int argc, char **argv)
{
ros::init(argc, argv, "move_circle"); //初始化ros节点
ros::AsyncSpinner spinner(1); //异步启停，开启指定数量的Spinner线程并发执行Callback队列中的可用回调。
spinner.start();
LeBai lebai("manipulator","lebai_gripper");
lebai.set_MaxVelocity_Accelerate(1,0.5);//设置运动速度，如需更快的速度可在lebai_lm3_support/urdf/s300_lm3.xacro 文件修改各个jointx_vel_limit的值
lebai.move_Look();
lebai.move_C("xz",0.1);//xz平面
sleep(1);
ros::shutdown();
}
可以通过修改 start_pose.position 的位置来选择圆弧的运动平面，‘xy’在水平面，’xz’在垂直面，当 th 值的范围为 6.28 为整圆轨迹，3.14 为半圆轨迹。
机械臂正运算、逆运算、笛卡尔曲线、是非常重要的应用，务必掌握。