InverseKinematics.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_INVERSEKINEMATICS_HPP_
#define DART_DYNAMICS_INVERSEKINEMATICS_HPP_

#include <functional>
#include <memory>

#include <Eigen/SVD>

#include "dart/common/Signal.hpp"
#include "dart/common/Subject.hpp"
#include "dart/common/sub_ptr.hpp"
#include "dart/dynamics/JacobianNode.hpp"
#include "dart/dynamics/SmartPointer.hpp"
#include "dart/math/Geometry.hpp"
#include "dart/optimization/Function.hpp"
#include "dart/optimization/Problem.hpp"
#include "dart/optimization/Solver.hpp"

namespace dart {
namespace dynamics {

const double DefaultIKTolerance = 1e-6;
const double DefaultIKErrorClamp = 1.0;
const double DefaultIKGradientComponentClamp = 0.2;
const double DefaultIKGradientComponentWeight = 1.0;
const double DefaultIKDLSCoefficient = 0.05;
const double DefaultIKAngularWeight = 0.4;
const double DefaultIKLinearWeight = 1.0;

class InverseKinematics : public common::Subject
{
public:
  static InverseKinematicsPtr create(JacobianNode* _node);

  InverseKinematics(const InverseKinematics&) = delete;

  InverseKinematics& operator=(const InverseKinematics&) = delete;

  virtual ~InverseKinematics();

  DART_DEPRECATED(6.8)
  bool solve(bool applySolution = true);

  DART_DEPRECATED(6.8)
  bool solve(Eigen::VectorXd& positions, bool applySolution = true);

  bool findSolution(Eigen::VectorXd& positions);

  bool solveAndApply(bool allowIncompleteResult = true);

  bool solveAndApply(
      Eigen::VectorXd& positions, bool allowIncompleteResult = true);

  InverseKinematicsPtr clone(JacobianNode* _newNode) const;

  // For the definitions of these classes, see the bottom of this header
  class Function;

  // Methods for computing IK error
  class ErrorMethod;
  class TaskSpaceRegion;

  // Methods for computing IK gradient
  class GradientMethod;
  class JacobianDLS;
  class JacobianTranspose;
  class Analytical;

  void setActive(bool _active = true);

  void setInactive();

  bool isActive() const;

  void setHierarchyLevel(std::size_t _level);

  std::size_t getHierarchyLevel() const;

  void useChain();

  void useWholeBody();

  template <class DegreeOfFreedomT>
  void setDofs(const std::vector<DegreeOfFreedomT*>& _dofs);

  void setDofs(const std::vector<std::size_t>& _dofs);

  const std::vector<std::size_t>& getDofs() const;

  const std::vector<int>& getDofMap() const;

  void setObjective(const std::shared_ptr<optimization::Function>& _objective);

  const std::shared_ptr<optimization::Function>& getObjective();

  std::shared_ptr<const optimization::Function> getObjective() const;

  void setNullSpaceObjective(
      const std::shared_ptr<optimization::Function>& _nsObjective);

  const std::shared_ptr<optimization::Function>& getNullSpaceObjective();

  std::shared_ptr<const optimization::Function> getNullSpaceObjective() const;

  bool hasNullSpaceObjective() const;

  template <class IKErrorMethod, typename... Args>
  IKErrorMethod& setErrorMethod(Args&&... args);

  ErrorMethod& getErrorMethod();

  const ErrorMethod& getErrorMethod() const;

  template <class IKGradientMethod, typename... Args>
  IKGradientMethod& setGradientMethod(Args&&... args);

  GradientMethod& getGradientMethod();

  const GradientMethod& getGradientMethod() const;

  Analytical* getAnalytical();

  const Analytical* getAnalytical() const;

  const std::shared_ptr<optimization::Problem>& getProblem();

  std::shared_ptr<const optimization::Problem> getProblem() const;

  void resetProblem(bool _clearSeeds = false);

  void setSolver(const std::shared_ptr<optimization::Solver>& _newSolver);

  const std::shared_ptr<optimization::Solver>& getSolver();

  std::shared_ptr<const optimization::Solver> getSolver() const;

  void setOffset(const Eigen::Vector3d& _offset = Eigen::Vector3d::Zero());

