Skip to content

Commit 0cc7a0a

Browse files
ahcordeiche033j-rivero
committed
Model: Enable/disable: gravity and static (#919)
Signed-off-by: Alejandro Hernandez Cordero <ahcorde@gmail.com> Signed-off-by: Ian Chen <iche@intrinsic.ai> Co-authored-by: Ian Chen <iche@intrinsic.ai> Co-authored-by: Jose Luis Rivero <jrivero@honurobotics.com> (cherry picked from commit 0109a34) Signed-off-by: Ian Chen <iche@intrinsic.ai>
1 parent 09bdd86 commit 0cc7a0a

21 files changed

Lines changed: 1650 additions & 51 deletions

‎bullet-featherstone/src/Base.hh‎

Lines changed: 50 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -387,6 +387,56 @@ inline Eigen::Isometry3d GetWorldTransformOfLink(
387387
return convert(body.getBaseWorldTransform()) * model.baseInertiaToLinkFrame;
388388
}
389389

390+
/////////////////////////////////////////////////
391+
inline void makeColliderStatic(LinkInfo *_linkInfo)
392+
{
393+
btMultiBodyLinkCollider *childCollider = _linkInfo->collider.get();
394+
if (!childCollider)
395+
return;
396+
397+
// if link is already static or fixed, we do not need to change its
398+
// collision flags
399+
if (_linkInfo->isStaticOrFixed)
400+
return;
401+
402+
btBroadphaseProxy *childProxy = childCollider->getBroadphaseHandle();
403+
if (!childProxy)
404+
return;
405+
406+
childProxy->m_collisionFilterGroup = btBroadphaseProxy::StaticFilter;
407+
childProxy->m_collisionFilterMask =
408+
btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter;
409+
#if BT_BULLET_VERSION >= 307
410+
childCollider->setDynamicType(btCollisionObject::CF_STATIC_OBJECT);
411+
#endif
412+
}
413+
414+
/////////////////////////////////////////////////
415+
inline void makeColliderDynamic(LinkInfo *_linkInfo)
416+
{
417+
btMultiBodyLinkCollider *childCollider = _linkInfo->collider.get();
418+
if (!childCollider)
419+
return;
420+
421+
btBroadphaseProxy *childProxy = childCollider->getBroadphaseHandle();
422+
if (!childProxy)
423+
return;
424+
425+
// If broadphase and collision object flags do not agree, the
426+
// link was originally non-static but made static by AttachJoint
427+
if (!_linkInfo->isStaticOrFixed &&
428+
((childProxy->m_collisionFilterGroup &
429+
btBroadphaseProxy::StaticFilter) > 0))
430+
{
431+
childProxy->m_collisionFilterGroup =
432+
btBroadphaseProxy::DefaultFilter;
433+
childProxy->m_collisionFilterMask = btBroadphaseProxy::AllFilter;
434+
#if BT_BULLET_VERSION >= 307
435+
childCollider->setDynamicType(btCollisionObject::CF_DYNAMIC_OBJECT);
436+
#endif
437+
}
438+
}
439+
390440
class Base : public Implements3d<FeatureList<Feature>>
391441
{
392442
// Note: Entity ID 0 is reserved for the "engine"

‎bullet-featherstone/src/JointFeatures.cc‎

Lines changed: 0 additions & 50 deletions
Original file line numberDiff line numberDiff line change
@@ -53,56 +53,6 @@ void recreateJointLimitConstraint(JointInfo *_jointInfo, ModelInfo *_modelInfo,
5353
_worldInfo->world->addMultiBodyConstraint(_jointInfo->jointLimits.get());
5454
}
5555

56-
/////////////////////////////////////////////////
57-
void makeColliderStatic(LinkInfo *_linkInfo)
58-
{
59-
btMultiBodyLinkCollider *childCollider = _linkInfo->collider.get();
60-
if (!childCollider)
61-
return;
62-
63-
// if link is already static or fixed, we do not need to change its
64-
// collision flags
65-
if (_linkInfo->isStaticOrFixed)
66-
return;
67-
68-
btBroadphaseProxy *childProxy = childCollider->getBroadphaseHandle();
69-
if (!childProxy)
70-
return;
71-
72-
childProxy->m_collisionFilterGroup = btBroadphaseProxy::StaticFilter;
73-
childProxy->m_collisionFilterMask =
74-
btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter;
75-
#if BT_BULLET_VERSION >= 307
76-
childCollider->setDynamicType(btCollisionObject::CF_STATIC_OBJECT);
77-
#endif
78-
}
79-
80-
/////////////////////////////////////////////////
81-
void makeColliderDynamic(LinkInfo *_linkInfo)
82-
{
83-
btMultiBodyLinkCollider *childCollider = _linkInfo->collider.get();
84-
if (!childCollider)
85-
return;
86-
87-
btBroadphaseProxy *childProxy = childCollider->getBroadphaseHandle();
88-
if (!childProxy)
89-
return;
90-
91-
// If broadphase and collision object flags do not agree, the
92-
// link was originally non-static but made static by AttachJoint
93-
if (!_linkInfo->isStaticOrFixed &&
94-
((childProxy->m_collisionFilterGroup &
95-
btBroadphaseProxy::StaticFilter) > 0))
96-
{
97-
childProxy->m_collisionFilterGroup =
98-
btBroadphaseProxy::DefaultFilter;
99-
childProxy->m_collisionFilterMask = btBroadphaseProxy::AllFilter;
100-
#if BT_BULLET_VERSION >= 307
101-
childCollider->setDynamicType(btCollisionObject::CF_DYNAMIC_OBJECT);
102-
#endif
103-
}
104-
}
105-
10656
/////////////////////////////////////////////////
10757
void updateColliderFlagsRecursive(std::size_t _linkID,
10858
const std::unordered_map<std::size_t, std::shared_ptr<JointInfo>> &_joints,

‎bullet-featherstone/src/ModelFeatures.cc‎

Lines changed: 107 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,107 @@
1+
/*
2+
* Copyright (C) 2026 Open Source Robotics Foundation
3+
*
4+
* Licensed under the Apache License, Version 2.0 (the "License");
5+
* you may not use this file except in compliance with the License.
6+
* You may obtain a copy of the License at
7+
*
8+
* http://www.apache.org/licenses/LICENSE-2.0
9+
*
10+
* Unless required by applicable law or agreed to in writing, software
11+
* distributed under the License is distributed on an "AS IS" BASIS,
12+
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13+
* See the License for the specific language governing permissions and
14+
* limitations under the License.
15+
*
16+
*/
17+
18+
#include "ModelFeatures.hh"
19+
20+
namespace gz {
21+
namespace physics {
22+
namespace bullet_featherstone {
23+
24+
/////////////////////////////////////////////////
25+
void ModelFeatures::SetModelStatic(const Identity &_id, bool _static)
26+
{
27+
auto *modelInfo = this->ReferenceInterface<ModelInfo>(_id);
28+
if (!modelInfo)
29+
return;
30+
31+
if (!modelInfo->body)
32+
{
33+
gzerr << "Error setting model static state. Model is missing a btMultiBody."
34+
<< std::endl;
35+
}
36+
37+
// Zero out velocities and forces
38+
modelInfo->body->setBaseVel(btVector3(0, 0, 0));
39+
modelInfo->body->setBaseOmega(btVector3(0, 0, 0));
40+
modelInfo->body->clearForcesAndTorques();
41+
modelInfo->body->clearConstraintForces();
42+
modelInfo->body->clearVelocities();
43+
44+
for (const auto linkID : modelInfo->linkEntityIds)
45+
{
46+
auto linkIt = this->links.find(linkID);
47+
if (linkIt == this->links.end())
48+
continue;
49+
50+
auto *linkInfo = linkIt->second.get();
51+
if (_static)
52+
{
53+
// Make dynamic links static collision objects
54+
makeColliderStatic(linkInfo);
55+
// Explicitly promote this link to become permanent static
56+
linkInfo->isStaticOrFixed = true;
57+
}
58+
else
59+
{
60+
// Reset flag to allow restore back to dynamic status
61+
linkInfo->isStaticOrFixed = false;
62+
makeColliderDynamic(linkInfo);
63+
}
64+
}
65+
66+
if (!_static)
67+
{
68+
modelInfo->body->wakeUp();
69+
}
70+
71+
// Apply recursively to nested models
72+
for (const auto nestedID : modelInfo->nestedModelEntityIds)
73+
{
74+
auto nestedIt = this->models.find(nestedID);
75+
if (nestedIt != this->models.end())
76+
{
77+
this->SetModelStatic(
78+
this->GenerateIdentity(nestedID, nestedIt->second), _static);
79+
}
80+
}
81+
}
82+
83+
/////////////////////////////////////////////////
84+
bool ModelFeatures::GetModelStatic(const Identity &_id) const
85+
{
86+
const auto *modelInfo = this->ReferenceInterface<ModelInfo>(_id);
87+
if (!modelInfo)
88+
return false;
89+
90+
// Check if first link is static as typical representation of whole
91+
// model state
92+
if (!modelInfo->linkEntityIds.empty())
93+
{
94+
const auto linkID = modelInfo->linkEntityIds.front();
95+
auto linkIt = this->links.find(linkID);
96+
if (linkIt != this->links.end())
97+
{
98+
return linkIt->second->isStaticOrFixed;
99+
}
100+
}
101+
102+
return false;
103+
}
104+
105+
}
106+
}
107+
}

‎bullet-featherstone/src/ModelFeatures.hh‎

Lines changed: 49 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,49 @@
1+
/*
2+
* Copyright (C) 2026 Open Source Robotics Foundation
3+
*
4+
* Licensed under the Apache License, Version 2.0 (the "License");
5+
* you may not use this file except in compliance with the License.
6+
* You may obtain a copy of the License at
7+
*
8+
* http://www.apache.org/licenses/LICENSE-2.0
9+
*
10+
* Unless required by applicable law or agreed to in writing, software
11+
* distributed under the License is distributed on an "AS IS" BASIS,
12+
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13+
* See the License for the specific language governing permissions and
14+
* limitations under the License.
15+
*
16+
*/
17+
18+
#ifndef GZ_PHYSICS_BULLET_FEATHERSTONE_SRC_MODELFEATURES_HH_
19+
#define GZ_PHYSICS_BULLET_FEATHERSTONE_SRC_MODELFEATURES_HH_
20+
21+
#include <gz/physics/Model.hh>
22+
23+
#include "Base.hh"
24+
25+
namespace gz {
26+
namespace physics {
27+
namespace bullet_featherstone {
28+
29+
struct ModelFeatureList : FeatureList<
30+
ModelStaticState
31+
> { };
32+
33+
class ModelFeatures :
34+
public virtual Base,
35+
public virtual Implements3d<ModelFeatureList>
36+
{
37+
// Documentation inherited
38+
public: void SetModelStatic(
39+
const Identity &_id, bool _static) override;
40+
41+
// Documentation inherited
42+
public: bool GetModelStatic(const Identity &_id) const override;
43+
};
44+
45+
}
46+
}
47+
}
48+
49+
#endif

‎bullet-featherstone/src/plugin.cc‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -27,6 +27,7 @@
2727
#include "JointFeatures.hh"
2828
#include "KinematicsFeatures.hh"
2929
#include "LinkFeatures.hh"
30+
#include "ModelFeatures.hh"
3031
#include "SDFFeatures.hh"
3132
#include "SimulationFeatures.hh"
3233
#include "WorldFeatures.hh"
@@ -41,6 +42,7 @@ struct BulletFeatures : FeatureList <
4142
FreeGroupFeatureList,
4243
KinematicsFeatureList,
4344
LinkFeatureList,
45+
ModelFeatureList,
4446
SDFFeatureList,
4547
ShapeFeatureList,
4648
JointFeatureList,
@@ -55,6 +57,7 @@ class Plugin :
5557
public virtual FreeGroupFeatures,
5658
public virtual KinematicsFeatures,
5759
public virtual LinkFeatures,
60+
public virtual ModelFeatures,
5861
public virtual SDFFeatures,
5962
public virtual ShapeFeatures,
6063
public virtual JointFeatures,

‎dartsim/src/Base.hh‎

Lines changed: 34 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -519,6 +519,40 @@ class Base : public Implements3d<FeatureList<Feature>>
519519
return this->models.at(_modelID);
520520
}
521521

522+
/// \brief Helper function for setting gravity mode on a link
523+
public: static void SetLinkGravityMode(LinkInfo * _linkInfo, bool _enabled)
524+
{
525+
if (!_linkInfo || !_linkInfo->link)
526+
return;
527+
528+
// Added-mass links must keep DART gravity disabled: gravity is applied
529+
// manually as F=ma each step. Let SetLinkAddedMass own that flag.
530+
if (_linkInfo->inertial.has_value() &&
531+
_linkInfo->inertial->FluidAddedMass().has_value())
532+
{
533+
return;
534+
}
535+
536+
_linkInfo->link->setGravityMode(_enabled);
537+
}
538+
539+
/// \brief Helper function for getting gravity mode of a link
540+
public: static bool GetLinkGravityMode(const LinkInfo * _linkInfo)
541+
{
542+
if (!_linkInfo || !_linkInfo->link)
543+
return true;
544+
545+
// Added-mass links have DART gravity forcibly disabled; exclude them so
546+
// their internal state doesn't pollute the user-visible gravity flag.
547+
if (_linkInfo->inertial.has_value() &&
548+
_linkInfo->inertial->FluidAddedMass().has_value())
549+
{
550+
return true;
551+
}
552+
553+
return _linkInfo->link->getGravityMode();
554+
}
555+
522556
public: detail::EntityStorage<DartWorldPtr, std::string> worlds;
523557
public: detail::EntityStorage<ModelInfoPtr, DartConstSkeletonPtr> models;
524558
public: detail::EntityStorage<LinkInfoPtr, const DartBodyNode*> links;

‎dartsim/src/LinkFeatures.cc‎

Lines changed: 14 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -41,6 +41,20 @@ void LinkFeatures::AddLinkExternalTorqueInWorld(
4141
bn->addExtTorque(_torque, false);
4242
}
4343

44+
/////////////////////////////////////////////////
45+
void LinkFeatures::SetLinkGravityEnabled(const Identity &_id, bool _enabled)
46+
{
47+
auto linkInfo = this->ReferenceInterface<LinkInfo>(_id);
48+
this->SetLinkGravityMode(linkInfo, _enabled);
49+
}
50+
51+
/////////////////////////////////////////////////
52+
bool LinkFeatures::GetLinkGravityEnabled(const Identity &_id) const
53+
{
54+
auto linkInfo = this->ReferenceInterface<LinkInfo>(_id);
55+
return this->GetLinkGravityMode(linkInfo);
56+
}
57+
4458
}
4559
}
4660
}

