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 in the script and solves the required joint angles. The example and the Python script are described on these websites:
To run the IK example to find the joint angles of the left limb (arm) for the fixed position and orientation in the Python script, type this command:
$ rosrun baxter_examples ik_service_client.py -l left
The pose of the left end-effector taken from the ik_service_client.py script is as follows:
'left': PoseStamped(
header=hdr,
pose=Pose(
position=Point(
x=0.657579481614,
y=0.851981417433,
z=0.0388352386502,
),
orientation=Quaternion(
x=-0.366894936773,
y=0.885980397775,
z=0.108155782462,
w=0.262162481772,
),
),
),Executing this yields the joint angles to move Baxter's left arm to the pose defined in the script. These angles would be used to move Baxter's arm to this pose from an arbitrary position, as shown by the example in the next section.
The script also sets an initial pose for the right arm. The endpoint position and orientation of the right arm can be found using the same command but with the right option. See the code to find the specific pose assigned for this.
To demonstrate the IK service example using the real Baxter's left arm, we will perform the following steps:
- Power up Baxter and untuck both arms. This is the home position for the arms.
- Record the endpoint state in the position and orientation of the left arm.
- Move Baxter's left arm to an arbitrary position.
- Modify the
ik_service_client.pyscript in thebaxter_examplespackage by entering the position and orientation of the untucked left arm and save the file under a different name incatkin_workspace. - Execute the script to get the joint angles of the left arm.
- Type the angles into a modified
home_arms.pyscript and execute it. - Record the new endpoint positions and orientations and compare them to the original values recorded in step 2.
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 the left-arm endpoint pose position and orientation and record the values to two decimal places with the following command:
$ rostopic echo /robot/limb/left/endpoint_state/pose -n1 -w4
Our output for the pose of the left arm is as follows:
position: x: 0.57 y: 0.18 z: 0.10 orientation: x: 0.13 y: 0.99 z: 0.00 w: 0.02
The endpoint of Baxter should be out about 0.57 meters in x, 0.18 meters to the left of Baxter's vertical centerline in y, and about 0.10 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 modify the script ik_service_client.py, first use the following command:
$ roscd baxter_examples/scripts
Find the Python script to modify in this directory. To use the IK service with the endpoints of the untucked position and get angles for the left limb, put the x, y, z values and the orientation into the script ik_service_client.py file by editing the script with the values shown here, or use the values you obtained:
poses = {
'left': PoseStamped(
header=hdr,
pose=Pose(
position=Point(
x=0.57,
y=0.18,
z=0.10,
),
orientation=Quaternion(
x=0.13,
y=0.99,
z=0.00,
w=0.02,
),
),
),After editing ik_service_client.py, you should rename the file. Our new file was named ik_home_arms_ch6RealBaxter.py. To make it executable, type the following command:
$ chmod +x ik_home_arms_ch6RealBaxter.py
To run this script to find the joint angles of the left arm that would move Baxter's arm to the specific endpoint position, type this:
$ 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': -1.8582664616409326, 'left_w1': -1.460468102595922, 'left_w2': 2.2756459061545797, 'left_e0': -1.6081637990992477, 'left_e1': 1.9645288022495901, 'left_s0': 0.044896665837355125, 'left_s1': -0.3326492980686455} ------------------
Use the resulting angles to move Baxter's 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 MoveLeftArmToHome.py filename. Make the file executable using this command:
$ chmod +x MoveLeftArmToHome.py
Execute the new script and watch Baxter's left endpoint return to the desired position, if all goes well:
$ python MoveLeftArmToHome.py
Finally, display the left arm endpoint pose position and orientation and record the values to compare them to the original values for position and orientation:
$ rostopic echo /robot/limb/left/endpoint_state/pose -n1 -w4
After Baxter's arm moved, our values were fairly close to the originals:
position: x: 0.57 y: 0.18 z: 0.09 orientation: x: 0.12 y: 0.99 z: 0.00 w: 0.02