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,
61  ConstRefVector q,
62  ConstRefVector v,
63  Data & data);
64 
65  virtual const ConstraintInequality & computeForceTask(const double t,
66  ConstRefVector q,
67  ConstRefVector v,
68  const Data & data);
69 
70  virtual const Matrix & getForceGeneratorMatrix();
71 
72  virtual const ConstraintEquality & computeForceRegularizationTask(const double t,
73  ConstRefVector q,
74  ConstRefVector v,
75  const Data & data);
76 
77  const TaskSE3Equality & getMotionTask() const;
78  const ConstraintBase & getMotionConstraint() const;
79  const ConstraintInequality & getForceConstraint() const;
80  const ConstraintEquality & getForceRegularizationTask() 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);
101  void setRegularizationTaskWeightVector(ConstRefVector & w);
102 
112  void useLocalFrame(bool local_frame);
113 
114  protected:
115 
119 
120  TaskSE3Equality m_motionTask;
121  ConstraintInequality m_forceInequality;
122  ConstraintEquality m_forceRegTask;
124  Vector3 m_fRef;
126  Matrix3x m_contactPoints;
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__
Definition: constraint-equality.hpp:28
const Eigen::Ref< const Matrix > ConstRefMatrix
Definition: fwd.hpp:53
void updateForceGeneratorMatrix()
Definition: contact-point.cpp:119
const Vector & Kp()
Definition: contact-point.cpp:127
const Matrix3x & getContactPoints() const
Definition: contact-point.cpp:99
Matrix m_forceGenMat
Definition: contact-point.hpp:133
Vector3 m_weightForceRegTask
Definition: contact-point.hpp:125
ConstraintInequality m_forceInequality
Definition: contact-point.hpp:121
const TaskSE3Equality & getMotionTask() const
Definition: contact-point.cpp:241
virtual const ConstraintInequality & computeForceTask(const double t, ConstRefVector q, ConstRefVector v, const Data &data)
Definition: contact-point.cpp:216
pinocchio::SE3 SE3
Definition: trajectory-base.hpp:34
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
Vector m_Kp3
Definition: contact-point.hpp:127
Eigen::Matrix< Scalar, 3, Eigen::Dynamic > Matrix3x
Definition: fwd.hpp:44
virtual const ConstraintEquality & computeForceRegularizationTask(const double t, ConstRefVector q, ConstRefVector v, const Data &data)
Definition: contact-point.cpp:230
virtual const Matrix & getForceGeneratorMatrix()
Definition: contact-point.cpp:224
void setReference(const SE3 &ref)
Definition: contact-point.cpp:203
Definition: task-se3-equality.hpp:33
Definition: contact-point.hpp:30
Base template of a Contact.
Definition: contact-base.hpp:34
bool setFrictionCoefficient(const double frictionCoefficient)
Definition: contact-point.cpp:165
virtual unsigned int n_force() const
Return the number of force variables.
Definition: contact-point.cpp:125
double getMinNormalForce() const
Definition: contact-point.cpp:238
Vector m_Kd3
Definition: contact-point.hpp:127
bool setContactNormal(ConstRefVector contactNormal)
Definition: contact-point.cpp:155
double getMaxNormalForce() const
Definition: contact-point.cpp:239
Eigen::Matrix< Scalar, 3, 1 > Vector3
Definition: fwd.hpp:42
Vector3 m_contactNormal
Definition: contact-point.hpp:123
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef math::ConstRefMatrix ConstRefMatrix
Definition: contact-point.hpp:35
math::Vector Vector
Definition: contact-point.hpp:40
TaskSE3Equality m_motionTask
Definition: contact-point.hpp:120
const ConstraintInequality & getForceConstraint() const
Definition: contact-point.cpp:245
bool setMotionTaskWeight(const double w)
math::Matrix Matrix
Definition: contact-base.hpp:43
const Eigen::Ref< const Vector > ConstRefVector
Definition: fwd.hpp:50
double m_motionTaskWeight
Definition: contact-point.hpp:132
math::ConstraintEquality ConstraintEquality
Definition: contact-point.hpp:43
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > Vector
Definition: fwd.hpp:37
const std::string & name() const
Definition: contact-base.cpp:30
double m_mu
Definition: contact-point.hpp:128
Wrapper for a robot based on pinocchio.
Definition: robot-wrapper.hpp:40
Definition: constraint-inequality.hpp:28
math::ConstraintInequality ConstraintInequality
Definition: contact-point.hpp:42
const ConstraintBase & getMotionConstraint() const
Definition: contact-point.cpp:243
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef math::ConstraintBase ConstraintBase
Definition: contact-base.hpp:39
void setForceReference(ConstRefVector &f_ref)
Definition: contact-point.cpp:197
ConstraintEquality m_forceRegTask
Definition: contact-point.hpp:122
double m_fMin
Definition: contact-point.hpp:129
pinocchio::Data Data
Definition: contact-base.hpp:46
virtual const ConstraintBase & computeMotionTask(const double t, ConstRefVector q, ConstRefVector v, Data &data)
Definition: contact-point.cpp:208
Eigen::Matrix< Scalar, 6, 1 > Vector6
Definition: fwd.hpp:43
math::ConstRefVector ConstRefVector
Definition: contact-point.hpp:36
void updateForceInequalityConstraints()
Definition: contact-point.cpp:64
bool setMinNormalForce(const double minNormalForce)
Definition: contact-point.cpp:175
virtual unsigned int n_motion() const
Return the number of motion constraints.
Definition: contact-point.cpp:124
double m_fMax
Definition: contact-point.hpp:130
void updateForceRegularizationTask()
Definition: contact-point.cpp:110
pinocchio::SE3 SE3
Definition: contact-point.hpp:44
double m_regularizationTaskWeight
Definition: contact-point.hpp:131
Definition: constraint-bound.hpp:26
Matrix3x m_contactPoints
Definition: contact-point.hpp:126
Vector3 m_fRef
Definition: contact-point.hpp:124
tasks::TaskSE3Equality TaskSE3Equality
Definition: contact-point.hpp:41
const Vector & Kd()
Definition: contact-point.cpp:133
math::Matrix3x Matrix3x
Definition: contact-point.hpp:37
const ConstraintEquality & getForceRegularizationTask() const
Definition: contact-point.cpp:247
void setRegularizationTaskWeightVector(ConstRefVector &w)
Definition: contact-point.cpp:104
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
math::Vector3 Vector3
Definition: contact-point.hpp:39
bool setMaxNormalForce(const double maxNormalForce)
Definition: contact-point.cpp:186
double getNormalForce(ConstRefVector f) const
Definition: contact-point.cpp:93
math::Vector6 Vector6
Definition: contact-point.hpp:38
double getMotionTaskWeight() const