‎dartsim/src/LinkFeatures.hh‎

Lines changed: 9 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -18,6 +18,7 @@
1818
#ifndef GZ_PHYSICS_DARTSIM_SRC_LINKFEATURES_HH_
1919
#define GZ_PHYSICS_DARTSIM_SRC_LINKFEATURES_HH_
2020

21+
#include <gz/physics/Gravity.hh>
2122
#include <gz/physics/Link.hh>
2223

2324
#include "Base.hh"
@@ -27,7 +28,8 @@ namespace physics {
2728
namespace dartsim {
2829

2930
struct LinkFeatureList : FeatureList<
30-
AddLinkExternalForceTorque
31+
AddLinkExternalForceTorque,
32+
GravityEnabled
3133
> { };
3234

3335
class LinkFeatures :
@@ -42,6 +44,12 @@ class LinkFeatures :
4244

4345
public: void AddLinkExternalTorqueInWorld(
4446
const Identity &_id, const AngularVectorType &_torque) override;
47+
48+
// ----- Link Gravity Enabled -----
49+
public: void SetLinkGravityEnabled(
50+
const Identity &_id, bool _enabled) override;
51+
52+
public: bool GetLinkGravityEnabled(const Identity &_id) const override;
4553
};
4654

4755
}

0 commit comments

Comments
 (0)