Gripper Customization

From sdk-wiki
Jump to: navigation, search

Gripper Customization

The single largest change in RSDK 1.2.0 is the ability to add joints and linkages to Baxter's [URDF](http://wiki.ros.org/urdf) at runtime.

Mutable Robot State Publisher

To have a modifiable robot_description parameter at runtime, we have forked the ROS robot state publisher into the mutable_robot_state_publisher or MRSP. The MRSP gives you the ability to, at runtime, update the robot_description parameter by passing a URDF Fragment of the relevant piece of URDF that you want to add or modify. The MRSP uses a ROS Topic interface on the /robot/urdf topic, using the baxter_core_msgs/URDFConfiguration message.
In order to preserve the core pieces of the robot's robot_description, a new parameter was added into the mix, the robot_base_description. The robot_base_description contains the core pieces of robot that cannot be removed or modified. The robot_base_description is loaded as the MRSP node is started (when the robot boots), and is thereafter immutable.

   Interface:
       topic: /robot/urdf
       message type: baxter_core_msgs/URDFConfiguration.msg

Here you can see a comparison between the robot_base_description and robot_description parameters onboard the robot:

Where does the robot_base_description end?

For Baxter, the left_hand and right_hand links are the end of robot_base_description, and they are the parent links that you will want to attach to when designing end effector mount points for custom end effectors. If you desire to replace the end effector completely, <side>_gripper_base needs to be the name of your top-level joint in your added URDF fragment.

Similarly, the torso link is the end of the base of the robot. To replace the pedestal with your own, you will need to name your fragment's top-level joint pedestal_fixed.
With that said, you can attach new trees of joints and links to any link that currently exists on the robot.

URDFConfiguration Message

   baxter_core_msgs/URDFConfiguration.msg

Let's see an example of this in action. We will start simple. Let's replace Baxter's pedestal with a block: First, download my beautifully hastily constructed block URDF Fragment yaml:

   time:
     secs: 1449703751
     nsecs: 232993
   link: 'torso'
   joint: 'pedestal_fixed'
   urdf: '<robot name="pedestal">
     <link name="block">
       <inertial>
         <origin xyz="0.0 0.0 0.0" />
         <mass value="40" />
         <inertia  ixx="0.5" ixy="0.0"  ixz="0.0"  iyy="0.5"  iyz="0.0"  izz="0.5" />
       </inertial>
       <visual>
         <origin xyz="0.0 0.0 0.0"/>
         <geometry>
           <box size="0.5 0.5 0.5" />
         </geometry>
         <material name="holidaygreen">
            <color rgba="0.0 1 0 1" />
         </material>
       </visual>
       <collision>
         <origin xyz="0.0 0.0 0.0"/>
         <geometry>
           <box size="0.5 0.5 0.5" />
         </geometry>
       </collision>
     </link>
     <joint name="pedestal_fixed" type="fixed">
         <origin rpy="0 0 0" xyz="0.0 0.0 -0.32"/>
         <parent link="torso"/>
         <child link="block"/>
     </joint>
   </robot>'

Let's go over the fields in this fragment before we go further.

time:
This field is used as the unique identifier to signal to the MRSP that a "new" change has occurred. If the time stamp here is less than the timestamp of the last change, the MRSP will ignore this request entirely. If you are populating this field within your code, you can use ROS Time.
link:
This is the parent link which you want your fragment to attach to. This link must be a valid link in the robot's current URDF. Because we are replacing the pedestal, this needs to be the torso link.
joint:
This is the top level joint which will attach to the parent link specified above. This link must exist in the next URDF fragment field.
urdf:
Finally, A URDF! This URDF fragment must start and end with the <robot> tags to be considered valid URDF. Here, we are just adding a fixed joint and block link. However, you can add arbitrarily complex valid URDF XML in this section. Keep in mind, you are responsible for publishing any revolute or prismatic joints that you add here to Baxter's /robot/joint_states topic. Otherwise RViz will show any downstream linkages as grey and at the center of the robot's torso (indicating the robot state publisher cannot give valid transforms for these links).

