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