QPConstr.h

Go to the documentation of this file.

/*
 * Copyright 2012-2019 CNRS-UM LIRMM, CNRS-AIST JRL
 */
 
#pragma once
 
// includes
// Eigen
#include <Eigen/Core>
#include <Eigen/StdVector>
 
// RBDyn
#include <RBDyn/CoM.h>
#include <RBDyn/Jacobian.h>
 
// sch
#include <sch/Matrix/SCH_Types.h>
 
// Tasks
#include "QPSolver.h"
 
// unique_ptr
#include <memory>
 
// forward declaration
// sch
namespace sch
{
class S_Object;
class CD_Pair;
} // namespace sch
 
namespace tasks
{
struct QBound;
struct AlphaBound;
struct AlphaDBound;
struct AlphaDDBound;
 
namespace qp
{
 
TASKS_DLLAPI sch::Matrix4x4 tosch(const sva::PTransformd & t);
 
class TASKS_DLLAPI JointLimitsConstr : public ConstraintFunction<Bound>
{
public:
  JointLimitsConstr(const std::vector<rbd::MultiBody> & mbs, int robotIndex, QBound bound, double step);
 
  JointLimitsConstr(const std::vector<rbd::MultiBody> & mbs,
                    int robotIndex,
                    QBound bound,
                    const AlphaDBound & aDBound,
                    double step);
 
  JointLimitsConstr(const std::vector<rbd::MultiBody> & mbs,
                    int robotIndex,
                    QBound bound,
                    const AlphaDBound & aDBound,
                    const AlphaDDBound & aDDBound,
                    double step);
 
  // Constraint
  virtual void updateNrVars(const std::vector<rbd::MultiBody> & mbs, const SolverData & data) override;
 
  virtual void update(const std::vector<rbd::MultiBody> & mbs,
                      const std::vector<rbd::MultiBodyConfig> & mbcs,
                      const SolverData & data) override;
 
  virtual std::string nameBound() const override;
  virtual std::string descBound(const std::vector<rbd::MultiBody> & mbs, int line) override;
 
  // Bound Constraint
  virtual int beginVar() const override;
 
  virtual const Eigen::VectorXd & Lower() const override;
  virtual const Eigen::VectorXd & Upper() const override;
 
private:
  int robotIndex_, alphaDBegin_, alphaDOffset_;
  double step_;
  Eigen::VectorXd qMin_, qMax_;
  Eigen::VectorXd qVec_, alphaVec_;
  Eigen::VectorXd lower_, upper_;
  Eigen::VectorXd alphaDLower_, alphaDUpper_;
  Eigen::VectorXd alphaDDLower_, alphaDDUpper_;
  Eigen::VectorXd prevAlphaD_;
};
 
class TASKS_DLLAPI DamperJointLimitsConstr : public ConstraintFunction<Bound>
{
public:
  DamperJointLimitsConstr(const std::vector<rbd::MultiBody> & mbs,
                          int robotIndex,
                          const QBound & qBound,
                          const AlphaBound & aBound,
                          double interPercent,
                          double securityPercent,
                          double damperOffset,
                          double step);
  DamperJointLimitsConstr(const std::vector<rbd::MultiBody> & mbs,
                          int robotIndex,
                          const QBound & qBound,
                          const AlphaBound & aBound,
                          const AlphaDBound & aDBound,
                          double interPercent,
                          double securityPercent,
                          double damperOffset,
                          double step);
 
  DamperJointLimitsConstr(const std::vector<rbd::MultiBody> & mbs,
                          int robotIndex,
                          const QBound & qBound,
                          const AlphaBound & aBound,
                          const AlphaDBound & aDBound,
                          const AlphaDDBound & aDDBound,
                          double interPercent,
                          double securityPercent,
                          double damperOffset,
                          double step);
 
  // Constraint
  virtual void updateNrVars(const std::vector<rbd::MultiBody> & mbs, const SolverData & data) override;
 
  virtual void update(const std::vector<rbd::MultiBody> & mbs,
                      const std::vector<rbd::MultiBodyConfig> & mbcs,
                      const SolverData & data) override;
 
