Skip to content

Commit

Permalink
Add unit tests
Browse files Browse the repository at this point in the history 
Signed-off-by: Arjo Chakravarty <[email protected]>
  • Loading branch information
@arjo129
arjo129 committed Jun 6, 2022
1 parent fdf7d52 commit 393d029
Show file tree
Hide file tree
Showing 2 changed files with 246 additions and 0 deletions.
136 changes: 136 additions & 0 deletions dartsim/src/JointFeatures_TEST.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
110 changes: 110 additions & 0 deletions dartsim/worlds/joint_constraint.sdf
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,110 @@
<?xml version="1.0"?>
<sdf version="1.6">
<world name="default">

<model name="M1">
<pose>0 -0.2 0.45 0 0 0</pose>
<static>false</static>
<link name="link">
<collision name="collision">
<geometry>
<box>
<size>0.1 0.1 0.1</size>
</box>
</geometry>
</collision>
<visual name="visual">
<geometry>
<box>
<size>0.1 0.1 0.1</size>
</box>
</geometry>
<material>
<ambient>0.8 0.8 0.8 1</ambient>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.8 0.8 0.8 1</specular>
</material>
</visual>
</link>
</model>

<model name="M2">
<pose>0 0.2 0.45 0 0 0</pose>
<static>false</static>
<link name="link">
<collision name="collision">
<geometry>
<box>
<size>0.1 0.1 0.1</size>
</box>
</geometry>
</collision>
<visual name="visual">
<geometry>
<box>
<size>0.1 0.1 0.1</size>
</box>
</geometry>
<material>
<ambient>0.8 0.8 0.8 1</ambient>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.8 0.8 0.8 1</specular>
</material>
</visual>
</link>
</model>

<model name="M3">
<pose>0 0.6 0.45 0 0 0</pose>
<static>false</static>
<link name="link">
<collision name="collision">
<geometry>
<box>
<size>0.1 0.1 0.1</size>
</box>
</geometry>
</collision>
<visual name="visual">
<geometry>
<box>
<size>0.1 0.1 0.1</size>
</box>
</geometry>
<material>
<ambient>0.8 0.8 0.8 1</ambient>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.8 0.8 0.8 1</specular>
</material>
</visual>
</link>
</model>

<model name="tabletop">
<pose>0 0.2 0.2 0 0 0</pose>
<static>true</static>
<link name="link">
<collision name="collision">
<geometry>
<box>
<size>0.4 0.4 0.4</size>
</box>
</geometry>
</collision>
<visual name="visual">
<geometry>
<box>
<size>0.4 0.4 0.4</size>
</box>
</geometry>
<material>
<ambient>0.8 0.8 0.8 1</ambient>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.8 0.8 0.8 1</specular>
</material>
</visual>
</link>
</model>

</world>
</sdf>

0 comments on commit 393d029

Please sign in to comment.