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

1518 lines
49 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 <map>
#include <string>
#include <vector>
#include <ignition/math/Rand.hh>
#include "gazebo/test/ServerFixture.hh"
#include "gazebo/physics/physics.hh"
#include "SimplePendulumIntegrator.hh"
#include "gazebo/msgs/msgs.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 PhysicsTest : public ServerFixture,
public testing::WithParamInterface<const char*>
{
public: void InelasticCollision(const std::string &_physicsEngine);
public: void EmptyWorld(const std::string &_physicsEngine);
public: void SpawnDrop(const std::string &_physicsEngine);
public: void SpawnDropCoGOffset(const std::string &_physicsEngine);
public: void SphereAtlasLargeError(const std::string &_physicsEngine);
public: void CollisionFiltering(const std::string &_physicsEngine);
public: void JointDampingTest(const std::string &_physicsEngine);
public: void DropStuff(const std::string &_physicsEngine);
};
////////////////////////////////////////////////////////////////////////
// EmptyWorld:
// Load a world, take a few steps, and verify that time is increasing.
// This is the most basic physics engine test.
////////////////////////////////////////////////////////////////////////
void PhysicsTest::EmptyWorld(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);
// Verify physics engine type
physics::PhysicsEnginePtr physics = world->GetPhysicsEngine();
ASSERT_TRUE(physics != NULL);
EXPECT_EQ(physics->GetType(), _physicsEngine);
// simulate 1 step
world->Step(1);
double t = world->GetSimTime().Double();
// verify that time moves forward
EXPECT_GT(t, 0);
// simulate a few steps
int steps = 20;
world->Step(steps);
double dt = world->GetPhysicsEngine()->GetMaxStepSize();
EXPECT_GT(dt, 0);
t = world->GetSimTime().Double();
EXPECT_GT(t, 0.99*dt*static_cast<double>(steps+1));
}
TEST_P(PhysicsTest, EmptyWorld)
{
EmptyWorld(GetParam());
}
////////////////////////////////////////////////////////////////////////
// SpawnDrop:
// Load a world, check that gravity points along z axis, spawn simple
// shapes (box, sphere, cylinder), verify that they fall and hit the
// ground plane. The test currently assumes inelastic collisions.
////////////////////////////////////////////////////////////////////////
void PhysicsTest::SpawnDrop(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);
// 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 and check to see that they start falling
double z0 = 3;
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_empty"] = math::Vector3(12, 0, z0);
modelPos["link_offset_box"] = math::Vector3(0, 0, z0);
// FIXME Trimesh drop test passes in bullet but fails in ode because
// the mesh bounces to the side when it hits the ground.
// See issue #513. Uncomment test when issue is resolved.
// modelPos["test_trimesh"] = math::Vector3(16, 0, z0);
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);
SpawnEmptyLink("test_empty", modelPos["test_empty"], math::Vector3::Zero);
std::ostringstream linkOffsetStream;
math::Pose linkOffsetPose1(0, 0, z0, 0, 0, 0);
math::Pose linkOffsetPose2(1000, 1000, 0, 0, 0, 0);
math::Vector3 linkOffsetSize(1, 1, 1);
linkOffsetStream << "<sdf version='" << SDF_VERSION << "'>"
<< "<model name ='link_offset_box'>"
<< "<pose>" << linkOffsetPose1 << "</pose>"
<< "<allow_auto_disable>false</allow_auto_disable>"
<< "<link name ='body'>"
<< " <pose>" << linkOffsetPose2 << "</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>" << linkOffsetSize << "</size></box>"
<< " </geometry>"
<< " </collision>"
<< " <visual name ='visual'>"
<< " <geometry>"
<< " <box><size>" << linkOffsetSize << "</size></box>"
<< " </geometry>"
<< " </visual>"
<< "</link>"
<< "</model>"
<< "</sdf>";
SpawnSDF(linkOffsetStream.str());
/// \TODO: bullet needs this to pass
if (physics->GetType() == "bullet")
physics->SetParam("iters", 300);
// std::string trimeshPath =
// "file://media/models/cube_20k/meshes/cube_20k.stl";
// SpawnTrimesh("test_trimesh", trimeshPath, math::Vector3(0.5, 0.5, 0.5),
// modelPos["test_trimesh"], math::Vector3::Zero);
int steps = 2;
physics::ModelPtr model;
math::Pose pose1, pose2;
math::Vector3 vel1, vel2;
double t, x0 = 0;
// This loop steps the world forward and makes sure that each model falls,
// expecting downward z velocity and decreasing z position.
for (std::map<std::string, math::Vector3>::iterator iter = modelPos.begin();
iter != modelPos.end(); ++iter)
{
std::string name = iter->first;
// Make sure the model is loaded
model = world->GetModel(name);
if (model != NULL)
{
gzdbg << "Check freefall of model " << name << '\n';
// Step once and check downward z velocity
world->Step(1);
vel1 = model->GetWorldLinearVel();
t = world->GetSimTime().Double();
EXPECT_EQ(vel1.x, 0);
EXPECT_EQ(vel1.y, 0);
EXPECT_NEAR(vel1.z, g.z*t, -g.z*t*PHYSICS_TOL);
// Need to step at least twice to check decreasing z position
world->Step(steps - 1);
pose1 = model->GetWorldPose();
x0 = modelPos[name].x;
EXPECT_EQ(pose1.pos.x, x0);
EXPECT_EQ(pose1.pos.y, 0);
EXPECT_NEAR(pose1.pos.z, z0 + g.z/2*t*t, (z0+g.z/2*t*t)*PHYSICS_TOL);
// Check once more and just make sure they keep falling
world->Step(steps);
vel2 = model->GetWorldLinearVel();
pose2 = model->GetWorldPose();
EXPECT_LT(vel2.z, vel1.z);
EXPECT_LT(pose2.pos.z, pose1.pos.z);
// if (physics->GetType() == "bullet")
// {
// gzerr << "m[" << model->GetName()
// << "] p[" << model->GetWorldPose()
// << "] v[" << model->GetWorldLinearVel()
// << "]\n";
// gzerr << "wait: ";
// getchar();
// }
}
else
{
gzerr << "Error loading model " << name << '\n';
EXPECT_TRUE(model != NULL);
}
}
// Predict time of contact with ground plane.
double tHit = sqrt(2*(z0-0.5) / (-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);
// debug
// for (int i = 0; i < steps; ++i)
// {
// world->Step(1);
// if (physics->GetType() == "bullet")
// {
// model = world->GetModel("link_offset_box");
// gzerr << "m[" << model->GetName()
// << "] i[" << i << "/" << steps
// << "] pm[" << model->GetWorldPose()
// << "] pb[" << model->GetLink("body")->GetWorldPose()
// << "] v[" << model->GetWorldLinearVel()
// << "]\n";
// if (model->GetWorldPose().pos.z < 0.6)
// {
// gzerr << "wait: ";
// getchar();
// }
// }
// }
// This loop checks the velocity and pose of each model 0.5 seconds
// after the time of predicted ground contact. The velocity is expected
// to be small, and the pose is expected to be underneath the initial pose.
for (std::map<std::string, math::Vector3>::iterator iter = modelPos.begin();
iter != modelPos.end(); ++iter)
{
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 velocity is small
vel1 = model->GetWorldLinearVel();
t = world->GetSimTime().Double();
EXPECT_NEAR(vel1.x, 0, PHYSICS_TOL);
EXPECT_NEAR(vel1.y, 0, PHYSICS_TOL);
if (name == "test_empty")
EXPECT_NEAR(vel1.z, g.z*t, -g.z*t*PHYSICS_TOL);
else
EXPECT_NEAR(vel1.z, 0, PHYSICS_TOL);
// Check that model is resting on ground
pose1 = model->GetWorldPose();
x0 = modelPos[name].x;
double posTolerance = PHYSICS_TOL;
#ifdef HAVE_BULLET
if (_physicsEngine == "bullet" && sizeof(btScalar) == 4)
{
posTolerance *= 1400;
}
#endif
EXPECT_NEAR(pose1.pos.x, x0, posTolerance);
EXPECT_NEAR(pose1.pos.y, 0, posTolerance);
// debug
// if (physics->GetType() == "bullet")
// {
// gzerr << "m[" << model->GetName()
// << "] p[" << model->GetWorldPose()
// << "] v[" << model->GetWorldLinearVel()
// << "]\n";
// gzerr << "wait: ";
// getchar();
// }
if (name == "test_empty")
{
EXPECT_NEAR(pose1.pos.z, z0+g.z/2*t*t,
fabs((z0+g.z/2*t*t)*PHYSICS_TOL));
}
else
EXPECT_NEAR(pose1.pos.z, 0.5, PHYSICS_TOL);
}
else
{
gzerr << "Error loading model " << name << '\n';
EXPECT_TRUE(model != NULL);
}
}
// Compute and check link pose of link_offset_box
gzdbg << "Check link pose of link_offset_box\n";
model = world->GetModel("link_offset_box");
ASSERT_TRUE(model != NULL);
physics::LinkPtr link = model->GetLink();
ASSERT_TRUE(link != NULL);
// relative pose of link in linkOffsetPose2
for (int i = 0; i < 20; ++i)
{
pose1 = model->GetWorldPose();
pose2 = linkOffsetPose2 + pose1;
EXPECT_NEAR(pose2.pos.x, linkOffsetPose2.pos.x, PHYSICS_TOL);
EXPECT_NEAR(pose2.pos.y, linkOffsetPose2.pos.y, PHYSICS_TOL);
EXPECT_NEAR(pose2.pos.z, 0.5, PHYSICS_TOL);
world->Step(1);
}
}
TEST_P(PhysicsTest, SpawnDrop)
{
SpawnDrop(GetParam());
}
////////////////////////////////////////////////////////////////////////
// SpawnDropCoGOffset:
// Load a world, check that gravity points along z axis, spawn several
// spheres of varying radii and center of gravity (cg) location.
// sphere1: smaller radius, centered cg
// sphere2: larger radius, centered cg
// sphere3: larger radius, lowered cg
// sphere4: larger radius, raised cg
// sphere5: larger radius, y offset cg
// sphere6: larger radius, x offset cg
// sphere7: larger radius, 45 deg offset cg
// sphere8: larger radius, -30 deg offset cg
// The bottom of each sphere is at the same height, and it is verified
// that they hit the ground at the same time. Also, sphere5 should start
// rolling to the side when it hits the ground.
////////////////////////////////////////////////////////////////////////
void PhysicsTest::SpawnDropCoGOffset(const std::string &_physicsEngine)
{
if (_physicsEngine == "dart")
{
gzerr << "Skipping SpawnDropCoGOffset for physics engine ["
<< _physicsEngine
<< "] due to issue #1209.\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_LT(g.z, 0);
// get physics time step
double dt = physics->GetMaxStepSize();
EXPECT_GT(dt, 0);
// spawn some spheres and check to see that they start falling
double z0 = 3;
double r1 = 0.5, r2 = 1.5;
math::Vector3 v30 = math::Vector3::Zero;
math::Vector3 cog;
math::Angle angle;
std::vector<std::string> modelNames;
std::vector<double> x0s;
std::vector<double> y0s;
std::vector<double> radii;
std::vector<math::Vector3> cogs;
// sphere1 and sphere2 have c.g. at center of sphere, different sizes
modelNames.push_back("small_centered_sphere");
x0s.push_back(0);
y0s.push_back(0);
radii.push_back(r1);
cogs.push_back(v30);
modelNames.push_back("large_centered_sphere");
x0s.push_back(4);
y0s.push_back(0);
radii.push_back(r2);
cogs.push_back(v30);
// sphere3 has c.g. below the center
modelNames.push_back("lowered_cog_sphere");
x0s.push_back(8);
y0s.push_back(0);
radii.push_back(r2);
cogs.push_back(math::Vector3(0, 0, -r1));
// sphere4 has c.g. above the center
modelNames.push_back("raised_cog_sphere");
x0s.push_back(-4);
y0s.push_back(0);
radii.push_back(r2);
cogs.push_back(math::Vector3(0, 0, r1));
// sphere5 has c.g. to the side along y axis; it will roll
modelNames.push_back("cog_y_offset_sphere");
x0s.push_back(-8);
y0s.push_back(0);
radii.push_back(r2);
cogs.push_back(math::Vector3(0, r1, 0));
// sphere6 has c.g. to the side along x axis; it will roll
modelNames.push_back("cog_x_offset_sphere");
x0s.push_back(15);
y0s.push_back(0);
radii.push_back(r2);
cogs.push_back(math::Vector3(r1, 0, 0));
// sphere7 has c.g. to the side diagonally; it will roll
modelNames.push_back("cog_xy_45deg_offset_sphere");
x0s.push_back(0);
y0s.push_back(8);
radii.push_back(r2);
angle.SetFromDegree(45);
cogs.push_back(math::Vector3(r1*cos(angle.Radian()),
r1*sin(angle.Radian()), 0));
// sphere8 has c.g. to the side diagonally; it will roll
modelNames.push_back("cog_xy_-30deg_offset_sphere");
x0s.push_back(0);
y0s.push_back(-8);
radii.push_back(r2);
angle.SetFromDegree(-30);
cogs.push_back(math::Vector3(r1*cos(angle.Radian()),
r1*sin(angle.Radian()), 0));
unsigned int i;
for (i = 0; i < modelNames.size(); ++i)
{
SpawnSphere(modelNames[i], math::Vector3(x0s[i], y0s[i], z0+radii[i]),
v30, cogs[i], radii[i]);
}
int steps = 2;
physics::ModelPtr model;
math::Pose pose1, pose2;
math::Vector3 vel1, vel2;
double t, x0 = 0, y0 = 0, radius;
// This loop steps the world forward and makes sure that each model falls,
// expecting downward z velocity and decreasing z position.
for (i = 0; i < modelNames.size(); ++i)
{
// Make sure the model is loaded
model = world->GetModel(modelNames[i]);
x0 = x0s[i];
y0 = y0s[i];
radius = radii[i];
if (model != NULL)
{
gzdbg << "Check freefall of model " << modelNames[i] << '\n';
// Step once and check downward z velocity
world->Step(1);
vel1 = model->GetWorldLinearVel();
t = world->GetSimTime().Double();
EXPECT_NEAR(vel1.x, 0, 1e-16);
EXPECT_NEAR(vel1.y, 0, 1e-16);
EXPECT_NEAR(vel1.z, g.z*t, -g.z*t*PHYSICS_TOL);
// Need to step at least twice to check decreasing z position
world->Step(steps - 1);
pose1 = model->GetWorldPose();
EXPECT_NEAR(pose1.pos.x, x0, PHYSICS_TOL*PHYSICS_TOL);
EXPECT_NEAR(pose1.pos.y, y0, PHYSICS_TOL*PHYSICS_TOL);
EXPECT_NEAR(pose1.pos.z, z0+radius + g.z/2*t*t,
(z0+radius+g.z/2*t*t)*PHYSICS_TOL);
// Check once more and just make sure they keep falling
world->Step(steps);
vel2 = model->GetWorldLinearVel();
pose2 = model->GetWorldPose();
EXPECT_LT(vel2.z, vel1.z);
EXPECT_LT(pose2.pos.z, pose1.pos.z);
}
else
{
gzerr << "Error loading model " << modelNames[i] << '\n';
EXPECT_TRUE(model != NULL);
}
}
// Predict time of contact with ground plane.
double tHit = sqrt(2*(z0-0.5) / (-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);
// This loop checks the velocity and pose of each model 0.5 seconds
// after the time of predicted ground contact. Except for sphere5,
// the velocity is expected to be small, and the pose is expected
// to be underneath the initial pose. sphere5 is expected to be rolling.
for (i = 0; i < modelNames.size(); ++i)
{
// Make sure the model is loaded
model = world->GetModel(modelNames[i]);
x0 = x0s[i];
y0 = y0s[i];
radius = radii[i];
cog = cogs[i];
if (model != NULL)
{
gzdbg << "Check ground contact and roll without slip of model "
<< modelNames[i] << '\n';
// Check that velocity is small
vel1 = model->GetWorldLinearVel();
vel2 = model->GetWorldAngularVel();
// vertical component of linear and angular velocity should be small
EXPECT_NEAR(vel1.z, 0, PHYSICS_TOL);
EXPECT_NEAR(vel2.z, 0, PHYSICS_TOL);
// expect small values for directions with no offset
if (cog.x == 0)
{
EXPECT_NEAR(vel1.x, 0, PHYSICS_TOL);
EXPECT_NEAR(vel2.y, 0, PHYSICS_TOL);
}
// expect rolling in direction of cog offset
else
{
EXPECT_GT(vel1.x*cog.x, 0.2*cog.x*cog.x);
EXPECT_GT(vel2.y*cog.x, 0.2*cog.x*cog.x);
}
if (cog.y == 0)
{
EXPECT_NEAR(vel1.y, 0, PHYSICS_TOL);
EXPECT_NEAR(vel2.x, 0, PHYSICS_TOL);
}
else
{
EXPECT_GT(vel1.y*cog.y, 0.2*cog.y*cog.y);
EXPECT_LT(vel2.x*cog.y, -0.2*cog.y*cog.y);
}
// Expect roll without slip
EXPECT_NEAR(vel1.x, vel2.y*radius, PHYSICS_TOL);
EXPECT_NEAR(vel1.y, -vel2.x*radius, PHYSICS_TOL);
// Use GetWorldLinearVel with global offset to check roll without slip
// Expect small linear velocity at contact point
math::Vector3 vel3 = model->GetLink()->GetWorldLinearVel(
math::Vector3(0, 0, -radius), math::Quaternion(0, 0, 0));
EXPECT_NEAR(vel3.x, 0, PHYSICS_TOL);
EXPECT_NEAR(vel3.y, 0, PHYSICS_TOL);
EXPECT_NEAR(vel3.z, 0, PHYSICS_TOL);
// Expect speed at top of sphere to be double the speed at center
math::Vector3 vel4 = model->GetLink()->GetWorldLinearVel(
math::Vector3(0, 0, radius), math::Quaternion(0, 0, 0));
EXPECT_NEAR(vel4.y, 2*vel1.y, PHYSICS_TOL);
EXPECT_NEAR(vel4.x, 2*vel1.x, PHYSICS_TOL);
EXPECT_NEAR(vel4.z, 0, PHYSICS_TOL);
// Check that model is resting on ground
pose1 = model->GetWorldPose();
EXPECT_NEAR(pose1.pos.z, radius, PHYSICS_TOL);
// expect no pose change for directions with no offset
if (cog.x == 0)
{
EXPECT_NEAR(pose1.pos.x, x0, PHYSICS_TOL);
}
// expect rolling in direction of cog offset
else
{
EXPECT_GT((pose1.pos.x-x0) * cog.x, cog.x * cog.x);
}
// expect no pose change for directions with no offset
if (cog.y == 0)
{
EXPECT_NEAR(pose1.pos.y, y0, PHYSICS_TOL);
}
// expect rolling in direction of cog offset
else
{
EXPECT_GT((pose1.pos.y-y0) * cog.y, cog.y * cog.y);
}
}
else
{
gzerr << "Error loading model " << modelNames[i] << '\n';
EXPECT_TRUE(model != NULL);
}
}
}
TEST_P(PhysicsTest, SpawnDropCoGOffset)
{
SpawnDropCoGOffset(GetParam());
}
//////////////////////////////////////////////////
TEST_F(PhysicsTest, EmptyStates)
{
this->Load("worlds/empty.world");
auto world = physics::get_world("default");
EXPECT_TRUE(world != NULL);
// Current world state
physics::WorldState worldState(world);
auto modelStates = worldState.GetModelStates();
EXPECT_EQ(1u, worldState.GetModelStateCount());
EXPECT_EQ(1u, modelStates.size());
// Model state
auto modelState = modelStates["box"];
EXPECT_EQ(ignition::math::Pose3d::Zero, modelState.GetPose().Ign());
}
//////////////////////////////////////////////////
TEST_F(PhysicsTest, StateChange)
{
this->Load("worlds/shapes.world");
auto world = physics::get_world("default");
EXPECT_TRUE(world != NULL);
// Check initial world state
physics::WorldState oldWorldState(world);
EXPECT_EQ(4u, oldWorldState.GetModelStateCount());
auto oldModelState = oldWorldState.GetModelState("box");
ignition::math::Pose3d oldPose(0, 0, 0.5, 0, 0, 0);
EXPECT_EQ(oldPose, oldModelState.GetPose().Ign());
// Move the box
world->SetPaused(true);
ignition::math::Pose3d newPose(1, 2, 0.5, 0, 0, 0);
world->GetModel("box")->SetWorldPose(newPose);
world->Step(1);
// Make sure the box state reflects the move
physics::WorldState newWorldState(world);
EXPECT_EQ(4u, newWorldState.GetModelStateCount());
auto newModelState = newWorldState.GetModelState("box");
EXPECT_EQ(newPose, newModelState.GetPose().Ign());
// Reset world state, and check for correctness
world->SetState(oldWorldState);
physics::WorldState resetWorldState(world);
EXPECT_EQ(4u, resetWorldState.GetModelStateCount());
auto resetModelState = resetWorldState.GetModelState("box");
EXPECT_EQ(oldPose, resetModelState.GetPose().Ign());
}
//////////////////////////////////////////////////
TEST_F(PhysicsTest, StateInsertion)
{
this->Load("worlds/empty.world");
auto world = physics::get_world("default");
EXPECT_TRUE(world != NULL);
std::string newModelName("new_model");
std::string newLightName("new_light");
// Check initial count
EXPECT_EQ(1u, world->GetModelCount());
EXPECT_EQ(1u, world->LightCount());
EXPECT_TRUE(world->GetModel(newModelName) == NULL);
EXPECT_TRUE(world->Light(newLightName) == NULL);
// Current world state
physics::WorldState worldState(world);
EXPECT_EQ(1u, worldState.GetModelStateCount());
EXPECT_EQ(1u, worldState.LightStateCount());
// Insertions
std::stringstream newModelStr;
newModelStr << "<model name ='" << newModelName << "'>"
<< "<link name ='link'>"
<< " <collision name ='collision'>"
<< " <geometry>"
<< " <box><size>1 1 1</size></box>"
<< " </geometry>"
<< " </collision>"
<< " <visual name ='visual'>"
<< " <geometry>"
<< " <box><size>1 1 1</size></box>"
<< " </geometry>"
<< " </visual>"
<< "</link>"
<< "</model>";
std::stringstream newModelSDFStr;
newModelSDFStr << "<sdf version='" << SDF_VERSION << "'>"
<< "<model name ='" << newModelName << "_SDF'>"
<< "<link name ='link'>"
<< " <collision name ='collision'>"
<< " <geometry>"
<< " <box><size>2 2 2</size></box>"
<< " </geometry>"
<< " </collision>"
<< " <visual name ='visual'>"
<< " <geometry>"
<< " <box><size>2 2 2</size></box>"
<< " </geometry>"
<< " </visual>"
<< "</link>"
<< "</model>"
<< "</sdf>";
std::stringstream newLightStr;
newLightStr << "<light name ='" << newLightName << "' type='spot'>"
<< "<diffuse>0 1 0 1</diffuse>"
<< "</light>";
std::stringstream newLightSDFStr;
newLightSDFStr << "<sdf version='" << SDF_VERSION << "'>"
<< "<light name ='" << newLightName << "_SDF' type='point'>"
<< "<diffuse>0 1 0 1</diffuse>"
<< "</light>"
<< "</sdf>";
std::vector<std::string> insertions;
insertions.push_back(newModelStr.str());
insertions.push_back(newModelSDFStr.str());
insertions.push_back(newLightStr.str());
insertions.push_back(newLightSDFStr.str());
worldState.SetInsertions(insertions);
// Set state which includes insertion
world->SetPaused(true);
world->SetState(worldState);
world->Step(1);
// Check entities were inserted
EXPECT_EQ(3u, world->GetModelCount());
EXPECT_EQ(3u, world->LightCount());
EXPECT_NE(nullptr, world->GetModel(newModelName));
EXPECT_NE(nullptr, world->GetModel(newModelName + "_SDF"));
EXPECT_NE(nullptr, world->Light(newLightName));
EXPECT_NE(nullptr, world->Light(newLightName + "_SDF"));
// New world state
physics::WorldState newWorldState(world);
EXPECT_EQ(3u, newWorldState.GetModelStateCount());
EXPECT_EQ(3u, newWorldState.LightStateCount());
}
//////////////////////////////////////////////////
TEST_F(PhysicsTest, StateDeletion)
{
this->Load("worlds/shapes.world");
auto world = physics::get_world("default");
EXPECT_TRUE(world != NULL);
std::string deleteModelName("box");
std::string deleteLightName("sun");
// Check initial count
EXPECT_EQ(4u, world->GetModelCount());
EXPECT_EQ(1u, world->LightCount());
EXPECT_FALSE(world->GetModel(deleteModelName) == NULL);
EXPECT_FALSE(world->Light(deleteLightName) == NULL);
// Current world state
physics::WorldState worldState(world);
EXPECT_EQ(4u, worldState.GetModelStateCount());
EXPECT_EQ(1u, worldState.LightStateCount());
// Deletions
std::vector<std::string> deletions;
deletions.push_back(deleteModelName);
deletions.push_back(deleteLightName);
worldState.SetDeletions(deletions);
// Set state which includes deletion
world->SetState(worldState);
gazebo::common::Time::MSleep(10);
// Check entities were deleted
EXPECT_EQ(3u, world->GetModelCount());
EXPECT_EQ(0u, world->LightCount());
EXPECT_TRUE(world->GetModel(deleteModelName) == NULL);
EXPECT_TRUE(world->Light(deleteLightName) == NULL);
// New world state
physics::WorldState newWorldState(world);
EXPECT_EQ(3u, newWorldState.GetModelStateCount());
EXPECT_EQ(0u, newWorldState.LightStateCount());
}
////////////////////////////////////////////////////////////////////////
void PhysicsTest::JointDampingTest(const std::string &_physicsEngine)
{
// Random seed is set to prevent brittle failures (gazebo issue #479)
ignition::math::Rand::Seed(18420503);
Load("worlds/damp_test.world", true, _physicsEngine);
physics::WorldPtr world = physics::get_world("default");
ASSERT_TRUE(world != NULL);
int i = 0;
while (!this->HasEntity("model_4_mass_1_ixx_1_damping_10") && i < 20)
{
common::Time::MSleep(100);
++i;
}
if (i > 20)
gzthrow("Unable to get model_4_mass_1_ixx_1_damping_10");
physics::ModelPtr model = world->GetModel("model_4_mass_1_ixx_1_damping_10");
EXPECT_TRUE(model != NULL);
{
// compare against recorded data only
double test_duration = 1.5;
double dt = world->GetPhysicsEngine()->GetMaxStepSize();
int steps = test_duration/dt;
for (int i = 0; i < steps; ++i)
{
world->Step(1); // theoretical contact, but
// gzdbg << "box time [" << world->GetSimTime().Double()
// << "] vel [" << model->GetWorldLinearVel()
// << "] pose [" << model->GetWorldPose()
// << "]\n";
}
EXPECT_EQ(world->GetSimTime().Double(), 1.5);
// This test expects a linear velocity at the CoG
math::Vector3 vel = model->GetLink()->GetWorldCoGLinearVel();
math::Pose pose = model->GetWorldPose();
EXPECT_EQ(vel.x, 0.0);
if (_physicsEngine == "dart")
{
// DART needs greater tolerance. The reason is not sure yet.
// Please see issue #904
EXPECT_NEAR(vel.y, -10.2009, 0.012);
EXPECT_NEAR(vel.z, -6.51755, 0.012);
}
else
{
EXPECT_NEAR(vel.y, -10.2009, PHYSICS_TOL);
EXPECT_NEAR(vel.z, -6.51755, PHYSICS_TOL);
}
EXPECT_DOUBLE_EQ(pose.pos.x, 3.0);
EXPECT_NEAR(pose.pos.y, 0.0, PHYSICS_TOL);
EXPECT_NEAR(pose.pos.z, 10.099, PHYSICS_TOL);
EXPECT_NEAR(pose.rot.GetAsEuler().x, 0.567334, PHYSICS_TOL);
EXPECT_DOUBLE_EQ(pose.rot.GetAsEuler().y, 0.0);
EXPECT_DOUBLE_EQ(pose.rot.GetAsEuler().z, 0.0);
}
}
TEST_P(PhysicsTest, JointDampingTest)
{
JointDampingTest(GetParam());
}
////////////////////////////////////////////////////////////////////////
void PhysicsTest::DropStuff(const std::string &_physicsEngine)
{
Load("worlds/drop_test.world", true, _physicsEngine);
physics::WorldPtr world = physics::get_world("default");
EXPECT_TRUE(world != NULL);
int i = 0;
while (!this->HasEntity("cylinder") && i < 20)
{
common::Time::MSleep(100);
++i;
}
if (i > 20)
gzthrow("Unable to get cylinder");
{
// todo: get parameters from drop_test.world
double test_duration = 1.5;
double z = 10.5;
double v = 0.0;
double g = -10.0;
double dt = world->GetPhysicsEngine()->GetMaxStepSize();
// world->Step(1428); // theoretical contact, but
// world->Step(100); // integration error requires few more steps
int steps = test_duration/dt;
bool post_contact_correction = false;
for (int i = 0; i < steps; ++i)
{
// integrate here to see when the collision should happen
v = v + dt * g;
z = z + dt * v;
world->Step(1); // theoretical contact, but
{
physics::ModelPtr box_model = world->GetModel("box");
if (box_model)
{
math::Vector3 vel = box_model->GetWorldLinearVel();
math::Pose pose = box_model->GetWorldPose();
// gzdbg << "box time [" << world->GetSimTime().Double()
// << "] sim z [" << pose.pos.z
// << "] exact z [" << z
// << "] sim vz [" << vel.z
// << "] exact vz [" << v << "]\n";
if (z > 0.5 || !post_contact_correction)
{
EXPECT_LT(fabs(vel.z - v) , 0.0001);
EXPECT_LT(fabs(pose.pos.z - z) , 0.0001);
}
else
{
EXPECT_LT(fabs(vel.z), 0.0101); // sometimes -0.01, why?
if (_physicsEngine == "dart")
{
// DART needs more tolerance until supports 'correction for
// penetration' feature.
// Please see issue #902
EXPECT_LT(fabs(pose.pos.z - 0.5), 0.00410);
}
else
{
EXPECT_LT(fabs(pose.pos.z - 0.5), 0.00001);
}
}
}
physics::ModelPtr sphere_model = world->GetModel("sphere");
if (sphere_model)
{
math::Vector3 vel = sphere_model->GetWorldLinearVel();
math::Pose pose = sphere_model->GetWorldPose();
// gzdbg << "sphere time [" << world->GetSimTime().Double()
// << "] sim z [" << pose.pos.z
// << "] exact z [" << z
// << "] sim vz [" << vel.z
// << "] exact vz [" << v << "]\n";
if (z > 0.5 || !post_contact_correction)
{
EXPECT_LT(fabs(vel.z - v), 0.0001);
EXPECT_LT(fabs(pose.pos.z - z), 0.0001);
}
else
{
if (_physicsEngine == "dart")
{
// DART needs more tolerance until supports 'correction for
// penetration' feature.
// Please see issue #902
EXPECT_LT(fabs(vel.z), 0.015);
EXPECT_LT(fabs(pose.pos.z - 0.5), 0.00410);
}
else
{
EXPECT_LT(fabs(vel.z), 3e-5);
EXPECT_LT(fabs(pose.pos.z - 0.5), 0.00001);
}
}
}
physics::ModelPtr cylinder_model = world->GetModel("cylinder");
if (cylinder_model)
{
math::Vector3 vel = cylinder_model->GetWorldLinearVel();
math::Pose pose = cylinder_model->GetWorldPose();
// gzdbg << "cylinder time [" << world->GetSimTime().Double()
// << "] sim z [" << pose.pos.z
// << "] exact z [" << z
// << "] sim vz [" << vel.z
// << "] exact vz [" << v << "]\n";
if (z > 0.5 || !post_contact_correction)
{
EXPECT_LT(fabs(vel.z - v), 0.0001);
EXPECT_LT(fabs(pose.pos.z - z), 0.0001);
}
else
{
EXPECT_LT(fabs(vel.z), 0.011);
if (_physicsEngine == "dart")
{
// DART needs more tolerance until supports 'correction for
// penetration' feature.
// Please see issue #902
EXPECT_LT(fabs(pose.pos.z - 0.5), 0.0041);
}
else
{
EXPECT_LT(fabs(pose.pos.z - 0.5), 0.0001);
}
}
}
}
if (z < 0.5) post_contact_correction = true;
}
}
}
// This test doesn't pass yet in Bullet or Simbody
TEST_F(PhysicsTest, DropStuffODE)
{
DropStuff("ode");
}
#ifdef HAVE_DART
TEST_F(PhysicsTest, DropStuffDART)
{
DropStuff("dart");
}
#endif // HAVE_DART
////////////////////////////////////////////////////////////////////////
void PhysicsTest::InelasticCollision(const std::string &_physicsEngine)
{
// check conservation of mementum for linear inelastic collision
Load("worlds/collision_test.world", true, _physicsEngine);
physics::WorldPtr world = physics::get_world("default");
EXPECT_TRUE(world != NULL);
int i = 0;
while (!this->HasEntity("sphere") && i < 20)
{
common::Time::MSleep(100);
++i;
}
if (i > 20)
gzthrow("Unable to get sphere");
{
// todo: get parameters from drop_test.world
double test_duration = 1.1;
double dt = world->GetPhysicsEngine()->GetMaxStepSize();
physics::ModelPtr box_model = world->GetModel("box");
physics::LinkPtr box_link = box_model->GetLink("link");
double f = 1000.0;
double v = 0;
double x = 0;
double m = box_link->GetInertial()->GetMass();
int steps = test_duration/dt;
for (int i = 0; i < steps; ++i)
{
double t = world->GetSimTime().Double();
double velTolerance = PHYSICS_TOL;
#ifdef HAVE_BULLET
if (_physicsEngine == "bullet" && sizeof(btScalar) == 4)
{
velTolerance *= 11;
}
#endif
world->Step(1); // theoretical contact, but
if (box_model)
{
math::Vector3 vel = box_model->GetWorldLinearVel();
math::Pose pose = box_model->GetWorldPose();
// gzdbg << "box time [" << t
// << "] sim x [" << pose.pos.x
// << "] ideal x [" << x
// << "] sim vx [" << vel.x
// << "] ideal vx [" << v
// << "]\n";
if (i == 0)
{
box_model->GetLink("link")->SetForce(math::Vector3(f, 0, 0));
// The following has been failing since pull request #1284,
// so it has been disabled.
// See bitbucket.org/osrf/gazebo/issue/1394
// EXPECT_EQ(box_model->GetLink("link")->GetWorldForce(),
// math::Vector3(f, 0, 0));
}
if (t > 1.000 && t < 1.01)
{
// collision transition, do nothing
}
else
{
// collision happened
EXPECT_NEAR(pose.pos.x, x, PHYSICS_TOL);
EXPECT_NEAR(vel.x, v, velTolerance);
}
}
physics::ModelPtr sphere_model = world->GetModel("sphere");
if (sphere_model)
{
math::Vector3 vel = sphere_model->GetWorldLinearVel();
math::Pose pose = sphere_model->GetWorldPose();
// gzdbg << "sphere time [" << world->GetSimTime().Double()
// << "] sim x [" << pose.pos.x
// << "] ideal x [" << x
// << "] sim vx [" << vel.x
// << "] ideal vx [" << v
// << "]\n";
if (t > 1.000 && t < 1.01)
{
// collision transition, do nothing
}
else if (t <= 1.00)
{
// no collision
EXPECT_EQ(pose.pos.x, 2);
EXPECT_EQ(vel.x, 0);
}
else
{
// collision happened
EXPECT_NEAR(pose.pos.x, x + 1.0, PHYSICS_TOL);
EXPECT_NEAR(vel.x, v, velTolerance);
}
}
/*
// integrate here to see when the collision should happen
double impulse = dt*f;
if (i == 0) v = v + impulse;
else if (t >= 1.0) v = dt*f/ 2.0; // inelastic col. w/ eqal mass.
x = x + dt * v;
*/
// integrate here to see when the collision should happen
double vold = v;
if (i == 0)
v = vold + dt* (f / m);
else if (t >= 1.0)
v = dt*f/ 2.0; // inelastic col. w/ eqal mass.
x = x + dt * (v + vold) / 2.0;
}
}
}
TEST_P(PhysicsTest, InelasticCollision)
{
InelasticCollision(GetParam());
}
////////////////////////////////////////////////////////////////////////
// SphereAtlasLargeError:
// Check algorithm's ability to re-converge after a large LCP error is
// introduced.
// In this test, a model with similar dynamics properties to Atlas V3
// is pinned to the world by both feet. Robot is moved by a large
// distance, violating the joints between world and feet temporarily.
// Robot is then allowed to settle. Check to see that the LCP solution
// does not become unstable.
////////////////////////////////////////////////////////////////////////
void PhysicsTest::SphereAtlasLargeError(const std::string &_physicsEngine)
{
if (_physicsEngine != "ode")
{
gzerr << "Skipping SphereAtlasLargeError for physics engine ["
<< _physicsEngine
<< "] as this test only works for ODE for now.\n";
return;
}
Load("worlds/sphere_atlas_demo.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);
physics->SetGravity(math::Vector3(0, 0, 0));
int i = 0;
while (!this->HasEntity("sphere_atlas") && i < 20)
{
common::Time::MSleep(100);
++i;
}
if (i > 20)
gzthrow("Unable to get sphere_atlas");
physics::ModelPtr model = world->GetModel("sphere_atlas");
EXPECT_TRUE(model != NULL);
physics::LinkPtr head = model->GetLink("head");
EXPECT_TRUE(head != NULL);
{
gzdbg << "Testing large perturbation with PID controller active.\n";
// Test: With Robot PID controller active, introduce a large
// constraint error by breaking some model joints to the world
model->SetWorldPose(math::Pose(1000, 0, 0, 0, 0, 0));
// let model settle
world->Step(2000);
for (unsigned int n = 0; n < 10; ++n)
{
world->Step(1);
// manually check joint constraint violation for each joint
physics::Link_V links = model->GetLinks();
for (unsigned int i = 0; i < links.size(); ++i)
{
math::Pose childInWorld = links[i]->GetWorldPose();
physics::Joint_V parentJoints = links[i]->GetParentJoints();
for (unsigned int j = 0; j < parentJoints.size(); ++j)
{
// anchor position in world frame
math::Vector3 anchorPos = parentJoints[j]->GetAnchor(0);
// anchor pose in child link frame
math::Pose anchorInChild =
math::Pose(anchorPos, math::Quaternion()) - childInWorld;
// initial anchor pose in child link frame
math::Pose anchorInitialInChild =
parentJoints[j]->GetInitialAnchorPose();
physics::LinkPtr parent = parentJoints[j]->GetParent();
if (parent)
{
// compare everything in the parent frame
math::Pose childInitialInParent =
links[i]->GetInitialRelativePose() - // rel to model
parent->GetInitialRelativePose(); // rel to model
math::Pose parentInWorld = parent->GetWorldPose();
math::Pose childInParent = childInWorld - parentInWorld;
math::Pose anchorInParent = anchorInChild + childInParent;
math::Pose anchorInitialInParent =
anchorInitialInChild + childInitialInParent;
math::Pose jointError = anchorInParent - anchorInitialInParent;
// joint constraint violation must be less than...
EXPECT_LT(jointError.pos.GetSquaredLength(), PHYSICS_TOL);
// debug
if (jointError.pos.GetSquaredLength() >= PHYSICS_TOL)
gzdbg << "i [" << n
<< "] link [" << links[i]->GetName()
// << "] parent[" << parent->GetName()
<< "] error[" << jointError.pos.GetSquaredLength()
// << "] pose[" << childInWorld
<< "] anchor[" << anchorInChild
<< "] cinp[" << childInParent
<< "] ainp0[" << anchorInitialInParent
<< "] ainp[" << anchorInParent
<< "] diff[" << jointError
<< "]\n";
}
}
}
}
}
{
gzdbg << "Testing large perturbation with PID controller disabled.\n";
// Test: Turn off Robot PID controller, then introduce a large
// constraint error by breaking some model joints to the world
// special hook in SphereAtlasTestPlugin disconnects
// PID controller on Reset.
world->Reset();
world->Step(1);
model->SetWorldPose(math::Pose(1000, 0, 0, 0, 0, 0));
// let model settle
world->Step(2000);
for (unsigned int n = 0; n < 10; ++n)
{
world->Step(1);
// manually check joint constraint violation for each joint
physics::Link_V links = model->GetLinks();
for (unsigned int i = 0; i < links.size(); ++i)
{
math::Pose childInWorld = links[i]->GetWorldPose();
physics::Joint_V parentJoints = links[i]->GetParentJoints();
for (unsigned int j = 0; j < parentJoints.size(); ++j)
{
// anchor position in world frame
math::Vector3 anchorPos = parentJoints[j]->GetAnchor(0);
// anchor pose in child link frame
math::Pose anchorInChild =
math::Pose(anchorPos, math::Quaternion()) - childInWorld;
// initial anchor pose in child link frame
math::Pose anchorInitialInChild =
parentJoints[j]->GetInitialAnchorPose();
physics::LinkPtr parent = parentJoints[j]->GetParent();
if (parent)
{
// compare everything in the parent frame
math::Pose childInitialInParent =
links[i]->GetInitialRelativePose() - // rel to model
parent->GetInitialRelativePose(); // rel to model
math::Pose parentInWorld = parent->GetWorldPose();
math::Pose childInParent = childInWorld - parentInWorld;
math::Pose anchorInParent = anchorInChild + childInParent;
math::Pose anchorInitialInParent =
anchorInitialInChild + childInitialInParent;
math::Pose jointError = anchorInParent - anchorInitialInParent;
// joint constraint violation must be less than...
EXPECT_LT(jointError.pos.GetSquaredLength(), PHYSICS_TOL);
// debug
if (jointError.pos.GetSquaredLength() >= PHYSICS_TOL)
gzdbg << "i [" << n
<< "] link [" << links[i]->GetName()
// << "] parent[" << parent->GetName()
<< "] error[" << jointError.pos.GetSquaredLength()
// << "] pose[" << childInWorld
<< "] anchor[" << anchorInChild
<< "] cinp[" << childInParent
<< "] ainp0[" << anchorInitialInParent
<< "] ainp[" << anchorInParent
<< "] diff[" << jointError
<< "]\n";
}
}
}
}
}
}
TEST_P(PhysicsTest, SphereAtlasLargeError)
{
SphereAtlasLargeError(GetParam());
}
////////////////////////////////////////////////////////////////////////
// CollisionFiltering:
// Load a world, spawn a model with two overlapping links. By default,
// the links should not collide with each other as they have the same
// parent model. Check the x and y velocities to see if they are 0
////////////////////////////////////////////////////////////////////////
void PhysicsTest::CollisionFiltering(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);
std::stringstream newModelStr;
std::string modelName = "multiLinkModel";
math::Pose modelPose(0, 0, 2, 0, 0, 0);
math::Pose link01Pose(0, 0.1, 0, 0, 0, 0);
math::Pose link02Pose(0, -0.1, 0, 0, 0, 0);
// A model composed of two overlapping links at fixed y offset from origin
newModelStr << "<sdf version='" << SDF_VERSION << "'>"
<< "<model name ='" << modelName << "'>"
<< "<pose>" << modelPose.pos.x << " "
<< modelPose.pos.y << " "
<< modelPose.pos.z << " "
<< modelPose.rot.GetAsEuler().x << " "
<< modelPose.rot.GetAsEuler().y << " "
<< modelPose.rot.GetAsEuler().z << "</pose>"
<< "<link name ='link01'>"
<< " <pose>" << link01Pose.pos.x << " "
<< link01Pose.pos.y << " "
<< link01Pose.pos.z << " "
<< link01Pose.rot.GetAsEuler().x << " "
<< link01Pose.rot.GetAsEuler().y << " "
<< link01Pose.rot.GetAsEuler().z << "</pose>"
<< " <collision name ='geom'>"
<< " <geometry>"
<< " <box><size>1 1 1</size></box>"
<< " </geometry>"
<< " </collision>"
<< " <visual name ='visual'>"
<< " <geometry>"
<< " <box><size>1 1 1</size></box>"
<< " </geometry>"
<< " </visual>"
<< "</link>"
<< "<link name ='link02'>"
<< " <pose>" << link02Pose.pos.x << " "
<< link02Pose.pos.y << " "
<< link02Pose.pos.z << " "
<< link02Pose.rot.GetAsEuler().x << " "
<< link02Pose.rot.GetAsEuler().y << " "
<< link02Pose.rot.GetAsEuler().z << "</pose>"
<< " <collision name ='geom'>"
<< " <geometry>"
<< " <box><size>1 1 1</size></box>"
<< " </geometry>"
<< " </collision>"
<< " <visual name ='visual'>"
<< " <geometry>"
<< " <box><size>1 1 1</size></box>"
<< " </geometry>"
<< " </visual>"
<< "</link>"
<< "</model>"
<< "</sdf>";
SpawnSDF(newModelStr.str());
// Wait for the entity to spawn
int i = 0;
while (!this->HasEntity(modelName) && i < 20)
{
common::Time::MSleep(100);
++i;
}
if (i > 20)
gzthrow("Unable to spawn model");
world->Step(5);
physics::ModelPtr model = world->GetModel(modelName);
math::Vector3 vel;
physics::Link_V links = model->GetLinks();
EXPECT_EQ(links.size(), 2u);
for (physics::Link_V::const_iterator iter = links.begin();
iter != links.end(); ++iter)
{
std::cout << "LinkName[" << (*iter)->GetScopedName() << "]\n";
// Links should not repel each other hence expecting zero x, y vel
vel = (*iter)->GetWorldLinearVel();
EXPECT_EQ(vel.x, 0);
EXPECT_EQ(vel.y, 0);
// Model should be falling
EXPECT_LT(vel.z, 0);
}
}
/////////////////////////////////////////////////
TEST_P(PhysicsTest, CollisionFiltering)
{
CollisionFiltering(GetParam());
}
/////////////////////////////////////////////////
// This test verifies that gazebo doesn't crash when collisions occur
// and the <world><physics><ode><max_contacts> value is zero.
// The crash was reported in issue #593 on bitbucket
TEST_F(PhysicsTest, ZeroMaxContactsODE)
{
// Load an empty world
Load("worlds/zero_max_contacts.world");
physics::WorldPtr world = physics::get_world("default");
ASSERT_TRUE(world != NULL);
physics::ModelPtr model = world->GetModel("ground_plane");
ASSERT_TRUE(model != NULL);
}
INSTANTIATE_TEST_CASE_P(PhysicsEngines, PhysicsTest, 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