Using forward kinematics, we can determine the position of the gripper at any time. The inverse kinematic problem is to place the gripper at a desired location and orientation. This requires the calculation of the joint angles then sending Baxter the seven joint angles and commanding the arm to move.

Rethink Robotics provides an Inverse Kinematic (IK) example that sets a specific endpoint position and orientation and solves for the required joint angles. The example and the Python script are described in these websites:

To run the example to find the joint angles of the left limb (arm) for the fixed position and orientation in the Python script, type:

$ rosrun baxter_examples ik_service_client.py -l left

The endpoint position and orientation of the right arm can be found using the same command but with the option right. See the code to find the specific pose assigned for these examples.

To demonstrate the IK service example using the real Baxter's left arm, we will perform the following steps:

First, execute the script to move Baxter's arms to the untucked position:

$ cd baxter_ws
$ ./baxter.sh
$ rosrun baxter_tools tuck_arms.py -u

Then, display left arm endpoint pose positions and orientation and record the values:

$ rostopic echo /robot/limb/left/endpoint_state

Our output is as follows:

pose:
  position:
    x: 0.584753440252
    y: 0.195382237943
    z: 0.101405569316
  orientation:
    x: 0.131891460542
    y: 0.990928450502
    z: 0.0117922898402
    w: 0.0229432020794

The gripper of Baxter should be out about 0.6 meters in x, 0.19 meters to the left of the vertical centerline in y, and about 0.1 meters up from the base in z. Next, by hand move Baxter's arms arbitrarily so that you can test the IK server routine.

To use the IK service with the endpoints of the untucked position and get angles for endpoints of the left limb, put the x, y, z values and the Quaternion into the script by editing the script ik_service_client.py with the values shown here or use the values you obtained:

poses = {
  'left': PoseStamped(
    header=hdr,
    pose=Pose(
      position=Point(
        x: 0.584753440252,
        y: 0.195382237943,
        z: 0.101405569316,

      ),
      orientation=Quaternion(
        x: 0.131891460542,
        y: 0.990928450502,
        z: 0.0117922898402,
        w: 0.0229432020794,

      ),
    ),
  ),
}

After editing ik_service_client.py, you should rename the file. Our new file was named ik_home_arms_ch6RealBaxter.py. To made it executable, type:

$ chmod +x ik_home_arms_ch6RealBaxter.py

To run the example to find the joint angles of the left arm that would move Baxter's arm to the untucked position, type:

$ python ik_home_arms_ch6RealBaxter.py -l left

The output should be similar to the following:

SUCCESS - Valid Joint Solution Found from Seed Type: Current Joint Angles

IK Joint Solution:

{'left_w0': 0.59208994893573, 'left_w1': 0.991464695921505, 'left_w2': -0.44186139221262843, 'left_e0': -1.09990187443917, 'left_e1': 1.9511256097791254, 'left_s0': -0.12534096010690407, 'left_s1': -1.063859702348023}

------------------

Use the resulting angles to move Baxter arms using the edited Python script home_arms.py. Change the values of the left arm joints and save the file with a new name. We used the filename MoveLeftArmToHome.py. Make the file executable using the command:

$ chmod +x MoveLeftArmToHome.py

Execute the new script and watch Baxter's left arm return to the untucked position, if all goes well:

$ python MoveLeftArmToHome.py -l left

Finally, display left arm endpoint pose positions and orientation and record the values to compare to the original values in position and orientation:

$ rostopic echo /robot/limb/left/endpoint_state

After Baxter's arm moved, our values were fairly close to the originals:

header:
  seq: 405035
  stamp:
    secs: 1459992006
    nsecs: 87484528
  frame_id: ''
pose:
  position:
    x: 0.581561073269
    y: 0.192636180771
    z: 0.10043051263
  orientation:
    x: 0.134391972572
    y: 0.990591354182
    z: 0.0121901390617
    w: 0.0227808524492