  virtual std::string nameBound() const override;
  virtual std::string descBound(const std::vector<rbd::MultiBody> & mbs, int line) override;
 
  // Bound Constraint
  virtual int beginVar() const override;
 
  virtual const Eigen::VectorXd & Lower() const override;
  virtual const Eigen::VectorXd & Upper() const override;
 
  double computeDamping(double alpha, double dist, double iDist, double sDist);
  double computeDamper(double dist, double iDist, double sDist, double damping);
 
private:
  struct DampData
  {
    enum State
    {
      Low,
      Upp,
      Free
    };
 
    DampData(double mi, double ma, double miV, double maV, double idi, double sdi, int aDB, int i)
    : min(mi), max(ma), minVel(miV), maxVel(maV), iDist(idi), sDist(sdi), jointIndex(i), alphaDBegin(aDB), damping(0.),
      state(Free)
    {
    }
 
    double min, max;
    double minVel, maxVel;
    double iDist, sDist;
    int jointIndex;
    int alphaDBegin;
    double damping;
    State state;
  };
 
private:
  int robotIndex_, alphaDBegin_;
  std::vector<DampData> data_;
 
  Eigen::VectorXd lower_, upper_;
  Eigen::VectorXd alphaDLower_, alphaDUpper_;
  Eigen::VectorXd alphaDDLower_, alphaDDUpper_;
  Eigen::VectorXd prevAlphaD_;
  double step_;
  double damperOff_;
};
 
class TASKS_DLLAPI CollisionConstr : public ConstraintFunction<Inequality>
{
public:
  CollisionConstr(const std::vector<rbd::MultiBody> & mbs, double step);
 
  void addCollision(const std::vector<rbd::MultiBody> & mbs,
                    int collId,
                    int r1Index,
                    const std::string & r1BodyName,
                    sch::S_Object * body1,
                    const sva::PTransformd & X_op1_o1,
                    int r2Index,
                    const std::string & r2BodyName,
                    sch::S_Object * body2,
                    const sva::PTransformd & X_op2_o2,
                    double di,
                    double ds,
                    double damping,
                    double dampingOff = 0.,
                    const Eigen::VectorXd & r1Selector = Eigen::VectorXd::Zero(0),
                    const Eigen::VectorXd & r2Selector = Eigen::VectorXd::Zero(0));
 
  bool rmCollision(int collId);
 
  std::size_t nrCollisions() const;
 
  void reset();
 
  void updateNrCollisions();
 
  // Constraint
  virtual void updateNrVars(const std::vector<rbd::MultiBody> & mbs, const SolverData & data) override;
 
  virtual void update(const std::vector<rbd::MultiBody> & mbs,
                      const std::vector<rbd::MultiBodyConfig> & mbcs,
                      const SolverData & data) override;
 
  virtual std::string nameInEq() const override;
  virtual std::string descInEq(const std::vector<rbd::MultiBody> & mbs, int line) override;
 
  // In Inequality Constraint
  virtual int nrInEq() const override;
  virtual int maxInEq() const override;
 
  virtual const Eigen::MatrixXd & AInEq() const override;
  virtual const Eigen::VectorXd & bInEq() const override;
 
private:
  struct BodyCollData
  {
    BodyCollData(const rbd::MultiBody & mb,
                 int rIndex,
                 const std::string & bodyName,
                 sch::S_Object * hull,
                 const sva::PTransformd & X_op_o,
                 const Eigen::VectorXd & selector);
 
    sch::S_Object * hull;
    rbd::Jacobian jac;
    sva::PTransformd X_op_o;
    int rIndex, bIndex;
    std::string bodyName;
    Eigen::VectorXd selector;
  };
 
  struct CollData
  {
    enum class DampingType
    {
      Hard,
      Soft,
      Free
    };
    CollData(std::vector<BodyCollData> bcds,
             int collId,
             sch::S_Object * body1,
             sch::S_Object * body2,
             double di,
             double ds,
             double damping,
             double dampingOff);
    CollData(CollData &&) = default;
    CollData(const CollData &) = delete;
    CollData & operator=(const CollData &) = delete;
    CollData & operator=(CollData &&) = default;
 
