p81523470
/
ppovb5fc7
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity
ppovb5fc7/gazebo/test/integration/joint_test.cc

535 lines
16 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 <gtest/gtest.h>
#include "gazebo/physics/physics.hh"
#include "gazebo/physics/Joint.hh"
#include "gazebo/test/helper_physics_generator.hh"
#include "test/integration/joint_test.hh"
#define TOL 1e-6
#define TOL_CONT 2.0
using namespace gazebo;
//////////////////////////////////////////////////
void JointTest::JointCreationDestructionTest(const std::string &_physicsEngine)
{
/// \TODO: Disable for now until functionality is implemented
/// bullet collision parameters needs tweaking
if (_physicsEngine == "bullet")
{
gzerr << "Aborting test for bullet, see issue #590.\n";
return;
}
/// \TODO: simbody not complete for this test
if (_physicsEngine == "simbody")
{
gzerr << "Aborting test for Simbody, see issue #862.\n";
return;
}
/// \TODO: dart not complete for this test
if (_physicsEngine == "dart")
{
gzerr << "Aborting test for DART, see issue #903.\n";
return;
}
// Load our inertial test world
Load("worlds/joint_test.world", true, _physicsEngine);
// Get a pointer to the world, make sure world loads
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);
// create some fake links
physics::ModelPtr model = world->GetModel("model_1");
ASSERT_TRUE(model != NULL);
physics::LinkPtr link = model->GetLink("link_1");
ASSERT_TRUE(link != NULL);
physics::LinkPtr parentLink;
physics::LinkPtr childLink(link);
physics::JointPtr joint;
math::Pose anchor;
math::Vector3 axis(1, 0, 0);
double upper = M_PI;
double lower = -M_PI;
double residentLast = 0, shareLast = 0;
double residentCur = 0, shareCur = 0;
// The memory footprint on osx can take around 190 cycles to stabilize.
// So this test gives 250 cycles to stabilize and then verifies stability
// for another 250.
unsigned int cyclesMax = 500;
unsigned int cyclesStabilize = cyclesMax / 2;
for (unsigned int i = 0; i < cyclesMax; ++i)
{
// try creating a joint
{
joint = world->GetPhysicsEngine()->CreateJoint(
"revolute", model);
joint->Attach(parentLink, childLink);
// load adds the joint to a vector of shared pointers kept
// in parent and child links, preventing joint from being destroyed.
joint->Load(parentLink, childLink, anchor);
// joint->SetAnchor(0, anchor);
joint->SetAxis(0, axis);
joint->SetHighStop(0, upper);
joint->SetLowStop(0, lower);
if (parentLink)
joint->SetName(parentLink->GetName() + std::string("_") +
childLink->GetName() + std::string("_joint"));
else
joint->SetName(std::string("world_") +
childLink->GetName() + std::string("_joint"));
joint->Init();
joint->SetAxis(0, axis);
}
// remove the joint
{
bool paused = world->IsPaused();
world->SetPaused(true);
if (joint)
{
// reenable collision between the link pair
physics::LinkPtr parent = joint->GetParent();
physics::LinkPtr child = joint->GetChild();
if (parent)
parent->SetCollideMode("all");
if (child)
child->SetCollideMode("all");
joint->Detach();
joint.reset();
}
world->SetPaused(paused);
}
world->Step(200);
this->GetMemInfo(residentCur, shareCur);
// give it 2 cycles to stabilize
if (i > cyclesStabilize)
{
EXPECT_LE(residentCur, residentLast);
EXPECT_LE(shareCur, shareLast);
}
// gzdbg << "memory res[" << residentCur
// << "] shr[" << shareCur
// << "] res[" << residentLast
// << "] shr[" << shareLast
// << "]\n";
residentLast = residentCur;
shareLast = shareCur;
}
}
//////////////////////////////////////////////////
void JointTest::GetInertiaRatio(const std::string &_physicsEngine)
{
// Load our inertia ratio world
Load("worlds/inertia_ratio.world", true, _physicsEngine);
// Get a pointer to the world, make sure world loads
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);
physics::ModelPtr model = world->GetModel("double_pendulum");
ASSERT_TRUE(model != NULL);
{
physics::JointPtr joint = model->GetJoint("lower_joint");
ASSERT_TRUE(joint != NULL);
EXPECT_NEAR(joint->GetInertiaRatio(0), 3125, 1e-2);
EXPECT_NEAR(joint->GetInertiaRatio(math::Vector3::UnitX), 3125, 1e-2);
EXPECT_NEAR(joint->GetInertiaRatio(math::Vector3::UnitY), 87.50, 1e-2);
}
}
//////////////////////////////////////////////////
void JointTest::SpringDamperTest(const std::string &_physicsEngine)
{
/// bullet collision parameters needs tweaking
if (_physicsEngine == "bullet")
{
gzerr << "Aborting test for bullet, see issue #887.\n";
return;
}
if (_physicsEngine == "dart")
{
gzerr << "Aborting test for " << _physicsEngine
<< ", see issue #975.\n";
return;
}
// Load our inertial test world
Load("worlds/spring_damper_test.world", true, _physicsEngine);
// Get a pointer to the world, make sure world loads
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);
// All models should oscillate with the same frequency
physics::ModelPtr modelPrismatic = world->GetModel("model_3_prismatic");
physics::ModelPtr modelRevolute = world->GetModel("model_3_revolute");
physics::ModelPtr modelPlugin = world->GetModel("model_4_prismatic_plugin");
physics::ModelPtr modelContact = world->GetModel("model_5_soft_contact");
physics::ModelPtr modelPrismatic2 = world->GetModel("model_6_prismatic_sdf");
physics::ModelPtr modelRevolute2 = world->GetModel("model_7_revolute_sdf");
ASSERT_TRUE(modelPrismatic != NULL);
ASSERT_TRUE(modelRevolute != NULL);
ASSERT_TRUE(modelPlugin != NULL);
ASSERT_TRUE(modelContact != NULL);
ASSERT_TRUE(modelPrismatic2 != NULL);
ASSERT_TRUE(modelRevolute2 != NULL);
physics::LinkPtr linkPrismatic = modelPrismatic->GetLink("link_1");
physics::LinkPtr linkRevolute = modelRevolute->GetLink("link_1");
physics::LinkPtr linkPluginExplicit = modelPlugin->GetLink("link_1");
physics::LinkPtr linkPluginImplicit = modelPlugin->GetLink("link_2");
physics::LinkPtr linkContact = modelContact->GetLink("link_1");
physics::LinkPtr linkPrismatic2 = modelPrismatic2->GetLink("link");
physics::LinkPtr linkRevolute2 = modelRevolute2->GetLink("link");
ASSERT_TRUE(linkPrismatic != NULL);
ASSERT_TRUE(linkRevolute != NULL);
ASSERT_TRUE(linkPluginExplicit != NULL);
ASSERT_TRUE(linkPluginImplicit != NULL);
ASSERT_TRUE(linkContact != NULL);
ASSERT_TRUE(linkPrismatic2 != NULL);
ASSERT_TRUE(linkRevolute2 != NULL);
physics::JointPtr jointPluginImplicit = modelPlugin->GetJoint("joint_1");
ASSERT_TRUE(jointPluginImplicit != NULL);
int cyclesPrismatic = 0;
int cyclesRevolute = 0;
int cyclesPluginExplicit = 0;
int cyclesPluginImplicit = 0;
int cyclesContact = 0;
int cyclesPrismatic2 = 0;
int cyclesRevolute2 = 0;
double velPrismatic = 1.0;
double velRevolute = 1.0;
double velPluginExplicit = 1.0;
double velPluginImplicit = 1.0;
double velContact = 1.0;
double velPrismatic2 = 1.0;
double velRevolute2 = 1.0;
const double vT = 0.01;
double energyPluginImplicit0 = linkPluginImplicit->GetWorldEnergy()
+ jointPluginImplicit->GetWorldEnergyPotentialSpring(0);
// check number of oscillations for each of the setup. They should all
// be the same.
// run 5000 steps, at which point, contact is the first one to damp out
// and lose it's oscillatory behavior due to larger dissipation in
// contact behavior.
for (int i = 0; i < 5000; ++i)
{
world->Step(1);
// count up and down cycles
if (linkPrismatic->GetWorldLinearVel().z > vT && velPrismatic < -vT)
{
cyclesPrismatic++;
velPrismatic = 1.0;
}
else if (linkPrismatic->GetWorldLinearVel().z < -vT && velPrismatic > vT)
{
cyclesPrismatic++;
velPrismatic = -1.0;
}
if (-linkRevolute->GetRelativeAngularVel().y > vT && velRevolute < -vT)
{
cyclesRevolute++;
velRevolute = 1.0;
}
else if (-linkRevolute->GetRelativeAngularVel().y < -vT && velRevolute > vT)
{
cyclesRevolute++;
velRevolute = -1.0;
}
if (linkPluginExplicit->GetWorldLinearVel().z > vT &&
velPluginExplicit < -vT)
{
cyclesPluginExplicit++;
velPluginExplicit = 1.0;
}
else if (linkPluginExplicit->GetWorldLinearVel().z < -vT &&
velPluginExplicit > vT)
{
cyclesPluginExplicit++;
velPluginExplicit = -1.0;
}
if (linkPluginImplicit->GetWorldLinearVel().z > vT &&
velPluginImplicit < -vT)
{
cyclesPluginImplicit++;
velPluginImplicit = 1.0;
}
else if (linkPluginImplicit->GetWorldLinearVel().z < -vT &&
velPluginImplicit > vT)
{
cyclesPluginImplicit++;
velPluginImplicit = -1.0;
}
if (linkContact->GetWorldLinearVel().z > vT && velContact < -vT)
{
cyclesContact++;
velContact = 1.0;
}
else if (linkContact->GetWorldLinearVel().z < -vT && velContact > vT)
{
cyclesContact++;
velContact = -1.0;
}
if (linkPrismatic2->GetWorldLinearVel().z > vT && velPrismatic2 < -vT)
{
cyclesPrismatic2++;
velPrismatic2 = 1.0;
}
else if (linkPrismatic2->GetWorldLinearVel().z < -vT && velPrismatic2 > vT)
{
cyclesPrismatic2++;
velPrismatic2 = -1.0;
}
if (-linkRevolute2->GetRelativeAngularVel().y > vT && velRevolute2 < -vT)
{
cyclesRevolute2++;
velRevolute2 = 1.0;
}
else if (-linkRevolute2->GetRelativeAngularVel().y < -vT &&
velRevolute2 > vT)
{
cyclesRevolute2++;
velRevolute2 = -1.0;
}
double energy = linkPluginImplicit->GetWorldEnergy() +
jointPluginImplicit->GetWorldEnergyPotentialSpring(0);
if (_physicsEngine.compare("dart") == 0)
{
if (i == 0)
{
gzerr << _physicsEngine
<< " has reduced accuracy for spring energy conservation"
<< ", see #975"
<< std::endl;
}
EXPECT_NEAR(energy / energyPluginImplicit0, 1.0, 2e-2);
}
else
{
EXPECT_NEAR(energy / energyPluginImplicit0, 1.0, 1e-3);
}
// gzdbg << i << "\n";
// gzdbg << cyclesPrismatic << " : "
// << linkPrismatic->GetWorldLinearVel() << "\n";
// gzdbg << cyclesRevolute << " : "
// << linkRevolute->GetRelativeAngularVel() << "\n";
// gzdbg << cyclesContact << " : "
// << linkContact->GetWorldLinearVel() << "\n";
}
if (_physicsEngine.compare("ode") == 0)
{
gzdbg << "Extra tests for ode" << std::endl;
EXPECT_EQ(cyclesContact, 17);
}
if (_physicsEngine.compare("dart") == 0)
{
gzerr << _physicsEngine
<< " doesn't support joint limit stiffness"
<< ", see #975"
<< std::endl;
}
else
{
EXPECT_EQ(cyclesPrismatic, 17);
EXPECT_EQ(cyclesRevolute, 17);
}
EXPECT_EQ(cyclesPluginExplicit, 17);
EXPECT_EQ(cyclesPluginImplicit, 17);
EXPECT_EQ(cyclesPrismatic2, 17);
EXPECT_EQ(cyclesRevolute2, 17);
}
//////////////////////////////////////////////////
void JointTest::DynamicJointVisualization(const std::string &_physicsEngine)
{
/// \TODO: simbody not complete for this test
if (_physicsEngine == "simbody")
{
gzerr << "Aborting test for Simbody, see issue #862.\n";
return;
}
// Load empty world
Load("worlds/empty.world", true, _physicsEngine);
// Get a pointer to the world, make sure world loads
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);
// Spawn two boxes
SpawnBox("box1", math::Vector3(1, 1, 1), math::Vector3(1, 0, 0.5),
math::Vector3::Zero, false);
SpawnBox("box2", math::Vector3(1, 1, 1), math::Vector3(-1, 0, 0.5),
math::Vector3::Zero, false);
physics::ModelPtr model = world->GetModel("box1");
physics::ModelPtr model2 = world->GetModel("box2");
ASSERT_TRUE(model != NULL);
ASSERT_TRUE(model2 != NULL);
// Get link pointers
physics::LinkPtr parent = model->GetLink();
physics::LinkPtr child = model2->GetLink();
// Create dynamic joint
std::string name = "dynamic_joint_unique";
physics::JointPtr joint;
joint = model->CreateJoint(name, "revolute", parent, child);
joint->Init();
// Get model joints (used for visualization)
physics::Joint_V joints = model->GetJoints();
bool jointFound = false;
for (auto const &joint : joints)
{
if (joint->GetName() == name)
{
jointFound = true;
break;
}
}
EXPECT_TRUE(jointFound);
// Try to create the joint with the same name
EXPECT_TRUE(model->CreateJoint(name, "revolute", parent, child) == NULL);
// step to let joint creation finish before removing it
world->Step(1000);
// test remove joint
EXPECT_TRUE(model->RemoveJoint(name));
joint = model->GetJoint(name);
EXPECT_TRUE(joint == NULL);
// test that removing a non-existent joint will not break gazebo
EXPECT_FALSE(model->RemoveJoint("this_joint_doees_not_exist"));
}
//////////////////////////////////////////////////
TEST_F(JointTest, joint_SDF14)
{
Load("worlds/SDF_1_4.world");
physics::WorldPtr world = physics::get_world("default");
ASSERT_TRUE(world != NULL);
physics::PhysicsEnginePtr physics = world->GetPhysicsEngine();
ASSERT_TRUE(physics != NULL);
int i = 0;
while (!this->HasEntity("joint14_model") && i < 20)
{
common::Time::MSleep(100);
++i;
}
if (i > 20)
gzthrow("Unable to get joint14_model");
physics::PhysicsEnginePtr physicsEngine = world->GetPhysicsEngine();
EXPECT_TRUE(physicsEngine != NULL);
physics::ModelPtr model = world->GetModel("joint14_model");
EXPECT_TRUE(model != NULL);
physics::LinkPtr link1 = model->GetLink("body1");
EXPECT_TRUE(link1 != NULL);
physics::LinkPtr link2 = model->GetLink("body2");
EXPECT_TRUE(link2 != NULL);
EXPECT_EQ(model->GetJointCount(), 1u);
physics::JointPtr joint = model->GetJoint("joint14_revolute_joint");
EXPECT_TRUE(joint != NULL);
physics::LinkPtr parent = joint->GetParent();
EXPECT_TRUE(parent != NULL);
physics::LinkPtr child = joint->GetChild();
EXPECT_TRUE(child != NULL);
EXPECT_EQ(parent->GetName(), "body2");
EXPECT_EQ(child->GetName(), "body1");
}
TEST_P(JointTest, JointCreationDestructionTest)
{
JointCreationDestructionTest(this->physicsEngine);
}
TEST_P(JointTest, GetInertiaRatio)
{
GetInertiaRatio(this->physicsEngine);
}
TEST_P(JointTest, SpringDamperTest)
{
SpringDamperTest(this->physicsEngine);
}
TEST_P(JointTest, DynamicJointVisualization)
{
DynamicJointVisualization(this->physicsEngine);
}
INSTANTIATE_TEST_CASE_P(PhysicsEngines, JointTest,
::testing::Combine(PHYSICS_ENGINE_VALUES,
::testing::Values("")));
int main(int argc, char **argv)
{
::testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}
Reference in New Issue View Git Blame Copy Permalink