243 lines
7.4 KiB
C++
243 lines
7.4 KiB
C++
/*
|
|
* Copyright (C) 2015 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 <math.h>
|
|
|
|
#include <gazebo/msgs/msgs.hh>
|
|
#include <gazebo/physics/physics.hh>
|
|
#include <gazebo/test/helper_physics_generator.hh>
|
|
#include <gazebo/test/ServerFixture.hh>
|
|
#include <gazebo/transport/transport.hh>
|
|
|
|
#include <ignition/math/Rand.hh>
|
|
|
|
#define PHYSICS_TOL 0.008
|
|
|
|
using namespace gazebo;
|
|
|
|
class PhysicsTest : public ServerFixture,
|
|
public testing::WithParamInterface<const char*>
|
|
{
|
|
/// \brief test elastic modulus
|
|
public: void ElasticModulusContact(const std::string &_physicsEngine);
|
|
|
|
/// \brief Callback for contact subscribers in depth test.
|
|
/// \param[in] _msg Contact message
|
|
private: void ContactCallback(const ConstContactsPtr &_msg);
|
|
|
|
/// \brief Message to be filled with the latest contacts message.
|
|
private: msgs::Contacts contactsMsg;
|
|
|
|
/// \brief Mutex to protect reads and writes to contactsMsg.
|
|
public: mutable boost::mutex mutex;
|
|
};
|
|
|
|
/////////////////////////////////////////////////
|
|
void PhysicsTest::ContactCallback(const ConstContactsPtr &_msg)
|
|
{
|
|
boost::mutex::scoped_lock lock(this->mutex);
|
|
this->contactsMsg = *_msg;
|
|
}
|
|
|
|
////////////////////////////////////////////////////////////////////////
|
|
void PhysicsTest::ElasticModulusContact(const std::string &_physicsEngine)
|
|
{
|
|
if (_physicsEngine != "ode")
|
|
{
|
|
gzerr << "Elastic Modulus is only implemented for ODE.\n";
|
|
return;
|
|
}
|
|
|
|
// check conservation of mementum for linear elastic collision
|
|
Load("worlds/elastic_modulus_contact_test.world", true, _physicsEngine);
|
|
physics::WorldPtr world = physics::get_world("default");
|
|
EXPECT_TRUE(world != NULL);
|
|
physics::PhysicsEnginePtr physics = world->GetPhysicsEngine();
|
|
|
|
int i = 0;
|
|
while (!this->HasEntity("sphere") && i < 20)
|
|
{
|
|
common::Time::MSleep(100);
|
|
++i;
|
|
}
|
|
|
|
if (i > 20)
|
|
gzthrow("Unable to get sphere");
|
|
|
|
// get models and links
|
|
physics::ModelPtr box_model = world->GetModel("box");
|
|
physics::LinkPtr box_link = box_model->GetLink("link");
|
|
physics::ModelPtr sphere_model = world->GetModel("sphere");
|
|
physics::LinkPtr sphere_link = sphere_model->GetLink("link");
|
|
|
|
// Sleep to ensure transport topics are all advertised
|
|
common::Time::MSleep(100);
|
|
std::list<std::string> topics =
|
|
transport::getAdvertisedTopics("gazebo.msgs.Contacts");
|
|
topics.sort();
|
|
EXPECT_FALSE(topics.empty());
|
|
EXPECT_EQ(topics.size(), 1u);
|
|
|
|
auto topic = topics.front();
|
|
|
|
gzdbg << "Listening to " << topic << std::endl;
|
|
transport::SubscriberPtr sub = this->node->Subscribe(topic,
|
|
&PhysicsTest::ContactCallback, this);
|
|
|
|
// step to let contact happen and settle
|
|
world->Step(3000);
|
|
|
|
// Wait for contact messages to be received
|
|
int maxSleep = 100;
|
|
int sleep = 0;
|
|
while (this->contactsMsg.contact().size() < 1 && sleep < maxSleep)
|
|
{
|
|
common::Time::MSleep(100);
|
|
sleep++;
|
|
}
|
|
ASSERT_EQ(this->contactsMsg.contact().size(), 1);
|
|
|
|
// recorded from opende/src/joints/contact.cpp:208
|
|
const double d_converged = 0.0258629;
|
|
const double k_converged = 71454.617;
|
|
const double k_lin_converged = 11491.286;
|
|
|
|
// sphere
|
|
const double nu1 = 0.4;
|
|
const double e1 = 600000;
|
|
const double m1 = 3;
|
|
const double r1 = 0.06;
|
|
// static box
|
|
const double nu2 = 0.3;
|
|
const double e2 = 500000;
|
|
const double r2 = 0.08;
|
|
double eStar = 1.0 / ((1.0 - nu1*nu1)/e1 + (1.0 - nu2*nu2)/e2);
|
|
double rStar = 1.0 / (1.0/r1 + 1.0/r2);
|
|
// contact force
|
|
double f1 = -physics->GetGravity().x * m1;
|
|
|
|
for (int n = 0; n < 10; ++n)
|
|
{
|
|
world->Step(1);
|
|
// Copy message to local variable
|
|
msgs::Contacts contacts;
|
|
{
|
|
boost::mutex::scoped_lock lock(this->mutex);
|
|
contacts = this->contactsMsg;
|
|
}
|
|
|
|
// recompute stiffness by hand to double check physics
|
|
|
|
// get min contact depth from model
|
|
const double minDepth = std::min(0.0015, 0.001);
|
|
|
|
// GET CONTACT DEPTH FROM CONTACT MANAGER
|
|
ASSERT_EQ(contacts.contact().size(), 1);
|
|
double d1 = 0;
|
|
for (auto const &contact : contacts.contact())
|
|
{
|
|
gzdbg << "col1 [" << contact.collision1()
|
|
<< "] col2 [" << contact.collision2()
|
|
<< "]\n";
|
|
EXPECT_EQ(contact.depth().size(), 1);
|
|
for (auto const &d : contact.depth())
|
|
{
|
|
d1 = d - minDepth;
|
|
// gzerr << d1 << "\n";
|
|
}
|
|
}
|
|
double d_convergence_error = (d1 - d_converged)/d_converged;
|
|
EXPECT_LT(d1, d_converged);
|
|
EXPECT_LT(fabs(d_convergence_error), PHYSICS_TOL);
|
|
|
|
// GET CONTACT DEPTH FROM LINK POSES AND KNOWN GEOMETRY INFORMATION
|
|
double d2 = 1.0 - (sphere_link->GetWorldPose().pos.x -
|
|
box_link->GetWorldPose().pos.x) - minDepth;
|
|
|
|
EXPECT_FLOAT_EQ(d1, d2);
|
|
|
|
// GET CONTACT DEPTH BASED ON CONTACT MODEL
|
|
// k = f / d^1.5 or f = k * d^1.5.
|
|
// And we know that:
|
|
// k = 4.0 / 3.0 * e_star * sqrt(patch_radius);
|
|
// or
|
|
// k = 4.0 / 3.0 * e_star * sqrt(sqrt(rStar * depth));
|
|
// and
|
|
// f = k * depth^1.5
|
|
// therefore
|
|
// f = 4.0 / 3.0 * e_star * sqrt(sqrt(rStar)) * depth^1.75
|
|
// knowing f,
|
|
// solving for d yeilds analytical depth:
|
|
double d3 = pow(f1 / (4.0 / 3.0 * eStar * sqrt(sqrt(rStar))), 1.0/1.75);
|
|
|
|
// check that d1 and d3 should be near
|
|
EXPECT_LT(fabs(d1-d3)/d3, PHYSICS_TOL);
|
|
|
|
// linearized contact patch radius
|
|
double patchRadius = sqrt(rStar * d1);
|
|
|
|
// check stiffness
|
|
double k = 4.0 / 3.0 * eStar * sqrt(patchRadius);
|
|
// check linearized stiffness (should match ode internal k_hertz_sqrtx)
|
|
double k_lin = 4.0 / 3.0 * eStar * sqrt(patchRadius*d1);
|
|
|
|
// recorded from opende/src/joints/contact.cpp:208
|
|
EXPECT_FLOAT_EQ(k, k_converged);
|
|
// recorded from opende/src/joints/contact.cpp:208
|
|
EXPECT_FLOAT_EQ(k_lin, k_lin_converged);
|
|
|
|
gzdbg << "Contact State:\n"
|
|
<< " t [" << world->GetSimTime().Double()
|
|
<< "]\n f1 [" << f1
|
|
<< "]\n E* [" << eStar
|
|
<< "]\n R* [" << rStar
|
|
<< "]\n patch radius [" << patchRadius
|
|
<< "]\n d contact manager [" << d1
|
|
<< "]\n d convergence error [" << d_convergence_error
|
|
<< "]\n d geom [" << d2
|
|
<< "]\n d solution [" << d3
|
|
<< "]\n d relative error [" << (d1-d3)/d3
|
|
<< "]\n patch radius [" << patchRadius
|
|
<< "]\n k(Hertzian) [" << k
|
|
<< "]\n k relative error [" << (k - k_converged)/k_converged
|
|
<< "]\n k(liearized about depth) [" << k_lin
|
|
<< "]\n k lin relative error ["
|
|
<< (k_lin - k_lin_converged)/k_lin_converged
|
|
<< "] \n";
|
|
}
|
|
}
|
|
|
|
/////////////////////////////////////////////////
|
|
TEST_P(PhysicsTest, ElasticModulusContact)
|
|
{
|
|
ElasticModulusContact(GetParam());
|
|
}
|
|
|
|
/////////////////////////////////////////////////
|
|
INSTANTIATE_TEST_CASE_P(PhysicsEngines, PhysicsTest, PHYSICS_ENGINE_VALUES);
|
|
|
|
/////////////////////////////////////////////////
|
|
int main(int argc, char **argv)
|
|
{
|
|
::testing::InitGoogleTest(&argc, argv);
|
|
return RUN_ALL_TESTS();
|
|
}
|
|
|