contact-6d.hpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2017 CNRS, NYU, MPI Tübingen
3 //
4 // This file is part of tsid
5 // tsid is free software: you can redistribute it
6 // and/or modify it under the terms of the GNU Lesser General Public
7 // License as published by the Free Software Foundation, either version
8 // 3 of the License, or (at your option) any later version.
9 // tsid is distributed in the hope that it will be
10 // useful, but WITHOUT ANY WARRANTY; without even the implied warranty
11 // of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
12 // General Lesser Public License for more details. You should have
13 // received a copy of the GNU Lesser General Public License along with
14 // tsid If not, see
15 // <http://www.gnu.org/licenses/>.
16 //
17 
18 #ifndef __invdyn_contact_6d_hpp__
19 #define __invdyn_contact_6d_hpp__
20 
21 #include "tsid/deprecated.hh"
26 
27 namespace tsid
28 {
29  namespace contacts
30  {
31  class Contact6d : public ContactBase
32  {
33  public:
34  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
35 
46 
47  Contact6d(const std::string & name,
48  RobotWrapper & robot,
49  const std::string & frameName,
50  ConstRefMatrix contactPoints,
51  ConstRefVector contactNormal,
52  const double frictionCoefficient,
53  const double minNormalForce,
54  const double maxNormalForce);
55 
56  TSID_DEPRECATED Contact6d(const std::string & name,
57  RobotWrapper & robot,
58  const std::string & frameName,
59  ConstRefMatrix contactPoints,
60  ConstRefVector contactNormal,
61  const double frictionCoefficient,
62  const double minNormalForce,
63  const double maxNormalForce,
64  const double forceRegWeight);
65 
67  virtual unsigned int n_motion() const;
68 
70  virtual unsigned int n_force() const;
71 
72  virtual const ConstraintBase & computeMotionTask(const double t,
75  Data & data);
76 
77  virtual const ConstraintInequality & computeForceTask(const double t,
80  const Data & data);
81 
82  virtual const Matrix & getForceGeneratorMatrix();
83 
84  virtual const ConstraintEquality & computeForceRegularizationTask(const double t,
87  const Data & data);
88 
89  const TaskSE3Equality & getMotionTask() const;
90  const ConstraintBase & getMotionConstraint() const;
93 
94  double getNormalForce(ConstRefVector f) const;
95  double getMinNormalForce() const;
96  double getMaxNormalForce() const;
97  const Matrix3x & getContactPoints() const;
98 
99  const Vector & Kp() const;
100  const Vector & Kd() const;
101  void Kp(ConstRefVector Kp);
102  void Kd(ConstRefVector Kp);
103 
104  bool setContactPoints(ConstRefMatrix contactPoints);
105  bool setContactNormal(ConstRefVector contactNormal);
106 
107  bool setFrictionCoefficient(const double frictionCoefficient);
108  bool setMinNormalForce(const double minNormalForce);
109  bool setMaxNormalForce(const double maxNormalForce);
110  void setReference(const SE3 & ref);
111  void setForceReference(ConstRefVector & f_ref);
113 
114  private:
115  void init();
116 
117  protected:
118 
122 
130  double m_mu;
131  double m_fMin;
132  double m_fMax;
134  };
135  }
136 }
137 
138 #endif // ifndef __invdyn_contact_6d_hpp__
tsid::contacts::Contact6d::computeMotionTask
virtual const ConstraintBase & computeMotionTask(const double t, ConstRefVector q, ConstRefVector v, Data &data)
Definition: contact-6d.cpp:232
tsid::contacts::Contact6d::getForceGeneratorMatrix
virtual const Matrix & getForceGeneratorMatrix()
Definition: contact-6d.cpp:248
tsid::contacts::Contact6d::setMinNormalForce
bool setMinNormalForce(const double minNormalForce)
Definition: contact-6d.cpp:199
tsid::contacts::ContactBase
Base template of a Contact.
Definition: contact-base.hpp:34
tsid::contacts::Contact6d::Vector3
math::Vector3 Vector3
Definition: contact-6d.hpp:40
tsid::contacts::Contact6d::updateForceRegularizationTask
void updateForceRegularizationTask()
Definition: contact-6d.cpp:135
tsid::contacts::Contact6d::setFrictionCoefficient
bool setFrictionCoefficient(const double frictionCoefficient)
Definition: contact-6d.cpp:189
tsid::contacts::Contact6d::getForceRegularizationTask
const ConstraintEquality & getForceRegularizationTask() const
Definition: contact-6d.cpp:271
tsid::math::ConstraintEquality
Definition: constraint-equality.hpp:28
tsid::contacts::Contact6d::TaskSE3Equality
tasks::TaskSE3Equality TaskSE3Equality
Definition: contact-6d.hpp:42
tsid::contacts::Contact6d::setReference
void setReference(const SE3 &ref)
Definition: contact-6d.cpp:227
tsid::contacts::Contact6d::m_contactNormal
Vector3 m_contactNormal
Definition: contact-6d.hpp:127
tsid::contacts::Contact6d::m_fMin
double m_fMin
Definition: contact-6d.hpp:131
tsid::contacts::Contact6d::n_force
virtual unsigned int n_force() const
Return the number of force variables.
Definition: contact-6d.cpp:156
tsid::contacts::ContactBase::Matrix
math::Matrix Matrix
Definition: contact-base.hpp:43
tsid::math::Vector
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > Vector
Definition: fwd.hpp:37
tsid::math::Vector6
Eigen::Matrix< Scalar, 6, 1 > Vector6
Definition: fwd.hpp:43
tsid::contacts::Contact6d::m_fRef
Vector6 m_fRef
Definition: contact-6d.hpp:128
tsid::contacts::Contact6d::m_forceGenMat
Matrix m_forceGenMat
Definition: contact-6d.hpp:133
constraint-equality.hpp
tsid::python::SE3
pinocchio::SE3 SE3
Definition: trajectory-base.hpp:34
tsid::contacts::Contact6d::m_mu
double m_mu
Definition: contact-6d.hpp:130
tsid::contacts::Contact6d::updateForceInequalityConstraints
void updateForceInequalityConstraints()
Definition: contact-6d.cpp:83
tsid::contacts::Contact6d::m_forceInequality
ConstraintInequality m_forceInequality
Definition: contact-6d.hpp:124
tsid::contacts::Contact6d::setRegularizationTaskWeightVector
void setRegularizationTaskWeightVector(ConstRefVector &w)
Definition: contact-6d.cpp:129
tsid::contacts::ContactBase::ConstraintBase
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef math::ConstraintBase ConstraintBase
Definition: contact-base.hpp:39
tsid::contacts::Contact6d::getContactPoints
const Matrix3x & getContactPoints() const
Definition: contact-6d.cpp:174
tsid::contacts::Contact6d::m_contactPoints
Matrix3x m_contactPoints
Definition: contact-6d.hpp:126
tsid::contacts::Contact6d::setContactPoints
bool setContactPoints(ConstRefMatrix contactPoints)
Definition: contact-6d.cpp:163
tsid::contacts::Contact6d::computeForceTask
virtual const ConstraintInequality & computeForceTask(const double t, ConstRefVector q, ConstRefVector v, const Data &data)
Definition: contact-6d.cpp:240
tsid::contacts::Contact6d::Contact6d
Contact6d(const std::string &name, RobotWrapper &robot, const std::string &frameName, ConstRefMatrix contactPoints, ConstRefVector contactNormal, const double frictionCoefficient, const double minNormalForce, const double maxNormalForce)
Definition: contact-6d.cpp:29
tsid::contacts::Contact6d::getMaxNormalForce
double getMaxNormalForce() const
Definition: contact-6d.cpp:263
tsid::contacts::Contact6d
Definition: contact-6d.hpp:31
tsid::contacts::Contact6d::n_motion
virtual unsigned int n_motion() const
Return the number of motion constraints.
Definition: contact-6d.cpp:155
tsid::contacts::Contact6d::Matrix3x
math::Matrix3x Matrix3x
Definition: contact-6d.hpp:38
tsid::contacts::Contact6d::getNormalForce
double getNormalForce(ConstRefVector f) const
Definition: contact-6d.cpp:120
tsid::contacts::Contact6d::getMotionTask
const TaskSE3Equality & getMotionTask() const
Definition: contact-6d.cpp:265
tsid::contacts::Contact6d::m_forceRegTask
ConstraintEquality m_forceRegTask
Definition: contact-6d.hpp:125
tsid::contacts::Contact6d::updateForceGeneratorMatrix
void updateForceGeneratorMatrix()
Definition: contact-6d.cpp:144
tsid::contacts::Contact6d::setMaxNormalForce
bool setMaxNormalForce(const double maxNormalForce)
Definition: contact-6d.cpp:210
tsid::contacts::Contact6d::Vector
math::Vector Vector
Definition: contact-6d.hpp:41
task-se3-equality.hpp
tsid::math::ConstraintInequality
Definition: constraint-inequality.hpp:28
tsid::math::ConstRefVector
const typedef Eigen::Ref< const Vector > ConstRefVector
Definition: fwd.hpp:50
tsid::contacts::ContactBase::name
const std::string & name() const
Definition: contact-base.cpp:30
tsid::contacts::Contact6d::m_motionTask
TaskSE3Equality m_motionTask
Definition: contact-6d.hpp:123
tsid::contacts::Contact6d::ConstRefMatrix
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef math::ConstRefMatrix ConstRefMatrix
Definition: contact-6d.hpp:36
constraint-inequality.hpp
tsid::contacts::Contact6d::getMotionConstraint
const ConstraintBase & getMotionConstraint() const
Definition: contact-6d.cpp:267
tsid::contacts::Contact6d::Vector6
math::Vector6 Vector6
Definition: contact-6d.hpp:39
tsid::contacts::Contact6d::m_weightForceRegTask
Vector6 m_weightForceRegTask
Definition: contact-6d.hpp:129
tsid::contacts::Contact6d::setContactNormal
bool setContactNormal(ConstRefVector contactNormal)
Definition: contact-6d.cpp:179
tsid::contacts::Contact6d::getMinNormalForce
double getMinNormalForce() const
Definition: contact-6d.cpp:262
tsid
Definition: constraint-bound.hpp:26
tsid::contacts::Contact6d::setForceReference
void setForceReference(ConstRefVector &f_ref)
Definition: contact-6d.cpp:221
tsid::robots::RobotWrapper
Wrapper for a robot based on pinocchio.
Definition: robot-wrapper.hpp:40
tsid::contacts::Contact6d::Kd
const Vector & Kd() const
Definition: contact-6d.cpp:159
tsid::contacts::Contact6d::getForceConstraint
const ConstraintInequality & getForceConstraint() const
Definition: contact-6d.cpp:269
tsid::contacts::ContactBase::Data
pinocchio::Data Data
Definition: contact-base.hpp:46
tsid::contacts::Contact6d::ConstRefVector
math::ConstRefVector ConstRefVector
Definition: contact-6d.hpp:37
tsid::contacts::Contact6d::ConstraintEquality
math::ConstraintEquality ConstraintEquality
Definition: contact-6d.hpp:44
tsid::math::Matrix3x
Eigen::Matrix< Scalar, 3, Eigen::Dynamic > Matrix3x
Definition: fwd.hpp:44
tsid::math::ConstRefMatrix
const typedef Eigen::Ref< const Matrix > ConstRefMatrix
Definition: fwd.hpp:53
tsid::contacts::Contact6d::SE3
pinocchio::SE3 SE3
Definition: contact-6d.hpp:45
tsid::contacts::Contact6d::computeForceRegularizationTask
virtual const ConstraintEquality & computeForceRegularizationTask(const double t, ConstRefVector q, ConstRefVector v, const Data &data)
Definition: contact-6d.cpp:254
tsid::tasks::TaskSE3Equality
Definition: task-se3-equality.hpp:33
tsid::math::Vector3
Eigen::Matrix< Scalar, 3, 1 > Vector3
Definition: fwd.hpp:42
tsid::contacts::Contact6d::ConstraintInequality
math::ConstraintInequality ConstraintInequality
Definition: contact-6d.hpp:43
contact-base.hpp
tsid::contacts::Contact6d::m_fMax
double m_fMax
Definition: contact-6d.hpp:132
tsid::contacts::Contact6d::Kp
const Vector & Kp() const
Definition: contact-6d.cpp:158