1 |
|
|
// |
2 |
|
|
// Copyright (c) 2017 CNRS |
3 |
|
|
// Authors: Steve Tonneau |
4 |
|
|
// |
5 |
|
|
// This file is part of hpp-rbprm |
6 |
|
|
// hpp-core 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-core 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 <hpp/constraints/relative-com.hh> |
20 |
|
|
#include <hpp/constraints/symbolic-calculus.hh> |
21 |
|
|
#include <hpp/constraints/symbolic-function.hh> |
22 |
|
|
#include <hpp/pinocchio/joint.hh> |
23 |
|
|
#include <hpp/rbprm/interpolation/interpolation-constraints.hh> |
24 |
|
|
#include <hpp/rbprm/projection/projection.hh> |
25 |
|
|
|
26 |
|
|
#ifdef PROFILE |
27 |
|
|
#include "hpp/rbprm/rbprm-profiler.hh" |
28 |
|
|
#endif |
29 |
|
|
|
30 |
|
|
namespace hpp { |
31 |
|
|
namespace rbprm { |
32 |
|
|
namespace projection { |
33 |
|
|
|
34 |
|
|
const double epsilon = 10e-4; |
35 |
|
|
|
36 |
|
|
ProjectionReport::ProjectionReport(const ProjectionReport& from) |
37 |
|
|
: success_(from.success_), result_(from.result_), status_(from.status_) { |
38 |
|
|
// NOTHING |
39 |
|
|
} |
40 |
|
|
|
41 |
|
102 |
std::vector<bool> setMaintainRotationConstraints() { |
42 |
|
102 |
std::vector<bool> res; |
43 |
✓✓✓✗
|
408 |
for (std::size_t i = 0; i < 3; ++i) res.push_back(true); |
44 |
|
102 |
return res; |
45 |
|
|
} |
46 |
|
|
|
47 |
|
102 |
void CreateContactConstraints(hpp::rbprm::RbPrmFullBodyPtr_t fullBody, |
48 |
|
|
const hpp::rbprm::State& currentState, |
49 |
|
|
core::ConfigProjectorPtr_t proj) { |
50 |
|
204 |
pinocchio::DevicePtr_t device = fullBody->device_; |
51 |
✓✗ |
204 |
std::vector<bool> cosntraintsR = setMaintainRotationConstraints(); |
52 |
✓✗ |
204 |
std::vector<std::string> fixed = currentState.fixedContacts(currentState); |
53 |
|
510 |
for (std::vector<std::string>::const_iterator cit = fixed.begin(); |
54 |
✓✓ |
918 |
cit != fixed.end(); ++cit) { |
55 |
|
408 |
const std::string& effector = *cit; |
56 |
✓✗ |
816 |
RbPrmLimbPtr_t limb = fullBody->GetLimbs().at(effector); |
57 |
✓✗ |
408 |
const fcl::Vec3f& ppos = currentState.contactPositions_.at(effector); |
58 |
|
|
const pinocchio::Frame effectorFrame = |
59 |
✓✗✓✗
|
816 |
device->getFrameByName(limb->effector_.name()); |
60 |
✓✗ |
816 |
pinocchio::JointPtr_t effectorJoint = effectorFrame.joint(); |
61 |
|
|
|
62 |
|
816 |
std::vector<bool> mask; |
63 |
✓✗ |
408 |
mask.push_back(true); |
64 |
✓✗ |
408 |
mask.push_back(true); |
65 |
✓✗ |
408 |
mask.push_back(true); |
66 |
✓✗✓✗
|
408 |
pinocchio::Transform3f localFrame(1), globalFrame(1); |
67 |
✓✗ |
408 |
globalFrame.translation(ppos); |
68 |
|
|
const constraints::DifferentiableFunctionPtr_t& function = |
69 |
✓✗✓✗
|
1224 |
constraints::Position::create( |
70 |
|
|
effector, device, effectorJoint, |
71 |
✓✗✓✗
|
816 |
effectorFrame.pinocchio().placement * localFrame, globalFrame, |
72 |
|
816 |
mask); |
73 |
|
408 |
constraints::ComparisonTypes_t comp(function->outputDerivativeSize(), |
74 |
✓✗ |
1224 |
constraints::EqualToZero); |
75 |
✓✗✓✗
✓✗ |
408 |
proj->add(constraints::Implicit::create(function, comp)); |
76 |
|
|
|
77 |
|
|
/*proj->add(constraints::Implicit::create ( |
78 |
|
|
constraints:::Position::create("",device, |
79 |
|
|
effectorJoint,fcl::Vec3f(0,0,0), |
80 |
|
|
ppos)));*/ |
81 |
✗✓ |
408 |
if (limb->contactType_ == hpp::rbprm::_6_DOF) { |
82 |
|
|
pinocchio::Transform3f rotation(1); |
83 |
|
|
rotation.rotation( |
84 |
|
|
currentState.contactRotation_.at(effector) * |
85 |
|
|
effectorFrame.pinocchio().placement.rotation().transpose()); |
86 |
|
|
const constraints::DifferentiableFunctionPtr_t& function_ = |
87 |
|
|
constraints::Orientation::create("", device, effectorJoint, rotation, |
88 |
|
|
cosntraintsR); |
89 |
|
|
constraints::ComparisonTypes_t comp_(function_->outputDerivativeSize(), |
90 |
|
|
constraints::EqualToZero); |
91 |
|
|
proj->add(constraints::Implicit::create(function_, comp_)); |
92 |
|
|
|
93 |
|
|
// const fcl::Matrix3f& rotation = |
94 |
|
|
// currentState.contactRotation_.at(effector); |
95 |
|
|
/*proj->add(constraints::Implicit::create |
96 |
|
|
(constraints::deprecated::Orientation::create("", device, |
97 |
|
|
effectorJoint, |
98 |
|
|
rotation, |
99 |
|
|
cosntraintsR)));*/ |
100 |
|
|
} |
101 |
|
|
} |
102 |
|
102 |
} |
103 |
|
|
|
104 |
|
|
void CreateRootPosConstraint(hpp::rbprm::RbPrmFullBodyPtr_t fullBody, |
105 |
|
|
const fcl::Vec3f& target, |
106 |
|
|
core::ConfigProjectorPtr_t proj) { |
107 |
|
|
pinocchio::Transform3f position(1); |
108 |
|
|
position.translation(target); |
109 |
|
|
const constraints::DifferentiableFunctionPtr_t& function = |
110 |
|
|
constraints::Position::create("", fullBody->device_, |
111 |
|
|
fullBody->device_->rootJoint(), |
112 |
|
|
pinocchio::Transform3f(1), position); |
113 |
|
|
constraints::ComparisonTypes_t comp(function->outputDerivativeSize(), |
114 |
|
|
constraints::EqualToZero); |
115 |
|
|
proj->add(constraints::Implicit::create(function, comp)); |
116 |
|
|
} |
117 |
|
|
|
118 |
|
|
typedef constraints::PointCom PointCom; |
119 |
|
|
typedef constraints::CalculusBaseAbstract<PointCom::ValueType_t, |
120 |
|
|
PointCom::JacobianType_t> |
121 |
|
|
s_t; |
122 |
|
|
typedef constraints::SymbolicFunction<s_t> PointComFunction; |
123 |
|
|
typedef constraints::SymbolicFunction<s_t>::Ptr_t PointComFunctionPtr_t; |
124 |
|
|
|
125 |
|
102 |
void CreateComPosConstraint(hpp::rbprm::RbPrmFullBodyPtr_t fullBody, |
126 |
|
|
const fcl::Vec3f& target, |
127 |
|
|
core::ConfigProjectorPtr_t proj) { |
128 |
|
204 |
pinocchio::DevicePtr_t device = fullBody->device_; |
129 |
|
|
pinocchio::CenterOfMassComputationPtr_t comComp = |
130 |
✓✗ |
204 |
pinocchio::CenterOfMassComputation::create(device); |
131 |
✓✗✓✗
|
102 |
comComp->add(device->rootJoint()); |
132 |
✓✗ |
102 |
comComp->compute(); |
133 |
|
|
PointComFunctionPtr_t comFunc = PointComFunction::create( |
134 |
✓✗✓✗
✓✗ |
306 |
"COM-constraint", device, PointCom::create(comComp)); |
135 |
✓✗ |
204 |
constraints::ComparisonTypes_t equals(3, constraints::Equality); |
136 |
|
|
constraints::ImplicitPtr_t comEq = |
137 |
✓✗✓✗
|
204 |
constraints::Implicit::create(comFunc, equals); |
138 |
✓✗ |
102 |
proj->add(comEq); |
139 |
✓✗✓✗
|
102 |
proj->rightHandSide(comEq, target); |
140 |
|
102 |
} |
141 |
|
|
|
142 |
|
|
void CreatePosturalTaskConstraint(hpp::rbprm::RbPrmFullBodyPtr_t fullBody, |
143 |
|
|
core::ConfigProjectorPtr_t proj) { |
144 |
|
|
// hppDout(notice,"create postural task, in projection.cc, ref config = |
145 |
|
|
// "<<pinocchio::displayConfig(fullBody->referenceConfig())); |
146 |
|
|
std::vector<bool> mask(fullBody->device_->numberDof(), false); |
147 |
|
|
// mask : 0 for the freeflyer and the extraDoFs : |
148 |
|
|
for (size_t i = 0; i < 3; i++) { |
149 |
|
|
mask[i] = false; |
150 |
|
|
} |
151 |
|
|
for (size_type i = fullBody->device_->numberDof() - 7; |
152 |
|
|
i < fullBody->device_->numberDof(); i++) { |
153 |
|
|
mask[i] = false; |
154 |
|
|
} |
155 |
|
|
|
156 |
|
|
/*for(size_t i = 3 ; i <= 9 ; i++){ |
157 |
|
|
mask[i] = true; |
158 |
|
|
}*/ |
159 |
|
|
// mask[5] = false; // z root rotation ???? |
160 |
|
|
|
161 |
|
|
Configuration_t weight(fullBody->device_->numberDof()); |
162 |
|
|
if (fullBody->postureWeights().size() == fullBody->device_->numberDof()) { |
163 |
|
|
weight = fullBody->postureWeights(); |
164 |
|
|
} else { |
165 |
|
|
for (size_type i = 0; i < fullBody->device_->numberDof(); ++i) |
166 |
|
|
weight[i] = 1.; |
167 |
|
|
} |
168 |
|
|
|
169 |
|
|
// normalize weight array : |
170 |
|
|
/*value_type moy =0; |
171 |
|
|
int num_active = 0; |
172 |
|
|
for (size_type i = 0 ; i < weight.size() ; i++){ |
173 |
|
|
if(mask[i]){ |
174 |
|
|
moy += weight[i]; |
175 |
|
|
num_active++; |
176 |
|
|
} |
177 |
|
|
} |
178 |
|
|
moy = moy/num_active; |
179 |
|
|
for (size_type i = 0 ; i < weight.size() ; i++) |
180 |
|
|
weight[i] = weight[i]/moy; |
181 |
|
|
|
182 |
|
|
*/ |
183 |
|
|
|
184 |
|
|
std::ostringstream oss; |
185 |
|
|
for (size_type i = 0; i < weight.size(); ++i) { |
186 |
|
|
oss << weight[i] << ","; |
187 |
|
|
} |
188 |
|
|
// hppDout(notice,"weight postural task = "<<oss.str()); |
189 |
|
|
|
190 |
|
|
// constraints::ConfigurationConstraintPtr_t postFunc = |
191 |
|
|
// constraints::ConfigurationConstraint::create("Postural_Task",fullBody->device_,fullBody->referenceConfig(),weight,mask); |
192 |
|
|
constraints::ConfigurationConstraintPtr_t postFunc = |
193 |
|
|
constraints::ConfigurationConstraint::create( |
194 |
|
|
"Postural_Task", fullBody->device_, fullBody->referenceConfig(), |
195 |
|
|
weight); |
196 |
|
|
ComparisonTypes_t comps; |
197 |
|
|
comps.push_back(constraints::Equality); |
198 |
|
|
const constraints::ImplicitPtr_t posturalTask = |
199 |
|
|
constraints::Implicit::create(postFunc, comps); |
200 |
|
|
proj->add(posturalTask, 1); |
201 |
|
|
// proj->updateRightHandSide(); |
202 |
|
|
} |
203 |
|
|
|
204 |
|
|
ProjectionReport projectToRootPosition(hpp::rbprm::RbPrmFullBodyPtr_t fullBody, |
205 |
|
|
const fcl::Vec3f& target, |
206 |
|
|
const hpp::rbprm::State& currentState) { |
207 |
|
|
ProjectionReport res; |
208 |
|
|
core::ConfigProjectorPtr_t proj = |
209 |
|
|
core::ConfigProjector::create(fullBody->device_, "proj", 1e-4, 100); |
210 |
|
|
CreateContactConstraints(fullBody, currentState, proj); |
211 |
|
|
CreateRootPosConstraint(fullBody, target, proj); |
212 |
|
|
pinocchio::Configuration_t configuration = currentState.configuration_; |
213 |
|
|
res.success_ = proj->apply(configuration); |
214 |
|
|
res.result_ = currentState; |
215 |
|
|
res.result_.configuration_ = configuration; |
216 |
|
|
return res; |
217 |
|
|
} |
218 |
|
|
|
219 |
|
|
typedef std::vector<pinocchio::JointPtr_t> T_Joint; |
220 |
|
|
|
221 |
|
|
T_Joint getJointsFromLimbs(const rbprm::T_Limb limbs) { |
222 |
|
|
T_Joint res; |
223 |
|
|
for (rbprm::CIT_Limb cit = limbs.begin(); cit != limbs.end(); ++cit) |
224 |
|
|
res.push_back(cit->second->limb_); |
225 |
|
|
return res; |
226 |
|
|
} |
227 |
|
|
|
228 |
|
|
bool not_a_limb(pinocchio::JointPtr_t cJoint, T_Joint limbs) { |
229 |
|
|
for (T_Joint::const_iterator cit = limbs.begin(); cit != limbs.end(); ++cit) { |
230 |
|
|
if (cJoint->name() == (*cit)->name()) return false; |
231 |
|
|
} |
232 |
|
|
return true; |
233 |
|
|
} |
234 |
|
|
|
235 |
|
|
void LockFromRootRec(pinocchio::JointPtr_t cJoint, |
236 |
|
|
const std::vector<pinocchio::JointPtr_t>& jointLimbs, |
237 |
|
|
pinocchio::ConfigurationIn_t targetRootConfiguration, |
238 |
|
|
core::ConfigProjectorPtr_t& projector) { |
239 |
|
|
if (not_a_limb(cJoint, jointLimbs)) { |
240 |
|
|
core::size_type rankInConfiguration = (cJoint->rankInConfiguration()); |
241 |
|
|
projector->add(core::LockedJoint::create( |
242 |
|
|
cJoint, LiegroupElement(targetRootConfiguration.segment( |
243 |
|
|
rankInConfiguration, cJoint->configSize()), |
244 |
|
|
cJoint->configurationSpace()))); |
245 |
|
|
// if (cJoint->numberChildJoints() !=1) |
246 |
|
|
// return; |
247 |
|
|
for (std::size_t i = 0; i < cJoint->numberChildJoints(); ++i) |
248 |
|
|
LockFromRootRec(cJoint->childJoint(i), jointLimbs, |
249 |
|
|
targetRootConfiguration, projector); |
250 |
|
|
} |
251 |
|
|
} |
252 |
|
|
|
253 |
|
|
void LockFromRoot(hpp::pinocchio::DevicePtr_t device, |
254 |
|
|
const rbprm::T_Limb& limbs, |
255 |
|
|
pinocchio::ConfigurationIn_t targetRootConfiguration, |
256 |
|
|
core::ConfigProjectorPtr_t& projector) { |
257 |
|
|
std::vector<pinocchio::JointPtr_t> jointLimbs = getJointsFromLimbs(limbs); |
258 |
|
|
pinocchio::JointPtr_t cJoint = device->rootJoint(); |
259 |
|
|
LockFromRootRec(cJoint, jointLimbs, targetRootConfiguration, projector); |
260 |
|
|
} |
261 |
|
|
|
262 |
|
|
ProjectionReport projectToRootConfiguration( |
263 |
|
|
hpp::rbprm::RbPrmFullBodyPtr_t fullBody, |
264 |
|
|
const pinocchio::ConfigurationIn_t conf, |
265 |
|
|
const hpp::rbprm::State& currentState, const Vector3 offset) { |
266 |
|
|
ProjectionReport res; |
267 |
|
|
core::ConfigProjectorPtr_t proj = |
268 |
|
|
core::ConfigProjector::create(fullBody->device_, "proj", 1e-4, 100); |
269 |
|
|
CreateContactConstraints(fullBody, currentState, proj); |
270 |
|
|
if (offset == Vector3::Zero()) |
271 |
|
|
LockFromRoot(fullBody->device_, fullBody->GetLimbs(), conf, proj); |
272 |
|
|
else { |
273 |
|
|
const std::string rootJointName("root_joint"); |
274 |
|
|
const fcl::Vec3f ppos = conf.head<3>(); |
275 |
|
|
const pinocchio::Frame effectorFrame = |
276 |
|
|
fullBody->device_->getFrameByName(rootJointName); |
277 |
|
|
pinocchio::JointPtr_t effectorJoint = effectorFrame.joint(); |
278 |
|
|
|
279 |
|
|
std::vector<bool> mask; |
280 |
|
|
mask.push_back(true); |
281 |
|
|
mask.push_back(true); |
282 |
|
|
mask.push_back(true); |
283 |
|
|
pinocchio::Transform3f localFrame(1), globalFrame(1); |
284 |
|
|
localFrame.translation(offset); |
285 |
|
|
globalFrame.translation(ppos); |
286 |
|
|
const constraints::DifferentiableFunctionPtr_t& function = |
287 |
|
|
constraints::Position::create( |
288 |
|
|
rootJointName, fullBody->device_, effectorJoint, |
289 |
|
|
effectorFrame.pinocchio().placement * localFrame, globalFrame, |
290 |
|
|
mask); |
291 |
|
|
constraints::ComparisonTypes_t comp(function->outputDerivativeSize(), |
292 |
|
|
constraints::EqualToZero); |
293 |
|
|
proj->add(constraints::Implicit::create(function, comp)); |
294 |
|
|
} |
295 |
|
|
pinocchio::Configuration_t configuration = currentState.configuration_; |
296 |
|
|
res.success_ = proj->apply(configuration); |
297 |
|
|
res.result_ = currentState; |
298 |
|
|
res.result_.configuration_ = configuration; |
299 |
|
|
return res; |
300 |
|
|
} |
301 |
|
|
|
302 |
|
|
ProjectionReport setCollisionFree( |
303 |
|
|
hpp::rbprm::RbPrmFullBodyPtr_t fullBody, |
304 |
|
|
const core::CollisionValidationPtr_t& validation, |
305 |
|
|
const std::string& limbName, const hpp::rbprm::State& currentState) { |
306 |
|
|
ProjectionReport res; |
307 |
|
|
res.result_ = currentState; |
308 |
|
|
pinocchio::Configuration_t configuration = currentState.configuration_; |
309 |
|
|
hpp::core::ValidationReportPtr_t valRep( |
310 |
|
|
new hpp::core::CollisionValidationReport); |
311 |
|
|
if (validation->validate(configuration, valRep)) { |
312 |
|
|
res.result_.configuration_ = configuration; |
313 |
|
|
res.success_ = true; |
314 |
|
|
hppDout(notice, |
315 |
|
|
"Found collision free conf : current configuration was already " |
316 |
|
|
"valid !"); |
317 |
|
|
return res; |
318 |
|
|
} |
319 |
|
|
|
320 |
|
|
RbPrmLimbPtr_t limb = fullBody->GetLimb(limbName); |
321 |
|
|
sampling::T_Sample& samples = limb->sampleContainer_.samples_; |
322 |
|
|
for (sampling::SampleVector_t::const_iterator cit = samples.begin(); |
323 |
|
|
cit != samples.end(); ++cit) { |
324 |
|
|
sampling::Load(*cit, configuration); |
325 |
|
|
hppDout(notice, |
326 |
|
|
"Set collision free : static value = " << cit->staticValue_); |
327 |
|
|
if (validation->validate(configuration, valRep)) { |
328 |
|
|
res.result_.configuration_ = configuration; |
329 |
|
|
res.success_ = true; |
330 |
|
|
hppDout(notice, "Found collision free conf !"); |
331 |
|
|
return res; |
332 |
|
|
} |
333 |
|
|
} |
334 |
|
|
return res; |
335 |
|
|
} |
336 |
|
|
|
337 |
|
|
std::vector<bool> setRotationConstraints() { |
338 |
|
|
std::vector<bool> res; |
339 |
|
|
for (std::size_t i = 0; i < 3; ++i) { |
340 |
|
|
res.push_back(true); |
341 |
|
|
} |
342 |
|
|
return res; |
343 |
|
|
} |
344 |
|
|
|
345 |
|
|
std::vector<bool> setTranslationConstraints() { |
346 |
|
|
std::vector<bool> res; |
347 |
|
|
for (std::size_t i = 0; i < 3; ++i) { |
348 |
|
|
res.push_back(true); |
349 |
|
|
} |
350 |
|
|
return res; |
351 |
|
|
} |
352 |
|
|
ProjectionReport projectEffector(hpp::core::ConfigProjectorPtr_t proj, |
353 |
|
|
const hpp::rbprm::RbPrmFullBodyPtr_t& body, |
354 |
|
|
const std::string& limbId, |
355 |
|
|
const hpp::rbprm::RbPrmLimbPtr_t& limb, |
356 |
|
|
core::CollisionValidationPtr_t validation, |
357 |
|
|
pinocchio::ConfigurationOut_t configuration, |
358 |
|
|
const fcl::Matrix3f& rotationTarget, |
359 |
|
|
std::vector<bool> rotationFilter, |
360 |
|
|
const fcl::Vec3f& positionTarget, |
361 |
|
|
const fcl::Vec3f& normal, |
362 |
|
|
const hpp::rbprm::State& current) { |
363 |
|
|
// hppDout(notice,"Project effector : "); |
364 |
|
|
ProjectionReport rep; |
365 |
|
|
// Add constraints to resolve Ik |
366 |
|
|
rep.success_ = false; |
367 |
|
|
rep.result_ = current; |
368 |
|
|
// Add constraints to resolve Ik |
369 |
|
|
hppDout(notice, "Project effector to position : " << positionTarget); |
370 |
|
|
if (body->usePosturalTaskContactCreation()) rotationFilter[2] = false; |
371 |
|
|
|
372 |
|
|
const pinocchio::Frame effectorFrame = |
373 |
|
|
body->device_->getFrameByName(limb->effector_.name()); |
374 |
|
|
pinocchio::JointPtr_t effectorJoint = effectorFrame.joint(); |
375 |
|
|
Transform3f localFrame(1), globalFrame(1); |
376 |
|
|
localFrame = effectorFrame.pinocchio().placement * localFrame; |
377 |
|
|
globalFrame.translation(positionTarget); |
378 |
|
|
const constraints::DifferentiableFunctionPtr_t& function = |
379 |
|
|
constraints::Position::create("", body->device_, effectorJoint, |
380 |
|
|
localFrame, globalFrame, |
381 |
|
|
setTranslationConstraints()); |
382 |
|
|
constraints::ComparisonTypes_t comp(function->outputDerivativeSize(), |
383 |
|
|
constraints::EqualToZero); |
384 |
|
|
proj->add(constraints::Implicit::create(function, comp)); |
385 |
|
|
if (limb->contactType_ == hpp::rbprm::_6_DOF) { |
386 |
|
|
// localFrame.rotation(effectorFrame.pinocchio().placement.rotation() * |
387 |
|
|
// rotationTarget.transpose()); |
388 |
|
|
globalFrame.rotation(rotationTarget); |
389 |
|
|
const constraints::DifferentiableFunctionPtr_t& function_ = |
390 |
|
|
constraints::Orientation::create("", body->device_, effectorJoint, |
391 |
|
|
localFrame, globalFrame, |
392 |
|
|
rotationFilter); |
393 |
|
|
constraints::ComparisonTypes_t comp_(function_->outputDerivativeSize(), |
394 |
|
|
constraints::EqualToZero); |
395 |
|
|
proj->add(constraints::Implicit::create(function_, comp_)); |
396 |
|
|
} |
397 |
|
|
|
398 |
|
|
if (body->usePosturalTaskContactCreation()) { |
399 |
|
|
CreatePosturalTaskConstraint(body, proj); |
400 |
|
|
proj->errorThreshold(1e-3); |
401 |
|
|
proj->maxIterations(1000); |
402 |
|
|
proj->lastIsOptional(true); |
403 |
|
|
} |
404 |
|
|
|
405 |
|
|
#ifdef PROFILE |
406 |
|
|
RbPrmProfiler& watch = getRbPrmProfiler(); |
407 |
|
|
watch.start("ik"); |
408 |
|
|
#endif |
409 |
|
|
if (proj->apply(configuration)) { |
410 |
|
|
hppDout(notice, "apply contact constraints"); |
411 |
|
|
#ifdef PROFILE |
412 |
|
|
watch.stop("ik"); |
413 |
|
|
#endif |
414 |
|
|
#ifdef PROFILE |
415 |
|
|
RbPrmProfiler& watch = getRbPrmProfiler(); |
416 |
|
|
watch.start("collision"); |
417 |
|
|
#endif |
418 |
|
|
hpp::core::ValidationReportPtr_t valRep( |
419 |
|
|
new hpp::core::CollisionValidationReport); |
420 |
|
|
if (validation->validate(configuration, valRep)) { |
421 |
|
|
hppDout(notice, "No collision !"); |
422 |
|
|
#ifdef PROFILE |
423 |
|
|
watch.stop("collision"); |
424 |
|
|
#endif |
425 |
|
|
hppDout(notice, "Projection successfull, add new contact info :"); |
426 |
|
|
body->device_->currentConfiguration(configuration); |
427 |
|
|
body->device_->computeForwardKinematics(); |
428 |
|
|
State tmp(current); |
429 |
|
|
tmp.contacts_[limbId] = true; |
430 |
|
|
tmp.contactPositions_[limbId] = |
431 |
|
|
limb->effector_.currentTransformation().translation(); |
432 |
|
|
tmp.contactRotation_[limbId] = |
433 |
|
|
limb->effector_.currentTransformation().rotation(); |
434 |
|
|
tmp.contactNormals_[limbId] = normal; |
435 |
|
|
tmp.contactOrder_.push(limbId); |
436 |
|
|
tmp.configuration_ = configuration; |
437 |
|
|
++tmp.nbContacts; |
438 |
|
|
rep.success_ = true; |
439 |
|
|
rep.result_ = tmp; |
440 |
|
|
} else { |
441 |
|
|
#ifdef PROFILE |
442 |
|
|
watch.stop("collision"); |
443 |
|
|
#endif |
444 |
|
|
hppDout(notice, |
445 |
|
|
"state in collison after projection, report : " << *valRep); |
446 |
|
|
hppDout(notice, "state in collision : r([" |
447 |
|
|
<< pinocchio::displayConfig(configuration) << "])"); |
448 |
|
|
} |
449 |
|
|
} else { |
450 |
|
|
#ifdef PROFILE |
451 |
|
|
watch.stop("ik"); |
452 |
|
|
#endif |
453 |
|
|
hppDout(notice, "unable to apply contact constraints"); |
454 |
|
|
} |
455 |
|
|
return rep; |
456 |
|
|
} |
457 |
|
|
|
458 |
|
|
fcl::Transform3f computeProjectionMatrix( |
459 |
|
|
const hpp::rbprm::RbPrmFullBodyPtr_t& body, |
460 |
|
|
const hpp::rbprm::RbPrmLimbPtr_t& limb, |
461 |
|
|
const pinocchio::ConfigurationIn_t configuration, const fcl::Vec3f& normal, |
462 |
|
|
const fcl::Vec3f& position, const fcl::Matrix3f& rotation) { |
463 |
|
|
// hppDout(notice,"computeProjection matrice : normal = |
464 |
|
|
// "<<normal.transpose()); |
465 |
|
|
body->device_->currentConfiguration(configuration); |
466 |
|
|
body->device_->computeForwardKinematics(); |
467 |
|
|
// the normal is given by the normal of the contacted object |
468 |
|
|
// hppDout(notice,"effector rot : |
469 |
|
|
// \n"<<limb->effector_.currentTransformation().rotation()); |
470 |
|
|
// hppDout(notice,"limb normal : "<<limb->normal_.transpose()); |
471 |
|
|
const fcl::Vec3f z = |
472 |
|
|
limb->effector_.currentTransformation().rotation() * limb->normal_; |
473 |
|
|
fcl::Matrix3f rot; |
474 |
|
|
hppDout(notice, "in computeProjectionMatrix, desired rotation = \n" |
475 |
|
|
<< rotation); |
476 |
|
|
if (rotation.isZero(0)) { |
477 |
|
|
// hppDout(notice,"z = "<<z.transpose()); |
478 |
|
|
const fcl::Matrix3f alignRotation = tools::GetRotationMatrix(z, normal); |
479 |
|
|
// hppDout(notice,"alignRotation : \n"<<alignRotation); |
480 |
|
|
rot = alignRotation * limb->effector_.currentTransformation().rotation(); |
481 |
|
|
} else { |
482 |
|
|
rot = rotation; |
483 |
|
|
} |
484 |
|
|
// hppDout(notice,"rotation : \n"<<rotation); |
485 |
|
|
fcl::Vec3f posOffset = position - rot * limb->offset_; |
486 |
|
|
posOffset = posOffset + normal * epsilon; |
487 |
|
|
hppDout(notice, "in computeProjectionMatrix, rotation used = \n" << rot); |
488 |
|
|
return fcl::Transform3f(rot, posOffset); |
489 |
|
|
} |
490 |
|
|
|
491 |
|
|
ProjectionReport projectToObstacle( |
492 |
|
|
core::ConfigProjectorPtr_t proj, const hpp::rbprm::RbPrmFullBodyPtr_t& body, |
493 |
|
|
const std::string& limbId, const hpp::rbprm::RbPrmLimbPtr_t& limb, |
494 |
|
|
core::CollisionValidationPtr_t validation, |
495 |
|
|
pinocchio::ConfigurationOut_t configuration, |
496 |
|
|
const hpp::rbprm::State& current, const fcl::Vec3f& normal, |
497 |
|
|
const fcl::Vec3f& position, |
498 |
|
|
const fcl::Matrix3f& rotation = fcl::Matrix3f::Zero()) { |
499 |
|
|
fcl::Transform3f pM = computeProjectionMatrix(body, limb, configuration, |
500 |
|
|
normal, position, rotation); |
501 |
|
|
return projectEffector(proj, body, limbId, limb, validation, configuration, |
502 |
|
|
pM.getRotation(), setRotationConstraints(), |
503 |
|
|
pM.getTranslation(), normal, current); |
504 |
|
|
} |
505 |
|
|
|
506 |
|
|
// are p1 and p2 on the same side of the line AB ? |
507 |
|
|
bool SameSide(const fcl::Vec3f& p1, const fcl::Vec3f& p2, const fcl::Vec3f& a, |
508 |
|
|
const fcl::Vec3f& b) { |
509 |
|
|
fcl::Vec3f cp1 = (b - a).cross(p1 - a); |
510 |
|
|
fcl::Vec3f cp2 = (b - a).cross(p2 - a); |
511 |
|
|
return cp1.dot(cp2) >= 0; |
512 |
|
|
} |
513 |
|
|
|
514 |
|
|
// is p inside ABC ? |
515 |
|
|
bool PointInTriangle(const fcl::Vec3f& p, const fcl::Vec3f& a, |
516 |
|
|
const fcl::Vec3f& b, const fcl::Vec3f& c) { |
517 |
|
|
return SameSide(p, a, b, c) && SameSide(p, b, a, c) && SameSide(p, c, a, b); |
518 |
|
|
} |
519 |
|
|
|
520 |
|
|
double clamp(const double& val, const double& lo, const double& hi) { |
521 |
|
|
return std::min(std::max(val, lo), hi); |
522 |
|
|
} |
523 |
|
|
|
524 |
|
|
fcl::Vec3f closestPointInTriangle(const fcl::Vec3f& sourcePosition, |
525 |
|
|
const fcl::Vec3f& t0, const fcl::Vec3f& t1, |
526 |
|
|
const fcl::Vec3f& t2, |
527 |
|
|
const double epsilon = 0.) { |
528 |
|
|
hppDout(notice, "closestPointInTriangle : t0 = " |
529 |
|
|
<< t0.transpose() << " ; t1 = " << t1.transpose() |
530 |
|
|
<< " ; t2 = " << t2.transpose()); |
531 |
|
|
const fcl::Vec3f edge0 = t1 - t0; |
532 |
|
|
const fcl::Vec3f edge1 = t2 - t0; |
533 |
|
|
const fcl::Vec3f v0 = t0 - sourcePosition; |
534 |
|
|
|
535 |
|
|
double a = edge0.dot(edge0); |
536 |
|
|
double b = edge0.dot(edge1); |
537 |
|
|
double c = edge1.dot(edge1); |
538 |
|
|
double d = edge0.dot(v0); |
539 |
|
|
double e = edge1.dot(v0); |
540 |
|
|
|
541 |
|
|
double det = a * c - b * b; |
542 |
|
|
double s = b * e - c * d; |
543 |
|
|
double t = b * d - a * e; |
544 |
|
|
|
545 |
|
|
if (s + t < det) { |
546 |
|
|
if (s < 0.f) { |
547 |
|
|
if (t < 0.f) { |
548 |
|
|
if (d < 0.f) { |
549 |
|
|
s = clamp(-d / a, 0.f, 1.f); |
550 |
|
|
t = 0.f; |
551 |
|
|
} else { |
552 |
|
|
s = 0.f; |
553 |
|
|
t = clamp(-e / c, 0.f, 1.f); |
554 |
|
|
} |
555 |
|
|
} else { |
556 |
|
|
s = 0.f; |
557 |
|
|
t = clamp(-e / c, 0.f, 1.f); |
558 |
|
|
} |
559 |
|
|
} else if (t < 0.f) { |
560 |
|
|
s = clamp(-d / a, 0.f, 1.f); |
561 |
|
|
t = 0.f; |
562 |
|
|
} else { |
563 |
|
|
double invDet = 1.f / det; |
564 |
|
|
s *= invDet; |
565 |
|
|
t *= invDet; |
566 |
|
|
} |
567 |
|
|
} else { |
568 |
|
|
if (s < 0.f) { |
569 |
|
|
double tmp0 = b + d; |
570 |
|
|
double tmp1 = c + e; |
571 |
|
|
if (tmp1 > tmp0) { |
572 |
|
|
double numer = tmp1 - tmp0; |
573 |
|
|
double denom = a - 2 * b + c; |
574 |
|
|
s = clamp(numer / denom, 0.f, 1.f); |
575 |
|
|
t = 1 - s; |
576 |
|
|
} else { |
577 |
|
|
t = clamp(-e / c, 0.f, 1.f); |
578 |
|
|
s = 0.f; |
579 |
|
|
} |
580 |
|
|
} else if (t < 0.f) { |
581 |
|
|
if (a + d > b + e) { |
582 |
|
|
double numer = c + e - b - d; |
583 |
|
|
double denom = a - 2 * b + c; |
584 |
|
|
s = clamp(numer / denom, 0.f, 1.f); |
585 |
|
|
t = 1 - s; |
586 |
|
|
} else { |
587 |
|
|
s = clamp(-e / c, 0.f, 1.f); |
588 |
|
|
t = 0.f; |
589 |
|
|
} |
590 |
|
|
} else { |
591 |
|
|
double numer = c + e - b - d; |
592 |
|
|
double denom = a - 2 * b + c; |
593 |
|
|
s = clamp(numer / denom, 0.f, 1.f); |
594 |
|
|
t = 1.f - s; |
595 |
|
|
} |
596 |
|
|
} |
597 |
|
|
|
598 |
|
|
fcl::Vec3f res = t0 + s * edge0 + t * edge1; |
599 |
|
|
return res + (res - sourcePosition).normalized() * epsilon; |
600 |
|
|
} |
601 |
|
|
|
602 |
|
|
ProjectionReport projectSampleToObstacle( |
603 |
|
|
const hpp::rbprm::RbPrmFullBodyPtr_t& body, const std::string& limbId, |
604 |
|
|
const hpp::rbprm::RbPrmLimbPtr_t& limb, |
605 |
|
|
const sampling::OctreeReport& report, |
606 |
|
|
core::CollisionValidationPtr_t validation, |
607 |
|
|
pinocchio::ConfigurationOut_t configuration, |
608 |
|
|
const hpp::rbprm::State& current) { |
609 |
|
|
sampling::Load(*report.sample_, configuration); |
610 |
|
|
fcl::Vec3f normal = report.normal_; |
611 |
|
|
normal.normalize(); |
612 |
|
|
// value_type epsilon = 0.01; |
613 |
|
|
// hppDout(notice,"contact normal = "<<normal); |
614 |
|
|
Transform3f rootT; |
615 |
|
|
if (body->GetLimb(limbId)->limb_->parentJoint()) |
616 |
|
|
rootT = |
617 |
|
|
body->GetLimb(limbId)->limb_->parentJoint()->currentTransformation(); |
618 |
|
|
else |
619 |
|
|
rootT = body->GetLimb(limbId)->limb_->currentTransformation(); |
620 |
|
|
// compute the orthogonal projection of the end effector on the plan : |
621 |
|
|
const fcl::Vec3f pEndEff = (rootT.act( |
622 |
|
|
report.sample_ |
623 |
|
|
->effectorPosition_)); // compute absolute position (in world frame) |
624 |
|
|
fcl::Vec3f pos = |
625 |
|
|
pEndEff - (normal.dot(pEndEff - report.v1_)) * |
626 |
|
|
normal; // orthogonal projection on the obstacle surface |
627 |
|
|
hppDout(notice, |
628 |
|
|
"project sample to obstacle : orthogonal projection = " << pos); |
629 |
|
|
// make sure contact pos is actually on triangle, and take 1 cm margin ... |
630 |
|
|
// hppDout(notice,"projectSampleToObstacle, pos = |
631 |
|
|
// "<<pos.transpose()); |
632 |
|
|
pos = closestPointInTriangle(pEndEff, report.v1_, report.v2_, report.v3_, 0.); |
633 |
|
|
hppDout( |
634 |
|
|
notice, |
635 |
|
|
"project sample to obstacle : after project inside triangle = " << pos); |
636 |
|
|
// pos += normal*epsilon; |
637 |
|
|
// hppDout(notice,"project sample to obstacle : after epsilon = "<<pos); |
638 |
|
|
// hppDout(notice,"projectSampleToObstacle, pos after projection in triangle = |
639 |
|
|
// "<<pos.transpose()); hppDout(notice,"Effector position : |
640 |
|
|
// "<<report.sample_->effectorPosition_); hppDout(notice,"pEndEff = |
641 |
|
|
// ["<<pEndEff[0]<<","<<pEndEff[1]<<","<<pEndEff[2]<<"]"); hppDout(notice,"pos |
642 |
|
|
// = ["<<pos[0]<<","<<pos[1]<<","<<pos[2]<<"]"); |
643 |
|
|
core::ConfigProjectorPtr_t proj = |
644 |
|
|
core::ConfigProjector::create(body->device_, "proj", 1e-4, 100); |
645 |
|
|
hpp::tools::LockJointRec(limb->limb_->name(), body->device_->rootJoint(), |
646 |
|
|
proj); |
647 |
|
|
return projectToObstacle(proj, body, limbId, limb, validation, configuration, |
648 |
|
|
current, normal, pos); |
649 |
|
|
} |
650 |
|
|
|
651 |
|
|
ProjectionReport projectStateToObstacle( |
652 |
|
|
const hpp::rbprm::RbPrmFullBodyPtr_t& body, const std::string& limbId, |
653 |
|
|
const hpp::rbprm::RbPrmLimbPtr_t& limb, const hpp::rbprm::State& current, |
654 |
|
|
const fcl::Vec3f& normal, const fcl::Vec3f& position, bool lockOtherJoints, |
655 |
|
|
const fcl::Matrix3f& rotation) { |
656 |
|
|
// core::CollisionValidationPtr_t dummy = |
657 |
|
|
// core::CollisionValidation::create(body->device_); |
658 |
|
|
return projectStateToObstacle(body, limbId, limb, current, normal, position, |
659 |
|
|
body->GetCollisionValidation(), lockOtherJoints, |
660 |
|
|
rotation); |
661 |
|
|
} |
662 |
|
|
|
663 |
|
|
ProjectionReport projectStateToObstacle( |
664 |
|
|
const hpp::rbprm::RbPrmFullBodyPtr_t& body, const std::string& limbId, |
665 |
|
|
const hpp::rbprm::RbPrmLimbPtr_t& limb, const hpp::rbprm::State& current, |
666 |
|
|
const fcl::Vec3f& normal, const fcl::Vec3f& position, |
667 |
|
|
core::CollisionValidationPtr_t validation, bool lockOtherJoints, |
668 |
|
|
const fcl::Matrix3f& rotation) { |
669 |
|
|
hpp::rbprm::State state = current; |
670 |
|
|
state.RemoveContact(limbId); |
671 |
|
|
pinocchio::Configuration_t configuration = current.configuration_; |
672 |
|
|
core::ConfigProjectorPtr_t proj = |
673 |
|
|
core::ConfigProjector::create(body->device_, "proj", 1e-4, 1000); |
674 |
|
|
interpolation::addContactConstraints(body, body->device_, proj, state, |
675 |
|
|
state.fixedContacts(state)); |
676 |
|
|
if (lockOtherJoints) { // lock all joints expect the ones of the moving limb |
677 |
|
|
hpp::tools::LockJointRec(limb->limb_->name(), body->device_->rootJoint(), |
678 |
|
|
proj); |
679 |
|
|
} |
680 |
|
|
// get current normal orientation |
681 |
|
|
return projectToObstacle(proj, body, limbId, limb, validation, configuration, |
682 |
|
|
state, normal, position, rotation); |
683 |
|
|
} |
684 |
|
|
|
685 |
|
102 |
ProjectionReport projectToComPosition(hpp::rbprm::RbPrmFullBodyPtr_t fullBody, |
686 |
|
|
const fcl::Vec3f& target, |
687 |
|
|
const hpp::rbprm::State& currentState) { |
688 |
✓✗ |
102 |
ProjectionReport res; |
689 |
|
|
core::ConfigProjectorPtr_t proj = |
690 |
✓✗✓✗
|
306 |
core::ConfigProjector::create(fullBody->device_, "proj", 1e-4, 1000); |
691 |
✓✗ |
102 |
CreateContactConstraints(fullBody, currentState, proj); |
692 |
✓✗ |
102 |
CreateComPosConstraint(fullBody, target, proj); |
693 |
|
|
/* CreatePosturalTaskConstraint(fullBody,proj); |
694 |
|
|
proj->lastIsOptional(true); |
695 |
|
|
proj->numOptimize(500); |
696 |
|
|
proj->lastAsCost(false); |
697 |
|
|
proj->errorThreshold(1e-3);*/ |
698 |
|
|
|
699 |
✓✗ |
204 |
pinocchio::Configuration_t configuration = currentState.configuration_; |
700 |
✓✗✓✗
|
102 |
res.success_ = proj->apply(configuration); |
701 |
✓✗ |
102 |
res.result_ = currentState; |
702 |
✓✗ |
102 |
res.result_.configuration_ = configuration; |
703 |
|
204 |
return res; |
704 |
|
|
} |
705 |
|
|
|
706 |
|
|
std::vector<std::string> extractEffectorsName(const rbprm::T_Limb& limbs) { |
707 |
|
|
std::vector<std::string> res; |
708 |
|
|
for (rbprm::T_Limb::const_iterator cit = limbs.begin(); cit != limbs.end(); |
709 |
|
|
++cit) |
710 |
|
|
res.push_back(cit->first); |
711 |
|
|
return res; |
712 |
|
|
} |
713 |
|
|
|
714 |
|
|
ProjectionReport projectToColFreeComPosition( |
715 |
|
|
hpp::rbprm::RbPrmFullBodyPtr_t fullBody, const fcl::Vec3f& target, |
716 |
|
|
const hpp::rbprm::State& currentState) { |
717 |
|
|
ProjectionReport res, tmp; |
718 |
|
|
core::ConfigProjectorPtr_t proj = |
719 |
|
|
core::ConfigProjector::create(fullBody->device_, "proj", 1e-3, 1000); |
720 |
|
|
CreateContactConstraints(fullBody, currentState, proj); |
721 |
|
|
CreateComPosConstraint(fullBody, target, proj); |
722 |
|
|
CreatePosturalTaskConstraint(fullBody, proj); |
723 |
|
|
proj->lastIsOptional(true); |
724 |
|
|
// proj->numOptimize(500); |
725 |
|
|
proj->maxIterations(500); |
726 |
|
|
// proj->lastAsCost(true); |
727 |
|
|
proj->errorThreshold(1e-2); |
728 |
|
|
|
729 |
|
|
pinocchio::Configuration_t configuration = currentState.configuration_; |
730 |
|
|
res.success_ = proj->apply(configuration); |
731 |
|
|
res.result_ = currentState; |
732 |
|
|
res.result_.configuration_ = configuration; |
733 |
|
|
hppDout(notice, |
734 |
|
|
"Project to col free, first projection done : " << res.success_); |
735 |
|
|
hppDout(notice, |
736 |
|
|
"projected state : " << pinocchio::displayConfig(configuration)); |
737 |
|
|
if (res.success_) { |
738 |
|
|
std::vector<std::string> effNames( |
739 |
|
|
extractEffectorsName(fullBody->GetLimbs())); |
740 |
|
|
std::vector<std::string> freeLimbs = |
741 |
|
|
rbprm::freeEffectors(currentState, effNames.begin(), effNames.end()); |
742 |
|
|
for (std::vector<std::string>::const_iterator cit = freeLimbs.begin(); |
743 |
|
|
cit != freeLimbs.end() && res.success_; ++cit) { |
744 |
|
|
hppDout(notice, "free effector in projection : " << *cit); |
745 |
|
|
tmp = projection::setCollisionFree( |
746 |
|
|
fullBody, fullBody->GetLimbCollisionValidation().at(*cit), *cit, |
747 |
|
|
res.result_); |
748 |
|
|
if (!tmp.success_) res.success_ = false; |
749 |
|
|
} |
750 |
|
|
} |
751 |
|
|
hppDout(notice, |
752 |
|
|
"project to col free, set coll free success = " << res.success_); |
753 |
|
|
if (res.success_) { |
754 |
|
|
res.success_ = proj->apply(configuration); |
755 |
|
|
res.result_.configuration_ = configuration; |
756 |
|
|
if (res.success_) { |
757 |
|
|
ValidationReportPtr_t report( |
758 |
|
|
ValidationReportPtr_t(new CollisionValidationReport)); |
759 |
|
|
res.success_ = |
760 |
|
|
fullBody->GetCollisionValidation()->validate(configuration, report); |
761 |
|
|
hppDout(notice, "project to col free, collision test : " << res.success_); |
762 |
|
|
if (!res.success_) { |
763 |
|
|
CollisionValidationReportPtr_t repCast = |
764 |
|
|
dynamic_pointer_cast<CollisionValidationReport>(report); |
765 |
|
|
hppDout(notice, "collision between " << repCast->object1->name() |
766 |
|
|
<< " and " |
767 |
|
|
<< repCast->object2->name()); |
768 |
|
|
} |
769 |
|
|
} |
770 |
|
|
} |
771 |
|
|
return res; |
772 |
|
|
} |
773 |
|
|
|
774 |
|
|
} // namespace projection |
775 |
|
|
} // namespace rbprm |
776 |
|
|
} // namespace hpp |