  const Eigen::Vector3d& getOffset() const;

  bool hasOffset() const;

  void setTarget(std::shared_ptr<SimpleFrame> _newTarget);

  std::shared_ptr<SimpleFrame> getTarget();

  std::shared_ptr<const SimpleFrame> getTarget() const;

  JacobianNode* getNode();

  const JacobianNode* getNode() const;

  JacobianNode* getAffiliation();

  const JacobianNode* getAffiliation() const;

  const math::Jacobian& computeJacobian() const;

  Eigen::VectorXd getPositions() const;

  void setPositions(const Eigen::VectorXd& _q);

  void clearCaches();

protected:
  // For the definitions of these functions, see the bottom of this header
  class Objective;
  friend class Objective;
  class Constraint;
  friend class Constraint;

  InverseKinematics(JacobianNode* _node);

  void initialize();

  void resetTargetConnection();

  void resetNodeConnection();

  common::Connection mTargetConnection;

  common::Connection mNodeConnection;

  bool mActive;

  std::size_t mHierarchyLevel;

  std::vector<std::size_t> mDofs;

  std::vector<int> mDofMap;

  std::shared_ptr<optimization::Function> mObjective;

  std::shared_ptr<optimization::Function> mNullSpaceObjective;

  std::unique_ptr<ErrorMethod> mErrorMethod;

  std::unique_ptr<GradientMethod> mGradientMethod;

  Analytical* mAnalytical;

  std::shared_ptr<optimization::Problem> mProblem;

  std::shared_ptr<optimization::Solver> mSolver;

  Eigen::Vector3d mOffset;

  bool mHasOffset;

  std::shared_ptr<SimpleFrame> mTarget;

  sub_ptr<JacobianNode> mNode;

  mutable math::Jacobian mJacobian;
};

typedef InverseKinematics IK;

//==============================================================================
class InverseKinematics::Function
{
public:
  virtual optimization::FunctionPtr clone(InverseKinematics* _newIK) const = 0;

  virtual ~Function() = default;
};

//==============================================================================
class InverseKinematics::ErrorMethod : public common::Subject
{
public:
  typedef std::pair<Eigen::Vector6d, Eigen::Vector6d> Bounds;

  struct Properties
  {
    Properties(
        const Bounds& _bounds = Bounds(
            Eigen::Vector6d::Constant(-DefaultIKTolerance),
            Eigen::Vector6d::Constant(DefaultIKTolerance)),

        double _errorClamp = DefaultIKErrorClamp,

        const Eigen::Vector6d& _errorWeights = Eigen::compose(
            Eigen::Vector3d::Constant(DefaultIKAngularWeight),
            Eigen::Vector3d::Constant(DefaultIKLinearWeight)));

    std::pair<Eigen::Vector6d, Eigen::Vector6d> mBounds;

    double mErrorLengthClamp;

    Eigen::Vector6d mErrorWeights;

    // To get byte-aligned Eigen vectors
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  };

  ErrorMethod(
      InverseKinematics* _ik,
      const std::string& _methodName,
      const Properties& _properties = Properties());

  virtual ~ErrorMethod() = default;

  virtual std::unique_ptr<ErrorMethod> clone(
      InverseKinematics* _newIK) const = 0;

  virtual Eigen::Vector6d computeError() = 0;

  virtual Eigen::Isometry3d computeDesiredTransform(
      const Eigen::Isometry3d& _currentTf, const Eigen::Vector6d& _error)
      = 0;

  const Eigen::Vector6d& evalError(const Eigen::VectorXd& _q);

  const std::string& getMethodName() const;

  void setBounds(
      const Eigen::Vector6d& _lower
      = Eigen::Vector6d::Constant(-DefaultIKTolerance),
      const Eigen::Vector6d& _upper
      = Eigen::Vector6d::Constant(DefaultIKTolerance));

  void setBounds(const std::pair<Eigen::Vector6d, Eigen::Vector6d>& _bounds);

  const std::pair<Eigen::Vector6d, Eigen::Vector6d>& getBounds() const;

