使用Baxter仿真器学习一下机械臂的控制

Baxter simulator由ReThink Robotics公司提供，是一个合理可靠的机器人模型。仿真器和实体机器人模型都有相同的ROS接口，所以在仿真器下开发的程序都可以在相应的实体机器人上快速部署。

我准备使用Baxter模型和仿真器来练习正逆运动学、角空间规划、笛卡尔规划、运动伺服和抓取的执行。

运行Baxter simulator

使用以下命令开始仿真界面：

roslaunch baxter_gazebo baxter_world.launch

启动文件如下

<launch>

  
  <arg name="paused" default="false"/>
  <arg name="use_sim_time" default="true"/>
  <arg name="gui" default="true"/>
  <arg name="headless" default="false"/>
  <arg name="debug" default="false"/>

  
  <arg name="left_electric_gripper" default="true"/>
  <arg name="right_electric_gripper" default="true"/>

  
  
  <arg name="load_robot_description" default="true"/>
  <param if="$(arg load_robot_description)" name="robot_description"
      command="$(find xacro)/xacro.py --inorder $(find baxter_description)/urdf/baxter.urdf.xacro gazebo:=true"/>

  
  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="world_name" value="$(find baxter_gazebo)/worlds/baxter.world"/>
    <arg name="debug" value="$(arg debug)" />
    <arg name="gui" value="$(arg gui)" />
    <arg name="paused" value="$(arg paused)"/>
    <arg name="use_sim_time" value="$(arg use_sim_time)"/>
    <arg name="headless" value="$(arg headless)"/>
  include>

  
  <param name="rethink/software_version" value="1.2.0" />

  
  <node pkg="tf2_ros" type="static_transform_publisher" name="base_to_world" args="0 0 0 0 0 0 1 world base" />

  
   <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
    args="-param robot_description -urdf -z 0.93 -model baxter
          -J baxter::right_s0 -0.272659
          -J baxter::right_s1 1.04701
          -J baxter::right_e0 -0.00123203
          -J baxter::right_e1 0.49262
          -J baxter::right_w0 -0.0806423
          -J baxter::right_w1 -0.0620532
          -J baxter::right_w2 0.0265941
          -J baxter::left_s0 0.192483
          -J baxter::left_s1 1.047
          -J baxter::left_e0 0.000806359
          -J baxter::left_e1 0.491094
          -J baxter::left_w0 -0.178079
          -J baxter::left_w1 -0.0610333
          -J baxter::left_w2 -0.0124707" />

  
  <include file="$(find baxter_sim_hardware)/launch/baxter_sdk_control.launch">
      <arg name="right_electric_gripper" value="$(arg right_electric_gripper)"/>
      <arg name="left_electric_gripper" value="$(arg left_electric_gripper)"/>
  include>

launch>

这个过程启动了带有Baxter机器人的Gazebo环境。启动过程有些慢。仿真过程取消了重力补偿的影响。

使用以下命令打开关节使能，监听运动命令

rosrun baxter_tools enable_robot.py -e

使用以下命令将机器人的手臂移动到便于运输的姿势，合起来

rosrun baxter_tools tuck_arms.py -t

使用以下命令可以把他拆开

rosrun baxter_tools tuck_arms.py -u

另一个有趣的例子是

rosrun baxter_examples joint_velocity_wobbler.py

使机器人到达一个初始命令后进行小幅度的正弦运动。

Baxter的关节和话题topic

Baxter机器人 有15个伺服自由度，包括七个左手自由度和七个右手自由度和一个脖子左右运动的自由度。头部也可以进行点头运动，但是是通过二进制命令控制的。Baxter机器人和仿真器包含三个摄像机：一个在头部，另外两个分别在左右手腕部。其它传感器包括顶点周围的声呐、每个工具法兰的短程红外线距离传感器、伺服关节的角度传感器和关节力矩传感器。

启动rviz可以让我们观察到更多的机器人信息。

在数学上，为了控制六个自由度的末端抓手的姿态，需要至少六个独立的关节自由度。然而，一旦关节施加了限制，就很难通过六个关节达到六个自由度的目标姿态了。使用七个自由度就戏剧性的提高了可操控性。同时，这引入了另一项挑战，那就是运动学求解，因为手臂是运动学冗余的。越来越多的工业机器人提供了七个自由度可控关节的机器人，因此运动学冗余求解也越来越重要。

Baxter的另一项运动学挑战是机器人没有球形腕部关节，即末端的三个关节轴不是相交于一点的。球形腕部关节求解运动学逆解的解析解时比较方便。而Baxter机器人最后两个旋转轴相交于一点，但是前臂旋转轴并不与他们相交，有一定的偏移。由于偏差很小，有时可以忽略，近似成球形关节，计算运动学逆解，可以快速地得到一个运动学逆解。

