Skip to content

Commit

Permalink
Merge branch 'gz-physics7' into jointFeedback
Browse files Browse the repository at this point in the history
  • Loading branch information
@iche033
iche033 authored Jun 18, 2024
2 parents dabbbb8 + a1cb989 commit beaacc7
Show file tree
Hide file tree
Showing 22 changed files with 1,442 additions and 117 deletions.
2 changes: 1 addition & 1 deletion CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -78,7 +78,7 @@ gz_find_package(DART
utils
utils-urdf
CONFIG
VERSION 6.9
VERSION 6.10
REQUIRED_BY dartsim
PKGCONFIG dart
PKGCONFIG_VER_COMPARISON >=)
Expand Down
22 changes: 21 additions & 1 deletion bullet-featherstone/src/Base.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -112,6 +112,24 @@ struct ModelInfo
}
};

/// \brief Custom GzMultiBodyLinkCollider class
class GzMultiBodyLinkCollider: public btMultiBodyLinkCollider {
using btMultiBodyLinkCollider::btMultiBodyLinkCollider;

/// \brief Overrides base function to enable support for ignoring
/// collision with objects from other bodies if
/// btCollisionObject::setIgnoreCollisionCheck is called.
/// Note: originally btMultiBodyLinkCollider::checkCollideWithOverride
/// just returns true if the input collision object is from a
/// different body and disregards any setIgnoreCollisionCheck calls.
public: bool checkCollideWithOverride(const btCollisionObject *_co) const
override
{
return btMultiBodyLinkCollider::checkCollideWithOverride(_co) &&
btCollisionObject::checkCollideWithOverride(_co);
}
};

/// Link information is embedded inside the model, so all we need to store here
/// is a reference to the model and the index of this link inside of it.
struct LinkInfo
Expand All @@ -120,10 +138,12 @@ struct LinkInfo
std::optional<int> indexInModel;
Identity model;
Eigen::Isometry3d inertiaToLinkFrame;
std::unique_ptr<btMultiBodyLinkCollider> collider = nullptr;
std::unique_ptr<GzMultiBodyLinkCollider> collider = nullptr;
std::unique_ptr<btCompoundShape> shape = nullptr;
std::vector<std::size_t> collisionEntityIds = {};
std::unordered_map<std::string, std::size_t> collisionNameToEntityId = {};
// Link is either static, fixed to world, or has zero dofs
bool isStaticOrFixed = false;
};

struct CollisionInfo
Expand Down
88 changes: 88 additions & 0 deletions bullet-featherstone/src/FreeGroupFeatures.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,11 +18,67 @@
#include "FreeGroupFeatures.hh"

#include <memory>
#include <unordered_map>

