diff --git a/gazebo/physics/dart/DARTBoxShape.cc b/gazebo/physics/dart/DARTBoxShape.cc index c368f19e6b..0b1a6f27ac 100644 --- a/gazebo/physics/dart/DARTBoxShape.cc +++ b/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)); diff --git a/gazebo/physics/dart/DARTBoxShape.hh b/gazebo/physics/dart/DARTBoxShape.hh index 94457d94ed..314b1c2e06 100644 --- a/gazebo/physics/dart/DARTBoxShape.hh +++ b/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; }; /// \} } diff --git a/gazebo/physics/dart/DARTCollision.cc b/gazebo/physics/dart/DARTCollision.cc index 49df75f7ea..179609cf89 100644 --- a/gazebo/physics/dart/DARTCollision.cc +++ b/gazebo/physics/dart/DARTCollision.cc @@ -251,6 +251,12 @@ dart::dynamics::BodyNode *DARTCollision::DARTBodyNode() const return boost::static_pointer_cast(this->link)->DARTBodyNode(); } +////////////////////////////////////////////////// +bool DARTCollision::IsSoftBody() const +{ + return boost::static_pointer_cast(this->link)->IsSoftBody(); +} + ////////////////////////////////////////////////// void DARTCollision::SetDARTCollisionShapeNode( dart::dynamics::ShapeNodePtr _shape, diff --git a/gazebo/physics/dart/DARTCollision.hh b/gazebo/physics/dart/DARTCollision.hh index 2c87ce9a89..46c5e5b0d4 100644 --- a/gazebo/physics/dart/DARTCollision.hh +++ b/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. diff --git a/gazebo/physics/dart/DARTLink.cc b/gazebo/physics/dart/DARTLink.cc index 0b458b0015..ea30e40d33 100644 --- a/gazebo/physics/dart/DARTLink.cc +++ b/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("size")); + Eigen::Vector3d boxSize = DARTTypes::ConvVec3(boxEle->Get("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)); diff --git a/gazebo/physics/dart/DARTLink.hh b/gazebo/physics/dart/DARTLink.hh index 5e5e9c23db..6b40eef47a 100644 --- a/gazebo/physics/dart/DARTLink.hh +++ b/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. diff --git a/gazebo/physics/dart/DARTLinkPrivate.hh b/gazebo/physics/dart/DARTLinkPrivate.hh index 03624f4ae0..d3f74ad2e6 100644 --- a/gazebo/physics/dart/DARTLinkPrivate.hh +++ b/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; diff --git a/gazebo/physics/dart/DARTModelPrivate.hh b/gazebo/physics/dart/DARTModelPrivate.hh index d988d7f2ab..fb9f9c0931 100644 --- a/gazebo/physics/dart/DARTModelPrivate.hh +++ b/gazebo/physics/dart/DARTModelPrivate.hh @@ -210,6 +210,11 @@ namespace gazebo _skeleton, _parent, jointType, jointProperties, static_cast( *(dartLink->DARTProperties()))); + + dart::dynamics::Inertia inertia; + inertia.setMoment(1e-8*Eigen::Matrix3d::Identity()); + inertia.setMass(1e-8); + pair.second->setInertia(inertia); } else {