  void setAngularBounds(
      const Eigen::Vector3d& _lower
      = Eigen::Vector3d::Constant(-DefaultIKTolerance),
      const Eigen::Vector3d& _upper
      = Eigen::Vector3d::Constant(DefaultIKTolerance));

  void setAngularBounds(
      const std::pair<Eigen::Vector3d, Eigen::Vector3d>& _bounds);

  std::pair<Eigen::Vector3d, Eigen::Vector3d> getAngularBounds() const;

  void setLinearBounds(
      const Eigen::Vector3d& _lower
      = Eigen::Vector3d::Constant(-DefaultIKTolerance),
      const Eigen::Vector3d& _upper
      = Eigen::Vector3d::Constant(DefaultIKTolerance));

  void setLinearBounds(
      const std::pair<Eigen::Vector3d, Eigen::Vector3d>& _bounds);

  std::pair<Eigen::Vector3d, Eigen::Vector3d> getLinearBounds() const;

  void setErrorLengthClamp(double _clampSize = DefaultIKErrorClamp);

  double getErrorLengthClamp() const;

  void setErrorWeights(const Eigen::Vector6d& _weights);

  const Eigen::Vector6d& getErrorWeights() const;

  void setAngularErrorWeights(
      const Eigen::Vector3d& _weights
      = Eigen::Vector3d::Constant(DefaultIKAngularWeight));

  Eigen::Vector3d getAngularErrorWeights() const;

  void setLinearErrorWeights(
      const Eigen::Vector3d& _weights
      = Eigen::Vector3d::Constant(DefaultIKLinearWeight));

  Eigen::Vector3d getLinearErrorWeights() const;

  Properties getErrorMethodProperties() const;

  void clearCache();

protected:
  common::sub_ptr<InverseKinematics> mIK;

  std::string mMethodName;

  Eigen::VectorXd mLastPositions;

  Eigen::Vector6d mLastError;

  Properties mErrorP;

public:
  // To get byte-aligned Eigen vectors
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

//==============================================================================
class InverseKinematics::TaskSpaceRegion : public ErrorMethod
{
public:
  struct UniqueProperties
  {
    bool mComputeErrorFromCenter;

    SimpleFramePtr mReferenceFrame;

    UniqueProperties(
        bool computeErrorFromCenter = true,
        SimpleFramePtr referenceFrame = nullptr);
  };

  struct Properties : ErrorMethod::Properties, UniqueProperties
  {
    Properties(
        const ErrorMethod::Properties& errorProperties
        = ErrorMethod::Properties(),
        const UniqueProperties& taskSpaceProperties = UniqueProperties());
  };

  explicit TaskSpaceRegion(
      InverseKinematics* _ik, const Properties& _properties = Properties());

  virtual ~TaskSpaceRegion() = default;

  // Documentation inherited
  std::unique_ptr<ErrorMethod> clone(InverseKinematics* _newIK) const override;

  // Documentation inherited
  Eigen::Isometry3d computeDesiredTransform(
      const Eigen::Isometry3d& _currentTf,
      const Eigen::Vector6d& _error) override;

  // Documentation inherited
  Eigen::Vector6d computeError() override;

  void setComputeFromCenter(bool computeFromCenter);

  bool isComputingFromCenter() const;

  void setReferenceFrame(SimpleFramePtr referenceFrame);

  ConstSimpleFramePtr getReferenceFrame() const;

  Properties getTaskSpaceRegionProperties() const;

protected:
  UniqueProperties mTaskSpaceP;
};

//==============================================================================
class InverseKinematics::GradientMethod : public common::Subject
{
public:
  struct Properties
  {
    double mComponentWiseClamp;

    Eigen::VectorXd mComponentWeights;

    Properties(
        double clamp = DefaultIKGradientComponentClamp,
        const Eigen::VectorXd& weights = Eigen::VectorXd());
  };

  GradientMethod(
      InverseKinematics* _ik,
      const std::string& _methodName,
      const Properties& _properties);

  virtual ~GradientMethod() = default;

  virtual std::unique_ptr<GradientMethod> clone(
      InverseKinematics* _newIK) const = 0;

  virtual void computeGradient(
      const Eigen::Vector6d& _error, Eigen::VectorXd& _grad)
      = 0;

