通过ROS控制真实机械臂(5)—Moveit!真实机械臂(move_group和demo.launch修改)

306
0
2021年1月26日 09时17分

# 开始控制真实世界的机械臂 #######

 

我们的目的是使用moveit控制真实的机械臂,而我们真正需要的action是FollowJointTrajectoryAction,这个action是moveit留出来专门控制真实机器人的 。启动刚开始生成的moveit!配置文件中的demo.launch文件后,就能看见FollowJointTrajectoryAction,可以看一下control_msgs/FollowJointTrajectory Action的具体定义,它的内部有一个重要成员 trajectory,当你使用moveit规划了一个机械臂运动路径时,它记录了机械臂运动的各个轨迹点,每个轨迹点以机械臂关节角的形式表述,这个数据也是我们最终控制机械臂所想要的最终数据要想使用moveit来控制真实机械臂,我们需要修改配置文件夹下的几个文件,因为默认生成的moveit配置文件中,所使用的部分参数是针对虚拟机械臂的,可以在rviz环境下观察模型的运动,但真正的控制信号并不会发出来!但真正的控制信号并不会发出来!但真正的控制信号并不会发出来!重要的事情讲三遍

 

按照上一篇博客的说明,如下操作和修改,让move_group将控制信号发出来。

 

具体要修改以下几个地方:

 

##1、demo.launch文件中参数fake_execution的值改为false

 

  <arg name="fake_execution" value="false"/>

 

2、修改moveit_controller_manager参数

 

move_group.launch文件中,moveit_controller_manager在选择参数值时,“unless”前面那个velue值要修改,写一个自己机器人名称作为前缀

 

  <arg name="moveit_controller_manager" value="redwall_arm" unless="$(arg fake_execution)"/>

 

moveit_controller_manager的参数是有选择的,要么等于“redwall_arm”,要么等于“fake”,这要取决于后面的参数fake_execution, 而这个参数我们上一步已经改为了false,即当前moveit_controller_manager应该等于“redwall_arm”接下来,这个参数会传递给trajectory_execution.launch.xml文件,该文件中有下面这句话,按照原先的设置,本应该启动fake_moveit_controller_manager.launch.xml,而现在redwall_arm_moveit_controller_manager.launch.xml将会被启动

 

   <include file="$(find moveit)/launch/$(arg moveit_controller_manager)_moveit_controller_manager.launch.xml" />

 

##3、修改redwall_arm_moveit_controller_manager.launch.xml文件

 

双击打开这个文件,发现里面只写了一对launch标签,基本算是一个空文件,说明moveit早就帮你留好了位置,等着填充,之前已经填写完成了,如下修改

 

<launch>
 
    <!-- Set the param that trajectory_execution_manager needs to find the controller plugin -->
    <arg name="moveit_controller_manager" default="moveit_simple_controller_manager/MoveItSimpleControllerManager" />
    <param name="moveit_controller_manager" value="$(arg moveit_controller_manager)"/>
 
    <arg name="use_controller_manager" default="true" />
    <param name="use_controller_manager" value="$(arg use_controller_manager)" />
 
    <!-- load controller_list -->
    <rosparam file="$(find redwall_arm_moveit_config)/config/controllers.yaml"/>
 
</launch>

 

4、创建controllers.yaml配置文件

 

上一步的代码最后一句指向了config目录下一个叫做controller.yaml的文件,决定了你所使用的moveit控制器的基本参数。

 

controller_list:
  - name: redwall_arm
    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory
    joints:
    - Link1
    - Link2
    - Link3
    - Link4
    - Link5

 

5、解决真实机械臂与joint_states_publisher消息冲突的问题

 

当控制真实机械臂时,moveit的配置文件中,demo.launch文件中关于joint_states_publisher的一段代码需要注释掉,
因为该程序也在发布机械臂关节状态,而且是一个模拟的状态,与真实机械臂状态是不相符的,若不注释掉该代码,rviz下显示的机械臂状态总是在不停的跳变,将原本的注释,使用真实的传感器发布的/joint_states来控制。

 

  <!-- We do not have a robot connected, so publish fake joint states -->
  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
    <param name="/use_gui" value="$(arg use_gui)"/>
    <!--rosparam param="/source_list">[/move_group/fake_controller_joint_states]</rosparam -->
    <rosparam param="/source_list">[/joint_states]</rosparam >
  </node>

 

