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

512 lines
17 KiB
C++
Raw Permalink 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;
const double g_tolerance = 1e-4;
class JointSpawningTest : public JointTest
{
/// \brief Spawn model with each type of joint.
/// \param[in] _physicsEngine Type of physics engine to use.
/// \param[in] _jointType Type of joint to spawn and test.
public: void SpawnJointTypes(const std::string &_physicsEngine,
const std::string &_jointType);
/// \brief Spawn model with rotational joints. Set velocity on parent
/// and make sure child follows.
/// \param[in] _physicsEngine Type of physics engine to use.
/// \param[in] _jointType Type of joint to spawn and test.
public: void SpawnJointRotational(const std::string &_physicsEngine,
const std::string &_jointType);
/// \brief Spawn model with rotational joints. Attach to world and make
/// sure it doesn't fall.
/// \param[in] _physicsEngine Type of physics engine to use.
/// \param[in] _jointType Type of joint to spawn and test.
public: void SpawnJointRotationalWorld(const std::string &_physicsEngine,
const std::string &_jointType);
/// \brief Check joint properties.
/// \param[in] _index Joint angle index.
/// \param[in] _joint Joint to check.
public: void CheckJointProperties(unsigned int _index,
physics::JointPtr _joint);
};
// Fixture for testing all joint types.
class JointSpawningTest_All : public JointSpawningTest {};
// Fixture for testing rotational joints.
class JointSpawningTest_Rotational : public JointSpawningTest {};
// Fixture for testing rotational joints that can be attached to world.
class JointSpawningTest_RotationalWorld : public JointSpawningTest {};
////////////////////////////////////////////////////////////////////////
// Test for spawning each joint type
void JointSpawningTest::SpawnJointTypes(const std::string &_physicsEngine,
const std::string &_jointType)
{
// 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);
{
gzdbg << "SpawnJoint " << _jointType << " child parent" << std::endl;
physics::JointPtr joint = SpawnJoint(_jointType, false, false);
ASSERT_TRUE(joint != NULL);
// Check child and parent links
physics::LinkPtr child = joint->GetChild();
physics::LinkPtr parent = joint->GetParent();
ASSERT_TRUE(child != NULL);
EXPECT_EQ(child->GetParentJoints().size(), 1u);
EXPECT_EQ(child->GetChildJoints().size(), 0u);
ASSERT_TRUE(parent != NULL);
EXPECT_EQ(parent->GetChildJoints().size(), 1u);
EXPECT_EQ(parent->GetParentJoints().size(), 0u);
EXPECT_EQ(_jointType, msgs::ConvertJointType(joint->GetMsgType()));
for (unsigned int i = 0; i < joint->GetAngleCount(); ++i)
{
CheckJointProperties(i, joint);
}
}
if (_jointType == "gearbox")
{
gzerr << "Skip connect to world tests, since we aren't specifying "
<< "the reference body."
<< std::endl;
return;
}
{
gzdbg << "SpawnJoint " << _jointType << " child world" << std::endl;
physics::JointPtr joint = SpawnJoint(_jointType, false, true);
ASSERT_TRUE(joint != NULL);
// Check child link
physics::LinkPtr child = joint->GetChild();
physics::LinkPtr parent = joint->GetParent();
ASSERT_TRUE(child != NULL);
EXPECT_EQ(child->GetParentJoints().size(), 1u);
EXPECT_EQ(child->GetChildJoints().size(), 0u);
EXPECT_TRUE(parent == NULL);
for (unsigned int i = 0; i < joint->GetAngleCount(); ++i)
{
CheckJointProperties(i, joint);
}
}
if (_physicsEngine == "dart")
{
// DART assumes that: (i) every link has its parent joint (ii) root link
// is the only link that doesn't have parent link.
// Child world link breaks dart for now. Do we need to support it?
gzerr << "Skip tests for child world link cases "
<< "since DART does not allow joint with world as child. "
<< "Please see issue #914. "
<< "(https://bitbucket.org/osrf/gazebo/issue/914)"
<< std::endl;
}
else
{
gzdbg << "SpawnJoint " << _jointType << " world parent" << std::endl;
physics::JointPtr joint = SpawnJoint(_jointType, true, false);
ASSERT_TRUE(joint != NULL);
// Check parent link
physics::LinkPtr child = joint->GetChild();
physics::LinkPtr parent = joint->GetParent();
EXPECT_TRUE(child == NULL);
ASSERT_TRUE(parent != NULL);
EXPECT_EQ(parent->GetChildJoints().size(), 1u);
EXPECT_EQ(parent->GetParentJoints().size(), 0u);
for (unsigned int i = 0; i < joint->GetAngleCount(); ++i)
{
CheckJointProperties(i, joint);
}
}
}
////////////////////////////////////////////////////////////////////////
// Test for non-translational joints.
// Set velocity to parent and make sure child follows.
void JointSpawningTest::SpawnJointRotational(const std::string &_physicsEngine,
const std::string &_jointType)
{
// 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);
gzdbg << "SpawnJoint " << _jointType << std::endl;
physics::JointPtr joint = SpawnJoint(_jointType);
ASSERT_TRUE(joint != NULL);
physics::LinkPtr parent, child;
child = joint->GetChild();
parent = joint->GetParent();
ASSERT_TRUE(child != NULL);
ASSERT_TRUE(parent != NULL);
math::Vector3 pos(10, 10, 10);
math::Vector3 vel(10, 10, 10);
parent->SetWorldPose(math::Pose(pos, math::Quaternion()));
for (unsigned int i = 0; i < 10; ++i)
{
parent->SetLinearVel(vel);
world->Step(10);
}
world->Step(50);
math::Pose childPose = child->GetWorldPose();
math::Pose parentPose = parent->GetWorldPose();
EXPECT_TRUE(parentPose.pos != pos);
EXPECT_TRUE(parentPose.pos != math::Vector3::Zero);
EXPECT_TRUE(childPose.pos != math::Vector3::Zero);
EXPECT_TRUE(childPose.pos == parentPose.pos);
EXPECT_EQ(joint->GetWorldPose().pos, joint->GetParentWorldPose().pos);
EXPECT_EQ(joint->GetAnchorErrorPose().pos, math::Vector3::Zero);
}
////////////////////////////////////////////////////////////////////////
// Test for non-translational joints that can attach to world.
// Attach to world and see if it doesn't fall.
void JointSpawningTest::SpawnJointRotationalWorld(
const std::string &_physicsEngine,
const std::string &_jointType)
{
// 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);
physics::JointPtr joint;
for (unsigned int i = 0; i < 2; ++i)
{
bool worldChild = (i == 0);
bool worldParent = (i == 1);
std::string child = worldChild ? "world" : "child";
std::string parent = worldParent ? "world" : "parent";
gzdbg << "SpawnJoint " << _jointType << " "
<< child << " "
<< parent << std::endl;
if ((_physicsEngine == "dart" || _physicsEngine == "simbody")
&& worldChild)
{
// These physics engines don't support world as a child link.
// simbody https://bitbucket.org/osrf/gazebo/issue/861
// dart https://bitbucket.org/osrf/gazebo/issue/914
gzerr << "Skip tests for child world link cases since "
<< _physicsEngine
<< " does not allow joint with world as child. "
<< "Please see bitbucket issues #861, #914."
<< std::endl;
continue;
}
joint = SpawnJoint(_jointType, worldChild, worldParent);
ASSERT_TRUE(joint != NULL);
physics::LinkPtr link;
if (!worldChild)
link = joint->GetChild();
else if (!worldParent)
link = joint->GetParent();
ASSERT_TRUE(link != NULL);
math::Pose initialPose = link->GetWorldPose();
world->Step(100);
math::Pose afterPose = link->GetWorldPose();
EXPECT_TRUE(initialPose.pos == afterPose.pos);
EXPECT_EQ(joint->GetWorldPose().pos, joint->GetParentWorldPose().pos);
EXPECT_EQ(joint->GetAnchorErrorPose().pos, math::Vector3::Zero);
}
}
/////////////////////////////////////////////////
void JointSpawningTest::CheckJointProperties(unsigned int _index,
physics::JointPtr _joint)
{
physics::WorldPtr world = physics::get_world();
ASSERT_TRUE(world != NULL);
physics::PhysicsEnginePtr physics = world->GetPhysicsEngine();
ASSERT_TRUE(physics != NULL);
bool isBullet = physics->GetType().compare("bullet") == 0;
bool isDart = physics->GetType().compare("dart") == 0;
bool isSimbody = physics->GetType().compare("simbody") == 0;
double dt = physics->GetMaxStepSize();
if (_joint->HasType(physics::Base::HINGE2_JOINT) ||
_joint->HasType(physics::Base::GEARBOX_JOINT) ||
_joint->HasType(physics::Base::SCREW_JOINT))
{
gzerr << "This portion of the test fails for this joint type" << std::endl;
return;
}
if (!_joint->GetChild())
{
gzerr << "The rest of this test fails without a child link" << std::endl;
return;
}
// Reset world prior to testing SetVelocity
// This is needed for SimbodyUniversalJoint
world->Reset();
double velocityMagnitude = 1.0;
std::vector<double> velocities;
velocities.push_back(velocityMagnitude);
velocities.push_back(0.0);
velocities.push_back(-velocityMagnitude);
for (std::vector<double>::iterator iter = velocities.begin();
iter != velocities.end(); ++iter)
{
// Use Joint::SetVelocity with different values
double vel = *iter;
_joint->SetVelocity(_index, vel);
// Verify that Joint::GetVelocity returns the same value
EXPECT_NEAR(_joint->GetVelocity(_index), vel, g_tolerance);
// Take some steps and verify that it keeps spinning at same speed
world->Step(50);
EXPECT_NEAR(_joint->GetVelocity(_index), vel, g_tolerance);
}
// Test SetForce with positive value
{
// reset world and expect joint to be stopped at home position
world->Reset();
EXPECT_NEAR(_joint->GetAngle(_index).Radian(), 0.0, g_tolerance);
EXPECT_NEAR(_joint->GetVelocity(_index), 0.0, g_tolerance);
// set positive force
double angleStart = _joint->GetAngle(_index).Radian();
_joint->SetForce(_index, 5);
world->Step(1);
EXPECT_GT(_joint->GetVelocity(_index), 0.0);
world->Step(1);
EXPECT_GT(_joint->GetAngle(_index).Radian(), angleStart);
}
// Test SetForce with negative value
{
// reset world and expect joint to be stopped at home position
world->Reset();
EXPECT_NEAR(_joint->GetAngle(_index).Radian(), 0.0, g_tolerance);
EXPECT_NEAR(_joint->GetVelocity(_index), 0.0, g_tolerance);
// set negative force
double angleStart = _joint->GetAngle(_index).Radian();
_joint->SetForce(_index, -5);
world->Step(1);
EXPECT_LT(_joint->GetVelocity(_index), 0.0);
world->Step(1);
EXPECT_LT(_joint->GetAngle(_index).Radian(), angleStart);
}
// Test Coloumb friction
if (isBullet && _joint->HasType(physics::Base::UNIVERSAL_JOINT))
{
gzerr << "Skipping friction test for "
<< physics->GetType()
<< " "
<< msgs::ConvertJointType(_joint->GetMsgType())
<< " joint"
<< std::endl;
}
else if (isSimbody)
{
gzerr << "Skipping friction test for "
<< physics->GetType()
<< std::endl;
}
else
{
// reset world and expect joint to be stopped at home position
world->Reset();
EXPECT_NEAR(_joint->GetAngle(_index).Radian(), 0.0, g_tolerance);
EXPECT_NEAR(_joint->GetVelocity(_index), 0.0, g_tolerance);
// set friction to 1.0
const double friction = 1.0;
EXPECT_TRUE(_joint->SetParam("friction", _index, friction));
EXPECT_NEAR(_joint->GetParam("friction", _index), friction, g_tolerance);
for (unsigned int i = 0; i < 500; ++i)
{
// Apply force with smaller magnitude than friction
_joint->SetForce(_index, friction * 0.5);
world->Step(1);
}
// Expect no motion
EXPECT_NEAR(_joint->GetVelocity(_index), 0.0, g_tolerance);
EXPECT_NEAR(_joint->GetAngle(_index).Radian(), 0.0, g_tolerance);
for (unsigned int i = 0; i < 500; ++i)
{
// Apply force with larger magnitude than friction
_joint->SetForce(_index, friction * 1.5);
world->Step(1);
}
// Expect motion
EXPECT_GT(_joint->GetVelocity(_index), 0.2 * friction);
EXPECT_GT(_joint->GetAngle(_index).Radian(), 0.05 * friction);
// DART has problem with joint friction and joint limits
// https://github.com/dartsim/dart/issues/317
// Set friction back to zero to not interfere with other tests
// until this issue is resolved.
if (isDart)
{
EXPECT_TRUE(_joint->SetParam("friction", _index, 0.0));
EXPECT_NEAR(_joint->GetParam("friction", _index), 0.0, g_tolerance);
}
}
// SetHighStop
{
// reset world and expect joint to be stopped at home position
world->Reset();
EXPECT_NEAR(_joint->GetAngle(_index).Radian(), 0.0, g_tolerance);
EXPECT_NEAR(_joint->GetVelocity(_index), 0.0, g_tolerance);
unsigned int steps = 100;
double vel = 1.0;
math::Angle limit = math::Angle(steps * dt * vel * 0.5);
_joint->SetHighStop(_index, limit);
_joint->SetVelocity(_index, vel);
world->Step(steps);
EXPECT_LT(_joint->GetAngle(_index).Radian(), limit.Radian() + g_tolerance);
EXPECT_EQ(_joint->GetHighStop(_index), limit);
{
boost::any value = _joint->GetParam("hi_stop", _index);
EXPECT_NEAR(boost::any_cast<double>(value), limit.Radian(), g_tolerance);
}
}
// SetLowStop
{
// reset world and expect joint to be stopped at home position
world->Reset();
EXPECT_NEAR(_joint->GetAngle(_index).Radian(), 0.0, g_tolerance);
EXPECT_NEAR(_joint->GetVelocity(_index), 0.0, g_tolerance);
unsigned int steps = 100;
double vel = -1.0;
math::Angle limit = math::Angle(steps * dt * vel * 0.5);
_joint->SetLowStop(_index, limit);
_joint->SetVelocity(_index, vel);
world->Step(steps);
EXPECT_GT(_joint->GetAngle(_index).Radian(), limit.Radian() - g_tolerance);
EXPECT_EQ(_joint->GetLowStop(_index), limit);
{
boost::any value = _joint->GetParam("lo_stop", _index);
EXPECT_NEAR(boost::any_cast<double>(value), limit.Radian(), g_tolerance);
}
}
}
TEST_P(JointSpawningTest_All, SpawnJointTypes)
{
if (this->jointType == "gearbox" && this->physicsEngine != "ode")
{
gzerr << "Skip test, gearbox is only supported in ODE. " <<
"See issues: #859, #1914, #1915" << std::endl;
return;
}
if (physicsEngine == "simbody" && jointType == "revolute2")
{
gzerr << "Skip test, revolute2 not supported in simbody, see issue #859."
<< std::endl;
return;
}
SpawnJointTypes(this->physicsEngine, this->jointType);
}
TEST_P(JointSpawningTest_Rotational, SpawnJointRotational)
{
SpawnJointRotational(this->physicsEngine, this->jointType);
}
TEST_P(JointSpawningTest_RotationalWorld, SpawnJointRotationalWorld)
{
SpawnJointRotationalWorld(this->physicsEngine, this->jointType);
}
INSTANTIATE_TEST_CASE_P(TestRuns, JointSpawningTest_All,
::testing::Combine(PHYSICS_ENGINE_VALUES,
::testing::Values("revolute"
, "prismatic"
, "screw"
, "universal"
, "fixed"
, "ball"
, "revolute2"
, "gearbox")));
// Skip prismatic, screw, and revolute2 because they allow translation
INSTANTIATE_TEST_CASE_P(TestRuns, JointSpawningTest_Rotational,
::testing::Combine(PHYSICS_ENGINE_VALUES,
::testing::Values("revolute"
, "universal"
, "fixed"
, "ball")));
// Skip prismatic, screw, and revolute2 because they allow translation
INSTANTIATE_TEST_CASE_P(TestRuns, JointSpawningTest_RotationalWorld,
::testing::Combine(PHYSICS_ENGINE_VALUES,
::testing::Values("revolute"
, "universal"
, "fixed"
, "ball")));
int main(int argc, char **argv)
{
::testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}
Reference in New Issue View Git Blame Copy Permalink