  void evalGradient(
      const Eigen::VectorXd& _q, Eigen::Map<Eigen::VectorXd> _grad);

  const std::string& getMethodName() const;

  void clampGradient(Eigen::VectorXd& _grad) const;

  void setComponentWiseClamp(double _clamp = DefaultIKGradientComponentClamp);

  double getComponentWiseClamp() const;

  void applyWeights(Eigen::VectorXd& _grad) const;

  void setComponentWeights(const Eigen::VectorXd& _weights);

  const Eigen::VectorXd& getComponentWeights() const;

  void convertJacobianMethodOutputToGradient(
      Eigen::VectorXd& grad, const std::vector<std::size_t>& dofs);

  Properties getGradientMethodProperties() const;

  void clearCache();

  InverseKinematics* getIK();

  const InverseKinematics* getIK() const;

protected:
  common::sub_ptr<InverseKinematics> mIK;

  std::string mMethodName;

  Eigen::VectorXd mLastPositions;

  Eigen::VectorXd mLastGradient;

  Properties mGradientP;

private:
  Eigen::VectorXd mInitialPositionsCache;
};

//==============================================================================
class InverseKinematics::JacobianDLS : public GradientMethod
{
public:
  struct UniqueProperties
  {
    double mDamping;

    UniqueProperties(double damping = DefaultIKDLSCoefficient);
  };

  struct Properties : GradientMethod::Properties, UniqueProperties
  {
    Properties(
        const GradientMethod::Properties& gradientProperties
        = GradientMethod::Properties(),
        const UniqueProperties& dlsProperties = UniqueProperties());
  };

  explicit JacobianDLS(
      InverseKinematics* _ik, const Properties& properties = Properties());

  virtual ~JacobianDLS() = default;

  // Documentation inherited
  std::unique_ptr<GradientMethod> clone(
      InverseKinematics* _newIK) const override;

  // Documentation inherited
  void computeGradient(
      const Eigen::Vector6d& _error, Eigen::VectorXd& _grad) override;

  void setDampingCoefficient(double _damping = DefaultIKDLSCoefficient);

  double getDampingCoefficient() const;

  Properties getJacobianDLSProperties() const;

protected:
  UniqueProperties mDLSProperties;
};

//==============================================================================
class InverseKinematics::JacobianTranspose : public GradientMethod
{
public:
  explicit JacobianTranspose(
      InverseKinematics* _ik, const Properties& properties = Properties());

  virtual ~JacobianTranspose() = default;

  // Documentation inherited
  std::unique_ptr<GradientMethod> clone(
      InverseKinematics* _newIK) const override;

  // Documentation inherited
  void computeGradient(
      const Eigen::Vector6d& _error, Eigen::VectorXd& _grad) override;
};

//==============================================================================
class InverseKinematics::Analytical : public GradientMethod
{
public:
  enum Validity_t
  {
    VALID = 0, 
    OUT_OF_REACH = 1 << 0,  
    LIMIT_VIOLATED = 1 << 1 
  };
  // TODO(JS): Change to enum class?

  enum ExtraDofUtilization
  {
    UNUSED = 0,
    PRE_ANALYTICAL,
    POST_ANALYTICAL,
    PRE_AND_POST_ANALYTICAL
  };
  // TODO(JS): Change to enum class?

  struct Solution
  {
    Solution(
        const Eigen::VectorXd& _config = Eigen::VectorXd(),
        int _validity = VALID);

    Eigen::VectorXd mConfig;

    int mValidity;
  };

  // std::function template for comparing the quality of configurations
  typedef std::function<bool(
      const Eigen::VectorXd& _better,
      const Eigen::VectorXd& _worse,
      const InverseKinematics* _ik)>
      QualityComparison;

  struct UniqueProperties
  {
    ExtraDofUtilization mExtraDofUtilization;

    double mExtraErrorLengthClamp;

    QualityComparison mQualityComparator;

    UniqueProperties(
        ExtraDofUtilization extraDofUtilization = UNUSED,
        double extraErrorLengthClamp = DefaultIKErrorClamp);

    UniqueProperties(
        ExtraDofUtilization extraDofUtilization,
        double extraErrorLengthClamp,
        QualityComparison qualityComparator);

