16-264: Humanoids: Build and Control a Humanoid

The point of this assignment is to explore simulating and controlling a humanoid.

1. Create a simulation of a planar model of a human. It should have the following parts, with the following masses. Each part should have the height given (length in the case of the foot). You can choose the other dimensions. The two legs are combined into one, and the two arms are also combined, so this humanoid has only one leg and one arm. The center of mass of each piece should be the center of the shape (ODE default, not true for real humans). Let ODE generate the moments of inertia, or if you are using some other simulation tool use the moments of inertia of the shape you have chosen with uniform density. The ankle is located 0.3*foot_length from the back of the foot.

Body Part Height (m) Mass (kg)
Head 0.153 4.0
Torso 0.762 34.0
Upper arms combined 0.282 3.68
Forearms combined 0.264 2.22
Hands combined 0.16 0.8
Thighs combined 0.406 13.04
Shanks combined 0.412 5.38
Feet combined 0.254 1.6

This code is a start on building the humanoid in simulation. Unfortunately, I had to decrease the simulation time step to 0.0001 to keep it stable, so the simulation runs much more slowly now.

New version of sample code that writes data files. This code expands into a directory humanoids-ass3 with subdirectories. Look in body2. The program animate allows you to play back the data file. In the subdirectory mrdplot there is matlab code to examine the data file. Ignore drawstuff-linux, drawstuff-windows, and lib. They are probably not necessary for you since you are using full ODE, unlike my other class.

2. Create a controller that allows this humanoid to stand, balance, and touch targets in space (in the appropriate plane). Consider both external targets (x, y locations) and targets on the body (such as the nose or knee). When touching targets on the body you should take into account various body configurations.

3. Create a controller that allows this humanoid to get up from lying down. Ignore human joint angle limitations.

4. Put a golf club in this humanoids hand (just weld it there, no need to simulate gripping) and get the humanoid to swing the club and hit a golf ball on the ground.

You can use any type of computer/OS/language/simulation package you want. You can work in groups or alone.

What to turn in?

Some Questions and Answers

> We took the C program which also tries to keep COMx over the foot and
> incorporated it into the starter code that you gave us.
> During initialization, we use the gradient descent code to determine
> all of the angles.
Good. Do the angles seem reasonable? If you tell it to go where it is,
does it stay there?

> Then we put those angles into the joint_positions_desired array.  We
> run into confusion with the x_desired and y_desired coordinates. We don't
> know where positive x and positive y, or where 0, 0 are in relation to ODE.
You can either figure this out by experimenting, or looking at where
stuff is put. The location of the ball and the hole in the golf course
are visual cues (look at the hole file to see where they are).

In body1.cpp:
 In draw_stuff
  uncommenting the printf line will tell you where the robot is.
  // redraw robot
  for ( i = 0; i < n_robot_parts; i++ )
      pos = dGeomGetPosition( robot_geoms[i] );
      // printf( "pos: %g %g %g\n", pos[0], pos[1], pos[2] );
 In create_bodies_and_geoms()
  The foot is put at:
  dBodySetPosition( robot_bodies[i],
                    FOOT_THICKNESS/2.0 + FLOOR_HEIGHT );

> Furthermore, we are not sure how to go about adding the arm elements into > the gradient descent algorithm. In body1.cpp In create_bodies_and_geoms() You see that the FOOT is created, then the SHANK, THIGH, and TORSO. You need to add corresponding stuff for an arm. Something like (after the torso section) // ARM i++; robot_bodies[i] = dBodyCreate( world ); robot_geoms[i] = dCreateBox( space, BODY_THICKNESS, BODY_WIDTH, ARM_LENGTH ); dMassSetBoxTotal( &m, ARM_MASS, BODY_THICKNESS, BODY_WIDTH, ARM_HEIGHT ); dBodySetMass( robot_bodies[i], &m ); dGeomSetBody( robot_geoms[i], robot_bodies[i] ); dBodySetPosition( robot_bodies[i], 0.0, 0.0, // minus ARM_HEIGHT since arm goes down -ARM_HEIGHT/2 + TORSO_HEIGHT + THIGH_HEIGHT + SHANK_HEIGHT + FOOT_THICKNESS + FLOOR_HEIGHT ); j_shoulder = j = n_robot_joints++; robot_joints[j] = dJointCreateHinge( world, robot_joint_group ); dJointAttach( robot_joints[j], robot_bodies[i-1], robot_bodies[i] ); dJointSetHingeAnchor( robot_joints[j], 0.0, 0.0, TORSO_HEIGHT + THIGH_HEIGHT + SHANK_HEIGHT + FOOT_THICKNESS + FLOOR_HEIGHT ); dJointSetHingeAxis( robot_joints[j], 0.0, 1.0, 0.0 ); You will need to control that joint, so you will need to add values for desired position and control gains in initialize().

