Tutorials/CloudSim/notebook tabletop manip

Under Construction

In this tutorial, we demonstrate how to make the Atlas robot manipulate objects on a tabletop.
 * 1) Tabletop manipulation

You should have previously completed the introductory gzweb and iPython Notebook tutorials.

Note: later parts of this tutorial are only known to work in Chrome.

Similar to what we did in the introductory tutorial, we'll start by creating a task that will bring up Atlas in a world with a table, a drill, and a bin (this world was used as the second task in the VRC Qualification). Click the `Create task...` button and fill in the following values for the first three fields:
 * 1) Create the task


 * Task title : `Tabletop manipulation`
 * ROS package : `drcsim_gazebo`
 * Launch file : `qual_task_2.launch`

Leave the other fields with their defaults and click the `Create` button. Then start the task.


 * 1) The world

Click on the `3D view` link to go to the WebGL interface for your remote simulation. You should see Atlas with its arms spread wide, standing before a table with a drill on it. To Atlas's left of the table is a bin.



Our goal is to pick up the drill from the table and put it in the bin.


 * 1) The code

Click on the `Python notebook` link to go to the iPython Notebook interface for your remote simulation. Click `New Notebook` to create a new workspace for this tutorial. If you like, you can click on the title of the new notebook to rename it (e.g., to `Tabletop manipulation`).


 * 1) Initialization

First, we do the usual initialization to get rospy loaded and running, with a good set of package dependencies. Copy and past the following code into a cell and execute it (`Shift-Enter`):

import rospy rospy.init_node('tabletop_manip')

Because we don't want to worry about the robot falling over as we swing its arms around, we're going to cheat a little bit by "pinning" the robot's hip joint to the inertial frame of the world. We'll do this by publishing to the `/atlas/mode` topic, which is one of the "development aids" offered in the Atlas ROS API:

from std_msgs.msg import String mode_pub = rospy.Publisher('/atlas/mode', String, latch=True) mode_pub.publish('pinned')

If you looked closely, you might have seen Atlas's arms wave slightly, a result of the hip-pinning step.


 * 1) Position control of the arms

Now let's start moving the robot's arms around. The ROS API includes the `/atlas/atlas_command` topic, to which we can publish control commands for the robot's body (note that, mirroring the way that the actual robot is architected, control of the head and hands happens over separate topics; we'll get to those later). Let's learn more about that topic by shelling out to rostopic and rosmsg:

!rostopic info /atlas/atlas_command
 * 1) What type is this topic?  Who's publishing and/or subscribing to it?

 Type: atlas_msgs/AtlasCommand

Publishers: * /gazebo (http://10.0.0.51:51410/)

Subscribers: * /gazebo (http://10.0.0.51:51410/)

!rosmsg show atlas_msgs/AtlasCommand
 * 1) What's the structure of an atlas_msgs/AtlasCommand message?

 std_msgs/Header header uint32 seq time stamp string frame_id float64[] position float64[] velocity float64[] effort float32[] kp_position float32[] ki_position float32[] kd_position float32[] kp_velocity uint8[] k_effort float32[] i_effort_min float32[] i_effort_max uint8 desired_controller_period_ms

(To see the raw message definition, which includes explanatory comments, you can pass the `-r` option to `rosmsg`; i.e.: `!rosmsg show -r atlas_msgs/AtlasCommand`. Or you can consult the source.)

That's a somewhat complex message structure. But to do position control, we can fill out just a few of the fields, namely the P and D gains (`kp_position` and `kd_position`) and the desired angles (`position`) for each joint. Let's create a publisher and also build up a basic message that we'll reuse:

from atlas_msgs.msg import AtlasCommand, AtlasState ac_pub = rospy.Publisher('/atlas/atlas_command', AtlasCommand, latch=True) ac = AtlasCommand ac.kp_position = [4000, 4000, 14000, 20, # back 2000, 2000, 2000, 1000, 900, 300, # left leg 2000, 2000, 2000, 1000, 900, 300, # right leg 2000, 1000, 200, 200, 50, 100, # left arm 2000, 1000, 200, 200, 50, 100] ac.kd_position = [100.0, 100.0, 100.0, 1.0, # back 10.0, 10.0, 10.0, 10.0, 2.0, 1.0, # left leg 10.0, 10.0, 10.0, 10.0, 2.0, 1.0, # right leg 20.0, 20.0, 3.0, 3.0, 0.1, 0.2, # left arm 20.0, 20.0, 3.0, 3.0, 0.1, 0.2] # right arm ac.position = [0] * 28
 * 1) A message that we'll reuse
 * 1) Per-joint P gains
 * 1) Per-joint D gains
 * 1) Per-joint position targets (angles).  There are 28 joints and 0 is the default position for each of them.

