InverseKinematics.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_INVERSEKINEMATICS_HPP_
#define DART_DYNAMICS_INVERSEKINEMATICS_HPP_
 
#include <memory>
#include <functional>
 
#include <Eigen/SVD>
 
#include "dart/common/sub_ptr.hpp"
#include "dart/common/Signal.hpp"
#include "dart/common/Subject.hpp"
#include "dart/math/Geometry.hpp"
#include "dart/optimizer/Solver.hpp"
#include "dart/optimizer/Problem.hpp"
#include "dart/optimizer/Function.hpp"
#include "dart/dynamics/SmartPointer.hpp"
#include "dart/dynamics/JacobianNode.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();
 
  bool solve(bool _applySolution = true);
 
  bool solve(Eigen::VectorXd& positions, bool _applySolution = 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<optimizer::Function>& _objective);
 
  const std::shared_ptr<optimizer::Function>& getObjective();
 
  std::shared_ptr<const optimizer::Function> getObjective() const;
 
  void setNullSpaceObjective(
      const std::shared_ptr<optimizer::Function>& _nsObjective);
 
  const std::shared_ptr<optimizer::Function>& getNullSpaceObjective();
 
  std::shared_ptr<const optimizer::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<optimizer::Problem>& getProblem();
 
  std::shared_ptr<const optimizer::Problem> getProblem() const;
 
  void resetProblem(bool _clearSeeds=false);
 
  void setSolver(const std::shared_ptr<optimizer::Solver>& _newSolver);
 
  const std::shared_ptr<optimizer::Solver>& getSolver();
 
  std::shared_ptr<const optimizer::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<optimizer::Function> mObjective;
 
  std::shared_ptr<optimizer::Function> mNullSpaceObjective;
 
  std::unique_ptr<ErrorMethod> mErrorMethod;
 
  std::unique_ptr<GradientMethod> mGradientMethod;
 
  Analytical* mAnalytical;
 
  std::shared_ptr<optimizer::Problem> mProblem;
 
  std::shared_ptr<optimizer::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 optimizer::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;
 
    UniqueProperties(bool computeErrorFromCenter = true);
  };
 
  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;
 
  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 optimizer::Function
{
public:
 
  DART_DEFINE_ALIGNED_SHARED_OBJECT_CREATOR(InverseKinematics::Objective)
 
  
  Objective(InverseKinematics* _ik);
 
  virtual ~Objective() = default;
 
  // Documentation inherited
  optimizer::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 optimizer::Function
{
public:
 
  Constraint(InverseKinematics* _ik);
 
  virtual ~Constraint() = default;
 
  // Documentation inherited
  optimizer::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_