> We are able to get the arm and head onto the robot, however we don't know > how to determine the new jacobian matrix and other values for the gradient > descent algorithm. Follow the pattern in the examples. One version of the pattern: tipx = l1*cos(a1) + l2*cos(a1 + a2); tipy = l1*sin(a1) + l2*sin(a1 + a2); Another: pos1 = [l1*cos(a1) l1*sin(a1)]'; pos2 = pos1 + [l2*cos(a12) l2*sin(a12)]'; pos = pos2 + [l3*cos(a123) l2*sin(a123)]'; Another: tip = Ra * ( link1 + Rk * ( link2 + Rh * link3 ) ) Everything else stays the same, except the Jacobian/gradient also includes derivatives with respect to new joint variables you have added as well. Matlab or Mathematica or Maple is helpful in taking these derivatives.

> ´┐ŻOur robot isn't reaching for a point using the shank, thigh, and torso, but > rather by holding those constant and using upper arm, fore arm, and > wrist. My intent is that you reach from your feet, not from your shoulder. As your arm moves forward, your ankle, knee, hip, waist should also move. Why? 1) to keep the center of pressure in the middle of the feet, something needs to move backwards when the arm moves forward (especially if there is a big load in your hand). 2) you won't be able to pick a ball up off the floor without crouching. 3) doing stuff like swinging a club (or kicking or punching) are whole body behaviors.

> right now we're working on the part of the controller where the > robot has to stand and balance, but basically all of the controller > is built upon the idea of torques and we don't know how to set a > torque for a given joint. and > I'm wondering how to balance the robot. I'm thinking that maybe I need to > use 'torque'. So I'm using a function in ODE called > "dJointAddHingeTorque". I don't think I'm using it right, because no > matter what value I put into it for 'torque', it doesn't seem to have any > effect on the robot. Or maybe that's not what I'm supposed to be using at > all for balancing? I was just wondering what to do with that. The software I wrote is already set up to apply torques. Look in body1.cpp for control_robot( HOLE *h ). This allows you to set desired positions for the joints and run controllers. Multiplying position error by a position gain and velocity error by a velocity gain is often referred to as a "PD (Position Derivative) servo" dJointAddHingeTorque( robot_joints[j], joint_torques[j] ); applies torque to a specific joint.

> xxx isn't working, The simulation does wierd stuff It is often useful to separate planning, control, and simulation. Controllers can blow up due to dynamics, and simulations can blow up due to bad conditioning, even with a good plan. Try to debug your plan separately from running it.

> I have a question regarding the following code from the examples: > tipx = -lshank*sa - lthigh*sak - ltorso*sakh; > tipy = lshank*ca + lthigh*cak + ltorso*cakh; > I just wanted to confirm - I'm thinking that here, the x-component is > referring to the vertical component, from ODE's point of view - is that > right? And the y-component is referring to the horizontal component? > Because otherwise it wouldn't really make much sense to me... You are defining the coordinate system you will use, not ODE. In general (and for ODE) x is horizontal. For ODE, z is up. For you and this equation, tipy = lshank*ca + lthigh*cak + ltorso*cakh; y is up. If you want to call this tipz, go right ahead.

> Ok - I just needed some more clarification though... > So if tipy is up, then I'm not exactly sure what the angles here are > measuring... Because if "aa" - ankle angle - was measuring the angle > between the foot and the thigh, NOT THIGH, SHANK (or CALF) > then I would think that "lshank*ca" would > actually be measuring the x-component of the location of the knee, which > should be an irrelevant calculation to find out the y-component of the > tip. y is up, lshank*ca says that when the angle is zero, the knee is lshank above the ankle. > is "aa" measuring the angle between the vertical > and the thigh? Because that would then measure the y-location of the > knee, right? Yes, ankle angle is with respect to vertical. > But then why would "aa" be called the "ankle angle" in the > case, if it doesn't have anything to directly do with the angle (but > rather it does with the knee) ? Was just kind of confused about that... What definition of ankle angle would you prefer? In this case ankle angle measures the deviation from the zero position, which is the leg straight up (an arbitrary definition, but common). The first angle in a chain of links like this has to be defined with respect to a fixed reference, usually with repect to vertical or horizontal. If we lifted the robot up and held it, the foot would move when we changed the ankle angle, and the deviation of the foot from horizontal would be the SAME ankle angle. To match your equations tipx = -lshank*sa - lthigh*sak - ltorso*sakh; tipy = lshank*ca + lthigh*cak + ltorso*cakh; aa would be the angle of the shank (not thigh) with respect to vertical. Knee backward would be a positive ankle angle. Flip the sign of tipx to change this. ak would be the amount of bending of the knee (straight leg is zero). Hip backward would be a positive knee angle. Flip the sign of tipx to change this. ah would be the amount of bending of the hip (torso and thigh in straight line is zero). Head backwards would be a positive hip angle. Flip the sign of tipx to fix change this.