Now let's move one joint, say, the left shoulder tilt. The joint numbering scheme is defined as a set of constants in the AtlasState message. The joint that we want to move is called `l_arm_shx`. So to move it, we can overwrite its slot in the `position` array and then publish the message:

ac.position[AtlasState.l_arm_shx] = 1.0 ac_pub.publish(ac)
 * 1) Set the desired joint position (in radians)
 * 1) Publish the command

The robot's left arm arm should swing upward:



We can swing the right arm up to match (passing -1.0 because the right arm mirrors the left):

ac.position[AtlasState.r_arm_shx] = -1.0 ac_pub.publish(ac)

Triumphant robot!




 * 1) Higher-level control of the arms

Controlling the arm by setting desired positions on all of the joints works, but will get pretty tedious. Let's step up a level and do something a little more usable. This is where you might use inverse kinematics to get Cartesian control of the end effector. Or you might employ a motion planner to automatically generate arm trajectories. In this tutorial, we'll do something simpler, which is to define and then blend a handful of useful arm configurations. If you're familiar with principal component analysis (PCA), you can think of this as an *unprincipled* way of reducing the dimensionality of the arm control problem.

First, we define two configurations: one is the origin (all zeros) and the other places the robot's right hand near the drill:

origin = [0] * 28 pose1 = [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,-0.43,1.26,1.47,-0.97,-0.24,-0.68]

Try sending the robot to the new configuration:

ac.position = list(pose1) ac_pub.publish(ac)
 * 1) Call list explicitly to avoid later overwriting the contents of pose1



You probably knocked the drill over. That's OK. In the `gz3d` window, click on `Edit->Reset Model Poses` to put it back in place.

Now we'll define a function that linearly interpolates between the origin and the hand-near-drill pose:

def interp(pose, val): for i in range(0,len(pose)): ac.position[i] = origin[i] + val* pose[i] ac_pub.publish(ac)
 * 1) Given pose and 0 <= val <= 1, send robot to origin + val * pose

With this function, we can easily move the robot between its home position and the hand-near-drill pose:

interp(pose1, 0.0)
 * 1) Send the robot home

interp(pose1, 1.0)
 * 1) Send the robot hand's near the drill

More importantly, we can pick intermediate poses:

interp(pose1, 0.5)
 * 1) Send the robot's hand halfway to the drill

That's neat, but typing in floating values is tedious. Let's add a little slider UI element to make things easier.

'''Note: this part works best in Chrome. In other browsers, including Firefox, the slider will probably be rendered as a text box (which probably won't work).'''

In a new cell, tell iPython Notebook that you want the current cell interpreted as Markdown by selecting the menu item Cell->Cell Type->Markdown. The display of the cell will change slightly (e.g., the `In [NN]` tag will disappear). Now copy and paste the following content into the cell and execute it in the usual way (`Shift-Enter`):

Axis 1  0

