Dart soft box

gazebo/physics/dart/DARTBoxShape.cc

@@ -57,6 +57,8 @@ void DARTBoxShape::Init()
   this->dataPtr->CreateShape(bodyNode);
   _collisionParent->SetDARTCollisionShapeNode(
                       this->dataPtr->ShapeNode(), false);
+
+  this->isSoftBody = _collisionParent->IsSoftBody();
 
   BoxShape::Init();
 }
@@ -97,6 +99,12 @@ void DARTBoxShape::SetSize(const ignition::math::Vector3d &_size)
 
   BoxShape::SetSize(size);
 
+  // Rigid Bone at the core of the soft body which is 0.6 times smaller
+  if(this->isSoftBody)
+  {
+    size *= 0.6;
+  }
+
   GZ_ASSERT(this->dataPtr->Shape(),
             "Box shape node or shape itself is NULL");
   this->dataPtr->Shape()->setSize(DARTTypes::ConvVec3(size));

gazebo/physics/dart/DARTBoxShape.hh

@@ -50,6 +50,9 @@ namespace gazebo
       /// \internal
       /// \brief Pointer to private data
       private: DARTBoxShapePrivate *dataPtr;
+
+      /// \brief True if this link is soft body.
+      private: bool isSoftBody;
     };
     /// \}
   }

gazebo/physics/dart/DARTCollision.cc

@@ -251,6 +251,12 @@ dart::dynamics::BodyNode *DARTCollision::DARTBodyNode() const
   return boost::static_pointer_cast<DARTLink>(this->link)->DARTBodyNode();
 }
 
+//////////////////////////////////////////////////
+bool DARTCollision::IsSoftBody() const
+{
+  return boost::static_pointer_cast<DARTLink>(this->link)->IsSoftBody();
+}
+
 //////////////////////////////////////////////////
 void DARTCollision::SetDARTCollisionShapeNode(
                                           dart::dynamics::ShapeNodePtr _shape,

gazebo/physics/dart/DARTCollision.hh

@@ -77,6 +77,10 @@ namespace gazebo
       /// \return Pointer to the dart BodyNode.
       public: dart::dynamics::BodyNode *DARTBodyNode() const;
 
+      /// \brief Check is DART body is SoftBodyNode
+      /// \return True if this link is soft body.
+      public: bool IsSoftBody() const;
+
       /// \brief Set DART collision shape.
       /// \param[in] _shape DART Collision shape
       /// \param[in] _placeable True to make the object movable.

gazebo/physics/dart/DARTLink.cc

@@ -144,10 +144,9 @@ void DARTLink::Load(sdf::ElementPtr _sdf)
     if (softGeomElem->HasElement("box"))
     {
       sdf::ElementPtr boxEle = softGeomElem->GetElement("box");
-      Eigen::Vector3d size =
-          DARTTypes::ConvVec3(boxEle->Get<ignition::math::Vector3d>("size"));
+      Eigen::Vector3d boxSize = DARTTypes::ConvVec3(boxEle->Get<ignition::math::Vector3d>("size"));
       softProperties = dart::dynamics::SoftBodyNodeHelper::makeBoxProperties(
-            size, T, fleshMassFraction, boneAttachment, stiffness, damping);
+            boxSize, T, Eigen::Vector3i(4, 4, 4), fleshMassFraction, boneAttachment, stiffness, damping);
     }
 //    else if (geomElem->HasElement("ellipsoid"))
 //    {
@@ -167,13 +166,18 @@ void DARTLink::Load(sdf::ElementPtr _sdf)
       gzerr << "Unknown soft shape" << std::endl;
     }
 
+    softProperties.mKv = boneAttachment;
+    softProperties.mKe = stiffness;
+    softProperties.mDampCoeff = damping;
+
     // Create DART SoftBodyNode properties
     dart::dynamics::BodyNode::AspectProperties properties(bodyName);
     this->dataPtr->dtProperties.reset(
           new dart::dynamics::SoftBodyNode::Properties(
             properties, softProperties));
 
-    this->dataPtr->isSoftBody;
+    this->dataPtr->isSoftBody = true;
+    this->dataPtr->fleshMass = fleshMassFraction;
   }
   else
   {
@@ -268,8 +272,14 @@ void DARTLink::UpdateMass()
   {
     double nFragments = 1.0 + this->dataPtr->dtSlaveNodes.size();
     double mass = this->inertial->Mass()/nFragments;
-    this->dataPtr->dtBodyNode->setMass(mass);
 
+    // Update mass of the underlying BodyNode
+    if(this->dataPtr->isSoftBody)
+    {
+      mass -= this->dataPtr->fleshMass;
+    }
+
+    this->dataPtr->dtBodyNode->setMass(mass);
     const ignition::math::Quaterniond R_inertial = this->inertial->Pose().Rot();
 
     const ignition::math::Matrix3d I_link =
@@ -278,6 +288,8 @@ void DARTLink::UpdateMass()
         *ignition::math::Matrix3d(R_inertial.Inverse())
         *(1.0/nFragments);
 
+    // Need to potentially update the objects inertia 
+    // (as for box shape the body node is 0.6 times smaller)
     this->dataPtr->dtBodyNode->setMomentOfInertia(
         I_link(0, 0), I_link(1, 1), I_link(2, 2),
         I_link(0, 1), I_link(0, 2), I_link(1, 2));

gazebo/physics/dart/DARTLink.hh

@@ -179,6 +179,7 @@ namespace gazebo
       /// \brief Set pointer to DART BodyNode associated with this link.
       /// \param[in] Pointer to DART BodyNode.
       public: void SetDARTBodyNode(dart::dynamics::BodyNode *_dtBodyNode);
+      public: void SetDARTBodyNode(dart::dynamics::SoftBodyNode *_dtBodyNode);
 
       /// \brief Add pointer to a BodyNode representing a fragment of this link.
       /// \param[in] Pointer to DART BodyNode.

gazebo/physics/dart/DARTLinkPrivate.hh

@@ -175,6 +175,9 @@ namespace gazebo
       /// \brief True if this link is soft body.
       public: bool isSoftBody;
 
+      /// \brief Mass of the softBody that envelopes the rigid bone.
+      public: double fleshMass;
+
       /// \brief If true, freeze link to world (inertial) frame.
       public: bool staticLink;
 

gazebo/physics/dart/DARTModelPrivate.hh

@@ -210,6 +210,11 @@ namespace gazebo
               _skeleton, _parent, jointType, jointProperties,
               static_cast<const dart::dynamics::SoftBodyNode::Properties&>(
               *(dartLink->DARTProperties())));
+
+          dart::dynamics::Inertia inertia;
+          inertia.setMoment(1e-8*Eigen::Matrix3d::Identity());
+          inertia.setMass(1e-8);
+          pair.second->setInertia(inertia);
         }
         else
         {