Simulation visualizer is not properly visualizing the system when moving the torso

[traversaro]

It appears like that (just for the visualizer) the base body is assumed to be fixed, and so the foot appears as moving even if their position is constant.

It easy to reproduce the problem by setting a sine trajectory in the input to the desired value of the torso yaw.

[traversaro]

cc @naveenoid @gabrielenava

[naveenoid]

Could you push some test code in some branch to test this? The visualiser assumes feet are fixed to ground. Not torso. Or do you mean the defunct visualiser of C++ based on iCubGui?

---

Naveen Kuppuswamy, PhD Post-doctoral Fellow, Cognitive Humanoids Lab, Department of Robotics, Brain and Cognitive Sciences (RBCS), Istituto Italiano di Tecnologia, Genova, Italy

On Wed, Oct 21, 2015 at 1:05 PM, Silvio Traversaro <notifications@github.com

wrote:

cc @naveenoid https://github.com/naveenoid @gabrielenava https://github.com/gabrielenava

— Reply to this email directly or view it on GitHub https://github.com/robotology/mex-wholebodymodel/issues/36#issuecomment-149856740 .

[traversaro]

Test code in https://github.com/robotology/mex-wholebodymodel/compare/tue . Just run mex-wholebodymodel/integrateForwardDynamics.m to see the issue. Originally reported by @Eric092 .

[traversaro]

Apparently the problem is not occurring on the master branch, but just on the tue branch.

Given that in the tue branch the only difference is that the desired position given to the impedance are time variant instead of being constant, probably the bug is related to some wrong use of the desired joint position vector that the visualizer is doing.

[gabrielenava]

The constrains for the feet are in term of acceleration, hence the feet position is obtained by integration and there are integration errors. In integrateForwardDynamics, try to reduce both RelTol and AbsTol in the options for ode15s tolerances to a very small value, for exemple 1e-9. then, plot also the left foot orientation and not just the position and verify that their movements are very little (1e-8 at least)

[traversaro]

I am quite sure those are not integration errors. Feel free to check for yourself using the tue branch. For some reason, the floating base is fixed in the visualization while the foot are moving.

[gabrielenava]

is the problem only in the visualizer-demo or in the visualizer-graphics too? I tried to reproduce the described situation in the original code because I'm now working on something similar but in the graphics everything seems ok, both the feet position and orientation and the CoM are practically fixed while the torso jaw is moving with a sine trajectory. If the problem is only in visualizer-demo, I can check but I haven't written the function VisualizeForwardDynamics.m and I don't know exactly how to discover the bug

[gabrielenava]

however I'll try also the tue branch right now

[traversaro]

What do you mean by in visualizer-demo or in the visualizer-graphics ?

[traversaro]

I am just running integrateForwardDynamics.m script.

[traversaro]

I plan to look into the VisualizeForwardDynamics.m function, to fix problems like this and https://github.com/robotology/mex-wholebodymodel/issues/39 .

[traversaro]

First step @gabrielenava ... where is located the VisualizeForwardDynamics.m function? :D

[gabrielenava]

In integrateForwardDynamics there are 2 different functions for visualization, one is visualizer_graphics.m which uses forwardDynamics.m to plot left foot position, CoM error, joints torques, ecc.... . The other is visualizer_demo.m which uses storedTestTrajectory.mat to show the movements to the robot. Since visualizer_graphics.m calls forwardDynamics.m, if there's an error in the plots too the issue probably regards the entire balancing code. But if all the plots are correct and only the demo is wrong, I think the issue is limited to the function VisualizeForwardDynamics.m, that is not in the mex_wholbodymodel_balancing folder but is in the mex_wholebodymodel folder because it was an "already given" function.

[gabrielenava]

/codyco-superbuild/main/mexWholeBodyModel/matlab-src/VisualizeForwardDynamics.m this is the path from codyco folder

[traversaro]

Found it, it was just visualizeForwardDynamics.m instead of VisualizeForwardDynamics.m.

[DanielePucci]

What does

tue

mean?

[traversaro]

https://www.tue.nl/en/

[DanielePucci]

ack.

[naveenoid]

status of this issue? I need to know where to focus to avoid clashes on solving

[traversaro]

This issues is still not investigated.

[gabrielenava]

I tried to reproduce this issue with both the stack of task and the joint space controller, in my branch. Since this problem is not reproducible, I suppose that it has been fixed indirectly while fixing other issues.

[traversaro]

Yes, perfect!