| GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
| Line | Branch | Exec | Source |
1 |
// |
||
2 |
// Copyright (c) 2014 CNRS |
||
3 |
// Authors: Steve Tonneau (steve.tonneau@laas.fr) |
||
4 |
// |
||
5 |
// This file is part of hpp-rbprm. |
||
6 |
// hpp-rbprm is free software: you can redistribute it |
||
7 |
// and/or modify it under the terms of the GNU Lesser General Public |
||
8 |
// License as published by the Free Software Foundation, either version |
||
9 |
// 3 of the License, or (at your option) any later version. |
||
10 |
// |
||
11 |
// hpp-rbprm is distributed in the hope that it will be |
||
12 |
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty |
||
13 |
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU |
||
14 |
// General Lesser Public License for more details. You should have |
||
15 |
// received a copy of the GNU Lesser General Public License along with |
||
16 |
// hpp-core If not, see |
||
17 |
// <http://www.gnu.org/licenses/>. |
||
18 |
|||
19 |
#include <Eigen/Dense> |
||
20 |
#include <hpp/pinocchio/center-of-mass-computation.hh> |
||
21 |
#include <hpp/pinocchio/configuration.hh> |
||
22 |
#include <hpp/pinocchio/device.hh> |
||
23 |
#include <hpp/pinocchio/joint.hh> |
||
24 |
#include <hpp/rbprm/stability/stability.hh> |
||
25 |
#include <hpp/rbprm/stability/support.hh> |
||
26 |
#include <hpp/rbprm/tools.hh> |
||
27 |
#include <map> |
||
28 |
#include <string> |
||
29 |
#include <vector> |
||
30 |
|||
31 |
#ifdef PROFILE |
||
32 |
#include "hpp/rbprm/rbprm-profiler.hh" |
||
33 |
#endif |
||
34 |
|||
35 |
using namespace hpp; |
||
36 |
using namespace hpp::core; |
||
37 |
using namespace hpp::pinocchio; |
||
38 |
using namespace hpp::rbprm; |
||
39 |
using namespace centroidal_dynamics; |
||
40 |
|||
41 |
namespace hpp { |
||
42 |
namespace rbprm { |
||
43 |
namespace stability { |
||
44 |
|||
45 |
64 |
void computeRectangleContact(const std::string& name, const RbPrmLimbPtr_t limb, |
|
46 |
const State& state, Ref_matrix43 p, double lx = 0, |
||
47 |
double ly = 0) { |
||
48 |
hppDout(notice, "Compute rectangular contact : "); |
||
49 |
✗✓ | 64 |
if (lx == 0) lx = limb->x_; |
50 |
✗✓ | 64 |
if (ly == 0) ly = limb->y_; |
51 |
✓✗ | 64 |
const fcl::Vec3f& position = state.contactPositions_.at(name); |
52 |
hppDout(notice, "Position of center : " << position.transpose()); |
||
53 |
// create rotation matrix from normal |
||
54 |
✓✗ | 64 |
Eigen::Matrix3d R; |
55 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
64 |
p << lx, ly, 0, lx, -ly, 0, -lx, -ly, 0, -lx, ly, 0; |
56 |
✗✓ | 64 |
if (limb->contactType_ == _3_DOF) { |
57 |
// create rotation matrix from normal |
||
58 |
const fcl::Vec3f& normal = state.contactNormals_.at(name); |
||
59 |
const fcl::Vec3f z_current = |
||
60 |
limb->effector_.currentTransformation().rotation() * limb->normal_; |
||
61 |
const fcl::Matrix3f alignRotation = |
||
62 |
tools::GetRotationMatrix(z_current, normal); |
||
63 |
const fcl::Matrix3f rotation = |
||
64 |
alignRotation * limb->effector_.currentTransformation().rotation(); |
||
65 |
const fcl::Vec3f offset = rotation * limb->offset_; |
||
66 |
Eigen::Vector3d z, x, y; |
||
67 |
for (int i = 0; i < 3; ++i) z[i] = normal[i]; |
||
68 |
x = z.cross(Eigen::Vector3d(0, -1, 0)); |
||
69 |
if (x.norm() < 10e-6) { |
||
70 |
y = z.cross(fcl::Vec3f(1, 0, 0)); |
||
71 |
y.normalize(); |
||
72 |
x = y.cross(z); |
||
73 |
} else { |
||
74 |
x.normalize(); |
||
75 |
y = z.cross(x); |
||
76 |
} |
||
77 |
R.block<3, 1>(0, 0) = x; |
||
78 |
R.block<3, 1>(0, 1) = y; |
||
79 |
R.block<3, 1>(0, 2) = z; |
||
80 |
|||
81 |
for (std::size_t i = 0; i < 4; ++i) { |
||
82 |
p.row(i) = position + (R * (p.row(i).transpose())) + offset; |
||
83 |
} |
||
84 |
} else { |
||
85 |
✓✗ | 64 |
fcl::Vec3f z_axis(0, 0, 1); |
86 |
fcl::Matrix3f rotationLocal = |
||
87 |
✓✗✓✗ ✓✗ |
64 |
tools::GetRotationMatrix(z_axis, limb->normal_).inverse(); |
88 |
✓✗ | 64 |
fcl::Transform3f roWorld; |
89 |
✓✗✓✗ |
64 |
roWorld.setRotation(state.contactRotation_.at(name)); |
90 |
✓✗ | 64 |
roWorld.setTranslation(position); |
91 |
✓✓ | 320 |
for (std::size_t i = 0; i < 4; ++i) { |
92 |
fcl::Vec3f pLocal = |
||
93 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
256 |
rotationLocal * (p.row(i).transpose()) + limb->offset_; |
94 |
✓✗✓✗ ✓✗✓✗ |
256 |
p.row(i) = (roWorld * pLocal).getTranslation(); |
95 |
hppDout(notice, "position : " << p.row(i)); |
||
96 |
} |
||
97 |
} |
||
98 |
64 |
} |
|
99 |
|||
100 |
Vector3 computePointContact(const std::string& name, const RbPrmLimbPtr_t limb, |
||
101 |
const State& state) { |
||
102 |
const fcl::Vec3f& position = state.contactPositions_.at(name); |
||
103 |
// create rotation matrix from normal |
||
104 |
const fcl::Vec3f& normal = state.contactNormals_.at(name); |
||
105 |
const fcl::Vec3f z_current = |
||
106 |
limb->effector_.currentTransformation().rotation() * limb->normal_; |
||
107 |
const fcl::Matrix3f alignRotation = |
||
108 |
tools::GetRotationMatrix(z_current, normal); |
||
109 |
const fcl::Matrix3f rotation = |
||
110 |
alignRotation * limb->effector_.currentTransformation().rotation(); |
||
111 |
const fcl::Vec3f offset = rotation * limb->offset_; |
||
112 |
return position + offset; |
||
113 |
} |
||
114 |
|||
115 |
Equilibrium initLibrary(const RbPrmFullBodyPtr_t fullbody) { |
||
116 |
return Equilibrium(fullbody->device_->name(), fullbody->device_->mass(), 4, |
||
117 |
SOLVER_LP_QPOASES, true, 10, false); |
||
118 |
} |
||
119 |
|||
120 |
64 |
std::size_t numContactPoints(const RbPrmLimbPtr_t& limb) { |
|
121 |
✗✓ | 64 |
if (limb->contactType_ == _3_DOF) |
122 |
return 1; |
||
123 |
else |
||
124 |
64 |
return 4; |
|
125 |
} |
||
126 |
|||
127 |
64 |
const std::vector<std::size_t> numContactPoints( |
|
128 |
const T_Limb& limbs, const std::vector<std::string>& contacts, |
||
129 |
std::size_t& totalNumContacts) { |
||
130 |
std::size_t n; |
||
131 |
64 |
std::vector<std::size_t> res; |
|
132 |
128 |
for (std::vector<std::string>::const_iterator cit = contacts.begin(); |
|
133 |
✓✓ | 192 |
cit != contacts.end(); ++cit) { |
134 |
✓✗✓✗ |
64 |
n = numContactPoints(limbs.at(*cit)); |
135 |
64 |
totalNumContacts += n; |
|
136 |
✓✗ | 64 |
res.push_back(n); |
137 |
} |
||
138 |
128 |
return res; |
|
139 |
} |
||
140 |
|||
141 |
32 |
centroidal_dynamics::Vector3 setupLibrary(const RbPrmFullBodyPtr_t fullbody, |
|
142 |
State& state, Equilibrium& sEq, |
||
143 |
EquilibriumAlgorithm& alg, |
||
144 |
core::value_type friction, |
||
145 |
const double feetX, |
||
146 |
const double feetY) { |
||
147 |
32 |
friction = fullbody->getFriction(); |
|
148 |
hppDout(notice, "Setup centroidal dynamic lib, friction = " << friction); |
||
149 |
32 |
const rbprm::T_Limb& limbs = fullbody->GetLimbs(); |
|
150 |
hpp::pinocchio::ConfigurationIn_t save = |
||
151 |
✓✗✓✗ |
64 |
fullbody->device_->currentConfiguration(); |
152 |
64 |
std::vector<std::string> contacts; |
|
153 |
64 |
std::vector<std::string> graspscontacts; |
|
154 |
64 |
for (std::map<std::string, fcl::Vec3f>::const_iterator cit = |
|
155 |
32 |
state.contactPositions_.begin(); |
|
156 |
✓✓ | 160 |
cit != state.contactPositions_.end(); ++cit) { |
157 |
✓✗✗✓ |
64 |
if (limbs.at(cit->first)->grasps_) |
158 |
graspscontacts.push_back(cit->first); |
||
159 |
else |
||
160 |
✓✗ | 64 |
contacts.push_back(cit->first); |
161 |
} |
||
162 |
✓✗✓✗ |
32 |
fullbody->device_->currentConfiguration(state.configuration_); |
163 |
✓✗ | 32 |
fullbody->device_->computeForwardKinematics(); |
164 |
32 |
std::size_t nbContactPoints(0); |
|
165 |
std::vector<std::size_t> contactPointsInc = |
||
166 |
✓✗ | 64 |
numContactPoints(limbs, contacts, nbContactPoints); |
167 |
std::vector<std::size_t> contactGraspPointsInc = |
||
168 |
✓✗ | 64 |
numContactPoints(limbs, graspscontacts, nbContactPoints); |
169 |
✓✗ | 64 |
centroidal_dynamics::MatrixX3 normals(nbContactPoints, 3); |
170 |
✓✗ | 64 |
centroidal_dynamics::MatrixX3 positions(nbContactPoints, 3); |
171 |
32 |
std::size_t currentIndex(0), c(0); |
|
172 |
96 |
for (std::vector<std::size_t>::const_iterator cit = contactPointsInc.begin(); |
|
173 |
✓✓ | 160 |
cit != contactPointsInc.end(); ++cit, ++c) { |
174 |
✓✗ | 64 |
const RbPrmLimbPtr_t limb = limbs.at(contacts[c]); |
175 |
✓✗ | 64 |
const fcl::Vec3f& n = state.contactNormals_.at(contacts[c]); |
176 |
✓✗✓✗ ✓✗✓✗ |
64 |
Vector3 normal(n[0], n[1], n[2]); |
177 |
✓✗ | 64 |
normal.normalize(); |
178 |
64 |
const std::size_t& inc = *cit; |
|
179 |
✓✗ | 64 |
if (inc > 1) |
180 |
✓✗✓✗ |
64 |
computeRectangleContact(contacts[c], limb, state, |
181 |
✓✗ | 128 |
positions.middleRows<4>(currentIndex), feetX, |
182 |
feetY); |
||
183 |
else |
||
184 |
positions.middleRows<1>(currentIndex, inc) = |
||
185 |
computePointContact(contacts[c], limb, state); |
||
186 |
✓✓ | 320 |
for (std::size_t i = 0; i < inc; ++i) { |
187 |
✓✗✓✗ |
256 |
normals.middleRows<1>(currentIndex + i) = normal; |
188 |
} |
||
189 |
64 |
currentIndex += inc; |
|
190 |
} |
||
191 |
32 |
int graspIndex = -1; |
|
192 |
✗✓ | 32 |
if (graspscontacts.size() > 0) { |
193 |
c = 0; |
||
194 |
graspIndex = (int)currentIndex; |
||
195 |
for (std::vector<std::size_t>::const_iterator cit = |
||
196 |
contactGraspPointsInc.begin(); |
||
197 |
cit != contactGraspPointsInc.end(); ++cit, ++c) { |
||
198 |
const RbPrmLimbPtr_t limb = limbs.at(graspscontacts[c]); |
||
199 |
const fcl::Vec3f& n = state.contactNormals_.at(graspscontacts[c]); |
||
200 |
Vector3 normal(n[0], n[1], n[2]); |
||
201 |
const std::size_t& inc = *cit; |
||
202 |
if (inc > 1) |
||
203 |
computeRectangleContact(graspscontacts[c], limb, state, |
||
204 |
positions.middleRows<4>(currentIndex)); |
||
205 |
else |
||
206 |
positions.middleRows<1>(currentIndex, inc) = |
||
207 |
computePointContact(graspscontacts[c], limb, state); |
||
208 |
for (std::size_t i = 0; i < inc; ++i) { |
||
209 |
normals.middleRows<1>(currentIndex + i) = normal; |
||
210 |
} |
||
211 |
currentIndex += inc; |
||
212 |
} |
||
213 |
} |
||
214 |
✓✗ | 32 |
centroidal_dynamics::Vector3 com; |
215 |
/*pinocchio::CenterOfMassComputationPtr_t comcptr = |
||
216 |
pinocchio::CenterOfMassComputation::create(fullbody->device_); |
||
217 |
comcptr->add(fullbody->device_->getJointByName("romeo/base_joint_xyz")); |
||
218 |
comcptr->computeMass(); |
||
219 |
comcptr->compute(); |
||
220 |
const fcl::Vec3f comfcl = comcptr->com();*/ |
||
221 |
✓✗✓✗ |
32 |
const fcl::Vec3f comfcl = fullbody->device_->positionCenterOfMass(); |
222 |
✓✓✓✗ ✓✗ |
128 |
for (int i = 0; i < 3; ++i) com(i) = comfcl[i]; |
223 |
✓✗✓✗ |
32 |
fullbody->device_->currentConfiguration(save); |
224 |
✗✓✗✗ |
32 |
if (graspIndex > -1 && alg != EQUILIBRIUM_ALGORITHM_PP) { |
225 |
alg = EQUILIBRIUM_ALGORITHM_PP; |
||
226 |
} |
||
227 |
hppDout(notice, "Setup cone contacts : "); |
||
228 |
hppDout(notice, "position : \n" << positions); |
||
229 |
hppDout(notice, "normal : \n" << normals); |
||
230 |
✓✗ | 32 |
bool success = sEq.setNewContacts(positions, normals, friction, alg); |
231 |
✗✓ | 32 |
if (!success) |
232 |
throw std::runtime_error( |
||
233 |
"Error in centroidal-dynamic lib while computing new contacts"); |
||
234 |
64 |
return com; |
|
235 |
} |
||
236 |
|||
237 |
std::pair<MatrixXX, VectorX> ComputeCentroidalCone( |
||
238 |
const RbPrmFullBodyPtr_t fullbody, State& state, |
||
239 |
const hpp::core::value_type friction) { |
||
240 |
std::pair<MatrixXX, VectorX> res; |
||
241 |
MatrixXX& H = res.first; |
||
242 |
VectorX& h = res.second; |
||
243 |
#ifdef PROFILE |
||
244 |
RbPrmProfiler& watch = getRbPrmProfiler(); |
||
245 |
watch.start("test balance"); |
||
246 |
#endif |
||
247 |
Equilibrium staticEquilibrium(initLibrary(fullbody)); |
||
248 |
centroidal_dynamics::EquilibriumAlgorithm alg = EQUILIBRIUM_ALGORITHM_PP; |
||
249 |
setupLibrary(fullbody, state, staticEquilibrium, alg, friction); |
||
250 |
#ifdef PROFILE |
||
251 |
watch.stop("test balance"); |
||
252 |
#endif |
||
253 |
LP_status status = LP_STATUS_OPTIMAL; |
||
254 |
if (status != LP_STATUS_OPTIMAL) { |
||
255 |
std::cout << "error " << std::endl; |
||
256 |
} else { |
||
257 |
status = staticEquilibrium.getPolytopeInequalities(H, h); |
||
258 |
if (status != LP_STATUS_OPTIMAL) { |
||
259 |
std::cout << "error " << std::endl; |
||
260 |
H = Eigen::MatrixXd::Zero(6, 6); |
||
261 |
h = Eigen::MatrixXd::Zero(6, 1); |
||
262 |
} |
||
263 |
} |
||
264 |
return res; |
||
265 |
} |
||
266 |
|||
267 |
double IsStable(const RbPrmFullBodyPtr_t fullbody, State& state, fcl::Vec3f acc, |
||
268 |
fcl::Vec3f com, |
||
269 |
const centroidal_dynamics::EquilibriumAlgorithm algorithm) { |
||
270 |
#ifdef PROFILE |
||
271 |
RbPrmProfiler& watch = getRbPrmProfiler(); |
||
272 |
watch.start("test balance"); |
||
273 |
#endif |
||
274 |
centroidal_dynamics::EquilibriumAlgorithm alg = algorithm; |
||
275 |
// centroidal_dynamics::EquilibriumAlgorithm alg= |
||
276 |
// centroidal_dynamics::EQUILIBRIUM_ALGORITHM_PP; |
||
277 |
if (fullbody->device_->extraConfigSpace().dimension() >= 6) { |
||
278 |
if (acc.norm() == 0) { |
||
279 |
hppDout(notice, "isStable ? called with acc = 0"); |
||
280 |
hppDout(notice, "configuration in state = " |
||
281 |
<< pinocchio::displayConfig(state.configuration_)); |
||
282 |
core::size_type configSize = |
||
283 |
fullbody->device_->configSize() - |
||
284 |
fullbody->device_->extraConfigSpace().dimension(); |
||
285 |
acc = state.configuration_.segment<3>(configSize + 3); |
||
286 |
hppDout(notice, "new acceleration = " << acc); |
||
287 |
} |
||
288 |
} |
||
289 |
Equilibrium staticEquilibrium(initLibrary(fullbody)); |
||
290 |
centroidal_dynamics::Vector3 comComputed = |
||
291 |
setupLibrary(fullbody, state, staticEquilibrium, alg); |
||
292 |
if (!com.isZero()) { |
||
293 |
hppDout(notice, |
||
294 |
"isStable : a CoM was given as parameter, use this one. : " << com); |
||
295 |
comComputed = centroidal_dynamics::Vector3(com); |
||
296 |
} |
||
297 |
double res; |
||
298 |
LP_status status; |
||
299 |
if (alg == EQUILIBRIUM_ALGORITHM_PP) { |
||
300 |
hppDout(notice, "isStable Called with STATIC_EQUILIBRIUM_ALGORITHM_PP"); |
||
301 |
bool isStable(false); |
||
302 |
if (fullbody->staticStability()) { |
||
303 |
status = staticEquilibrium.checkRobustEquilibrium(comComputed, isStable); |
||
304 |
} else { |
||
305 |
status = |
||
306 |
staticEquilibrium.checkRobustEquilibrium(comComputed, acc, isStable); |
||
307 |
} |
||
308 |
res = isStable |
||
309 |
? std::numeric_limits<double>::max() |
||
310 |
: -1.; // FIXME robustness not implemented with PP algorithm ... |
||
311 |
} else // STATIC_EQUILIBRIUM_ALGORITHM_DLP |
||
312 |
{ |
||
313 |
if (fullbody->staticStability()) { |
||
314 |
status = staticEquilibrium.computeEquilibriumRobustness(comComputed, res); |
||
315 |
hppDout(notice, "isStable Called with staticStability"); |
||
316 |
} else { |
||
317 |
status = |
||
318 |
staticEquilibrium.computeEquilibriumRobustness(comComputed, acc, res); |
||
319 |
hppDout(notice, "isStable : config = " |
||
320 |
<< pinocchio::displayConfig(state.configuration_)); |
||
321 |
hppDout(notice, "isStable : COM = " << comComputed.transpose()); |
||
322 |
hppDout(notice, "isStable : acc = " << acc); |
||
323 |
} |
||
324 |
} |
||
325 |
#ifdef PROFILE |
||
326 |
watch.stop("test balance"); |
||
327 |
#endif |
||
328 |
if (status != LP_STATUS_OPTIMAL) { |
||
329 |
if (status == LP_STATUS_UNBOUNDED) |
||
330 |
hppDout(notice, "isStable : lp unbounded"); |
||
331 |
if (status == LP_STATUS_INFEASIBLE || status == LP_STATUS_UNBOUNDED) { |
||
332 |
// return 1.1; // completely arbitrary: TODO |
||
333 |
// hppDout(notice,"isStable LP infeasible"); |
||
334 |
return -1.1; // completely arbitrary: TODO |
||
335 |
} |
||
336 |
// hppDout(notice,"isStable error in LP"); |
||
337 |
return -std::numeric_limits<double>::max(); |
||
338 |
} |
||
339 |
hppDout(notice, "isStable LP successfully solved : robustness = " << res); |
||
340 |
/* centroidal_dynamics::MatrixXX Hrow; VectorX h; |
||
341 |
staticEquilibrium.getPolytopeInequalities(Hrow,h); |
||
342 |
MatrixXX H = -Hrow; |
||
343 |
H.rowwise().normalize(); |
||
344 |
int dimH = (int)(H.rows()); |
||
345 |
hppDout(notice,"Dim H rows : "<<dimH<<" ; col : "<<H.cols()); |
||
346 |
hppDout(notice,"H = "<<H); |
||
347 |
hppDout(notice,"h = "<<h);*/ |
||
348 |
return res; |
||
349 |
} |
||
350 |
} // namespace stability |
||
351 |
} // namespace rbprm |
||
352 |
} // namespace hpp |
| Generated by: GCOVR (Version 4.2) |