namespace gz {
namespace physics {
namespace bullet_featherstone {

/////////////////////////////////////////////////
btTransform getWorldTransformForLink(btMultiBody *_body, int _linkIndex)
{
if (_linkIndex == -1)
{
return _body->getBaseWorldTransform();
}
else
{
btMultiBodyLinkCollider *collider = _body->getLinkCollider(_linkIndex);
return collider->getWorldTransform();
}
}

/////////////////////////////////////////////////
void enforceFixedConstraint(
btMultiBodyFixedConstraint *_fixedConstraint,
const std::unordered_map<std::size_t, Base::JointInfoPtr> &_allJoints)
{
// Update fixed constraint's child link pose to maintain a fixed transform
// from the parent link.
btMultiBody *parent = _fixedConstraint->getMultiBodyA();
btMultiBody *child = _fixedConstraint->getMultiBodyB();

btTransform parentToChildTf;
parentToChildTf.setOrigin(_fixedConstraint->getPivotInA());
parentToChildTf.setBasis(_fixedConstraint->getFrameInA());

int parentLinkIndex = _fixedConstraint->getLinkA();
int childLinkIndex = _fixedConstraint->getLinkB();

btTransform parentLinkTf = getWorldTransformForLink(parent, parentLinkIndex);
btTransform childLinkTf = getWorldTransformForLink(child, childLinkIndex);

btTransform expectedChildLinkTf = parentLinkTf * parentToChildTf;
btTransform childBaseTf = child->getBaseWorldTransform();
btTransform childBaseToLink =
childBaseTf.inverse() * childLinkTf;
btTransform newChildBaseTf =
expectedChildLinkTf * childBaseToLink.inverse();
child->setBaseWorldTransform(newChildBaseTf);
for (const auto &joint : _allJoints)
{
if (joint.second->fixedConstraint)
{
// Recursively enforce constraints where the child here is a parent to
// other constraints.
if (joint.second->fixedConstraint->getMultiBodyA() == child)
{
enforceFixedConstraint(joint.second->fixedConstraint.get(), _allJoints);
}
}
}
}

/////////////////////////////////////////////////
Identity FreeGroupFeatures::FindFreeGroupForModel(
const Identity &_modelID) const
Expand All @@ -33,6 +89,20 @@ Identity FreeGroupFeatures::FindFreeGroupForModel(
if (model->body->hasFixedBase())
return this->GenerateInvalidId();

// Also reject if the model is a child of a fixed constraint
// (detachable joint)
for (const auto & joint : this->joints)
{
if (joint.second->fixedConstraint)
{
if (joint.second->fixedConstraint->getMultiBodyB() == model->body.get())
{
return this->GenerateInvalidId();
}
}
}


return _modelID;
}

Expand Down Expand Up @@ -97,7 +167,25 @@ void FreeGroupFeatures::SetFreeGroupWorldPose(
{
model->body->setBaseWorldTransform(
convertTf(_pose * model->baseInertiaToLinkFrame.inverse()));

model->body->wakeUp();

for (auto & joint : this->joints)
{
if (joint.second->fixedConstraint)
{
// Only the parent body of a fixed joint can be moved but we need to
// enforce the fixed constraint by updating the child pose so it
// remains at a fixed transform from parent. We do this recursively to
// account for other constraints attached to the child.
btMultiBody *parent = joint.second->fixedConstraint->getMultiBodyA();
if (parent == model->body.get())
{
enforceFixedConstraint(joint.second->fixedConstraint.get(),
this->joints);
}
}
}
}
}

Expand Down
155 changes: 150 additions & 5 deletions bullet-featherstone/src/JointFeatures.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,7 @@

#include <algorithm>
#include <memory>
#include <unordered_map>

#include <gz/common/Console.hh>
#include <sdf/Joint.hh>
Expand All @@ -27,6 +28,85 @@ namespace gz {
namespace physics {
namespace bullet_featherstone {

/////////////////////////////////////////////////
void makeColliderStatic(LinkInfo *_linkInfo)
{
btMultiBodyLinkCollider *childCollider = _linkInfo->collider.get();
if (!childCollider)
return;

// if link is already static or fixed, we do not need to change its
// collision flags
if (_linkInfo->isStaticOrFixed)
return;

btBroadphaseProxy *childProxy = childCollider->getBroadphaseHandle();
if (!childProxy)
return;

childProxy->m_collisionFilterGroup = btBroadphaseProxy::StaticFilter;
childProxy->m_collisionFilterMask =
btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter;
#if BT_BULLET_VERSION >= 307
childCollider->setDynamicType(btCollisionObject::CF_STATIC_OBJECT);
#endif
}

/////////////////////////////////////////////////
void makeColliderDynamic(LinkInfo *_linkInfo)
{
btMultiBodyLinkCollider *childCollider = _linkInfo->collider.get();
if (!childCollider)
return;

btBroadphaseProxy *childProxy = childCollider->getBroadphaseHandle();
if (!childProxy)
return;

// If broadphase and collision object flags do not agree, the
// link was originally non-static but made static by AttachJoint
if (!_linkInfo->isStaticOrFixed &&
((childProxy->m_collisionFilterGroup &
btBroadphaseProxy::StaticFilter) > 0))
{
childProxy->m_collisionFilterGroup =
btBroadphaseProxy::DefaultFilter;
childProxy->m_collisionFilterMask = btBroadphaseProxy::AllFilter;
#if BT_BULLET_VERSION >= 307
childCollider->setDynamicType(btCollisionObject::CF_DYNAMIC_OBJECT);
#endif
}
}

/////////////////////////////////////////////////
void updateColliderFlagsRecursive(std::size_t _linkID,
const std::unordered_map<std::size_t, std::shared_ptr<JointInfo>> &_joints,
const std::unordered_map<std::size_t, std::shared_ptr<LinkInfo>> &_links,
std::function<void(LinkInfo *)> _updateColliderCb)
{
btMultiBodyFixedConstraint *fixedConstraint = nullptr;
std::size_t childLinkID = 0u;
for (const auto &joint : _joints)
{
if (!joint.second->fixedConstraint)
continue;
if (!joint.second->parentLinkID.has_value() ||
joint.second->parentLinkID.value() != _linkID)
continue;

fixedConstraint = joint.second->fixedConstraint.get();
childLinkID = std::size_t(joint.second->childLinkID);
}

if (!fixedConstraint)
return;

auto childInfo = _links.at(childLinkID);
_updateColliderCb(childInfo.get());

updateColliderFlagsRecursive(childLinkID, _joints, _links, _updateColliderCb);
}

/////////////////////////////////////////////////
double JointFeatures::GetJointPosition(
const Identity &_id, const std::size_t _dof) const
Expand Down Expand Up @@ -382,6 +462,31 @@ Identity JointFeatures::AttachFixedJoint(
if (world != nullptr && world->world)
{
world->world->addMultiBodyConstraint(jointInfo->fixedConstraint.get());
jointInfo->fixedConstraint->setMaxAppliedImpulse(btScalar(1e9));

// Make child link static if parent is static
// This is done by updating collision flags
btMultiBodyLinkCollider *parentCollider = parentLinkInfo->collider.get();
btMultiBodyLinkCollider *childCollider = linkInfo->collider.get();
if (parentCollider && childCollider)
{
// disable collision between parent and child collider
// \todo(iche033) if self collide is true, extend this to
// disable collisions between all the links in the parent's model with
// all the links in the child's model.
parentCollider->setIgnoreCollisionCheck(childCollider, true);
childCollider->setIgnoreCollisionCheck(parentCollider, true);

// If parent link is static or fixed, recusively update child colliders
// collision flags to be static.
if (parentLinkInfo->isStaticOrFixed && !linkInfo->isStaticOrFixed)
{
makeColliderStatic(linkInfo);
updateColliderFlagsRecursive(std::size_t(_childID),
this->joints, this->links, makeColliderStatic);
}
}

return this->GenerateIdentity(jointID, this->joints.at(jointID));
}

Expand All @@ -394,13 +499,45 @@ void JointFeatures::DetachJoint(const Identity &_jointId)
auto jointInfo = this->ReferenceInterface<JointInfo>(_jointId);
if (jointInfo->fixedConstraint)
{
// Make links dynamic again as they were originally not static
// This is done by reverting any collision flag changes
// made in AttachJoint
auto *linkInfo = this->ReferenceInterface<LinkInfo>(jointInfo->childLinkID);
if (jointInfo->parentLinkID.has_value())
{
auto parentLinkInfo = this->links.at(jointInfo->parentLinkID.value());

btMultiBodyLinkCollider *parentCollider = parentLinkInfo->collider.get();
btMultiBodyLinkCollider *childCollider = linkInfo->collider.get();
if (parentCollider && childCollider)
{
parentCollider->setIgnoreCollisionCheck(childCollider, false);
childCollider->setIgnoreCollisionCheck(parentCollider, false);
btBroadphaseProxy *childProxy = childCollider->getBroadphaseHandle();
if (childProxy)
{
// Recursively make child colliders dynamic if they were originally
// not static
if (!linkInfo->isStaticOrFixed &&
((childProxy->m_collisionFilterGroup &
btBroadphaseProxy::StaticFilter) > 0))
{
makeColliderDynamic(linkInfo);
updateColliderFlagsRecursive(std::size_t(jointInfo->childLinkID),
this->joints, this->links, makeColliderDynamic);
}
}
}
}

auto modelInfo = this->ReferenceInterface<ModelInfo>(jointInfo->model);
if (modelInfo)
{
auto *world = this->ReferenceInterface<WorldInfo>(modelInfo->world);
world->world->removeMultiBodyConstraint(jointInfo->fixedConstraint.get());
jointInfo->fixedConstraint.reset();
jointInfo->fixedConstraint = nullptr;
modelInfo->body->wakeUp();
}
}
}
Expand All @@ -413,11 +550,19 @@ void JointFeatures::SetJointTransformFromParent(

if (jointInfo->fixedConstraint)
{
auto tf = convertTf(_pose);
jointInfo->fixedConstraint->setPivotInA(
tf.getOrigin());
jointInfo->fixedConstraint->setFrameInA(
tf.getBasis());
Eigen::Isometry3d parentInertiaToLinkFrame = Eigen::Isometry3d::Identity();
if (jointInfo->parentLinkID.has_value())
{
auto parentLinkInfo = this->links.at(jointInfo->parentLinkID.value());
parentInertiaToLinkFrame = parentLinkInfo->inertiaToLinkFrame;
}
auto *linkInfo = this->ReferenceInterface<LinkInfo>(jointInfo->childLinkID);
auto tf = convertTf(parentInertiaToLinkFrame * _pose *
linkInfo->inertiaToLinkFrame.inverse());
jointInfo->fixedConstraint->setPivotInA(
tf.getOrigin());
jointInfo->fixedConstraint->setFrameInA(
tf.getBasis());
}
}

Expand Down
Loading

0 comments on commit beaacc7

Please sign in to comment.