Difference between revisions of "Custom IKFast for your Baxter"

From sdk-wiki
Jump to: navigation, search
(Modify Simplified URDFs with your robot's joints)
(Modify Simplified URDFs with your robot's joints)
 
(43 intermediate revisions by one user not shown)
Line 1: Line 1:
 
== Overview ==
 
== Overview ==
Every Baxter's URDF is slightly different. This tutorial will show you how to generate IKFast for use with [http://moveit.ros.org/wiki/Kinematics/IKFast MoveIt] specific to your Baxter. This is a fairly involved process, requiring many steps. For additional background information on the algorithm behind IKFast, please see [http://www.programmingvision.com/rosen_diankov_thesis.pdf Rosen Diankov's Doctoral thesis].
+
Every Baxter's URDF is slightly different. This tutorial will show you how to generate IKFast for use with [http://moveit.ros.org/wiki/Kinematics/IKFast MoveIt] specific to your Baxter. This is a fairly involved process, requiring many steps. For additional background information on the algorithm behind IKFast, please see [http://www.programmingvision.com/rosen_diankov_thesis.pdf Rosen Diankov's Doctoral thesis]. Another really good tutorial for generating IKFast for generic (non-Baxter) robots can be found on MoveIt's website: http://moveit.ros.org/wiki/Kinematics/IKFast
  
 
=== What is IKFast? ===
 
=== What is IKFast? ===
Line 15: Line 15:
 
