1 /* 2 * Copyright (c) 2011-2021, The DART development contributors 3 * All rights reserved. 4 * 5 * The list of contributors can be found at: 6 * https://github.com/dartsim/dart/blob/master/LICENSE 7 * 8 * This file is provided under the following "BSD-style" License: 9 * Redistribution and use in source and binary forms, with or 10 * without modification, are permitted provided that the following 11 * conditions are met: 12 * * Redistributions of source code must retain the above copyright 13 * notice, this list of conditions and the following disclaimer. 14 * * Redistributions in binary form must reproduce the above 15 * copyright notice, this list of conditions and the following 16 * disclaimer in the documentation and/or other materials provided 17 * with the distribution. 18 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND 19 * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, 20 * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF 21 * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 23 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 24 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 25 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF 26 * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 * POSSIBILITY OF SUCH DAMAGE. 31 */ 32 33 #ifndef DART_DYNAMICS_SOFTBODYNODE_HPP_ 34 #define DART_DYNAMICS_SOFTBODYNODE_HPP_ 35 36 #include "dart/dynamics/detail/SoftBodyNodeAspect.hpp" 37 38 namespace dart { 39 namespace dynamics { 40 41 /// SoftBodyNode represent a soft body that has one deformable skin 42 /// 43 /// This class is implementation of Sumit Jain and C. Karen Liu's paper: 44 /// http://www.cc.gatech.edu/graphics/projects/Sumit/homepage/projects/softcontacts/index.html 45 class SoftBodyNode : public detail::SoftBodyNodeBase 46 { 47 public: 48 using UniqueProperties = detail::SoftBodyNodeUniqueProperties; 49 using Properties = detail::SoftBodyNodeProperties; 50 using Base = detail::SoftBodyNodeBase; 51 52 friend class Skeleton; 53 friend class PointMass; 54 friend class PointMassNotifier; 55 56 DART_BAKE_SPECIALIZED_ASPECT_IRREGULAR(Aspect, SoftBodyAspect) 57 58 /// \brief 59 virtual ~SoftBodyNode(); 60 61 // Documentation inherited 62 SoftBodyNode* asSoftBodyNode() override; 63 64 // Documentation inherited 65 const SoftBodyNode* asSoftBodyNode() const override; 66 67 /// Set the Properties of this SoftBodyNode 68 void setProperties(const Properties& _properties); 69 70 /// Set the Properties of this SoftBodyNode 71 void setProperties(const UniqueProperties& _properties); 72 73 /// Set the AspectState of this SoftBodyNode 74 void setAspectState(const AspectState& state); 75 76 /// Set the AspectProperties of this SoftBodyNode 77 void setAspectProperties(const AspectProperties& properties); 78 79 using SkeletonRefCountingBase::getSkeleton; 80 81 /// Get the Properties of this SoftBodyNode 82 Properties getSoftBodyNodeProperties() const; 83 84 /// Copy the Properties of another SoftBodyNode 85 void copy(const SoftBodyNode& _otherSoftBodyNode); 86 87 /// Copy the Properties of another SoftBodyNode 88 void copy(const SoftBodyNode* _otherSoftBodyNode); 89 90 /// Copy the Properties of another SoftBodyNode 91 SoftBodyNode& operator=(const SoftBodyNode& _otherSoftBodyNode); 92 93 /// Get the update notifier for the PointMasses of this SoftBodyNode 94 PointMassNotifier* getNotifier(); 95 96 /// Get the update notifier for the PointMasses of this SoftBodyNode 97 const PointMassNotifier* getNotifier() const; 98 99 /// \brief Get mass. 100 double getMass() const; 101 102 /// \brief 103 void setVertexSpringStiffness(double _kv); 104 105 /// \brief 106 double getVertexSpringStiffness() const; 107 108 /// \brief 109 void setEdgeSpringStiffness(double _ke); 110 111 /// \brief 112 double getEdgeSpringStiffness() const; 113 114 /// \brief 115 void setDampingCoefficient(double _damp); 116 117 /// \brief 118 double getDampingCoefficient() const; 119 120 /// \brief 121 void removeAllPointMasses(); 122 123 /// \brief 124 PointMass* addPointMass(const PointMass::Properties& _properties); 125 126 /// \brief 127 std::size_t getNumPointMasses() const; 128 129 /// \brief 130 PointMass* getPointMass(std::size_t _idx); 131 132 /// \brief 133 const PointMass* getPointMass(std::size_t _idx) const; 134 135 /// Return all the point masses in this SoftBodyNode 136 const std::vector<PointMass*>& getPointMasses() const; 137 138 /// \brief 139 void connectPointMasses(std::size_t _idx1, std::size_t _idx2); 140 141 /// \brief 142 void addFace(const Eigen::Vector3i& _face); 143 144 /// \brief 145 const Eigen::Vector3i& getFace(std::size_t _idx) const; 146 147 /// \brief 148 std::size_t getNumFaces() const; 149 150 // Documentation inherited. 151 void clearConstraintImpulse() override; 152 153 protected: 154 /// Constructor called by Skeleton class 155 SoftBodyNode( 156 BodyNode* _parentBodyNode, 157 Joint* _parentJoint, 158 const Properties& _properties); 159 160 /// Create a clone of this SoftBodyNode. This may only be called by the 161 /// Skeleton class. 162 BodyNode* clone( 163 BodyNode* _parentBodyNode, 164 Joint* _parentJoint, 165 bool cloneNodes) const override; 166 167 /// Used by SoftBodyAspect to have this SoftBodyNode reconstruct its 168 /// SoftMeshShape 169 void configurePointMasses(ShapeNode* softNode); 170 171 //-------------------------------------------------------------------------- 172 // Sub-functions for Recursive Kinematics Algorithms 173 //-------------------------------------------------------------------------- 174 // Documentation inherited. 175 void init(const SkeletonPtr& _skeleton) override; 176 177 // Documentation inherited. 178 // virtual void aggregateGenCoords(std::vector<GenCoord*>* _genCoords); 179 180 // Documentation inherited. 181 // virtual void aggregatePointMassGenCoords(std::vector<GenCoord*>* 182 // _genCoords); 183 184 //---------------------------------------------------------------------------- 185 /// \{ \name Recursive dynamics routines 186 //---------------------------------------------------------------------------- 187 188 /// Update articulated inertia if necessary 189 void checkArticulatedInertiaUpdate() const; 190 191 // Documentation inherited. 192 void updateTransform() override; 193 194 // Documentation inherited. 195 void updateVelocity() override; 196 197 // Documentation inherited. 198 void updatePartialAcceleration() const override; 199 200 // Documentation inherited. 201 void updateArtInertia(double _timeStep) const override; 202 203 // Documentation inherited. 204 void updateBiasForce( 205 const Eigen::Vector3d& _gravity, double _timeStep) override; 206 207 // Documentation inherited. 208 void updateBiasImpulse() override; 209 210 // Documentation inherited. 211 void updateAccelerationID() override; 212 213 // Documentation inherited. 214 void updateAccelerationFD() override; 215 216 // Documentation inherited. 217 void updateVelocityChangeFD() override; 218 219 // Documentation inherited. 220 void updateTransmittedForceID( 221 const Eigen::Vector3d& _gravity, 222 bool _withExternalForces = false) override; 223 224 // Documentation inherited. 225 void updateTransmittedForceFD() override; 226 227 // Documentation inherited. 228 void updateTransmittedImpulse() override; 229 230 // Documentation inherited. 231 void updateJointForceID( 232 double _timeStep, 233 bool _withDampingForces, 234 bool _withSpringForces) override; 235 236 // Documentation inherited. 237 void updateJointForceFD( 238 double _timeStep, 239 bool _withDampingForces, 240 bool _withSpringForces) override; 241 242 // Documentation inherited. 243 void updateJointImpulseFD() override; 244 245 // Documentation inherited. 246 void updateConstrainedTerms(double _timeStep) override; 247 248 /// \} 249 250 //---------------------------------------------------------------------------- 251 /// \{ \name Equations of motion related routines 252 //---------------------------------------------------------------------------- 253 254 // Documentation inherited. 255 void updateMassMatrix() override; 256 257 // Documentation inherited. 258 void aggregateMassMatrix(Eigen::MatrixXd& _MCol, std::size_t _col) override; 259 260 // Documentation inherited. 261 void aggregateAugMassMatrix( 262 Eigen::MatrixXd& _MCol, std::size_t _col, double _timeStep) override; 263 264 // Documentation inherited. 265 void updateInvMassMatrix() override; 266 267 // Documentation inherited. 268 void updateInvAugMassMatrix() override; 269 270 // Documentation inherited. 271 void aggregateInvMassMatrix( 272 Eigen::MatrixXd& _InvMCol, std::size_t _col) override; 273 274 // Documentation inherited. 275 void aggregateInvAugMassMatrix( 276 Eigen::MatrixXd& _InvMCol, std::size_t _col, double _timeStep) override; 277 278 // Documentation inherited. 279 // TODO(JS): Not implemented yet. 280 void aggregateCoriolisForceVector(Eigen::VectorXd& _C) override; 281 282 // Documentation inherited. 283 void aggregateGravityForceVector( 284 Eigen::VectorXd& _g, const Eigen::Vector3d& _gravity) override; 285 286 // Documentation inherited. 287 void updateCombinedVector() override; 288 289 // Documentation inherited. 290 void aggregateCombinedVector( 291 Eigen::VectorXd& _Cg, const Eigen::Vector3d& _gravity) override; 292 293 // Documentation inherited. 294 void aggregateExternalForces(Eigen::VectorXd& _Fext) override; 295 296 /// \} 297 298 // Documentation inherited. 299 void clearExternalForces() override; 300 301 void clearInternalForces() override; 302 303 protected: 304 /// \brief List of point masses composing deformable mesh. 305 std::vector<PointMass*> mPointMasses; 306 307 /// An Entity which tracks when the point masses need to be updated 308 PointMassNotifier* mNotifier; 309 310 /// \brief Soft mesh shape belonging to this node. 311 WeakShapeNodePtr mSoftShapeNode; 312 313 /// Generalized inertia with point masses 314 math::Inertia mI2; 315 316 /// 317 math::Inertia mArtInertia2; 318 319 /// 320 math::Inertia mArtInertiaImplicit2; 321 322 private: 323 /// \brief 324 void _addPiToArtInertia(const Eigen::Vector3d& _p, double _Pi) const; 325 326 /// \brief 327 void _addPiToArtInertiaImplicit( 328 const Eigen::Vector3d& _p, double _ImplicitPi) const; 329 330 /// 331 void updateInertiaWithPointMass(); 332 }; 333 334 class SoftBodyNodeHelper 335 { 336 public: 337 /// Create a Properties struct for a box-shaped SoftBodyNode with 8 338 /// PointMasses 339 static SoftBodyNode::UniqueProperties makeBoxProperties( 340 const Eigen::Vector3d& _size, 341 const Eigen::Isometry3d& _localTransform, 342 double _totalMass, 343 double _vertexStiffness = DART_DEFAULT_VERTEX_STIFFNESS, 344 double _edgeStiffness = DART_DEFAULT_EDGE_STIFNESS, 345 double _dampingCoeff = DART_DEFAULT_DAMPING_COEFF); 346 347 /// \brief 348 /// This should be called before SoftBodyNode::init() is called 349 static void setBox( 350 SoftBodyNode* _softBodyNode, 351 const Eigen::Vector3d& _size, 352 const Eigen::Isometry3d& _localTransform, 353 double _totalMass, 354 double _vertexStiffness = DART_DEFAULT_VERTEX_STIFFNESS, 355 double _edgeStiffness = DART_DEFAULT_EDGE_STIFNESS, 356 double _dampingCoeff = DART_DEFAULT_DAMPING_COEFF); 357 358 /// Create a Properties struct for a box-shaped SoftBodyNode. Specify the 359 /// number of vertices along each axis with _frags. Each component should be 360 /// equal to or greater than 3. For example, [3 3 3] is allowed but [2 2 2] is 361 /// not. 362 static SoftBodyNode::UniqueProperties makeBoxProperties( 363 const Eigen::Vector3d& _size, 364 const Eigen::Isometry3d& _localTransform, 365 const Eigen::Vector3i& _frags, 366 double _totalMass, 367 double _vertexStiffness = DART_DEFAULT_VERTEX_STIFFNESS, 368 double _edgeStiffness = DART_DEFAULT_EDGE_STIFNESS, 369 double _dampingCoeff = DART_DEFAULT_DAMPING_COEFF); 370 371 /// \brief 372 /// This should be called before SoftBodyNode::init() is called 373 /// \param[in] _softBodyNode 374 /// \param[in] _size 375 /// \param[in] _localTransform 376 /// \param[in] _frags Number of vertices of box mesh. Each component should be 377 /// equal or greater than 3. For example, [3 3 3] is allowed but [2 2 2] is 378 /// not. 379 // TODO: The component of _frags should allow 2. 380 /// \param[in] _totalMass 381 /// \param[in] _vertexStiffness 382 /// \param[in] _edgeStiffness 383 /// \param[in] _dampingCoeff 384 static void setBox( 385 SoftBodyNode* _softBodyNode, 386 const Eigen::Vector3d& _size, 387 const Eigen::Isometry3d& _localTransform, 388 const Eigen::Vector3i& _frags, 389 double _totalMass, 390 double _vertexStiffness = DART_DEFAULT_VERTEX_STIFFNESS, 391 double _edgeStiffness = DART_DEFAULT_EDGE_STIFNESS, 392 double _dampingCoeff = DART_DEFAULT_DAMPING_COEFF); 393 394 /// Create a Properties struct for a SoftBodyNode with a single PointMass 395 static SoftBodyNode::UniqueProperties makeSinglePointMassProperties( 396 double _totalMass, 397 double _vertexStiffness = DART_DEFAULT_VERTEX_STIFFNESS, 398 double _edgeStiffness = DART_DEFAULT_EDGE_STIFNESS, 399 double _dampingCoeff = DART_DEFAULT_DAMPING_COEFF); 400 401 /// \brief 402 /// This should be called before SoftBodyNode::init() is called 403 static void setSinglePointMass( 404 SoftBodyNode* _softBodyNode, 405 double _totalMass, 406 double _vertexStiffness = DART_DEFAULT_VERTEX_STIFFNESS, 407 double _edgeStiffness = DART_DEFAULT_EDGE_STIFNESS, 408 double _dampingCoeff = DART_DEFAULT_DAMPING_COEFF); 409 410 /// Create a Properties struct for an Sphere-shaped SoftBodyNode 411 static SoftBodyNode::UniqueProperties makeSphereProperties( 412 double _radius, 413 std::size_t _nSlices, 414 std::size_t _nStacks, 415 double _totalMass, 416 double _vertexStiffness = DART_DEFAULT_VERTEX_STIFFNESS, 417 double _edgeStiffness = DART_DEFAULT_EDGE_STIFNESS, 418 double _dampingCoeff = DART_DEFAULT_DAMPING_COEFF); 419 420 /// Create a Properties struct for an Ellipsoid-shaped SoftBodyNode 421 static SoftBodyNode::UniqueProperties makeEllipsoidProperties( 422 const Eigen::Vector3d& _size, 423 std::size_t _nSlices, 424 std::size_t _nStacks, 425 double _totalMass, 426 double _vertexStiffness = DART_DEFAULT_VERTEX_STIFFNESS, 427 double _edgeStiffness = DART_DEFAULT_EDGE_STIFNESS, 428 double _dampingCoeff = DART_DEFAULT_DAMPING_COEFF); 429 430 /// \brief 431 /// This should be called before SoftBodyNode::init() is called 432 static void setEllipsoid( 433 SoftBodyNode* _softBodyNode, 434 const Eigen::Vector3d& _size, 435 std::size_t _nSlices, 436 std::size_t _nStacks, 437 double _totalMass, 438 double _vertexStiffness = DART_DEFAULT_VERTEX_STIFFNESS, 439 double _edgeStiffness = DART_DEFAULT_EDGE_STIFNESS, 440 double _dampingCoeff = DART_DEFAULT_DAMPING_COEFF); 441 442 /// Create a Properties struct for a cylinder-shaped SoftBodyNode 443 static SoftBodyNode::UniqueProperties makeCylinderProperties( 444 double _radius, 445 double _height, 446 std::size_t _nSlices, 447 std::size_t _nStacks, 448 std::size_t _nRings, 449 double _totalMass, 450 double _vertexStiffness = DART_DEFAULT_VERTEX_STIFFNESS, 451 double _edgeStiffness = DART_DEFAULT_EDGE_STIFNESS, 452 double _dampingCoeff = DART_DEFAULT_DAMPING_COEFF); 453 454 /// 455 /// This should be called before SoftBodyNode::init() is called 456 static void setCylinder( 457 SoftBodyNode* _softBodyNode, 458 double _radius, 459 double _height, 460 std::size_t _nSlices, 461 std::size_t _nStacks, 462 std::size_t _nRings, 463 double _totalMass, 464 double _vertexStiffness = DART_DEFAULT_VERTEX_STIFFNESS, 465 double _edgeStiffness = DART_DEFAULT_EDGE_STIFNESS, 466 double _dampingCoeff = DART_DEFAULT_DAMPING_COEFF); 467 }; 468 469 } // namespace dynamics 470 } // namespace dart 471 472 #endif // DART_DYNAMICS_SOFTBODYNODE_HPP_ 473