1 |
|
|
// Copyright (c) 2014, LAAS-CNRS |
2 |
|
|
// Authors: Steve Tonneau (steve.tonneau@laas.fr) |
3 |
|
|
// |
4 |
|
|
// This file is part of hpp-rbprm. |
5 |
|
|
// hpp-rbprm 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 |
|
|
// |
10 |
|
|
// hpp-rbprm is distributed in the hope that it will be |
11 |
|
|
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty |
12 |
|
|
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU |
13 |
|
|
// General Lesser Public License for more details. You should have |
14 |
|
|
// received a copy of the GNU Lesser General Public License along with |
15 |
|
|
// hpp-rbprm. If not, see <http://www.gnu.org/licenses/>. |
16 |
|
|
|
17 |
|
|
#include <hpp/fcl/BVH/BVH_model.h> |
18 |
|
|
|
19 |
|
|
#include <hpp/constraints/generic-transformation.hh> |
20 |
|
|
#include <hpp/constraints/locked-joint.hh> |
21 |
|
|
#include <hpp/core/collision-validation.hh> |
22 |
|
|
#include <hpp/core/config-projector.hh> |
23 |
|
|
#include <hpp/core/constraint-set.hh> |
24 |
|
|
#include <hpp/pinocchio/configuration.hh> |
25 |
|
|
#include <hpp/pinocchio/device.hh> |
26 |
|
|
#include <hpp/pinocchio/joint.hh> |
27 |
|
|
#include <hpp/rbprm/contact_generation/algorithm.hh> |
28 |
|
|
#include <hpp/rbprm/contact_generation/contact_generation.hh> |
29 |
|
|
#include <hpp/rbprm/projection/projection.hh> |
30 |
|
|
#include <hpp/rbprm/rbprm-fullbody.hh> |
31 |
|
|
#include <hpp/rbprm/stability/stability.hh> |
32 |
|
|
#include <hpp/rbprm/tools.hh> |
33 |
|
|
#include <stack> |
34 |
|
|
|
35 |
|
|
#ifdef PROFILE |
36 |
|
|
#include "hpp/rbprm/rbprm-profiler.hh" |
37 |
|
|
#endif |
38 |
|
|
|
39 |
|
|
using namespace hpp::rbprm::projection; |
40 |
|
|
|
41 |
|
|
namespace hpp { |
42 |
|
|
namespace rbprm { |
43 |
|
|
|
44 |
|
|
const double epsilon = 10e-3; |
45 |
|
|
|
46 |
|
4 |
RbPrmFullBodyPtr_t RbPrmFullBody::create(const pinocchio::DevicePtr_t& device) { |
47 |
✓✗ |
4 |
RbPrmFullBody* fullBody = new RbPrmFullBody(device); |
48 |
|
4 |
RbPrmFullBodyPtr_t res(fullBody); |
49 |
✓✗ |
4 |
res->init(res); |
50 |
|
4 |
return res; |
51 |
|
|
} |
52 |
|
|
|
53 |
|
16 |
RbPrmFullBody::~RbPrmFullBody() { |
54 |
|
|
// NOTHING |
55 |
|
16 |
} |
56 |
|
|
|
57 |
|
|
bool RbPrmFullBody::AddHeuristic(const std::string& name, |
58 |
|
|
const sampling::heuristic func) { |
59 |
|
|
return factory_.AddHeuristic(name, func); |
60 |
|
|
} |
61 |
|
|
|
62 |
|
10 |
void RbPrmFullBody::AddLimbPrivate( |
63 |
|
|
rbprm::RbPrmLimbPtr_t limb, const std::string& id, const std::string& name, |
64 |
|
|
const hpp::core::ObjectStdVector_t& collisionObjects, |
65 |
|
|
const bool disableEffectorCollision, const bool nonContactingLimb) { |
66 |
|
|
core::CollisionValidationPtr_t limbcollisionValidation_ = |
67 |
✓✗ |
20 |
core::CollisionValidation::create(this->device_); |
68 |
|
20 |
rbprm::T_Limb limbs; |
69 |
✗✓ |
10 |
if (nonContactingLimb) |
70 |
|
|
limbs = nonContactingLimbs_; |
71 |
|
|
else |
72 |
✓✗ |
10 |
limbs = limbs_; |
73 |
|
|
// pinocchio::JointPtr_t effectorJoint (new |
74 |
|
|
// pinocchio::Joint(limb->effector_.joint())); |
75 |
✓✗ |
10 |
hpp::tools::addLimbCollisionRec<hpp::core::CollisionValidation>( |
76 |
|
10 |
limb->limb_, limb->effector_, collisionObjects, |
77 |
|
10 |
(*limbcollisionValidation_.get()), disableEffectorCollision); |
78 |
✓✓ |
10 |
if (limbs.empty()) { |
79 |
✓✗ |
4 |
hpp::tools::addLimbCollisionRec<hpp::core::CollisionValidation>( |
80 |
✓✗ |
8 |
device_->rootJoint(), limb->effector_, collisionObjects, |
81 |
|
4 |
(*collisionValidation_.get()), disableEffectorCollision); |
82 |
|
|
} |
83 |
|
|
// adding collision validation |
84 |
|
|
/*for(hpp::core::ObjectStdVector_t::const_iterator cit = |
85 |
|
|
collisionObjects.begin(); cit != collisionObjects.end(); ++cit) |
86 |
|
|
{ |
87 |
|
|
if(limbs.empty()) |
88 |
|
|
{ |
89 |
|
|
collisionValidation_->addObstacle(*cit); |
90 |
|
|
} |
91 |
|
|
std::cout << "adding obstacle to limb validation " <<(*cit)->name() << |
92 |
|
|
std::endl; limbcollisionValidation_->addObstacle(*cit); |
93 |
|
|
//remove effector collision |
94 |
|
|
if(disableEffectorCollision) |
95 |
|
|
{ |
96 |
|
|
hpp::tools::RemoveEffectorCollision<hpp::core::CollisionValidation>((*collisionValidation_.get()), |
97 |
|
|
limb->effector_, *cit); |
98 |
|
|
hpp::tools::RemoveEffectorCollision<hpp::core::CollisionValidation>((*limbcollisionValidation_.get()), |
99 |
|
|
limb->effector_, *cit); |
100 |
|
|
} |
101 |
|
|
}*/ |
102 |
✗✓ |
10 |
if (nonContactingLimb) |
103 |
|
|
nonContactingLimbs_.insert(std::make_pair(id, limb)); |
104 |
|
|
else |
105 |
✓✗✓✗
|
10 |
limbs_.insert(std::make_pair(id, limb)); |
106 |
|
|
// tools::RemoveNonLimbCollisionRec<core::CollisionValidation>(device_->rootJoint(),name,collisionObjects,*limbcollisionValidation_.get()); |
107 |
|
|
hpp::core::RelativeMotion::matrix_type m = |
108 |
✓✗ |
20 |
hpp::core::RelativeMotion::matrix(device_); |
109 |
✓✗ |
10 |
limbcollisionValidation_->filterCollisionPairs(m); |
110 |
✓✗ |
10 |
collisionValidation_->filterCollisionPairs(m); |
111 |
|
|
hppDout(notice, "insert limb validation with id = " << id); |
112 |
|
|
limbcollisionValidations_.insert( |
113 |
✓✗✓✗
|
10 |
std::make_pair(id, limbcollisionValidation_)); |
114 |
|
|
// insert limb to root group |
115 |
✓✗ |
10 |
T_LimbGroup::iterator cit = limbGroups_.find(name); |
116 |
✗✓ |
10 |
if (cit != limbGroups_.end()) { |
117 |
|
|
cit->second.push_back(id); |
118 |
|
|
} else { |
119 |
|
10 |
std::vector<std::string> group; |
120 |
✓✗ |
10 |
group.push_back(id); |
121 |
✓✗✓✗
|
10 |
limbGroups_.insert(std::make_pair(name, group)); |
122 |
|
|
} |
123 |
|
10 |
} |
124 |
|
|
|
125 |
|
10 |
std::map<std::string, const sampling::heuristic>::const_iterator checkLimbData( |
126 |
|
|
const std::string& id, const rbprm::T_Limb& limbs, |
127 |
|
|
const rbprm::sampling::HeuristicFactory& factory, |
128 |
|
|
const std::string& heuristicName) { |
129 |
✓✗ |
10 |
rbprm::T_Limb::const_iterator cit = limbs.find(id); |
130 |
|
|
std::map<std::string, const sampling::heuristic>::const_iterator hit = |
131 |
✓✗ |
10 |
factory.heuristics_.find(heuristicName); |
132 |
✗✓ |
10 |
if (cit != limbs.end()) |
133 |
|
|
throw std::runtime_error("Impossible to add limb for joint " + id + |
134 |
|
|
" to robot; limb already exists"); |
135 |
✗✓ |
10 |
else if (hit == factory.heuristics_.end()) |
136 |
|
|
throw std::runtime_error("Impossible to add limb for joint " + id + |
137 |
|
|
" to robot; heuristic not found " + heuristicName + |
138 |
|
|
"."); |
139 |
|
10 |
return hit; |
140 |
|
|
} |
141 |
|
|
|
142 |
|
10 |
void RbPrmFullBody::AddLimb( |
143 |
|
|
const std::string& id, const std::string& name, |
144 |
|
|
const std::string& effectorName, const fcl::Vec3f& offset, |
145 |
|
|
const fcl::Vec3f& limbOffset, const fcl::Vec3f& normal, const double x, |
146 |
|
|
const double y, const hpp::core::ObjectStdVector_t& collisionObjects, |
147 |
|
|
const std::size_t nbSamples, const std::string& heuristicName, |
148 |
|
|
const double resolution, ContactType contactType, |
149 |
|
|
const bool disableEffectorCollision, const bool grasp, |
150 |
|
|
const std::string& kinematicConstraintsPath, |
151 |
|
|
const double kinematicConstraintsMin) { |
152 |
|
|
std::map<std::string, const sampling::heuristic>::const_iterator hit = |
153 |
✓✗ |
10 |
checkLimbData(id, limbs_, factory_, heuristicName); |
154 |
✓✗ |
20 |
pinocchio::JointPtr_t joint = device_->getJointByName(name); |
155 |
|
|
rbprm::RbPrmLimbPtr_t limb = rbprm::RbPrmLimb::create( |
156 |
|
|
joint, effectorName, offset, limbOffset, normal, x, y, nbSamples, |
157 |
|
20 |
hit->second, resolution, contactType, disableEffectorCollision, grasp, |
158 |
✓✗ |
10 |
kinematicConstraintsPath, kinematicConstraintsMin); |
159 |
✓✗ |
10 |
AddLimbPrivate(limb, id, name, collisionObjects, disableEffectorCollision); |
160 |
|
10 |
} |
161 |
|
|
|
162 |
|
|
void RbPrmFullBody::AddNonContactingLimb( |
163 |
|
|
const std::string& id, const std::string& name, |
164 |
|
|
const std::string& effectorName, |
165 |
|
|
const hpp::core::ObjectStdVector_t& collisionObjects, |
166 |
|
|
const std::size_t nbSamples) { |
167 |
|
|
std::map<std::string, const sampling::heuristic>::const_iterator hit = |
168 |
|
|
checkLimbData(id, nonContactingLimbs_, factory_, "static"); |
169 |
|
|
pinocchio::JointPtr_t joint = device_->getJointByName(name); |
170 |
|
|
rbprm::RbPrmLimbPtr_t limb = rbprm::RbPrmLimb::create( |
171 |
|
|
joint, effectorName, fcl::Vec3f(0, 0, 0), fcl::Vec3f(0, 0, 0), |
172 |
|
|
fcl::Vec3f(0, 0, 1), 0, 0, nbSamples, hit->second, 0.03); |
173 |
|
|
AddLimbPrivate(limb, id, name, collisionObjects, false, true); |
174 |
|
|
} |
175 |
|
|
|
176 |
|
|
void RbPrmFullBody::AddLimb( |
177 |
|
|
const std::string& database, const std::string& id, |
178 |
|
|
const hpp::core::ObjectStdVector_t& collisionObjects, |
179 |
|
|
const std::string& heuristicName, const bool loadValues, |
180 |
|
|
const bool disableEffectorCollision, const bool grasp) { |
181 |
|
|
std::map<std::string, const sampling::heuristic>::const_iterator hit = |
182 |
|
|
checkLimbData(id, limbs_, factory_, heuristicName); |
183 |
|
|
; |
184 |
|
|
std::ifstream myfile(database.c_str()); |
185 |
|
|
if (!myfile.good()) throw std::runtime_error("Impossible to open database"); |
186 |
|
|
rbprm::RbPrmLimbPtr_t limb = |
187 |
|
|
rbprm::RbPrmLimb::create(device_, myfile, loadValues, hit->second, |
188 |
|
|
disableEffectorCollision, grasp); |
189 |
|
|
myfile.close(); |
190 |
|
|
AddLimbPrivate(limb, id, limb->limb_->name(), collisionObjects, |
191 |
|
|
disableEffectorCollision); |
192 |
|
|
} |
193 |
|
|
|
194 |
|
144 |
const rbprm::RbPrmLimbPtr_t RbPrmFullBody::GetLimb(std::string name, |
195 |
|
|
bool onlyWithContact) { |
196 |
✓✗✓✗
|
144 |
T_Limb::const_iterator lit = GetLimbs().find(std::string(name)); |
197 |
✗✓ |
144 |
if (lit == GetLimbs().end()) { |
198 |
|
|
if (onlyWithContact) { |
199 |
|
|
std::string err("No limb " + std::string(name) + |
200 |
|
|
" was defined for robot" + device_->name()); |
201 |
|
|
throw std::runtime_error(err.c_str()); |
202 |
|
|
} |
203 |
|
|
lit = GetNonContactingLimbs().find(std::string(name)); |
204 |
|
|
if (lit == GetNonContactingLimbs().end()) { |
205 |
|
|
std::string err("No limb " + std::string(name) + |
206 |
|
|
" was defined for robot" + device_->name()); |
207 |
|
|
throw std::runtime_error(err.c_str()); |
208 |
|
|
} |
209 |
|
|
} |
210 |
|
288 |
return lit->second; |
211 |
|
|
} |
212 |
|
|
|
213 |
|
|
bool RbPrmFullBody::addEffectorTrajectory( |
214 |
|
|
const size_t pathId, const std::string& effectorName, |
215 |
|
|
const std::vector<bezier_Ptr>& trajectories) { |
216 |
|
|
bool success; |
217 |
|
|
if (effectorsTrajectoriesMaps_.find(pathId) == |
218 |
|
|
effectorsTrajectoriesMaps_.end()) { |
219 |
|
|
// no map for this index, create a new one with the pair (name,path) |
220 |
|
|
EffectorTrajectoriesMap_t map; |
221 |
|
|
map.insert(std::make_pair(effectorName, trajectories)); |
222 |
|
|
success = |
223 |
|
|
effectorsTrajectoriesMaps_.insert(std::make_pair(pathId, map)).second; |
224 |
|
|
} else { |
225 |
|
|
// there is already a trajectory at this index, we add the trajectory for |
226 |
|
|
// this effector to the map |
227 |
|
|
success = effectorsTrajectoriesMaps_.at(pathId) |
228 |
|
|
.insert(std::make_pair(effectorName, trajectories)) |
229 |
|
|
.second; |
230 |
|
|
} |
231 |
|
|
return success; |
232 |
|
|
} |
233 |
|
|
|
234 |
|
|
bool RbPrmFullBody::addEffectorTrajectory(const size_t pathId, |
235 |
|
|
const std::string& effectorName, |
236 |
|
|
const bezier_Ptr& trajectory) { |
237 |
|
|
bool success; |
238 |
|
|
if (effectorsTrajectoriesMaps_.find(pathId) == |
239 |
|
|
effectorsTrajectoriesMaps_.end()) { |
240 |
|
|
// no map for this index, create a new one with the pair (name,path) |
241 |
|
|
EffectorTrajectoriesMap_t map; |
242 |
|
|
std::vector<bezier_Ptr> vec; |
243 |
|
|
vec.push_back(trajectory); |
244 |
|
|
map.insert(std::make_pair(effectorName, vec)); |
245 |
|
|
success = |
246 |
|
|
effectorsTrajectoriesMaps_.insert(std::make_pair(pathId, map)).second; |
247 |
|
|
} else { |
248 |
|
|
// there is already a trajectory at this index, we check if there is already |
249 |
|
|
// one for the given effector name : |
250 |
|
|
if (effectorsTrajectoriesMaps_.at(pathId).find(effectorName) == |
251 |
|
|
effectorsTrajectoriesMaps_.at(pathId).end()) { |
252 |
|
|
// there is no trajectory for this effector name, we create a vector with |
253 |
|
|
// the trajectory and add it |
254 |
|
|
std::vector<bezier_Ptr> vector; |
255 |
|
|
vector.push_back(trajectory); |
256 |
|
|
success = effectorsTrajectoriesMaps_.at(pathId) |
257 |
|
|
.insert(std::make_pair(effectorName, vector)) |
258 |
|
|
.second; |
259 |
|
|
} else { |
260 |
|
|
// there is already a trajectory with this effector, we add the new one to |
261 |
|
|
// the vector |
262 |
|
|
effectorsTrajectoriesMaps_.at(pathId) |
263 |
|
|
.at(effectorName) |
264 |
|
|
.push_back(trajectory); |
265 |
|
|
success = true; // ?? |
266 |
|
|
} |
267 |
|
|
} |
268 |
|
|
return success; |
269 |
|
|
} |
270 |
|
|
|
271 |
|
|
bool RbPrmFullBody::getEffectorsTrajectories( |
272 |
|
|
const size_t pathId, EffectorTrajectoriesMap_t& result) { |
273 |
|
|
if (effectorsTrajectoriesMaps_.find(pathId) == |
274 |
|
|
effectorsTrajectoriesMaps_.end()) |
275 |
|
|
return false; |
276 |
|
|
result = effectorsTrajectoriesMaps_.at(pathId); |
277 |
|
|
return true; |
278 |
|
|
} |
279 |
|
|
|
280 |
|
|
bool RbPrmFullBody::getEffectorTrajectory(const size_t pathId, |
281 |
|
|
const std::string& effectorName, |
282 |
|
|
std::vector<bezier_Ptr>& result) { |
283 |
|
|
if (effectorsTrajectoriesMaps_.find(pathId) == |
284 |
|
|
effectorsTrajectoriesMaps_.end()) |
285 |
|
|
return false; |
286 |
|
|
EffectorTrajectoriesMap_t map = effectorsTrajectoriesMaps_.at(pathId); |
287 |
|
|
if (map.find(effectorName) == map.end()) return false; |
288 |
|
|
result = map.at(effectorName); |
289 |
|
|
return true; |
290 |
|
|
} |
291 |
|
|
|
292 |
|
|
bool RbPrmFullBody::toggleNonContactingLimb(std::string name) { |
293 |
|
|
CIT_Limb cit = limbs_.find(name); |
294 |
|
|
if (cit != limbs_.end()) { |
295 |
|
|
nonContactingLimbs_.insert(std::make_pair(cit->first, cit->second)); |
296 |
|
|
limbs_.erase(cit->first); |
297 |
|
|
std::cout << "j enleve " << std::endl; |
298 |
|
|
return true; |
299 |
|
|
} |
300 |
|
|
cit = nonContactingLimbs_.find(name); |
301 |
|
|
if (cit != nonContactingLimbs_.end()) { |
302 |
|
|
limbs_.insert(std::make_pair(cit->first, cit->second)); |
303 |
|
|
nonContactingLimbs_.erase(cit->first); |
304 |
|
|
std::cout << "j ajoute " << std::endl; |
305 |
|
|
return true; |
306 |
|
|
} |
307 |
|
|
return false; |
308 |
|
|
} |
309 |
|
|
|
310 |
|
4 |
void RbPrmFullBody::referenceConfig( |
311 |
|
|
pinocchio::Configuration_t referenceConfig) { |
312 |
✓✗ |
4 |
reference_ = referenceConfig; |
313 |
|
|
// create transform of the freeflyer in the world frame : |
314 |
✓✗ |
4 |
fcl::Transform3f tRoot; |
315 |
✓✗✓✗
|
4 |
fcl::Transform3f tJoint_world, tJoint_robot; |
316 |
✓✗✓✗
✓✗ |
4 |
tRoot.setTranslation(fcl::Vec3f(referenceConfig.head<3>())); |
317 |
✓✗ |
4 |
fcl::Quaternion3f quatRoot(referenceConfig[6], referenceConfig[3], |
318 |
✓✗✓✗
✓✗✓✗
|
8 |
referenceConfig[4], referenceConfig[5]); |
319 |
✓✗ |
4 |
tRoot.setQuatRotation(quatRoot); |
320 |
|
|
hppDout(notice, "reference root transform : " << tRoot.getTranslation() |
321 |
|
|
<< " ; " |
322 |
|
|
<< tRoot.getRotation()); |
323 |
|
|
// retrieve transform of each effector joint |
324 |
✓✗✓✗
|
4 |
device_->currentConfiguration(referenceConfig); |
325 |
✓✗ |
4 |
device_->computeForwardKinematics(); |
326 |
|
4 |
if (limbs_.empty()) |
327 |
|
|
hppDout(warning, "No limbs found when setting reference configuration."); |
328 |
✗✓ |
4 |
for (CIT_Limb lit = limbs_.begin(); lit != limbs_.end(); ++lit) { |
329 |
|
|
hpp::pinocchio::Transform3f tf = |
330 |
|
|
lit->second->effector_.currentTransformation(); |
331 |
|
|
tJoint_world = fcl::Transform3f(tf.rotation(), tf.translation()); |
332 |
|
|
hppDout(notice, "tJoint of " << lit->first << " : " |
333 |
|
|
<< tJoint_world.getTranslation() << " ; " |
334 |
|
|
<< tJoint_world.getRotation()); |
335 |
|
|
tJoint_robot = tRoot.inverseTimes(tJoint_world); |
336 |
|
|
hppDout(notice, "tJoint relative : " << tJoint_robot.getTranslation() |
337 |
|
|
<< " ; " |
338 |
|
|
<< tJoint_robot.getRotation()); |
339 |
|
|
lit->second->effectorReferencePosition_ = tJoint_robot.getTranslation(); |
340 |
|
|
} |
341 |
|
4 |
} |
342 |
|
|
|
343 |
|
|
void RbPrmFullBody::postureWeights(pinocchio::Configuration_t postureWeights) { |
344 |
|
|
assert(postureWeights.size() == device_->numberDof() && |
345 |
|
|
"Posture weights must be the same size as device's degree of freedom"); |
346 |
|
|
postureWeights_ = postureWeights; |
347 |
|
|
} |
348 |
|
|
|
349 |
|
4 |
void RbPrmFullBody::init(const RbPrmFullBodyWkPtr_t& weakPtr) { |
350 |
|
4 |
weakPtr_ = weakPtr; |
351 |
|
4 |
} |
352 |
|
|
|
353 |
|
4 |
RbPrmFullBody::RbPrmFullBody(const pinocchio::DevicePtr_t& device) |
354 |
|
|
: device_(device), |
355 |
|
|
collisionValidation_(core::CollisionValidation::create(device)), |
356 |
|
|
staticStability_(true), |
357 |
|
|
mu_(0.5), |
358 |
|
4 |
reference_(device_->neutralConfiguration()), |
359 |
|
|
postureWeights_(), |
360 |
|
|
usePosturalTaskContactCreation_(false), |
361 |
|
|
effectorsTrajectoriesMaps_(), |
362 |
✓✗✓✗
✓✗✓✗
|
4 |
weakPtr_() { |
363 |
|
|
hppDout(notice, "Neutralconfig when creating fullBody : " |
364 |
|
|
<< pinocchio::displayConfig(reference_)); |
365 |
|
4 |
} |
366 |
|
|
} // namespace rbprm |
367 |
|
|
} // namespace hpp |