Skip to content

Commit

Permalink
Added tests between different joint types
Browse files Browse the repository at this point in the history 
Signed-off-by: Aditya <[email protected]>
  • Loading branch information
@adityapande-1995
adityapande-1995 committed Oct 11, 2022
1 parent 4ce9d72 commit 1317a6d
Show file tree
Hide file tree
Showing 2 changed files with 218 additions and 91 deletions.
157 changes: 66 additions & 91 deletions test/common_test/joint_features.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1842,84 +1842,15 @@ using JointMimicFeatureTestTypes =
TYPED_TEST_SUITE(JointMimicFeatureFixture,
JointMimicFeatureTestTypes);

TYPED_TEST(JointMimicFeatureFixture, RevoluteMimicTest)
{
for (const std::string &name : this->pluginNames)
{
if(this->PhysicsEngineName(name) != "dartsim")
{
GTEST_SKIP();
}

std::cout << "Testing plugin: " << name << std::endl;
gz::plugin::PluginPtr plugin = this->loader.Instantiate(name);

auto engine = gz::physics::RequestEngine3d<JointMimicFeatureList>::From(plugin);
ASSERT_NE(nullptr, engine);

sdf::Root root;
const sdf::Errors errors = root.Load(gz::common::joinPaths(TEST_WORLD_DIR, "test.world"));
ASSERT_TRUE(errors.empty()) << errors.front();

auto world = engine->ConstructWorld(*root.WorldByIndex(0));

// Test mimic constraint between two revolute joints.
auto model = world->GetModel("double_pendulum_with_base");
auto upperJoint = model->GetJoint("upper_joint");
auto lowerJoint = model->GetJoint("lower_joint");

// Ensure both joints start from zero angle.
EXPECT_EQ(upperJoint->GetPosition(0), 0);
EXPECT_EQ(lowerJoint->GetPosition(0), 0);

gz::physics::ForwardStep::Output output;
gz::physics::ForwardStep::State state;
gz::physics::ForwardStep::Input input;

// Case : Without mimic constraint

// Let the simulation run without mimic constraint.
// The positions of joints should not be equal.
double upperJointPrevPos = 0;
for (int _ = 0; _ < 10; _++)
{
world->Step(output, state, input);
EXPECT_NE(upperJointPrevPos, lowerJoint->GetPosition(0));
upperJointPrevPos = upperJoint->GetPosition(0);
}

auto testMimicFcn = [&](double multiplier, double offset)
{
// Set mimic joint constraint.
lowerJoint->SetMimicConstraint("upper_joint", multiplier, offset);
// Reset positions and run a few iterations so the positions reach nontrivial values.
upperJoint->SetPosition(0, 0);
lowerJoint->SetPosition(0, 0);
for (int _ = 0; _ < 10; _++)
world->Step(output, state, input);

// Lower joint's position should be equal to that of upper joint in previous timestep.
upperJointPrevPos = upperJoint->GetPosition(0);
for (int _ = 0; _ < 10; _++)
{
world->Step(output, state, input);
EXPECT_FLOAT_EQ(multiplier * upperJointPrevPos + offset, lowerJoint->GetPosition(0));
upperJointPrevPos = upperJoint->GetPosition(0);
}
};

// Testing with different (multiplier, offset) combinations.
testMimicFcn(1, 0);
testMimicFcn(-1, 0);
testMimicFcn(1, 0.1);
testMimicFcn(-1, 0.2);
testMimicFcn(-2, 0);
testMimicFcn(2, 0.1);
}
}

