Torque measures differ from references

francesco-romano commented 8 years ago

We are expecting torques references to be equal to the measured one on the iCub. Unfortunately during some experiments we are not seeing zero error, and this does not seem a simple delay.

I attach some plots: First two are related to a balancing experiment (where the robot is not properly balancing anyway..) 1a) desired des 1b) measured

Second two: impedance with sinusoidal reference 2a) desired des2 2b) measured mes

cc @traversaro @DanielePucci

traversaro commented 7 years ago

This is something that could be tested in icub-tests, putting Priority: IFI Day for this.

traversaro commented 6 years ago

@gabrielenava noted something similar recently:

screenshot from 2018-05-31 16-45-29

Something that it is possible to note is that the problems appears to be negative spikes, and probably the strange behaviour we see in the plots of the original issues are due to the low pass filter contained in the yarp-wholebodyinterface that was used in the WBI-Toolbox 1.0/WB-Toolbox 2.0 that was used in those experiments.

traversaro commented 6 years ago

In analysing the data provided by @gabrielenava, I have to main observations:

I would concentrate the inquiry on the two arms, in which the spikes seems to be more predictable: in those case the measurements go quite close to zero everytime there is an abroupt change.
I always thought that this was some strange race condition due to the use of the Gazebo IPC communication layer inside the gazebo_yarp_controlboard plugin, but apparently this behaviour is still there even after we moved the PID to run sincronously with the gazebo_yarp_controlboard plugin, see https://github.com/robotology/gazebo-yarp-plugins/pull/324 . This surprises me, because... ok, thinking about it I think I find at least one issue, commenting on it in the next comment.

traversaro commented 6 years ago

I think I found at least an issue in the gazebo_yarp_controlboard . As a preliminary step, note that in Gazebo the input to the robotic structure is an input that has the measured of the torque of the joint, that is set using the gazebo::physics::Joint::SetForce method and is get using the gazebo::physics::Joint::GetForce method. In the onUpdate method of the gazebo_yarp_controlboard (the method that is called by the Gazebo physics thread before integrating for one fixed time-step the system) the method gazebo::physics::Joint::GetForce is called before the call to gazebo::physics::Joint::SetForce.

While the semantics of the gazebo::physics::Joint::SetForce/GetForce method is not completly clear from the documentation [1, 2], it is possible to understand their behaviour by checking the code relative to ODE, the default physics engine in Gazebo.

Relevant source code:

The interesting part is in https://bitbucket.org/osrf/gazebo/src/84ae5943bf02843b91a10f82ba084db0d753aee7/gazebo/physics/ode/ODEJoint.cc?at=gazebo9_9.0.0&fileviewer=file-view-default#ODEJoint.cc-1139 . In this method (called by gazebo::physics::ODEJoint::SetForce) the force is reset only after a call to setForce at a timeinstant greater then the last timeinstant at which the method was call. According to this, calling gazebo::physics::Joint::GetForce before gazebo::physics::Joint::SetForce would give us the torque set at the previous timestep. However, I guess something is going wrong sometimes (there is some code that is also calling gazebo::physics::ODEJoint::SetForce?) and we are obtaining the artifact that we see.

As a first test, I would move the for loop invoking m_torques[jnt_cnt] = m_jointPointers[jnt_cnt]->GetForce(0u) to the end of the method, so they will be able to directly get the input set by gazebo::physics::ODEJoint::SetForce , and in theory this problem in the torque feedback should disappear. cc @gabrielenava

[1] https://bitbucket.org/osrf/gazebo/issues/878/joint-setforce-documentation-incomplete [2] https://bitbucket.org/osrf/gazebo/issues/1321/proper-joint-torque-feedback

gabrielenava commented 6 years ago

I initially moved the entire for loop at the end of the onUpdate method, just after the SetForce method:

screenshot from 2018-06-01 10-13-28

however, something went wrong: as soon as I put the robot in Gazebo, it started to move as in the GIF:

icubevil

this is the error I got in the terminal:


yarp: Port /icubSim/floating_base/state:o active at tcp://10.255.35.143:10053/
[ERROR]AnalogWrapper: AnalogServer: Sensor returned timeout error (code 3).
[ERROR]AnalogWrapper: AnalogServer: Sensor returned timeout error (code 3).
yarp: Port /icub-jets/jets/input:i active at tcp://10.255.35.143:10054/
yarp: Port /icub-jets/jets/thrust:o active at tcp://10.255.35.143:10055/
[ERROR]AnalogWrapper: AnalogServer: Sensor returned timeout error (code 3).
[ERROR]An hardware fault occurred on joint  2  torque too big! (  2087.98  ) 
[ERROR]An hardware fault occurred on joint  4  torque too big! (  -2467.88  ) 
[ERROR]An hardware fault occurred on joint  4  torque too big! (  -2207.87  ) 
[ERROR]An hardware fault occurred on joint  5  torque too big! (  -2326.25  ) 
[ERROR]An hardware fault occurred on joint  3  torque too big! (  2092.47  ) 
[ERROR]An hardware fault occurred on joint  5  torque too big! (  -2428.15  ) 
[ERROR]An hardware fault occurred on joint  3  torque too big! (  -2613.7  )

The second time I tried to move at the end the GetForce only:

screenshot from 2018-06-01 10-20-48

This time the robot was correctly loaded in Gazebo, even if I observed that one of the previous error in the terminal is still there:

[ERROR]AnalogWrapper: AnalogServer: Sensor returned timeout error (code 3).
[ERROR]AnalogWrapper: AnalogServer: Sensor returned timeout error (code 3).
yarp: Port /icub-jets/jets/input:i active at tcp://10.255.35.143:10054/
yarp: Port /icub-jets/jets/thrust:o active at tcp://10.255.35.143:10055/
[ERROR]AnalogWrapper: AnalogServer: Sensor returned timeout error (code 3).

However, this didn't solve the issue. This is a picture of the same simulation I performed before:

screenshot from 2018-06-01 10-47-15

traversaro commented 6 years ago

Mhh... Can you dump the stateExt:o port and check the values to make sure that the problem is not on the WB-Toolbox/remotecontrolboardremapper/remote_controlboard side? Thanks!

traversaro commented 6 years ago

cc @prashanthr05 @nunoguedelha @jeljaik

gabrielenava commented 6 years ago

I dumped the stateExt:o of the right leg in a .log file, than I used the readStateExt.m function to plot the joint torque. The spikes are still there, see the attached picture:

screenshot from 2018-06-01 15-24-20

traversaro commented 6 years ago

I am extremly confused by this.

I am expecting that the couple of calls setForce and getForce should be just a set/get of a buffer. At this point, if we are measuring this kind of data, there are two possibilities:

Either setForce is not called with the correct argument, for some reason,
Or setForce and getForce are not a transparent set/get of a buffer.

I think the debug of these two hypothesis would be drastically simplified if we are able to reproduce this behaviour using a single joint controller in torque. In that case, we can easily using a debugger/debug prints to print the arguments and the return values of the setForce and getForce.

FabioBergonti commented 6 years ago

I've the same problem of spikes. This problem often occurs, so I decide to perform a very accurate simulation but is still present.

Ts = 0.0005 %[s] $ Sampling Time Simulink
MaxStepSize = 0.000050 %[s] $ Sampling Time World Gazebo

I did my simulation with iCubGazeboV2_5_plus

Here below I plot the Joint Torque of Torso Pitch Here a zoom of one of the spike That spike occurs at the instant 0.387[s] In the same instant I have a spike also in the torso roll, torso yaw

prashanthr05 commented 6 years ago

@traversaro If you check this line, it says multiple calls to the method SetForce() will accumulate the forces. Could this be causing some kind of issue ? Just wondering if the method is called multiple times and the timestamp is not being updated ?

fiorisi commented 6 years ago

@traversaro @gabrielenava, any updates on this issue?

gabrielenava commented 6 years ago

Not yet. I think I will investigate this issue this week.

prashanthr05 commented 6 years ago

Just a curiosity.

@traversaro If you check this line, it says multiple calls to the method SetForce() will accumulate the forces. Could this be causing some kind of issue ? Just wondering if the method is called multiple times and the timestamp is not being updated ?

One way to test this hypothesis would be to use the Simbody as Gazebo physics engine, in which the SetForce method does not seem to be additive/accumulative.

See this issue

@fiorisi @gabrielenava @traversaro

traversaro commented 6 years ago

@prashanthr05 point is correct, this is a test that you could easily do @gabrielenava , just launch gazebo with the option -e simbody.

gabrielenava commented 6 years ago

I had issues when inserting the model in Gazebo 9.0 with the option -e simbody. In particular:

with icubGazeboSim, Gazebo crashed with the following error:

yarp: Port /icubSim/right_arm/analog:o/rpc:i active at tcp://10.255.111.114:10015/
[Err] [SimbodyMesh.cc:52] SimbodyMesh is not supported
[Err] [SimbodyMesh.cc:52] SimbodyMesh is not supported
[INFO]AnalogServer : no ROS initialization required 
[INFO]created wrapper <analogServer>. See C++ class yarp::dev::AnalogWrapper for documentation.
[INFO]created device <gazebo_forcetorque>. See C++ class GazeboYarpForceTorqueDriver for documentation.
[Err] [SimbodyMesh.cc:52] SimbodyMesh is not supported
[Err] [SimbodyMesh.cc:52] SimbodyMesh is not supported
[Err] [SimbodyMesh.cc:52] SimbodyMesh is not supported
[Err] [SimbodyMesh.cc:52] SimbodyMesh is not supported
[Err] [SimbodyMesh.cc:52] SimbodyMesh is not supported
[Err] [SimbodyMesh.cc:52] SimbodyMesh is not supported
[Err] [SimbodyMesh.cc:52] SimbodyMesh is not supported
yarp: Port /icubSim/right_arm/analog:o active at tcp://10.255.111.114:10016/
[Err] [SimbodyMesh.cc:52] SimbodyMesh is not supported
[Err] [SimbodyMesh.cc:52] SimbodyMesh is not supported
[Err] [SimbodyMesh.cc:52] SimbodyMesh is not supported
[Err] [SimbodyMesh.cc:52] SimbodyMesh is not supported
[Err] [SimbodyMesh.cc:52] SimbodyMesh is not supported
[Err] [SimbodyMesh.cc:52] SimbodyMesh is not supported
[Err] [SimbodyMesh.cc:52] SimbodyMesh is not supported
[Err] [SimbodyMesh.cc:52] SimbodyMesh is not supported
[Err] [SimbodyMesh.cc:52] SimbodyMesh is not supported
[Err] [SimbodyMesh.cc:52] SimbodyMesh is not supported
[Err] [SimbodyMesh.cc:52] SimbodyMesh is not supported
[Err] [SimbodyMesh.cc:52] SimbodyMesh is not supported
[Err] [SimbodyMesh.cc:52] SimbodyMesh is not supported
[Err] [SimbodyMesh.cc:52] SimbodyMesh is not supported
[Err] [SimbodyMesh.cc:52] SimbodyMesh is not supported
[Err] [SimbodyMesh.cc:52] SimbodyMesh is not supported
[Err] [SimbodyMesh.cc:52] SimbodyMesh is not supported
[Err] [SimbodyPhysics.cc:1534] Collision type [33619968] unimplemented
[Err] [SimbodyPhysics.cc:1534] Collision type [33619968] unimplemented
[Err] [SimbodyPhysics.cc:1534] Collision type [33619968] unimplemented
[Err] [SimbodyPhysics.cc:1534] Collision type [33619968] unimplemented
[Err] [SimbodyPhysics.cc:1534] Collision type [33619968] unimplemented
[Err] [SimbodyPhysics.cc:1534] Collision type [33619968] unimplemented
[Err] [SimbodyPhysics.cc:1534] Collision type [33619968] unimplemented
[Err] [SimbodyPhysics.cc:1534] Collision type [33619968] unimplemented
[Err] [SimbodyPhysics.cc:1534] Collision type [33619968] unimplemented
[Err] [SimbodyPhysics.cc:1534] Collision type [33619968] unimplemented
[Err] [SimbodyPhysics.cc:1534] Collision type [33619968] unimplemented
[Err] [SimbodyPhysics.cc:1534] Collision type [33619968] unimplemented
[Err] [SimbodyPhysics.cc:1534] Collision type [33619968] unimplemented
[Err] [SimbodyPhysics.cc:1534] Collision type [33619968] unimplemented
[Err] [SimbodyPhysics.cc:1534] Collision type [33619968] unimplemented
[Err] [SimbodyPhysics.cc:1534] Collision type [33619968] unimplemented
[Err] [SimbodyPhysics.cc:1534] Collision type [33619968] unimplemented
[Err] [SimbodyPhysics.cc:1534] Collision type [33619968] unimplemented
[Err] [SimbodyPhysics.cc:1534] Collision type [33619968] unimplemented
[Err] [SimbodyPhysics.cc:1534] Collision type [33619968] unimplemented
[Err] [SimbodyPhysics.cc:1534] Collision type [33619968] unimplemented
[Err] [SimbodyPhysics.cc:1534] Collision type [33619968] unimplemented
[Err] [SimbodyPhysics.cc:1534] Collision type [33619968] unimplemented
[Err] [SimbodyPhysics.cc:1534] Collision type [33619968] unimplemented
[Err] [SimbodyPhysics.cc:1534] Collision type [33619968] unimplemented
[Err] [SimbodyPhysics.cc:1534] Collision type [33619968] unimplemented
[Err] [World.cc:2147] Loading model from factory message failed
terminate called after throwing an instance of 'SimTK::Exception::CacheEntryOutOfDate'
  what():  SimTK Exception thrown at StateImpl.h:1678:
  State Cache entry was out of date at Stage Time. This entry depends on version 1 of Stage Time but was last updated at version 0.