rostopic list可以查看话题

Baxter的抓手

Baxter仿真模型中的抓手是电子的，平口钳抓手。与ROS进行通信是采用发布和订阅消息的方式。抓手的状态发布在/robot/endeffector/rightgripper/state和/robot/endeffector/leftgripper/state话题上，使用的消息类型是baxtercoremsgs/EndeffectorState。该消息定义定义了多个助记符和16个字段。这样，描述抓手状态的值有位置和力，用来检测一个物体是否被抓住或者抓手是空的。

为了控制抓手，需要向话题/robot/endeffector/rightgripper/command and /robot/endeffector/leftgripper/command发布命令，发布消息的类型是baxtercoremsgs/EndEffectorCommand。这个消息包含了12个助记字符串用来与抓手和命令消息的五个作用域沟通。

就目前而言，简单地命令夹具处于位置模式并且命令完全打开（100）或完全关闭（0）的位置就足够了，这对应于44mm的抛光。 如果夹爪之间夹有可抓物体（既不太宽也不太窄），指挥完全关闭的运动将导致夹爪在达到完全关闭位置之前失速。 这可以通过检查夹具状态来检测。

为了简化抓取器接口，在同名包中定义了一个simple_baxter_gripper_interface库。 该库定义了一个类BaxterGripper，它为左右夹具设置发布者和订阅者。 夹具命令消息定义并填充：

// baxter_gripper:   a library to simplify gripper I/O
// wsn, August, 2016
#include<ros/ros.h>
#include<simple_baxter_gripper_interface/simple_baxter_gripper_interface.h>

BaxterGripper::BaxterGripper(ros::NodeHandle* nodehandle): nh_(*nodehandle) {
    gripper_cmd_open.id = 65538;
    gripper_cmd_open.command ="go";
    //gripper_cmd_open.args = "{'position': 100.0}'"; //oops
    gripper_cmd_open.args = "{\"position\": 100.0}";
    gripper_cmd_open.sender = "gripper_publisher";
    gripper_cmd_open.sequence = 2;

    gripper_cmd_close.id = 65538;
    gripper_cmd_close.command ="go";
    //gripper_cmd_close.args = "{'position': 0.0}'"; //oops
    gripper_cmd_close.args = "{\"position\": 0.0}";
    gripper_cmd_close.sender = "gripper_publisher"; 
    gripper_cmd_close.sequence = 3;

    gripper_pos_filter_val_ = 0.2;
    right_gripper_pos_ = -0.2;
    left_gripper_pos_ = -10.2;
    initializeSubscribers(); 
    initializePublishers();
}
void BaxterGripper::initializeSubscribers()
{
    ROS_INFO("Initializing Subscribers");
    gripper_subscriber_right_ = nh_.subscribe("/robot/end_effector/right_gripper/state", 1, &BaxterGripper::right_gripper_CB,this); 
    gripper_subscriber_left_ = nh_.subscribe("/robot/end_effector/left_gripper/state", 1, &BaxterGripper::left_gripper_CB,this);     
}

void BaxterGripper::initializePublishers()
{
    ROS_INFO("Initializing Publishers");
    gripper_publisher_right_ = nh_.advertise<baxter_core_msgs::EndEffectorCommand>("/robot/end_effector/right_gripper/command", 1, true); 
    gripper_publisher_left_ = nh_.advertise<baxter_core_msgs::EndEffectorCommand>("/robot/end_effector/left_gripper/command", 1, true); 
}
void BaxterGripper::right_gripper_CB(const baxter_core_msgs::EndEffectorState& gripper_state) {
     //low-pass filter the gripper position for more reliable threshold tests
    right_gripper_pos_ = (1.0- gripper_pos_filter_val_)*right_gripper_pos_ + gripper_pos_filter_val_*gripper_state.position; 
} 
void BaxterGripper::left_gripper_CB(const baxter_core_msgs::EndEffectorState& gripper_state) {
     //low-pass filter the gripper position for more reliable threshold tests
    left_gripper_pos_ = (1.0- gripper_pos_filter_val_)*left_gripper_pos_ + gripper_pos_filter_val_*gripper_state.position; 
} 

位置反馈可能会出现噪声，所以在回调函数中使用低通滤波器。
控制抓手的例子如下：

// baxter_gripper_lib_test_main: 
// wsn, August, 2016
// illustrates use of library/class BaxterGripper for simplified Baxter gripper I/O

