Entity::SetStatic() does not work after the model is loaded

[osrf-migration]

Original report (archived issue) by Andrei Haidu (Bitbucket: ahaidu).

The original report had attachments: static_test.cc, static_test.world

---

When calling Entity::SetStatic from a plugin (in the Update loop or in the Load method) it has no effect on the model. This might be included in issue 528 (#528).

[osrf-migration]

Original comment by Andrei Haidu (Bitbucket: ahaidu).

---

• set attachment to "static_test.world"

world uploaded

[osrf-migration]

Original comment by Andrei Haidu (Bitbucket: ahaidu).

---

I would like to try fixing this, any pointers on how this should be done?

[osrf-migration]

Original comment by Nate Koenig (Bitbucket: Nathan Koenig).

---

This would be a great feature to implement. Just want to warn you that it may be a bit complicated.

When a model is loaded and static=true, then it does not get a dynamic body in the physics engine. See gazebo/physics/ode/ODELink.cc:Init.

You try adding a function to ODELink that assigns a body to a static object when SetStatic is set to true.

Grep for IsStatic in the physics directory to see where else the flag is used.

[osrf-migration]

Original comment by Steve Peters (Bitbucket: Steven Peters, GitHub: scpeters).

---

It is different for each physics engine as well. Bullet determines whether bodies are static by checking their mass and inertia for zero values.

[osrf-migration]

Original comment by Andrei Haidu (Bitbucket: ahaidu).

---

So, I managed to put together a way to make this work for ODE.

In the ODE user guide at 12.4 is explained is how static objects are created. Basically by not creating a dynamic body as mentioned by @nkoenig :

#!c++
this->linkId = dBodyCreate(this->odePhysics->GetWorldId());
dBodySetData(this->linkId, this);

For the implementation I re-used the on the TODO list function: Link::SetLinkStatic , more specifically ODELink::SetLinkStatic:

#!c++
//////////////////////////////////////////////////
void ODELink::SetLinkStatic(bool _static)
{
    // set static flag, the models' flag is not included
    this->SetStatic(_static);

    if(_static)
    {
      // remove dBody making the object static  
      if(this->linkId)
      {
        dBodyDestroy(this->linkId);
      }

      this->linkId = NULL;
    }
    else
    {
        // re-initialize Link, thus re-creating a dBody with a new linkId
        this->Init();
    }
}

This surprisingly worked well. I tried it with the following models: a single collision, two collisions in a link, and two links. In all cases it worked well.

Now the issues:

• This method allows the links to be set to static independently. The isStatic flag will only be updated for the links, not for the model. However, I think this would be nice to have it this way. This might also require an update to the SDF so links can be set as static as well.

• There is no method from the Model side that allows this, however something like SetModelStatic could be easily implemented, or using the general Entity::SetStatic and checking for the Link type and calling SetLinkStatic :

#!c++
//////////////////////////////////////////////////
void Entity::SetStatic(const bool &_s)
{
  Base_V::iterator iter;

  this->isStatic = _s;

  for (iter = this->children.begin(); iter != this->children.end(); ++iter)
  {
    EntityPtr e = boost::dynamic_pointer_cast<Entity>(*iter);
    if (e)
      e->SetStatic(_s);

    // check if type link
    if((*iter)->GetType == 5)
    {
        (*iter)->SetLinkStatic(_s);
    }

  }
}

• I did not check if the linkID needs to stay constant, since for every toggle of static/non-static it gets a new ID. This is binded to the MoveCallback method, however for every new ID its re-binded in the Init.
• I am also not sure what happens to the Callback functions ( MoveCallback , DisabledCallback) since I do not remove them anywhere.

[osrf-migration]

Original comment by Steve Peters (Bitbucket: Steven Peters, GitHub: scpeters).

---

Nice!

Yes, we would need some new sdformat tags to mark individual links as static. It could follow the pattern of the self_collide tags, which are defined for both links and models, and use the model's value if the link value is not specified.

[osrf-migration]

Original comment by Andrei Haidu (Bitbucket: ahaidu).

---

I did not test the functionality of joints either. However the ode user guide explains at point 12.1 and 12.4 how to use these:

12.1. How do I connect a body to the static environment with a joint?

• Use dJointAttach with arguments (body,0) or (0,body).

  12.4. How can an immovable body be created?

• [...] The geom is associated with a rigid body ID of zero. Then in the contact callback when you detect a collision between two geoms with a nonzero body ID and a zero body ID, you can simply pass those two IDs to the dJointAttach function as normal. This will create a contact between the rigid body and the static environment.

[osrf-migration]

Original comment by Nate Koenig (Bitbucket: Nathan Koenig).

---

Excellent work.

How about we do this in two parts:

1. Implement the whole model setstatic functionality via Entity::SetStatic.

2. Add the ability to set each link to static. This will require a change to SDF like Steve suggested.

[osrf-migration]

Original comment by Andrei Haidu (Bitbucket: ahaidu).

---

Some updates on this issue, until I will have time to look over it in more detail (this works for basic cases).

Here are the diffs.

Known issues with this fix:

• If the model is initially loaded as static and then it set to movable it does not collide with other models
• When switched to true / false very quickly I noticed a segfault at line 97 in contact.cpp
• If the model initally has joints, these are disabled afterwards

[osrf-migration]

Original comment by Fadri Furrer (Bitbucket: ffurrer).

---

Hi, is there any update on this?

[osrf-migration]

Original comment by Nate Koenig (Bitbucket: Nathan Koenig).

---

• set version to "all"

[osrf-migration]

Original comment by Louise Poubel (Bitbucket: chapulina, GitHub: chapulina).

---

Hi @ahaidu , I was thinking of taking a look at this to see if it could be implemented relatively soon. Do you have the original link to the diff you mention above by any chance? Bitbucket is currently just redirecting it to this page...

Thanks for the help!

[osrf-migration]

Original comment by Peter Mitrano (Bitbucket: peter_mitrano).

---

:thumbsup: would like to see this fixed if it isn’t already!