TYPED_TEST(JointMimicFeatureFixture, PrismaticMimicTest)
TYPED_TEST(JointMimicFeatureFixture, PrismaticRevoluteMimicTest)
{
// This test contains 5 joints : 3 prismatic and 2 revolute.
// They are connected as follows :
// prismatic_joint_1 : Free joint
// prismatic_joint_2 : Mimics prismatic_joint_1
// revolute_joint_1 : Mimics prismatic_joint_1
// revolute_joint_2 : Mimics revolute_joint_1
// prismatic_joint_3 : Mimics revolute_joint_1
for (const std::string &name : this->pluginNames)
{
if(this->PhysicsEngineName(name) != "dartsim")
Expand All @@ -1940,14 +1871,20 @@ TYPED_TEST(JointMimicFeatureFixture, PrismaticMimicTest)

auto world = engine->ConstructWorld(*root.WorldByIndex(0));

// Test mimic constraint between two revolute joints.
auto model = world->GetModel("prismatic_model");

auto parentJoint = model->GetJoint("prismatic_joint_1");
auto childJoint = model->GetJoint("prismatic_joint_2");
auto prismaticChildJoint1 = model->GetJoint("prismatic_joint_2");
auto revoluteChildJoint1 = model->GetJoint("revolute_joint_1");
auto revoluteChildJoint2 = model->GetJoint("revolute_joint_2");
auto prismaticChildJoint2 = model->GetJoint("prismatic_joint_3");

// Ensure both joints start from zero angle.
EXPECT_EQ(parentJoint->GetPosition(0), 0);
EXPECT_EQ(childJoint->GetPosition(0), 0);
EXPECT_EQ(prismaticChildJoint1->GetPosition(0), 0);
EXPECT_EQ(revoluteChildJoint1->GetPosition(0), 0);
EXPECT_EQ(revoluteChildJoint2->GetPosition(0), 0);
EXPECT_EQ(prismaticChildJoint2->GetPosition(0), 0);

gz::physics::ForwardStep::Output output;
gz::physics::ForwardStep::State state;
Expand All @@ -1957,31 +1894,69 @@ TYPED_TEST(JointMimicFeatureFixture, PrismaticMimicTest)

// Let the simulation run without mimic constraint.
// The positions of joints should not be equal.
double parentJointPrevPos = 0;
for (int _ = 0; _ < 10; _++)
double prismaticJointPrevPos = 0;
double revoluteJointPrevPos = 0;
for (int i = 0; i < 10; i++)
{
world->Step(output, state, input);
EXPECT_NE(parentJointPrevPos, childJoint->GetPosition(0));
parentJointPrevPos = parentJoint->GetPosition(0);
if (i > 5)
{
EXPECT_NE(prismaticJointPrevPos, prismaticChildJoint1->GetPosition(0));
EXPECT_NE(prismaticJointPrevPos, revoluteChildJoint1->GetPosition(0));
EXPECT_NE(revoluteJointPrevPos, revoluteChildJoint2->GetPosition(0));
EXPECT_NE(revoluteJointPrevPos, prismaticChildJoint2->GetPosition(0));
}

// Update previous positions.
prismaticJointPrevPos = parentJoint->GetPosition(0);
revoluteJointPrevPos = revoluteChildJoint1->GetPosition(0);
}

auto testMimicFcn = [&](double multiplier, double offset)
{
// Set mimic joint constraint.
childJoint->SetMimicConstraint("prismatic_joint_1", multiplier, offset);
// Set mimic joint constraints.
// Parent --> Child
// prismatic_joint_1 -> prismatic_joint_2
// prismatic_joint_1 -> revolute_joint_1
// revolute_joint_1 --> revolute_joint_2
// revolute_joint_1 --> prismatic_joint_3
prismaticChildJoint1->SetMimicConstraint("prismatic_joint_1", multiplier, offset);
revoluteChildJoint1->SetMimicConstraint("prismatic_joint_1", multiplier, offset);
revoluteChildJoint2->SetMimicConstraint("revolute_joint_1", multiplier, offset);
prismaticChildJoint2->SetMimicConstraint("revolute_joint_1", multiplier, offset);

// Reset positions and run a few iterations so the positions reach nontrivial values.
parentJoint->SetPosition(0, 0);
childJoint->SetPosition(0, 0);
prismaticChildJoint1->SetPosition(0, 0);
prismaticChildJoint2->SetPosition(0, 0);
revoluteChildJoint1->SetPosition(0, 0);
revoluteChildJoint2->SetPosition(0, 0);
for (int _ = 0; _ < 10; _++)
world->Step(output, state, input);

// Child joint's position should be equal to that of parent joint in previous timestep.
parentJointPrevPos = parentJoint->GetPosition(0);
// Child joint's position should be equal to that of parent joint in previous timestep,
// considering the offsets and multipliers.
prismaticJointPrevPos = parentJoint->GetPosition(0);
revoluteJointPrevPos = revoluteChildJoint1->GetPosition(0);
for (int _ = 0; _ < 10; _++)
{
world->Step(output, state, input);
EXPECT_FLOAT_EQ(multiplier * parentJointPrevPos + offset, childJoint->GetPosition(0));
parentJointPrevPos = parentJoint->GetPosition(0);
// Check for prismatic -> prismatic mimicking.
EXPECT_FLOAT_EQ(multiplier * prismaticJointPrevPos + offset,
prismaticChildJoint1->GetPosition(0));
// Check for prismatic -> revolute mimicking.
EXPECT_FLOAT_EQ(multiplier * prismaticJointPrevPos + offset,
revoluteChildJoint1->GetPosition(0));
// Check for revolute -> revolute mimicking.
EXPECT_FLOAT_EQ(multiplier * revoluteJointPrevPos + offset,
revoluteChildJoint2->GetPosition(0));
// Check for revolute --> prismatic mimicking.
EXPECT_FLOAT_EQ(multiplier * revoluteJointPrevPos + offset,
prismaticChildJoint2->GetPosition(0));

// Update previous positions.
prismaticJointPrevPos = parentJoint->GetPosition(0);
revoluteJointPrevPos = revoluteChildJoint1->GetPosition(0);
}
};

Expand Down
152 changes: 152 additions & 0 deletions test/common_test/worlds/mimic_prismatic_world.sdf
View file
Original file line number Diff line number Diff line change
Expand Up @@ -212,6 +212,123 @@
</visual>
</link>

<link name="revolute_link_1">
<!-- Put the slide near the top of the base -->
<pose >-0.5 0 0.70 0 0 0</pose>
<inertial>
<inertia>
<!-- (mass / 12) * (y*y + z*z) -->
<ixx>0.0060417</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<!-- (mass / 12) * (x*x + z*z) -->
<iyy>0.0060417</iyy>
<iyz>0</iyz>
<!-- (mass / 12) * (y*y + x*x) -->
<izz>0.0016667</izz>
</inertia>
</inertial>
<!-- Same geometry as visual -->
<collision name="prismatic_slide_collision">
<geometry>
<box>
<size>0.1 0.1 0.25</size>
</box>
</geometry>
</collision>
<!-- Same geometry as collision -->
<visual name="prismatic_slide_visual">
<geometry>
<box>
<size>0.1 0.1 0.25</size>
</box>
</geometry>
<material>
<ambient>1 0 0 1</ambient>
<diffuse>1 0 0 1</diffuse>
<specular>1 0 0 1</specular>
</material>
</visual>
</link>

<link name="revolute_link_2">
<!-- Put the slide near the top of the base -->
<pose >-1 0 0.70 0 0 0</pose>
<inertial>
<inertia>
<!-- (mass / 12) * (y*y + z*z) -->
<ixx>0.0060417</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<!-- (mass / 12) * (x*x + z*z) -->
<iyy>0.0060417</iyy>
<iyz>0</iyz>
<!-- (mass / 12) * (y*y + x*x) -->
<izz>0.0016667</izz>
</inertia>
</inertial>
<!-- Same geometry as visual -->
<collision name="prismatic_slide_collision">
<geometry>
<box>
<size>0.1 0.1 0.25</size>
</box>
</geometry>
</collision>
<!-- Same geometry as collision -->
<visual name="prismatic_slide_visual">
<geometry>
<box>
<size>0.1 0.1 0.25</size>
</box>
</geometry>
<material>
<ambient>1 0 0 1</ambient>
<diffuse>1 0 0 1</diffuse>
<specular>1 0 0 1</specular>
</material>
</visual>
</link>

<link name="prismatic_slide_3">
<!-- Put the slide near the top of the base -->
<pose >-1.5 0 0.70 0 0 0</pose>
<inertial>
<inertia>
<!-- (mass / 12) * (y*y + z*z) -->
<ixx>0.0060417</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<!-- (mass / 12) * (x*x + z*z) -->
<iyy>0.0060417</iyy>
<iyz>0</iyz>
<!-- (mass / 12) * (y*y + x*x) -->
<izz>0.0016667</izz>
</inertia>
</inertial>
<!-- Same geometry as visual -->
<collision name="prismatic_slide_collision">
<geometry>
<box>
<size>0.1 0.1 0.25</size>
</box>
</geometry>
</collision>
<!-- Same geometry as collision -->
<visual name="prismatic_slide_visual">
<geometry>
<box>
<size>0.1 0.1 0.25</size>
</box>
</geometry>
<material>
<ambient>1 0 0 1</ambient>
<diffuse>1 0 0 1</diffuse>
<specular>1 0 0 1</specular>
</material>
</visual>
</link>

<joint name="prismatic_joint_1" type="prismatic">
<parent>prismatic_base</parent>
<child>prismatic_slide</child>
Expand Down Expand Up @@ -244,6 +361,41 @@
</axis>
</joint>

<joint name="revolute_joint_1" type="revolute">
<parent>prismatic_base</parent>
<child>revolute_link_1</child>
<axis>
<xyz>1.0 0 0</xyz>
<dynamics>
<damping>3.0</damping>
</dynamics>
</axis>
</joint>

<joint name="revolute_joint_2" type="revolute">
<parent>prismatic_base</parent>
<child>revolute_link_2</child>
<axis>
<xyz>1.0 0 0</xyz>
<dynamics>
<damping>3.0</damping>
</dynamics>
</axis>
</joint>

<joint name="prismatic_joint_3" type="prismatic">
<parent>prismatic_base</parent>
<child>prismatic_slide_3</child>
<axis>
<xyz>0 0 1</xyz>
<!-- Make it springy to keep the demo moving -->
<dynamics>
<spring_stiffness>100</spring_stiffness>
</dynamics>
</axis>
</joint>

</model>
</world>
</sdf>

0 comments on commit 1317a6d

Please sign in to comment.