IK.h

Go to the documentation of this file.

/*
 * Copyright 2012-2019 CNRS-UM LIRMM, CNRS-AIST JRL
 */
 
#pragma once
 
// includes
// std
#include <vector>
 
// SpaceVecAlg
#include <SpaceVecAlg/SpaceVecAlg>
 
#include "Jacobian.h"
 
namespace rbd
{
class MultiBody;
struct MultiBodyConfig;
 
namespace ik
{
 
static constexpr int MAX_ITERATIONS = 50;
static constexpr double LAMBDA = 0.9;
static constexpr double THRESHOLD = 1e-8;
static constexpr double ALMOST_ZERO = 1e-8;
 
} // namespace ik
 
class RBDYN_DLLAPI InverseKinematics
{
public:
  InverseKinematics(const MultiBody & mb, int ef_index);
  bool inverseKinematics(const MultiBody & mb, MultiBodyConfig & mbc, const sva::PTransformd & ef_target);
 
  bool sInverseKinematics(const MultiBody & mb, MultiBodyConfig & mbc, const sva::PTransformd & ef_target);
  // @brief Maximum number of iterations
  int max_iterations_;
  // @brief Learning rate
  double lambda_;
  // @brief Stopping criterion
  double threshold_;
  // @brief Rounding threshold for the Jacobian
  double almost_zero_;
 
private:
  // @brief ef_index is the End Effector index used to build jacobian
  int ef_index_;
  Jacobian jac_;
  Eigen::JacobiSVD<Eigen::MatrixXd> svd_;
};
 
} // namespace rbd