p80426359
/
pxmlw6n2f
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity
pxmlw6n2f/Gazebo_Distributed_MPI/test/integration/physics_msgs.cc

1092 lines
35 KiB
C++
Raw Blame History

/*
* Copyright (C) 2012 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 "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"
#ifdef HAVE_BULLET
#include "gazebo/physics/bullet/bullet_math_inc.h"
#endif
#define PHYSICS_TOL 1e-2
using namespace gazebo;
class PhysicsMsgsTest : public ServerFixture,
public testing::WithParamInterface<const char*>
{
public: void MoveTool(const std::string &_physicsEngine);
public: void SetGravity(const std::string &_physicsEngine);
public: void LinkProperties(const std::string &_physicsEngine);
public: void LinkPose(const std::string &_physicsEngine);
public: void SimpleShapeResize(const std::string &_physicsEngine);
public: void LinkVisualMsg(const std::string &_physicsEngine);
public: void JointMsg(const std::string &_physicsEngine);
};
/////////////////////////////////////////////////
void PhysicsMsgsTest::SetGravity(const std::string &_physicsEngine)
{
Load("worlds/empty.world", true, _physicsEngine);
physics::WorldPtr world = physics::get_world("default");
ASSERT_TRUE(world != NULL);
// check the gravity vector
physics::PhysicsEnginePtr physics = world->GetPhysicsEngine();
ASSERT_TRUE(physics != NULL);
EXPECT_EQ(physics->GetType(), _physicsEngine);
math::Vector3 g = physics->GetGravity();
// Assume gravity vector points down z axis only.
EXPECT_EQ(g.x, 0);
EXPECT_EQ(g.y, 0);
EXPECT_LE(g.z, -9.8);
// Set Gravity by publishing to "~/physics"
transport::PublisherPtr physicsPub =
this->node->Advertise<msgs::Physics>("~/physics");
msgs::Physics msg;
// it doesn't actually seem to matter what type you set
msg.set_type(msgs::Physics::Type_MIN);
std::vector<math::Vector3> gravity;
gravity.push_back(math::Vector3(0, 0, 9.81));
gravity.push_back(math::Vector3(0, 0, -20));
gravity.push_back(math::Vector3(0, 0, 20));
gravity.push_back(math::Vector3(0, 0, 0));
gravity.push_back(math::Vector3(0, 0, -9.81));
gravity.push_back(math::Vector3(1, 1, 9.81));
gravity.push_back(math::Vector3(2, 3, -20));
gravity.push_back(math::Vector3(2, -3, 20));
gravity.push_back(math::Vector3(-2, 3, 0));
gravity.push_back(math::Vector3(-2, -3, -9.81));
for (std::vector<math::Vector3>::iterator iter = gravity.begin();
iter != gravity.end(); ++iter)
{
msgs::Set(msg.mutable_gravity(), (*iter).Ign());
physicsPub->Publish(msg);
while (*iter != physics->GetGravity())
{
world->Step(1);
common::Time::MSleep(1);
}
EXPECT_EQ(*iter, physics->GetGravity());
}
}
/////////////////////////////////////////////////
void PhysicsMsgsTest::MoveTool(const std::string &_physicsEngine)
{
Load("worlds/empty.world", true, _physicsEngine);
physics::WorldPtr world = physics::get_world("default");
ASSERT_TRUE(world != NULL);
// set gravity to zero
physics::PhysicsEnginePtr physics = world->GetPhysicsEngine();
ASSERT_TRUE(physics != NULL);
EXPECT_EQ(physics->GetType(), _physicsEngine);
world->SetGravity(ignition::math::Vector3d::Zero);
// spawn a box
std::string name = "test_box";
double z0 = 5;
math::Vector3 pos = math::Vector3(0, 0, z0);
math::Vector3 size = math::Vector3(1, 1, 1);
SpawnBox(name, size, pos, math::Vector3::Zero);
gzdbg << "SpawnBox called" << std::endl;
// advertise on "~/model/modify"
transport::PublisherPtr modelPub =
this->node->Advertise<msgs::Model>("~/model/modify");
// list of poses to move to
std::vector<math::Pose> poses;
poses.push_back(math::Pose(5, 0, z0, 0, 0, 0));
poses.push_back(math::Pose(0, 8, z0, 0, 0, 0));
poses.push_back(math::Pose(-99, 0, z0, 0, 0, 0));
poses.push_back(math::Pose(0, 999, z0, 0, 0, 0));
poses.push_back(math::Pose(123.456, 456.123, z0*10, 0.1, -0.2, 0.3));
poses.push_back(math::Pose(-123.456, 456.123, z0*10, 0.2, 0.4, -0.6));
poses.push_back(math::Pose(123.456, -456.123, z0*10, 0.3, -0.6, 0.9));
poses.push_back(math::Pose(-123.456, -456.123, z0*10, -0.4, 0.8, -1.2));
physics::ModelPtr model = world->GetModel(name);
ASSERT_TRUE(model != NULL);
{
math::Pose initialPose = model->GetWorldPose();
EXPECT_EQ(pos, initialPose.pos);
}
{
msgs::Model msg;
msg.set_name(name);
msg.set_id(model->GetId());
for (std::vector<math::Pose>::iterator iter = poses.begin();
iter != poses.end(); ++iter)
{
msgs::Set(msg.mutable_pose(), (*iter).Ign());
modelPub->Publish(msg);
while (*iter != model->GetWorldPose())
{
world->Step(1);
common::Time::MSleep(1);
}
// Take a few steps to verify the correct model pose.
// dart has a failure mode that was not exposed without
// this change to the test.
world->Step(10);
EXPECT_EQ(*iter, model->GetWorldPose());
}
}
// This test block demonstrates issue #2309 titled:
// Simbody exception when manipulating object twice while paused
// https://bitbucket.org/osrf/gazebo/issues/2309
{
transport::PublisherPtr userCmdPub =
this->node->Advertise<msgs::UserCmd>("~/user_cmd");
msgs::UserCmd userCmdMsg;
userCmdMsg.set_description("Translate [" + name + "]");
userCmdMsg.set_type(msgs::UserCmd::MOVING);
msgs::Model msg;
msg.set_name(name);
msg.set_id(model->GetId());
auto modelMsg = userCmdMsg.add_model();
modelMsg->CopyFrom(msg);
for (std::vector<math::Pose>::iterator iter = poses.begin();
iter != poses.end(); ++iter)
{
msgs::Set(modelMsg->mutable_pose(), (*iter).Ign());
userCmdPub->Publish(userCmdMsg);
while (*iter != model->GetWorldPose())
{
common::Time::MSleep(1);
}
if (_physicsEngine == "simbody")
{
gzerr << "Relaxing part of MoveTool test for Simbody see issue #2309"
<< std::endl;
world->Step(1);
}
EXPECT_EQ(*iter, model->GetWorldPose());
}
}
}
/////////////////////////////////////////////////
void PhysicsMsgsTest::LinkProperties(const std::string &_physicsEngine)
{
// TODO simbody currently fails this test
if (_physicsEngine == "simbody")
{
gzerr << "Aborting LinkProperties test for Simbody" << std::endl;
return;
}
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);
// spawn a box
std::string name = "test_box";
double z0 = 5;
math::Vector3 pos = math::Vector3(0, 0, z0);
math::Vector3 size = math::Vector3(1, 1, 1);
SpawnBox(name, size, pos, math::Vector3::Zero);
// advertise on "~/model/modify"
transport::PublisherPtr modelPub =
this->node->Advertise<msgs::Model>("~/model/modify");
physics::ModelPtr model = world->GetModel(name);
ASSERT_TRUE(model != NULL);
// change gravity mode and verify the msg gets through
physics::LinkPtr link = model->GetLink();
ASSERT_TRUE(link != NULL);
// check default link gravity and kinematic properties
{
// gravity mode should be enabled by default
EXPECT_TRUE(link->GetGravityMode());
// TODO Bullet currently fails this test
if (_physicsEngine != "bullet")
{
// kinematic mode should be disabled by default
EXPECT_TRUE(!link->GetKinematic());
}
else
{
gzerr << "Skipping LinkProperties's kinematic test for Bullet" <<
std::endl;
}
// self collide mode should be disabled by default
EXPECT_TRUE(!link->GetSelfCollide());
}
{
// change gravity mode and verify the msg gets through
msgs::Model msg;
msg.set_name(name);
msg.set_id(model->GetId());
msgs::Link *linkMsg = msg.add_link();
linkMsg->set_id(link->GetId());
linkMsg->set_name(link->GetScopedName());
bool newGravityMode = false;
EXPECT_TRUE(newGravityMode != link->GetGravityMode());
linkMsg->set_gravity(newGravityMode);
modelPub->Publish(msg);
int sleep = 0;
int maxSleep = 50;
while (link->GetGravityMode() != newGravityMode && sleep < maxSleep)
{
world->Step(1);
common::Time::MSleep(100);
sleep++;
}
ASSERT_TRUE(link->GetGravityMode() == newGravityMode);
}
// TODO Bullet and DART currently fail this test
if (_physicsEngine != "bullet" && _physicsEngine != "dart")
{
// change kinematic mode and verify the msg gets through
msgs::Model msg;
msg.set_name(name);
msg.set_id(model->GetId());
msgs::Link *linkMsg = msg.add_link();
linkMsg->set_id(link->GetId());
linkMsg->set_name(link->GetScopedName());
bool newKinematicMode = true;
EXPECT_TRUE(newKinematicMode != link->GetKinematic());
linkMsg->set_kinematic(newKinematicMode);
modelPub->Publish(msg);
int sleep = 0;
int maxSleep = 50;
while (link->GetKinematic() != newKinematicMode && sleep < maxSleep)
{
world->Step(1);
common::Time::MSleep(100);
sleep++;
}
EXPECT_TRUE(link->GetKinematic() == newKinematicMode);
}
else
{
gzerr << "Skipping LinkProperties's kinematic test for "
<< _physicsEngine << std::endl;
}
{
// change self collide mode and verify the msg gets through
msgs::Model msg;
msg.set_name(name);
msg.set_id(model->GetId());
msgs::Link *linkMsg = msg.add_link();
linkMsg->set_id(link->GetId());
linkMsg->set_name(link->GetName());
bool newSelfCollideMode = true;
EXPECT_TRUE(newSelfCollideMode != link->GetSelfCollide());
linkMsg->set_self_collide(newSelfCollideMode);
modelPub->Publish(msg);
int sleep = 0;
int maxSleep = 50;
while (link->GetSelfCollide() != newSelfCollideMode && sleep < maxSleep)
{
world->Step(1);
common::Time::MSleep(100);
sleep++;
}
EXPECT_TRUE(link->GetSelfCollide() == newSelfCollideMode);
}
}
/////////////////////////////////////////////////
void PhysicsMsgsTest::LinkPose(const std::string &_physicsEngine)
{
Load("worlds/multilink_shape.world", true, _physicsEngine);
physics::WorldPtr world = physics::get_world("default");
ASSERT_TRUE(world != NULL);
// set gravity to zero
physics::PhysicsEnginePtr physics = world->GetPhysicsEngine();
ASSERT_TRUE(physics != NULL);
EXPECT_EQ(physics->GetType(), _physicsEngine);
world->SetGravity(ignition::math::Vector3d::Zero);
// advertise on "~/model/modify"
transport::PublisherPtr modelPub =
this->node->Advertise<msgs::Model>("~/model/modify");
double z0 = 5;
// list of poses to move to
std::vector<math::Pose> poses;
poses.push_back(math::Pose(5, 0, z0, 0, 0, 0));
poses.push_back(math::Pose(0, 8, z0, 0, 0, 0));
poses.push_back(math::Pose(-99, 0, z0, 0, 0, 0));
poses.push_back(math::Pose(0, 999, z0, 0, 0, 0));
poses.push_back(math::Pose(123.456, 456.123, z0*10, 0.1, -0.2, 0.3));
poses.push_back(math::Pose(-123.456, 456.123, z0*10, 0.2, 0.4, -0.6));
poses.push_back(math::Pose(123.456, -456.123, z0*10, 0.3, -0.6, 0.9));
poses.push_back(math::Pose(-123.456, -456.123, z0*10, -0.4, 0.8, -1.2));
std::string name = "multilink";
physics::ModelPtr model = world->GetModel(name);
ASSERT_TRUE(model != NULL);
{
for (unsigned int i = 0; i < model->GetLinks().size(); ++i)
{
physics::LinkPtr link = model->GetLinks()[i];
ASSERT_TRUE(link != NULL);
if (link->IsCanonicalLink())
continue;
for (std::vector<math::Pose>::iterator iter = poses.begin();
iter != poses.end(); ++iter)
{
msgs::Model msg;
msg.set_name(name);
msg.set_id(model->GetId());
msgs::Link *linkMsg = msg.add_link();
linkMsg->set_id(link->GetId());
linkMsg->set_name(link->GetScopedName());
msgs::Set(linkMsg->mutable_pose(), (*iter).Ign());
modelPub->Publish(msg);
int sleep = 0;
int maxSleep = 50;
while (*iter != link->GetRelativePose() && sleep < maxSleep)
{
world->Step(1);
common::Time::MSleep(1);
}
// Take a few steps to verify the correct link pose.
// dart has a failure mode that was not exposed without
// this change to the test.
world->Step(10);
EXPECT_EQ(*iter, link->GetRelativePose());
}
}
}
}
////////////////////////////////////////////////////////////////////////
// SimpleShapeResize: (Test adapted from PhysicsTest::SpawnDrop)
// Load a world, check that gravity points along z axis, spawn simple
// shapes (box, sphere, cylinder), resize them to be smaller, verify that they
// then start falling.
////////////////////////////////////////////////////////////////////////
void PhysicsMsgsTest::SimpleShapeResize(const std::string &_physicsEngine)
{
if (_physicsEngine == "dart")
{
gzerr << "Aborting test for DART, see issue #1175.\n";
return;
}
// load an empty world
Load("worlds/empty.world", true, _physicsEngine);
physics::WorldPtr world = physics::get_world("default");
ASSERT_TRUE(world != NULL);
// check the gravity vector
physics::PhysicsEnginePtr physics = world->GetPhysicsEngine();
ASSERT_TRUE(physics != NULL);
EXPECT_EQ(physics->GetType(), _physicsEngine);
math::Vector3 g = physics->GetGravity();
// Assume gravity vector points down z axis only.
EXPECT_EQ(g.x, 0);
EXPECT_EQ(g.y, 0);
EXPECT_LE(g.z, -9.8);
// get physics time step
double dt = physics->GetMaxStepSize();
EXPECT_GT(dt, 0);
// spawn some simple shapes with unit size
double z0 = 0.5;
std::map<std::string, math::Vector3> modelPos;
modelPos["test_box"] = math::Vector3(0, 0, z0);
modelPos["test_sphere"] = math::Vector3(4, 0, z0);
modelPos["test_cylinder"] = math::Vector3(8, 0, z0);
modelPos["test_non_unit_box"] = math::Vector3(16, 0, z0);
std::map<std::string, math::Vector3> modelSize;
modelSize["test_box"] = math::Vector3::One;
modelSize["test_sphere"] = math::Vector3::One;
modelSize["test_cylinder"] = math::Vector3::One;
modelSize["test_non_unit_box"] = math::Vector3(3, 8, 1.0);
SpawnBox("test_box", math::Vector3(1, 1, 1), modelPos["test_box"],
math::Vector3::Zero);
SpawnSphere("test_sphere", modelPos["test_sphere"], math::Vector3::Zero);
SpawnCylinder("test_cylinder", modelPos["test_cylinder"],
math::Vector3::Zero);
SpawnBox("test_non_unit_box", modelSize["test_non_unit_box"],
modelPos["test_non_unit_box"], math::Vector3::Zero);
// spawn another set of shapes and use messages to resize these
modelPos["test_box2"] = math::Vector3(0, 9, z0);
modelPos["test_sphere2"] = math::Vector3(4, 9, z0);
modelPos["test_cylinder2"] = math::Vector3(8, 9, z0);
modelSize["test_box2"] = math::Vector3::One;
modelSize["test_sphere2"] = math::Vector3::One;
modelSize["test_cylinder2"] = math::Vector3::One;
SpawnBox("test_box2", math::Vector3(1, 1, 1), modelPos["test_box2"],
math::Vector3::Zero);
SpawnSphere("test_sphere2", modelPos["test_sphere2"], math::Vector3::Zero);
SpawnCylinder("test_cylinder2", modelPos["test_cylinder2"],
math::Vector3::Zero);
// advertise on "~/model/modify" to generate resize messages
transport::PublisherPtr modelPub =
this->node->Advertise<msgs::Model>("~/model/modify");
int steps = 2;
physics::ModelPtr model;
math::Pose pose1, pose2;
math::Vector3 vel1, vel2;
double x0, y0;
// Allow objects to settle on ground_plane
world->Step(100);
// Verify the initial model pose is where we set it to be.
for (auto const &iter : modelPos)
{
std::string name = iter.first;
// Make sure the model is loaded
model = world->GetModel(name);
EXPECT_TRUE(model != NULL);
pose1 = model->GetWorldPose();
x0 = modelPos[name].x;
y0 = modelPos[name].y;
EXPECT_NEAR(pose1.pos.x, x0, PHYSICS_TOL);
EXPECT_NEAR(pose1.pos.y, y0, PHYSICS_TOL);
EXPECT_NEAR(pose1.pos.z, z0, PHYSICS_TOL);
}
// resize model to half of it's size
double scaleFactor = 0.5;
for (auto const &iter : modelPos)
{
std::string name = iter.first;
model = world->GetModel(name);
if (*(name.rbegin()) == '2')
{
// Use a message to resize this one
msgs::Model msg;
msg.set_name(name);
msg.set_id(model->GetId());
msgs::Set(msg.mutable_scale(),
scaleFactor * ignition::math::Vector3d::One);
modelPub->Publish(msg);
}
else
{
// Use physics API to resize
model->SetScale(scaleFactor * ignition::math::Vector3d::One);
}
}
// Predict time of contact with ground plane.
double tHit = sqrt(2*(z0-0.5*scaleFactor) / (-g.z));
// Time to advance, allow 0.5 s settling time.
// This assumes inelastic collisions with the ground.
double dtHit = tHit+0.5 - world->GetSimTime().Double();
steps = ceil(dtHit / dt);
EXPECT_GT(steps, 0);
world->Step(steps);
// Issue #856, simbody doesn't support shape resizes.
if (_physicsEngine == "simbody")
{
gzerr << "Aborting test since simbody doesn't support shape resizes (#856)"
<< std::endl;
return;
}
// This loop checks the velocity and pose of each model 0.5 seconds
// after the time of predicted ground contact. The pose is expected to be
// underneath the initial pose.
for (auto const &iter : modelPos)
{
std::string name = iter.first;
// Make sure the model is loaded
model = world->GetModel(name);
if (model != NULL)
{
gzdbg << "Check ground contact of model " << name << '\n';
// Check that model is resting on ground
pose1 = model->GetWorldPose();
x0 = modelPos[name].x;
y0 = modelPos[name].y;
double xTolerance = PHYSICS_TOL;
double yTolerance = PHYSICS_TOL;
#ifdef HAVE_BULLET
if (_physicsEngine == "bullet" && sizeof(btScalar) == 4)
{
if (name.find("test_box") != std::string::npos)
{
xTolerance *= 1.6;
yTolerance *= 2.3;
}
}
#endif
EXPECT_NEAR(pose1.pos.x, x0, xTolerance);
EXPECT_NEAR(pose1.pos.y, y0, yTolerance);
EXPECT_NEAR(pose1.pos.z, 0.5*scaleFactor, PHYSICS_TOL);
}
else
{
gzerr << "Error loading model " << name << '\n';
EXPECT_TRUE(model != NULL);
}
}
// verify geom msgs contain resized values.
for (auto const &iter : modelPos)
{
std::string name = iter.first;
model = world->GetModel(name);
msgs::Model modelMsg;
model->FillMsg(modelMsg);
EXPECT_EQ(msgs::ConvertIgn(modelMsg.scale()),
scaleFactor * ignition::math::Vector3d::One);
for (int i = 0; i < modelMsg.link_size(); ++i)
{
msgs::Link linkMsg = modelMsg.link(i);
// verify visual geom msgs
for (int j = 0; j < linkMsg.visual_size(); ++j)
{
msgs::Visual visualMsg = linkMsg.visual(j);
msgs::Geometry geomMsg = visualMsg.geometry();
if (geomMsg.has_box())
{
EXPECT_EQ(msgs::ConvertIgn(geomMsg.box().size()),
modelSize[name].Ign() * scaleFactor);
}
else if (geomMsg.has_sphere())
{
EXPECT_DOUBLE_EQ(geomMsg.sphere().radius(),
modelSize[name].x * 0.5 * scaleFactor);
}
else if (geomMsg.has_cylinder())
{
EXPECT_DOUBLE_EQ(geomMsg.cylinder().radius(),
modelSize[name].x * 0.5 * scaleFactor);
EXPECT_DOUBLE_EQ(geomMsg.cylinder().length(),
modelSize[name].z * scaleFactor);
}
}
// verify collision geom msgs
for (int j = 0; j < linkMsg.collision_size(); ++j)
{
msgs::Collision collisionMsg = linkMsg.collision(j);
msgs::Geometry geomMsg = collisionMsg.geometry();
if (geomMsg.has_box())
{
EXPECT_EQ(msgs::ConvertIgn(geomMsg.box().size()),
modelSize[name].Ign() * scaleFactor);
}
else if (geomMsg.has_sphere())
{
EXPECT_DOUBLE_EQ(geomMsg.sphere().radius(),
modelSize[name].x * 0.5 * scaleFactor);
}
else if (geomMsg.has_cylinder())
{
EXPECT_DOUBLE_EQ(geomMsg.cylinder().radius(),
modelSize[name].x * 0.5 * scaleFactor);
EXPECT_DOUBLE_EQ(geomMsg.cylinder().length(),
modelSize[name].z * scaleFactor);
}
}
}
}
// verify geom sdfs contain resized values.
for (auto const &iter : modelPos)
{
std::string name = iter.first;
model = world->GetModel(name);
sdf::ElementPtr modelElem = model->GetSDF();
EXPECT_TRUE(modelElem->HasElement("link"));
sdf::ElementPtr linkElem = modelElem->GetElement("link");
while (linkElem)
{
// verify visual geom sdf
EXPECT_TRUE(linkElem->HasElement("visual"));
sdf::ElementPtr visualElem = linkElem->GetElement("visual");
EXPECT_TRUE(visualElem->HasElement("geometry"));
sdf::ElementPtr visualGeomElem = visualElem->GetElement("geometry");
if (visualGeomElem->HasElement("box"))
{
sdf::ElementPtr boxElem = visualGeomElem->GetElement("box");
math::Vector3 size = boxElem->Get<math::Vector3>("size");
EXPECT_EQ(size, modelSize[name] * scaleFactor);
}
else if (visualGeomElem->HasElement("sphere"))
{
sdf::ElementPtr sphereElem = visualGeomElem->GetElement("sphere");
double radius = sphereElem->Get<double>("radius");
EXPECT_EQ(radius, modelSize[name].x * 0.5 * scaleFactor);
}
else if (visualGeomElem->HasElement("cylinder"))
{
sdf::ElementPtr cylinderElem = visualGeomElem->GetElement("cylinder");
double radius = cylinderElem->Get<double>("radius");
EXPECT_EQ(radius, modelSize[name].x * 0.5 * scaleFactor);
double length = cylinderElem->Get<double>("length");
EXPECT_EQ(length, modelSize[name].z * scaleFactor);
}
// verify collision geom sdf
EXPECT_TRUE(linkElem->HasElement("collision"));
sdf::ElementPtr collisionElem = linkElem->GetElement("collision");
EXPECT_TRUE(collisionElem->HasElement("geometry"));
sdf::ElementPtr collisionGeomElem = collisionElem->GetElement("geometry");
if (collisionGeomElem->HasElement("box"))
{
sdf::ElementPtr boxElem = collisionGeomElem->GetElement("box");
math::Vector3 size = boxElem->Get<math::Vector3>("size");
EXPECT_EQ(size, modelSize[name] * scaleFactor);
}
else if (collisionGeomElem->HasElement("sphere"))
{
sdf::ElementPtr sphereElem = collisionGeomElem->GetElement("sphere");
double radius = sphereElem->Get<double>("radius");
EXPECT_EQ(radius, modelSize[name].x * 0.5 * scaleFactor);
}
else if (collisionGeomElem->HasElement("cylinder"))
{
sdf::ElementPtr cylinderElem =
collisionGeomElem->GetElement("cylinder");
double radius = cylinderElem->Get<double>("radius");
EXPECT_EQ(radius, modelSize[name].x * 0.5 * scaleFactor);
double length = cylinderElem->Get<double>("length");
EXPECT_EQ(length, modelSize[name].z * scaleFactor);
}
linkElem = linkElem->GetNextElement("link");
}
}
}
////////////////////////////////////////////////////////////////////////
void PhysicsMsgsTest::LinkVisualMsg(const std::string &_physicsEngine)
{
// 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;
math::Pose pose(0, 0, 0, 0, 0, 0);
math::Vector3 boxSize(1, 1, 1);
sdfStream << "<sdf version='" << SDF_VERSION << "'>"
<< "<model name ='box_test'>"
<< "<allow_auto_disable>false</allow_auto_disable>"
<< "<link name ='body'>"
<< " <pose>" << pose << "</pose>"
<< " <inertial>"
<< " <mass>4.0</mass>"
<< " <inertia>"
<< " <ixx>0.1667</ixx> <ixy>0.0</ixy> <ixz>0.0</ixz>"
<< " <iyy>0.1667</iyy> <iyz>0.0</iyz>"
<< " <izz>0.1667</izz>"
<< " </inertia>"
<< " </inertial>"
<< " <collision name ='geom'>"
<< " <geometry>"
<< " <box><size>" << boxSize << "</size></box>"
<< " </geometry>"
<< " </collision>"
<< " <visual name ='visual'>"
<< " <geometry>"
<< " <box><size>" << boxSize << "</size></box>"
<< " </geometry>"
<< " </visual>"
<< " <visual name ='visual2'>"
<< " <geometry>"
<< " <box><size>" << boxSize << "</size></box>"
<< " </geometry>"
<< " </visual>"
<< "</link>"
<< "</model>"
<< "</sdf>";
SpawnSDF(sdfStream.str());
physics::ModelPtr model;
model = world->GetModel("box_test");
EXPECT_TRUE(model != NULL);
msgs::Model msg;
model->FillMsg(msg);
EXPECT_EQ(msg.link_size(), 1);
msgs::Link linkMsg = msg.link(0);
// 3 visuals to be created: 1 link body + 2 visuals
EXPECT_EQ(linkMsg.visual_size(), 3);
// verify link body visual
msgs::Visual visualMsg = linkMsg.visual(0);
EXPECT_EQ(visualMsg.name(), "box_test::body");
EXPECT_TRUE(visualMsg.has_type());
EXPECT_EQ(visualMsg.type(), msgs::Visual::LINK);
EXPECT_FALSE(visualMsg.has_geometry());
// verify remaining visual msgs
for (int i = 1; i < linkMsg.visual_size(); ++i)
{
msgs::Visual visualMsg = linkMsg.visual(i);
std::stringstream visName;
visName << "box_test::body::visual";
if (i > 1)
visName << i;
EXPECT_EQ(visualMsg.name(), visName.str());
EXPECT_TRUE(visualMsg.has_type());
EXPECT_EQ(visualMsg.type(), msgs::Visual::VISUAL);
msgs::Geometry geomMsg = visualMsg.geometry();
EXPECT_TRUE(geomMsg.has_box());
EXPECT_EQ(msgs::ConvertIgn(geomMsg.box().size()), boxSize.Ign());
}
}
////////////////////////////////////////////////////////////////////////
void PhysicsMsgsTest::JointMsg(const std::string &_physicsEngine)
{
// 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 ='joint_msg_test'>"
<< "<link name ='link1'>"
<< " <pose>0 0 0 0 0 0</pose>"
<< " <collision name ='geom'>"
<< " <geometry>"
<< " <box><size>1 1 1</size></box>"
<< " </geometry>"
<< " </collision>"
<< "</link>"
<< "<link name ='link2'>"
<< " <pose>0 1 0 0 0 0</pose>"
<< " <collision name ='geom'>"
<< " <geometry>"
<< " <box><size>1 1 1</size></box>"
<< " </geometry>"
<< " </collision>"
<< "</link>"
<< "<link name ='link3'>"
<< " <pose>0 0 1 0 0 0</pose>"
<< " <collision name ='geom'>"
<< " <geometry>"
<< " <box><size>1 1 1</size></box>"
<< " </geometry>"
<< " </collision>"
<< "</link>"
<< "<joint name='revolute_joint' type='revolute'>"
<< " <parent>link1</parent>"
<< " <child>link2</child>"
<< " <pose>0 0 0 0 0 0</pose>"
<< " <axis>"
<< " <xyz>1 0 0</xyz>"
<< " <use_parent_model_frame>0</use_parent_model_frame>"
<< " <limit>"
<< " <lower>-1</lower>"
<< " <upper>1</upper>"
<< " <effort>-1</effort>"
<< " <velocity>-1</velocity>"
<< " </limit>"
<< " <dynamics>"
<< " <damping>0.2</damping>"
<< " <friction>0.1</friction>"
<< " </dynamics>"
<< " </axis>"
<< "</joint>"
<< "<joint name='screw_joint' type='screw'>"
<< " <parent>link2</parent>"
<< " <child>link3</child>"
<< " <pose>0 0.2 0 0 0 0</pose>"
<< " <axis>"
<< " <xyz>0 1 0</xyz>"
<< " <use_parent_model_frame>0</use_parent_model_frame>"
<< " <limit>"
<< " <lower>-2</lower>"
<< " <upper>2</upper>"
<< " <effort>-0.7</effort>"
<< " <velocity>-1</velocity>"
<< " </limit>"
<< " <dynamics>"
<< " <damping>0.3</damping>"
<< " <friction>0.2</friction>"
<< " </dynamics>"
<< " </axis>"
<< " <thread_pitch>2</thread_pitch>"
<< "</joint>"
<< "<joint name='gearbox_joint' type='gearbox'>"
<< " <parent>link3</parent>"
<< " <child>link1</child>"
<< " <pose>0 0 0.1 0 0 0</pose>"
<< " <axis>"
<< " <xyz>1 0 0</xyz>"
<< " <use_parent_model_frame>0</use_parent_model_frame>"
<< " <limit>"
<< " <lower>-1e6</lower>"
<< " <upper>1e6</upper>"
<< " <effort>-0.9</effort>"
<< " <velocity>-0.1</velocity>"
<< " </limit>"
<< " <dynamics>"
<< " <damping>0.4</damping>"
<< " <friction>0.11</friction>"
<< " </dynamics>"
<< " </axis>"
<< " <axis2>"
<< " <xyz>0 0 1</xyz>"
<< " <use_parent_model_frame>1</use_parent_model_frame>"
<< " <limit>"
<< " <lower>-1e3</lower>"
<< " <upper>1e3</upper>"
<< " <effort>-0.8</effort>"
<< " <velocity>-0.2</velocity>"
<< " </limit>"
<< " <dynamics>"
<< " <damping>0.23</damping>"
<< " <friction>0.32</friction>"
<< " </dynamics>"
<< " </axis2>"
<< " <gearbox_ratio>6.6</gearbox_ratio>"
<< " <gearbox_reference_body>link_2</gearbox_reference_body>"
<< "</joint>"
<< "</model>"
<< "</sdf>";
SpawnSDF(sdfStream.str());
physics::ModelPtr model;
model = world->GetModel("joint_msg_test");
EXPECT_TRUE(model != NULL);
msgs::Model msg;
model->FillMsg(msg);
// only ode supports gearbox joint
int jointSize = 2;
if (_physicsEngine == "ode")
jointSize = 3;
EXPECT_EQ(msg.joint_size(), jointSize);
{
msgs::Joint jointMsg = msg.joint(0);
EXPECT_EQ(jointMsg.name(), "joint_msg_test::revolute_joint");
EXPECT_EQ(jointMsg.parent(), "joint_msg_test::link1");
EXPECT_EQ(jointMsg.child(), "joint_msg_test::link2");
EXPECT_EQ(msgs::ConvertIgn(jointMsg.pose()),
ignition::math::Pose3d(0, 0, 0, 0, 0, 0));
EXPECT_TRUE(jointMsg.has_axis1());
EXPECT_TRUE(!jointMsg.has_axis2());
msgs::Axis axis1Msg = jointMsg.axis1();
EXPECT_EQ(msgs::ConvertIgn(axis1Msg.xyz()),
ignition::math::Vector3d(1, 0, 0));
EXPECT_EQ(axis1Msg.use_parent_model_frame(), false);
EXPECT_DOUBLE_EQ(axis1Msg.limit_lower(), -1);
EXPECT_DOUBLE_EQ(axis1Msg.limit_upper(), 1);
EXPECT_DOUBLE_EQ(axis1Msg.limit_effort(), -1);
EXPECT_DOUBLE_EQ(axis1Msg.limit_velocity(), -1);
EXPECT_DOUBLE_EQ(axis1Msg.damping(), 0.2);
// only ode and bullet return correct hinge friction param value
if (_physicsEngine == "ode" || _physicsEngine == "bullet")
EXPECT_FLOAT_EQ(axis1Msg.friction(), 0.1);
}
{
msgs::Joint jointMsg = msg.joint(1);
EXPECT_EQ(jointMsg.name(), "joint_msg_test::screw_joint");
EXPECT_EQ(jointMsg.parent(), "joint_msg_test::link2");
EXPECT_EQ(jointMsg.child(), "joint_msg_test::link3");
EXPECT_EQ(msgs::ConvertIgn(jointMsg.pose()),
ignition::math::Pose3d(0, 0.2, 0, 0, 0, 0));
EXPECT_TRUE(jointMsg.has_axis1());
EXPECT_TRUE(jointMsg.has_axis2());
msgs::Axis axis1Msg = jointMsg.axis1();
EXPECT_EQ(msgs::ConvertIgn(axis1Msg.xyz()),
ignition::math::Vector3d(0, 1, 0));
EXPECT_EQ(axis1Msg.use_parent_model_frame(), false);
EXPECT_DOUBLE_EQ(axis1Msg.limit_lower(), -2);
EXPECT_DOUBLE_EQ(axis1Msg.limit_upper(), 2);
EXPECT_DOUBLE_EQ(axis1Msg.limit_effort(), -0.7);
EXPECT_DOUBLE_EQ(axis1Msg.limit_velocity(), -1);
EXPECT_DOUBLE_EQ(axis1Msg.damping(), 0.3);
// only ode returns correct screw friction param value
if (_physicsEngine == "ode")
EXPECT_DOUBLE_EQ(axis1Msg.friction(), 0.2);
msgs::Joint::Screw screwMsg = jointMsg.screw();
EXPECT_DOUBLE_EQ(screwMsg.thread_pitch(), 2);
}
// only ode supports gearbox joints
if (_physicsEngine == "bullet" || _physicsEngine == "simbody"
|| _physicsEngine == "dart")
return;
{
msgs::Joint jointMsg = msg.joint(2);
EXPECT_EQ(jointMsg.name(), "joint_msg_test::gearbox_joint");
EXPECT_EQ(jointMsg.parent(), "joint_msg_test::link3");
EXPECT_EQ(jointMsg.child(), "joint_msg_test::link1");
EXPECT_EQ(msgs::ConvertIgn(jointMsg.pose()),
ignition::math::Pose3d(0, 0, 0.1, 0, 0, 0));
EXPECT_TRUE(jointMsg.has_axis1());
EXPECT_TRUE(jointMsg.has_axis2());
msgs::Axis axis1Msg = jointMsg.axis1();
EXPECT_EQ(msgs::ConvertIgn(axis1Msg.xyz()),
ignition::math::Vector3d(1, 0, 0));
EXPECT_EQ(axis1Msg.use_parent_model_frame(), false);
EXPECT_DOUBLE_EQ(axis1Msg.limit_lower(), -1e6);
EXPECT_DOUBLE_EQ(axis1Msg.limit_upper(), 1e6);
EXPECT_DOUBLE_EQ(axis1Msg.limit_effort(), -0.9);
EXPECT_EQ(axis1Msg.limit_velocity(), -0.1);
EXPECT_DOUBLE_EQ(axis1Msg.damping(), 0.4);
// gearbox friction param does not return correct value
// EXPECT_DOUBLE_EQ(axis1Msg.friction(), 0.11);
msgs::Axis axis2Msg = jointMsg.axis2();
EXPECT_EQ(msgs::ConvertIgn(axis2Msg.xyz()),
ignition::math::Vector3d(0, 0, 1));
EXPECT_EQ(axis2Msg.use_parent_model_frame(), true);
EXPECT_DOUBLE_EQ(axis2Msg.limit_lower(), -1e3);
EXPECT_DOUBLE_EQ(axis2Msg.limit_upper(), 1e3);
EXPECT_DOUBLE_EQ(axis2Msg.limit_effort(), -0.8);
EXPECT_DOUBLE_EQ(axis2Msg.limit_velocity(), -0.2);
EXPECT_DOUBLE_EQ(axis2Msg.damping(), 0.23);
// gearbox friction param does not return correct value
// EXPECT_DOUBLE_EQ(axis2Msg.friction(), 0.32);
msgs::Joint::Gearbox gearboxMsg = jointMsg.gearbox();
EXPECT_EQ(gearboxMsg.gearbox_reference_body(),
"link_2");
EXPECT_DOUBLE_EQ(gearboxMsg.gearbox_ratio(), 6.6);
}
}
/////////////////////////////////////////////////
TEST_P(PhysicsMsgsTest, SetGravity)
{
SetGravity(GetParam());
}
/////////////////////////////////////////////////
TEST_P(PhysicsMsgsTest, MoveTool)
{
MoveTool(GetParam());
}
/////////////////////////////////////////////////
TEST_P(PhysicsMsgsTest, LinkProperties)
{
LinkProperties(GetParam());
}
/////////////////////////////////////////////////
TEST_P(PhysicsMsgsTest, LinkPose)
{
LinkPose(GetParam());
}
/////////////////////////////////////////////////
TEST_P(PhysicsMsgsTest, SimpleShapeResize)
{
SimpleShapeResize(GetParam());
}
/////////////////////////////////////////////////
TEST_P(PhysicsMsgsTest, LinkVisualMsg)
{
LinkVisualMsg(GetParam());
}
/////////////////////////////////////////////////
TEST_P(PhysicsMsgsTest, JointMsg)
{
JointMsg(GetParam());
}
INSTANTIATE_TEST_CASE_P(PhysicsEngines, PhysicsMsgsTest,
PHYSICS_ENGINE_VALUES);
/////////////////////////////////////////////////
int main(int argc, char **argv)
{
::testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}
Reference in New Issue View Git Blame Copy Permalink