$(function{   var currentValue1 = $('#currentValue1')    $('#value1').change(function { // Grab a handle to the iPython kernel, move the robot, and also update the HTML display of the value var kernel = IPython.notebook.kernel; kernel.execute("interp(pose1, float("+this.value+"))"); currentValue1.html(this.value); });   $('#value1').change; });

You should see a slider rendered in the browser:



Drag the slider back and forth to move the robot between the two poses that we defined earlier.

That's fun, but is not quite enough to pick up the drill. Let's define more poses for the right arm, along with some global state variables:

pose2 = [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,-1,1,0,-1,0,0] pose3 = [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,2.0,0] pose4 = [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,-2.0,0] pose5 = [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.0,2.0] pose6 = [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.0,-2.0] pose1_val = 0 pose2_val = 0 pose3_val = 0 pose4_val = 0 pose5_val = 0 pose6_val = 0

Now we want a slider for each pose. First, let's define a new callback function that will use the global state variables that we just defined:

def move_arm: for i in range(0,len(pose1)): ac.position[i] = origin[i] + pose1_val * pose1[i] + pose2_val * pose2[i] + pose3_val * pose3[i] + pose4_val * pose4[i] + pose5_val * pose5[i] + pose6_val * pose6[i] ac_pub.publish(ac)
 * 1) Blend all poses together and move the robot:

Now create a new Markdown cell (`Cell->Cell Type->Markdown`) and define a slider for each pose:

Axis 1  0 Axis 2  0 Axis 3  0 Axis 4  0 Axis 5  0 Axis 6  0 $(function{    var poseValue1 = $('#poseValue1')    $('#pose1').change(function { var kernel = IPython.notebook.kernel; kernel.execute("pose1_val = float("+this.value+"); move_arm") poseValue1.html(this.value); });   $('#pose1').change;    var poseValue2 = $('#poseValue2')    $('#pose2').change(function { var kernel = IPython.notebook.kernel; kernel.execute("pose2_val = float("+this.value+"); move_arm") poseValue2.html(this.value); });   $('#pose2').change;    var poseValue3 = $('#poseValue3')    $('#pose3').change(function { var kernel = IPython.notebook.kernel; kernel.execute("pose3_val = float("+this.value+"); move_arm") poseValue3.html(this.value); });   $('#pose3').change;    var poseValue4 = $('#poseValue4')    $('#pose4').change(function { var kernel = IPython.notebook.kernel; kernel.execute("pose4_val = float("+this.value+"); move_arm") poseValue4.html(this.value); });   $('#pose4').change;    var poseValue5 = $('#poseValue5')    $('#pose5').change(function { var kernel = IPython.notebook.kernel; kernel.execute("pose5_val = float("+this.value+"); move_arm") poseValue5.html(this.value); });   $('#pose5').change;    var poseValue6 = $('#poseValue6')    $('#pose6').change(function { var kernel = IPython.notebook.kernel; kernel.execute("pose6_val = float("+this.value+"); move_arm") poseValue6.html(this.value); });   $('#pose6').change; });

Now you should have six sliders, which you can drag back and forth to get the right arm into various configurations:



Now all we're missing is control of the hand itself. Conveniently, the hands accept a simple command message to execute a few basic grasps:

!rostopic info /sandia_hands/r_hand/simple_grasp

 Type: sandia_hand_msgs/SimpleGrasp

Publishers: None

Subscribers: * /sandia_hands/r_hand/simple_grasp_right (http://10.0.0.51:50087/)

!rosmsg show sandia_hand_msgs/SimpleGrasp

 string name float64 closed_amount

As noted in the documentation, the `name` determines the type of grasp and should be one of: `cylindrical`, `spherical`, or `prismatic`. To pick up the drill, let's use a cylindrical grasp. Similar to before, we need to create a publisher to send command messages:

from sandia_hand_msgs.msg import SimpleGrasp rh = SimpleGrasp rh.name = 'cylindrical' rh_pub = rospy.Publisher('/sandia_hands/r_hand/simple_grasp', SimpleGrasp, latch=True)

Try closing and opening the hand:

rh.closed_amount = 1.0 rh_pub.publish(rh)

rh.closed_amount = 0.0 rh_pub.publish(rh)

Now define a function that we can use as a callback:

def move_hand(val): rh.closed_amount = val rh_pub.publish(rh)

Now create a new Markdown cell (`Cell->Cell Type->Markdown`) and create a slider for the hand that uses the new callback:

Hand  0 $(function{    var rhandValue = $('#rhandValue')    $('#rhand').change(function { var kernel = IPython.notebook.kernel; kernel.execute("move_hand(float("+this.value+"))"); rhandValue.html(this.value); });   $('#rhand').change; });

Drag the slider back and forth to close and open the hand.

Using the 6 arm sliders and the 1 hand slider (and a bit of practice), you should be able to pick up the drill and drop it in the bin:

Remember that you can always reset things by, in the `gz3d` window, clicking `Edit->Reset Model Poses`.

Other tutorials from DRCSim
 * 1) Next ##