QPContacts.h

Go to the documentation of this file.

/*
 * Copyright 2012-2019 CNRS-UM LIRMM, CNRS-AIST JRL
 */
 
#pragma once
 
// include
// std
#include <vector>
 
// Eigen
#include <Eigen/Core>
 
// SpaceVecAlg
#include <tasks/config.hh>
 
#include <SpaceVecAlg/SpaceVecAlg>
 
namespace tasks
{
 
namespace qp
{
 
struct TASKS_DLLAPI FrictionCone
{
  FrictionCone() {}
 
  FrictionCone(const Eigen::Matrix3d & frame, int nrGen, double mu, double direction = 1.);
 
  std::vector<Eigen::Vector3d> generators;
};
 
struct TASKS_DLLAPI ContactId
{
  ContactId();
  ContactId(int r1Index,
            int r2Index,
            const std::string & r1BodyName,
            const std::string & r2BodyName,
            int ambiguityId = -1);
 
  bool operator==(const ContactId & cId) const;
  bool operator!=(const ContactId & cId) const;
 
  bool operator<(const ContactId & cId) const;
 
  int r1Index, r2Index;
  std::string r1BodyName, r2BodyName;
  int ambiguityId;
};
 
struct TASKS_DLLAPI UnilateralContact
{
  UnilateralContact() {}
 
  UnilateralContact(int r1Index,
                    int r2Index,
                    const std::string & r1BodyName,
                    const std::string & r2BodyName,
                    std::vector<Eigen::Vector3d> r1Points,
                    const Eigen::Matrix3d & r1Frame,
                    const sva::PTransformd & X_b1_b2,
                    int nrGen,
                    double mu,
                    const sva::PTransformd & X_b1_cf = sva::PTransformd::Identity());
 
  UnilateralContact(int r1Index,
                    int r2Index,
                    const std::string & r1BodyName,
                    const std::string & r2BodyName,
                    int ambId,
                    std::vector<Eigen::Vector3d> r1Points,
                    const Eigen::Matrix3d & r1Frame,
                    const sva::PTransformd & X_b1_b2,
                    int nrGen,
                    double mu,
                    const sva::PTransformd & X_b1_cf = sva::PTransformd::Identity());
 
  UnilateralContact(const ContactId & cId,
                    std::vector<Eigen::Vector3d> r1Points,
                    const Eigen::Matrix3d & r1Frame,
                    const sva::PTransformd & X_b1_b2,
                    int nrGen,
                    double mu,
                    const sva::PTransformd & X_b1_cf = sva::PTransformd::Identity());
 
  Eigen::Vector3d force(const Eigen::VectorXd & lambda, int p, const FrictionCone & c) const;
  Eigen::Vector3d force(const Eigen::VectorXd & lambda, const FrictionCone & c) const;
 
  sva::ForceVecd force(const Eigen::VectorXd & lambda,
                       const std::vector<Eigen::Vector3d> & r_b_pi,
                       const FrictionCone & c_pi_b) const;
 
  int nrLambda(int point) const;
  int nrLambda() const;
 
  Eigen::Vector3d sForce(const Eigen::VectorXd & lambda, int point, const FrictionCone & c) const;
  Eigen::Vector3d sForce(const Eigen::VectorXd & lambda, const FrictionCone & c) const;
  sva::ForceVecd sForce(const Eigen::VectorXd & lambda,
                        const std::vector<Eigen::Vector3d> & r_b_pi,
                        const FrictionCone & c_pi_b) const;
 
  int sNrLambda(int point) const;
 
  ContactId contactId;
  std::vector<Eigen::Vector3d> r1Points, r2Points;
  FrictionCone r1Cone, r2Cone;
  sva::PTransformd X_b1_b2;
  sva::PTransformd X_b1_cf;
 
private:
  void construct(const Eigen::MatrixXd & r1Frame, int nrGen, double mu);
};
 
struct TASKS_DLLAPI BilateralContact
{
  BilateralContact() {}
 
  BilateralContact(int r1Index,
                   int r2Index,
                   const std::string & r1BodyName,
                   const std::string & r2BodyName,
                   std::vector<Eigen::Vector3d> r1Points,
                   const std::vector<Eigen::Matrix3d> & r1Frames,
                   const sva::PTransformd & X_b1_b2,
                   int nrGen,
                   double mu,
                   const sva::PTransformd & X_b1_cf = sva::PTransformd::Identity());
 
  BilateralContact(int r1Index,
                   int r2Index,
                   const std::string & r1BodyName,
                   const std::string & r2BodyName,
                   int ambId,
                   std::vector<Eigen::Vector3d> r1Points,
                   const std::vector<Eigen::Matrix3d> & r1Frames,
                   const sva::PTransformd & X_b1_b2,
                   int nrGen,
                   double mu,
                   const sva::PTransformd & X_b1_cf = sva::PTransformd::Identity());
 
  BilateralContact(const ContactId & cId,
                   std::vector<Eigen::Vector3d> r1Points,
                   const std::vector<Eigen::Matrix3d> & r1Frames,
                   const sva::PTransformd & X_b1_b2,
                   int nrGen,
                   double mu,
                   const sva::PTransformd & X_b1_cf = sva::PTransformd::Identity());
 
  BilateralContact(const UnilateralContact & c);
 
  Eigen::Vector3d force(const Eigen::VectorXd & lambda, int point, const std::vector<FrictionCone> & c) const;
  Eigen::Vector3d force(const Eigen::VectorXd & lambda, const std::vector<FrictionCone> & c) const;
 
  sva::ForceVecd force(const Eigen::VectorXd & lambda,
                       const std::vector<Eigen::Vector3d> & r_b_pi,
                       const std::vector<FrictionCone> & c_pi_b) const;
 
  int nrLambda(int point) const;
  int nrLambda() const;
 
  Eigen::Vector3d sForce(const Eigen::VectorXd & lambda, int point, const std::vector<FrictionCone> & c) const;
  Eigen::Vector3d sForce(const Eigen::VectorXd & lambda, const std::vector<FrictionCone> & c) const;
  sva::ForceVecd sForce(const Eigen::VectorXd & lambda,
                        const std::vector<Eigen::Vector3d> & r_b_pi,
                        const std::vector<FrictionCone> & c_pi_b) const;
 
  int sNrLambda(int point) const;
 
  ContactId contactId;
  std::vector<Eigen::Vector3d> r1Points, r2Points;
  std::vector<FrictionCone> r1Cones, r2Cones;
  sva::PTransformd X_b1_b2;
  sva::PTransformd X_b1_cf;
 
private:
  void construct(const std::vector<Eigen::Matrix3d> & r1Frames, int nrGen, double mu);
};
 
} // namespace qp
 
} // namespace tasks