Skip to content

Commit

Permalink
Set friction params in DARTCollision for dart 6.10
Browse files Browse the repository at this point in the history
They are currently being set in DARTLink; set them in DARTCollision
instead if dart 6.10 is in use.
Also set the DART friction direction and enable corresponding tests

Signed-off-by: Steve Peters <scpeters@openrobotics.org>
  • Loading branch information
scpeters committed Jul 8, 2020
1 parent 293ede8 commit 49b5421
Show file tree
Hide file tree
Showing 7 changed files with 44 additions and 5 deletions.
17 changes: 16 additions & 1 deletion gazebo/physics/dart/DARTCollision.cc
Original file line number Diff line number Diff line change
Expand Up @@ -26,6 +26,7 @@
#include "gazebo/physics/dart/DARTCollision.hh"
#include "gazebo/physics/dart/DARTPlaneShape.hh"
#include "gazebo/physics/dart/DARTSurfaceParams.hh"
#include "gazebo/physics/dart/DARTTypes.hh"

#include "gazebo/physics/dart/DARTCollisionPrivate.hh"

Expand Down Expand Up @@ -69,7 +70,7 @@ void DARTCollision::Init()
// Init(), because the BodyNode will only have been created in Load()
// and is not guaranteed to exist before.

// Set the pose offset.
// Set the pose offset and friction parameters.
if (this->dataPtr->dtCollisionShape)
{
// TODO: Remove type check once DART completely supports plane shape.
Expand All @@ -82,6 +83,20 @@ void DARTCollision::Init()
Eigen::Isometry3d tf = DARTTypes::ConvPose(this->RelativePose());
this->dataPtr->dtCollisionShape->setRelativeTransform(tf);
}

#if DART_MAJOR_MINOR_VERSION_AT_LEAST(6, 10)
// Set friction parameters.
auto surf = this->DARTSurface();
GZ_ASSERT(surf, "Surface pointer is invalid");
FrictionPyramidPtr friction = surf->FrictionPyramid();
GZ_ASSERT(friction, "Friction pointer is invalid");
auto aspect = this->dataPtr->dtCollisionShape->getDynamicsAspect();
GZ_ASSERT(aspect, "DynamicsAspect pointer is invalid");
aspect->setFrictionCoeff(friction->MuPrimary());
aspect->setSecondaryFrictionCoeff(friction->MuSecondary());
aspect->setFirstFrictionDirection(
DARTTypes::ConvVec3(friction->direction1));
#endif
}
}

Expand Down
2 changes: 2 additions & 0 deletions gazebo/physics/dart/DARTLink.cc
Original file line number Diff line number Diff line change
Expand Up @@ -204,6 +204,7 @@ void DARTLink::Init()
// Gravity mode
this->SetGravityMode(this->sdf->Get<bool>("gravity"));

#if DART_MAJOR_MINOR_VERSION_AT_MOST(6, 9)
// Friction coefficient

/// \todo FIXME: Friction Parameters
Expand Down Expand Up @@ -246,6 +247,7 @@ void DARTLink::Init()
// friction coefficient may not be negative in DART
coeff = std::max(0.0f, coeff);
this->dataPtr->dtBodyNode->setFrictionCoeff(coeff);
#endif

// We don't add dart body node to the skeleton here because dart body node
// should be set its parent joint before being added. This body node will be
Expand Down
2 changes: 2 additions & 0 deletions gazebo/physics/dart/DARTSurfaceParams.cc
Original file line number Diff line number Diff line change
Expand Up @@ -64,6 +64,8 @@ void DARTSurfaceParams::Load(sdf::ElementPtr _sdf)
frictionOdeElem->Get<double>("mu"));
this->dataPtr->frictionPyramid->SetMuSecondary(
frictionOdeElem->Get<double>("mu2"));
this->dataPtr->frictionPyramid->direction1 =
frictionOdeElem->Get<ignition::math::Vector3d>("fdir1");
}
}
}
Expand Down
4 changes: 3 additions & 1 deletion gazebo/physics/dart/DARTSurfaceParams.hh
Original file line number Diff line number Diff line change
Expand Up @@ -34,7 +34,9 @@ namespace gazebo
/// Forward declare private data class
class DARTSurfaceParamsPrivate;

/// \brief DART surface parameters.
/// \brief Data structure containing DART surface parameters.
/// Updating the parameters in this class doesn't update the
/// actual DART objects.
class GZ_PHYSICS_VISIBLE DARTSurfaceParams : public SurfaceParams
{
/// \brief Constructor.
Expand Down
14 changes: 11 additions & 3 deletions test/integration/physics_friction.cc
Original file line number Diff line number Diff line change
Expand Up @@ -29,6 +29,10 @@
#include "gazebo/test/helper_physics_generator.hh"
#include "gazebo/gazebo_config.h"

#ifdef HAVE_DART
#include "gazebo/physics/dart/DARTTypes.hh"
#endif

using namespace gazebo;

const double g_friction_tolerance = 1e-3;
Expand Down Expand Up @@ -380,13 +384,17 @@ void PhysicsFrictionTest::BoxDirectionRing(const std::string &_physicsEngine)
<< std::endl;
return;
}
#ifdef HAVE_DART
#if DART_MAJOR_MINOR_VERSION_AT_MOST(6, 9)
if (_physicsEngine == "dart")
{
gzerr << "Aborting test since there's an issue with dart's friction"
<< " parameters (#1000)"
gzerr << "Aborting test since dart 6.9 and earlier doesn't support"
<< " body-fixed friction directions (#1000)."
<< std::endl;
return;
}
#endif
#endif

// Load an empty world
Load("worlds/friction_dir_test.world", true, _physicsEngine);
Expand Down Expand Up @@ -454,7 +462,7 @@ void PhysicsFrictionTest::BoxDirectionRing(const std::string &_physicsEngine)
// so spinning the spheres should cause them to start rolling
// check that spheres are spinning about the X axis
auto w = link->WorldAngularVel();
EXPECT_LT(w.X(), -4) << "Checking " << link->GetScopedName() << std::endl;
EXPECT_LT(w.X(), -3) << "Checking " << link->GetScopedName() << std::endl;
}
}

Expand Down
5 changes: 5 additions & 0 deletions test/worlds/friction_dir_test.world
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,11 @@
<uri>model://ground_plane</uri>
</include>
<gravity>0 1 -9.81</gravity>
<physics type="ode">
<dart>
<collision_detector>bullet</collision_detector>
</dart>
</physics>

<model name='spheres'>
<pose>0 3 0.25 0 0 0</pose>
Expand Down
5 changes: 5 additions & 0 deletions test/worlds/friction_dir_test.world.erb
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,11 @@
<uri>model://ground_plane</uri>
</include>
<gravity>0 1 -9.81</gravity>
<physics type="ode">
<dart>
<collision_detector>bullet</collision_detector>
</dart>
</physics>
<%
# Test pyramid friction model
# Set asymmetric friction coefficients and friction direction
Expand Down

0 comments on commit 49b5421

Please sign in to comment.