contact6d.hpp

Go to the documentation of this file.

#ifndef AIG_CONTACT_6D
#define AIG_CONTACT_6D

// clang-format off
#include <Eigen/Dense>
#include "pinocchio/spatial/se3.hpp"
#include <memory>
// clang-format on

namespace aig {

struct Contact6DSettings {
public:
 double mu, gu;
 double half_length, half_width;
 Eigen::Matrix<double, 6, 1> weights;
 std::string frame_name;

 std::string to_string() {
 std::ostringstream out;
 out << "Contact6D "
 << ":\n";
 out << " mu: " << this->mu << "\n";
 out << " gu: " << this->gu << "\n";
 out << " weights: " << this->weights.transpose() << "\n";
 out << " Surface half_length: " << this->half_length << "\n";
 out << " Surface half_width: " << this->half_width << std::endl;

 return out.str();
 }

 std::ostream &operator<<(std::ostream &out) {
 out << this->to_string();
 return out;
 }

 bool operator!=(const Contact6DSettings &rhs) { return !(*this == rhs); }

 bool operator==(const Contact6DSettings &rhs) {
 bool test = true;
 test &= this->frame_name == rhs.frame_name;
 test &= this->mu == rhs.mu;
 test &= this->gu == rhs.gu;
 test &= this->half_length == rhs.half_length;
 test &= this->half_width == rhs.half_width;
 test &= this->weights == rhs.weights;
 return test;
 }
};

class Contact6D {
private:
 Contact6DSettings settings_;
 pinocchio::SE3 oMs_, cMo_;

 // matrices
 Eigen::Matrix<double, 5, 6> unilaterality_A_;
 Eigen::Matrix<double, 5, 1> unilaterality_b_;
 Eigen::Matrix<double, 6, 6> friction_A_;
 Eigen::Matrix<double, 6, 1> friction_b_;
 Eigen::Matrix<double, 6, 1> regularization_A_;
 Eigen::Matrix<double, 6, 1> regularization_b_;
 Eigen::Matrix<double, 6, 6> newton_euler_A_;
 Eigen::Matrix<double, 6, 1> contactForce_;
 size_t frameID_;

public:
 Contact6D();
 Contact6D(const Contact6DSettings &settings);
 void initialize(const Contact6DSettings &settings);

 // ~Contact6D();

 // setters
 void setMu(const double &mu);
 void setGu(const double &gu);
 void setForceWeights(const Eigen::Vector3d &force_weights);
 void setTorqueWeights(const Eigen::Vector3d &torque_weights);
 void setSurfaceHalfWidth(const double &half_width);
 void setSurfaceHalfLength(const double &half_length);
 void updateNewtonEuler(const Eigen::Vector3d &CoM, const pinocchio::SE3 &oMf);
 void setFrameID(const size_t frameID) { frameID_ = frameID; }
 void applyForce(const Eigen::Matrix<double, 6, 1> &force) {
 contactForce_ << force;
 }
 void setPose(pinocchio::SE3 &pose) { oMs_ = pose; }
 void deactivate() { contactForce_.setZero(); }

 // getters
 const Contact6DSettings &getSettings() { return settings_; }
 const Eigen::Matrix<double, 6, 6> toWorldForces() {
 return oMs_.toActionMatrixInverse().transpose();
 }
 const Eigen::Matrix<double, 6, 6> toCoMForces() {
 return cMo_.act(oMs_).toActionMatrixInverse().transpose();
 }
 size_t uni_rows() const { return unilaterality_A_.rows(); }
 size_t fri_rows() const { return friction_A_.rows(); }
 size_t cols() const { return newton_euler_A_.cols(); }
 size_t getFrameID() const { return frameID_; }
 const pinocchio::SE3 &getPose() const { return oMs_; }

