GCC Code Coverage Report


Directory: ./
File: src/static-stability.cc
Date: 2025-05-05 12:19:30
Exec Total Coverage
Lines: 0 94 0.0%
Branches: 0 394 0.0%

Line Branch Exec Source
1 // Copyright (c) 2014, LAAS-CNRS
2 // Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
3 //
4
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are
7 // met:
8 //
9 // 1. Redistributions of source code must retain the above copyright
10 // notice, this list of conditions and the following disclaimer.
11 //
12 // 2. Redistributions in binary form must reproduce the above copyright
13 // notice, this list of conditions and the following disclaimer in the
14 // documentation and/or other materials provided with the distribution.
15 //
16 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
17 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
18 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
19 // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
20 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
21 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
22 // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
23 // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
24 // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
26 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
27 // DAMAGE.
28
29 #include "hpp/constraints/static-stability.hh"
30
31 #include <hpp/pinocchio/device.hh>
32 #include <hpp/pinocchio/joint.hh>
33 #include <hpp/pinocchio/liegroup-element.hh>
34 #include <limits>
35
36 #include "hpp/constraints/tools.hh"
37
38 namespace hpp {
39 namespace constraints {
40
41 using hpp::pinocchio::LiegroupElement;
42
43 const value_type StaticStability::G = 9.81;
44 const Eigen::Matrix<value_type, 6, 1> StaticStability::Gravity =
45 (Eigen::Matrix<value_type, 6, 1>() << 0, 0, -1, 0, 0, 0).finished();
46
47 StaticStability::StaticStability(const std::string& name,
48 const DevicePtr_t& robot,
49 const Contacts_t& contacts,
50 const CenterOfMassComputationPtr_t& com)
51 : DifferentiableFunction(robot->configSize(), robot->numberDof(),
52 contacts.size() + 6, name),
53 robot_(robot),
54 contacts_(contacts),
55 com_(com),
56 phi_(Eigen::Matrix<value_type, 6, Eigen::Dynamic>::Zero(6,
57 contacts.size()),
58 Eigen::Matrix<value_type, 6, Eigen::Dynamic>::Zero(
59 6, contacts.size() * robot->numberDof())),
60 u_(contacts.size()),
61 uMinus_(contacts.size()),
62 v_(contacts.size()),
63 uDot_(contacts.size(), robot->numberDof()),
64 uMinusDot_(contacts.size(), robot->numberDof()),
65 vDot_(contacts.size(), robot->numberDof()),
66 lambdaDot_(robot->numberDof()) {
67 phi_.setSize(2, contacts.size());
68 Traits<PointCom>::Ptr_t OG = PointCom::create(com);
69 for (std::size_t i = 0; i < contacts.size(); ++i) {
70 Traits<PointInJoint>::Ptr_t OP2 = PointInJoint::create(
71 contacts[i].joint, contacts[i].point, robot->numberDof());
72 Traits<VectorInJoint>::Ptr_t n2 = VectorInJoint::create(
73 contacts[i].joint, contacts[i].normal, robot->numberDof());
74
75 phi_(0, i) = n2;
76 phi_(1, i) = (OG - OP2) ^ n2;
77 }
78 }
79
80 StaticStabilityPtr_t StaticStability::create(
81 const std::string& name, const DevicePtr_t& robot,
82 const Contacts_t& contacts, const CenterOfMassComputationPtr_t& com) {
83 return StaticStabilityPtr_t(new StaticStability(name, robot, contacts, com));
84 }
85
86 StaticStabilityPtr_t StaticStability::create(
87 const DevicePtr_t& robot, const Contacts_t& contacts,
88 const CenterOfMassComputationPtr_t& com) {
89 return create("StaticStability", robot, contacts, com);
90 }
91
92 void StaticStability::impl_compute(LiegroupElementRef result,
93 ConfigurationIn_t argument) const {
94 robot_->currentConfiguration(argument);
95 robot_->computeForwardKinematics(pinocchio::JOINT_POSITION);
96
97 phi_.invalidate();
98
99 phi_.computeSVD(argument);
100
101 const Eigen::Matrix<value_type, 6, 1> G = -1 * Gravity;
102 u_.noalias() = phi_.svd().solve(G);
103
104 if (computeUminusAndV(u_, uMinus_, v_)) {
105 // value_type lambda, unused_lMax; size_type iMax, iMin;
106 // findBoundIndex (u_, v_, lambda, &iMin, unused_lMax, &iMax);
107 value_type lambda = 1;
108
109 result.vector().segment(0, contacts_.size()) = u_ + lambda * v_;
110 } else {
111 result.vector().segment(0, contacts_.size()) = u_;
112 }
113 result.vector().segment<6>(contacts_.size()) = Gravity + phi_.value() * u_;
114 }
115
116 void StaticStability::impl_jacobian(matrixOut_t jacobian,
117 ConfigurationIn_t argument) const {
118 robot_->currentConfiguration(argument);
119 robot_->computeForwardKinematics(pinocchio::JOINT_POSITION |
120 pinocchio::JACOBIAN);
121
122 phi_.invalidate();
123
124 phi_.computeSVD(argument);
125 phi_.computeJacobian(argument);
126 phi_.computePseudoInverse(argument);
127
128 const Eigen::Matrix<value_type, 6, 1> G = -1 * Gravity;
129 u_.noalias() = phi_.svd().solve(G);
130 phi_.computePseudoInverseJacobian(argument, G);
131 uDot_.noalias() = phi_.pinvJacobian();
132
133 jacobian.block(0, 0, contacts_.size(), robot_->numberDof()).noalias() = uDot_;
134
135 if (computeUminusAndV(u_, uMinus_, v_)) {
136 matrix_t S = -matrix_t::Identity(u_.size(), u_.size());
137 S.diagonal() = 1 * (u_.array() >= 0).select(0, -vector_t::Ones(u_.size()));
138
139 // value_type lambda, unused_lMax; size_type iMax, iMin;
140 // findBoundIndex (u_, v_, lambda, &iMin, unused_lMax, &iMax);
141 // if (lambda < Eigen::NumTraits <value_type>::dummy_precision())
142 // return;
143 value_type lambda = 1;
144
145 computeVDot(argument, uMinus_, S.diagonal(), uDot_, uMinusDot_, vDot_);
146
147 // computeLambdaDot (u_, v_, iMin, uDot_, vDot_, lambdaDot_);
148
149 // jacobian.block (0, 0, contacts_.size(), robot_->numberDof()).noalias ()
150 // += lambda * vDot_ + v_ * lambdaDot_.transpose ();
151
152 jacobian.block(0, 0, contacts_.size(), robot_->numberDof()).noalias() +=
153 lambda * vDot_;
154 }
155
156 phi_.jacobianTimes(
157 argument, u_,
158 jacobian.block(contacts_.size(), 0, 6, robot_->numberDof()));
159 phi_.computePseudoInverseJacobian(argument, Gravity);
160 jacobian.block(contacts_.size(), 0, 6, robot_->numberDof()) +=
161 -phi_.value() * phi_.pinvJacobian();
162 }
163
164 void StaticStability::findBoundIndex(vectorIn_t u, vectorIn_t v,
165 value_type& lambdaMin, size_type* iMin,
166 value_type& lambdaMax, size_type* iMax) {
167 // Be carefull when v has small values.
168 // Consider them as 0
169 const value_type eps = Eigen::NumTraits<value_type>::dummy_precision();
170 vector_t lambdas =
171 (v.array().cwiseAbs() < eps).select(0, -u.cwiseQuotient(v));
172 lambdaMin = (v.array() > eps).select(lambdas, 0).maxCoeff(iMin);
173 lambdaMax = (v.array() < eps).select(lambdas, 0).minCoeff(iMax);
174 }
175
176 bool StaticStability::computeUminusAndV(vectorIn_t u, vectorOut_t uMinus,
177 vectorOut_t v) const {
178 using namespace hpp::pinocchio;
179
180 uMinus.noalias() = (u.array() >= 0).select(0, -u);
181
182 if (uMinus.isZero()) return false;
183
184 size_type rank = phi_.svd().rank();
185 v.noalias() = getV2<MoE_t::SVD_t>(phi_.svd(), rank) *
186 (getV2<MoE_t::SVD_t>(phi_.svd(), rank).adjoint() * uMinus);
187 // v.noalias() = uMinus;
188 // v.noalias() -= getV1 <MoE_t::SVD_t> (phi_.svd()) *
189 // ( getV1 <MoE_t::SVD_t> (phi_.svd()).adjoint() * uMinus );
190 return true;
191 }
192
193 void StaticStability::computeVDot(const ConfigurationIn_t arg,
194 vectorIn_t uMinus, vectorIn_t S,
195 matrixIn_t uDot, matrixOut_t uMinusDot,
196 matrixOut_t vDot) const {
197 using namespace hpp::pinocchio;
198
199 size_type rank = phi_.svd().rank();
200 uMinusDot.noalias() = S.asDiagonal() * uDot;
201 vDot.noalias() = uMinusDot;
202 vDot.noalias() -=
203 getV1<MoE_t::SVD_t>(phi_.svd(), rank) *
204 (getV1<MoE_t::SVD_t>(phi_.svd(), rank).adjoint() * uMinusDot);
205
206 // TODO: preallocate this matrix
207 Eigen::Matrix<value_type, 6, Eigen::Dynamic> JphiTimesUMinus(
208 6, robot_->numberDof());
209 phi_.jacobianTimes(arg, uMinus, JphiTimesUMinus);
210 vDot.noalias() -= phi_.pinv() * JphiTimesUMinus;
211
212 phi_.computePseudoInverseJacobian(arg, phi_.value() * uMinus);
213 vDot.noalias() -= phi_.pinvJacobian();
214 }
215
216 void StaticStability::computeLambdaDot(vectorIn_t u, vectorIn_t v,
217 const std::size_t i0, matrixIn_t uDot,
218 matrixIn_t vDot,
219 vectorOut_t lambdaDot) const {
220 if (std::abs(v(i0)) < Eigen::NumTraits<value_type>::dummy_precision())
221 lambdaDot.setZero();
222 else {
223 lambdaDot.noalias() = -uDot.row(i0) / v(i0);
224 lambdaDot.noalias() += (u(i0) / (v(i0) * v(i0))) * vDot.row(i0);
225 }
226 }
227 } // namespace constraints
228 } // namespace hpp
229