    std::unique_ptr<sch::CD_Pair> pair;
    double distance;
    Eigen::Vector3d p1;
    Eigen::Vector3d p2;
    Eigen::Vector3d normVecDist;
    double di, ds;
    double damping;
    std::vector<BodyCollData> bodies;
 
    DampingType dampingType;
    double dampingOff;
    int collId;
  };
 
public:
  const CollData & getCollisionData(int collId) const;
 
private:
  double computeDamping(const std::vector<rbd::MultiBody> & mbs,
                        const std::vector<rbd::MultiBodyConfig> & mbcs,
                        const CollData & cd,
                        const Eigen::Vector3d & normalVecDist,
                        double dist) const;
 
private:
  std::vector<CollData> dataVec_;
  double step_;
  int nrActivated_, totalAlphaD_;
 
  Eigen::MatrixXd AInEq_;
  Eigen::VectorXd bInEq_;
 
  Eigen::MatrixXd fullJac_, distJac_;
 
  int nrVars_;
 
  CollisionConstr(const CollisionConstr &) = delete;
  CollisionConstr & operator=(const CollisionConstr &) = delete;
};
 
class TASKS_DLLAPI CoMIncPlaneConstr : public ConstraintFunction<Inequality>
{
public:
  CoMIncPlaneConstr(const std::vector<rbd::MultiBody> & mbs, int robotIndex, double step);
 
  void addPlane(int planeId,
                const Eigen::Vector3d & normal,
                double offset,
                double di,
                double ds,
                double damping,
                double dampingOff = 0.);
 
  void addPlane(int planeId,
                const Eigen::Vector3d & normal,
                double offset,
                double di,
                double ds,
                double damping,
                const Eigen::Vector3d & speed,
                const Eigen::Vector3d & normalDot,
                double dampingOff = 0.);
 
  bool rmPlane(int planeId);
 
  std::size_t nrPlanes() const;
 
  void reset();
 
  inline Eigen::VectorXd & selector() noexcept { return selector_; }
 
  inline const Eigen::VectorXd & selector() const noexcept { return selector_; }
 
  void updateNrPlanes();
 
  // Constraint
  virtual void updateNrVars(const std::vector<rbd::MultiBody> & mbs, const SolverData & data) override;
 
  virtual void update(const std::vector<rbd::MultiBody> & mbs,
                      const std::vector<rbd::MultiBodyConfig> & mbcs,
                      const SolverData & data) override;
 
  virtual std::string nameInEq() const override;
  virtual std::string descInEq(const std::vector<rbd::MultiBody> & mbs, int line) override;
 
  // In Inequality Constraint
  virtual int nrInEq() const override;
  virtual int maxInEq() const override;
 
  virtual const Eigen::MatrixXd & AInEq() const override;
  virtual const Eigen::VectorXd & bInEq() const override;
 
private:
  struct PlaneData
  {
    enum class DampingType
    {
      Hard,
      Soft,
      Free
    };
    PlaneData(int planeId,
              const Eigen::Vector3d & normal,
              double offset,
              double di,
              double ds,
              double damping,
              double dampingOff,
              const Eigen::Vector3d & speed,
              const Eigen::Vector3d & normalDot);
    Eigen::Vector3d normal;
    Eigen::Vector3d normalDot;
    double offset;
    double dist;
    double di, ds;
    double damping;
    int planeId;
    DampingType dampingType;
    double dampingOff;
    Eigen::Vector3d speed;
  };
 
private:
  int robotIndex_, alphaDBegin_;
  std::vector<PlaneData> dataVec_;
  double step_;
  int nrVars_;
  int nrActivated_;
  std::vector<std::size_t> activated_;
 
  rbd::CoMJacobian jacCoM_;
  Eigen::VectorXd selector_;
  Eigen::MatrixXd AInEq_;
  Eigen::VectorXd bInEq_;
};
 
class TASKS_DLLAPI GripperTorqueConstr : public ConstraintFunction<Inequality>
{
public:
  GripperTorqueConstr();
 
  void addGripper(const ContactId & cId,
                  double torqueLimit,
                  const Eigen::Vector3d & origin,
                  const Eigen::Vector3d & axis);
  bool rmGripper(const ContactId & cId);
  void reset();
 