    void resetQualityComparisonFunction();
  };

  struct Properties : GradientMethod::Properties, UniqueProperties
  {
    // Default constructor
    Properties(
        const GradientMethod::Properties& gradientProperties
        = GradientMethod::Properties(),
        const UniqueProperties& analyticalProperties = UniqueProperties());

    // Construct Properties by specifying the UniqueProperties. The
    // GradientMethod::Properties components will be set to defaults.
    Properties(const UniqueProperties& analyticalProperties);
  };

  Analytical(
      InverseKinematics* _ik,
      const std::string& _methodName,
      const Properties& _properties);

  virtual ~Analytical() = default;

  const std::vector<Solution>& getSolutions();

  const std::vector<Solution>& getSolutions(
      const Eigen::Isometry3d& _desiredTf);

  void computeGradient(
      const Eigen::Vector6d& _error, Eigen::VectorXd& _grad) override;

  virtual const std::vector<Solution>& computeSolutions(
      const Eigen::Isometry3d& _desiredBodyTf)
      = 0;

  virtual const std::vector<std::size_t>& getDofs() const = 0;

  void setPositions(const Eigen::VectorXd& _config);

  Eigen::VectorXd getPositions() const;

  void setExtraDofUtilization(ExtraDofUtilization _utilization);

  ExtraDofUtilization getExtraDofUtilization() const;

  void setExtraErrorLengthClamp(double _clamp);

  double getExtraErrorLengthClamp() const;

  void setQualityComparisonFunction(const QualityComparison& _func);

  void resetQualityComparisonFunction();

  Properties getAnalyticalProperties() const;

  void constructDofMap();

protected:
  virtual void addExtraDofGradient(
      Eigen::VectorXd& grad,
      const Eigen::Vector6d& error,
      ExtraDofUtilization utilization);

  void checkSolutionJointLimits();

  std::vector<Solution> mSolutions;

  UniqueProperties mAnalyticalP;

private:
  std::vector<int> mDofMap;

  std::vector<std::size_t> mExtraDofs;

  std::vector<Solution> mValidSolutionsCache;

  std::vector<Solution> mOutOfReachCache;

  std::vector<Solution> mLimitViolationCache;

  Eigen::VectorXd mConfigCache;

  Eigen::VectorXd mRestoreConfigCache;

  Eigen::VectorXd mExtraDofGradCache;
};

//==============================================================================
class InverseKinematics::Objective final : public Function,
                                           public optimization::Function
{
public:
  DART_DEFINE_ALIGNED_SHARED_OBJECT_CREATOR(InverseKinematics::Objective)

  
  Objective(InverseKinematics* _ik);

  virtual ~Objective() = default;

  // Documentation inherited
  optimization::FunctionPtr clone(InverseKinematics* _newIK) const override;

  // Documentation inherited
  double eval(const Eigen::VectorXd& _x) override;

  // Documentation inherited
  void evalGradient(
      const Eigen::VectorXd& _x, Eigen::Map<Eigen::VectorXd> _grad) override;

protected:
  sub_ptr<InverseKinematics> mIK;

  Eigen::VectorXd mGradCache;

  Eigen::JacobiSVD<math::Jacobian> mSVDCache;
  // TODO(JS): Need to define aligned operator new for this?

  Eigen::MatrixXd mNullSpaceCache;

public:
  // To get byte-aligned Eigen vectors
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

//==============================================================================
class InverseKinematics::Constraint final : public Function,
                                            public optimization::Function
{
public:
  Constraint(InverseKinematics* _ik);

  virtual ~Constraint() = default;

  // Documentation inherited
  optimization::FunctionPtr clone(InverseKinematics* _newIK) const override;

  // Documentation inherited
  double eval(const Eigen::VectorXd& _x) override;

  // Documentation inherited
  void evalGradient(
      const Eigen::VectorXd& _x, Eigen::Map<Eigen::VectorXd> _grad) override;

protected:
  sub_ptr<InverseKinematics> mIK;
};

} // namespace dynamics
} // namespace dart

#include "dart/dynamics/detail/InverseKinematics.hpp"

#endif // DART_DYNAMICS_INVERSEKINEMATICS_HPP_