GCC Code Coverage Report


Directory: ./
File: src/contacts/contact-6d.cpp
Date: 2024-11-10 01:12:44
Exec Total Coverage
Lines: 0 137 0.0%
Branches: 0 280 0.0%

Line Branch Exec Source
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 #include "tsid/math/utils.hpp"
19 #include "tsid/contacts/contact-6d.hpp"
20
21 #include <pinocchio/spatial/skew.hpp>
22
23 using namespace tsid;
24 using namespace contacts;
25 using namespace math;
26 using namespace trajectories;
27 using namespace tasks;
28
29 Contact6d::Contact6d(const std::string& name, RobotWrapper& robot,
30 const std::string& frameName, ConstRefMatrix contactPoints,
31 ConstRefVector contactNormal,
32 const double frictionCoefficient,
33 const double minNormalForce, const double maxNormalForce)
34 : ContactBase(name, robot),
35 m_motionTask(name, robot, frameName),
36 m_forceInequality(name, 17, 12),
37 m_forceRegTask(name, 6, 12),
38 m_contactPoints(contactPoints),
39 m_contactNormal(contactNormal),
40 m_mu(frictionCoefficient),
41 m_fMin(minNormalForce),
42 m_fMax(maxNormalForce) {
43 this->init();
44 }
45
46 Contact6d::Contact6d(const std::string& name, RobotWrapper& robot,
47 const std::string& frameName, ConstRefMatrix contactPoints,
48 ConstRefVector contactNormal,
49 const double frictionCoefficient,
50 const double minNormalForce, const double maxNormalForce,
51 const double)
52 : ContactBase(name, robot),
53 m_motionTask(name, robot, frameName),
54 m_forceInequality(name, 17, 12),
55 m_forceRegTask(name, 6, 12),
56 m_contactPoints(contactPoints),
57 m_contactNormal(contactNormal),
58 m_mu(frictionCoefficient),
59 m_fMin(minNormalForce),
60 m_fMax(maxNormalForce) {
61 std::cout << "[Contact6d] The constructor with forceRegWeight is deprecated "
62 "now. forceRegWeight should now be specified when calling "
63 "addRigidContact()\n";
64 this->init();
65 }
66
67 void Contact6d::init() {
68 m_weightForceRegTask << 1, 1, 1e-3, 2, 2, 2;
69 m_forceGenMat.resize(6, 12);
70 m_fRef = Vector6::Zero();
71 updateForceGeneratorMatrix();
72 updateForceInequalityConstraints();
73 updateForceRegularizationTask();
74 }
75
76 void Contact6d::updateForceInequalityConstraints() {
77 Vector3 t1, t2;
78 const int n_in = 4 * 4 + 1;
79 const int n_var = 3 * 4;
80 Matrix B = Matrix::Zero(n_in, n_var);
81 Vector lb = -1e10 * Vector::Ones(n_in);
82 Vector ub = Vector::Zero(n_in);
83 t1 = m_contactNormal.cross(Vector3::UnitX());
84 if (t1.norm() < 1e-5) t1 = m_contactNormal.cross(Vector3::UnitY());
85 t2 = m_contactNormal.cross(t1);
86 t1.normalize();
87 t2.normalize();
88
89 B.block<1, 3>(0, 0) = (-t1 - m_mu * m_contactNormal).transpose();
90 B.block<1, 3>(1, 0) = (t1 - m_mu * m_contactNormal).transpose();
91 B.block<1, 3>(2, 0) = (-t2 - m_mu * m_contactNormal).transpose();
92 B.block<1, 3>(3, 0) = (t2 - m_mu * m_contactNormal).transpose();
93
94 for (int i = 1; i < 4; i++) {
95 B.block<4, 3>(4 * i, 3 * i) = B.topLeftCorner<4, 3>();
96 }
97
98 B.block<1, 3>(n_in - 1, 0) = m_contactNormal.transpose();
99 B.block<1, 3>(n_in - 1, 3) = m_contactNormal.transpose();
100 B.block<1, 3>(n_in - 1, 6) = m_contactNormal.transpose();
101 B.block<1, 3>(n_in - 1, 9) = m_contactNormal.transpose();
102 ub(n_in - 1) = m_fMax;
103 lb(n_in - 1) = m_fMin;
104
105 m_forceInequality.setMatrix(B);
106 m_forceInequality.setLowerBound(lb);
107 m_forceInequality.setUpperBound(ub);
108 }
109
110 double Contact6d::getNormalForce(ConstRefVector f) const {
111 PINOCCHIO_CHECK_INPUT_ARGUMENT(
112 f.size() == n_force(),
113 "f needs to contain " + std::to_string(n_force()) + " rows");
114 double n = 0.0;
115 for (int i = 0; i < 4; i++) n += m_contactNormal.dot(f.segment<3>(i * 3));
116 return n;
117 }
118
119 void Contact6d::setRegularizationTaskWeightVector(ConstRefVector& w) {
120 m_weightForceRegTask = w;
121 updateForceRegularizationTask();
122 }
123
124 void Contact6d::updateForceRegularizationTask() {
125 typedef Eigen::Matrix<double, 6, 6> Matrix6;
126 Matrix6 A = Matrix6::Zero();
127 A.diagonal() = m_weightForceRegTask;
128 m_forceRegTask.setMatrix(A * m_forceGenMat);
129 m_forceRegTask.setVector(A * m_fRef);
130 }
131
132 void Contact6d::updateForceGeneratorMatrix() {
133 assert(m_contactPoints.rows() == 3);
134 assert(m_contactPoints.cols() == 4);
135 for (int i = 0; i < 4; i++) {
136 m_forceGenMat.block<3, 3>(0, i * 3).setIdentity();
137 m_forceGenMat.block<3, 3>(3, i * 3) =
138 pinocchio::skew(m_contactPoints.col(i));
139 }
140 }
141
142 unsigned int Contact6d::n_motion() const { return 6; }
143 unsigned int Contact6d::n_force() const { return 12; }
144
145 const Vector& Contact6d::Kp() const { return m_motionTask.Kp(); }
146 const Vector& Contact6d::Kd() const { return m_motionTask.Kd(); }
147 void Contact6d::Kp(ConstRefVector Kp) { m_motionTask.Kp(Kp); }
148 void Contact6d::Kd(ConstRefVector Kd) { m_motionTask.Kd(Kd); }
149
150 bool Contact6d::setContactPoints(ConstRefMatrix contactPoints) {
151 PINOCCHIO_CHECK_INPUT_ARGUMENT(contactPoints.rows() == 3,
152 "The number of rows needs to be 3");
153 PINOCCHIO_CHECK_INPUT_ARGUMENT(contactPoints.cols() == 4,
154 "The number of cols needs to be 4");
155 if (contactPoints.rows() != 3 || contactPoints.cols() != 4) return false;
156 m_contactPoints = contactPoints;
157 updateForceGeneratorMatrix();
158 return true;
159 }
160
161 const Matrix3x& Contact6d::getContactPoints() const { return m_contactPoints; }
162
163 bool Contact6d::setContactNormal(ConstRefVector contactNormal) {
164 PINOCCHIO_CHECK_INPUT_ARGUMENT(
165 contactNormal.size() == 3,
166 "The size of the contactNormal vector needs to equal 3");
167 if (contactNormal.size() != 3) return false;
168 m_contactNormal = contactNormal;
169 updateForceInequalityConstraints();
170 return true;
171 }
172
173 bool Contact6d::setFrictionCoefficient(const double frictionCoefficient) {
174 PINOCCHIO_CHECK_INPUT_ARGUMENT(
175 frictionCoefficient > 0.0,
176 "The friction coefficient needs to be positive");
177 if (frictionCoefficient <= 0.0) return false;
178 m_mu = frictionCoefficient;
179 updateForceInequalityConstraints();
180 return true;
181 }
182
183 bool Contact6d::setMinNormalForce(const double minNormalForce) {
184 PINOCCHIO_CHECK_INPUT_ARGUMENT(
185 minNormalForce > 0.0 && minNormalForce <= m_fMax,
186 "The minimal normal force needs to be greater than 0 and less than or "
187 "equal to the maximal force");
188 if (minNormalForce <= 0.0 || minNormalForce > m_fMax) return false;
189 m_fMin = minNormalForce;
190 Vector& lb = m_forceInequality.lowerBound();
191 lb(lb.size() - 1) = m_fMin;
192 return true;
193 }
194
195 bool Contact6d::setMaxNormalForce(const double maxNormalForce) {
196 PINOCCHIO_CHECK_INPUT_ARGUMENT(maxNormalForce >= m_fMin,
197 "The maximal force needs to be greater than "
198 "or equal to the minimal force");
199 if (maxNormalForce < m_fMin) return false;
200 m_fMax = maxNormalForce;
201 Vector& ub = m_forceInequality.upperBound();
202 ub(ub.size() - 1) = m_fMax;
203 return true;
204 }
205
206 void Contact6d::setForceReference(ConstRefVector& f_ref) {
207 m_fRef = f_ref;
208 updateForceRegularizationTask();
209 }
210
211 void Contact6d::setReference(const SE3& ref) { m_motionTask.setReference(ref); }
212
213 const ConstraintBase& Contact6d::computeMotionTask(const double t,
214 ConstRefVector q,
215 ConstRefVector v,
216 Data& data) {
217 return m_motionTask.compute(t, q, v, data);
218 }
219
220 const ConstraintInequality& Contact6d::computeForceTask(const double,
221 ConstRefVector,
222 ConstRefVector,
223 const Data&) {
224 return m_forceInequality;
225 }
226
227 const Matrix& Contact6d::getForceGeneratorMatrix() { return m_forceGenMat; }
228
229 const ConstraintEquality& Contact6d::computeForceRegularizationTask(
230 const double, ConstRefVector, ConstRefVector, const Data&) {
231 return m_forceRegTask;
232 }
233
234 double Contact6d::getMinNormalForce() const { return m_fMin; }
235 double Contact6d::getMaxNormalForce() const { return m_fMax; }
236
237 const TaskSE3Equality& Contact6d::getMotionTask() const { return m_motionTask; }
238
239 const ConstraintBase& Contact6d::getMotionConstraint() const {
240 return m_motionTask.getConstraint();
241 }
242
243 const ConstraintInequality& Contact6d::getForceConstraint() const {
244 return m_forceInequality;
245 }
246
247 const ConstraintEquality& Contact6d::getForceRegularizationTask() const {
248 return m_forceRegTask;
249 }
250