with iCubGazeboV2_5 Gazebo didn't crash, but:
- the real time factor is 0.02;
- the initial position is wrong, the robot has both arms in this strange position:
- the left leg hip pitch joint is in hardware fault. It is due to too big torque on that joint:
```
[ERROR]An hardware fault occurred on joint  0  torque too big! (  1.17231e+166  ) 
```
- Even if Gazebo did not crash there are several errors and warnings on the terminal. See the attached file : gazeboLogger.txt

gabrielenava commented 6 years ago

I did a couple of tests.

Test 1: is this issue independent from the robot model?

yes, it seems this issue is present with both icubGazeboSim and iCubGazeboV2_5 even if the effect is not on the same joints:

icubGazeboSim yoga not extended left foot, measured torques

screenshot from 2018-08-27 16-13-18

iCubGazeboV2_5 yoga not extended left foot, measured torques

screenshot from 2018-08-27 17-01-22

is this issue independent from the Gazebo integration step? It seems yes, even if decreasing the Gazebo integration step to 0.0005 [s] seemed to improve the torque measurements a bit:

iCubGazeboV2_5, yoga extended both feet Gazebo step 0.0005 screenshot from 2018-08-27 16-53-21

iCubGazeboV2_5, yoga extended both feet Gazebo step 0.001 screenshot from 2018-08-27 16-20-59

Is this issue repeatable? It seems yes. Compare the following picture with the one above:

iCubGazeboV2_5, yoga extended both feet Gazebo step 0.001, test 2

screenshot from 2018-08-27 17-09-54

Now, I will try to understand if it is possible to perform a simulation with the simscape solver.

fiorisi commented 6 years ago

Very interesting @gabrielenava. Probably SimBody is not fully supported anymore.

traversaro commented 6 years ago

Yes, I would say that if simbody was working out of the box it was a nice test, but otherwise let's focus on understanding what's going on with Gazebo's ODE fork (the default physics engine). I guess DART is probably better supported than Gazebo (one of the main developer of the physics part of Gazebo is one of the main DART developers) but unfortunately it is not available out of the box (you need to compiled Gazebo from source to use it). I think can look back at using multiple physics engine when ign-physics is ready.

@gabrielenava Your plots highlight something that I think had never emerged before: during the yoga for some reason the issue is extremely evident on the ankle roll of the support leg (if I am not wrong).

traversaro commented 6 years ago

If we were able to replicate the problem on a single joint (the ankle one) I guess we could try to do what is described in https://github.com/robotology/gazebo-yarp-plugins/issues/238#issuecomment-393884752 for that joint. At the first stage, I would do it at the gazebo-yarp-plugins level to make sure that we are not doing anything stupid in the plugins/YARP level, and the issue is actually in Gazebo. Perhaps this will require some ugly if such as if ( m_jointPointers[jnt_cnt]->GetName() == "l_ankle_roll") or something similar, but at least we will understand a bit more about the issue.

fiorisi commented 6 years ago

As discussed F2F we can also try to run the simulation once with Bullet.

gabrielenava commented 6 years ago

As forsimbody it was not possible to test the simulation with bullet as physics engine. With both icubGazeboSim and iCubGazeboV2_5, Gazebo crashed with the following error:

[Err] [BulletHingeJoint.cc:414] Joint must be created before setting friction
[INFO]created device <gazebo_imu>. See C++ class GazeboYarpIMUDriver for documentation.
[INFO]No ROS group found in config file ... skipping ROS initialization. 
yarp: Port /icubSim/inertial active at tcp://10.255.35.143:10004/
[INFO]Server Inertial : no ROS initialization required 
[INFO]Starting server Inertial thread
[INFO]created wrapper <inertial>. See C++ class yarp::dev::ServerInertial for documentation.
[DEBUG]Writing an Inertial measurement.
[INFO]No ROS group found in config file ... skipping ROS initialization. 
yarp: Port /icubSim/left_leg/analog:o/rpc:i active at tcp://10.255.35.143:10005/
[INFO]AnalogServer : no ROS initialization required 
[INFO]created wrapper <analogServer>. See C++ class yarp::dev::AnalogWrapper for documentation.
[INFO]created device <gazebo_forcetorque>. See C++ class GazeboYarpForceTorqueDriver for documentation.
yarp: Port /icubSim/left_leg/analog:o active at tcp://10.255.35.143:10006/
[Err] [BulletJoint.cc:389] Bullet Joint [l_leg_ft_sensor] ID is invalid
gzserver: /var/lib/jenkins/workspace/gazebo9-debbuilder/build/gazebo-9.3.1/gazebo/physics/bullet/BulletJoint.cc:368: virtual gazebo::physics::JointWrench gazebo::physics::BulletJoint::GetForceTorque(unsigned int): Assertion `(this->constraint != nullptr)&&("constraint should be valid")' failed.

maybe if we remove the FT sensors it may work?

traversaro commented 6 years ago

I do not think it is worth spending time trying to get bullet to work.

fiorisi commented 6 years ago

Thanks @gabrielenava for the test. I agree with @traversaro, let's not invest other time with Bullet.

gabrielenava commented 6 years ago

I had an intuition concerning this issue that may help in better understanding what is going on. I first run the Yoga extended demo on left foot only with the iCubGazeboV2_5. The result is the same already obtained in this comment:

Yoga left foot

screenshot from 2018-08-31 11-27-46

The joints that show the biggest spikes are the r_ankle_pitch and the r_hip_yaw. Then, looking at the measured joint positions it looks like both these joints are touching some sort of joint limit during the yoga movements:

screenshot from 2018-08-31 11-49-58

I identified the limit as the one set in the plugin configuration files and I therefore modified the r_ankle_pitch joint limits from 30 [deg] to 300 [deg]. This is my modification:

[LIMITS]
jntPosMax 85.0 90.0 70.0 0.0 300.0 20.0
jntPosMin -35.0 -15.0 -70.0 -100.0 -300.0 -20.0

Then, I run again the yoga extended demo. As you can see, the torques spikes related to the r_ankle_pitch (the light green line) completely disappeared.

screenshot from 2018-08-31 12-01-44

Thinking about it, I looked again at the measured joint positions and this is now the ankle pitch joint position during the yoga:

screenshot from 2018-08-31 12-04-57

as you can see it is slightly above 30 [deg], that was the previous joint limit. To be precise, the reference joint position that the Simulink controller is asking for the ankle pitch joint is 35 [deg].

I still don't know why, but it seems that the spikes are due to:

the high-level controller asking for a reference position that is outside the joint limits;
the joint blocked for a long period of time at the joint limit.

cc @traversaro @DanielePucci @fiorisi

traversaro commented 6 years ago

Great @gabrielenava ! Do you think this explains also the original behaviour that you observed in https://github.com/robotology/gazebo-yarp-plugins/issues/238#issuecomment-393696603 ?

gabrielenava commented 6 years ago

I think it is possible, I can try to reproduce the experiment and see if the joint is reaching the joint limit.

DanielePucci commented 6 years ago

Very good analysis @gabrielenava, thanks a lot. Just a question: have we ever tried to do the same tests with the robot on the pole, namely fixed joint at the base? I fear that when handling contacts, the integration may cause some fo these spikes

gabrielenava commented 6 years ago

Very good analysis @gabrielenava, thanks a lot. Just a question: have we ever tried to do the same tests with the robot on the pole, namely fixed joint at the base? I fear that when handling contacts, the integration may cause some fo these spikes

Yes, I did the same test with the robot on the pole. For example, here we have the results obtained with the icubGazeboSim model fixed on the pole. The robot is performing gravity compensation + each joint follows a sine reference trajectory with amplitude 5 [deg]. The only joints that are touching the limit while following the reference are the knee joints. And every time the joints touch the limit, there are spikes (see the following figure):

screenshot from 2018-09-03 10-06-28

gabrielenava commented 6 years ago

I was able to replicate the original experiment by reverting the ironbot controller to an old commit from May. The result is not exactly the same but the experiment is very similar to the previous one, and the spikes are clearly visible:

Desired joint torques vs measured joint torques:

screenshot from 2018-09-05 14-22-13

Then, I increased all the controlled joints' range of motion as explained here. The spikes disappeared:

Desired joint torques vs measured joint torques:

screenshot from 2018-09-05 14-42-18

Note: these first tests with the flying controller were not super-repeatable, that's why the controller's behavior is different. By the way, the second plot is more similar to the one in the original experiment .

This confirmed that the spikes in the measured torques are triggered every time the joint hits the joint limits specified in the controlboard configuration file. We still don't know why this happens.

robotology / gazebo-yarp-plugins

Torque measures differ from references #238

Test 1: is this issue independent from the robot model?