  // Constraint
  virtual void updateNrVars(const std::vector<rbd::MultiBody> & mb, const SolverData & data) override;
 
  virtual void update(const std::vector<rbd::MultiBody> & mb,
                      const std::vector<rbd::MultiBodyConfig> & mbc,
                      const SolverData & data) override;
 
  virtual std::string nameInEq() const override;
  virtual std::string descInEq(const std::vector<rbd::MultiBody> & mb, int line) override;
 
  // In Inequality Constraint
  virtual int maxInEq() const override;
 
  virtual const Eigen::MatrixXd & AInEq() const override;
  virtual const Eigen::VectorXd & bInEq() const override;
 
private:
  struct GripperData
  {
    GripperData(const ContactId & cId, double tl, const Eigen::Vector3d & o, const Eigen::Vector3d & a);
 
    ContactId contactId;
    double torqueLimit;
    Eigen::Vector3d origin;
    Eigen::Vector3d axis;
  };
 
private:
  std::vector<GripperData> dataVec_;
 
  Eigen::MatrixXd AInEq_;
  Eigen::VectorXd bInEq_;
};
 
class TASKS_DLLAPI BoundedSpeedConstr : public ConstraintFunction<GenInequality>
{
public:
  BoundedSpeedConstr(const std::vector<rbd::MultiBody> & mbs, int robotIndex, double timeStep);
 
  void addBoundedSpeed(const std::vector<rbd::MultiBody> & mbs,
                       const std::string & bodyName,
                       const Eigen::Vector3d & bodyPoint,
                       const Eigen::MatrixXd & dof,
                       const Eigen::VectorXd & speed);
 
  void addBoundedSpeed(const std::vector<rbd::MultiBody> & mbs,
                       const std::string & bodyName,
                       const Eigen::Vector3d & bodyPoint,
                       const Eigen::MatrixXd & dof,
                       const Eigen::VectorXd & lowerSpeed,
                       const Eigen::VectorXd & upperSpeed);
 
  bool removeBoundedSpeed(const std::string & bodyName);
 
  void resetBoundedSpeeds();
 
  std::size_t nrBoundedSpeeds() const;
 
  void updateBoundedSpeeds();
 
  // Constraint
  virtual void updateNrVars(const std::vector<rbd::MultiBody> & mbs, const SolverData & data) override;
 
  virtual void update(const std::vector<rbd::MultiBody> & mbs,
                      const std::vector<rbd::MultiBodyConfig> & mbc,
                      const SolverData & data) override;
 
  virtual std::string nameGenInEq() const override;
  virtual std::string descGenInEq(const std::vector<rbd::MultiBody> & mb, int line) override;
 
  // Inequality Constraint
  virtual int maxGenInEq() const override;
 
  virtual const Eigen::MatrixXd & AGenInEq() const override;
  virtual const Eigen::VectorXd & LowerGenInEq() const override;
  virtual const Eigen::VectorXd & UpperGenInEq() const override;
 
private:
  struct BoundedSpeedData
  {
    BoundedSpeedData(rbd::Jacobian j,
                     const Eigen::MatrixXd & d,
                     const Eigen::VectorXd & ls,
                     const Eigen::VectorXd & us,
                     const std::string & bName)
    : jac(j), bodyPoint(j.point()), dof(d), lSpeed(ls), uSpeed(us), body(j.jointsPath().back()), bodyName(bName)
    {
    }
 
    rbd::Jacobian jac;
    sva::PTransformd bodyPoint;
    Eigen::MatrixXd dof;
    Eigen::VectorXd lSpeed, uSpeed;
    int body;
    std::string bodyName;
  };
 
private:
  void updateNrEq();
 
private:
  int robotIndex_, alphaDBegin_;
  std::vector<BoundedSpeedData> cont_;
 
  Eigen::MatrixXd fullJac_;
 
  Eigen::MatrixXd A_;
  Eigen::VectorXd lower_, upper_;
 
