search

simbody / simbody

High-performance C++ multibody dynamics/physics library for simulating articulated biomechanical and mechanical systems like vehicles, robots, and the human skeleton.
https://simtk.org/home/simbody
Apache License 2.0
2.29k stars 468 forks source link

Gazebo2Simbody can't generate correct simbody model for gazebo static model #636

Open jacknlliu opened 6 years ago

jacknlliu commented 6 years ago

From gazebo issue 2466, we find that the static model can't be set on correct pose in the gazebo world when using simbody physics engine.

We add two static models 'table', but they are all set on the origin point. But actually they should like

gazebo-ode-static-model

But using examples/Gazebo2Simbody or gazebo with simbody, we get something like

Gazebo2Simbody static model

The link frame seems right, but the visual parts of two static models are on the origin point.

The SDF file is here.

sherm1 commented 6 years ago

Thanks for reporting this, @jacknlliu. I looked at the Gazebo2Simbody example (which has a crude partial sdf parser built in). I found this at Gazebo2Simbody.cpp:627:

if (model.isStatic) {
    mobod = matter.updGround();
} else if ...

indicating that I thought static meant "attached to the ground (world) body". The tables are actually attached to their own links though, with those links fixed to world instead. Although I don't think this is the right fix, commenting that out to read:

/*if (model.isStatic) {
    mobod = matter.updGround();
} else*/ if ...

produced the right starting image: gazebo2simbody_1

I didn't look in Gazebo but I wouldn't be surprised if it also has the same mistake, probably copied over from this example.

There is still a problem, at least in Gazebo2Simbody. It still doesn't understand that the links in the static models should be welded to world. Instead it is making them free with respect to world. If you allow the simulation to run you can see the tables bounce around (tighten the Accuracy setting from 0.1 to 0.01 to get a stable simulation where they sit still).

I think the best fix would be that the MultibodyGraphMaker should be told that the links are static and that it should create weld joints for them rather than free joints, when no other joint is specified.

A workaround in the sdf file, with the same effect, would be to make the table models non-static, but specify weld joints for the links to make them effectively static.

jacknlliu commented 6 years ago

@sherm1 Thanks for your reply. I find that the codes of Gazebo are copied over from this example, and here has the same mistake.

I think this example is the best reference for gazebo, hope it will be fixed by simbody community. Then we can cast it to gazebo.

jacknlliu commented 6 years ago

I try to fix it. But which one is the appropriate method for this, Constraint::Weld or MobilizedBody::Weld? I'm not familiar with the simbody terminology and API. Any more help about this? @sherm1

sherm1 commented 6 years ago

Hi, Jack. MobilizedBody::Weld would be the right choice here. That is much more efficient (essentially free since there are zero dofs) than Constraint::Weld which would add 6 constraint equations to prevent motion.

In internal coordinate multibody systems, tree joints (what Simbody calls "mobilizers") are implemented with one differential equation per degree of freedom, so for example one equation for a pin (revolute) joint or sliding (prismatic) joint, but six equations for a free joint. Constraints are the opposite -- it would take 5 constraint equations to implement a pin joint, and zero for a free "constraint".

sherm1 commented 6 years ago

Basically, for bodies that have no connection to the world we need to distinguish the ones that should be free to move from those that should be welded to ground. Currently MultibodyGraphMaker adds a Free mobilizer (six dofs) to those unconnected bodies; for static bodies it should b adding a Weld mobilizer (zero dofs) instead.