Joint.hpp

Go to the documentation of this file.

/*
 * Copyright (c) 2011-2018, The DART development contributors
 * All rights reserved.
 *
 * The list of contributors can be found at:
 *   https://github.com/dartsim/dart/blob/master/LICENSE
 *
 * This file is provided under the following "BSD-style" License:
 *   Redistribution and use in source and binary forms, with or
 *   without modification, are permitted provided that the following
 *   conditions are met:
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
 *   CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
 *   INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *   MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 *   DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
 *   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 *   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 *   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
 *   USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *   AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *   LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *   ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *   POSSIBILITY OF SUCH DAMAGE.
 */
 
#ifndef DART_DYNAMICS_JOINT_HPP_
#define DART_DYNAMICS_JOINT_HPP_
 
#include <string>
#include <vector>
#include <memory>
 
#include "dart/common/Deprecated.hpp"
#include "dart/common/Subject.hpp"
#include "dart/common/VersionCounter.hpp"
#include "dart/common/EmbeddedAspect.hpp"
#include "dart/math/MathTypes.hpp"
#include "dart/dynamics/SmartPointer.hpp"
#include "dart/dynamics/detail/JointAspect.hpp"
 
namespace dart {
namespace dynamics {
 
class BodyNode;
class Skeleton;
class DegreeOfFreedom;
 
class Joint : public virtual common::Subject,
              public virtual common::VersionCounter,
              public common::EmbedProperties<Joint, detail::JointProperties>
{
public:
 
  using CompositeProperties = common::Composite::Properties;
  using Properties = detail::JointProperties;
 
  typedef detail::ActuatorType ActuatorType;
  static constexpr ActuatorType FORCE        = detail::FORCE;
  static constexpr ActuatorType PASSIVE      = detail::PASSIVE;
  static constexpr ActuatorType SERVO        = detail::SERVO;
  static constexpr ActuatorType ACCELERATION = detail::ACCELERATION;
  static constexpr ActuatorType VELOCITY     = detail::VELOCITY;
  static constexpr ActuatorType LOCKED       = detail::LOCKED;
 
  DART_BAKE_SPECIALIZED_ASPECT_IRREGULAR(Aspect, JointAspect)
 
  struct ExtendedProperties : Properties
  {
    ExtendedProperties(
        const Properties& standardProperties = Properties(),
        const CompositeProperties& aspectProperties = CompositeProperties());
 
    ExtendedProperties(
        Properties&& standardProperties,
        CompositeProperties&& aspectProperties);
 
    CompositeProperties mCompositeProperties;
  };
 
  static const ActuatorType DefaultActuatorType;
 
  Joint(const Joint&) = delete;
 
  virtual ~Joint();
 
  void setProperties(const Properties& properties);
 
  void setAspectProperties(const AspectProperties& properties);
 
  const Properties& getJointProperties() const;
 
  void copy(const Joint& _otherJoint);
 
  void copy(const Joint* _otherJoint);
 
  Joint& operator=(const Joint& _otherJoint);
 
  const std::string& setName(const std::string& _name,
                             bool _renameDofs = true);
 
  const std::string& getName() const;
 
  virtual const std::string& getType() const = 0;
 
  void setActuatorType(ActuatorType _actuatorType);
 
  ActuatorType getActuatorType() const;
 
  bool isKinematic() const;
 
  bool isDynamic() const;
 
  BodyNode* getChildBodyNode();
 
  const BodyNode* getChildBodyNode() const;
 
  BodyNode* getParentBodyNode();
 
  const BodyNode* getParentBodyNode() const;
 
  SkeletonPtr getSkeleton();
 
  std::shared_ptr<const Skeleton> getSkeleton() const;
 
  virtual void setTransformFromParentBodyNode(const Eigen::Isometry3d& _T);
 
  virtual void setTransformFromChildBodyNode(const Eigen::Isometry3d& _T);
 
  const Eigen::Isometry3d& getTransformFromParentBodyNode() const;
 
  const Eigen::Isometry3d& getTransformFromChildBodyNode() const;
 
  void setPositionLimitEnforced(bool _isPositionLimitEnforced);
 
  bool isPositionLimitEnforced() const;
 
  virtual std::size_t getIndexInSkeleton(std::size_t _index) const = 0;
 
  virtual std::size_t getIndexInTree(std::size_t _index) const = 0;
 
  std::size_t getJointIndexInSkeleton() const;
 
  std::size_t getJointIndexInTree() const;
 
  std::size_t getTreeIndex() const;
 
  virtual DegreeOfFreedom* getDof(std::size_t _index) = 0;
 
  virtual const DegreeOfFreedom* getDof(std::size_t _index) const = 0;
 
  virtual const std::string& setDofName(std::size_t _index,
                                const std::string& _name,
                                bool _preserveName=true) = 0;
 
  virtual void preserveDofName(std::size_t _index, bool _preserve) = 0;
 
  virtual bool isDofNamePreserved(std::size_t _index) const = 0;
 
  virtual const std::string& getDofName(std::size_t _index) const = 0;
 
  virtual std::size_t getNumDofs() const = 0;
 
  //----------------------------------------------------------------------------
  //----------------------------------------------------------------------------
 
  virtual void setCommand(std::size_t _index, double _command) = 0;
 
  virtual double getCommand(std::size_t _index) const = 0;
 
  virtual void setCommands(const Eigen::VectorXd& _commands) = 0;
 
  virtual Eigen::VectorXd getCommands() const = 0;
 
  virtual void resetCommands() = 0;
 
 
  //----------------------------------------------------------------------------
  //----------------------------------------------------------------------------
 
  virtual void setPosition(std::size_t _index, double _position) = 0;
 
  virtual double getPosition(std::size_t _index) const = 0;
 
  virtual void setPositions(const Eigen::VectorXd& _positions) = 0;
 
  virtual Eigen::VectorXd getPositions() const = 0;
 
  virtual void setPositionLowerLimit(std::size_t _index, double _position) = 0;
 
  virtual double getPositionLowerLimit(std::size_t _index) const = 0;
 
  virtual void setPositionLowerLimits(const Eigen::VectorXd& lowerLimits) = 0;
 
  virtual Eigen::VectorXd getPositionLowerLimits() const = 0;
 
  virtual void setPositionUpperLimit(std::size_t _index, double _position) = 0;
 
  virtual double getPositionUpperLimit(std::size_t _index) const = 0;
 
  virtual void setPositionUpperLimits(const Eigen::VectorXd& upperLimits) = 0;
 
  virtual Eigen::VectorXd getPositionUpperLimits() const = 0;
 
  virtual bool isCyclic(std::size_t _index) const = 0;
 
  virtual bool hasPositionLimit(std::size_t _index) const = 0;
 
  virtual void resetPosition(std::size_t _index) = 0;
 
  virtual void resetPositions() = 0;
 
  virtual void setInitialPosition(std::size_t _index, double _initial) = 0;
 
  virtual double getInitialPosition(std::size_t _index) const = 0;
 
  virtual void setInitialPositions(const Eigen::VectorXd& _initial) = 0;
 
  virtual Eigen::VectorXd getInitialPositions() const = 0;
 
 
  //----------------------------------------------------------------------------
  //----------------------------------------------------------------------------
 
  virtual void setVelocity(std::size_t _index, double _velocity) = 0;
 
  virtual double getVelocity(std::size_t _index) const = 0;
 
  virtual void setVelocities(const Eigen::VectorXd& _velocities) = 0;
 
  virtual Eigen::VectorXd getVelocities() const = 0;
 
  virtual void setVelocityLowerLimit(std::size_t _index, double _velocity) = 0;
 
  virtual double getVelocityLowerLimit(std::size_t _index) const = 0;
 
  virtual void setVelocityLowerLimits(const Eigen::VectorXd& lowerLimits) = 0;
 
  virtual Eigen::VectorXd getVelocityLowerLimits() const = 0;
 
  virtual void setVelocityUpperLimit(std::size_t _index, double _velocity) = 0;
 
  virtual double getVelocityUpperLimit(std::size_t _index) const = 0;
 
  virtual void setVelocityUpperLimits(const Eigen::VectorXd& upperLimits) = 0;
 
  virtual Eigen::VectorXd getVelocityUpperLimits() const = 0;
 
  virtual void resetVelocity(std::size_t _index) = 0;
 
  virtual void resetVelocities() = 0;
 
  virtual void setInitialVelocity(std::size_t _index, double _initial) = 0;
 
  virtual double getInitialVelocity(std::size_t _index) const = 0;
 
  virtual void setInitialVelocities(const Eigen::VectorXd& _initial) = 0;
 
  virtual Eigen::VectorXd getInitialVelocities() const = 0;
 
 
  //----------------------------------------------------------------------------
  //----------------------------------------------------------------------------
 
  virtual void setAcceleration(std::size_t _index, double _acceleration) = 0;
 
  virtual double getAcceleration(std::size_t _index) const = 0;
 
  virtual void setAccelerations(const Eigen::VectorXd& _accelerations) = 0;
 
  virtual Eigen::VectorXd getAccelerations() const = 0;
 
  virtual void resetAccelerations() = 0;
 
  virtual void setAccelerationLowerLimit(std::size_t _index, double _acceleration) = 0;
 
  virtual double getAccelerationLowerLimit(std::size_t _index) const = 0;
 
  virtual void setAccelerationLowerLimits(const Eigen::VectorXd& lowerLimits) = 0;
 
  virtual Eigen::VectorXd getAccelerationLowerLimits() const = 0;
 
  virtual void setAccelerationUpperLimit(std::size_t _index, double _acceleration) = 0;
 
  virtual double getAccelerationUpperLimit(std::size_t _index) const = 0;
 
  virtual void setAccelerationUpperLimits(const Eigen::VectorXd& upperLimits) = 0;
 
  virtual Eigen::VectorXd getAccelerationUpperLimits() const = 0;
 
 
  //----------------------------------------------------------------------------
  //----------------------------------------------------------------------------
 
  virtual void setForce(std::size_t _index, double _force) = 0;
 
  virtual double getForce(std::size_t _index) const = 0;
 
  virtual void setForces(const Eigen::VectorXd& _forces) = 0;
 
  virtual Eigen::VectorXd getForces() const = 0;
 
  virtual void resetForces() = 0;
 
  virtual void setForceLowerLimit(std::size_t _index, double _force) = 0;
 
  virtual double getForceLowerLimit(std::size_t _index) const = 0;
 
  virtual void setForceLowerLimits(const Eigen::VectorXd& lowerLimits) = 0;
 
  virtual Eigen::VectorXd getForceLowerLimits() const = 0;
 
  virtual void setForceUpperLimit(std::size_t _index, double _force) = 0;
 
  virtual double getForceUpperLimit(std::size_t _index) const = 0;
 
  virtual void setForceUpperLimits(const Eigen::VectorXd& upperLimits) = 0;
 
  virtual Eigen::VectorXd getForceUpperLimits() const = 0;
 
 
  //----------------------------------------------------------------------------
  //----------------------------------------------------------------------------
 
  // TODO: Consider extending this to a more comprehensive sanity check
  bool checkSanity(bool _printWarnings = true) const;
 
  //----------------------------------------------------------------------------
  //----------------------------------------------------------------------------
 
  virtual void setVelocityChange(std::size_t _index, double _velocityChange) = 0;
 
  virtual double getVelocityChange(std::size_t _index) const = 0;
 
  virtual void resetVelocityChanges() = 0;
 
 
  //----------------------------------------------------------------------------
  //----------------------------------------------------------------------------
 
  virtual void setConstraintImpulse(std::size_t _index, double _impulse) = 0;
 
  virtual double getConstraintImpulse(std::size_t _index) const = 0;
 
  virtual void resetConstraintImpulses() = 0;
 
 
  //----------------------------------------------------------------------------
  //----------------------------------------------------------------------------
 
  virtual void integratePositions(double _dt) = 0;
 
  virtual void integrateVelocities(double _dt) = 0;
 
  virtual Eigen::VectorXd getPositionDifferences(
      const Eigen::VectorXd& _q2, const Eigen::VectorXd& _q1) const = 0;
 
 
  //----------------------------------------------------------------------------
  //----------------------------------------------------------------------------
 
  virtual void setSpringStiffness(std::size_t _index, double _k) = 0;
 
  virtual double getSpringStiffness(std::size_t _index) const = 0;
 
  virtual void setRestPosition(std::size_t _index, double _q0) = 0;
 
  virtual double getRestPosition(std::size_t _index) const = 0;
 
  virtual void setDampingCoefficient(std::size_t _index, double _coeff) = 0;
 
  virtual double getDampingCoefficient(std::size_t _index) const = 0;
 
  virtual void setCoulombFriction(std::size_t _index, double _friction) = 0;
 
  virtual double getCoulombFriction(std::size_t _index) const = 0;
 
 
  //----------------------------------------------------------------------------
 
  DART_DEPRECATED(6.1)
  double getPotentialEnergy() const;
 
  virtual double computePotentialEnergy() const = 0;
 
  //----------------------------------------------------------------------------
 
  DART_DEPRECATED(6.0)
  const Eigen::Isometry3d& getLocalTransform() const;
 
  DART_DEPRECATED(6.0)
  const Eigen::Vector6d& getLocalSpatialVelocity() const;
 
  DART_DEPRECATED(6.0)
  const Eigen::Vector6d& getLocalSpatialAcceleration() const;
 
  DART_DEPRECATED(6.0)
  const Eigen::Vector6d& getLocalPrimaryAcceleration() const;
 
  DART_DEPRECATED(6.0)
  const math::Jacobian getLocalJacobian() const;
 
  DART_DEPRECATED(6.0)
  math::Jacobian getLocalJacobian(const Eigen::VectorXd& positions) const;
 
  DART_DEPRECATED(6.0)
  const math::Jacobian getLocalJacobianTimeDeriv() const;
 
  const Eigen::Isometry3d& getRelativeTransform() const;
 
  const Eigen::Vector6d& getRelativeSpatialVelocity() const;
 
  const Eigen::Vector6d& getRelativeSpatialAcceleration() const;
 
  const Eigen::Vector6d& getRelativePrimaryAcceleration() const;
 
  virtual const math::Jacobian getRelativeJacobian() const = 0;
 
  virtual math::Jacobian getRelativeJacobian(
      const Eigen::VectorXd& positions) const = 0;
 
  virtual const math::Jacobian getRelativeJacobianTimeDeriv() const = 0;
 
  virtual Eigen::Vector6d getBodyConstraintWrench() const = 0;
  // TODO: Need more informative name.
 
//  Eigen::VectorXd getSpringForces(double _timeStep) const;
 
//  Eigen::VectorXd getDampingForces() const;
 
 
  //----------------------------------------------------------------------------
  //----------------------------------------------------------------------------
 
  DART_DEPRECATED(6.2)
  void notifyPositionUpdate();
 
  void notifyPositionUpdated();
 
  DART_DEPRECATED(6.2)
  void notifyVelocityUpdate();
 
  void notifyVelocityUpdated();
 
  DART_DEPRECATED(6.2)
  void notifyAccelerationUpdate();
 
  void notifyAccelerationUpdated();
 
 
  //----------------------------------------------------------------------------
  // Friendship
  //----------------------------------------------------------------------------
 
  friend class BodyNode;
  friend class SoftBodyNode;
  friend class Skeleton;
 
protected:
 
  Joint();
 
  virtual Joint* clone() const = 0;
 
  virtual void registerDofs() = 0;
 
  DegreeOfFreedom* createDofPointer(std::size_t _indexInJoint);
 
  virtual void updateDegreeOfFreedomNames() = 0;
 
  //----------------------------------------------------------------------------
  //----------------------------------------------------------------------------
 
  DART_DEPRECATED(6.0)
  void updateLocalTransform() const;
 
  DART_DEPRECATED(6.0)
  void updateLocalSpatialVelocity() const;
 
  DART_DEPRECATED(6.0)
  void updateLocalSpatialAcceleration() const;
 
  DART_DEPRECATED(6.0)
  void updateLocalPrimaryAcceleration() const;
 
  DART_DEPRECATED(6.0)
  void updateLocalJacobian(bool mandatory = true) const;
 
  DART_DEPRECATED(6.0)
  void updateLocalJacobianTimeDeriv() const;
 
  virtual void updateRelativeTransform() const = 0;
 
  virtual void updateRelativeSpatialVelocity() const = 0;
 
  virtual void updateRelativeSpatialAcceleration() const = 0;
 
  virtual void updateRelativePrimaryAcceleration() const = 0;
 
  virtual void updateRelativeJacobian(bool mandatory = true) const = 0;
 
  virtual void updateRelativeJacobianTimeDeriv() const = 0;
 
  void updateArticulatedInertia() const;
 
  virtual void addVelocityTo(Eigen::Vector6d& _vel) = 0;
 
  virtual void setPartialAccelerationTo(
      Eigen::Vector6d& _partialAcceleration,
      const Eigen::Vector6d& _childVelocity) = 0;
  // TODO(JS): Rename with more informative name
 
  virtual void addAccelerationTo(Eigen::Vector6d& _acc) = 0;
 
  virtual void addVelocityChangeTo(Eigen::Vector6d& _velocityChange) = 0;
 
  virtual void addChildArtInertiaTo(
      Eigen::Matrix6d& _parentArtInertia,
      const Eigen::Matrix6d& _childArtInertia) = 0;
 
  virtual void addChildArtInertiaImplicitTo(
      Eigen::Matrix6d& _parentArtInertiaImplicit,
      const Eigen::Matrix6d& _childArtInertiaImplicit) = 0;
  // TODO(JS): rename to updateAInertiaChildAInertia()
 
  virtual void updateInvProjArtInertia(const Eigen::Matrix6d& _artInertia) = 0;
 
  virtual void updateInvProjArtInertiaImplicit(
      const Eigen::Matrix6d& _artInertia,
      double _timeStep) = 0;
  // TODO(JS): rename to updateAInertiaPsi()
 
  virtual void addChildBiasForceTo(
      Eigen::Vector6d& _parentBiasForce,
      const Eigen::Matrix6d& _childArtInertia,
      const Eigen::Vector6d& _childBiasForce,
      const Eigen::Vector6d& _childPartialAcc) = 0;
 
  virtual void addChildBiasImpulseTo(
      Eigen::Vector6d& _parentBiasImpulse,
      const Eigen::Matrix6d& _childArtInertia,
      const Eigen::Vector6d& _childBiasImpulse) = 0;
 
  virtual void updateTotalForce(const Eigen::Vector6d& _bodyForce,
                                double _timeStep) = 0;
  // TODO: rename
 
  virtual void updateTotalImpulse(const Eigen::Vector6d& _bodyImpulse) = 0;
  // TODO: rename
 
  virtual void resetTotalImpulses() = 0;
 
  virtual void updateAcceleration(const Eigen::Matrix6d& _artInertia,
                                  const Eigen::Vector6d& _spatialAcc) = 0;
 
  virtual void updateVelocityChange(
      const Eigen::Matrix6d& _artInertia,
      const Eigen::Vector6d& _velocityChange) = 0;
 
  virtual void updateForceID(const Eigen::Vector6d& _bodyForce,
                             double _timeStep,
                             bool _withDampingForces,
                             bool _withSpringForces) = 0;
 
  virtual void updateForceFD(const Eigen::Vector6d& _bodyForce,
                             double _timeStep,
                             bool _withDampingForcese,
                             bool _withSpringForces) = 0;
 
  virtual void updateImpulseID(const Eigen::Vector6d& _bodyImpulse) = 0;
 
  virtual void updateImpulseFD(const Eigen::Vector6d& _bodyImpulse) = 0;
 
  virtual void updateConstrainedTerms(double _timeStep) = 0;
 
 
  //----------------------------------------------------------------------------
  //----------------------------------------------------------------------------
 
  virtual void addChildBiasForceForInvMassMatrix(
      Eigen::Vector6d& _parentBiasForce,
      const Eigen::Matrix6d& _childArtInertia,
      const Eigen::Vector6d& _childBiasForce) = 0;
 
  virtual void addChildBiasForceForInvAugMassMatrix(
      Eigen::Vector6d& _parentBiasForce,
      const Eigen::Matrix6d& _childArtInertia,
      const Eigen::Vector6d& _childBiasForce) = 0;
 
  virtual void updateTotalForceForInvMassMatrix(
      const Eigen::Vector6d& _bodyForce) = 0;
 
  virtual void getInvMassMatrixSegment(Eigen::MatrixXd& _invMassMat,
                                       const std::size_t _col,
                                       const Eigen::Matrix6d& _artInertia,
                                       const Eigen::Vector6d& _spatialAcc) = 0;
 
  virtual void getInvAugMassMatrixSegment(Eigen::MatrixXd& _invMassMat,
                                       const std::size_t _col,
                                       const Eigen::Matrix6d& _artInertia,
                                       const Eigen::Vector6d& _spatialAcc) = 0;
 
  virtual void addInvMassMatrixSegmentTo(Eigen::Vector6d& _acc) = 0;
 
  virtual Eigen::VectorXd getSpatialToGeneralized(
      const Eigen::Vector6d& _spatial) = 0;
 
 
protected:
 
  BodyNode* mChildBodyNode;
 
  mutable Eigen::Isometry3d mT;
 
  mutable Eigen::Vector6d mSpatialVelocity;
 
  mutable Eigen::Vector6d mSpatialAcceleration;
 
  mutable Eigen::Vector6d mPrimaryAcceleration;
 
  mutable bool mNeedTransformUpdate;
  // TODO(JS): Rename this to mIsTransformDirty in DART 7
 
  mutable bool mNeedSpatialVelocityUpdate;
  // TODO(JS): Rename this to mIsSpatialVelocityDirty in DART 7
 
  mutable bool mNeedSpatialAccelerationUpdate;
  // TODO(JS): Rename this to mIsSpatialAccelerationDirty in DART 7
 
  mutable bool mNeedPrimaryAccelerationUpdate;
  // TODO(JS): Rename this to mIsPrimaryAccelerationDirty in DART 7
 
  mutable bool mIsRelativeJacobianDirty;
 
  mutable bool mIsRelativeJacobianTimeDerivDirty;
 
public:
 
  // To get byte-aligned Eigen vectors
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
 
}  // namespace dynamics
}  // namespace dart
 
#endif  // DART_DYNAMICS_JOINT_HPP_