  int nrVars_;
  double timeStep_;
};
 
class TASKS_DLLAPI ImageConstr : public ConstraintFunction<Inequality>
{
public:
  ImageConstr(const std::vector<rbd::MultiBody> & mbs,
              int robotIndex,
              const std::string & bName,
              const sva::PTransformd & X_b_gaze,
              double step,
              double constrDirection = 1.);
 
  ImageConstr(const ImageConstr & rhs);
 
  ImageConstr(ImageConstr &&) = default;
 
  ImageConstr & operator=(const ImageConstr & rhs);
 
  ImageConstr & operator=(ImageConstr &&) = default;
 
  void setLimits(const Eigen::Vector2d & min,
                 const Eigen::Vector2d & max,
                 const double iPercent,
                 const double sPercent,
                 const double damping,
                 const double dampingOffsetPercent);
 
  int addPoint(const Eigen::Vector2d & point2d, const double depthEstimate);
  int addPoint(const Eigen::Vector3d & point3d);
 
  void addPoint(const std::vector<rbd::MultiBody> & mbs,
                const std::string & bName,
                const sva::PTransformd & X_b_p = sva::PTransformd::Identity());
  void reset();
  void updatePoint(const int pointId, const Eigen::Vector2d & point2d);
  void updatePoint(const int pointId, const Eigen::Vector2d & point2d, const double depthEstimate);
  void updatePoint(const int pointId, const Eigen::Vector3d & point3d);
 
  void computeComponents(const rbd::MultiBody & mb,
                         const rbd::MultiBodyConfig & mbc,
                         const SolverData & data,
                         const Eigen::Vector2d & point2d,
                         const double depth,
                         rbd::Jacobian & jac,
                         const int bodyIndex,
                         const sva::PTransformd & X_b_p,
                         Eigen::MatrixXd & fullJacobian,
                         Eigen::Vector2d & bCommonTerm);
 
  // Constraint
  virtual void updateNrVars(const std::vector<rbd::MultiBody> & mbs, const SolverData & data) override;
 
  virtual void update(const std::vector<rbd::MultiBody> & mbs,
                      const std::vector<rbd::MultiBodyConfig> & mbcs,
                      const SolverData & data) override;
 
  // In Inequality Constraint
  virtual std::string nameInEq() const override;
  virtual std::string descInEq(const std::vector<rbd::MultiBody> & mbs, int line) override;
  virtual int nrInEq() const override;
  virtual int maxInEq() const override;
 
  virtual const Eigen::MatrixXd & AInEq() const override;
  virtual const Eigen::VectorXd & bInEq() const override;
 
private:
  struct PointData
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    PointData(const Eigen::Vector2d & pt, const double d);
    Eigen::Vector2d point2d;
    double depthEstimate;
  };
  struct RobotPointData
  {
    RobotPointData(const std::string & bName, const sva::PTransformd & X, const rbd::Jacobian & j);
    std::string bName;
    sva::PTransformd X_b_p;
    rbd::Jacobian jac;
  };
 
private:
  std::vector<PointData, Eigen::aligned_allocator<PointData>> dataVec_;
  std::vector<RobotPointData> dataVecRob_;
  int robotIndex_, bodyIndex_, alphaDBegin_;
  int nrVars_;
  double step_, accelFactor_;
  int nrActivated_;
 
  rbd::Jacobian jac_;
  sva::PTransformd X_b_gaze_;
  std::unique_ptr<Eigen::Matrix<double, 2, 6>> L_img_;
  std::unique_ptr<Eigen::Matrix<double, 6, 1>> surfaceVelocity_;
  std::unique_ptr<Eigen::Matrix<double, 1, 6>> L_Z_dot_;
  std::unique_ptr<Eigen::Matrix<double, 2, 6>> L_img_dot_;
  std::unique_ptr<Eigen::Vector2d> speed_;
  std::unique_ptr<Eigen::Vector2d> normalAcc_;
  Eigen::MatrixXd jacMat_;
  std::unique_ptr<Eigen::Vector2d> iDistMin_, iDistMax_, sDistMin_, sDistMax_;
  double damping_, dampingOffset_, ineqInversion_, constrDirection_;
  Eigen::MatrixXd AInEq_;
  Eigen::VectorXd bInEq_;
};
 
} // namespace qp
 
} // namespace tasks