Skip to content

Commit

Permalink
Add a test to verify behavior of detachable joints
Browse files Browse the repository at this point in the history 
The reported bug was that the detachable joint was not correctly linked
between objects' canonical links with bullet-featherstone.  While this
was addressed and confirmed working in #530, this adds a test to prevent
future regression (or for future physics implementations)

Signed-off-by: Michael Carroll <[email protected]>
  • Loading branch information
@mjcarroll
mjcarroll committed Oct 24, 2023
1 parent 128f740 commit ef7c334
Show file tree
Hide file tree
Showing 3 changed files with 341 additions and 0 deletions.
1 change: 1 addition & 0 deletions test/common_test/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,7 @@ set(tests
basic_test
collisions
construct_empty_world
detachable_joint
free_joint_features
joint_features
joint_transmitted_wrench_features
Expand Down
202 changes: 202 additions & 0 deletions test/common_test/detachable_joint.cc
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,202 @@
/*
* Copyright (C) 2022 Open Source Robotics Foundation
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
*/
#include <gtest/gtest.h>

#include <chrono>

#include <gz/common/Console.hh>
#include <gz/plugin/Loader.hh>

#include <gz/math/Helpers.hh>
#include <gz/math/eigen3/Conversions.hh>

#include "TestLibLoader.hh"
#include "../Utils.hh"

#include "gz/physics/FrameSemantics.hh"
#include <gz/physics/FindFeatures.hh>
#include <gz/physics/FixedJoint.hh>
#include <gz/physics/ForwardStep.hh>
#include <gz/physics/FreeGroup.hh>
#include <gz/physics/FreeJoint.hh>
#include <gz/physics/GetEntities.hh>
#include <gz/physics/Joint.hh>
#include <gz/physics/RemoveEntities.hh>
#include <gz/physics/RequestEngine.hh>
#include <gz/physics/RevoluteJoint.hh>
#include <gz/physics/Shape.hh>
#include <gz/physics/sdf/ConstructModel.hh>
#include <gz/physics/sdf/ConstructWorld.hh>

#include <sdf/Root.hh>

template <class T>
class DetachableJointTest:
public testing::Test, public gz::physics::TestLibLoader
{
// Documentation inherited
public: void SetUp() override
{
gz::common::Console::SetVerbosity(4);

std::cerr << "DetachableJointTest::GetLibToTest() " << DetachableJointTest::GetLibToTest() << '\n';

loader.LoadLib(DetachableJointTest::GetLibToTest());

pluginNames = gz::physics::FindFeatures3d<T>::From(loader);
if (pluginNames.empty())
{
std::cerr << "No plugins with required features found in "
<< GetLibToTest() << std::endl;
GTEST_SKIP();
}
for (const std::string &name : this->pluginNames)
{
if(this->PhysicsEngineName(name) == "tpe")
{
GTEST_SKIP();
}
}
}

public: std::set<std::string> pluginNames;
public: gz::plugin::Loader loader;
};

struct DetachableJointFeatureList: gz::physics::FeatureList<
gz::physics::ForwardStep,
gz::physics::GetBasicJointProperties,
gz::physics::GetBasicJointState,
gz::physics::GetEngineInfo,
gz::physics::GetJointFromModel,
gz::physics::GetLinkFromModel,
gz::physics::GetModelFromWorld,
gz::physics::LinkFrameSemantics,
gz::physics::SetBasicJointState,
gz::physics::AttachFixedJointFeature,
gz::physics::DetachJointFeature,
gz::physics::SetJointTransformFromParentFeature,
gz::physics::sdf::ConstructSdfWorld
> { };

using DetachableJointTestTypes =
::testing::Types<DetachableJointFeatureList>;
TYPED_TEST_SUITE(DetachableJointTest,
DetachableJointTestTypes);

TYPED_TEST(DetachableJointTest, CorrectAttachmentPoints)
{
for (const std::string &name : this->pluginNames)
{
std::cout << "Testing plugin: " << name << std::endl;
gz::plugin::PluginPtr plugin = this->loader.Instantiate(name);

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

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

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

auto cylinder1 = world->GetModel("cylinder1");
ASSERT_NE(nullptr, cylinder1);
auto cylinder1_base_link = cylinder1->GetLink("base_link");
ASSERT_NE(nullptr, cylinder1_base_link);
auto cylinder1_link1 = cylinder1->GetLink("link1");
ASSERT_NE(nullptr, cylinder1_link1);

auto cylinder2 = world->GetModel("cylinder2");
ASSERT_NE(nullptr, cylinder2);
auto cylinder2_link2 = cylinder2->GetLink("link2");
ASSERT_NE(nullptr, cylinder2_link2);

// Create a detachable joint
const auto poseParent =
cylinder1_link1->FrameDataRelativeToWorld().pose;
const auto poseChild =
cylinder2_link2->FrameDataRelativeToWorld().pose;
const auto poseParentChild = poseParent.inverse() * poseChild;
auto jointPtrPhys =
cylinder2_link2->AttachFixedJoint(cylinder1_link1);
ASSERT_NE(nullptr, jointPtrPhys);
jointPtrPhys->SetTransformFromParent(poseParentChild);

{
// Check initial conditions
// Cylinder 1 is fixed 0.5m above cylinder 2, which is 1.5m above the
// ground.
auto frameDataC1L1 = cylinder1_link1->FrameDataRelativeToWorld();
auto frameDataC2L2 = cylinder2_link2->FrameDataRelativeToWorld();
EXPECT_EQ(gz::math::Pose3d(0, 0, 2, 0, 0, 0),
gz::math::eigen3::convert(frameDataC1L1.pose));
EXPECT_EQ(gz::math::Pose3d(0, 0, 1.5, 0, 0, 0),
gz::math::eigen3::convert(frameDataC2L2.pose));
}

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

for (std::size_t i = 0; i < 1000 ; ++i)
{
// step forward and expect lower link to fall
world->Step(output, state, input);
}

{
// Check final conditions with joint attached
// If the joint was attached correctly, the top cylinder should be
// fixed 0.5m above the bottom cylinder, which is resting on the ground.
auto frameDataC1L1 = cylinder1_link1->FrameDataRelativeToWorld();
auto frameDataC2L2 = cylinder2_link2->FrameDataRelativeToWorld();
EXPECT_NEAR(0.55, frameDataC1L1.pose.translation().z(), 1e-2);
EXPECT_NEAR(0.05, frameDataC2L2.pose.translation().z(), 1e-2);
}

// Detach joint and step physics
jointPtrPhys->Detach();
for (std::size_t i = 0; i < 1000; ++i)
{
world->Step(output, state, input);
}

{
// Check final conditions after joint detached
// If the joint was detached correctly, the top cylinder should be
// resting directly on the bottom cylinder, which is resting on the
// ground.
auto frameDataC1L1 = cylinder1_link1->FrameDataRelativeToWorld();
auto frameDataC2L2 = cylinder2_link2->FrameDataRelativeToWorld();
EXPECT_NEAR(0.15, frameDataC1L1.pose.translation().z(), 1e-2);
EXPECT_NEAR(0.05, frameDataC2L2.pose.translation().z(), 1e-2);
}
}
}

int main(int argc, char *argv[])
{
::testing::InitGoogleTest(&argc, argv);
if (!DetachableJointTest<DetachableJointFeatureList>::init(
argc, argv))
return -1;
return RUN_ALL_TESTS();
}
138 changes: 138 additions & 0 deletions test/common_test/worlds/detachable_joint.world
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,138 @@
<?xml version="1.0" encoding="UTF-8"?>
<sdf version="1.9">
<world name="default">
<plugin
filename="gz-sim-physics-system"
name="gz::sim::systems::Physics">
<engine><filename>gz-physics-bullet_featherstone-plugin</filename></engine>
</plugin>
<plugin
filename="gz-sim-scene-broadcaster-system"
name="gz::sim::systems::SceneBroadcaster">
</plugin>
<model name="ground_plane">
<static>true</static>
<link name="surface">
<collision name="surface">
<geometry>
<plane>
<normal>0 0 1</normal>
<size>2 2</size>
</plane>
</geometry>
</collision>
</link>
</model>

<model name="cylinder1">
<joint name="world_joint" type="fixed">
<parent>world</parent>
<child>base_link</child>
</joint>
<pose>0 0.0 2.0 0 0 0</pose>
<link name="base_link">
<inertial>
<mass>1.0</mass>
<inertia>
<ixx>0.0068</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.0068</iyy>
<iyz>0</iyz>
<izz>0.0032</izz>
</inertia>
</inertial>
</link>
<joint name="base_joint" type="prismatic">
<parent>base_link</parent>
<child>link1</child>
<axis>
<xyz>0 0 1</xyz>
</axis>
</joint>
<link name="link1">
<inertial>
<mass>1.0</mass>
<inertia>
<ixx>0.0068</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.0068</iyy>
<iyz>0</iyz>
<izz>0.0032</izz>
</inertia>
</inertial>
<collision name="collision1">
<geometry>
<cylinder>
<radius>0.2</radius>
<length>0.1</length>
</cylinder>
</geometry>
</collision>
<visual name="visual1">
<geometry>
<cylinder>
<radius>0.2</radius>
<length>0.1</length>
</cylinder>
</geometry>
<material>
<ambient>0.6 0.6 0.9 1</ambient>
<diffuse>0.6 0.6 0.9 1</diffuse>
<specular>1.0 1.0 1.0 1</specular>
</material>
</visual>
</link>

<!-- This is purely for executing in downstream gz-sim.
gz-physics does not parse plugin tags -->
<plugin
filename="gz-sim-detachable-joint-system"
name="gz::sim::systems::DetachableJoint">
<parent_link>link1</parent_link>
<child_model>cylinder2</child_model>
<child_link>link2</child_link>
</plugin>
</model>

<model name="cylinder2">
<pose>0 0.0 1.5 0 0 0</pose>
<link name="link2">
<inertial>
<mass>1.0</mass>
<inertia>
<ixx>0.0068</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.0068</iyy>
<iyz>0</iyz>
<izz>0.0032</izz>
</inertia>
</inertial>
<collision name="collision2">
<geometry>
<cylinder>
<radius>0.2</radius>
<length>0.1</length>
</cylinder>
</geometry>
</collision>
<visual name="visual2">
<geometry>
<cylinder>
<radius>0.2</radius>
<length>0.1</length>
</cylinder>
</geometry>
<material>
<ambient>0.9 0.6 0.2 1</ambient>
<diffuse>0.9 0.6 0.2 1</diffuse>
<specular>1.0 1.0 1.0 1</specular>
</material>
</visual>
</link>
</model>

</world>
</sdf>

0 comments on commit ef7c334

Please sign in to comment.