264 lines
8.8 KiB
C++
264 lines
8.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.
|
|
*
|
|
*/
|
|
|
|
#include "gazebo/common/Assert.hh"
|
|
#include "gazebo/common/Console.hh"
|
|
#include "gazebo/physics/ode/ODESurfaceParams.hh"
|
|
|
|
using namespace gazebo;
|
|
using namespace physics;
|
|
|
|
//////////////////////////////////////////////////
|
|
ODESurfaceParams::ODESurfaceParams()
|
|
: SurfaceParams(),
|
|
bounce(0), bounceThreshold(100000),
|
|
kp(1000000000000), kd(1), cfm(0), erp(0.2),
|
|
maxVel(0.01), minDepth(0),
|
|
slip1(0), slip2(0),
|
|
frictionPyramid(new physics::FrictionPyramid())
|
|
{
|
|
}
|
|
|
|
//////////////////////////////////////////////////
|
|
ODESurfaceParams::~ODESurfaceParams()
|
|
{
|
|
}
|
|
|
|
//////////////////////////////////////////////////
|
|
void ODESurfaceParams::Load(sdf::ElementPtr _sdf)
|
|
{
|
|
// Load parent class
|
|
SurfaceParams::Load(_sdf);
|
|
|
|
if (!_sdf)
|
|
gzerr << "Surface _sdf is NULL" << std::endl;
|
|
else
|
|
{
|
|
{
|
|
sdf::ElementPtr bounceElem = _sdf->GetElement("bounce");
|
|
if (!bounceElem)
|
|
gzerr << "Surface bounce sdf member is NULL" << std::endl;
|
|
else
|
|
{
|
|
this->bounce = bounceElem->Get<double>("restitution_coefficient");
|
|
if (this->bounce < 0)
|
|
{
|
|
gzwarn << "bounce restitution_coefficient ["
|
|
<< this->bounce
|
|
<< "] < 0, so it will not be applied by ODE."
|
|
<< std::endl;
|
|
}
|
|
else if (this->bounce > 1)
|
|
{
|
|
gzwarn << "bounce restitution_coefficient ["
|
|
<< this->bounce
|
|
<< "] > 1, which is outside the recommended range."
|
|
<< std::endl;
|
|
}
|
|
this->bounceThreshold = bounceElem->Get<double>("threshold");
|
|
}
|
|
}
|
|
|
|
{
|
|
sdf::ElementPtr frictionElem = _sdf->GetElement("friction");
|
|
if (!frictionElem)
|
|
gzerr << "Surface friction sdf member is NULL" << std::endl;
|
|
else
|
|
{
|
|
sdf::ElementPtr torsionalElem = frictionElem->GetElement("torsional");
|
|
if (torsionalElem)
|
|
{
|
|
this->frictionPyramid->SetMuTorsion(
|
|
torsionalElem->Get<double>("coefficient"));
|
|
this->frictionPyramid->SetPatchRadius(
|
|
torsionalElem->Get<double>("patch_radius"));
|
|
this->frictionPyramid->SetSurfaceRadius(
|
|
torsionalElem->Get<double>("surface_radius"));
|
|
this->frictionPyramid->SetUsePatchRadius(
|
|
torsionalElem->Get<bool>("use_patch_radius"));
|
|
|
|
sdf::ElementPtr torsionalOdeElem = torsionalElem->GetElement("ode");
|
|
if (torsionalOdeElem)
|
|
this->slipTorsion = torsionalOdeElem->Get<double>("slip");
|
|
}
|
|
|
|
sdf::ElementPtr frictionOdeElem = frictionElem->GetElement("ode");
|
|
if (!frictionOdeElem)
|
|
gzerr << "Surface friction ode sdf member is NULL" << std::endl;
|
|
else
|
|
{
|
|
this->frictionPyramid->SetMuPrimary(
|
|
frictionOdeElem->Get<double>("mu"));
|
|
this->frictionPyramid->SetMuSecondary(
|
|
frictionOdeElem->Get<double>("mu2"));
|
|
this->frictionPyramid->direction1 =
|
|
frictionOdeElem->Get<math::Vector3>("fdir1");
|
|
|
|
this->slip1 = frictionOdeElem->Get<double>("slip1");
|
|
this->slip2 = frictionOdeElem->Get<double>("slip2");
|
|
}
|
|
}
|
|
}
|
|
|
|
{
|
|
sdf::ElementPtr contactElem = _sdf->GetElement("contact");
|
|
if (!contactElem)
|
|
gzerr << "Surface contact sdf member is NULL" << std::endl;
|
|
else
|
|
{
|
|
sdf::ElementPtr contactOdeElem = contactElem->GetElement("ode");
|
|
if (!contactOdeElem)
|
|
gzerr << "Surface contact ode sdf member is NULL" << std::endl;
|
|
{
|
|
this->kp = contactOdeElem->Get<double>("kp");
|
|
this->kd = contactOdeElem->Get<double>("kd");
|
|
this->cfm = contactOdeElem->Get<double>("soft_cfm");
|
|
this->erp = contactOdeElem->Get<double>("soft_erp");
|
|
this->maxVel = contactOdeElem->Get<double>("max_vel");
|
|
this->minDepth = contactOdeElem->Get<double>("min_depth");
|
|
}
|
|
this->frictionPyramid->SetPoissonsRatio(
|
|
contactElem->Get<double>("poissons_ratio"));
|
|
this->frictionPyramid->SetElasticModulus(
|
|
contactElem->Get<double>("elastic_modulus"));
|
|
// gzerr << "poisson: [" << this->frictionPyramid->PoissonsRatio()
|
|
// << "] em: [" << this->frictionPyramid->ElasticModulus()
|
|
// << "] \n";
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
/////////////////////////////////////////////////
|
|
void ODESurfaceParams::FillMsg(msgs::Surface &_msg)
|
|
{
|
|
SurfaceParams::FillMsg(_msg);
|
|
|
|
_msg.mutable_friction()->set_mu(this->frictionPyramid->MuPrimary());
|
|
_msg.mutable_friction()->set_mu2(this->frictionPyramid->MuSecondary());
|
|
_msg.mutable_friction()->set_slip1(this->slip1);
|
|
_msg.mutable_friction()->set_slip2(this->slip2);
|
|
msgs::Set(_msg.mutable_friction()->mutable_fdir1(),
|
|
this->frictionPyramid->direction1.Ign());
|
|
|
|
_msg.mutable_friction()->mutable_torsional()->set_coefficient(
|
|
this->frictionPyramid->MuTorsion());
|
|
|
|
_msg.mutable_friction()->mutable_torsional()->set_patch_radius(
|
|
this->frictionPyramid->PatchRadius());
|
|
_msg.mutable_friction()->mutable_torsional()->set_surface_radius(
|
|
this->frictionPyramid->SurfaceRadius());
|
|
_msg.mutable_friction()->mutable_torsional()->set_use_patch_radius(
|
|
this->frictionPyramid->UsePatchRadius());
|
|
_msg.mutable_friction()->mutable_torsional()->mutable_ode()->set_slip(
|
|
this->slipTorsion);
|
|
|
|
_msg.set_restitution_coefficient(this->bounce);
|
|
_msg.set_bounce_threshold(this->bounceThreshold);
|
|
|
|
_msg.set_soft_cfm(this->cfm);
|
|
_msg.set_soft_erp(this->erp);
|
|
_msg.set_elastic_modulus(this->frictionPyramid->ElasticModulus());
|
|
_msg.set_kp(this->kp);
|
|
_msg.set_kd(this->kd);
|
|
_msg.set_max_vel(this->maxVel);
|
|
_msg.set_min_depth(this->minDepth);
|
|
}
|
|
|
|
/////////////////////////////////////////////////
|
|
void ODESurfaceParams::ProcessMsg(const msgs::Surface &_msg)
|
|
{
|
|
SurfaceParams::ProcessMsg(_msg);
|
|
|
|
if (_msg.has_friction())
|
|
{
|
|
if (_msg.friction().has_mu())
|
|
this->frictionPyramid->SetMuPrimary(_msg.friction().mu());
|
|
if (_msg.friction().has_mu2())
|
|
this->frictionPyramid->SetMuSecondary(_msg.friction().mu2());
|
|
if (_msg.friction().has_slip1())
|
|
this->slip1 = _msg.friction().slip1();
|
|
if (_msg.friction().has_slip2())
|
|
this->slip2 = _msg.friction().slip2();
|
|
if (_msg.friction().has_fdir1())
|
|
{
|
|
this->frictionPyramid->direction1 =
|
|
msgs::ConvertIgn(_msg.friction().fdir1());
|
|
}
|
|
|
|
if (_msg.friction().has_torsional())
|
|
{
|
|
if (_msg.friction().torsional().has_coefficient())
|
|
{
|
|
this->frictionPyramid->SetMuTorsion(
|
|
_msg.friction().torsional().coefficient());
|
|
}
|
|
if (_msg.friction().torsional().has_patch_radius())
|
|
{
|
|
this->frictionPyramid->SetPatchRadius(
|
|
_msg.friction().torsional().patch_radius());
|
|
}
|
|
if (_msg.friction().torsional().has_surface_radius())
|
|
{
|
|
this->frictionPyramid->SetSurfaceRadius(
|
|
_msg.friction().torsional().surface_radius());
|
|
}
|
|
if (_msg.friction().torsional().has_use_patch_radius())
|
|
{
|
|
this->frictionPyramid->SetUsePatchRadius(
|
|
_msg.friction().torsional().use_patch_radius());
|
|
}
|
|
if (_msg.friction().torsional().has_ode())
|
|
{
|
|
if (_msg.friction().torsional().ode().has_slip())
|
|
this->slipTorsion = _msg.friction().torsional().ode().slip();
|
|
}
|
|
}
|
|
}
|
|
|
|
if (_msg.has_restitution_coefficient())
|
|
this->bounce = _msg.restitution_coefficient();
|
|
if (_msg.has_bounce_threshold())
|
|
this->bounceThreshold = _msg.bounce_threshold();
|
|
if (_msg.has_soft_cfm())
|
|
this->cfm = _msg.soft_cfm();
|
|
if (_msg.has_soft_erp())
|
|
this->erp = _msg.soft_erp();
|
|
if (_msg.has_elastic_modulus())
|
|
this->frictionPyramid->SetElasticModulus(_msg.elastic_modulus());
|
|
if (_msg.has_kp())
|
|
this->kp = _msg.kp();
|
|
if (_msg.has_kd())
|
|
this->kd = _msg.kd();
|
|
if (_msg.has_max_vel())
|
|
this->maxVel = _msg.max_vel();
|
|
if (_msg.has_min_depth())
|
|
this->minDepth = _msg.min_depth();
|
|
}
|
|
|
|
/////////////////////////////////////////////////
|
|
FrictionPyramidPtr ODESurfaceParams::GetFrictionPyramid() const
|
|
{
|
|
return this->frictionPyramid;
|
|
}
|
|
|
|
/////////////////////////////////////////////////
|
|
FrictionPyramidPtr ODESurfaceParams::FrictionPyramid() const
|
|
{
|
|
return this->frictionPyramid;
|
|
}
|