QPContactConstr.h

Go to the documentation of this file.

/*
 * Copyright 2012-2019 CNRS-UM LIRMM, CNRS-AIST JRL
 */
 
#pragma once
 
// includes
// std
#include <map>
#include <set>
 
// Eigen
#include <Eigen/Core>
 
// RBDyn
#include <RBDyn/Jacobian.h>
 
// Tasks
#include "QPSolver.h"
 
namespace tasks
{
 
namespace qp
{
 
class TASKS_DLLAPI ContactConstrCommon
{
public:
  bool addVirtualContact(const ContactId & contactId);
 
  bool removeVirtualContact(const ContactId & contactId);
 
  void resetVirtualContacts();
 
  bool addDofContact(const ContactId & contactId, const Eigen::MatrixXd & dof);
 
  bool removeDofContact(const ContactId & contactId);
 
  bool hasDoFContact(const ContactId & id) const;
 
  const Eigen::MatrixXd & dofContact(const ContactId & contactId);
 
  void resetDofContacts();
 
protected:
  struct ContactCommon
  {
    ContactId cId;
    sva::PTransformd X_b1_cf;
    sva::PTransformd X_b1_b2;
 
    bool operator==(const ContactCommon & cc) const;
    bool operator<(const ContactCommon & cc) const;
  };
 
protected:
  std::set<ContactCommon> contactCommonInContact(const std::vector<rbd::MultiBody> & mbs, const SolverData & data);
 
protected:
  std::set<ContactId> virtualContacts_;
  std::map<ContactId, Eigen::MatrixXd> dofContacts_;
};
 
class TASKS_DLLAPI ContactConstr : public ConstraintFunction<Equality>, public ContactConstrCommon
{
public:
  ContactConstr();
 
  void updateDofContacts();
 
  // Constraint
  virtual void updateNrVars(const std::vector<rbd::MultiBody> & mbs, const SolverData & data) override;
 
  virtual std::string descEq(const std::vector<rbd::MultiBody> & mbs, int line) override;
 
  // Inequality Constraint
  virtual int nrEq() const override;
  virtual int maxEq() const override;
 
  virtual const Eigen::MatrixXd & AEq() const override;
  virtual const Eigen::VectorXd & bEq() const override;
 
protected:
  struct ContactSideData
  {
    ContactSideData(int rI, int aDB, double s, const rbd::Jacobian & j, const sva::PTransformd & Xbp)
    : robotIndex(rI), alphaDBegin(aDB), bodyIndex(j.jointsPath().back()), sign(s), jac(j), X_b_p(Xbp)
    {
    }
 
    int robotIndex, alphaDBegin, bodyIndex;
    double sign;
    rbd::Jacobian jac;
    sva::PTransformd X_b_p;
  };
 
  struct ContactData
  {
    ContactData(std::vector<ContactSideData> csds,
                const Eigen::MatrixXd & d,
                int r1,
                int r2,
                int b1,
                int b2,
                const sva::PTransformd & X_bb,
                const sva::PTransformd & X_bcf,
                const ContactId & cId)
    : contacts(std::move(csds)), dof(d), r1Index(r1), r2Index(r2), b1Index(b1), b2Index(b2), X_b1_b2(X_bb),
      X_b1_cf(X_bcf), contactId(cId)
    {
      revDof = Eigen::Matrix6d::Zero();
      // Find which dofs are selected and reverse that selection
      for(int j = 0; j < 6; ++j)
      {
        bool dof_j_active = false;
        for(int r = 0; r < dof.rows(); ++r) { dof_j_active = dof_j_active || dof(r, j) == 1; }
        if(!dof_j_active) { revDof(j, j) = 1; }
      }
    }
 
    void update(const std::vector<rbd::MultiBodyConfig> & mbcs);
 
    std::vector<ContactSideData> contacts;
    Eigen::MatrixXd dof;
    Eigen::MatrixXd revDof;
    int r1Index, r2Index;
    int b1Index, b2Index;
    sva::PTransformd X_b1_b2;
    sva::PTransformd X_b1_cf;
    ContactId contactId;
  };
 
protected:
  void updateNrEq();
 
protected:
  std::vector<ContactData> cont_;
 
  Eigen::MatrixXd fullJac_, dofJac_;
 
  Eigen::MatrixXd A_;
  Eigen::VectorXd b_;
 
  int nrEq_, totalAlphaD_;
  double timeStep_;
};
 
class TASKS_DLLAPI ContactAccConstr : public ContactConstr
{
public:
  ContactAccConstr();
 
  virtual void update(const std::vector<rbd::MultiBody> & mbs,
                      const std::vector<rbd::MultiBodyConfig> & mbcs,
                      const SolverData & data) override;
 
  virtual std::string nameEq() const override;
};
 
class TASKS_DLLAPI ContactSpeedConstr : public ContactConstr
{
public:
  ContactSpeedConstr(double timeStep);
 
  virtual void update(const std::vector<rbd::MultiBody> & mbs,
                      const std::vector<rbd::MultiBodyConfig> & mbcs,
                      const SolverData & data) override;
 
  virtual std::string nameEq() const override;
 
private:
  double timeStep_;
};
 
class TASKS_DLLAPI ContactPosConstr : public ContactConstr
{
public:
  ContactPosConstr(double timeStep);
 
  virtual void update(const std::vector<rbd::MultiBody> & mbs,
                      const std::vector<rbd::MultiBodyConfig> & mbcs,
                      const SolverData & data) override;
 
  virtual std::string nameEq() const override;
 
private:
  double timeStep_;
};
 
} // namespace qp
 
} // namespace tasks