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_ZERODOFJOINT_HPP_ 34 #define DART_DYNAMICS_ZERODOFJOINT_HPP_ 35 36 #include <string> 37 38 #include "dart/dynamics/Joint.hpp" 39 40 namespace dart { 41 namespace dynamics { 42 43 class BodyNode; 44 class Skeleton; 45 46 /// class ZeroDofJoint 47 class ZeroDofJoint : public Joint 48 { 49 public: 50 struct Properties : Joint::Properties 51 { 52 Properties(const Joint::Properties& _properties = Joint::Properties()); 53 virtual ~Properties() = default; 54 }; 55 56 ZeroDofJoint(const ZeroDofJoint&) = delete; 57 58 /// Destructor 59 virtual ~ZeroDofJoint(); 60 61 /// Get the Properties of this ZeroDofJoint 62 Properties getZeroDofJointProperties() const; 63 64 //---------------------------------------------------------------------------- 65 // Interface for generalized coordinates 66 //---------------------------------------------------------------------------- 67 68 // Documentation inherited 69 DegreeOfFreedom* getDof(std::size_t) override; 70 71 // Documentation inherited 72 const DegreeOfFreedom* getDof(std::size_t) const override; 73 74 // Documentation inherited 75 const std::string& setDofName(std::size_t, const std::string&, bool) override; 76 77 // Documentation inherited 78 void preserveDofName(std::size_t, bool) override; 79 80 // Documentation inherited 81 bool isDofNamePreserved(std::size_t) const override; 82 83 const std::string& getDofName(std::size_t) const override; 84 85 // Documentation inherited 86 std::size_t getNumDofs() const override; 87 88 // Documentation inherited 89 std::size_t getIndexInSkeleton(std::size_t _index) const override; 90 91 // Documentation inherited 92 std::size_t getIndexInTree(std::size_t _index) const override; 93 94 //---------------------------------------------------------------------------- 95 // Command 96 //---------------------------------------------------------------------------- 97 98 // Documentation inherited 99 void setCommand(std::size_t _index, double _command) override; 100 101 // Documentation inherited 102 double getCommand(std::size_t _index) const override; 103 104 // Documentation inherited 105 void setCommands(const Eigen::VectorXd& _commands) override; 106 107 // Documentation inherited 108 Eigen::VectorXd getCommands() const override; 109 110 // Documentation inherited 111 void resetCommands() override; 112 113 //---------------------------------------------------------------------------- 114 // Position 115 //---------------------------------------------------------------------------- 116 117 // Documentation inherited 118 void setPosition(std::size_t, double) override; 119 120 // Documentation inherited 121 double getPosition(std::size_t _index) const override; 122 123 // Documentation inherited 124 void setPositions(const Eigen::VectorXd& _positions) override; 125 126 // Documentation inherited 127 Eigen::VectorXd getPositions() const override; 128 129 // Documentation inherited 130 void setPositionLowerLimit(std::size_t _index, double _position) override; 131 132 // Documentation inherited 133 double getPositionLowerLimit(std::size_t _index) const override; 134 135 // Documentation inherited 136 void setPositionLowerLimits(const Eigen::VectorXd& lowerLimits) override; 137 138 // Documentation inherited 139 Eigen::VectorXd getPositionLowerLimits() const override; 140 141 // Documentation inherited 142 void setPositionUpperLimit(std::size_t index, double position) override; 143 144 // Documentation inherited 145 double getPositionUpperLimit(std::size_t index) const override; 146 147 // Documentation inherited 148 void setPositionUpperLimits(const Eigen::VectorXd& upperLimits) override; 149 150 // Documentation inherited 151 Eigen::VectorXd getPositionUpperLimits() const override; 152 153 // Documentation inherited 154 bool hasPositionLimit(std::size_t _index) const override; 155 156 // Documentation inherited 157 void resetPosition(std::size_t _index) override; 158 159 // Documentation inherited 160 void resetPositions() override; 161 162 // Documentation inherited 163 void setInitialPosition(std::size_t _index, double _initial) override; 164 165 // Documentation inherited 166 double getInitialPosition(std::size_t _index) const override; 167 168 // Documentation inherited 169 void setInitialPositions(const Eigen::VectorXd& _initial) override; 170 171 // Documentation inherited 172 Eigen::VectorXd getInitialPositions() const override; 173 174 //---------------------------------------------------------------------------- 175 // Velocity 176 //---------------------------------------------------------------------------- 177 178 // Documentation inherited 179 void setVelocity(std::size_t _index, double _velocity) override; 180 181 // Documentation inherited 182 double getVelocity(std::size_t _index) const override; 183 184 // Documentation inherited 185 void setVelocities(const Eigen::VectorXd& _velocities) override; 186 187 // Documentation inherited 188 Eigen::VectorXd getVelocities() const override; 189 190 // Documentation inherited 191 void setVelocityLowerLimit(std::size_t _index, double _velocity) override; 192 193 // Documentation inherited 194 double getVelocityLowerLimit(std::size_t _index) const override; 195 196 // Documentation inherited 197 void setVelocityLowerLimits(const Eigen::VectorXd& lowerLimits) override; 198 199 // Documentation inherited 200 Eigen::VectorXd getVelocityLowerLimits() const override; 201 202 // Documentation inherited 203 void setVelocityUpperLimit(std::size_t _index, double _velocity) override; 204 205 // Documentation inherited 206 double getVelocityUpperLimit(std::size_t _index) const override; 207 208 // Documentation inherited 209 void setVelocityUpperLimits(const Eigen::VectorXd& upperLimits) override; 210 211 // Documentation inherited 212 Eigen::VectorXd getVelocityUpperLimits() const override; 213 214 // Documentation inherited 215 void resetVelocity(std::size_t _index) override; 216 217 // Documentation inherited 218 void resetVelocities() override; 219 220 // Documentation inherited 221 void setInitialVelocity(std::size_t _index, double _initial) override; 222 223 // Documentation inherited 224 double getInitialVelocity(std::size_t _index) const override; 225 226 // Documentation inherited 227 void setInitialVelocities(const Eigen::VectorXd& _initial) override; 228 229 // Documentation inherited 230 Eigen::VectorXd getInitialVelocities() const override; 231 232 //---------------------------------------------------------------------------- 233 // Acceleration 234 //---------------------------------------------------------------------------- 235 236 // Documentation inherited 237 void setAcceleration(std::size_t _index, double _acceleration) override; 238 239 // Documentation inherited 240 double getAcceleration(std::size_t _index) const override; 241 242 // Documentation inherited 243 void setAccelerations(const Eigen::VectorXd& _accelerations) override; 244 245 // Documentation inherited 246 Eigen::VectorXd getAccelerations() const override; 247 248 // Documentation inherited 249 void resetAccelerations() override; 250 251 // Documentation inherited 252 void setAccelerationLowerLimit( 253 std::size_t _index, double _acceleration) override; 254 255 // Documentation inherited 256 double getAccelerationLowerLimit(std::size_t _index) const override; 257 258 // Documentation inherited 259 void setAccelerationLowerLimits(const Eigen::VectorXd& lowerLimits) override; 260 261 // Documentation inherited 262 Eigen::VectorXd getAccelerationLowerLimits() const override; 263 264 // Documentation inherited 265 void setAccelerationUpperLimit( 266 std::size_t _index, double _acceleration) override; 267 268 // Documentation inherited 269 double getAccelerationUpperLimit(std::size_t _index) const override; 270 271 // Documentation inherited 272 void setAccelerationUpperLimits(const Eigen::VectorXd& upperLimits) override; 273 274 // Documentation inherited 275 Eigen::VectorXd getAccelerationUpperLimits() const override; 276 277 //---------------------------------------------------------------------------- 278 // Force 279 //---------------------------------------------------------------------------- 280 281 // Documentation inherited 282 void setForce(std::size_t _index, double _force) override; 283 284 // Documentation inherited 285 double getForce(std::size_t _index) const override; 286 287 // Documentation inherited 288 void setForces(const Eigen::VectorXd& _forces) override; 289 290 // Documentation inherited 291 Eigen::VectorXd getForces() const override; 292 293 // Documentation inherited 294 void resetForces() override; 295 296 // Documentation inherited 297 void setForceLowerLimit(std::size_t _index, double _force) override; 298 299 // Documentation inherited 300 double getForceLowerLimit(std::size_t _index) const override; 301 302 // Documentation inherited 303 void setForceLowerLimits(const Eigen::VectorXd& lowerLimits) override; 304 305 // Documentation inherited 306 Eigen::VectorXd getForceLowerLimits() const override; 307 308 // Documentation inherited 309 void setForceUpperLimit(std::size_t _index, double _force) override; 310 311 // Documentation inherited 312 double getForceUpperLimit(std::size_t _index) const override; 313 314 // Documentation inherited 315 void setForceUpperLimits(const Eigen::VectorXd& upperLimits) override; 316 317 // Documentation inherited 318 Eigen::VectorXd getForceUpperLimits() const override; 319 320 //---------------------------------------------------------------------------- 321 // Velocity change 322 //---------------------------------------------------------------------------- 323 324 // Documentation inherited 325 void setVelocityChange(std::size_t _index, double _velocityChange) override; 326 327 // Documentation inherited 328 double getVelocityChange(std::size_t _index) const override; 329 330 // Documentation inherited 331 void resetVelocityChanges() override; 332 333 //---------------------------------------------------------------------------- 334 // Constraint impulse 335 //---------------------------------------------------------------------------- 336 337 // Documentation inherited 338 void setConstraintImpulse(std::size_t _index, double _impulse) override; 339 340 // Documentation inherited 341 double getConstraintImpulse(std::size_t _index) const override; 342 343 // Documentation inherited 344 void resetConstraintImpulses() override; 345 346 //---------------------------------------------------------------------------- 347 // Integration and finite difference 348 //---------------------------------------------------------------------------- 349 350 // Documentation inherited 351 void integratePositions(double _dt) override; 352 353 // Documentation inherited 354 void integrateVelocities(double _dt) override; 355 356 // Documentation inherited 357 Eigen::VectorXd getPositionDifferences( 358 const Eigen::VectorXd& _q2, const Eigen::VectorXd& _q1) const override; 359 360 //---------------------------------------------------------------------------- 361 /// \{ \name Passive forces - spring, viscous friction, Coulomb friction 362 //---------------------------------------------------------------------------- 363 364 // Documentation inherited 365 void setSpringStiffness(std::size_t _index, double _k) override; 366 367 // Documentation inherited 368 double getSpringStiffness(std::size_t _index) const override; 369 370 // Documentation inherited 371 void setRestPosition(std::size_t _index, double _q0) override; 372 373 // Documentation inherited 374 double getRestPosition(std::size_t _index) const override; 375 376 // Documentation inherited 377 void setDampingCoefficient(std::size_t _index, double _d) override; 378 379 // Documentation inherited 380 double getDampingCoefficient(std::size_t _index) const override; 381 382 // Documentation inherited 383 void setCoulombFriction(std::size_t _index, double _friction) override; 384 385 // Documentation inherited 386 double getCoulombFriction(std::size_t _index) const override; 387 388 /// \} 389 390 //---------------------------------------------------------------------------- 391 392 // Documentation inherited 393 double computePotentialEnergy() const override; 394 395 // Documentation inherited 396 Eigen::Vector6d getBodyConstraintWrench() const override; 397 398 protected: 399 /// Constructor called by inheriting classes 400 ZeroDofJoint(); 401 402 // Documentation inherited 403 void registerDofs() override; 404 405 // Documentation inherited 406 void updateDegreeOfFreedomNames() override; 407 408 //---------------------------------------------------------------------------- 409 /// \{ \name Recursive dynamics routines 410 //---------------------------------------------------------------------------- 411 412 // Documentation inherited 413 const math::Jacobian getRelativeJacobian() const override; 414 415 // Documentation inherited 416 math::Jacobian getRelativeJacobian( 417 const Eigen::VectorXd& _positions) const override; 418 419 // Documentation inherited 420 const math::Jacobian getRelativeJacobianTimeDeriv() const override; 421 422 // Documentation inherited 423 void addVelocityTo(Eigen::Vector6d& _vel) override; 424 425 // Documentation inherited 426 void setPartialAccelerationTo( 427 Eigen::Vector6d& _partialAcceleration, 428 const Eigen::Vector6d& _childVelocity) override; 429 430 // Documentation inherited 431 void addAccelerationTo(Eigen::Vector6d& _acc) override; 432 433 // Documentation inherited 434 void addVelocityChangeTo(Eigen::Vector6d& _velocityChange) override; 435 436 // Documentation inherited 437 void addChildArtInertiaTo( 438 Eigen::Matrix6d& _parentArtInertia, 439 const Eigen::Matrix6d& _childArtInertia) override; 440 441 // Documentation inherited 442 void addChildArtInertiaImplicitTo( 443 Eigen::Matrix6d& _parentArtInertia, 444 const Eigen::Matrix6d& _childArtInertia) override; 445 446 // Documentation inherited 447 void updateInvProjArtInertia(const Eigen::Matrix6d& _artInertia) override; 448 449 // Documentation inherited 450 void updateInvProjArtInertiaImplicit( 451 const Eigen::Matrix6d& _artInertia, double _timeStep) override; 452 453 // Documentation inherited 454 void addChildBiasForceTo( 455 Eigen::Vector6d& _parentBiasForce, 456 const Eigen::Matrix6d& _childArtInertia, 457 const Eigen::Vector6d& _childBiasForce, 458 const Eigen::Vector6d& _childPartialAcc) override; 459 460 // Documentation inherited 461 void addChildBiasImpulseTo( 462 Eigen::Vector6d& _parentBiasImpulse, 463 const Eigen::Matrix6d& _childArtInertia, 464 const Eigen::Vector6d& _childBiasImpulse) override; 465 466 // Documentation inherited 467 void updateTotalForce( 468 const Eigen::Vector6d& _bodyForce, double _timeStep) override; 469 470 // Documentation inherited 471 void updateTotalImpulse(const Eigen::Vector6d& _bodyImpulse) override; 472 473 // Documentation inherited 474 void resetTotalImpulses() override; 475 476 // Documentation inherited 477 void updateAcceleration( 478 const Eigen::Matrix6d& _artInertia, 479 const Eigen::Vector6d& _spatialAcc) override; 480 481 // Documentation inherited 482 void updateVelocityChange( 483 const Eigen::Matrix6d& _artInertia, 484 const Eigen::Vector6d& _velocityChange) override; 485 486 // Documentation inherited 487 void updateForceID( 488 const Eigen::Vector6d& _bodyForce, 489 double _timeStep, 490 bool _withDampingForces, 491 bool _withSpringForces) override; 492 493 // Documentation inherited 494 void updateForceFD( 495 const Eigen::Vector6d& _bodyForce, 496 double _timeStep, 497 bool _withDampingForces, 498 bool _withSpringForces) override; 499 500 // Documentation inherited 501 void updateImpulseID(const Eigen::Vector6d& _bodyImpulse) override; 502 503 // Documentation inherited 504 void updateImpulseFD(const Eigen::Vector6d& _bodyImpulse) override; 505 506 // Documentation inherited 507 void updateConstrainedTerms(double _timeStep) override; 508 509 /// \} 510 511 //---------------------------------------------------------------------------- 512 /// \{ \name Recursive algorithm routines for equations of motion 513 //---------------------------------------------------------------------------- 514 515 /// Add child's bias force to parent's one 516 void addChildBiasForceForInvMassMatrix( 517 Eigen::Vector6d& _parentBiasForce, 518 const Eigen::Matrix6d& _childArtInertia, 519 const Eigen::Vector6d& _childBiasForce) override; 520 521 /// Add child's bias force to parent's one 522 void addChildBiasForceForInvAugMassMatrix( 523 Eigen::Vector6d& _parentBiasForce, 524 const Eigen::Matrix6d& _childArtInertia, 525 const Eigen::Vector6d& _childBiasForce) override; 526 527 /// 528 void updateTotalForceForInvMassMatrix( 529 const Eigen::Vector6d& _bodyForce) override; 530 531 // Documentation inherited 532 void getInvMassMatrixSegment( 533 Eigen::MatrixXd& _invMassMat, 534 const std::size_t _col, 535 const Eigen::Matrix6d& _artInertia, 536 const Eigen::Vector6d& _spatialAcc) override; 537 538 // Documentation inherited 539 void getInvAugMassMatrixSegment( 540 Eigen::MatrixXd& _invMassMat, 541 const std::size_t _col, 542 const Eigen::Matrix6d& _artInertia, 543 const Eigen::Vector6d& _spatialAcc) override; 544 545 // Documentation inherited 546 void addInvMassMatrixSegmentTo(Eigen::Vector6d& _acc) override; 547 548 // Documentation inherited 549 Eigen::VectorXd getSpatialToGeneralized( 550 const Eigen::Vector6d& _spatial) override; 551 552 /// \} 553 554 private: 555 /// Used by getDofName() 556 const std::string emptyString; 557 }; 558 559 } // namespace dynamics 560 } // namespace dart 561 562 #endif // DART_DYNAMICS_ZERODOFJOINT_HPP_ 563