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.
> 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.
Good
> 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],
-ANKLE_LOCATION*FOOT_LENGTH + FOOT_LENGTH/2,
0.0,
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.