contact-point.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_point_hpp__
19 #define __invdyn_contact_point_hpp__
20 
25 
26 namespace tsid
27 {
28  namespace contacts
29  {
30  class ContactPoint : public ContactBase
31  {
32  public:
33  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
34 
45 
46  ContactPoint(const std::string & name,
47  RobotWrapper & robot,
48  const std::string & frameName,
49  ConstRefVector contactNormal,
50  const double frictionCoefficient,
51  const double minNormalForce,
52  const double maxNormalForce);
53 
55  virtual unsigned int n_motion() const;
56 
58  virtual unsigned int n_force() const;
59 
60  virtual const ConstraintBase & computeMotionTask(const double t,
63  Data & data);
64 
65  virtual const ConstraintInequality & computeForceTask(const double t,
68  const Data & data);
69 
70  virtual const Matrix & getForceGeneratorMatrix();
71 
72  virtual const ConstraintEquality & computeForceRegularizationTask(const double t,
75  const Data & data);
76 
77  const TaskSE3Equality & getMotionTask() const;
78  const ConstraintBase & getMotionConstraint() const;
81  double getMotionTaskWeight() const;
82  const Matrix3x & getContactPoints() const;
83 
84  double getNormalForce(ConstRefVector f) const;
85  double getMinNormalForce() const;
86  double getMaxNormalForce() const;
87 
88  const Vector & Kp(); // cannot be const because it set a member variable inside
89  const Vector & Kd(); // cannot be const because it set a member variable inside
90  void Kp(ConstRefVector Kp);
91  void Kd(ConstRefVector Kp);
92 
93  bool setContactNormal(ConstRefVector contactNormal);
94 
95  bool setFrictionCoefficient(const double frictionCoefficient);
96  bool setMinNormalForce(const double minNormalForce);
97  bool setMaxNormalForce(const double maxNormalForce);
98  bool setMotionTaskWeight(const double w);
99  void setReference(const SE3 & ref);
100  void setForceReference(ConstRefVector & f_ref);
102 
112  void useLocalFrame(bool local_frame);
113 
114  protected:
115 
119 
127  Vector m_Kp3, m_Kd3; // gain vectors to be returned by reference
128  double m_mu;
129  double m_fMin;
130  double m_fMax;
134  };
135  }
136 }
137 
138 #endif // ifndef __invdyn_contact_6d_hpp__
tsid::contacts::ContactPoint::getForceRegularizationTask
const ConstraintEquality & getForceRegularizationTask() const
Definition: contact-point.cpp:247
tsid::contacts::ContactPoint::m_weightForceRegTask
Vector3 m_weightForceRegTask
Definition: contact-point.hpp:125
tsid::contacts::ContactPoint::getContactPoints
const Matrix3x & getContactPoints() const
Definition: contact-point.cpp:99
tsid::contacts::ContactPoint::setMaxNormalForce
bool setMaxNormalForce(const double maxNormalForce)
Definition: contact-point.cpp:186
tsid::contacts::ContactPoint::getNormalForce
double getNormalForce(ConstRefVector f) const
Definition: contact-point.cpp:93
tsid::contacts::ContactPoint::updateForceGeneratorMatrix
void updateForceGeneratorMatrix()
Definition: contact-point.cpp:119
tsid::contacts::ContactBase
Base template of a Contact.
Definition: contact-base.hpp:34
tsid::contacts::ContactPoint::Vector6
math::Vector6 Vector6
Definition: contact-point.hpp:38
tsid::contacts::ContactPoint::getMotionTaskWeight
double getMotionTaskWeight() const
tsid::contacts::ContactPoint::computeForceTask
virtual const ConstraintInequality & computeForceTask(const double t, ConstRefVector q, ConstRefVector v, const Data &data)
Definition: contact-point.cpp:216
tsid::math::ConstraintEquality
Definition: constraint-equality.hpp:28
tsid::contacts::ContactPoint::ContactPoint
ContactPoint(const std::string &name, RobotWrapper &robot, const std::string &frameName, ConstRefVector contactNormal, const double frictionCoefficient, const double minNormalForce, const double maxNormalForce)
Definition: contact-point.cpp:29
tsid::contacts::ContactPoint::m_forceInequality
ConstraintInequality m_forceInequality
Definition: contact-point.hpp:121
tsid::contacts::ContactPoint::useLocalFrame
void useLocalFrame(bool local_frame)
Specifies if properties of the contact point and motion task are expressed in the local or local worl...
Definition: contact-point.cpp:59
tsid::contacts::ContactBase::Matrix
math::Matrix Matrix
Definition: contact-base.hpp:43
tsid::contacts::ContactPoint::getForceGeneratorMatrix
virtual const Matrix & getForceGeneratorMatrix()
Definition: contact-point.cpp:224
tsid::math::Vector
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > Vector
Definition: fwd.hpp:37
tsid::contacts::ContactPoint::setReference
void setReference(const SE3 &ref)
Definition: contact-point.cpp:203
tsid::contacts::ContactPoint::computeForceRegularizationTask
virtual const ConstraintEquality & computeForceRegularizationTask(const double t, ConstRefVector q, ConstRefVector v, const Data &data)
Definition: contact-point.cpp:230
tsid::contacts::ContactPoint::m_Kp3
Vector m_Kp3
Definition: contact-point.hpp:127
tsid::math::Vector6
Eigen::Matrix< Scalar, 6, 1 > Vector6
Definition: fwd.hpp:43
tsid::contacts::ContactPoint::getMotionTask
const TaskSE3Equality & getMotionTask() const
Definition: contact-point.cpp:241
constraint-equality.hpp
tsid::python::SE3
pinocchio::SE3 SE3
Definition: trajectory-base.hpp:34
tsid::contacts::ContactPoint::getMaxNormalForce
double getMaxNormalForce() const
Definition: contact-point.cpp:239
tsid::contacts::ContactBase::ConstraintBase
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef math::ConstraintBase ConstraintBase
Definition: contact-base.hpp:39
tsid::contacts::ContactPoint::m_motionTask
TaskSE3Equality m_motionTask
Definition: contact-point.hpp:120
tsid::contacts::ContactPoint::m_motionTaskWeight
double m_motionTaskWeight
Definition: contact-point.hpp:132
tsid::contacts::ContactPoint::m_contactNormal
Vector3 m_contactNormal
Definition: contact-point.hpp:123
tsid::contacts::ContactPoint::ConstRefMatrix
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef math::ConstRefMatrix ConstRefMatrix
Definition: contact-point.hpp:35
tsid::contacts::ContactPoint::getMinNormalForce
double getMinNormalForce() const
Definition: contact-point.cpp:238
tsid::contacts::ContactPoint::m_Kd3
Vector m_Kd3
Definition: contact-point.hpp:127
tsid::contacts::ContactPoint::ConstraintEquality
math::ConstraintEquality ConstraintEquality
Definition: contact-point.hpp:43
tsid::contacts::ContactPoint::setMotionTaskWeight
bool setMotionTaskWeight(const double w)
tsid::contacts::ContactPoint::getForceConstraint
const ConstraintInequality & getForceConstraint() const
Definition: contact-point.cpp:245
tsid::contacts::ContactPoint::computeMotionTask
virtual const ConstraintBase & computeMotionTask(const double t, ConstRefVector q, ConstRefVector v, Data &data)
Definition: contact-point.cpp:208
task-se3-equality.hpp
tsid::math::ConstraintInequality
Definition: constraint-inequality.hpp:28
tsid::contacts::ContactPoint::ConstRefVector
math::ConstRefVector ConstRefVector
Definition: contact-point.hpp:36
tsid::contacts::ContactPoint::setFrictionCoefficient
bool setFrictionCoefficient(const double frictionCoefficient)
Definition: contact-point.cpp:165
tsid::contacts::ContactPoint::n_force
virtual unsigned int n_force() const
Return the number of force variables.
Definition: contact-point.cpp:125
tsid::contacts::ContactPoint::Vector
math::Vector Vector
Definition: contact-point.hpp:40
tsid::contacts::ContactPoint::setForceReference
void setForceReference(ConstRefVector &f_ref)
Definition: contact-point.cpp:197
tsid::contacts::ContactPoint::setContactNormal
bool setContactNormal(ConstRefVector contactNormal)
Definition: contact-point.cpp:155
tsid::contacts::ContactPoint::ConstraintInequality
math::ConstraintInequality ConstraintInequality
Definition: contact-point.hpp:42
tsid::contacts::ContactPoint::m_contactPoints
Matrix3x m_contactPoints
Definition: contact-point.hpp:126
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::ContactPoint::TaskSE3Equality
tasks::TaskSE3Equality TaskSE3Equality
Definition: contact-point.hpp:41
constraint-inequality.hpp
tsid::contacts::ContactPoint::m_mu
double m_mu
Definition: contact-point.hpp:128
tsid::contacts::ContactPoint::setMinNormalForce
bool setMinNormalForce(const double minNormalForce)
Definition: contact-point.cpp:175
tsid::contacts::ContactPoint::setRegularizationTaskWeightVector
void setRegularizationTaskWeightVector(ConstRefVector &w)
Definition: contact-point.cpp:104
tsid::contacts::ContactBase::Matrix3x
math::Matrix3x Matrix3x
Definition: contact-base.hpp:44
tsid
Definition: constraint-bound.hpp:26
tsid::contacts::ContactPoint::Vector3
math::Vector3 Vector3
Definition: contact-point.hpp:39
tsid::contacts::ContactPoint::m_fMax
double m_fMax
Definition: contact-point.hpp:130
tsid::contacts::ContactPoint::updateForceRegularizationTask
void updateForceRegularizationTask()
Definition: contact-point.cpp:110
tsid::contacts::ContactPoint::m_forceRegTask
ConstraintEquality m_forceRegTask
Definition: contact-point.hpp:122
tsid::robots::RobotWrapper
Wrapper for a robot based on pinocchio.
Definition: robot-wrapper.hpp:40
tsid::contacts::ContactBase::Data
pinocchio::Data Data
Definition: contact-base.hpp:46
tsid::contacts::ContactPoint::updateForceInequalityConstraints
void updateForceInequalityConstraints()
Definition: contact-point.cpp:64
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::ContactPoint::n_motion
virtual unsigned int n_motion() const
Return the number of motion constraints.
Definition: contact-point.cpp:124
tsid::contacts::ContactPoint::m_fRef
Vector3 m_fRef
Definition: contact-point.hpp:124
tsid::contacts::ContactPoint::SE3
pinocchio::SE3 SE3
Definition: contact-point.hpp:44
tsid::contacts::ContactPoint
Definition: contact-point.hpp:30
tsid::contacts::ContactPoint::Kp
const Vector & Kp()
Definition: contact-point.cpp:127
tsid::tasks::TaskSE3Equality
Definition: task-se3-equality.hpp:33
tsid::contacts::ContactPoint::m_forceGenMat
Matrix m_forceGenMat
Definition: contact-point.hpp:133
tsid::math::Vector3
Eigen::Matrix< Scalar, 3, 1 > Vector3
Definition: fwd.hpp:42
tsid::contacts::ContactBase::ConstRefVector
math::ConstRefVector ConstRefVector
Definition: contact-base.hpp:42
contact-base.hpp
tsid::contacts::ContactPoint::Matrix3x
math::Matrix3x Matrix3x
Definition: contact-point.hpp:37
tsid::contacts::ContactPoint::getMotionConstraint
const ConstraintBase & getMotionConstraint() const
Definition: contact-point.cpp:243
tsid::contacts::ContactPoint::m_regularizationTaskWeight
double m_regularizationTaskWeight
Definition: contact-point.hpp:131
tsid::contacts::ContactPoint::m_fMin
double m_fMin
Definition: contact-point.hpp:129
tsid::contacts::ContactPoint::Kd
const Vector & Kd()
Definition: contact-point.cpp:133