ProportionalDerivative.h

Go to the documentation of this file.

/*
 * Copyright 2017-2020 CNRS-AIST JRL and CNRS-UM LIRMM
 */
 
#pragma once
 
// std::variant is basically unusable in clang 6 + libstdc++ due to a bug
// see, e.g. https://bugs.llvm.org/show_bug.cgi?id=33222
// We use https://github.com/mpark/variant/ instead
#include <mpark/variant.hpp>
 
#include <tvm/task_dynamics/abstract/TaskDynamics.h>
 
namespace tvm
{
 
namespace task_dynamics
{
 
class TVM_DLLAPI ProportionalDerivative : public abstract::TaskDynamics
{
public:
  using Gain = mpark::variant<double, Eigen::VectorXd, Eigen::MatrixXd>;
 
  class TVM_DLLAPI Impl : public abstract::TaskDynamicsImpl
  {
  public:
    Impl(FunctionPtr f, constraint::Type t, const Eigen::VectorXd & rhs, const Gain & kp, const Gain & kv);
    void updateValue() override;
 
    ~Impl() override = default;
 
    std::pair<const Gain &, const Gain &> gains() const;
    void gains(double kp, double kv);
    void gains(const Eigen::VectorXd & kp, const Eigen::VectorXd & kv);
    void gains(const Eigen::MatrixXd & kp, const Eigen::MatrixXd & kv);
    void gains(double kp, const Eigen::VectorXd & kv);
    void gains(double kp, const Eigen::MatrixXd & kv);
    void gains(const Eigen::VectorXd & kp, double kv);
    void gains(const Eigen::VectorXd & kp, const Eigen::MatrixXd & kv);
    void gains(const Eigen::MatrixXd & kp, double kv);
    void gains(const Eigen::MatrixXd & kp, const Eigen::VectorXd & kv);
    void gains(double kp);
    void gains(const Eigen::VectorXd & kp);
    void gains(const Eigen::MatrixXd & kp);
 
    const Gain & kp() const { return kp_; }
    template<typename T>
    const T & kp() const
    { return mpark::get<T>(kp_); }
    Gain & kp() { return kp_; }
    template<typename T>
    T & kp()
    { return mpark::get<T>(kp_); }
 
    const Gain & kv() const { return kv_; }
    template<typename T>
    const T & kv() const
    { return mpark::get<T>(kv_); }
    Gain & kv() { return kv_; }
    template<typename T>
    T & kv()
    { return mpark::get<T>(kv_); }
 
  private:
    void checkGainSize(double k) const;
    void checkGainSize(const Eigen::VectorXd & k) const;
    void checkGainSize(const Eigen::MatrixXd & k) const;
    template<typename T, typename U>
    void gains_(const T & kp, const U & kv);
 
    Gain kp_; // Stiffness gain
    Gain kv_; // Damping gain
  };
 
  ProportionalDerivative(double kp, double kv);
  ProportionalDerivative(const Eigen::VectorXd & kp, const Eigen::VectorXd & kv);
  ProportionalDerivative(const Eigen::MatrixXd & kp, const Eigen::MatrixXd & kv);
  ProportionalDerivative(double kp, const Eigen::VectorXd & kv);
  ProportionalDerivative(double kp, const Eigen::MatrixXd & kv);
  ProportionalDerivative(const Eigen::VectorXd & kp, double kv);
  ProportionalDerivative(const Eigen::VectorXd & kp, const Eigen::MatrixXd & kv);
  ProportionalDerivative(const Eigen::MatrixXd & kp, double kv);
  ProportionalDerivative(const Eigen::MatrixXd & kp, const Eigen::VectorXd & kv);
  ProportionalDerivative(double kp);
  ProportionalDerivative(const Eigen::VectorXd & kp);
  ProportionalDerivative(const Eigen::MatrixXd & kp);
 
  ~ProportionalDerivative() override = default;
 
protected:
  std::unique_ptr<abstract::TaskDynamicsImpl> impl_(FunctionPtr f,
                                                    constraint::Type t,
                                                    const Eigen::VectorXd & rhs) const override;
  Order order_() const override;
 
  TASK_DYNAMICS_DERIVED_FACTORY(kp_, kv_)
 
private:
  Gain kp_; // Stiffness gain
  Gain kv_; // Damping gain
};
 
using PD = ProportionalDerivative;
 
template<typename T, typename U>
inline void ProportionalDerivative::Impl::gains_(const T & kp, const U & kv)
{
  checkGainSize(kp);
  checkGainSize(kv);
  kp_ = kp;
  kv_ = kv;
}
} // namespace task_dynamics
 
} // namespace tvm