pxmlw6n2f/Gazebo_Distributed_MPI/test/integration/joint_revolute.cc

771 lines
24 KiB
C++

/*
* 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 <ignition/math/Rand.hh>
#include "gazebo/test/ServerFixture.hh"
#include "gazebo/gazebo_config.h"
#include "gazebo/math/SignalStats.hh"
#include "gazebo/physics/physics.hh"
#include "SimplePendulumIntegrator.hh"
#include "gazebo/test/helper_physics_generator.hh"
#include "test/integration/joint_test.hh"
using namespace gazebo;
const double g_tolerance = 1e-2;
class JointTestRevolute : public JointTest
{
/// \brief Spawn single pendulum and test GetWorldEnergy functions.
/// \param[in] _physicsEngine Type of physics engine to use.
public: void PendulumEnergy(const std::string &_physicsEngine);
/// \brief Spin joints several rotations and verify that the angles
/// are properly unwrapped.
/// \param[in] _physicsEngine Type of physics engine to use.
public: void UnwrappedAngle(const std::string &_physicsEngine);
/// \brief Load 8 double pendulums arranged in a circle.
/// Measure angular velocity of links, and verify proper axis orientation.
/// Then set joint limits and verify that links remain within limits.
/// \param[in] _physicsEngine Type of physics engine to use.
/// \param[in] _solverType Type of solver to use.
public: void RevoluteJoint(const std::string &_physicsEngine,
const std::string &_solverType="quick");
/// \brief Load a simple pendulum, simulate and compare to numerical
/// results from SimplePendulumIntegrator.
/// \param[in] _physicsEngine Type of physics engine to use.
public: void SimplePendulum(const std::string &_physicsEngine);
};
////////////////////////////////////////////////////////////
void JointTestRevolute::PendulumEnergy(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);
{
SpawnJointOptions opt;
opt.type = "revolute";
opt.worldParent = true;
double Am = M_PI / 4;
opt.modelPose.rot.SetFromEuler(0, 0, Am);
opt.childLinkPose.pos.z = 3.0;
opt.jointPose.pos.y = 1.5;
opt.axis.Set(1, 0, 0);
gzdbg << "SpawnJoint " << opt.type << " child world" << std::endl;
physics::JointPtr joint = SpawnJoint(opt);
ASSERT_TRUE(joint != NULL);
// Get initial energy
physics::LinkPtr link = joint->GetChild();
ASSERT_TRUE(link != NULL);
physics::ModelPtr model = link->GetModel();
ASSERT_TRUE(model != NULL);
double energy0 = model->GetWorldEnergy();
EXPECT_NEAR(model->GetWorldEnergyKinetic(), 0.0, g_tolerance);
unsigned int stepSize = 5;
unsigned int stepCount = 500;
for (unsigned int i = 0; i < stepCount; ++i)
{
world->Step(stepSize);
double energy = model->GetWorldEnergy();
EXPECT_NEAR(energy / energy0, 1.0, g_tolerance);
}
}
}
////////////////////////////////////////////////////////////
void JointTestRevolute::UnwrappedAngle(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);
// disable gravity
physics->SetGravity(math::Vector3::Zero);
{
std::string jointType = "revolute";
gzdbg << "SpawnJoint " << jointType << " child world" << std::endl;
physics::JointPtr joint = SpawnJoint(jointType, false, true);
ASSERT_TRUE(joint != NULL);
// expect large joint limits
EXPECT_DOUBLE_EQ(-1e16, joint->GetLowerLimit(0).Radian());
EXPECT_DOUBLE_EQ(+1e16, joint->GetUpperLimit(0).Radian());
EXPECT_DOUBLE_EQ(-1e16, joint->GetLowStop(0).Radian());
EXPECT_DOUBLE_EQ(+1e16, joint->GetHighStop(0).Radian());
// set velocity to 2 pi rad/s and step forward 1.5 seconds.
// angle should reach 3 pi rad.
double vel = 2*M_PI;
unsigned int stepSize = 50;
unsigned int stepCount = 30;
double dt = physics->GetMaxStepSize();
// Verify that the joint should make more than 1 revolution
EXPECT_GT(vel * stepSize * stepCount * dt, 1.25 * 2 * M_PI);
joint->SetVelocity(0, vel);
math::SignalMaxAbsoluteValue angleErrorMax;
// expect that joint velocity is constant
// and that joint angle is unwrapped
for (unsigned int i = 0; i < stepCount; ++i)
{
world->Step(stepSize);
EXPECT_NEAR(joint->GetVelocity(0), vel, g_tolerance);
double time = world->GetSimTime().Double();
angleErrorMax.InsertData(joint->GetAngle(0).Radian() - time*vel);
}
#ifndef LIBBULLET_VERSION_GT_282
if (_physicsEngine == "bullet")
{
gzerr << "Skipping portion of test, angle wrapping requires bullet 2.83"
<< std::endl;
}
else
#endif
{
EXPECT_NEAR(angleErrorMax.Value(), 0.0, g_tolerance);
}
}
}
////////////////////////////////////////////////////////////////////////
void JointTestRevolute::RevoluteJoint(const std::string &_physicsEngine,
const std::string &_solverType)
{
ignition::math::Rand::Seed(0);
// Load world
Load("worlds/revolute_joint_test.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);
// Set solver type
physics->SetParam("solver_type", _solverType);
if (_solverType == "world")
{
physics->SetParam("ode_quiet", true);
}
// Model names
std::vector<std::string> modelNames;
modelNames.push_back("pendulum_0deg");
modelNames.push_back("pendulum_45deg");
modelNames.push_back("pendulum_90deg");
modelNames.push_back("pendulum_135deg");
modelNames.push_back("pendulum_180deg");
modelNames.push_back("pendulum_225deg");
modelNames.push_back("pendulum_270deg");
modelNames.push_back("pendulum_315deg");
// Global axis
double sqrt1_2 = sqrt(2.0) / 2.0;
std::vector<math::Vector3> globalAxes;
globalAxes.push_back(math::Vector3(1, 0, 0));
globalAxes.push_back(math::Vector3(sqrt1_2, sqrt1_2, 0));
globalAxes.push_back(math::Vector3(0, 1, 0));
globalAxes.push_back(math::Vector3(-sqrt1_2, sqrt1_2, 0));
globalAxes.push_back(math::Vector3(-1, 0, 0));
globalAxes.push_back(math::Vector3(-sqrt1_2, -sqrt1_2, 0));
globalAxes.push_back(math::Vector3(0, -1, 0));
globalAxes.push_back(math::Vector3(sqrt1_2, -sqrt1_2, 0));
// Link names
std::vector<std::string> linkNames;
linkNames.push_back("lower_link");
linkNames.push_back("upper_link");
// Joint names
std::vector<std::string> jointNames;
jointNames.push_back("lower_joint");
jointNames.push_back("upper_joint");
physics::ModelPtr model;
physics::LinkPtr link;
std::vector<std::string>::iterator modelIter;
physics::JointPtr joint;
double energy0 = 1.0;
// Check global axes before simulation starts
std::vector<math::Vector3>::iterator axisIter;
axisIter = globalAxes.begin();
for (modelIter = modelNames.begin();
modelIter != modelNames.end(); ++modelIter)
{
model = world->GetModel(*modelIter);
if (model)
{
energy0 = model->GetWorldEnergy();
gzdbg << "Check global axes of model " << *modelIter << '\n';
std::vector<std::string>::iterator jointIter;
for (jointIter = jointNames.begin();
jointIter != jointNames.end(); ++jointIter)
{
joint = model->GetJoint(*jointIter);
if (joint)
{
math::Vector3 axis = joint->GetGlobalAxis(0);
EXPECT_NEAR(axis.x, (*axisIter).x, g_tolerance);
EXPECT_NEAR(axis.y, (*axisIter).y, g_tolerance);
EXPECT_NEAR(axis.z, (*axisIter).z, g_tolerance);
}
else
{
gzerr << "Error loading joint " << *jointIter
<< " of model " << *modelIter << '\n';
EXPECT_TRUE(joint != NULL);
}
}
}
else
{
gzerr << "Error loading model " << *modelIter << '\n';
EXPECT_TRUE(model != NULL);
}
++axisIter;
}
// Step forward 0.75 seconds
double dt = physics->GetMaxStepSize();
ASSERT_GT(dt, 0);
int steps = ceil(0.75 / dt);
world->Step(steps);
// Get global angular velocity of each link
math::Vector3 angVel;
for (modelIter = modelNames.begin();
modelIter != modelNames.end(); ++modelIter)
{
model = world->GetModel(*modelIter);
if (model)
{
EXPECT_NEAR(model->GetWorldEnergy() / energy0, 1.0, g_tolerance);
gzdbg << "Check angular velocity of model " << *modelIter << '\n';
link = model->GetLink("base");
if (link)
{
// Expect stationary base
angVel = link->GetWorldAngularVel();
EXPECT_NEAR(angVel.x, 0, g_tolerance*10);
EXPECT_NEAR(angVel.y, 0, g_tolerance*10);
EXPECT_NEAR(angVel.z, 0, g_tolerance*10);
}
else
{
gzerr << "Error loading base link of model " << *modelIter << '\n';
EXPECT_TRUE(link != NULL);
}
std::vector<std::string>::iterator linkIter;
for (linkIter = linkNames.begin();
linkIter != linkNames.end(); ++linkIter)
{
link = model->GetLink(*linkIter);
if (link)
{
// Expect relative angular velocity of pendulum links to be negative
// and along x axis.
angVel = link->GetRelativeAngularVel().Normalize();
EXPECT_NEAR(angVel.x, -1, g_tolerance);
EXPECT_NEAR(angVel.y, 0, 2*g_tolerance);
EXPECT_NEAR(angVel.z, 0, 2*g_tolerance);
}
else
{
gzerr << "Error loading link " << *linkIter
<< " of model " << *modelIter << '\n';
EXPECT_TRUE(link != NULL);
}
}
}
else
{
gzerr << "Error loading model " << *modelIter << '\n';
EXPECT_TRUE(model != NULL);
}
}
// Keep stepping forward, verifying that joint angles move in the direction
// implied by the joint velocity
for (modelIter = modelNames.begin();
modelIter != modelNames.end(); ++modelIter)
{
model = world->GetModel(*modelIter);
if (model)
{
EXPECT_NEAR(model->GetWorldEnergy() / energy0, 1.0, g_tolerance);
double jointVel1, jointVel2;
double angle1, angle2, angle3;
gzdbg << "Check angle measurement for " << *modelIter << '\n';
std::vector<std::string>::iterator jointIter;
for (jointIter = jointNames.begin();
jointIter != jointNames.end(); ++jointIter)
{
joint = model->GetJoint(*jointIter);
if (joint)
{
// Get first joint angle
angle1 = joint->GetAngle(0).Radian();
// Get joint velocity and assume it is not too small
jointVel1 = joint->GetVelocity(0);
EXPECT_GT(fabs(jointVel1), 1e-1);
// Take 1 step and measure again
world->Step(1);
// Expect angle change in direction of joint velocity
angle2 = joint->GetAngle(0).Radian();
EXPECT_GT((angle2 - angle1) * math::clamp(jointVel1*1e4, -1.0, 1.0)
, 0);
jointVel2 = joint->GetVelocity(0);
EXPECT_GT(fabs(jointVel2), 1e-1);
// Take 1 step and measure the last angle, expect decrease
world->Step(1);
angle3 = joint->GetAngle(0).Radian();
EXPECT_GT((angle3 - angle2) * math::clamp(jointVel2*1e4, -1.0, 1.0)
, 0);
}
else
{
gzerr << "Error loading joint " << *jointIter
<< " of model " << *modelIter << '\n';
EXPECT_TRUE(joint != NULL);
}
}
}
else
{
gzerr << "Error loading model " << *modelIter << '\n';
EXPECT_TRUE(model != NULL);
}
}
// Reset the world, and impose joint limits
world->Reset();
for (modelIter = modelNames.begin();
modelIter != modelNames.end(); ++modelIter)
{
model = world->GetModel(*modelIter);
if (model)
{
std::vector<std::string>::iterator jointIter;
for (jointIter = jointNames.begin();
jointIter != jointNames.end(); ++jointIter)
{
joint = model->GetJoint(*jointIter);
if (joint)
{
joint->SetLowStop(0, math::Angle(-0.1));
joint->SetHighStop(0, math::Angle(0.1));
}
else
{
gzerr << "Error loading joint " << *jointIter
<< " of model " << *modelIter << '\n';
EXPECT_TRUE(joint != NULL);
}
}
}
else
{
gzerr << "Error loading model " << *modelIter << '\n';
EXPECT_TRUE(model != NULL);
}
}
// Step forward again for 0.75 seconds and check that joint angles
// are within limits
world->Step(steps);
for (modelIter = modelNames.begin();
modelIter != modelNames.end(); ++modelIter)
{
model = world->GetModel(*modelIter);
if (model)
{
gzdbg << "Check angle limits and velocity of joints of model "
<< *modelIter << '\n';
std::vector<std::string>::iterator jointIter;
for (jointIter = jointNames.begin();
jointIter != jointNames.end(); ++jointIter)
{
joint = model->GetJoint(*jointIter);
if (joint)
{
EXPECT_NEAR(joint->GetAngle(0).Radian(), 0, 0.11);
}
else
{
gzerr << "Error loading joint " << *jointIter
<< " of model " << *modelIter << '\n';
EXPECT_TRUE(joint != NULL);
}
}
}
else
{
gzerr << "Error loading model " << *modelIter << '\n';
EXPECT_TRUE(model != NULL);
}
}
// Reset world again, disable gravity, detach upper_joint
// Then apply torque at lower_joint and verify motion
world->Reset();
physics->SetGravity(math::Vector3::Zero);
for (modelIter = modelNames.begin();
modelIter != modelNames.end(); ++modelIter)
{
model = world->GetModel(*modelIter);
if (model)
{
gzdbg << "Check SetForce for model " << *modelIter << '\n';
joint = model->GetJoint("upper_joint");
if (joint)
{
if (_physicsEngine == "simbody" ||
_physicsEngine == "dart")
{
gzerr << "Skipping joint detachment per #862 / #903" << std::endl;
continue;
}
// Detach upper_joint.
joint->Detach();
// Simbody and DART will not easily detach joints,
// consider freezing joint limit instead
// math::Angle curAngle = joint->GetAngle(0u);
// joint->SetLowStop(0, curAngle);
// joint->SetHighStop(0, curAngle);
}
else
{
gzerr << "Error loading upper_joint "
<< " of model " << *modelIter << '\n';
EXPECT_TRUE(joint != NULL);
}
// Step forward and let things settle a bit.
world->Step(100);
joint = model->GetJoint("lower_joint");
if (joint)
{
double oldVel, newVel, force;
oldVel = joint->GetVelocity(0);
// Apply positive torque to the lower_joint and step forward.
force = 1;
for (int i = 0; i < 10; ++i)
{
joint->SetForce(0, force);
world->Step(1);
joint->SetForce(0, force);
world->Step(1);
joint->SetForce(0, force);
world->Step(1);
newVel = joint->GetVelocity(0);
// gzdbg << "model " << *modelIter
// << " i " << i
// << " oldVel " << oldVel
// << " newVel " << newVel
// << " upper joint v "
// << model->GetJoint("upper_joint")->GetVelocity(0)
// << " joint " << joint->GetName() << "\n";
// Expect increasing velocities
EXPECT_GT(newVel, oldVel);
oldVel = newVel;
// Check that GetForce returns what we set
EXPECT_NEAR(joint->GetForce(0u), force, g_tolerance);
// Expect joint velocity to be near angular velocity difference
// of child and parent, along global axis
// jointVel == DOT(angVelChild - angVelParent, axis)
double jointVel = joint->GetVelocity(0);
math::Vector3 axis = joint->GetGlobalAxis(0);
angVel = joint->GetChild()->GetWorldAngularVel();
angVel -= joint->GetParent()->GetWorldAngularVel();
EXPECT_NEAR(jointVel, axis.Dot(angVel), g_tolerance);
}
// Apply negative torque to lower_joint
force = -3;
for (int i = 0; i < 10; ++i)
{
joint->SetForce(0, force);
world->Step(1);
joint->SetForce(0, force);
world->Step(1);
joint->SetForce(0, force);
world->Step(1);
newVel = joint->GetVelocity(0);
// gzdbg << "model " << *modelIter
// << " i " << i
// << " oldVel " << oldVel
// << " newVel " << newVel
// << " upper joint v "
// << model->GetJoint("upper_joint")->GetVelocity(0)
// << " joint " << joint->GetName() << "\n";
// Expect decreasing velocities
EXPECT_LT(newVel, oldVel);
// Check that GetForce returns what we set
EXPECT_NEAR(joint->GetForce(0u), force, g_tolerance);
// Expect joint velocity to be near angular velocity difference
// of child and parent, along global axis
// jointVel == DOT(angVelChild - angVelParent, axis)
double jointVel = joint->GetVelocity(0);
math::Vector3 axis = joint->GetGlobalAxis(0);
angVel = joint->GetChild()->GetWorldAngularVel();
angVel -= joint->GetParent()->GetWorldAngularVel();
EXPECT_NEAR(jointVel, axis.Dot(angVel), g_tolerance);
}
}
else
{
gzerr << "Error loading lower_joint "
<< " of model " << *modelIter << '\n';
EXPECT_TRUE(joint != NULL);
}
}
else
{
gzerr << "Error loading model " << *modelIter << '\n';
EXPECT_TRUE(model != NULL);
}
}
}
////////////////////////////////////////////////////////////////////////
void JointTestRevolute::SimplePendulum(const std::string &_physicsEngine)
{
Load("worlds/simple_pendulums.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);
int i = 0;
while (!this->HasEntity("model_1") && i < 20)
{
common::Time::MSleep(100);
++i;
}
if (i > 20)
gzthrow("Unable to get model_1");
physics::PhysicsEnginePtr physicsEngine = world->GetPhysicsEngine();
EXPECT_TRUE(physicsEngine != NULL);
physics::ModelPtr model = world->GetModel("model_1");
EXPECT_TRUE(model != NULL);
physics::LinkPtr link = model->GetLink("link_2"); // sphere link at end
EXPECT_TRUE(link != NULL);
double g = 9.81;
double l = 10.0;
double m = 10.0;
double e_start;
{
// check velocity / energy
math::Vector3 vel = link->GetWorldLinearVel();
math::Pose pos = link->GetWorldPose();
double pe = g * m * pos.pos.z;
double ke = 0.5 * m * (vel.x*vel.x + vel.y*vel.y + vel.z*vel.z);
e_start = pe + ke;
// gzdbg << "total energy [" << e_start
// << "] pe[" << pe
// << "] ke[" << ke
// << "] p[" << pos.pos.z
// << "] v[" << vel
// << "]\n";
}
physicsEngine->SetMaxStepSize(0.0001);
physicsEngine->SetParam("iters", 1000);
{
// test with global contact_max_correcting_vel at 0 as set by world file
// here we expect significant energy loss as the velocity correction
// is set to 0
int steps = 10; // @todo: make this more general
for (int i = 0; i < steps; i ++)
{
world->Step(2000);
{
// check velocity / energy
math::Vector3 vel = link->GetWorldLinearVel();
math::Pose pos = link->GetWorldPose();
double pe = g * m * pos.pos.z;
double ke = 0.5 * m * (vel.x*vel.x + vel.y*vel.y + vel.z*vel.z);
double e = pe + ke;
double e_tol = 3.0*static_cast<double>(i+1)
/ static_cast<double>(steps);
// gzdbg << "total energy [" << e
// << "] pe[" << pe
// << "] ke[" << ke
// << "] p[" << pos.pos.z
// << "] v[" << vel
// << "] error[" << e - e_start
// << "] tol[" << e_tol
// << "]\n";
EXPECT_LT(fabs(e - e_start), e_tol);
}
physics::JointPtr joint = model->GetJoint("joint_0");
if (joint)
{
double integ_theta = (
PendulumAngle(g, l, 1.57079633, 0.0, world->GetSimTime().Double(),
0.000001) - 1.5707963);
double actual_theta = joint->GetAngle(0).Radian();
// gzdbg << "time [" << world->GetSimTime().Double()
// << "] exact [" << integ_theta
// << "] actual [" << actual_theta
// << "] pose [" << model->GetWorldPose()
// << "]\n";
EXPECT_LT(fabs(integ_theta - actual_theta) , 0.01);
}
}
}
{
// test with global contact_max_correcting_vel at 100
// here we expect much lower energy loss
world->Reset();
if (_physicsEngine == "ode")
EXPECT_TRUE(physicsEngine->SetParam("contact_max_correcting_vel", 100.0));
int steps = 10; // @todo: make this more general
for (int i = 0; i < steps; i ++)
{
world->Step(2000);
{
// check velocity / energy
math::Vector3 vel = link->GetWorldLinearVel();
math::Pose pos = link->GetWorldPose();
double pe = g * m * pos.pos.z;
double ke = 0.5 * m * (vel.x*vel.x + vel.y*vel.y + vel.z*vel.z);
double e = pe + ke;
double e_tol = 3.0*static_cast<double>(i+1)
/ static_cast<double>(steps);
// gzdbg << "total energy [" << e
// << "] pe[" << pe
// << "] ke[" << ke
// << "] p[" << pos.pos.z
// << "] v[" << vel
// << "] error[" << e - e_start
// << "] tol[" << e_tol
// << "]\n";
EXPECT_LT(fabs(e - e_start), e_tol);
}
physics::JointPtr joint = model->GetJoint("joint_0");
if (joint)
{
double integ_theta = (
PendulumAngle(g, l, 1.57079633, 0.0, world->GetSimTime().Double(),
0.000001) - 1.5707963);
double actual_theta = joint->GetAngle(0).Radian();
// gzdbg << "time [" << world->GetSimTime().Double()
// << "] exact [" << integ_theta
// << "] actual [" << actual_theta
// << "] pose [" << model->GetWorldPose()
// << "]\n";
EXPECT_LT(fabs(integ_theta - actual_theta) , 0.01);
}
}
}
}
TEST_P(JointTestRevolute, PendulumEnergy)
{
PendulumEnergy(this->physicsEngine);
}
TEST_P(JointTestRevolute, RevoluteJoint)
{
RevoluteJoint(this->physicsEngine);
}
TEST_F(JointTestRevolute, RevoluteJointWorldStep)
{
RevoluteJoint("ode", "world");
}
TEST_P(JointTestRevolute, SimplePendulum)
{
SimplePendulum(this->physicsEngine);
}
TEST_P(JointTestRevolute, UnwrappedAngle)
{
UnwrappedAngle(this->physicsEngine);
}
INSTANTIATE_TEST_CASE_P(PhysicsEngines, JointTestRevolute,
::testing::Combine(PHYSICS_ENGINE_VALUES,
::testing::Values("revolute")));
int main(int argc, char **argv)
{
::testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}