URDF Replacing Example

Ok, back to our YAML file. First, we need to update the time inside of the YAML with your current time (you're existing in the future!).
You can get your computer's current time with this bash command:

   $ date +'secs:%s   nsecs:%N'

Now, we need to publish the URDFConfiguration message to the /robot/urdf topic. From a baxter.sh shell:

   $ rostopic pub -l /robot/urdf baxter_core_msgs/URDFConfiguration -f block.yaml

Now, to view our masterpiece in RViz:

   $ rosrun rviz rviz

To revert Baxter back to normal, download this yaml:

   $ wget https://gist.githubusercontent.com/rethink-imcmahon/ef8aca16e3f3a0020826/raw/d1507ce5262ec197c7a31411c7925b266a3f0aa2/pedestal.yaml
   // update the timestamp inside the yaml with the outputs of $ date +'secs:%s   nsecs:%N'
   $ rostopic pub -l /robot/urdf baxter_core_msgs/URDFConfiguration -f pedestal.yaml

Endpoints

In addition to modifying the robot's URDF, the mutable_robot_state_publisher maintains the robot's kinematic chains. These are chains of joints which allow the robot to do advanced motion planning. The end of this chain is denoted as the "endpoint" of the robot. In Baxter's specific case, there are two of these chains, one for each arm:

Chain 1) from /base -> /left_gripper. The transform can be viewed either via TF:

   $ rosrun tf tf_echo /base /left_gripper
At time 1454447728.488
- Translation: [0.547, 0.166, 0.179]
- Rotation: in Quaternion [0.129, 0.991, -0.030, -0.009]
in RPY (radian) [-3.079, -0.009, 2.883]
in RPY (degree) [-176.421, -0.525, 165.197]

or by echoing the endpoint_state topic straight from the robot:

   
   $ rostopic echo -n 1 /robot/limb/left/endpoint_state
   header: 
     seq: 225575615
     stamp: 
       secs: 1454448022
       nsecs: 509830220
     frame_id: 
   pose: 
     position: 
       x: 0.546928461115
       y: 0.16673052608
       z: 0.17892916178
     orientation: 
       x: 0.12768790851
       y: 0.991310445483
       z: -0.0304320539757
       w: -0.00856088709305
   twist: 
     linear: 
       x: -0.016083629389
       y: -0.00160906701015
       z: -0.00548363789644
     angular: 
       x: -0.00792128042619
       y: 0.0393447730243
       z: -0.0123524196563
   wrench: 
     force: 
       x: -4.87127971649
       y: 5.12292146683
       z: 1.91428875923
     torque: 
       x: -2.10366725922
       y: -1.22839438915
       z: -0.276583731174
   ---


and Chain 2) from /base -> /right_gripper. The transform can be viewed either via TF:

   $ rosrun tf tf_echo /base /right_gripper
   

or by echoing the endpoint_state topic straight from the robot:

   
   $ rostopic echo -n 1 /robot/limb/right/endpoint_state
   


End Effectors and Endpoint Location

There is an "end_effector" ROS node running onboard Baxter which detects when different end effectors are plugged & unplugged. This node uses the same URDFConfiguration message above to load the "default" configuration for each end effector. In these defaults the <side>_gripper_base (mutable link) is co-located with <side>_hand (immutable link). The <side>_gripper frames will translate in the Z direction depending on the connected end effector (see below). Although <side>_gripper frame is mutable and moveable, it must exist for Baxter's kinematic chains to work properly. Otherwise, should you remove <side>_gripper entirely, you will find Baxter's Inverse Kinematic ROS Service calls failing rather quickly.


How exactly have the <side>_gripper endpoints changed?

Prior to RSDK 1.2,

Passive Gripper

