使用Baxter仿真器學習一下機械臂的控制

Baxter simulator由ReThink Robotics公司提供，是一個合理可靠的機器人模型。仿真器和實體機器人模型都有相同的ROS接口，是以在仿真器下開發的程式都可以在相應的實體機器人上快速部署。

我準備使用Baxter模型和仿真器來練習正逆運動學、角空間規劃、笛卡爾規劃、運動伺服和抓取的執行。

運作Baxter simulator

使用以下指令開始仿真界面：

啟動檔案如下

<?xml version="1.0" encoding="utf-8"?>
<launch>

  <!-- 這是一些控制選項, for example paused:=true -->
  <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"/>

  <!-- 這部分内容加載電子抓手, for example left_electric_gripper:=true -->
  <arg name="left_electric_gripper" default="true"/>
  <arg name="right_electric_gripper" default="true"/>

  <!-- 向ROS參數伺服器中加載URDF模型檔案 -->
  <!-- This xacro will pull in baxter_base.urdf.xacro, left_end_effector.urdf.xacro,
                                           and right_end_effector.urdf.xacro
       Note: if you set this to false, you MUST have set the robot_description prior
             to launching baxter_world -->
  <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"/>

  <!--加載啟動的仿真環境 We resume the logic in empty_world.launch, changing the name of the world to be launched -->
  <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>

  <!-- 在ROS參數伺服器中加載軟體版本 Load the software version into the ROS Parameter Server -->
  <param name="rethink/software_version" value="1.2.0" />

  <!-- 釋出世界坐标系與機器人基座之間的靜态變換 Publish a static transform between the world and the base of the robot -->
  <node pkg="tf2_ros" type="static_transform_publisher" name="base_to_world" args="0 0 0 0 0 0 1 world base" />

  <!--運作一個python腳本，發送服務啟動urdf機器人 Run a python script to the send a service call to gazebo_ros to spawn a URDF robot -->
   <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" />

  <!-- ros_control baxter launch file -->
  <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 tuck_arms.py -t
           

使用以下指令可以把他拆開

rosrun baxter_tools tuck_arms.py -u
           

另一個有趣的例子是

使機器人到達一個初始指令後進行小幅度的正弦運動。

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 = ;
    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 = ;

    gripper_cmd_close.id = ;
    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 = ;

    gripper_pos_filter_val_ = ;
    right_gripper_pos_ = -;
    left_gripper_pos_ = -;
    initializeSubscribers(); 
    initializePublishers();
}
void BaxterGripper::initializeSubscribers()
{
    ROS_INFO("Initializing Subscribers");
    gripper_subscriber_right_ = nh_.subscribe("/robot/end_effector/right_gripper/state", , &BaxterGripper::right_gripper_CB,this); 
    gripper_subscriber_left_ = nh_.subscribe("/robot/end_effector/left_gripper/state", , &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", , true); 
    gripper_publisher_left_ = nh_.advertise<baxter_core_msgs::EndEffectorCommand>("/robot/end_effector/left_gripper/command", , 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_ = (- 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_ = (- 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()<-) {
        ros::spinOnce();
        ros::Duration().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().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() < ) {
        baxterGripper.right_gripper_open();
        ros::spinOnce();
        ROS_INFO("gripper pos = %f", baxterGripper.get_right_gripper_pos());
        ros::Duration().sleep();
    }

    ROS_INFO("closing left gripper");
    baxterGripper.left_gripper_close();
    ros::Duration().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() > ) {
        baxterGripper.right_gripper_close();
        ros::spinOnce();
        ROS_INFO("gripper pos = %f", baxterGripper.get_right_gripper_pos());
        ros::Duration().sleep();
    }

    return ;
}
           

可以使用

rosrun simple_baxter_gripper_interfacbaxter_gripper_lib_test_main
           

檢視效果。

頭部控制

隻需要釋出一個頭部角度指令：

#include <ros/ros.h>
#include <baxter_core_msgs/HeadPanCommand.h>
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<baxter_core_msgs::HeadPanCommand>("/robot/head/command_head_pan", );

    baxter_core_msgs::HeadPanCommand headPanCommand; //message type for head-pan control
    headPanCommand.target = ;
    headPanCommand.enable_pan_request=;
    //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 = ;
    ros::Rate timer(/dt);
    double phase=;
    double theta;

    // oscillate head pan indefinitely
    while (ros::ok()) 
    {
        phase+= M_PI**freq*dt;
        if (phase>*M_PI) phase-= *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。使用以下指令可以檢視消息類型

結果是

int32 POSITION_MODE
int32 VELOCITY_MODE
int32 TORQUE_MODE
int32 RAW_POSITION_MODE
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
           

使用關節軌迹控制器

……