##6、解决真实机械臂轨迹执行时间超时的问题。

 

moveit配置文件中有几个参数还需要添加上,否则后续真实机械臂执行轨迹时会报超时的错误。需要在move_group.launch文件中增加几个参数,来延长允许执行轨迹的时间。 找到<node name=”move_group”> 所对应的标签组,在其中加入以下参数

 

<param name="trajectory_execution/allowed_execution_duration_scaling" value="6"/>      //允许轨迹执行时间的一个放大倍数,可以根据实际情况自行修改
    <param name="trajectory_execution/allowed_goal_duration_margin" value="0.5"/>            //超时的一个百分比范围
    或者加入下面的参数,直接关掉监视轨迹执行状况的一个monitoring
    <param name="trajectory_execution/execution_duration_monitoring" value="false"/>

 

至此,moveit!已经修改完成,先启动试试看!!!

 

$ roslaunch redwall_arm_moveit_config demo.launch 

 

出现如下报错信息:

 

[ INFO] [1564730909.628248268]: Constructing new MoveGroup connection for group 'arm_group' in namespace ''
[ INFO] [1564730910.984667942]: MoveItSimpleControllerManager: Waiting for redwall_arm/follow_joint_trajectory to come up
[ERROR] [1564730915.984849668]: MoveItSimpleControllerManager: Action client not connected: redwall_arm/follow_joint_trajectory
[ INFO] [1564730916.087709422]: Returned 0 controllers in list
[ INFO] [1564730916.107154984]: Trajectory execution is managing controllers

 

奇怪,明明fake_controllers调用好好的,到真实的controllers咋就不行了,为啥没有action client连接呢。使用rostopic list 查看发布的消息

 

微信图片_20210120161347

 

发现竟然没有找到follow_joint_trajectory消息,难道move_group没有发布吗? 纠结了一天,终于找到原因了,之前虚拟机械臂有gazebo的ros_control插件自动帮我们获取了follow_joint_trajectory的动作action信息,现在到真实机器人了,需要我们自己写程序了。问题找到了,最重要的是要理解一点,move_group此时是follow_joint_trajectory的client端,动作消息是client端的move_group发布的,听起来有点绕,但是千万不要把报错信息的action client not connect搞混淆了,我们现在client是存在且被感知的(就是move_group),而之所以报错是因为我缺少了action server端,我们需要添加一个server订阅这个action。

 

如下添加一个server节点启动即可

 

#include <ros/ros.h>
#include <iostream>
#include <actionlib/server/simple_action_server.h>
#include <control_msgs/FollowJointTrajectoryAction.h>
#include <std_msgs/Float32MultiArray.h>
 
#include <sstream>
#include <stdlib.h>
#include <sys/types.h>
#include <stdio.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <string.h>
#include <signal.h>
#include <arpa/inet.h>
#include <unistd.h>
#include <errno.h>
#include <pthread.h>
 
using namespace std;
 
typedef actionlib::SimpleActionServer<control_msgs::FollowJointTrajectoryAction> Server;
 
std::basic_string<char> Link1;
std::basic_string<char> Link2;
std::basic_string<char> Link3;
std::basic_string<char> Link4;
std::basic_string<char> Link5;
std::basic_string<char> Link6;
 
/* 目标位置 */
double cp1;
double cp2;
double cp3;
double cp4;
double cp5;
double cp6;
 
/* 目标位置 */
double tp1;
double tp2;
double tp3;
double tp4;
double tp5;
double tp6;
 