1.1.1 and prior 1.2.0
 <robot name="left_null_gripper_old">
     <link name="left_gripper_base">
         <inertial>
             <origin xyz="0 0 0" rpy="0 -0 0" />
             <mass value="0" />
             <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
         </inertial>
     </link>
     <link name="left_gripper">
         <inertial>
             <origin xyz="0 0 0" rpy="0 -0 0" />
             <mass value="0.0001" />
             <inertia ixx="1e-08" ixy="0" ixz="0" iyy="1e-08" iyz="0" izz="1e-08" />
         </inertial>
     </link>
     <joint name="left_gripper_base" type="fixed">
         <origin xyz="0 0 0" rpy="0 -0 0" />
         <parent link="left_hand" />
         <child link="left_gripper_base" />
     </joint>
     <joint name="left_endpoint" type="fixed">
         <origin xyz="0 0 0.025" rpy="0 -0 0" />
         <parent link="left_gripper_base" />
         <child link="left_gripper" />
     </joint>
</robot>
 <robot name="left_null_gripper">
     <link name="left_gripper_base">
       <inertial>
         <origin rpy="0 0 0" xyz="0.000000 0.000000 0.000000"/>
         <mass value="0.0001"/>
         <inertia ixx="1e-08" ixy="0" ixz="0" iyy="1e-08" iyz="0" izz="1e-08"/>
       </inertial>
     </link>
     <link name="left_gripper">
       <inertial>
         <origin rpy="0 0 0" xyz="0.000000 0.000000 0.000000"/>
         <mass value="0.0001"/>
         <inertia ixx="1e-08" ixy="0" ixz="0" iyy="1e-08" iyz="0" izz="1e-08"/>
       </inertial>
     </link>
     <joint name="left_gripper_base" type="fixed">
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <parent link="left_hand"/>
       <child link="left_gripper_base"/>
     </joint>
     <joint name="left_endpoint" type="fixed">
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <parent link="left_gripper_base"/>
       <child link="left_gripper"/>
     </joint>
   </robot>

Suction Gripper

1.1.1 and prior 1.2.0
 <robot name="left_vacuum_gripper_old">
     <link name="left_gripper_base">
         <inertial>
             <mass value="0.0475" />
             <origin xyz="-0.0005 0 0.009" rpy="0 -0 0" />
             <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
         </inertial>
     </link>
     <link name="left_gripper">
         <inertial>
             <mass value="0.0001" />
             <origin xyz="0 0 0" rpy="0 -0 0" />
             <inertia ixx="1e-08" ixy="0" ixz="0" iyy="1e-08" iyz="0" izz="1e-08" />
         </inertial>
     </link>
     <joint name="left_gripper_base" type="fixed">
         <origin xyz="0 0 0" rpy="0 -0 0" />
         <parent link="left_hand" />
         <child link="left_gripper_base" />
     </joint>
     <joint name="left_endpoint" type="fixed">
         <origin xyz="0 0 0" rpy="0 -0 0" />
         <parent link="left_gripper_base" />
         <child link="left_gripper" />
     </joint>
</robot>
 <robot name="left_vacuum_gripper">
   <link name="left_gripper_base">
     <collision>
       <origin rpy="0 3.14159265359 3.14159265359" xyz="0.0 0.0 0.04"/>
       <geometry>
         <cylinder length="0.08" radius="0.02"/>
       </geometry>
       <material name="darkred">
         <color rgba=".5 .1 .1 1"/>
       </material>
     </collision>
     <inertial>
       <origin rpy="0 3.14159265359 3.14159265359" xyz="0.0 0.0 0.0"/>
       <mass value="0.3"/>
       <inertia ixx="2e-08" ixy="0" ixz="0" iyy="3e-08" iyz="0" izz="2e-08"/>
     </inertial>
   </link>
   <link name="left_gripper">
     <inertial>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <mass value="0.0001"/>
       <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0.0"/>
     </inertial>
   </link>
   <joint name="left_gripper_base" type="fixed">
     <origin rpy="0 0 0" xyz="0 0 0.0"/>
     <parent link="left_hand"/>
     <child link="left_gripper_base"/>
   </joint>
   <joint name="left_endpoint" type="fixed">
     <origin rpy="0 0 0" xyz="0 0 0.08"/> 
     <parent link="left_gripper_base"/>
     <child link="left_gripper"/>
   </joint>
 </robot>

