# Force/Torque sensing with ZMP calculation [closed]

In the embedded image, the red arrow shows the reaction force at the calculated CoP/ZMP point while the robot is walking under the control of the BDI interface as described in tutorial.

Using the simulated force/torque values, the calculated ZMP always appears to the inside of the foot support polygon. For flat contact, it should be within support polygon.

I see three options:

1) I screwed up a calculation, CoP=( My/Rz, -Mx/Rz); I've looked through my code and don't see it.

2) Simulation values are not consistent with support.

3) BDI uses a "fall to center" walking strategy

Has anyone verified the support calculations?

edit retag reopen merge delete

### Closed for the following reason question is not relevant or outdated by nkoenig close date 2013-09-19 10:09:30.383629

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

( 2016-12-16 22:28:49 -0500 )edit

The "tutorial" would have been one of the early tutorials published as part of the drcsim development. Given the rapid pace of development,and subsequent revisions, I doubt that particular tutorial is still available.

( 2016-12-19 21:40:48 -0500 )edit

The "tutorial" was one of the early drcsim tutorials. May not be available at this point

( 2016-12-19 21:41:51 -0500 )edit

I wonder if you can help me with the steps to get the zmp for atlas robot?

( 2017-01-03 06:26:23 -0500 )edit

Sort by » oldest newest most voted

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

more

This was my calculation of left foot CoP during BDI walking left foot stance. I was assuming zero colocated with ankle joint. If you rotated this point 90 degrees about z-axis I'd think it reasonable.

( 2013-03-15 18:48:11 -0500 )edit

Hi Chris, the contact forces are indeed wrong. I managed to mangle the publish loop in drcsim 2.2 release. Ticketed https://bitbucket.org/osrf/drcsim/issue/153/foot-contact-ros-topic-published-values, and pull request https://bitbucket.org/osrf/drcsim/pull-request/156/fix-issue-153/diff should fix it, will make a patch release soon.

( 2013-03-16 04:25:34 -0500 )edit

As a follow up, can you confirm which frame the data is reported in. My quick check seemed to say that the code used "child frame", which should be foot. But per my comment above, the results I'm getting would make more sense if the data were in talus frame, and I needed to rotate them to foot frame. I'm going to hold off spending more resources on this until the patch is issued, but that is what I'll first look into.

( 2013-03-16 13:08:42 -0500 )edit
( 2013-03-16 13:59:48 -0500 )edit

@cga Hi Chris, the force torque sensor are reporting force and torque information from the l_leg_lax and r_leg_lax joints, oriented in the parent link l_talus and r_talus frames respectively.

( 2013-03-16 14:18:18 -0500 )edit

hmm. I was hoping the talus and foot frames were rotated given different joint axes, but that's not the case if I view in rviz. Now I wonder if we have enough information. Normally, you want the normal force to foot sole. Assuming rotation between, talus and foot, we can't find the total force in normal direction.. I don't think this is explains the ZMP offset b/c it shows up if the robot is standing, when axes should be aligned.

( 2013-03-16 16:24:58 -0500 )edit

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

more