Skip to content

Commit

Permalink
Try creating a loop joint
Browse files Browse the repository at this point in the history 
This PR supercedes #345 with a more general solution that would also help address #25 and solve a lot of issues related to casting joints. Open to feedback..

# TODO
- [ ] clean up all the excess joints and links  created during the process of loop joint creation.
- [ ] fix SetJointTransformChild to use only the joint.

Signed-off-by: Arjo Chakravarty <[email protected]>
  • Loading branch information
@arjo129
arjo129 committed May 17, 2022
1 parent 3325673 commit 19fe472
Show file tree
Hide file tree
Showing 3 changed files with 65 additions and 60 deletions.
23 changes: 6 additions & 17 deletions dartsim/src/Base.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -74,6 +74,7 @@ struct JointInfo
{
dart::dynamics::JointPtr joint;
dart::dynamics::SimpleFramePtr frame;
dart::dynamics::BodyNode *phantomBody{nullptr};

enum JointType
{
Expand Down Expand Up @@ -364,7 +365,6 @@ class Base : public Implements3d<FeatureList<Feature>>
this->joints.idToObject[id] = std::make_shared<JointInfo>();
this->joints.idToObject[id]->joint = _joint;
this->joints.objectToID[_joint] = id;
this->joints.idToObject[id]->type = JointInfo::JointType::JOINT;
dart::dynamics::SimpleFramePtr jointFrame =
dart::dynamics::SimpleFrame::createShared(
_joint->getChildBodyNode(), _joint->getName() + "_frame",
Expand All @@ -376,23 +376,12 @@ class Base : public Implements3d<FeatureList<Feature>>
return id;
}

public: inline std::size_t AddJointConstraint(DartWeldJointConsPtr _joint)
public: inline void AddJointWeldConstraint(const std::size_t _id,
DartWeldJointConsPtr _joint, dart::dynamics::BodyNode* _phantomBody)
{
const std::size_t id = this->GetNextEntity();
this->joints.idToObject[id] = std::make_shared<JointInfo>();
this->joints.idToObject[id]->constraint = _joint;
this->joints.idToObject[id]->type = JointInfo::JointType::CONSTRAINT;
// TODO(arjo): Refactor the joints.objectToId to support constraints.
// this->joints.objectToID[_joint] = id;
// dart::dynamics::SimpleFramePtr jointFrame =
// dart::dynamics::SimpleFrame::createShared(
// _joint->getChildBodyNode(), _joint->getName() + "_frame",
// _joint->getTransformFromChildBodyNode());

// this->joints.idToObject[id]->frame = jointFrame;
// this->frames[id] = this->joints.idToObject[id]->frame.get();

return id;
this->joints.idToObject[_id]->constraint = _joint;
this->joints.idToObject[_id]->type = JointInfo::JointType::CONSTRAINT;
this->joints.idToObject[_id]->phantomBody = _phantomBody;
}

public: inline std::size_t AddShape(
Expand Down
98 changes: 57 additions & 41 deletions dartsim/src/JointFeatures.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -331,29 +331,21 @@ void JointFeatures::SetJointMaxEffort(
/////////////////////////////////////////////////
std::size_t JointFeatures::GetJointDegreesOfFreedom(const Identity &_id) const
{
auto jointInfo = this->ReferenceInterface<JointInfo>(_id);
if (jointInfo->type == JointInfo::JointType::CONSTRAINT)
return 0;
return jointInfo->joint->getNumDofs();
return this->ReferenceInterface<JointInfo>(_id)->joint->getNumDofs();
}

/////////////////////////////////////////////////
Pose3d JointFeatures::GetJointTransformFromParent(const Identity &_id) const
{
auto jointInfo = this->ReferenceInterface<JointInfo>(_id);
if (jointInfo->type == JointInfo::JointType::CONSTRAINT)
return jointInfo->constraint->getRelativeTransform().inverse();
return jointInfo->joint->getTransformFromParentBodyNode();
return this->ReferenceInterface<JointInfo>(_id)
->joint->getTransformFromParentBodyNode();
}

/////////////////////////////////////////////////
Pose3d JointFeatures::GetJointTransformToChild(const Identity &_id) const
{
auto jointInfo = this->ReferenceInterface<JointInfo>(_id);
if (jointInfo->type == JointInfo::JointType::CONSTRAINT)
return jointInfo->constraint->getRelativeTransform();
return this->ReferenceInterface<JointInfo>(_id)
->joint->getTransformFromChildBodyNode().inverse();
->joint->getTransformFromChildBodyNode().inverse();
}

/////////////////////////////////////////////////
Expand Down Expand Up @@ -467,11 +459,6 @@ Identity JointFeatures::CastToFixedJoint(
const Identity &_jointID) const
{
auto jointInfo = this->ReferenceInterface<JointInfo>(_jointID);
if (jointInfo->type == JointInfo::JointType::CONSTRAINT)
{
// TODO(arjo): Handle constraint casts.
return this->GenerateInvalidId();
}
dart::dynamics::WeldJoint *const weld =
dynamic_cast<dart::dynamics::WeldJoint *>(
jointInfo->joint.get());
Expand Down Expand Up @@ -499,15 +486,25 @@ Identity JointFeatures::AttachFixedJoint(
if (bn->getParentJoint()->getType() != "FreeJoint")
{
// child already has a parent joint
// use a WeldJointConstraint between the two bodies
// Create a phantom link and a joint to attach the child to the phantom
const auto &[joint, phantomBody] =
bn->getSkeleton()->createJointAndBodyNodePair<
dart::dynamics::WeldJoint,
dart::dynamics::BodyNode>(
bn, properties, dart::dynamics::BodyNode::AspectProperties());
const std::size_t jointID = this->AddJoint(joint);
phantomBody->setCollidable(false);

// use a WeldJointConstraint between the phantom joint and the link
auto worldId = this->GetWorldOfModelImpl(
this->models.IdentityOf(bn->getSkeleton()));
auto dartWorld = this->worlds.at(worldId);

auto constraint =
std::make_shared<dart::constraint::WeldJointConstraint>(parentBn, bn);
std::make_shared<dart::constraint::WeldJointConstraint>(
parentBn, phantomBody);
dartWorld->getConstraintSolver()->addConstraint(constraint);
auto jointID = this->AddJointConstraint(constraint);
this->AddJointWeldConstraint(jointID, constraint, phantomBody);
return this->GenerateIdentity(jointID, this->joints.at(jointID));
}
{
Expand All @@ -530,11 +527,6 @@ Identity JointFeatures::CastToFreeJoint(
const Identity &_jointID) const
{
auto jointInfo = this->ReferenceInterface<JointInfo>(_jointID);
if (jointInfo->type == JointInfo::JointType::CONSTRAINT)
{
// TODO(arjo): Handle constraint casts.
return this->GenerateInvalidId();
}
auto *const freeJoint =
dynamic_cast<dart::dynamics::FreeJoint *>(
jointInfo->joint.get());
Expand All @@ -559,11 +551,6 @@ Identity JointFeatures::CastToRevoluteJoint(
const Identity &_jointID) const
{
auto jointInfo = this->ReferenceInterface<JointInfo>(_jointID);
if (jointInfo->type == JointInfo::JointType::CONSTRAINT)
{
// TODO(arjo): Handle constraint casts.
return this->GenerateInvalidId();
}
dart::dynamics::RevoluteJoint *const revolute =
dynamic_cast<dart::dynamics::RevoluteJoint *>(
jointInfo->joint.get());
Expand Down Expand Up @@ -609,9 +596,26 @@ Identity JointFeatures::AttachRevoluteJoint(

if (bn->getParentJoint()->getType() != "FreeJoint")
{
// child already has a parent joint
// TODO(scpeters): use a WeldJointConstraint between the two bodies
return this->GenerateInvalidId();
// Create a phantom link and a joint to attach the child to the phantom
const auto &[joint, phantomBody] =
bn->getSkeleton()->createJointAndBodyNodePair<
dart::dynamics::RevoluteJoint,
dart::dynamics::BodyNode>(
bn, properties, dart::dynamics::BodyNode::AspectProperties());
const std::size_t jointID = this->AddJoint(joint);
phantomBody->setCollidable(false);

// use a WeldJointConstraint between the phantom joint and the link
auto worldId = this->GetWorldOfModelImpl(
this->models.IdentityOf(bn->getSkeleton()));
auto dartWorld = this->worlds.at(worldId);

auto constraint =
std::make_shared<dart::constraint::WeldJointConstraint>(
parentBn, phantomBody);
dartWorld->getConstraintSolver()->addConstraint(constraint);
this->AddJointWeldConstraint(jointID, constraint, phantomBody);
return this->GenerateIdentity(jointID, this->joints.at(jointID));
}

{
Expand All @@ -634,11 +638,6 @@ Identity JointFeatures::CastToPrismaticJoint(
const Identity &_jointID) const
{
auto jointInfo = this->ReferenceInterface<JointInfo>(_jointID);
if (jointInfo->type == JointInfo::JointType::CONSTRAINT)
{
// TODO(arjo): Handle constraint casts.
return this->GenerateInvalidId();
}
dart::dynamics::PrismaticJoint *prismatic =
dynamic_cast<dart::dynamics::PrismaticJoint*>(jointInfo->joint.get());

Expand Down Expand Up @@ -683,9 +682,26 @@ Identity JointFeatures::AttachPrismaticJoint(

if (bn->getParentJoint()->getType() != "FreeJoint")
{
// child already has a parent joint
// TODO(scpeters): use a WeldJointConstraint between the two bodies
return this->GenerateInvalidId();
// Create a phantom link and a joint to attach the child to the phantom
const auto &[joint, phantomBody] =
bn->getSkeleton()->createJointAndBodyNodePair<
dart::dynamics::PrismaticJoint,
dart::dynamics::BodyNode>(
bn, properties, dart::dynamics::BodyNode::AspectProperties());
const std::size_t jointID = this->AddJoint(joint);
phantomBody->setCollidable(false);

// use a WeldJointConstraint between the phantom joint and the link
auto worldId = this->GetWorldOfModelImpl(
this->models.IdentityOf(bn->getSkeleton()));
auto dartWorld = this->worlds.at(worldId);

auto constraint =
std::make_shared<dart::constraint::WeldJointConstraint>(
parentBn, phantomBody);
dartWorld->getConstraintSolver()->addConstraint(constraint);
this->AddJointWeldConstraint(jointID, constraint, phantomBody);
return this->GenerateIdentity(jointID, this->joints.at(jointID));
}

{
Expand Down
4 changes: 2 additions & 2 deletions dartsim/src/JointFeatures_TEST.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -978,6 +978,7 @@ TEST_F(JointFeaturesFixture, JointAttachMultiple)
EXPECT_EQ(initialModel3Pose,
math::eigen3::convert(dartBody3->getWorldTransform()));


const std::size_t numSteps = 100;
for (std::size_t i = 0; i < numSteps; ++i)
{
Expand All @@ -995,12 +996,11 @@ TEST_F(JointFeaturesFixture, JointAttachMultiple)
EXPECT_NEAR(0.0, body2LinearVelocity.Z(), 1e-7);
EXPECT_NEAR(0.0, body3LinearVelocity.Z(), 1e-7);
}

// Detach the joints. M1 and M3 should fall as there is now nothing stopping
// them from falling.
fixedJoint1->Detach();
fixedJoint2->Detach();

for (std::size_t i = 0; i < numSteps; ++i)
{
world->Step(output, state, input);
Expand Down

0 comments on commit 19fe472

Please sign in to comment.