diff --git a/dartsim/src/Base.hh b/dartsim/src/Base.hh index 4d9248f91..767f40e40 100644 --- a/dartsim/src/Base.hh +++ b/dartsim/src/Base.hh @@ -21,6 +21,7 @@ #include #include #include +#include #include #include @@ -73,6 +74,15 @@ struct JointInfo { dart::dynamics::JointPtr joint; dart::dynamics::SimpleFramePtr frame; + + enum JointType + { + JOINT, + CONSTRAINT + }; + + JointType type{JOINT}; + dart::constraint::WeldJointConstraintPtr constraint; }; struct ShapeInfo @@ -221,6 +231,7 @@ class Base : public Implements3d> public: using DartBodyNodePtr = dart::dynamics::BodyNodePtr; public: using DartJoint = dart::dynamics::Joint; public: using DartJointPtr = dart::dynamics::JointPtr; + public: using DartWeldJointConsPtr = dart::constraint::WeldJointConstraintPtr; public: using DartShapeNode = dart::dynamics::ShapeNode; public: using DartShapeNodePtr = std::shared_ptr; public: using ModelInfoPtr = std::shared_ptr; @@ -353,6 +364,7 @@ class Base : public Implements3d> this->joints.idToObject[id] = std::make_shared(); 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", @@ -364,6 +376,25 @@ class Base : public Implements3d> return id; } + public: inline std::size_t AddJointConstraint(DartWeldJointConsPtr _joint) + { + const std::size_t id = this->GetNextEntity(); + this->joints.idToObject[id] = std::make_shared(); + 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; + } + public: inline std::size_t AddShape( const ShapeInfo &_info) { diff --git a/dartsim/src/JointFeatures.cc b/dartsim/src/JointFeatures.cc index 8f5ec221d..67af17de8 100644 --- a/dartsim/src/JointFeatures.cc +++ b/dartsim/src/JointFeatures.cc @@ -22,6 +22,10 @@ #include #include +#include + +#include + #include "JointFeatures.hh" namespace ignition { @@ -327,19 +331,27 @@ void JointFeatures::SetJointMaxEffort( ///////////////////////////////////////////////// std::size_t JointFeatures::GetJointDegreesOfFreedom(const Identity &_id) const { - return this->ReferenceInterface(_id)->joint->getNumDofs(); + auto jointInfo = this->ReferenceInterface(_id); + if (jointInfo->type == JointInfo::JointType::CONSTRAINT) + return 0; + return jointInfo->joint->getNumDofs(); } ///////////////////////////////////////////////// Pose3d JointFeatures::GetJointTransformFromParent(const Identity &_id) const { - return this->ReferenceInterface(_id) - ->joint->getTransformFromParentBodyNode(); + auto jointInfo = this->ReferenceInterface(_id); + if (jointInfo->type == JointInfo::JointType::CONSTRAINT) + return jointInfo->constraint->getRelativeTransform().inverse(); + return jointInfo->joint->getTransformFromParentBodyNode(); } ///////////////////////////////////////////////// Pose3d JointFeatures::GetJointTransformToChild(const Identity &_id) const { + auto jointInfo = this->ReferenceInterface(_id); + if (jointInfo->type == JointInfo::JointType::CONSTRAINT) + return jointInfo->constraint->getRelativeTransform(); return this->ReferenceInterface(_id) ->joint->getTransformFromChildBodyNode().inverse(); } @@ -348,22 +360,38 @@ Pose3d JointFeatures::GetJointTransformToChild(const Identity &_id) const void JointFeatures::SetJointTransformFromParent( const Identity &_id, const Pose3d &_pose) { - this->ReferenceInterface(_id) - ->joint->setTransformFromParentBodyNode(_pose); + auto jointInfo = this->ReferenceInterface(_id); + if (jointInfo->type == JointInfo::JointType::CONSTRAINT) + jointInfo->constraint->setRelativeTransform(_pose.inverse()); + else + jointInfo->joint->setTransformFromParentBodyNode(_pose); } ///////////////////////////////////////////////// void JointFeatures::SetJointTransformToChild( const Identity &_id, const Pose3d &_pose) { - this->ReferenceInterface(_id) - ->joint->setTransformFromChildBodyNode(_pose.inverse()); + auto jointInfo = this->ReferenceInterface(_id); + if (jointInfo->type == JointInfo::JointType::CONSTRAINT) + jointInfo->constraint->setRelativeTransform(_pose); + else + jointInfo->joint->setTransformFromChildBodyNode(_pose.inverse()); } ///////////////////////////////////////////////// void JointFeatures::DetachJoint(const Identity &_jointId) { - auto joint = this->ReferenceInterface(_jointId)->joint; + auto jointInfo = this->ReferenceInterface(_jointId); + if (jointInfo->type == JointInfo::JointType::CONSTRAINT) + { + auto constraint = jointInfo->constraint; + auto worldId = this->GetWorldOfModelImpl( + this->models.IdentityOf(constraint->getBodyNode1()->getSkeleton())); + auto dartWorld = this->worlds.at(worldId); + dartWorld->getConstraintSolver()->removeConstraint(constraint); + return; + } + auto &joint = jointInfo->joint; if (joint->getType() == "FreeJoint") { // don't need to do anything, joint is already a FreeJoint @@ -438,9 +466,15 @@ void JointFeatures::DetachJoint(const Identity &_jointId) Identity JointFeatures::CastToFixedJoint( const Identity &_jointID) const { + auto jointInfo = this->ReferenceInterface(_jointID); + if (jointInfo->type == JointInfo::JointType::CONSTRAINT) + { + // TODO(arjo): Handle constraint casts. + return this->GenerateInvalidId(); + } dart::dynamics::WeldJoint *const weld = dynamic_cast( - this->ReferenceInterface(_jointID)->joint.get()); + jointInfo->joint.get()); if (weld) return this->GenerateIdentity(_jointID, this->Reference(_jointID)); @@ -465,10 +499,17 @@ Identity JointFeatures::AttachFixedJoint( if (bn->getParentJoint()->getType() != "FreeJoint") { // child already has a parent joint - // TODO(scpeters): use a WeldJointConstraint between the two bodies - return this->GenerateInvalidId(); + // use a WeldJointConstraint between the two bodies + auto worldId = this->GetWorldOfModelImpl( + this->models.IdentityOf(bn->getSkeleton())); + auto dartWorld = this->worlds.at(worldId); + + auto constraint = + std::make_shared(parentBn, bn); + dartWorld->getConstraintSolver()->addConstraint(constraint); + auto jointID = this->AddJointConstraint(constraint); + return this->GenerateIdentity(jointID, this->joints.at(jointID)); } - { auto skeleton = bn->getSkeleton(); if (skeleton) @@ -488,9 +529,15 @@ Identity JointFeatures::AttachFixedJoint( Identity JointFeatures::CastToFreeJoint( const Identity &_jointID) const { + auto jointInfo = this->ReferenceInterface(_jointID); + if (jointInfo->type == JointInfo::JointType::CONSTRAINT) + { + // TODO(arjo): Handle constraint casts. + return this->GenerateInvalidId(); + } auto *const freeJoint = dynamic_cast( - this->ReferenceInterface(_jointID)->joint.get()); + jointInfo->joint.get()); if (freeJoint) return this->GenerateIdentity(_jointID, this->Reference(_jointID)); @@ -511,9 +558,15 @@ void JointFeatures::SetFreeJointRelativeTransform( Identity JointFeatures::CastToRevoluteJoint( const Identity &_jointID) const { + auto jointInfo = this->ReferenceInterface(_jointID); + if (jointInfo->type == JointInfo::JointType::CONSTRAINT) + { + // TODO(arjo): Handle constraint casts. + return this->GenerateInvalidId(); + } dart::dynamics::RevoluteJoint *const revolute = dynamic_cast( - this->ReferenceInterface(_jointID)->joint.get()); + jointInfo->joint.get()); if (revolute) return this->GenerateIdentity(_jointID, this->Reference(_jointID)); @@ -580,9 +633,14 @@ Identity JointFeatures::AttachRevoluteJoint( Identity JointFeatures::CastToPrismaticJoint( const Identity &_jointID) const { + auto jointInfo = this->ReferenceInterface(_jointID); + if (jointInfo->type == JointInfo::JointType::CONSTRAINT) + { + // TODO(arjo): Handle constraint casts. + return this->GenerateInvalidId(); + } dart::dynamics::PrismaticJoint *prismatic = - dynamic_cast( - this->ReferenceInterface(_jointID)->joint.get()); + dynamic_cast(jointInfo->joint.get()); if (prismatic) return this->GenerateIdentity(_jointID, this->Reference(_jointID)); diff --git a/dartsim/src/JointFeatures_TEST.cc b/dartsim/src/JointFeatures_TEST.cc index 4894b4ef9..38ae5952c 100644 --- a/dartsim/src/JointFeatures_TEST.cc +++ b/dartsim/src/JointFeatures_TEST.cc @@ -883,6 +883,142 @@ TEST_F(JointFeaturesFixture, JointAttachDetach) EXPECT_NEAR(0.0, dartBody2->getLinearVelocity().z(), 1e-3); } +///////////////////////////////////////////////// +// Essentially what happens is there are two floating boxes and a box in the +// middle that's resting. We start the system out by creating the two +// fixed joints between the boxes resting on the big box. The middle box will +// now have two parents. However there should be no movement as the middle box +// will be holding the other two boxes that are floating in mid air. We run +// this for 100 steps to make sure that there is no movement. This is because +// the middle box is holding on to the two side boxes. Then we release the +// joints the two boxes should fall away. +TEST_F(JointFeaturesFixture, JointAttachMultiple) +{ + sdf::Root root; + const sdf::Errors errors = + root.Load(TEST_WORLD_DIR "joint_constraint.sdf"); + ASSERT_TRUE(errors.empty()) << errors.front(); + + auto world = this->engine->ConstructWorld(*root.WorldByIndex(0)); + dart::simulation::WorldPtr dartWorld = world->GetDartsimWorld(); + ASSERT_NE(nullptr, dartWorld); + + // M1 and M3 are floating boxes + const std::string modelName1{"M1"}; + const std::string modelName2{"M2"}; + const std::string modelName3{"M3"}; + const std::string bodyName{"link"}; + + auto model1 = world->GetModel(modelName1); + auto model2 = world->GetModel(modelName2); + auto model3 = world->GetModel(modelName3); + + auto model1Body = model1->GetLink(bodyName); + auto model2Body = model2->GetLink(bodyName); + auto model3Body = model3->GetLink(bodyName); + + const dart::dynamics::SkeletonPtr skeleton1 = + dartWorld->getSkeleton(modelName1); + const dart::dynamics::SkeletonPtr skeleton2 = + dartWorld->getSkeleton(modelName2); + const dart::dynamics::SkeletonPtr skeleton3 = + dartWorld->getSkeleton(modelName3); + ASSERT_NE(nullptr, skeleton1); + ASSERT_NE(nullptr, skeleton2); + ASSERT_NE(nullptr, skeleton3); + + auto *dartBody1 = skeleton1->getBodyNode(bodyName); + auto *dartBody2 = skeleton2->getBodyNode(bodyName); + auto *dartBody3 = skeleton3->getBodyNode(bodyName); + + ASSERT_NE(nullptr, dartBody1); + ASSERT_NE(nullptr, dartBody2); + ASSERT_NE(nullptr, dartBody3); + + const math::Pose3d initialModel1Pose(0, -0.2, 0.45, 0, 0, 0); + const math::Pose3d initialModel2Pose(0, 0.2, 0.45, 0, 0, 0); + const math::Pose3d initialModel3Pose(0, 0.6, 0.45, 0, 0, 0); + + EXPECT_EQ(initialModel1Pose, + math::eigen3::convert(dartBody1->getWorldTransform())); + EXPECT_EQ(initialModel2Pose, + math::eigen3::convert(dartBody2->getWorldTransform())); + EXPECT_EQ(initialModel3Pose, + math::eigen3::convert(dartBody3->getWorldTransform())); + + physics::ForwardStep::Output output; + physics::ForwardStep::State state; + physics::ForwardStep::Input input; + + // Create the first joint. This should be a normal fixed joint. + const auto poseParent1 = dartBody1->getTransform(); + const auto poseChild1 = dartBody2->getTransform(); + auto poseParentChild1 = poseParent1.inverse() * poseChild1; + auto fixedJoint1 = model2Body->AttachFixedJoint(model1Body); + fixedJoint1->SetTransformFromParent(poseParentChild1); + + EXPECT_EQ(initialModel1Pose, + math::eigen3::convert(dartBody1->getWorldTransform())); + EXPECT_EQ(initialModel2Pose, + math::eigen3::convert(dartBody2->getWorldTransform())); + EXPECT_EQ(initialModel3Pose, + math::eigen3::convert(dartBody3->getWorldTransform())); + + // Create the second joint. This should be a WeldJoint constraint + const auto poseParent2 = dartBody3->getTransform(); + const auto poseChild2 = dartBody2->getTransform(); + auto poseParentChild2 = poseParent2.inverse() * poseChild2; + auto fixedJoint2 = model2Body->AttachFixedJoint(model3Body); + fixedJoint2->SetTransformFromParent(poseParentChild2); + + EXPECT_EQ(initialModel1Pose, + math::eigen3::convert(dartBody1->getWorldTransform())); + EXPECT_EQ(initialModel2Pose, + math::eigen3::convert(dartBody2->getWorldTransform())); + EXPECT_EQ(initialModel3Pose, + math::eigen3::convert(dartBody3->getWorldTransform())); + + const std::size_t numSteps = 100; + for (std::size_t i = 0; i < numSteps; ++i) + { + world->Step(output, state, input); + + // Expect all the bodies to be at rest. + // (since they're held in place by the joints) + math::Vector3d body1LinearVelocity = + math::eigen3::convert(dartBody1->getLinearVelocity()); + math::Vector3d body2LinearVelocity = + math::eigen3::convert(dartBody2->getLinearVelocity()); + math::Vector3d body3LinearVelocity = + math::eigen3::convert(dartBody3->getLinearVelocity()); + EXPECT_NEAR(0.0, body1LinearVelocity.Z(), 1e-7); + 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); + + // Expect the middle box to be still as it is already at rest. + // Expect the two side boxes to fall away. + math::Vector3d body1LinearVelocity = + math::eigen3::convert(dartBody1->getLinearVelocity()); + math::Vector3d body2LinearVelocity = + math::eigen3::convert(dartBody2->getLinearVelocity()); + math::Vector3d body3LinearVelocity = + math::eigen3::convert(dartBody3->getLinearVelocity()); + EXPECT_GT(0, body1LinearVelocity.Z()); + EXPECT_NEAR(0.0, body2LinearVelocity.Z(), 1e-7); + EXPECT_GT(0, body3LinearVelocity.Z()); + } +} + ///////////////////////////////////////////////// // Expectations on number of links before/after attach/detach TEST_F(JointFeaturesFixture, LinkCountsInJointAttachDetach) diff --git a/dartsim/worlds/joint_constraint.sdf b/dartsim/worlds/joint_constraint.sdf new file mode 100644 index 000000000..23575a5e9 --- /dev/null +++ b/dartsim/worlds/joint_constraint.sdf @@ -0,0 +1,110 @@ + + + + + + 0 -0.2 0.45 0 0 0 + false + + + + + 0.1 0.1 0.1 + + + + + + + 0.1 0.1 0.1 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 0 0.2 0.45 0 0 0 + false + + + + + 0.1 0.1 0.1 + + + + + + + 0.1 0.1 0.1 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 0 0.6 0.45 0 0 0 + false + + + + + 0.1 0.1 0.1 + + + + + + + 0.1 0.1 0.1 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 0 0.2 0.2 0 0 0 + true + + + + + 0.4 0.4 0.4 + + + + + + + 0.4 0.4 0.4 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + +