Set slip compliance

dartsim/src/SDFFeatures.cc

@@ -487,7 +487,7 @@ Identity SDFFeatures::ConstructSdfCollision(
                                   ->GetElement("surface")
                                   ->GetElement("friction")
                                   ->GetElement("ode");
-
+#if DART_VERSION_AT_LEAST(6, 10, 0)
     auto aspect = node->getDynamicsAspect();
     aspect->setFrictionCoeff(odeFriction->Get<double>("mu"));
     if (odeFriction->HasElement("mu2"))
@@ -508,7 +508,14 @@ Identity SDFFeatures::ConstructSdfCollision(
       aspect->setFirstFrictionDirection(math::eigen3::convert(fdir1));
     }
   }
-
+#else
+    // We are setting the friction coefficient of a collision element
+    // to be the coefficient for the whole link. If there are multiple collision
+    // elements, the value of the last one will be the coefficient for the link.
+    // TODO(addisu) Assign the coefficient to the shape node when support is
+    // added in DART.
+    bn->setFrictionCoeff(odeFriction->Get<double>("mu"));
+#endif
   node->setRelativeTransform(
         math::eigen3::convert(_collision.Pose()) * tf_shape);
 

dartsim/src/ShapeFeatures.cc

@@ -328,6 +328,80 @@ AlignedBox3d ShapeFeatures::GetShapeAxisAlignedBoundingBox(
   return AlignedBox3d(box.getMin(), box.getMax());
 }
 
+/////////////////////////////////////////////////
+double ShapeFeatures::GetShapeFrictionPyramidPrimarySlipCompliance(
+    const Identity &_shapeID) const
+{
+  auto &node = this->ReferenceInterface<ShapeInfo>(_shapeID)->node;
+  auto aspect = node->getDynamicsAspect();
+  if (nullptr == aspect)
+  {
+    ignerr
+      << "Attempt to get FrictionPyramidPrimarySlipCompliance for a "
+      << "ShapeNode that doesn't have a DynamicAspect. "
+      << "Returning default value of 0.0."
+      << std::endl;
+    return 0.0;
+  }
+  return aspect->getSlipCompliance();
+}
+
+/////////////////////////////////////////////////
+double ShapeFeatures::GetShapeFrictionPyramidSecondarySlipCompliance(
+    const Identity &_shapeID) const
+{
+  auto &node = this->ReferenceInterface<ShapeInfo>(_shapeID)->node;
+  auto aspect = node->getDynamicsAspect();
+  if (nullptr == aspect)
+  {
+    ignerr
+      << "Attempt to get FrictionPyramidSecondarySlipCompliance for a "
+      << "ShapeNode that doesn't have a DynamicAspect. "
+      << "Returning default value of 0.0."
+      << std::endl;
+    return 0.0;
+  }
+  return aspect->getSecondarySlipCompliance();
+}
+
+/////////////////////////////////////////////////
+bool ShapeFeatures::SetShapeFrictionPyramidPrimarySlipCompliance(
+    const Identity &_shapeID, double _value)
+{
+  auto &node = this->ReferenceInterface<ShapeInfo>(_shapeID)->node;
+  auto aspect = node->getDynamicsAspect();
+  if (nullptr == aspect)
+  {
+    ignerr
+      << "Attempt to set FrictionPyramidPrimarySlipCompliance for a "
+      << "ShapeNode that doesn't have a DynamicAspect. "
+      << "The parameter has not been set."
+      << std::endl;
+    return false;
+  }
+  aspect->setSlipCompliance(_value);
+  return true;
+}
+
+/////////////////////////////////////////////////
+bool ShapeFeatures::SetShapeFrictionPyramidSecondarySlipCompliance(
+    const Identity &_shapeID, double _value)
+{
+  auto &node = this->ReferenceInterface<ShapeInfo>(_shapeID)->node;
+  auto aspect = node->getDynamicsAspect();
+  if (nullptr == aspect)
+  {
+    ignerr
+      << "Attempt to set FrictionPyramidSecondarySlipCompliance for a "
+      << "ShapeNode that doesn't have a DynamicAspect. "
+      << "The parameter has not been set."
+      << std::endl;
+    return false;
+  }
+  aspect->setSecondarySlipCompliance(_value);
+  return true;
+}
+
 }
 }
 }

dartsim/src/ShapeFeatures.hh

@@ -36,6 +36,8 @@ namespace dartsim {
 using ShapeFeatureList = FeatureList<
   GetShapeKinematicProperties,
   SetShapeKinematicProperties,
+  GetShapeFrictionPyramidSlipCompliance,
+  SetShapeFrictionPyramidSlipCompliance,
   GetShapeBoundingBox,
   GetBoxShapeProperties,
   // dartsim cannot yet update shape properties without reloading the model into
@@ -150,6 +152,19 @@ class ShapeFeatures :
       const std::string &_name,
       const LinearVector3d &_normal,
       const LinearVector3d &_point) override;
+
+  // ----- Friction Features -----
+  public: virtual double GetShapeFrictionPyramidPrimarySlipCompliance(
+            const Identity &_shapeID) const override;
+
+  public: virtual double GetShapeFrictionPyramidSecondarySlipCompliance(
+            const Identity &_shapeID) const override;
+
+  public: virtual bool SetShapeFrictionPyramidPrimarySlipCompliance(
+            const Identity &_shapeID, double _value) override;
+
+  public: virtual bool SetShapeFrictionPyramidSecondarySlipCompliance(
+            const Identity &_shapeID, double _value) override;
 };
 
 }

dartsim/src/ShapeFeatures_TEST.cc

@@ -0,0 +1,263 @@
+/*
+ * Copyright (C) 2019 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 3.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 <dart/dynamics/BodyNode.hpp>
+#include <dart/dynamics/Skeleton.hpp>
+#include <dart/simulation/World.hpp>
+
+#include <gtest/gtest.h>
+
+#include <test/PhysicsPluginsList.hh>
+
+#include <iostream>
+
+#include <ignition/physics/FindFeatures.hh>
+#include <ignition/plugin/Loader.hh>
+#include <ignition/physics/RequestEngine.hh>
+
+#include <ignition/math/eigen3/Conversions.hh>
+
+// Features
+#include <ignition/physics/ConstructEmpty.hh>
+#include <ignition/physics/ForwardStep.hh>
+#include <ignition/physics/FrameSemantics.hh>
+#include <ignition/physics/FreeJoint.hh>
+#include <ignition/physics/FixedJoint.hh>
+#include <ignition/physics/GetEntities.hh>
+#include <ignition/physics/Joint.hh>
+#include <ignition/physics/Link.hh>
+#include <ignition/physics/Shape.hh>
+#include <ignition/physics/RevoluteJoint.hh>
+#include <ignition/physics/dartsim/World.hh>
+#include <ignition/physics/sdf/ConstructModel.hh>
+#include <ignition/physics/sdf/ConstructWorld.hh>
+#include <ignition/physics/sdf/ConstructLink.hh>
+
+#include <sdf/Model.hh>
+#include <sdf/Root.hh>
+#include <sdf/World.hh>
+
+#include "test/Utils.hh"
+#include "ShapeFeatures.hh"
+
+using namespace ignition;
+
+using TestFeatureList = ignition::physics::FeatureList<
+  physics::dartsim::RetrieveWorld,
+  physics::AttachFixedJointFeature,
+  physics::AddLinkExternalForceTorque,
+  physics::LinkFrameSemantics,
+  physics::DetachJointFeature,
+  physics::SetJointTransformFromParentFeature,
+  physics::ForwardStep,
+  physics::FreeJointCast,
+  physics::GetBasicJointState,
+  physics::GetEntities,
+  physics::RevoluteJointCast,
+  physics::SetJointVelocityCommandFeature,
+  physics::sdf::ConstructSdfModel,
+  physics::sdf::ConstructSdfWorld,
+  physics::sdf::ConstructSdfLink,
+  physics::SetShapeFrictionPyramidSlipCompliance
+>;
+
+using TestEnginePtr = physics::Engine3dPtr<TestFeatureList>;
+using TestWorldPtr = physics::World3dPtr<TestFeatureList>;
+
+class ShapeFeaturesFixture : public ::testing::Test
+{
+  protected: void SetUp() override
+  {
+    ignition::plugin::Loader loader;
+    loader.LoadLib(dartsim_plugin_LIB);
+
+    ignition::plugin::PluginPtr dartsim =
+        loader.Instantiate("ignition::physics::dartsim::Plugin");
+
+    this->engine =
+        ignition::physics::RequestEngine3d<TestFeatureList>::From(dartsim);
+    ASSERT_NE(nullptr, this->engine);
+  }
+  protected: TestEnginePtr engine;
+};
+
+TestWorldPtr LoadWorld(
+    const TestEnginePtr &_engine,
+    const std::string &_sdfFile,
+    const Eigen::Vector3d &_gravity = Eigen::Vector3d{0, 0, -9.8})
+{
+  sdf::Root root;
+  const sdf::Errors errors = root.Load(_sdfFile);
+  EXPECT_TRUE(errors.empty());
+  const sdf::World *sdfWorld = root.WorldByIndex(0);
+  // Make a copy of the world so we can set the gravity property
+  // TODO(addisu) Add a world property feature to set gravity instead of this
+  // hack
+  sdf::World worldCopy;
+  worldCopy.Load(sdfWorld->Element());
+
+  worldCopy.SetGravity(math::eigen3::convert(_gravity));
+  return _engine->ConstructWorld(worldCopy);
+}
+
+// A predicate-formatter for asserting that two vectors are approximately equal.
+class AssertVectorApprox
+{
+  public: explicit AssertVectorApprox(double _tol = 1e-6) : tol(_tol)
+  {
+  }
+
+  public: ::testing::AssertionResult operator()(
+              const char *_mExpr, const char *_nExpr, Eigen::Vector3d _m,
+              Eigen::Vector3d _n)
+  {
+    if (ignition::physics::test::Equal(_m, _n, this->tol))
+      return ::testing::AssertionSuccess();
+
+    return ::testing::AssertionFailure()
+           << _mExpr << " and " << _nExpr << " ([" << _m.transpose()
+           << "] and [" << _n.transpose() << "]"
+           << ") are not equal";
+  }
+
+  private: double tol;
+};
+
+/////////////////////////////////////////////////
+TEST_F(ShapeFeaturesFixture, PrimarySlipCompliance)
+{
+  sdf::Root root;
+  const sdf::Errors errors = root.Load(TEST_WORLD_DIR "slip_compliance.sdf");
+  ASSERT_TRUE(errors.empty()) << errors.front();
+
+  const std::string modelName{"box"};
+  const std::string linkName{"box_link"};
+  const std::string shapeName{"box_collision"};
+
+  auto world = this->engine->ConstructWorld(*root.WorldByIndex(0));
+
+  auto model = world->GetModel(modelName);
+  auto boxLink = model->GetLink(linkName);
+  auto boxShape = boxLink->GetShape(shapeName);
+
+  AssertVectorApprox vectorPredicate(1e-4);
+
+  ignition::physics::ForwardStep::Input input;
+  ignition::physics::ForwardStep::State state;
+  ignition::physics::ForwardStep::Output output;
+
+  // Check that link is at rest
+  {
+    const auto frameData = boxLink->FrameDataRelativeToWorld();
+
+    EXPECT_PRED_FORMAT2(vectorPredicate, Eigen::Vector3d::Zero(),
+                        frameData.linearVelocity);
+    EXPECT_PRED_FORMAT2(vectorPredicate, Eigen::Vector3d::Zero(),
+                        frameData.angularVelocity);
+    EXPECT_PRED_FORMAT2(vectorPredicate, Eigen::Vector3d::Zero(),
+                        frameData.linearAcceleration);
+    EXPECT_PRED_FORMAT2(vectorPredicate, Eigen::Vector3d::Zero(),
+                        frameData.angularAcceleration);
+  }
+
+  const Eigen::Vector3d cmdForce{1, 0, 0};
+  const double primarySlip = 0.5;
+
+  boxShape->SetPrimarySlipCompliance(primarySlip);
+
+  const std::size_t numSteps = 10000;
+  for (std::size_t i = 0; i < numSteps; ++i)
+  {
+    world->Step(output, state, input);
+    boxLink->AddExternalForce(
+        physics::RelativeForce3d(physics::FrameID::World(), cmdForce),
+        physics::RelativePosition3d(*boxLink, Eigen::Vector3d::Zero()));
+  }
+
+  {
+    // velocity = slip * applied force
+    const auto frameData = boxLink->FrameDataRelativeToWorld();
+    EXPECT_PRED_FORMAT2(vectorPredicate, primarySlip * cmdForce,
+                        (frameData.linearVelocity));
+  }
+}
+
+/////////////////////////////////////////////////
+TEST_F(ShapeFeaturesFixture, SecondarySlipCompliance)
+{
+  sdf::Root root;
+  const sdf::Errors errors = root.Load(TEST_WORLD_DIR "slip_compliance.sdf");
+  ASSERT_TRUE(errors.empty()) << errors.front();
+
+  const std::string modelName{"box"};
+  const std::string linkName{"box_link"};
+  const std::string shapeName{"box_collision"};
+
+  auto world = this->engine->ConstructWorld(*root.WorldByIndex(0));
+
+  auto model = world->GetModel(modelName);
+  auto boxLink = model->GetLink(linkName);
+  auto boxShape = boxLink->GetShape(shapeName);
+
+  AssertVectorApprox vectorPredicate(1e-4);
+
+  ignition::physics::ForwardStep::Input input;
+  ignition::physics::ForwardStep::State state;
+  ignition::physics::ForwardStep::Output output;
+
+  // Check that link is at rest
+  {
+    const auto frameData = boxLink->FrameDataRelativeToWorld();
+
+    EXPECT_PRED_FORMAT2(vectorPredicate, Eigen::Vector3d::Zero(),
+                        frameData.linearVelocity);
+    EXPECT_PRED_FORMAT2(vectorPredicate, Eigen::Vector3d::Zero(),
+                        frameData.angularVelocity);
+    EXPECT_PRED_FORMAT2(vectorPredicate, Eigen::Vector3d::Zero(),
+                        frameData.linearAcceleration);
+    EXPECT_PRED_FORMAT2(vectorPredicate, Eigen::Vector3d::Zero(),
+                        frameData.angularAcceleration);
+  }
+
+  const Eigen::Vector3d cmdForce{0, 1, 0};
+  const double secondarySlip = 0.25;
+
+  boxShape->SetSecondarySlipCompliance(secondarySlip);
+
+  const std::size_t numSteps = 10000;
+  for (std::size_t i = 0; i < numSteps; ++i)
+  {
+    world->Step(output, state, input);
+    boxLink->AddExternalForce(
+        physics::RelativeForce3d(physics::FrameID::World(), cmdForce),
+        physics::RelativePosition3d(*boxLink, Eigen::Vector3d::Zero()));
+  }
+
+  {
+    // velocity = slip * applied force
+    const auto frameData = boxLink->FrameDataRelativeToWorld();
+    EXPECT_PRED_FORMAT2(vectorPredicate, secondarySlip * cmdForce,
+                        (frameData.linearVelocity));
+  }
+}
+
+/////////////////////////////////////////////////
+int main(int argc, char *argv[])
+{
+  ::testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}