GCC Code Coverage Report

Directory: ./
File: src/contacts/contact-two-frame-positions.cpp
Date: 2024-08-26 20:29:39
Exec Total Coverage
Lines: 0 91 0.0%
Branches: 0 136 0.0%
Line Branch Exec Source
1 //
2 // Copyright (c) 2023 MIPT
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-two-frame-positions.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 ✗ ContactTwoFramePositions::ContactTwoFramePositions(
30 const std::string& name, RobotWrapper& robot, const std::string& frameName1,
31 const std::string& frameName2, const double minNormalForce,
32 ✗ const double maxNormalForce)
33 : ContactBase(name, robot),
34 ✗ m_motionTask(
35 name, robot, frameName1,
36 frameName2), // Actual motion task with type TaskTwoFramesEquality
37 ✗ m_forceInequality(name, 3, 3),
38 ✗ m_forceRegTask(name, 3, 3),
39 ✗ m_fMin(minNormalForce),
40 ✗ m_fMax(maxNormalForce) {
41 ✗ m_weightForceRegTask << 1, 1, 1;
42 ✗ m_forceGenMat.resize(3, 3);
43 ✗ m_fRef = Vector3::Zero();
44 ✗ m_contactPoints.resize(3, 1);
45 ✗ m_contactPoints.setZero();
46 ✗ updateForceGeneratorMatrix();
47 ✗ updateForceInequalityConstraints();
48 ✗ updateForceRegularizationTask();
49
50 // This contact has forceGenMat as 3x3 identity matrix, so it can be used only
51 // for emulating a ball joint between two frames The forces calculated will
52 // have only linear part (rotation will be unconstrained) So we need to set
53 // the appropriate mask for motion task (which can take into account rotation
54 // but we don't need it)
55 ✗ math::Vector motion_mask(6);
56 ✗ motion_mask << 1., 1., 1., 0., 0., 0.;
57 ✗ m_motionTask.setMask(motion_mask);
58 }
59
60 ✗ void ContactTwoFramePositions::updateForceInequalityConstraints() {
61 ✗ Matrix B = Matrix::Identity(3, 3); // Force "gluing" two frames together can
62 // be arbitrary in sign/direction
63 ✗ Vector lb = m_fMin * Vector::Ones(3);
64 ✗ Vector ub = m_fMax * Vector::Ones(3);
65
66 ✗ m_forceInequality.setMatrix(B);
67 ✗ m_forceInequality.setLowerBound(lb);
68 ✗ m_forceInequality.setUpperBound(ub);
69 }
70
71 ✗ double ContactTwoFramePositions::getNormalForce(ConstRefVector f) const {
72 ✗ return 0.0;
73 }
74
75 ✗ const Matrix3x& ContactTwoFramePositions::getContactPoints() const {
76 ✗ return m_contactPoints;
77 }
78
79 ✗ void ContactTwoFramePositions::setRegularizationTaskWeightVector(
80 ConstRefVector& w) {
81 ✗ m_weightForceRegTask = w;
82 ✗ updateForceRegularizationTask();
83 }
84
85 ✗ void ContactTwoFramePositions::updateForceRegularizationTask() {
86 typedef Eigen::Matrix<double, 3, 3> Matrix3;
87 ✗ Matrix3 A = Matrix3::Zero();
88 ✗ A.diagonal() = m_weightForceRegTask;
89 ✗ m_forceRegTask.setMatrix(A);
90 ✗ m_forceRegTask.setVector(A * m_fRef);
91 }
92
93 ✗ void ContactTwoFramePositions::updateForceGeneratorMatrix() {
94 ✗ m_forceGenMat.setIdentity();
95 }
96
97 ✗ unsigned int ContactTwoFramePositions::n_motion() const {
98 ✗ return m_motionTask.dim();
99 }
100 ✗ unsigned int ContactTwoFramePositions::n_force() const { return 3; }
101
102 ✗ const Vector& ContactTwoFramePositions::Kp() {
103 ✗ m_Kp3 = m_motionTask.Kp().head<3>();
104 ✗ return m_Kp3;
105 }
106
107 ✗ const Vector& ContactTwoFramePositions::Kd() {
108 ✗ m_Kd3 = m_motionTask.Kd().head<3>();
109 ✗ return m_Kd3;
110 }
111
112 ✗ void ContactTwoFramePositions::Kp(ConstRefVector Kp) {
113 ✗ assert(Kp.size() == 3);
114 ✗ Vector6 Kp6;
115 ✗ Kp6.head<3>() = Kp;
116 ✗ m_motionTask.Kp(Kp6);
117 }
118
119 ✗ void ContactTwoFramePositions::Kd(ConstRefVector Kd) {
120 ✗ assert(Kd.size() == 3);
121 ✗ Vector6 Kd6;
122 ✗ Kd6.head<3>() = Kd;
123 ✗ m_motionTask.Kd(Kd6);
124 }
125
126 ✗ bool ContactTwoFramePositions::setContactNormal(ConstRefVector contactNormal) {
127 ✗ return true;
128 }
129
130 ✗ bool ContactTwoFramePositions::setFrictionCoefficient(
131 const double frictionCoefficient) {
132 ✗ return true;
133 }
134
135 ✗ bool ContactTwoFramePositions::setMinNormalForce(const double minNormalForce) {
136 ✗ m_fMin = minNormalForce;
137 ✗ updateForceInequalityConstraints();
138 ✗ return true;
139 }
140
141 ✗ bool ContactTwoFramePositions::setMaxNormalForce(const double maxNormalForce) {
142 ✗ m_fMax = maxNormalForce;
143 ✗ updateForceInequalityConstraints();
144 ✗ return true;
145 }
146
147 ✗ void ContactTwoFramePositions::setForceReference(ConstRefVector& f_ref) {
148 ✗ m_fRef = f_ref;
149 ✗ updateForceRegularizationTask();
150 }
151
152 ✗ const ConstraintBase& ContactTwoFramePositions::computeMotionTask(
153 const double t, ConstRefVector q, ConstRefVector v, Data& data) {
154 ✗ return m_motionTask.compute(t, q, v, data);
155 }
156
157 ✗ const ConstraintInequality& ContactTwoFramePositions::computeForceTask(
158 const double, ConstRefVector, ConstRefVector, const Data&) {
159 ✗ return m_forceInequality;
160 }
161
162 ✗ const Matrix& ContactTwoFramePositions::getForceGeneratorMatrix() {
163 ✗ return m_forceGenMat;
164 }
165
166 const ConstraintEquality&
167 ✗ ContactTwoFramePositions::computeForceRegularizationTask(const double,
168 ConstRefVector,
169 ConstRefVector,
170 const Data&) {
171 ✗ return m_forceRegTask;
172 }
173
174 ✗ double ContactTwoFramePositions::getMinNormalForce() const { return m_fMin; }
175 ✗ double ContactTwoFramePositions::getMaxNormalForce() const { return m_fMax; }
176
177 ✗ const TaskTwoFramesEquality& ContactTwoFramePositions::getMotionTask() const {
178 ✗ return m_motionTask;
179 }
180
181 ✗ const ConstraintBase& ContactTwoFramePositions::getMotionConstraint() const {
182 ✗ return m_motionTask.getConstraint();
183 }
184
185 ✗ const ConstraintInequality& ContactTwoFramePositions::getForceConstraint()
186 const {
187 ✗ return m_forceInequality;
188 }
189
190 ✗ const ConstraintEquality& ContactTwoFramePositions::getForceRegularizationTask()
191 const {
192 ✗ return m_forceRegTask;
193 }
194