 const Eigen::Matrix<double, 5, 6> &uni_A() { return unilaterality_A_; }
 const Eigen::Matrix<double, 5, 1> &uni_b() { return unilaterality_b_; }
 const Eigen::Matrix<double, 6, 6> &fri_A() { return friction_A_; }
 const Eigen::Matrix<double, 6, 1> &fri_b() { return friction_b_; }
 const Eigen::Matrix<double, 6, 1> &reg_A() { return regularization_A_; }
 const Eigen::Matrix<double, 6, 1> &reg_b() { return regularization_b_; }
 const Eigen::Matrix<double, 6, 6> &NE_A() { return newton_euler_A_; }

 const Eigen::Matrix<double, 6, 1> &appliedForce() { return contactForce_; }
};


struct ContactPointSettings {
public:
 double mu;
 Eigen::Matrix<double, 3, 1> weights;
 std::string frame_name;

 std::string to_string() {
 std::ostringstream out;
 out << "Contact6D "
 << ":\n";
 out << " mu: " << this->mu << "\n";
 out << " weights: " << this->weights.transpose() << std::endl;

 return out.str();
 }

 std::ostream &operator<<(std::ostream &out) {
 out << this->to_string();
 return out;
 }

 bool operator==(const ContactPointSettings &rhs) {
 bool test = true;
 test &= this->frame_name == rhs.frame_name;
 test &= this->mu == rhs.mu;
 test &= this->weights == rhs.weights;
 return test;
 }

 bool operator!=(const ContactPointSettings &rhs) { return !(*this == rhs); }
};

class ContactPoint {
private:
 ContactPointSettings settings_;
 pinocchio::SE3 oMs_, cMo_;

 // matrices
 Eigen::Matrix<double, 1, 3> unilaterality_A_;
 Eigen::Matrix<double, 1, 1> unilaterality_b_;
 Eigen::Matrix<double, 4, 3> friction_A_;
 Eigen::Matrix<double, 4, 1> friction_b_;
 Eigen::Matrix<double, 3, 1> regularization_A_;
 Eigen::Matrix<double, 3, 1> regularization_b_;
 Eigen::Matrix<double, 6, 3> newton_euler_A_;
 size_t frameID_;
 Eigen::Matrix<double, 3, 1> contactForce_;

public:
 ContactPoint();
 ContactPoint(const ContactPointSettings &settings);
 void initialize(const ContactPointSettings &settings);

 // ~ContactPoint();

 // setters
 void setMu(const double &mu);
 void setForceWeights(const Eigen::Vector3d &force_weights);
 void updateNewtonEuler(const Eigen::Vector3d &CoM, const pinocchio::SE3 &oMf);
 void setFrameID(const size_t frameID) { frameID_ = frameID; }
 void applyForce(const Eigen::Matrix<double, 3, 1> &force) {
 contactForce_ << force;
 }

 // getters
 const ContactPointSettings &getSettings() { return settings_; }
 const Eigen::Matrix<double, 6, 3> toWorldForces() {
 return oMs_.toActionMatrixInverse().transpose().block<6, 3>(0, 0);
 }
 const Eigen::Matrix<double, 6, 3> toCoMForces() {
 return oMs_.act(cMo_).toActionMatrixInverse().transpose().block<6, 3>(0, 0);
 }
 size_t uni_rows() const { return unilaterality_A_.rows(); }
 size_t fri_rows() const { return friction_A_.rows(); }
 size_t cols() const { return newton_euler_A_.cols(); }
 size_t getFrameID() const { return frameID_; }

 const Eigen::Matrix<double, 1, 3> &uni_A() { return unilaterality_A_; }
 const Eigen::Matrix<double, 1, 1> &uni_b() { return unilaterality_b_; }
 const Eigen::Matrix<double, 4, 3> &fri_A() { return friction_A_; }
 const Eigen::Matrix<double, 4, 1> &fri_b() { return friction_b_; }
 const Eigen::Matrix<double, 3, 1> &reg_A() { return regularization_A_; }
 const Eigen::Matrix<double, 3, 1> &reg_b() { return regularization_b_; }
 const Eigen::Matrix<double, 6, 3> &NE_A() { return newton_euler_A_; }

 const Eigen::Matrix<double, 3, 1> &appliedForce() { return contactForce_; }
};

} // namespace aig

#endif // AIG_CONTACT_6D