VelocityDamper.h

Go to the documentation of this file.

/*
 * Copyright 2017-2020 CNRS-AIST JRL and CNRS-UM LIRMM
 */
 
#pragma once
 
#include <tvm/task_dynamics/abstract/TaskDynamics.h>
 
#include <optional>
 
namespace tvm
{
 
namespace task_dynamics
{
 
class TVM_DLLAPI VelocityDamper : public abstract::TaskDynamics
{
public:
  class TVM_DLLAPI Config
  {
  public:
    Config(double di, double ds, double xsi, double xsiOff = 0, double m = 0, double lambda = 0);
 
    double di_;
    double ds_;
    double xsi_;
    double xsiOff_;
    double m_;
    double lambda_;
  };
 
  class TVM_DLLAPI AnisotropicConfig
  {
  public:
    AnisotropicConfig(const VectorConstRef & di,
                      const VectorConstRef & ds,
                      const VectorConstRef & xsi,
                      const std::optional<VectorConstRef> & xsiOff = std::nullopt,
                      const std::optional<VectorConstRef> & m = std::nullopt,
                      const std::optional<VectorConstRef> & lambda = std::nullopt);
 
    AnisotropicConfig(const Config & config);
 
    Eigen::VectorXd di_;
    Eigen::VectorXd ds_;
    Eigen::VectorXd xsi_;
    Eigen::VectorXd xsiOff_;
    Eigen::VectorXd m_;
    Eigen::VectorXd lambda_;
  };
 
  class TVM_DLLAPI Impl : public abstract::TaskDynamicsImpl
  {
  public:
    // First order dynamics
    Impl(FunctionPtr f,
         constraint::Type t,
         const Eigen::VectorXd & rhs,
         bool autoXsi,
         const Eigen::VectorXd & di,
         const Eigen::VectorXd & ds,
         const Eigen::VectorXd & xsi,
         double big);
    // Second order dynamics
    Impl(FunctionPtr f,
         constraint::Type t,
         const Eigen::VectorXd & rhs,
         double dt,
         bool autoXsi,
         const Eigen::VectorXd & di,
         const Eigen::VectorXd & ds,
         const Eigen::VectorXd & xsi,
         double big,
         const Eigen::VectorXd & m,
         const Eigen::VectorXd & lambda);
 
    void updateValue() override;
 
    ~Impl() override = default;
 
  private:
    void updateValue_(double s);
 
    double dt_;
    Eigen::VectorXd ds_;
    Eigen::VectorXd di_;
    Eigen::VectorXd xsiOff_;
    Eigen::VectorXd a_; // -1 / (di - ds)
    double big_;
 
    bool autoXsi_;
    Eigen::VectorXd d_;
    Eigen::VectorXd axsi_; // a * xsi = -xsi / (di - ds)
    std::vector<bool> active_;
 
    Eigen::VectorXd m_;
    Eigen::VectorXd lambda_;
  };
 
  VelocityDamper(const Config & config, double big = constant::big_number);
 
  VelocityDamper(const AnisotropicConfig & config, double big = constant::big_number);
 
  VelocityDamper(double dt, const Config & config, double big = constant::big_number);
 
  VelocityDamper(double dt, const AnisotropicConfig & config, double big = constant::big_number);
 
  ~VelocityDamper() override = default;
 
protected:
  std::unique_ptr<abstract::TaskDynamicsImpl> impl_(FunctionPtr f,
                                                    constraint::Type t,
                                                    const Eigen::VectorXd & rhs) const override;
  Order order_() const override;
 
  template<typename Derived, typename... Args>
  std::unique_ptr<abstract::TaskDynamicsImpl> impl_(FunctionPtr f,
                                                    constraint::Type t,
                                                    const Eigen::VectorXd & rhs,
                                                    Args &&... args) const
  {
    if(dt_ > 0)
    {
      return std::make_unique<Derived>(f, t, rhs, std::forward<Args>(args)..., dt_, autoXsi_, di_, ds_, xsi_, big_, m_,
                                       lambda_);
    }
    return std::make_unique<Derived>(f, t, rhs, std::forward<Args>(args)..., autoXsi_, di_, ds_, xsi_, big_);
  }
 
private:
  double dt_;
  Eigen::VectorXd xsi_;
  Eigen::VectorXd ds_;
  Eigen::VectorXd di_;
  double big_;
  bool autoXsi_;
  Eigen::VectorXd m_;
  Eigen::VectorXd lambda_;
};
 
} // namespace task_dynamics
 
} // namespace tvm