Electric Gripper

1.1.1 and prior 1.2.0
 <robot name="left_electric_gripper_old">
     <link name="left_gripper_base">
         <inertial>
             <origin xyz="0.0014 0 0.024" rpy="0 -0 0" />
             <mass value="0.28" />
             <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
         </inertial>
     </link>
     <link name="left_gripper">
         <inertial>
             <origin xyz="0 0 0" rpy="0 -0 0" />
             <mass value="0.0001" />
             <inertia ixx="1e-08" ixy="0" ixz="0" iyy="1e-08" iyz="0" izz="1e-08" />
         </inertial>
     </link>
     <joint name="left_gripper_base" type="fixed">
         <origin xyz="0 0 0" rpy="0 -0 0" />
         <parent link="left_hand" />
         <child link="left_gripper_base" />
     </joint>
     <joint name="left_endpoint" type="fixed">
         <origin xyz="0 0 0.045" rpy="0 -0 0" />
         <parent link="left_gripper_base" />
         <child link="left_gripper" />
     </joint>
</robot>
 <robot name="left_electric_gripper">
   <link name="left_gripper_base">
     <collision>
       <origin rpy="-1.57079632679 3.14159265359 0" xyz="0.0 0.0 0"/>
       <geometry>
         <cylinder length="0.1" radius="0.029"/>
       </geometry>
       <material name="darkred">
         <color rgba=".5 .1 .1 1"/>
       </material>
     </collision>
     <inertial>
       <origin rpy="-1.57079632679 3.14159265359 0" xyz="0.0 0.0 0.0"/>
       <mass value="0.3"/>
       <inertia ixx="2e-08" ixy="0" ixz="0" iyy="3e-08" iyz="0" izz="2e-08"/>
     </inertial>
   </link>
   <link name="left_gripper">
     <inertial>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <mass value="0.0001"/>
       <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0.0"/>
     </inertial>
   </link>
   <joint name="left_gripper_base" type="fixed">
     <origin rpy="0 0 0" xyz="0 0 0.025"/>
     <parent link="left_hand"/>
     <child link="left_gripper_base"/>
   </joint>
   <joint name="left_endpoint" type="fixed">
     <origin rpy="0 0 0" xyz="0 0 0.1327"/>
     <parent link="left_gripper_base"/>
     <child link="left_gripper"/>
   </joint>
 
 </robot>

Modifying the Electric Gripper and Endpoint

Mass Modifications

Gripper and Object Mass

The user is allowed to set an expected mass for grippers as well as for grasped objects. This involves copying in a portion of a urdf pertaining to those properties. The previous section shows the user how to modify the end effector URDF. The end effector frames can change according to your specific usage, so we create two "fixed" frames that will allow the user to add additional mass to the gripper (that was not specified in their URDF) and account for object mass when it is grasped. These two frames are in parallel to the <side>_gripper_base and <side>_gripper end effector frames. They are:
<side>_gripper_mass: for any additional mass not accounted for in the URDF
<side>_gripper_object: for object mass. This is only accounted for when the robot is "grasping" an object.

Gripper ID's

Since this is an "end effector" configuration, the user needs to supply the robot with the expected "id" of the gripper:
Legacy Gripper ID (only incredibly old Rethink-electric grippers): 65664
Rethink Electric Gripper Id:65538
Rethink Suction Cup Gripper Id:65537
No gripper, or custom gripper Id:131073

Gripper Mass