/* 收到action的goal后调用的回调函数 */
void execute(const control_msgs::FollowJointTrajectoryGoalConstPtr& goal, Server* as)
{
	//ros::Rate rate(1);
 
    Link1 = goal->trajectory.joint_names[0];
    Link2 = goal->trajectory.joint_names[1];
    Link3 = goal->trajectory.joint_names[2];
    Link4 = goal->trajectory.joint_names[3];
    Link5 = goal->trajectory.joint_names[4];
    Link6 = goal->trajectory.joint_names[5];
 
    cp1 = goal->trajectory.points[0].positions[0];
    cp2 = goal->trajectory.points[0].positions[1];
    cp3 = goal->trajectory.points[0].positions[2];
    cp4 = goal->trajectory.points[0].positions[3];
    cp5 = goal->trajectory.points[0].positions[4];
    cp6 = goal->trajectory.points[0].positions[5];
 
    tp1 = goal->trajectory.points[1].positions[0];
    tp2 = goal->trajectory.points[1].positions[1];
    tp3 = goal->trajectory.points[1].positions[2];
    tp4 = goal->trajectory.points[1].positions[3];
    tp5 = goal->trajectory.points[1].positions[4];
    tp6 = goal->trajectory.points[1].positions[5];
 
	// 并且按照1hz的频率发布进度feedback
	// control_msgs::FollowJointTrajectoryFeedback feedback;
    //feedback = NULL;
    //as->publishFeedback(feedback);
 
    // 当action完成后,向客户端返回结果
    //printf("goal=[%f,%f,%f,%f,%f,%f]\n",tp1,tp2,tp3,tp4,tp5,tp6);
    ROS_INFO("Recieve action successful!");
    as->setSucceeded();
}
 
 
/* 主函数主要用于动作订阅和套接字通信 */
int main(int argc, char** argv)
{
	ros::init(argc, argv, "trajectory_server");
	ros::NodeHandle nh;
	// 定义一个服务器
	Server server(nh, "redwall_arm/follow_joint_trajectory", boost::bind(&execute, _1, &server), false);
	// 服务器开始运行
	server.start();
	ros::spin();
 
}
 

 

再次启动demo.launch和server节点:

 

微信图片_20210120161421

 

点击 plan and execute之后查看上述action内容:

 

 $ rostopic echo /redwall_arm/follow_joint_trajectory/goal

 

微信图片_20210120161442

 

##last补充、kinematic.yaml

 

最近完成了伺服电机控制的机器人的程序调试,发现了之前缺少的一些步骤,比如刚开始生产的moveit_config包下的kinematic.yaml文件是空的,然后每次启动demo程序之后拖动marker小球之后,机器人无法选取目标位置和姿态,找了很久发现是这个问题.如下所示,其中arm_group是通过建模时设定的规划组.

 

arm_group:
  kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
  kinematics_solver_search_resolution: 0.005
  kinematics_solver_timeout: 0.005
  kinematics_solver_attempts: 3

 

涉及到底层的控制:

 

1、机械臂底层驱动已经完成。
预先完成好底层驱动控制部分,如之前的文章通过手柄控制的机械臂。

 

2、机械臂能够按照路点(轨迹)运动
这应该是一般机械臂的一项基本功能。这里的轨迹由一个个路点组成,路点可以理解为机械臂在空间中运动时必须通过的点,一个个路  点串起来就形成了轨迹。路点的数据结构一般是一组关节角度,比如5自由度机械臂,有5个独立关节,其确定了机械臂在空间的唯一位置,那么,就可以用 [ joint1,joint2,joint3,joint4,joint5] 来表示一个路点。因为moveit最终发出来的控制指令的实质就是一连串的路点,只要机械臂能够执行这一串路点 就达到了机械臂按照规划的轨迹运动的目的。

 

3、ros_control除了作为action的服务端,还需要通过joint_states话题发布自身的关节角move_group订阅了该话题,就能够获取机械臂当前的状态了,joint_states话题存储了机械臂的当前真实的关节角。因此,构造该话题时,要通过机械臂开放给用户的编程接口,一般机械臂都允许用户获取关节角的~

 

4、 关于action服务端写在哪的问题,action服务端的编写需要结合你自己的机械臂来进行,因为它要将moveit的控制指令翻译给机械臂的驱动,action的一个优点就是避免了繁杂的话题处理,使用action处理两个节点的连接时客户端向服务端发送请求,服务端会在相应的回调函数里通过参数直接得到这个请求数据,数据的传递过程被action这种机制隐藏了,虽然底层也是话题传递,但没必要再找话题订阅者或发布者了

 

5、 moveit配置的时候movegroup注意名称,因为trajectory_gen中使用的是arm_group,名称要统一

 

6、出现如下报错,将URDF中base_link的inertia属性去除即可

 

The root link base_link has an inertia specified in the URDF, but KDL does not support a root link with an inertia. As a workaround, you can add an extra dummy link to your URDF

 

微信图片_20210120161514

发表评论

后才能评论