Joint.hpp

/*
 * Copyright (c) 2011-2021, 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 <memory>
#include <string>
#include <vector>

#include "dart/common/Deprecated.hpp"
#include "dart/common/EmbeddedAspect.hpp"
#include "dart/common/Subject.hpp"
#include "dart/common/VersionCounter.hpp"
#include "dart/dynamics/SmartPointer.hpp"
#include "dart/dynamics/detail/JointAspect.hpp"
#include "dart/math/MathTypes.hpp"

namespace dart {
namespace dynamics {

class BodyNode;
class Skeleton;
class DegreeOfFreedom;

DART_DECLARE_CLASS_WITH_VIRTUAL_BASE_BEGIN
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 MIMIC = detail::MIMIC;
  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;

  void setMimicJoint(
      const Joint* _mimicJoint,
      double _mimicMultiplier = 1.0,
      double _mimicOffset = 0.0);

  const Joint* getMimicJoint() const;

  double getMimicMultiplier() const;

  double getMimicOffset() 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;

  DART_DEPRECATED(6.10)
  void setPositionLimitEnforced(bool enforced);

  void setLimitEnforcement(bool enforced);

  DART_DEPRECATED(6.10)
  bool isPositionLimitEnforced() const;

  bool areLimitsEnforced() 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 _withDampingForces,
      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
};
DART_DECLARE_CLASS_WITH_VIRTUAL_BASE_END

} // namespace dynamics
} // namespace dart

#endif // DART_DYNAMICS_JOINT_HPP_