367 lines
13 KiB
C++
367 lines
13 KiB
C++
|
/*
|
||
|
|
* Copyright (C) 2015 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 <string.h>
|
||
|
|
|
||
|
|
#include <ignition/math/Pose3.hh>
|
||
|
|
|
||
|
|
#include "gazebo/msgs/msgs.hh"
|
||
|
|
#include "gazebo/physics/physics.hh"
|
||
|
|
#include "gazebo/transport/transport.hh"
|
||
|
|
#include "gazebo/test/ServerFixture.hh"
|
||
|
|
#include "gazebo/test/helper_physics_generator.hh"
|
||
|
|
|
||
|
|
#define PHYSICS_TOL 1e-2
|
||
|
|
using namespace gazebo;
|
||
|
|
|
||
|
|
class PhysicsMsgsTest : public ServerFixture,
|
||
|
|
public testing::WithParamInterface<const char*>
|
||
|
|
{
|
||
|
|
/// \brief Test loading a world with nested model.
|
||
|
|
/// \param[in] _physicsEngine Physics Engine type.
|
||
|
|
public: void LoadNestedModel(const std::string &_physicsEngine);
|
||
|
|
|
||
|
|
/// \brief Test spawning a nested model into the world.
|
||
|
|
/// \param[in] _physicsEngine Physics Engine type.
|
||
|
|
public: void SpawnNestedModel(const std::string &_physicsEngine);
|
||
|
|
};
|
||
|
|
|
||
|
|
|
||
|
|
////////////////////////////////////////////////////////////////////////
|
||
|
|
void PhysicsMsgsTest::LoadNestedModel(const std::string &_physicsEngine)
|
||
|
|
{
|
||
|
|
// load a world with a nested model
|
||
|
|
Load("worlds/nested_model.world", true, _physicsEngine);
|
||
|
|
physics::WorldPtr world = physics::get_world("default");
|
||
|
|
ASSERT_TRUE(world != NULL);
|
||
|
|
|
||
|
|
physics::PhysicsEnginePtr physics = world->GetPhysicsEngine();
|
||
|
|
ASSERT_TRUE(physics != NULL);
|
||
|
|
EXPECT_EQ(physics->GetType(), _physicsEngine);
|
||
|
|
|
||
|
|
// take a step to verify that simulation won't crash
|
||
|
|
world->Step(1);
|
||
|
|
|
||
|
|
// verify top level model
|
||
|
|
physics::ModelPtr model;
|
||
|
|
EXPECT_NO_THROW(model = world->GetModel("model_00"));
|
||
|
|
|
||
|
|
if (_physicsEngine == "simbody")
|
||
|
|
{
|
||
|
|
gzerr << "Nested models are not working in simbody yet, issue #1718"
|
||
|
|
<< std::endl;
|
||
|
|
EXPECT_TRUE(model == NULL);
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
else if (_physicsEngine == "dart")
|
||
|
|
{
|
||
|
|
gzerr << "Nested models are not working in dart yet, issue #1833"
|
||
|
|
<< std::endl;
|
||
|
|
|
||
|
|
EXPECT_TRUE(model == NULL);
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
EXPECT_TRUE(model != NULL);
|
||
|
|
}
|
||
|
|
|
||
|
|
EXPECT_EQ(model->GetWorldPose().Ign(),
|
||
|
|
ignition::math::Pose3d(0, 0, 0.5, 0, 0, 0));
|
||
|
|
|
||
|
|
// verify top level model link
|
||
|
|
auto links = model->GetLinks();
|
||
|
|
EXPECT_EQ(links.size(), 1u);
|
||
|
|
physics::LinkPtr link = links[0];
|
||
|
|
EXPECT_TRUE(link != NULL);
|
||
|
|
EXPECT_EQ(link->GetName(), "link_00");
|
||
|
|
EXPECT_EQ(link->GetScopedName(), "model_00::link_00");
|
||
|
|
EXPECT_EQ(link->GetWorldPose().Ign(),
|
||
|
|
ignition::math::Pose3d(0, 0, 0.5, 0, 0, 0));
|
||
|
|
|
||
|
|
// verify nested model
|
||
|
|
auto models = model->NestedModels();
|
||
|
|
EXPECT_EQ(models.size(), 1u);
|
||
|
|
physics::ModelPtr nestedModel = models[0];
|
||
|
|
EXPECT_TRUE(nestedModel != NULL);
|
||
|
|
EXPECT_EQ(nestedModel->GetName(), "model_01");
|
||
|
|
EXPECT_EQ(nestedModel->GetScopedName(), "model_00::model_01");
|
||
|
|
EXPECT_EQ(nestedModel->GetWorldPose().Ign(),
|
||
|
|
ignition::math::Pose3d(1, 0, 0.5, 0, 0, 0));
|
||
|
|
|
||
|
|
// verify nested model link
|
||
|
|
auto nestedModelLinks = nestedModel->GetLinks();
|
||
|
|
EXPECT_EQ(nestedModelLinks.size(), 1u);
|
||
|
|
physics::LinkPtr nestedModelLink = nestedModelLinks[0];
|
||
|
|
EXPECT_TRUE(nestedModelLink != NULL);
|
||
|
|
EXPECT_EQ(nestedModelLink->GetName(), "link_01");
|
||
|
|
EXPECT_EQ(nestedModelLink->GetScopedName(), "model_00::model_01::link_01");
|
||
|
|
EXPECT_EQ(nestedModelLink->GetWorldPose().Ign(),
|
||
|
|
ignition::math::Pose3d(1.25, 0, 0.5, 0, 0, 0));
|
||
|
|
|
||
|
|
// verify canonical link
|
||
|
|
physics::LinkPtr canonicalLink = model->GetLink();
|
||
|
|
EXPECT_TRUE(canonicalLink != NULL);
|
||
|
|
EXPECT_EQ(canonicalLink->GetName(), "link_00");
|
||
|
|
EXPECT_EQ(canonicalLink->GetScopedName(), "model_00::link_00");
|
||
|
|
|
||
|
|
// there should be only one canonical link in the whole model tree
|
||
|
|
// check if the nested model's canonical link is the same one as the top
|
||
|
|
// level model
|
||
|
|
physics::LinkPtr canonicalLink2 = nestedModel->GetLink();
|
||
|
|
EXPECT_TRUE(canonicalLink2 != NULL);
|
||
|
|
EXPECT_EQ(canonicalLink2->GetName(), "link_00");
|
||
|
|
EXPECT_EQ(canonicalLink2->GetScopedName(), "model_00::link_00");
|
||
|
|
|
||
|
|
// verify model joint
|
||
|
|
EXPECT_EQ(model->GetJointCount(), 1u);
|
||
|
|
auto joints = model->GetJoints();
|
||
|
|
physics::JointPtr joint = joints[0];
|
||
|
|
EXPECT_TRUE(joint != NULL);
|
||
|
|
EXPECT_EQ(joint->GetName(), "joint_00");
|
||
|
|
EXPECT_EQ(joint, model->GetJoint("joint_00"));
|
||
|
|
EXPECT_TRUE(joint->GetJointLink(0) != NULL);
|
||
|
|
EXPECT_TRUE(joint->GetJointLink(1) != NULL);
|
||
|
|
EXPECT_TRUE(joint->GetJointLink(0)->GetName() == "link_00" ||
|
||
|
|
joint->GetJointLink(0)->GetName() == "link_01");
|
||
|
|
EXPECT_TRUE(joint->GetJointLink(1)->GetName() == "link_00" ||
|
||
|
|
joint->GetJointLink(1)->GetName() == "link_01");
|
||
|
|
EXPECT_EQ(joint->GetMsgType(), msgs::Joint::REVOLUTE);
|
||
|
|
EXPECT_EQ(joint->GetLocalAxis(0), ignition::math::Vector3d::UnitX);
|
||
|
|
|
||
|
|
// verify nested model joint
|
||
|
|
EXPECT_EQ(nestedModel->GetJointCount(), 0u);
|
||
|
|
}
|
||
|
|
|
||
|
|
////////////////////////////////////////////////////////////////////////
|
||
|
|
void PhysicsMsgsTest::SpawnNestedModel(const std::string &_physicsEngine)
|
||
|
|
{
|
||
|
|
if (_physicsEngine == "simbody")
|
||
|
|
{
|
||
|
|
gzerr << "Nested models are not working in simbody yet, issue #1718"
|
||
|
|
<< std::endl;
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
|
||
|
|
if (_physicsEngine == "dart")
|
||
|
|
{
|
||
|
|
gzerr << "Nested models are not working in dart yet, issue #1833"
|
||
|
|
<< std::endl;
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
|
||
|
|
// load an empty world
|
||
|
|
Load("worlds/empty.world", true, _physicsEngine);
|
||
|
|
physics::WorldPtr world = physics::get_world("default");
|
||
|
|
ASSERT_TRUE(world != NULL);
|
||
|
|
|
||
|
|
physics::PhysicsEnginePtr physics = world->GetPhysicsEngine();
|
||
|
|
ASSERT_TRUE(physics != NULL);
|
||
|
|
EXPECT_EQ(physics->GetType(), _physicsEngine);
|
||
|
|
|
||
|
|
std::ostringstream sdfStream;
|
||
|
|
sdfStream << "<sdf version='" << SDF_VERSION << "'>"
|
||
|
|
<< "<model name ='model_00'>"
|
||
|
|
<< " <pose>0 0 1 0 0 0</pose>"
|
||
|
|
<< " <model name ='model_01'>"
|
||
|
|
<< " <pose>0 1 0 0 0 0</pose>"
|
||
|
|
<< " <link name ='link_01'>"
|
||
|
|
<< " <pose>1 0 0 0 0 0</pose>"
|
||
|
|
<< " <collision name ='collision_01'>"
|
||
|
|
<< " <geometry>"
|
||
|
|
<< " <box><size>1 1 1</size></box>"
|
||
|
|
<< " </geometry>"
|
||
|
|
<< " </collision>"
|
||
|
|
<< " <visual name ='visual_01'>"
|
||
|
|
<< " <geometry>"
|
||
|
|
<< " <box><size>1 1 1</size></box>"
|
||
|
|
<< " </geometry>"
|
||
|
|
<< " </visual>"
|
||
|
|
<< " </link>"
|
||
|
|
<< " <link name ='link_02'>"
|
||
|
|
<< " <pose>-1 0 0 0 0 0</pose>"
|
||
|
|
<< " <collision name ='collision_02'>"
|
||
|
|
<< " <geometry>"
|
||
|
|
<< " <cylinder>"
|
||
|
|
<< " <radius>0.5</radius>"
|
||
|
|
<< " <length>1.0</length>"
|
||
|
|
<< " </cylinder>"
|
||
|
|
<< " </geometry>"
|
||
|
|
<< " </collision>"
|
||
|
|
<< " <visual name ='visual_02'>"
|
||
|
|
<< " <geometry>"
|
||
|
|
<< " <cylinder>"
|
||
|
|
<< " <radius>0.5</radius>"
|
||
|
|
<< " <length>1.0</length>"
|
||
|
|
<< " </cylinder>"
|
||
|
|
<< " </geometry>"
|
||
|
|
<< " </visual>"
|
||
|
|
<< " </link>"
|
||
|
|
<< " <joint name ='joint_01' type='prismatic'>"
|
||
|
|
<< " <pose>0 1 0 0 0 0</pose>"
|
||
|
|
<< " <parent>link_01</parent>"
|
||
|
|
<< " <child>link_02</child>"
|
||
|
|
<< " <axis>"
|
||
|
|
<< " <xyz>0 0 1</xyz>"
|
||
|
|
<< " <use_parent_model_frame>true</use_parent_model_frame>"
|
||
|
|
<< " <limit>"
|
||
|
|
<< " <lower>-0.2</lower>"
|
||
|
|
<< " <upper>0.5</upper>"
|
||
|
|
<< " </limit>"
|
||
|
|
<< " </axis>"
|
||
|
|
<< " </joint>"
|
||
|
|
<< " <model name ='model_02'>"
|
||
|
|
<< " <pose>0 1.3 0 0 0 0</pose>"
|
||
|
|
<< " <link name ='link_01'>"
|
||
|
|
<< " <pose>1 0 0 0 0 0</pose>"
|
||
|
|
<< " <collision name ='collision_01'>"
|
||
|
|
<< " <geometry>"
|
||
|
|
<< " <sphere><radius>0.2</radius></sphere>"
|
||
|
|
<< " </geometry>"
|
||
|
|
<< " </collision>"
|
||
|
|
<< " <visual name ='visual_01'>"
|
||
|
|
<< " <geometry>"
|
||
|
|
<< " <sphere><radius>1 1 1</radius></sphere>"
|
||
|
|
<< " </geometry>"
|
||
|
|
<< " </visual>"
|
||
|
|
<< " </link>"
|
||
|
|
<< " </model>"
|
||
|
|
<< " </model>"
|
||
|
|
<< "</model>"
|
||
|
|
<< "</sdf>";
|
||
|
|
SpawnSDF(sdfStream.str());
|
||
|
|
|
||
|
|
// verify top level model
|
||
|
|
physics::ModelPtr model;
|
||
|
|
model = world->GetModel("model_00");
|
||
|
|
EXPECT_TRUE(model != NULL);
|
||
|
|
EXPECT_EQ(model->GetWorldPose().Ign(),
|
||
|
|
ignition::math::Pose3d(0, 0, 1, 0, 0, 0));
|
||
|
|
|
||
|
|
// verify top level model link
|
||
|
|
auto links = model->GetLinks();
|
||
|
|
EXPECT_EQ(links.size(), 0u);
|
||
|
|
|
||
|
|
// verify nested model
|
||
|
|
auto models = model->NestedModels();
|
||
|
|
EXPECT_EQ(models.size(), 1u);
|
||
|
|
physics::ModelPtr nestedModel = models[0];
|
||
|
|
EXPECT_TRUE(nestedModel != NULL);
|
||
|
|
EXPECT_EQ(nestedModel->GetName(), "model_01");
|
||
|
|
EXPECT_EQ(nestedModel->GetScopedName(), "model_00::model_01");
|
||
|
|
EXPECT_EQ(nestedModel->GetWorldPose().Ign(),
|
||
|
|
ignition::math::Pose3d(0, 1, 1, 0, 0, 0));
|
||
|
|
|
||
|
|
// verify nested model links
|
||
|
|
auto nestedModelLinks = nestedModel->GetLinks();
|
||
|
|
EXPECT_EQ(nestedModelLinks.size(), 2u);
|
||
|
|
physics::LinkPtr nestedModelLink = nestedModelLinks[0];
|
||
|
|
EXPECT_TRUE(nestedModelLink != NULL);
|
||
|
|
EXPECT_EQ(nestedModelLink->GetName(), "link_01");
|
||
|
|
EXPECT_EQ(nestedModelLink->GetScopedName(), "model_00::model_01::link_01");
|
||
|
|
EXPECT_EQ(nestedModelLink->GetWorldPose().Ign(),
|
||
|
|
ignition::math::Pose3d(1, 1, 1, 0, 0, 0));
|
||
|
|
physics::LinkPtr nestedModelLink2 = nestedModelLinks[1];
|
||
|
|
EXPECT_TRUE(nestedModelLink2 != NULL);
|
||
|
|
EXPECT_EQ(nestedModelLink2->GetName(), "link_02");
|
||
|
|
EXPECT_EQ(nestedModelLink2->GetScopedName(), "model_00::model_01::link_02");
|
||
|
|
EXPECT_EQ(nestedModelLink2->GetWorldPose().Ign(),
|
||
|
|
ignition::math::Pose3d(-1, 1, 1, 0, 0, 0));
|
||
|
|
|
||
|
|
// verify nested-nested model
|
||
|
|
auto doubleNestedModels = nestedModel->NestedModels();
|
||
|
|
EXPECT_EQ(doubleNestedModels.size(), 1u);
|
||
|
|
physics::ModelPtr doubleNestedModel = doubleNestedModels[0];
|
||
|
|
EXPECT_TRUE(doubleNestedModel != NULL);
|
||
|
|
EXPECT_EQ(doubleNestedModel->GetName(), "model_02");
|
||
|
|
EXPECT_EQ(doubleNestedModel->GetScopedName(), "model_00::model_01::model_02");
|
||
|
|
EXPECT_EQ(doubleNestedModel->GetWorldPose().Ign(),
|
||
|
|
ignition::math::Pose3d(0, 2.3, 1, 0, 0, 0));
|
||
|
|
|
||
|
|
// verify nested-nested model links
|
||
|
|
auto doubleNestedModelLinks = doubleNestedModel->GetLinks();
|
||
|
|
EXPECT_EQ(doubleNestedModelLinks.size(), 1u);
|
||
|
|
physics::LinkPtr doubleNestedModelLink = doubleNestedModelLinks[0];
|
||
|
|
EXPECT_TRUE(doubleNestedModelLink != NULL);
|
||
|
|
EXPECT_EQ(doubleNestedModelLink->GetName(), "link_01");
|
||
|
|
EXPECT_EQ(doubleNestedModelLink->GetScopedName(),
|
||
|
|
"model_00::model_01::model_02::link_01");
|
||
|
|
EXPECT_EQ(doubleNestedModelLink->GetWorldPose().Ign(),
|
||
|
|
ignition::math::Pose3d(1, 2.3, 1, 0, 0, 0));
|
||
|
|
|
||
|
|
// verify canonical link
|
||
|
|
physics::LinkPtr canonicalLink = model->GetLink();
|
||
|
|
EXPECT_TRUE(canonicalLink != NULL);
|
||
|
|
EXPECT_EQ(canonicalLink->GetName(), "link_01");
|
||
|
|
EXPECT_EQ(canonicalLink->GetScopedName(), "model_00::model_01::link_01");
|
||
|
|
|
||
|
|
// there should be only one canonical link in the whole model tree
|
||
|
|
// check if the nested model's canonical link is the same one as the top
|
||
|
|
// level model
|
||
|
|
physics::LinkPtr canonicalLink2 = nestedModel->GetLink();
|
||
|
|
EXPECT_TRUE(canonicalLink2 != NULL);
|
||
|
|
EXPECT_EQ(canonicalLink2->GetName(), "link_01");
|
||
|
|
EXPECT_EQ(canonicalLink2->GetScopedName(), "model_00::model_01::link_01");
|
||
|
|
|
||
|
|
physics::LinkPtr canonicalLink3 = doubleNestedModel->GetLink();
|
||
|
|
EXPECT_TRUE(canonicalLink3 != NULL);
|
||
|
|
EXPECT_EQ(canonicalLink3->GetName(), "link_01");
|
||
|
|
EXPECT_EQ(canonicalLink3->GetScopedName(), "model_00::model_01::link_01");
|
||
|
|
|
||
|
|
// verify joint
|
||
|
|
EXPECT_EQ(model->GetJointCount(), 0u);
|
||
|
|
|
||
|
|
// verify nested model joint
|
||
|
|
EXPECT_EQ(nestedModel->GetJointCount(), 1u);
|
||
|
|
auto nestedModelJoints = nestedModel->GetJoints();
|
||
|
|
physics::JointPtr nestedModelJoint = nestedModelJoints[0];
|
||
|
|
EXPECT_TRUE(nestedModelJoint != NULL);
|
||
|
|
EXPECT_EQ(nestedModelJoint->GetName(), "joint_01");
|
||
|
|
EXPECT_EQ(nestedModelJoint, nestedModel->GetJoint("joint_01"));
|
||
|
|
EXPECT_TRUE(nestedModelJoint->GetJointLink(0) != NULL);
|
||
|
|
EXPECT_TRUE(nestedModelJoint->GetJointLink(1) != NULL);
|
||
|
|
EXPECT_TRUE(nestedModelJoint->GetJointLink(0)->GetName() == "link_01" ||
|
||
|
|
nestedModelJoint->GetJointLink(0)->GetName() == "link_02");
|
||
|
|
EXPECT_TRUE(nestedModelJoint->GetJointLink(1)->GetName() == "link_01" ||
|
||
|
|
nestedModelJoint->GetJointLink(1)->GetName() == "link_02");
|
||
|
|
EXPECT_EQ(nestedModelJoint->GetMsgType(), msgs::Joint::PRISMATIC);
|
||
|
|
EXPECT_EQ(nestedModelJoint->GetLocalAxis(0), ignition::math::Vector3d::UnitZ);
|
||
|
|
EXPECT_EQ(nestedModelJoint->GetLowerLimit(0), -0.2);
|
||
|
|
EXPECT_EQ(nestedModelJoint->GetUpperLimit(0), 0.5);
|
||
|
|
}
|
||
|
|
|
||
|
|
/////////////////////////////////////////////////
|
||
|
|
TEST_P(PhysicsMsgsTest, LoadNestedModel)
|
||
|
|
{
|
||
|
|
LoadNestedModel(GetParam());
|
||
|
|
}
|
||
|
|
|
||
|
|
/////////////////////////////////////////////////
|
||
|
|
TEST_P(PhysicsMsgsTest, SpawnNestedModel)
|
||
|
|
{
|
||
|
|
SpawnNestedModel(GetParam());
|
||
|
|
}
|
||
|
|
|
||
|
|
INSTANTIATE_TEST_CASE_P(PhysicsEngines, PhysicsMsgsTest,
|
||
|
|
PHYSICS_ENGINE_VALUES);
|
||
|
|
|
||
|
|
/////////////////////////////////////////////////
|
||
|
|
int main(int argc, char **argv)
|
||
|
|
{
|
||
|
|
::testing::InitGoogleTest(&argc, argv);
|
||
|
|
return RUN_ALL_TESTS();
|
||
|
|
}
|