The command for setting the mass for a given gripper is:

    $ rostopic pub -1 /robot/end_effector/<limb>_gripper/command baxter_core_msgs/EndEffectorCommand '{ id : <gripper_id>,  command : "configure", args : "{ \"urdf\":{ \"name\": \"<limb>_gripper_mass\",  \"link\": [ { \"name\": \"<limb>_hand\" }, { \"name\": \"<limb>_gripper_mass\", \"inertial\": { \"mass\": { \"value\": <mass: in kg> }, \"origin\": { \"xyz\": [0.0014, 0.0, 0.074] } } } ] }}"}'

The maximum mass you should set here is 2.27 (~5 lbs) For example, to set the mass of the right gripper to 2.27 kilograms:

    $ rostopic pub -1 /robot/end_effector/right_gripper/command baxter_core_msgs/EndEffectorCommand '{ id : 131073,  command : "configure", args : "{ \"urdf\":{ \"name\": \"right_gripper_mass\",  \"link\": [ { \"name\": \"right_hand\" }, { \"name\": \"right_gripper_mass\", \"inertial\": { \"mass\": { \"value\": 2.27 }, \"origin\": { \"xyz\": [0.0014, 0.0, 0.074] } } } ] }}"}'

and to set it to zero:

    $ rostopic pub -1 /robot/end_effector/right_gripper/command baxter_core_msgs/EndEffectorCommand '{ id : 131073,  command : "configure", args : "{ \"urdf\":{ \"name\": \"right_gripper_mass\",  \"link\": [ { \"name\": \"right_hand\" }, { \"name\": \"right_gripper_mass\", \"inertial\": { \"mass\": { \"value\": 0 }, \"origin\": { \"xyz\": [0.0014, 0.0, 0.074] } } } ] }}"}'

Gripped Object mass

This will set the expected weight of a grasped object and will only be noticeable when the updated arm is currently grasping an object. Once again, the maximum mass that should be set is 2.27 kilograms (~5 lbs).

To set the mass of a gripped object:

    $ rostopic pub -1 /robot/end_effector/<limb>_gripper/command baxter_core_msgs/EndEffectorCommand '{ id : <gripper_id>,  command : "configure", args : "{ \"object\":{ \"name\": \"<limb>_object_mass\", \"inertial\": { \"mass\": { \"value\": <mass: in kg> }, \"origin\": { \"xyz\": [0.0, 0.0, 0.0] }, \"inertia\": { \"ixx\": 0.0, \"ixy\": 0.0, \"ixz\": 0.0, \"iyy\": 0.0, \"iyz\": 0.0, \"izz\": 0.0 } } } }}"}'

For example, to set the mass of a picked object to 2.27 kilograms:

    $ rostopic pub -1 /robot/end_effector/right_gripper/command baxter_core_msgs/EndEffectorCommand '{ id : 131073,  command : "configure", args : "{ \"object\":{ \"name\": \"right_object_mass\", \"inertial\": { \"mass\": { \"value\": 2.27 }, \"origin\": { \"xyz\": [0.0, 0.0, 0.0] }, \"inertia\": { \"ixx\": 0.0, \"ixy\": 0.0, \"ixz\": 0.0, \"iyy\": 0.0, \"iyz\": 0.0, \"izz\": 0.0 } } } }}"}'

and to set it back to 0:

    $ rostopic pub -1 /robot/end_effector/right_gripper/command baxter_core_msgs/EndEffectorCommand '{ id : 131073,  command : "configure", args : "{ \"object\":{ \"name\": \"right_object_mass\", \"inertial\": { \"mass\": { \"value\": 0.00 }, \"origin\": { \"xyz\": [0.0, 0.0, 0.0] }, \"inertia\": { \"ixx\": 0.0, \"ixy\": 0.0, \"ixz\": 0.0, \"iyy\": 0.0, \"iyz\": 0.0, \"izz\": 0.0 } } } }}"}'