Skip to content

Commit

Permalink
Add a test to verify behavior of detachable joints (#563)
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 authored Oct 30, 2023
1 parent 128f740 commit b83354d
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 b83354d

Please sign in to comment.