232 lines
7.8 KiB
C++
232 lines
7.8 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.
|
|
*
|
|
*/
|
|
|
|
#ifndef _JOINT_TEST_HH_
|
|
#define _JOINT_TEST_HH_
|
|
|
|
#include <string>
|
|
#include <sstream>
|
|
|
|
#include "gazebo/test/ServerFixture.hh"
|
|
|
|
#include "gazebo/common/Time.hh"
|
|
#include "gazebo/msgs/msgs.hh"
|
|
#include "gazebo/physics/physics.hh"
|
|
|
|
using namespace gazebo;
|
|
|
|
typedef std::tr1::tuple<const char *, const char *> std_string2;
|
|
|
|
class JointTest : public ServerFixture,
|
|
public ::testing::WithParamInterface<std_string2>
|
|
{
|
|
protected: JointTest() : ServerFixture()
|
|
{
|
|
}
|
|
|
|
/// \brief Test Joint::GetInertiaRatio.
|
|
/// \param[in] _physicsEngine Type of physics engine to use.
|
|
public: void GetInertiaRatio(const std::string &_physicsEngine);
|
|
|
|
/// \brief Test spring dampers
|
|
/// \param[in] _physicsEngine Type of physics engine to use.
|
|
public: void SpringDamperTest(const std::string &_physicsEngine);
|
|
|
|
/// \brief Create and destroy joints repeatedly, monitors memory usage.
|
|
/// \param[in] _physicsEngine Type of physics engine to use.
|
|
public: void JointCreationDestructionTest(const std::string &_physicsEngine);
|
|
|
|
/// \brief Create joints dynamically and verify that they will be visualized.
|
|
/// \param[in] _physicsEngine Type of physics engine to use.
|
|
public: void DynamicJointVisualization(const std::string &_physicsEngine);
|
|
|
|
// Documentation inherited.
|
|
public: virtual void SetUp()
|
|
{
|
|
const ::testing::TestInfo *const test_info =
|
|
::testing::UnitTest::GetInstance()->current_test_info();
|
|
if (test_info->value_param())
|
|
{
|
|
gzdbg << "Params: " << test_info->value_param() << std::endl;
|
|
this->physicsEngine = std::tr1::get<0>(GetParam());
|
|
this->jointType = std::tr1::get<1>(GetParam());
|
|
}
|
|
}
|
|
|
|
/// \brief Class to hold parameters for spawning joints.
|
|
public: class SpawnJointOptions
|
|
{
|
|
/// \brief Constructor.
|
|
public: SpawnJointOptions() : worldChild(false), worldParent(false),
|
|
wait(common::Time(99, 0)),
|
|
noLinkPose(false), axis(math::Vector3(1, 0, 0)),
|
|
useParentModelFrame(false)
|
|
{
|
|
}
|
|
|
|
/// \brief Destructor.
|
|
public: ~SpawnJointOptions()
|
|
{
|
|
}
|
|
|
|
/// \brief Type of joint to create.
|
|
public: std::string type;
|
|
|
|
/// \brief Flag to set child link to the world.
|
|
public: bool worldChild;
|
|
|
|
/// \brief Flag to set parent link to the world.
|
|
public: bool worldParent;
|
|
|
|
/// \brief Length of time to wait for model to spawn in order to return
|
|
/// Joint pointer.
|
|
public: common::Time wait;
|
|
|
|
/// \brief Model pose for spawned model.
|
|
public: math::Pose modelPose;
|
|
|
|
/// \brief Child link pose for spawned model.
|
|
public: math::Pose childLinkPose;
|
|
|
|
/// \brief Parent link pose for spawned model.
|
|
public: math::Pose parentLinkPose;
|
|
|
|
/// \brief Flag to disable including link pose per issue #978.
|
|
public: bool noLinkPose;
|
|
|
|
/// \brief Joint pose for spawned joint.
|
|
public: math::Pose jointPose;
|
|
|
|
/// \brief Axis value for spawned joint.
|
|
public: math::Vector3 axis;
|
|
|
|
/// \brief Use parent model frame (#494)
|
|
public: bool useParentModelFrame;
|
|
};
|
|
|
|
/// \brief Spawn a model with a joint connecting to the world. The function
|
|
/// will wait for duration _wait for the model to spawn and attempt
|
|
/// to return a pointer to the spawned joint. This function is not
|
|
/// guaranteed to return a valid JointPtr, so the output should be
|
|
/// checked.
|
|
/// \param[in] _type Type of joint to create.
|
|
/// \param[in] _worldChild Flag to set child link to the world.
|
|
/// \param[in] _worldParent Flag to set parent link to the world.
|
|
/// \param[in] _wait Length of time to wait for model to spawn in order
|
|
/// to return Joint pointer.
|
|
public: physics::JointPtr SpawnJoint(const std::string &_type,
|
|
bool _worldChild = false,
|
|
bool _worldParent = false,
|
|
common::Time _wait = common::Time(99, 0))
|
|
{
|
|
SpawnJointOptions opt;
|
|
opt.type = _type;
|
|
opt.worldChild = _worldChild;
|
|
opt.worldParent = _worldParent;
|
|
opt.wait = _wait;
|
|
return SpawnJoint(opt);
|
|
}
|
|
|
|
/// \brief Spawn a model with a joint connecting to the world. The function
|
|
/// will wait for duration _wait for the model to spawn and attempt
|
|
/// to return a pointer to the spawned joint. This function is not
|
|
/// guaranteed to return a valid JointPtr, so the output should be
|
|
/// checked.
|
|
/// \param[in] _opt Options for spawned model and joint.
|
|
public: physics::JointPtr SpawnJoint(const SpawnJointOptions &_opt)
|
|
{
|
|
msgs::Model msg;
|
|
std::string modelName = this->GetUniqueString("joint_model");
|
|
msg.set_name(modelName);
|
|
msgs::Set(msg.mutable_pose(), _opt.modelPose.Ign());
|
|
|
|
if (!_opt.worldParent)
|
|
{
|
|
msg.add_link();
|
|
int linkCount = msg.link_size();
|
|
auto link = msg.mutable_link(linkCount-1);
|
|
|
|
link->set_name("parent");
|
|
if (!_opt.noLinkPose)
|
|
{
|
|
msgs::Set(link->mutable_pose(), _opt.parentLinkPose.Ign());
|
|
}
|
|
}
|
|
if (!_opt.worldChild)
|
|
{
|
|
msg.add_link();
|
|
int linkCount = msg.link_size();
|
|
auto link = msg.mutable_link(linkCount-1);
|
|
|
|
link->set_name("child");
|
|
if (!_opt.noLinkPose)
|
|
{
|
|
msgs::Set(link->mutable_pose(), _opt.childLinkPose.Ign());
|
|
}
|
|
}
|
|
msg.add_joint();
|
|
auto jointMsg = msg.mutable_joint(0);
|
|
jointMsg->set_name("joint");
|
|
jointMsg->set_type(msgs::ConvertJointType(_opt.type));
|
|
msgs::Set(jointMsg->mutable_pose(), _opt.jointPose.Ign());
|
|
if (_opt.worldParent)
|
|
{
|
|
jointMsg->set_parent("world");
|
|
}
|
|
else
|
|
{
|
|
jointMsg->set_parent("parent");
|
|
}
|
|
if (_opt.worldChild)
|
|
{
|
|
jointMsg->set_child("world");
|
|
}
|
|
else
|
|
{
|
|
jointMsg->set_child("child");
|
|
}
|
|
|
|
{
|
|
auto axis = jointMsg->mutable_axis1();
|
|
msgs::Set(axis->mutable_xyz(), _opt.axis.Ign());
|
|
axis->set_use_parent_model_frame(_opt.useParentModelFrame);
|
|
}
|
|
// Hack: hardcode a second axis for universal joints
|
|
if (_opt.type == "universal")
|
|
{
|
|
auto axis2 = jointMsg->mutable_axis2();
|
|
msgs::Set(axis2->mutable_xyz(),
|
|
ignition::math::Vector3d(0, 1, 0));
|
|
axis2->set_use_parent_model_frame(_opt.useParentModelFrame);
|
|
}
|
|
|
|
auto model = this->SpawnModel(msg);
|
|
physics::JointPtr joint;
|
|
if (model != NULL)
|
|
joint = model->GetJoint("joint");
|
|
|
|
return joint;
|
|
}
|
|
|
|
/// \brief Physics engine for test.
|
|
protected: std::string physicsEngine;
|
|
|
|
/// \brief Joint type for test.
|
|
protected: std::string jointType;
|
|
};
|
|
#endif
|