#include<ros/ros.h>
#include<simple_baxter_gripper_interface/simple_baxter_gripper_interface.h>
//using namespace std;

int main(int argc, char** argv) {
    ros::init(argc, argv, "baxter_gripper_test_main"); // name this node 
    ros::NodeHandle nh; //standard ros node handle   

    //instantiate a BaxterGripper object to do gripper I/O
    BaxterGripper baxterGripper(&nh);
    //wait for filter warm-up on right-gripper position
    while (baxterGripper.get_right_gripper_pos()<-0.5) {
        ros::spinOnce();
        ros::Duration(0.01).sleep();
        ROS_INFO("waiting for right gripper position filter to settle; pos = %f", baxterGripper.get_right_gripper_pos());
    }

    ROS_INFO("closing right gripper");
    baxterGripper.right_gripper_close();
    ros::Duration(1.0).sleep();
    ROS_INFO("opening right gripper");
    baxterGripper.right_gripper_open();
    ros::spinOnce();
    ROS_INFO("right gripper pos = %f; waiting for pos>95", baxterGripper.get_right_gripper_pos());
    while (baxterGripper.get_right_gripper_pos() < 95.0) {
        baxterGripper.right_gripper_open();
        ros::spinOnce();
        ROS_INFO("gripper pos = %f", baxterGripper.get_right_gripper_pos());
        ros::Duration(0.01).sleep();
    }

    ROS_INFO("closing left gripper");
    baxterGripper.left_gripper_close();
    ros::Duration(1.0).sleep();
    ROS_INFO("opening left gripper");
    baxterGripper.left_gripper_open();

    ROS_INFO("closing right gripper");
    baxterGripper.right_gripper_close();
    ros::spinOnce();
    ROS_INFO("right gripper pos = %f; waiting for pos<90", baxterGripper.get_right_gripper_pos());
    while (baxterGripper.get_right_gripper_pos() > 90.0) {
        baxterGripper.right_gripper_close();
        ros::spinOnce();
        ROS_INFO("gripper pos = %f", baxterGripper.get_right_gripper_pos());
        ros::Duration(0.01).sleep();
    }

    return 0;
}

可以使用

rosrun simple_baxter_gripper_interfacbaxter_gripper_lib_test_main

查看效果。

头部控制

只需要发布一个头部角度命令：

#include 
#include 
using namespace std;

int main(int argc, char **argv) {
    ros::init(argc, argv, "baxter_head_pan"); 
    ros::NodeHandle n; 
    //create a publisher to send commands to Baxter's head pan
    ros::Publisher head_pan_pub = n.advertise("/robot/head/command_head_pan", 1);

    baxter_core_msgs::HeadPanCommand headPanCommand; //message type for head-pan control
    headPanCommand.target = 0.0;
    headPanCommand.enable_pan_request=1;
    //prompt user for amplitude and frequency of oscillating head pan
    double amp,freq;
    cout<<"enter pan amplitude (rad): ";
    cin>>amp;
    cout<<"enter pan freq (Hz): ";
    cin>>freq;
    double dt = 0.01;
    ros::Rate timer(1/dt);
    double phase=0.0;
    double theta;

    // oscillate head pan indefinitely
    while (ros::ok()) 
    {
        phase+= M_PI*2.0*freq*dt;
        if (phase>2.0*M_PI) phase-= 2.0*M_PI;
        theta = amp*sin(phase);
        headPanCommand.target = theta;
        head_pan_pub.publish(headPanCommand);
        timer.sleep();
    }
}

可以使用

rosrun baxter_head_pan baxter_head_pan

观察效果

关节控制命令

Baxter仿真器订阅话题/robot/limb/right/jointcommand和/robot/limb/left/jointcommand来接收关节位置命令控制左右手臂。消息类型是/baxter_core_msgs/JointCommand。使用以下命令可以查看消息类型

rosmsg show baxter_core_msgs/JointCommand

结果是

int32 POSITION_MODE=1
int32 VELOCITY_MODE=2
int32 TORQUE_MODE=3
int32 RAW_POSITION_MODE=4
int32 mode
float64[] command
string[] names

位置模式下，关节角度控制命令的发布方式如图

为了灵活的插入和执行轨迹，需要使用action server。

启动轨迹插值动作服务器（左右手）：

rosrun baxter_trajectory_streamer rt_arm_as
rosrun baxter_trajectory_streamer left_arm_as

服务器接收并执行轨迹，等待从action客户端接收目标位置。
使用以下命令可以发送一个姿态命令

rosrun baxter_trajectory_streamer pre_pose

使用关节轨迹控制器

……