Force/Torque sensing with ZMP calculation [closed]

edit

Also in the BDI simulation (nothing like some good reverse engineering ...)

The forces and torques in the /atlas/atlas_states topic are not the same as the forces and torques in the /atlas/debug/l_foot_contact and /atlas/debug/r_foot_contact topics.

The /atlas/atlas_states Fz, Mx, and My are reasonable with Fz around body weight (less than 1000N), and COP excursions of less than 0.1m. Perhaps the problem above is not using the correct foot COP zero, the foot is bigger than we think, or the gazebo simulation is holding the foot down and preventing it from tipping as it should. After all, the fingers stick to objects, why not the feet. See http://answers.gazebosim.org/question/1700/sticky-objects-pr2-simulation-objects-stick-to/

Better to think about individual foot COPs, rather than the overall COP. The foot COP is the overall COP when the system is in single support (the BDI walk has "trivial" double support with the touchdown foot having only a small Fz). When a foot Mx and My are zero, that puts the COP for that foot somewhere (needs to be defined more clearly) which may not be under the foot. Non-zero Mx/Fz and My/Fz for each foot are excursions of the individual foot COP from that point. It is true that Mx is about 80Nm, which is an 8cm excursion in the roll direction (guess BDI decided to use that ankle roll actuator after all ...). While standing, each foot has about a 40Nm torque (which could actually mean zero ankle roll torque, depending on where the foot COP zero is defined.). So the difference between the standing roll COP and the single support roll COP is only 4cm (which could be within the foot). The interesting question then is why the roll COP zero is not in the center of the foot. The pitch COP excursion is a full 8cm, which can be accomodated inside the foot if the pitch COP zero is at one end of the foot.

The /atlas/debug/x_foot_contact look like garbage and are about 10 times too big. It would be surprising to have horizontal forces on the foot that are larger than body weight.

OSRF folks: Where is the foot COP zero (where is the point where an applied force does not register a torque on the force/torque sensor)?

Thanks, Chris

Here is what I understand after plotting the data and groveling through the code.

1) The L/R_LEG_UAY effort agrees very well with the My reported for the foot force/torque sensor.

2) The L/R_LEG_LAX effort is proportional to Fz reported for the foot force torque/sensor. This indicates that there is a typo in the code or the moment arm used to calculate the rxF component of the torque has a wrong Y or Z component. It is probably the Y component, since the Z component is used in the My calculation for 1).

On further thought it has to be the Y component, since it is Fz that is the corrupting source through the ry*Fz term.

3) I did not see any typos in
AtlasPlugin.cpp
but the values seem to depend on stuff in something called
AtlasFootSensor
defined in BDI-land:
AtlasSimInterfaceTypes.h
At that point, I had to give up since I ran out of source code to look at.

4) So all the reported force/torque sensors are corrupted in some way. Use the L/R_LEG_UAY effort for My and the L/R_LEG_LAX effort for Mx. Fz seems to be okay.

5) The maximum roll torque (L/R_LEG_LAX effort) is around 50Nm, which means about a 5cm COP excursion. The foot is +/-6cm wide, so the COP remains within the foot.

Detective Chris

Could you give me the link of the tutorial you mentioned?