If you encounter any issues in building this repo, please let the developers know in the [https://github.com/rdiankov/openrave/issues issues tab of their repo].
 
If you encounter any issues in building this repo, please let the developers know in the [https://github.com/rdiankov/openrave/issues issues tab of their repo].
  
=== Clone the custom_baxter_ikfast Repository ===
+
=== Clone the baxter_custom_ikfast Repository ===
Clone this <code>custom_baxter_ikfast</code> repo anywhere, but make sure you are inside a [[RSDK_Shell | initialized baxter.sh shell]] for commands later on.
+
To create IKFast for Baxter, we need to break him down into her individual arms. For this tutorial, we split Baxter into left and right arm URDFs and then removed any link/joint not in the kinematic chain between the base and end-effector. The result of this is a simple little repository on GitHub with these split URDFs in their generic form. Clone the <code>baxter_custom_ikfast</code> repo anywhere, but make sure you are inside a [[RSDK_Shell | initialized baxter.sh shell]] for commands later on.
 
<source lang="bash">
 
<source lang="bash">
$ git clone https://github.com/RethinkRobotics/custom_baxter_ikfast.git
+
$ git clone https://github.com/RethinkRobotics/baxter_custom_ikfast.git
 
</source>
 
</source>
 
Navigate inside this cloned folder for the rest of this tutorial:
 
Navigate inside this cloned folder for the rest of this tutorial:
Line 36: Line 36:
 
<br/>
 
<br/>
  
Open <code>baxter_urdf.xml</code> in your favorite text editor, and search for <code>torso_arm_mount</code> to find the only joints that differ from Baxter to Baxter. This is an example of what you should expect if the robot description contains Baxter's custom joints:
+
Open <code>baxter_urdf.xml</code> in your favorite text editor, and search for <code>torso_arm_mount</code> to which are the '''only joints''' that differ from Baxter to Baxter. This is an example of what you should expect if the robot description contains Baxter's custom joints:
 
<source lang="xml">
 
<source lang="xml">
 
<joint name="left_torso_arm_mount" type="fixed">
 
<joint name="left_torso_arm_mount" type="fixed">
Line 54: Line 54:
 
</joint>
 
</joint>
 
</source>
 
</source>
Save these snippits of URDF for the next step.
+
*'''Note:''' Save '''just the origin''' transform snippits of URDF for the next step. Be sure to avoid copying the '''parent link''' value '''torso''' as IKFast optimizes it out due to it being an identity transform and replaces it with '''base'''.
  
 
<br/>
 
<br/>
*'''Note:''' If you see the following joint values, then you are using the generic URDF provided in <code>baxter_description</code> of the SDK. This can happen if the generic URDF is loaded to the param server via a <code>roslaunch</code> script (such as the one with MoveIt). Reboot your robot to clear the <code>robot_description</code> and try again:
+
*'''Note:''' If you see the following joint values, then you are using the [https://github.com/RethinkRobotics/baxter_common/blob/master/baxter_description/urdf/baxter.urdf generic URDF] provided in <code>baxter_description</code> of the SDK. This can happen if the generic URDF is loaded to the param server via a <code>roslaunch</code> script (such as the one with MoveIt). Reboot your robot to clear the <code>robot_description</code> and try again:
  
 
<source lang="xml">
 
<source lang="xml">
Line 76: Line 76:
  
 
=== Modify Simplified URDFs with your robot's joints ===
 
=== Modify Simplified URDFs with your robot's joints ===
The <code>baxter_arm.accurate.<side>.urdf</code> was extracted from Baxter's original urdf to separate out the left and right arms. Inside the <code>custom_baxter_ikfast</code> folder, edit both <code>baxter_arm.accurate.<side>.urdf</code>'s with your favorite text editor:<br/>
+
The <code>baxter_arm.accurate.<side>.urdf</code> was extracted from Baxter's original urdf to separate out the left and right arms. Inside the <code>baxter_custom_ikfast</code> folder, edit both <code>baxter_arm.accurate.<side>.urdf</code>'s with your favorite text editor:<br/>
Replace  
+
Replace the '''origin''' tag with your copied transform values inside the '''<side>_torso_arm_mount''' joints:
 +
<source lang="bash">
 
   <joint name="left_torso_arm_mount" type="fixed">
 
   <joint name="left_torso_arm_mount" type="fixed">
 
     <origin rpy="0 0 0.7854" xyz="0.024645 0.219645 0.118588"/>
 
     <origin rpy="0 0 0.7854" xyz="0.024645 0.219645 0.118588"/>
Line 83: Line 84:
 
     <child link="left_arm_mount"/>
 
     <child link="left_arm_mount"/>
 
   </joint>
 
   </joint>
with your custom joint copied from above. Save, quit, and repeat for the other arm's urdf.
+
</source>
 +
Save, quit, and repeat for the other arm's urdf.
  
 
=== Create DAEs for Each Arm ===
 
=== Create DAEs for Each Arm ===
Line 93: Line 95:
 
* '''Note:''' Please use the a simplified version of the urdf to only contain each manipulators links/joints modified with your robot's arm mount joint extracted above <br/>
 
* '''Note:''' Please use the a simplified version of the urdf to only contain each manipulators links/joints modified with your robot's arm mount joint extracted above <br/>
 
<source lang="bash">
 
<source lang="bash">
rosrun collada_urdf urdf_to_collada baxter_arm.accurate.left.urdf baxter_arm.left.dae
+
rosrun collada_urdf urdf_to_collada baxter_arm.accurate.right.urdf baxter_arm.right.dae
 
</source>
 
</source>
  
* The resulting DAE also needs some hand tuning - specifically the rotational portions of the description. Round values logically (ie. 119.9999999999999 to 120.0)<br/>
+
* '''Note:''' The resulting DAE also needs some tuning - specifically the rotational portions of the description. <br/>Specifically, we will need to round values logically (ie. 119.9999999999999 to 120.0) This will be addressed in the next step.
 
<br/>
 
<br/>
 
Please take note of the link indices using the following command:
 
Please take note of the link indices using the following command:
 
<pre>
 
<pre>
$ openrave0.9-robot.py baxter_arm.left.dae --info links
+
$ openrave0.9-robot.py baxter_arm.right.dae --info links
 
name                index parents             
 
name                index parents             
 
-----------------------------------------------
 
-----------------------------------------------
Line 119: Line 121:
 
Likewise with the joints:
 
Likewise with the joints:
 
<pre>
 
<pre>
$ openrave0.9-robot.py baxter_arm.left.dae --info joints
+
$ openrave0.9-robot.py baxter_arm.right.dae --info joints
 
name                  joint_index dof_index parent_link          child_link          mimic
 
name                  joint_index dof_index parent_link          child_link          mimic
 
-------------------------------------------------------------------------------------------
 
-------------------------------------------------------------------------------------------
Line 135: Line 137:
 
name                  joint_index dof_index parent_link          child_link          mimic
 
name                  joint_index dof_index parent_link          child_link          mimic
 
</pre>
 
</pre>
 +
 +
=== Rounding DAEs for Each Arm ===
 +
The openrave solver takes a lot longer to calculate solutions with many decimal points inside the numbers of the DAE files. To remedy this, download moveit_ikfast:<br/>
 +
<source lang="bash">
 +
$ sudo apt-get install ros-indigo-moveit-ikfast
 +
</source>
 +
Let's round the numbers to five decimal points:
 +
<source lang="bash">
 +
$ rosrun moveit_ikfast round_collada_numbers.py baxter_arm.right.dae baxter_arm.right.rounded.dae 5
 +
</source>
  
 
=== Choose IK Type ===
 
=== Choose IK Type ===
Line 144: Line 156:
 
Building the solver using the collada mesh, chosen iktype, base, and end effector links created and found in the previous steps.
 
Building the solver using the collada mesh, chosen iktype, base, and end effector links created and found in the previous steps.
 
<source lang="bash">
 
<source lang="bash">
$ python /usr/lib/python2.7/dist-packages/openravepy/_openravepy_0_9/ikfast.py --robot=baxter_arm.left.dae --iktype=transform6d --baselink=1 --eelink=10 --freeindex=5 --savefile=baxter_ikfast.w1.left.cpp
+
$ python /path_to_openrave/openrave/python/ikfast.py --robot=baxter_arm.right.rounded.dae --iktype=transform6d --baselink=1 --eelink=10 --freeindex=5 --savefile=baxter_right_arm_ikfast_solver.cpp
 
</source>
 
</source>
Completion time should ~24 minutes.
+
Completion time should be ~24 minutes.
 
<br/>
 
<br/>
Compile executable:
+
This library is what exists inside the official [https://github.com/ros-planning/moveit_robots/blob/master/baxter/baxter_ikfast_right_arm_plugin/src/baxter_right_arm_ikfast_solver.cpp moveit_robots/baxter_ikfast_<side>_arm_plugin package]. <br/>
 +
Simply drop this cpp file in there, rebuild your catkin workspace, and enable IKFast as explained in [[MoveIt_Tutorial#IKFast]].
 +
<br/><br/>
 +
Congrats! You've created a custom IKFast for your Baxter!<br/>
 +
 
 +
=== Testing with the IKFastDemo ===
 +
If you'd like to explore IKFast a bit further, you can compile the source code into an executable
 
<source lang="bash">
 
<source lang="bash">
g++ -lstdc++ -o baxter_ik baxter_ikfast.w1.left.cpp -llapack
+
$ g++ -lstdc++ -o baxter_right_ik baxter_right_arm_ikfast_solver.cpp -llapack
 
</source>
 
</source>
 
+
<br/>
=== Customizing the IKFastDemo ===
+
And finally, to go even further by testing out the ikfastdemo, first download it from the kaist-ros-pkg with wget:
Open up the <code>ikfastdemo.cpp</code> source file in your favorite text editor. <br/>
+
<source lang="bash">
 +
$ wget http://kaist-ros-pkg.googlecode.com/svn/trunk/arm_kinematics_tools/src/ikfastdemo/ikfastdemo.cpp
 +
</source>
 +
Then edit the <code>ikfastdemo.cpp</code> source file in your favorite text editor. <br/>
 
Modify the following lines at the top of the file:
 
Modify the following lines at the top of the file:
 
<source lang="c">
 
<source lang="c">
 
#define IK_VERSION 61
 
#define IK_VERSION 61
#include "baxter_ikfast.w1.left.cpp"
+
#include "baxter_right_arm_ikfast_solver.cpp"
 
</source>
 
</source>
 
Then compile your IKFastDemo:
 
Then compile your IKFastDemo:
 
<source lang="bash">
 
<source lang="bash">
g++ ikfastdemo.cpp -lstdc++ -llapack -o compute -lrt
+
$ g++ ikfastdemo.cpp -lstdc++ -llapack -o compute -lrt
 
</source>
 
</source>

Latest revision as of 16:50, 8 September 2015

Overview

Every Baxter's URDF is slightly different. This tutorial will show you how to generate IKFast for use with MoveIt specific to your Baxter. This is a fairly involved process, requiring many steps. For additional background information on the algorithm behind IKFast, please see Rosen Diankov's Doctoral thesis. Another really good tutorial for generating IKFast for generic (non-Baxter) robots can be found on MoveIt's website: http://moveit.ros.org/wiki/Kinematics/IKFast

What is IKFast?

#ikfast IKFast provides analytical closed-form solutions for manipulator inverse kinematics. Part of the OpenRAVE robot architecture/software framework, IKFast has been used to provide closed-form solutions for Baxter's inverse kinematics. Advantages include order of magnitude faster IK solutions and the ability explore the manipulator's null space!

Using Baxter's MoveIt! IKFast plugins

Please see MoveIt_Tutorial#IKFast for the exact commands required to enable IKFast in your baxter_moveit_config package.

Generating Custom IKFast for Baxter

Install OpenRAVE

As of this writing, the instructions for adding the openrave PPA and installing openrave debians does not work on Ubuntu Trusty since the latest release was created in 2012 for Precise. That leaves building from source as the only option. Please clone the OpenRAVE repo and follow this tutorial for building from source:

 git clone --branch latest_stable https://github.com/rdiankov/openrave.git

If you encounter any issues in building this repo, please let the developers know in the issues tab of their repo.

Clone the baxter_custom_ikfast Repository

To create IKFast for Baxter, we need to break him down into her individual arms. For this tutorial, we split Baxter into left and right arm URDFs and then removed any link/joint not in the kinematic chain between the base and end-effector. The result of this is a simple little repository on GitHub with these split URDFs in their generic form. Clone the baxter_custom_ikfast repo anywhere, but make sure you are inside a initialized baxter.sh shell for commands later on.

$ git clone https://github.com/RethinkRobotics/baxter_custom_ikfast.git

Navigate inside this cloned folder for the rest of this tutorial:

$ cd custom_baxter_ikfast

Extracting the Mount Joint Transform

The left and right arm mount joint's in Baxter's torso are hand-welded at manufacture time. These joints are carefully measured and the transforms are stored in your Baxter's torso joint board. Fortunately, we can see the transforms in your robot's URDF located in the /robot_description parameter on the param server. After a fresh boot (to ensure the custom URDF has not been overwritten), from a properly initialized Baxter environment, export the URDF from the /robot_description parameter on the ROS parameter server where it is stored, to a file of your choice (ex: baxter_urdf.xml):

$ rosparam get -p /robot_description | tail -n +2 > baxter_urdf.xml

The -p outputs the parameter using pretty print. The output urdf is piped through the tail command first to remove a dummy first line - an artifact of the pretty print.


Open baxter_urdf.xml in your favorite text editor, and search for torso_arm_mount to which are the only joints that differ from Baxter to Baxter. This is an example of what you should expect if the robot description contains Baxter's custom joints:

<joint name="left_torso_arm_mount" type="fixed">
    <origin xyz="0.0229088 0.220982 0.106714" rpy="-0.0179406 0.0117677 0.786034" />
    <axis xyz="0 0 0" />
    <parent link="torso" />
    <child link="left_arm_mount" />
</joint>

and

<joint name="right_torso_arm_mount" type="fixed">
    <origin xyz="0.0245182 -0.218624 0.10866" rpy="-0.00321838 -0.00564277 -0.78571" />
    <axis xyz="0 0 0" />
    <parent link="torso" />
    <child link="right_arm_mount" />
</joint>
  • Note: Save just the origin transform snippits of URDF for the next step. Be sure to avoid copying the parent link value torso as IKFast optimizes it out due to it being an identity transform and replaces it with base.


  • Note: If you see the following joint values, then you are using the generic URDF provided in baxter_description of the SDK. This can happen if the generic URDF is loaded to the param server via a roslaunch script (such as the one with MoveIt). Reboot your robot to clear the robot_description and try again:
    <joint name="left_torso_arm_mount" type="fixed">
        <origin rpy="0 0 0.7854" xyz="0.024645 0.219645 0.118588"/>
        <parent link="torso"/>
        <child link="left_arm_mount"/>
    </joint>
    <joint name="right_torso_arm_mount" type="fixed">
        <origin rpy="0 0 -0.7854" xyz="0.024645 -0.219645 0.118588"/>
        <parent link="torso"/>
        <child link="right_arm_mount"/>
    </joint>

Modify Simplified URDFs with your robot's joints

The baxter_arm.accurate.<side>.urdf was extracted from Baxter's original urdf to separate out the left and right arms. Inside the baxter_custom_ikfast folder, edit both baxter_arm.accurate.<side>.urdf's with your favorite text editor:
Replace the origin tag with your copied transform values inside the <side>_torso_arm_mount joints:

  <joint name="left_torso_arm_mount" type="fixed">
    <origin rpy="0 0 0.7854" xyz="0.024645 0.219645 0.118588"/>
    <parent link="base"/>
    <child link="left_arm_mount"/>
  </joint>

Save, quit, and repeat for the other arm's urdf.

Create DAEs for Each Arm

Create our DAEs for each arm:

sudo apt-get install ros-indigo-robot-model
  • Note: Please use the a simplified version of the urdf to only contain each manipulators links/joints modified with your robot's arm mount joint extracted above
rosrun collada_urdf urdf_to_collada baxter_arm.accurate.right.urdf baxter_arm.right.dae
  • Note: The resulting DAE also needs some tuning - specifically the rotational portions of the description.
    Specifically, we will need to round values logically (ie. 119.9999999999999 to 120.0) This will be addressed in the next step.


Please take note of the link indices using the following command:

$ openrave0.9-robot.py baxter_arm.right.dae --info links
name                 index parents             
-----------------------------------------------
base                 0                         
right_arm_mount      1     base                
right_upper_shoulder 2     right_arm_mount     
right_lower_shoulder 3     right_upper_shoulder
right_upper_elbow    4     right_lower_shoulder
right_lower_elbow    5     right_upper_elbow   
right_upper_forearm  6     right_lower_elbow   
right_lower_forearm  7     right_upper_forearm 
right_wrist          8     right_lower_forearm 
right_hand           9     right_wrist         
right_gripper        10    right_hand          
-----------------------------------------------
name                 index parents   

Likewise with the joints:

$ openrave0.9-robot.py baxter_arm.right.dae --info joints
name                  joint_index dof_index parent_link          child_link           mimic
-------------------------------------------------------------------------------------------
right_s0              0           0         right_arm_mount      right_upper_shoulder      
right_s1              1           1         right_upper_shoulder right_lower_shoulder      
right_e0              2           2         right_lower_shoulder right_upper_elbow         
right_e1              3           3         right_upper_elbow    right_lower_elbow         
right_w0              4           4         right_lower_elbow    right_upper_forearm       
right_w1              5           5         right_upper_forearm  right_lower_forearm       
right_w2              6           6         right_lower_forearm  right_wrist               
right_torso_arm_mount -1          -1        base                 right_arm_mount           
right_hand            -1          -1        right_wrist          right_hand                
right_endpoint        -1          -1        right_hand           right_gripper             
-------------------------------------------------------------------------------------------
name                  joint_index dof_index parent_link          child_link           mimic

Rounding DAEs for Each Arm

The openrave solver takes a lot longer to calculate solutions with many decimal points inside the numbers of the DAE files. To remedy this, download moveit_ikfast:

$ sudo apt-get install ros-indigo-moveit-ikfast

Let's round the numbers to five decimal points:

$ rosrun moveit_ikfast round_collada_numbers.py baxter_arm.right.dae baxter_arm.right.rounded.dae 5

Choose IK Type

Choose your IK Type (ex: Transform6D, Rotation3d, Translation3D, etc.)
http://openrave.org/docs/latest_stable/openravepy/ikfast/#ik-types
For most applications it makes sense to use transform6D - Baxter's end effector reaches desired 6D transformation

Generate the IKFast Solver

Building the solver using the collada mesh, chosen iktype, base, and end effector links created and found in the previous steps.

$ python /path_to_openrave/openrave/python/ikfast.py --robot=baxter_arm.right.rounded.dae --iktype=transform6d --baselink=1 --eelink=10 --freeindex=5 --savefile=baxter_right_arm_ikfast_solver.cpp

Completion time should be ~24 minutes.
This library is what exists inside the official moveit_robots/baxter_ikfast_<side>_arm_plugin package.
Simply drop this cpp file in there, rebuild your catkin workspace, and enable IKFast as explained in MoveIt_Tutorial#IKFast.

Congrats! You've created a custom IKFast for your Baxter!

Testing with the IKFastDemo

If you'd like to explore IKFast a bit further, you can compile the source code into an executable

$ g++ -lstdc++ -o  baxter_right_ik baxter_right_arm_ikfast_solver.cpp -llapack


And finally, to go even further by testing out the ikfastdemo, first download it from the kaist-ros-pkg with wget:

$ wget http://kaist-ros-pkg.googlecode.com/svn/trunk/arm_kinematics_tools/src/ikfastdemo/ikfastdemo.cpp

Then edit the ikfastdemo.cpp source file in your favorite text editor.
Modify the following lines at the top of the file:

#define IK_VERSION 61
#include "baxter_right_arm_ikfast_solver.cpp"

Then compile your IKFastDemo:

$ g++ ikfastdemo.cpp -lstdc++ -llapack -o compute -lrt