Loading...
Searching...
No Matches
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
27namespace tsid
28{
29 namespace contacts
30 {
31 class Contact6d : public ContactBase
32 {
33 public:
34 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
35
45 typedef pinocchio::SE3 SE3;
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__
Definition: contact-6d.hpp:32
math::ConstraintInequality ConstraintInequality
Definition: contact-6d.hpp:43
const ConstraintEquality & getForceRegularizationTask() const
Definition: contact-6d.cpp:271
virtual const ConstraintEquality & computeForceRegularizationTask(const double t, ConstRefVector q, ConstRefVector v, const Data &data)
Definition: contact-6d.cpp:254
bool setContactNormal(ConstRefVector contactNormal)
Definition: contact-6d.cpp:179
TSID_DEPRECATED Contact6d(const std::string &name, RobotWrapper &robot, const std::string &frameName, ConstRefMatrix contactPoints, ConstRefVector contactNormal, const double frictionCoefficient, const double minNormalForce, const double maxNormalForce, const double forceRegWeight)
bool setMinNormalForce(const double minNormalForce)
Definition: contact-6d.cpp:199
void setForceReference(ConstRefVector &f_ref)
Definition: contact-6d.cpp:221
Matrix3x m_contactPoints
Definition: contact-6d.hpp:126
Vector3 m_contactNormal
Definition: contact-6d.hpp:127
Vector6 m_fRef
Definition: contact-6d.hpp:128
const Vector & Kp() const
Definition: contact-6d.cpp:158
const ConstraintBase & getMotionConstraint() const
Definition: contact-6d.cpp:267
void updateForceRegularizationTask()
Definition: contact-6d.cpp:135
ConstraintInequality m_forceInequality
Definition: contact-6d.hpp:124
double m_fMax
Definition: contact-6d.hpp:132
bool setFrictionCoefficient(const double frictionCoefficient)
Definition: contact-6d.cpp:189
virtual unsigned int n_force() const
Return the number of force variables.
Definition: contact-6d.cpp:156
double getMinNormalForce() const
Definition: contact-6d.cpp:262
void setReference(const SE3 &ref)
Definition: contact-6d.cpp:227
Matrix m_forceGenMat
Definition: contact-6d.hpp:133
ConstraintEquality m_forceRegTask
Definition: contact-6d.hpp:125
double m_mu
Definition: contact-6d.hpp:130
virtual const Matrix & getForceGeneratorMatrix()
Definition: contact-6d.cpp:248
void updateForceGeneratorMatrix()
Definition: contact-6d.cpp:144
Vector6 m_weightForceRegTask
Definition: contact-6d.hpp:129
math::ConstraintEquality ConstraintEquality
Definition: contact-6d.hpp:44
double getMaxNormalForce() const
Definition: contact-6d.cpp:263
const Vector & Kd() const
Definition: contact-6d.cpp:159
virtual unsigned int n_motion() const
Return the number of motion constraints.
Definition: contact-6d.cpp:155
pinocchio::SE3 SE3
Definition: contact-6d.hpp:45
virtual const ConstraintInequality & computeForceTask(const double t, ConstRefVector q, ConstRefVector v, const Data &data)
Definition: contact-6d.cpp:240
tasks::TaskSE3Equality TaskSE3Equality
Definition: contact-6d.hpp:42
TaskSE3Equality m_motionTask
Definition: contact-6d.hpp:123
math::Vector6 Vector6
Definition: contact-6d.hpp:39
virtual const ConstraintBase & computeMotionTask(const double t, ConstRefVector q, ConstRefVector v, Data &data)
Definition: contact-6d.cpp:232
double m_fMin
Definition: contact-6d.hpp:131
void updateForceInequalityConstraints()
Definition: contact-6d.cpp:83
math::Matrix3x Matrix3x
Definition: contact-6d.hpp:38
void setRegularizationTaskWeightVector(ConstRefVector &w)
Definition: contact-6d.cpp:129
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef math::ConstRefMatrix ConstRefMatrix
Definition: contact-6d.hpp:36
bool setMaxNormalForce(const double maxNormalForce)
Definition: contact-6d.cpp:210
math::Vector3 Vector3
Definition: contact-6d.hpp:40
math::ConstRefVector ConstRefVector
Definition: contact-6d.hpp:37
const ConstraintInequality & getForceConstraint() const
Definition: contact-6d.cpp:269
math::Vector Vector
Definition: contact-6d.hpp:41
const TaskSE3Equality & getMotionTask() const
Definition: contact-6d.cpp:265
bool setContactPoints(ConstRefMatrix contactPoints)
Definition: contact-6d.cpp:163
double getNormalForce(ConstRefVector f) const
Definition: contact-6d.cpp:120
const Matrix3x & getContactPoints() const
Definition: contact-6d.cpp:174
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: 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
#define TSID_DEPRECATED
Definition: deprecated.hh:37
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