add ZMP computation

[thomas-moulard]

I was wondering if there has been attempt to add ZMP computation in metapod or if these is any plan to add ZMP computation support in the future?

Thanks!

[aelkhour]

Assuming you have already run the RNEA with all contact forces set to zero, it is fairly simple to compute the ZMP by retrieving the root joint spatial force, expressing it in the world frame, and computing the point on the floor where the moment horizontal components are zero.

All I can provide for now (sorry) is a code snippet I used to compute the ZMP:

// Express root spatial resultant force in world frame.
// BODY is the floating base link, WAIST is the floating joint.
metapod::Spatial::Force af = BODY::iX0.applyInv (WAIST::f);
// Compute ZMP
zmpPInWorld[0] = - af.n()[1] / af.f()[2];
zmpPInWorld[1] =   af.n()[0] / af.f()[2];
zmpPInWorld[2] =   0;

[thomas-moulard]

thanks for the snippet Antonio, I will probably go for this but it would be able to take advantage of metapod to avoid recomputing shared quantities...

[thomas-moulard]

It seems the API changed. Can we still access the free floating base? It seems that the simple humanoid robot does not have a ff anymore...?

I have currently this code:

  // Express root spatial resultant force in world frame.
  // BODY is the floating base link, WAIST is the floating joint.
  metapod::Spatial::Force af =
    boost::fusion::at_c<metapod::simple_humanoid::FIXME> // FIXME: no floating base?
    (robot.nodes).body.iX0.applyInv
    (boost::fusion::at_c<metapod::simple_humanoid::WAIST_LINK0>
     (robot.nodes).joint.f);

WDYT?

[aelkhour]

boost::fusion::at_c is templated by the node id, and the node encapsulates both link and joint. So the following snippet will work:

metapod::Spatial::Force af
  = boost::fusion::at_c<0> (robot.nodes).body.iX0.applyInv
  (boost::fusion::at_c<0> (robot.nodes).joint.f);
metapod::Vector3d zmp (- af.n()[1] / af.f()[2],
                         af.n()[0] / af.f()[2],
                         0.);

However, it makes the strong assumption that the node with node id equal to 0 is the root joint of the robot.

Sébastien, as you are the one who introduced the new structures, I would like your opinion on this: can we assume that the first node is always the root node, or is there a safer way to retrieve the root node id?

[sbarthelemy]

Hello,

I would not hardcode 0 since it won't work (for the second robot) if you have two robots in the world.

I have not thought much on the constraints on the ordering of the node ids, we might need to let the user choose the ordering itself someday. Just as we had to let the user pick its own dof ordering to keep compatibility with simple_humanoid. If we go that road, we can imagine 0 not being the robot root node.

There is an enum defined within the class which maps names to numeric ids. Hence here you can use simple_humanoid::WAIST_LINK0 instead of 0.

The names of joints and links are stored as strings too, you could also use them to look for an id at runtime.

You could also use the child[0-4]_id member variables of the simple_humanoid class to get the numeric ids of all the root joints in the world (ie. the joints just below the Galilean frame of reference). Hence here you can use simple_humanoid::child0_id instead of 0.

Note eventually that the child[0-4]_id member variables limit us to 5 child per node, I think we could replace them with compile-time list of integers from boost::mpl. That day, this zmp implementation will break again.