Line |
Branch |
Exec |
Source |
1 |
|
|
// Copyright (c) 2014 CNRS |
2 |
|
|
// Author: Florent Lamiraux |
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 "problem.impl.hh" |
30 |
|
|
|
31 |
|
|
#include <hpp/constraints/convex-shape-contact.hh> |
32 |
|
|
#include <hpp/constraints/implicit.hh> |
33 |
|
|
#include <hpp/corbaserver/conversions.hh> |
34 |
|
|
#include <hpp/corbaserver/manipulation/server.hh> |
35 |
|
|
#include <hpp/corbaserver/servant-base.hh> |
36 |
|
|
#include <hpp/core/config-projector.hh> |
37 |
|
|
#include <hpp/core/distance.hh> |
38 |
|
|
#include <hpp/core/parser/roadmap.hh> |
39 |
|
|
#include <hpp/core/path-projector.hh> |
40 |
|
|
#include <hpp/core/path-vector.hh> |
41 |
|
|
#include <hpp/core/weighed-distance.hh> |
42 |
|
|
#include <hpp/manipulation/path-planner/transition-planner.hh> |
43 |
|
|
#include <hpp/manipulation/roadmap.hh> |
44 |
|
|
#include <hpp/pinocchio/gripper.hh> |
45 |
|
|
#include <hpp/pinocchio/serialization.hh> |
46 |
|
|
#include <hpp/util/debug.hh> |
47 |
|
|
#ifdef HPP_CONSTRAINTS_USE_QPOASES |
48 |
|
|
#include <hpp/constraints/qp-static-stability.hh> |
49 |
|
|
#endif |
50 |
|
|
#include <hpp/manipulation/constraint-set.hh> |
51 |
|
|
#include <hpp/manipulation/device.hh> |
52 |
|
|
#include <hpp/manipulation/graph/edge.hh> |
53 |
|
|
#include <hpp/manipulation/graph/state.hh> |
54 |
|
|
#include <hpp/manipulation/graph/validation.hh> |
55 |
|
|
#include <hpp/manipulation/manipulation-planner.hh> |
56 |
|
|
#include <hpp/manipulation/problem.hh> |
57 |
|
|
#include <hpp/manipulation/roadmap.hh> |
58 |
|
|
#include <hpp/manipulation/serialization.hh> |
59 |
|
|
#include <hpp/manipulation/steering-method/graph.hh> |
60 |
|
|
|
61 |
|
|
#include "hpp/core_idl/distances-fwd.hh" // For hpp::core_impl::Roadmap |
62 |
|
|
#include "hpp/core_idl/path_planners-fwd.hh" // For hpp::core_impl::Roadmap |
63 |
|
|
#include "hpp/manipulation_idl/_graph.hh" |
64 |
|
|
#include "hpp/manipulation_idl/_path_planners.hh" |
65 |
|
|
#include "hpp/manipulation_idl/device-fwd.hh" |
66 |
|
|
#include "hpp/pinocchio_idl/robots-fwd.hh" |
67 |
|
|
#include "tools.hh" |
68 |
|
|
|
69 |
|
|
namespace hpp { |
70 |
|
|
namespace manipulation { |
71 |
|
|
namespace impl { |
72 |
|
|
using corbaServer::floatSeqToConfig; |
73 |
|
|
using corbaServer::makeServantDownCast; |
74 |
|
|
using corbaServer::reference_to_object; |
75 |
|
|
|
76 |
|
|
namespace { |
77 |
|
|
typedef core::ProblemSolver CPs_t; |
78 |
|
|
|
79 |
|
✗ |
Names_t* jointAndShapes(const JointAndShapes_t& js, intSeq_out indexes_out, |
80 |
|
|
floatSeqSeq_out points) { |
81 |
|
✗ |
char** nameList = Names_t::allocbuf((ULong)js.size()); |
82 |
|
|
Names_t* jointNames = |
83 |
|
✗ |
new Names_t((ULong)js.size(), (ULong)js.size(), nameList); |
84 |
|
|
|
85 |
|
✗ |
intSeq* indexes = new intSeq(); |
86 |
|
✗ |
indexes->length((ULong)js.size()); |
87 |
|
✗ |
indexes_out = indexes; |
88 |
|
|
|
89 |
|
✗ |
CORBA::Long rank = 0; |
90 |
|
✗ |
std::size_t nbPts = 0; |
91 |
|
✗ |
for (JointAndShapes_t::const_iterator itJs = js.begin(); itJs != js.end(); |
92 |
|
✗ |
++itJs) { |
93 |
|
✗ |
if (itJs->first) { |
94 |
|
✗ |
nameList[rank] = new char[itJs->first->name().length() + 1]; |
95 |
|
✗ |
strcpy(nameList[rank], itJs->first->name().c_str()); |
96 |
|
|
} else { |
97 |
|
✗ |
nameList[rank] = new char[9]; |
98 |
|
✗ |
strcpy(nameList[rank], "universe"); |
99 |
|
|
} |
100 |
|
✗ |
nbPts += itJs->second.size(); |
101 |
|
✗ |
(*indexes)[rank] = (int)nbPts; |
102 |
|
✗ |
++rank; |
103 |
|
|
} |
104 |
|
|
|
105 |
|
✗ |
floatSeqSeq* pts = new hpp::floatSeqSeq(); |
106 |
|
✗ |
points = pts; |
107 |
|
✗ |
pts->length((CORBA::ULong)nbPts); |
108 |
|
|
|
109 |
|
✗ |
rank = 0; |
110 |
|
✗ |
ULong iJs = 0; |
111 |
|
✗ |
for (JointAndShapes_t::const_iterator itJs = js.begin(); itJs != js.end(); |
112 |
|
✗ |
++itJs) { |
113 |
|
✗ |
for (std::size_t i = 0; i < itJs->second.size(); ++i) { |
114 |
|
✗ |
floatSeq p; |
115 |
|
✗ |
p.length(3); |
116 |
|
✗ |
p[0] = itJs->second[i][0]; |
117 |
|
✗ |
p[1] = itJs->second[i][1]; |
118 |
|
✗ |
p[2] = itJs->second[i][2]; |
119 |
|
✗ |
(*pts)[rank] = p; |
120 |
|
✗ |
++rank; |
121 |
|
|
} |
122 |
|
✗ |
assert((*indexes)[iJs] == rank); |
123 |
|
✗ |
++iJs; |
124 |
|
|
} |
125 |
|
✗ |
return jointNames; |
126 |
|
|
} |
127 |
|
|
} // namespace |
128 |
|
|
|
129 |
|
✗ |
Problem::Problem() : server_(0x0) {} |
130 |
|
|
|
131 |
|
✗ |
ProblemSolverPtr_t Problem::problemSolver() { return server_->problemSolver(); } |
132 |
|
|
|
133 |
|
✗ |
graph::GraphPtr_t Problem::graph(bool throwIfNull) { |
134 |
|
✗ |
graph::GraphPtr_t g = problemSolver()->constraintGraph(); |
135 |
|
✗ |
if (throwIfNull && !g) throw Error("You should create the graph"); |
136 |
|
✗ |
return g; |
137 |
|
|
} |
138 |
|
|
|
139 |
|
✗ |
bool Problem::selectProblem(const char* name) { |
140 |
|
✗ |
std::string psName(name); |
141 |
|
✗ |
corbaServer::ProblemSolverMapPtr_t psMap(server_->problemSolverMap()); |
142 |
|
✗ |
bool has = psMap->has(psName); |
143 |
|
✗ |
if (!has) psMap->add(psName, ProblemSolver::create()); |
144 |
|
✗ |
psMap->selected(psName); |
145 |
|
✗ |
return !has; |
146 |
|
|
} |
147 |
|
|
|
148 |
|
✗ |
void Problem::resetProblem() { |
149 |
|
✗ |
corbaServer::ProblemSolverMapPtr_t psMap(server_->problemSolverMap()); |
150 |
|
✗ |
psMap->replaceSelected(ProblemSolver::create()); |
151 |
|
|
} |
152 |
|
|
|
153 |
|
✗ |
Names_t* Problem::getAvailable(const char* what) { |
154 |
|
✗ |
std::string w(what); |
155 |
|
✗ |
std::transform(w.begin(), w.end(), w.begin(), |
156 |
|
✗ |
[](unsigned char c) { return std::tolower(c); }); |
157 |
|
|
typedef std::list<std::string> Ret_t; |
158 |
|
✗ |
Ret_t ret; |
159 |
|
|
|
160 |
|
✗ |
if (w == "gripper") { |
161 |
|
✗ |
ret = getRobotOrThrow(problemSolver())->grippers.getKeys<Ret_t>(); |
162 |
|
✗ |
} else if (w == "handle") { |
163 |
|
✗ |
ret = getRobotOrThrow(problemSolver())->handles.getKeys<Ret_t>(); |
164 |
|
✗ |
} else if (w == "robotcontact") { |
165 |
|
✗ |
ret = getRobotOrThrow(problemSolver())->jointAndShapes.getKeys<Ret_t>(); |
166 |
|
✗ |
} else if (w == "envcontact") { |
167 |
|
✗ |
ret = problemSolver()->jointAndShapes.getKeys<Ret_t>(); |
168 |
|
✗ |
} else if (w == "constraintgraph") { |
169 |
|
✗ |
ret = problemSolver()->graphs.getKeys<Ret_t>(); |
170 |
|
✗ |
} else if (w == "type") { |
171 |
|
✗ |
ret = {"Gripper", "Handle", "RobotContact", "EnvContact", |
172 |
|
✗ |
"ConstraintGraph"}; |
173 |
|
|
} else { |
174 |
|
✗ |
throw Error(("Type \"" + std::string(what) + "\" not known").c_str()); |
175 |
|
|
} |
176 |
|
|
|
177 |
|
✗ |
return toNames_t(ret.begin(), ret.end()); |
178 |
|
|
} |
179 |
|
|
|
180 |
|
✗ |
Names_t* Problem::getSelected(const char* what) { |
181 |
|
✗ |
std::string w(what); |
182 |
|
✗ |
std::transform(w.begin(), w.end(), w.begin(), |
183 |
|
✗ |
[](unsigned char c) { return std::tolower(c); }); |
184 |
|
|
typedef std::list<std::string> Ret_t; |
185 |
|
✗ |
Ret_t ret; |
186 |
|
|
|
187 |
|
✗ |
if (w == "constraintgraph") { |
188 |
|
✗ |
if (problemSolver()->constraintGraph()) |
189 |
|
✗ |
ret.push_back(problemSolver()->constraintGraph()->name()); |
190 |
|
|
else |
191 |
|
✗ |
throw Error("No constraint graph selected."); |
192 |
|
✗ |
} else if (w == "type") { |
193 |
|
✗ |
ret = {"ConstraintGraph"}; |
194 |
|
|
} else { |
195 |
|
✗ |
throw Error(("Type \"" + std::string(what) + "\" not known").c_str()); |
196 |
|
|
} |
197 |
|
|
|
198 |
|
✗ |
return toNames_t(ret.begin(), ret.end()); |
199 |
|
|
} |
200 |
|
|
|
201 |
|
✗ |
void Problem::loadRoadmap(const char* filename) { |
202 |
|
|
try { |
203 |
|
✗ |
ProblemSolverPtr_t ps(problemSolver()); |
204 |
|
✗ |
DevicePtr_t robot = getRobotOrThrow(ps); |
205 |
|
✗ |
graph::GraphPtr_t g = ps->constraintGraph(); |
206 |
|
|
|
207 |
|
|
using namespace core::parser; |
208 |
|
|
|
209 |
|
|
typedef hpp::serialization::archive_tpl< |
210 |
|
|
boost::archive::binary_iarchive, |
211 |
|
|
hpp::serialization::remove_duplicate::vector_archive> |
212 |
|
|
archive_type; |
213 |
|
✗ |
hpp::core::RoadmapPtr_t roadmap; |
214 |
|
✗ |
serializeRoadmap<archive_type>(roadmap, std::string(filename), |
215 |
|
✗ |
make_nvp(robot->name(), robot.get()), |
216 |
|
✗ |
make_nvp(g->name(), g.get())); |
217 |
|
✗ |
problemSolver()->roadmap(roadmap); |
218 |
|
✗ |
} catch (const std::exception& exc) { |
219 |
|
✗ |
throw hpp::Error(exc.what()); |
220 |
|
|
} |
221 |
|
|
} |
222 |
|
|
|
223 |
|
✗ |
void Problem::createGrasp(const char* graspName, const char* gripperName, |
224 |
|
|
const char* handleName) { |
225 |
|
|
try { |
226 |
|
✗ |
problemSolver()->createGraspConstraint(graspName, gripperName, handleName); |
227 |
|
✗ |
} catch (const std::exception& exc) { |
228 |
|
✗ |
throw Error(exc.what()); |
229 |
|
|
} |
230 |
|
|
} |
231 |
|
|
|
232 |
|
✗ |
void Problem::createPreGrasp(const char* graspName, const char* gripperName, |
233 |
|
|
const char* handleName) { |
234 |
|
|
try { |
235 |
|
✗ |
problemSolver()->createPreGraspConstraint(graspName, gripperName, |
236 |
|
|
handleName); |
237 |
|
✗ |
} catch (const std::exception& exc) { |
238 |
|
✗ |
throw Error(exc.what()); |
239 |
|
|
} |
240 |
|
|
} |
241 |
|
|
|
242 |
|
✗ |
Names_t* Problem::getEnvironmentContactNames() { |
243 |
|
|
try { |
244 |
|
|
typedef std::map<std::string, JointAndShapes_t> ShapeMap; |
245 |
|
✗ |
const ShapeMap& m = problemSolver()->jointAndShapes.map; |
246 |
|
|
|
247 |
|
✗ |
char** nameList = Names_t::allocbuf((ULong)m.size()); |
248 |
|
|
Names_t* jointNames = |
249 |
|
✗ |
new Names_t((ULong)m.size(), (ULong)m.size(), nameList); |
250 |
|
|
|
251 |
|
✗ |
std::size_t rank = 0; |
252 |
|
✗ |
for (ShapeMap::const_iterator it = m.begin(); it != m.end(); it++) { |
253 |
|
✗ |
nameList[rank] = (char*)malloc(sizeof(char) * (it->first.length() + 1)); |
254 |
|
✗ |
strcpy(nameList[rank], it->first.c_str()); |
255 |
|
✗ |
rank++; |
256 |
|
|
} |
257 |
|
✗ |
return jointNames; |
258 |
|
✗ |
} catch (const std::exception& exc) { |
259 |
|
✗ |
throw hpp::Error(exc.what()); |
260 |
|
|
} |
261 |
|
|
} |
262 |
|
|
|
263 |
|
✗ |
Names_t* Problem::getRobotContactNames() { |
264 |
|
|
try { |
265 |
|
|
typedef std::map<std::string, JointAndShapes_t> ShapeMap; |
266 |
|
✗ |
DevicePtr_t r = getRobotOrThrow(problemSolver()); |
267 |
|
✗ |
const ShapeMap& m = r->jointAndShapes.map; |
268 |
|
|
|
269 |
|
✗ |
char** nameList = Names_t::allocbuf((ULong)m.size()); |
270 |
|
|
Names_t* jointNames = |
271 |
|
✗ |
new Names_t((ULong)m.size(), (ULong)m.size(), nameList); |
272 |
|
|
|
273 |
|
✗ |
std::size_t rank = 0; |
274 |
|
✗ |
for (ShapeMap::const_iterator it = m.begin(); it != m.end(); it++) { |
275 |
|
✗ |
nameList[rank] = (char*)malloc(sizeof(char) * (it->first.length() + 1)); |
276 |
|
✗ |
strcpy(nameList[rank], it->first.c_str()); |
277 |
|
✗ |
rank++; |
278 |
|
|
} |
279 |
|
✗ |
return jointNames; |
280 |
|
✗ |
} catch (const std::exception& exc) { |
281 |
|
✗ |
throw hpp::Error(exc.what()); |
282 |
|
|
} |
283 |
|
|
} |
284 |
|
|
|
285 |
|
✗ |
Names_t* Problem::getEnvironmentContact(const char* name, intSeq_out indexes, |
286 |
|
|
floatSeqSeq_out points) { |
287 |
|
|
try { |
288 |
|
✗ |
const JointAndShapes_t& js = problemSolver()->jointAndShapes.get(name); |
289 |
|
|
|
290 |
|
✗ |
return jointAndShapes(js, indexes, points); |
291 |
|
✗ |
} catch (const std::exception& exc) { |
292 |
|
✗ |
throw hpp::Error(exc.what()); |
293 |
|
|
} |
294 |
|
|
} |
295 |
|
|
|
296 |
|
✗ |
Names_t* Problem::getRobotContact(const char* name, intSeq_out indexes, |
297 |
|
|
hpp::floatSeqSeq_out points) { |
298 |
|
|
try { |
299 |
|
✗ |
DevicePtr_t r = getRobotOrThrow(problemSolver()); |
300 |
|
✗ |
const JointAndShapes_t& js = r->jointAndShapes.get(name); |
301 |
|
|
|
302 |
|
✗ |
return jointAndShapes(js, indexes, points); |
303 |
|
✗ |
} catch (const std::exception& exc) { |
304 |
|
✗ |
throw hpp::Error(exc.what()); |
305 |
|
|
} |
306 |
|
|
} |
307 |
|
|
|
308 |
|
✗ |
void Problem::createPlacementConstraint(const char* placName, |
309 |
|
|
const Names_t& surface1, |
310 |
|
|
const Names_t& surface2) { |
311 |
|
|
try { |
312 |
|
✗ |
problemSolver()->createPlacementConstraint( |
313 |
|
✗ |
placName, corbaServer::toStrings<std::vector<std::string> >(surface1), |
314 |
|
✗ |
corbaServer::toStrings<std::vector<std::string> >(surface2), 1e-3); |
315 |
|
✗ |
} catch (const std::exception& exc) { |
316 |
|
✗ |
throw hpp::Error(exc.what()); |
317 |
|
|
} |
318 |
|
|
} |
319 |
|
|
|
320 |
|
✗ |
void Problem::createPrePlacementConstraint(const char* placName, |
321 |
|
|
const Names_t& surface1, |
322 |
|
|
const Names_t& surface2, |
323 |
|
|
CORBA::Double width) { |
324 |
|
|
try { |
325 |
|
✗ |
problemSolver()->createPrePlacementConstraint( |
326 |
|
✗ |
placName, corbaServer::toStrings<std::vector<std::string> >(surface1), |
327 |
|
✗ |
corbaServer::toStrings<std::vector<std::string> >(surface2), width, |
328 |
|
✗ |
1e-3); |
329 |
|
✗ |
} catch (const std::exception& exc) { |
330 |
|
✗ |
throw hpp::Error(exc.what()); |
331 |
|
|
} |
332 |
|
|
} |
333 |
|
|
|
334 |
|
✗ |
void Problem::createQPStabilityConstraint(const char* constraintName, |
335 |
|
|
const char* comRootJointName, |
336 |
|
|
const Names_t& shapesName) { |
337 |
|
|
try { |
338 |
|
|
#ifdef HPP_CONSTRAINTS_USE_QPOASES |
339 |
|
|
// Get robot in hppPlanner object. |
340 |
|
✗ |
DevicePtr_t robot = getRobotOrThrow(problemSolver()); |
341 |
|
|
|
342 |
|
|
using constraints::ConvexShape; |
343 |
|
|
using constraints::QPStaticStability; |
344 |
|
|
using constraints::QPStaticStabilityPtr_t; |
345 |
|
|
typedef constraints::QPStaticStability::ForceData ForceData; |
346 |
|
|
using pinocchio::CenterOfMassComputation; |
347 |
|
|
using pinocchio::CenterOfMassComputationPtr_t; |
348 |
|
|
|
349 |
|
✗ |
std::vector<ForceData> fds; |
350 |
|
✗ |
std::size_t nbPoints = 0; |
351 |
|
|
|
352 |
|
✗ |
for (CORBA::ULong i = 0; i < shapesName.length(); ++i) { |
353 |
|
|
JointAndShapes_t l = |
354 |
|
✗ |
robot->jointAndShapes.get(std::string(shapesName[i])); |
355 |
|
✗ |
if (l.empty()) throw Error("Robot shapes not found."); |
356 |
|
✗ |
for (JointAndShapes_t::const_iterator it = l.begin(); it != l.end(); |
357 |
|
✗ |
it++) { |
358 |
|
✗ |
ConvexShape c(ConvexShape(it->second, it->first)); |
359 |
|
✗ |
ForceData fd; |
360 |
|
✗ |
fd.joint = c.joint_; |
361 |
|
✗ |
fd.supportJoint = JointPtr_t(); |
362 |
|
✗ |
fd.normal = -c.N_; |
363 |
|
✗ |
fd.points = c.Pts_; |
364 |
|
✗ |
nbPoints += c.Pts_.size(); |
365 |
|
✗ |
fds.push_back(fd); |
366 |
|
|
} |
367 |
|
|
} |
368 |
|
|
|
369 |
|
✗ |
CenterOfMassComputationPtr_t com = CenterOfMassComputation::create(robot); |
370 |
|
✗ |
JointPtr_t comRJ(robot->getJointByName(comRootJointName)); |
371 |
|
✗ |
com->add(comRJ); |
372 |
|
|
QPStaticStabilityPtr_t c = |
373 |
|
✗ |
QPStaticStability::create(constraintName, robot, fds, com); |
374 |
|
✗ |
problemSolver()->addNumericalConstraint( |
375 |
|
✗ |
constraintName, Implicit::create(c, constraints::ComparisonTypes_t( |
376 |
|
✗ |
1, constraints::EqualToZero))); |
377 |
|
|
#else |
378 |
|
|
// Avoid unused-variable compilation warnings |
379 |
|
|
(void)constraintName; |
380 |
|
|
(void)shapesName; |
381 |
|
|
throw std::runtime_error( |
382 |
|
|
"Problem::createQPStabilityConstraint is not implemented"); |
383 |
|
|
#endif |
384 |
|
✗ |
} catch (const std::exception& exc) { |
385 |
|
✗ |
throw hpp::Error(exc.what()); |
386 |
|
|
} |
387 |
|
|
} |
388 |
|
|
|
389 |
|
✗ |
bool Problem::setConstraints(hpp::ID id, bool target) { |
390 |
|
|
/// First get the constraint. |
391 |
|
✗ |
ConstraintSetPtr_t constraint; |
392 |
|
|
try { |
393 |
|
✗ |
graph::GraphComponentPtr_t comp = graph()->get((size_t)id).lock(); |
394 |
|
✗ |
graph::EdgePtr_t edge = HPP_DYNAMIC_PTR_CAST(graph::Edge, comp); |
395 |
|
✗ |
graph::StatePtr_t state = HPP_DYNAMIC_PTR_CAST(graph::State, comp); |
396 |
|
✗ |
if (edge) { |
397 |
|
✗ |
if (target) |
398 |
|
✗ |
constraint = graph()->targetConstraint(edge); |
399 |
|
|
else |
400 |
|
✗ |
constraint = graph()->pathConstraint(edge); |
401 |
|
✗ |
} else if (state) { |
402 |
|
✗ |
constraint = graph()->configConstraint(state); |
403 |
|
|
} else { |
404 |
|
✗ |
std::stringstream ss; |
405 |
|
✗ |
ss << "ID " << id << " is neither an edge nor a state"; |
406 |
|
✗ |
std::string errmsg = ss.str(); |
407 |
|
✗ |
throw Error(errmsg.c_str()); |
408 |
|
|
} |
409 |
|
✗ |
if (constraint) { |
410 |
|
✗ |
problemSolver()->resetConstraints(); |
411 |
|
✗ |
problemSolver()->addConstraint(constraint->copy()); |
412 |
|
✗ |
return true; |
413 |
|
|
} else |
414 |
|
✗ |
return false; |
415 |
|
✗ |
} catch (const std::exception& exc) { |
416 |
|
✗ |
throw hpp::Error(exc.what()); |
417 |
|
|
} |
418 |
|
|
} |
419 |
|
|
|
420 |
|
✗ |
void Problem::registerConstraints(const char* constraint, |
421 |
|
|
const char* complement, const char* both) { |
422 |
|
|
using constraints::ImplicitPtr_t; |
423 |
|
|
try { |
424 |
|
✗ |
ImplicitPtr_t constr(problemSolver()->numericalConstraints.get(constraint)); |
425 |
|
✗ |
ImplicitPtr_t comp(problemSolver()->numericalConstraints.get(complement)); |
426 |
|
✗ |
ImplicitPtr_t comb(problemSolver()->numericalConstraints.get(both)); |
427 |
|
✗ |
problemSolver()->constraintsAndComplements.push_back( |
428 |
|
✗ |
ConstraintAndComplement_t(constr, comp, comb)); |
429 |
|
✗ |
} catch (const std::exception& exc) { |
430 |
|
✗ |
throw hpp::Error(exc.what()); |
431 |
|
|
} |
432 |
|
|
} |
433 |
|
|
|
434 |
|
✗ |
bool Problem::applyConstraints(hpp::ID id, const hpp::floatSeq& input, |
435 |
|
|
hpp::floatSeq_out output, |
436 |
|
|
double& residualError) { |
437 |
|
|
/// First get the constraint. |
438 |
|
✗ |
ConstraintSetPtr_t constraint; |
439 |
|
|
try { |
440 |
|
✗ |
graph::GraphComponentPtr_t comp = graph()->get((size_t)id).lock(); |
441 |
|
✗ |
graph::EdgePtr_t edge = HPP_DYNAMIC_PTR_CAST(graph::Edge, comp); |
442 |
|
✗ |
graph::StatePtr_t state = HPP_DYNAMIC_PTR_CAST(graph::State, comp); |
443 |
|
✗ |
if (edge) { |
444 |
|
✗ |
constraint = graph(false)->targetConstraint(edge); |
445 |
|
✗ |
DevicePtr_t robot = getRobotOrThrow(problemSolver()); |
446 |
|
✗ |
if (core::ConfigProjectorPtr_t cp = constraint->configProjector()) { |
447 |
|
✗ |
cp->rightHandSideFromConfig(robot->currentConfiguration()); |
448 |
|
|
} |
449 |
|
✗ |
} else if (state) |
450 |
|
✗ |
constraint = graph(false)->configConstraint(state); |
451 |
|
|
else { |
452 |
|
✗ |
std::stringstream ss; |
453 |
|
✗ |
ss << "ID " << id << " is neither an edge nor a state"; |
454 |
|
✗ |
std::string errmsg = ss.str(); |
455 |
|
✗ |
throw Error(errmsg.c_str()); |
456 |
|
|
} |
457 |
|
✗ |
bool success = false; |
458 |
|
✗ |
DevicePtr_t robot = getRobotOrThrow(problemSolver()); |
459 |
|
✗ |
Configuration_t config = floatSeqToConfig(robot, input, true); |
460 |
|
✗ |
success = constraint->apply(config); |
461 |
|
✗ |
if (hpp::core::ConfigProjectorPtr_t configProjector = |
462 |
|
✗ |
constraint->configProjector()) { |
463 |
|
✗ |
residualError = configProjector->residualError(); |
464 |
|
|
} |
465 |
|
✗ |
output = vectorToFloatSeq(config); |
466 |
|
✗ |
return success; |
467 |
|
✗ |
} catch (const std::exception& exc) { |
468 |
|
✗ |
throw hpp::Error(exc.what()); |
469 |
|
|
} |
470 |
|
|
} |
471 |
|
|
|
472 |
|
✗ |
bool Problem::applyConstraintsWithOffset(hpp::ID IDedge, |
473 |
|
|
const hpp::floatSeq& qnear, |
474 |
|
|
const hpp::floatSeq& input, |
475 |
|
|
hpp::floatSeq_out output, |
476 |
|
|
double& residualError) { |
477 |
|
|
/// First get the constraint. |
478 |
|
✗ |
graph::EdgePtr_t edge; |
479 |
|
|
try { |
480 |
|
|
edge = |
481 |
|
✗ |
HPP_DYNAMIC_PTR_CAST(graph::Edge, graph()->get((size_t)IDedge).lock()); |
482 |
|
✗ |
if (!edge) { |
483 |
|
✗ |
std::stringstream ss; |
484 |
|
✗ |
ss << "ID " << IDedge << " is not an edge"; |
485 |
|
✗ |
std::string errmsg = ss.str(); |
486 |
|
✗ |
throw Error(errmsg.c_str()); |
487 |
|
|
} |
488 |
|
✗ |
bool success = false; |
489 |
|
✗ |
DevicePtr_t robot = getRobotOrThrow(problemSolver()); |
490 |
|
✗ |
Configuration_t config = floatSeqToConfig(robot, input, true); |
491 |
|
✗ |
Configuration_t qoffset = floatSeqToConfig(robot, qnear, true); |
492 |
|
✗ |
value_type dist = 0; |
493 |
|
|
core::NodePtr_t nNode = |
494 |
|
✗ |
problemSolver()->roadmap()->nearestNode(qoffset, dist); |
495 |
|
✗ |
if (dist < 1e-8) |
496 |
|
✗ |
success = edge->generateTargetConfig(nNode, config); |
497 |
|
|
else |
498 |
|
✗ |
success = edge->generateTargetConfig(qoffset, config); |
499 |
|
|
|
500 |
|
|
hpp::core::ConfigProjectorPtr_t configProjector( |
501 |
|
✗ |
edge->targetConstraint()->configProjector()); |
502 |
|
✗ |
if (configProjector) { |
503 |
|
✗ |
residualError = configProjector->residualError(); |
504 |
|
|
} else { |
505 |
|
|
hppDout(info, "No config projector."); |
506 |
|
|
} |
507 |
|
✗ |
ULong size = (ULong)config.size(); |
508 |
|
✗ |
hpp::floatSeq* q_ptr = new hpp::floatSeq(); |
509 |
|
✗ |
q_ptr->length(size); |
510 |
|
|
|
511 |
|
✗ |
for (std::size_t i = 0; i < size; ++i) { |
512 |
|
✗ |
(*q_ptr)[(ULong)i] = config[i]; |
513 |
|
|
} |
514 |
|
✗ |
output = q_ptr; |
515 |
|
✗ |
return success; |
516 |
|
✗ |
} catch (const std::exception& exc) { |
517 |
|
✗ |
throw hpp::Error(exc.what()); |
518 |
|
|
} |
519 |
|
|
} |
520 |
|
|
|
521 |
|
✗ |
bool Problem::buildAndProjectPath(hpp::ID IDedge, const hpp::floatSeq& qb, |
522 |
|
|
const hpp::floatSeq& qe, |
523 |
|
|
CORBA::Long& indexNotProj, |
524 |
|
|
CORBA::Long& indexProj) { |
525 |
|
|
/// First get the constraint. |
526 |
|
✗ |
graph::EdgePtr_t edge; |
527 |
|
|
try { |
528 |
|
|
edge = |
529 |
|
✗ |
HPP_DYNAMIC_PTR_CAST(graph::Edge, graph()->get((size_t)IDedge).lock()); |
530 |
|
✗ |
if (!edge) { |
531 |
|
✗ |
std::stringstream ss; |
532 |
|
✗ |
ss << "ID " << IDedge << " is not an edge"; |
533 |
|
✗ |
std::string errmsg = ss.str(); |
534 |
|
✗ |
throw Error(errmsg.c_str()); |
535 |
|
|
} |
536 |
|
|
// If steering method is not completely set in the graph, create |
537 |
|
|
// one. |
538 |
|
✗ |
if (!edge->parentGraph()->problem()->manipulationSteeringMethod() || |
539 |
|
✗ |
!edge->parentGraph() |
540 |
|
✗ |
->problem() |
541 |
|
✗ |
->manipulationSteeringMethod() |
542 |
|
✗ |
->innerSteeringMethod()) { |
543 |
|
✗ |
problemSolver()->initSteeringMethod(); |
544 |
|
✗ |
if (!edge->parentGraph()->problem()->manipulationSteeringMethod() || |
545 |
|
✗ |
!edge->parentGraph() |
546 |
|
✗ |
->problem() |
547 |
|
✗ |
->manipulationSteeringMethod() |
548 |
|
✗ |
->innerSteeringMethod()) |
549 |
|
✗ |
throw Error("Could not initialize the steering method."); |
550 |
|
|
} |
551 |
|
✗ |
bool success = false; |
552 |
|
✗ |
DevicePtr_t robot = getRobotOrThrow(problemSolver()); |
553 |
|
✗ |
Configuration_t q1 = floatSeqToConfig(robot, qb, true); |
554 |
|
✗ |
Configuration_t q2 = floatSeqToConfig(robot, qe, true); |
555 |
|
✗ |
core::PathVectorPtr_t pv; |
556 |
|
✗ |
indexNotProj = -1; |
557 |
|
✗ |
indexProj = -1; |
558 |
|
✗ |
core::PathPtr_t path; |
559 |
|
✗ |
success = edge->build(path, q1, q2); |
560 |
|
✗ |
if (!success) return false; |
561 |
|
✗ |
pv = HPP_DYNAMIC_PTR_CAST(core::PathVector, path); |
562 |
|
✗ |
indexNotProj = (CORBA::Long)problemSolver()->paths().size(); |
563 |
|
✗ |
if (!pv) { |
564 |
|
✗ |
pv = core::PathVector::create(path->outputSize(), |
565 |
|
✗ |
path->outputDerivativeSize()); |
566 |
|
✗ |
pv->appendPath(path); |
567 |
|
|
} |
568 |
|
✗ |
problemSolver()->addPath(pv); |
569 |
|
|
|
570 |
|
✗ |
core::PathPtr_t projPath; |
571 |
|
|
PathProjectorPtr_t pathProjector( |
572 |
|
✗ |
problemSolver()->problem()->pathProjector()); |
573 |
|
✗ |
if (!pathProjector) { |
574 |
|
✗ |
problemSolver()->initPathProjector(); |
575 |
|
✗ |
pathProjector = problemSolver()->problem()->pathProjector(); |
576 |
|
|
} |
577 |
|
✗ |
if (pathProjector) { |
578 |
|
✗ |
success = pathProjector->apply(path, projPath); |
579 |
|
|
} else { |
580 |
|
✗ |
success = true; |
581 |
|
✗ |
projPath = path->copy(); |
582 |
|
|
} |
583 |
|
|
|
584 |
|
✗ |
if (!success) { |
585 |
|
✗ |
if (!projPath || projPath->length() == 0) return false; |
586 |
|
|
} |
587 |
|
✗ |
pv = HPP_DYNAMIC_PTR_CAST(core::PathVector, projPath); |
588 |
|
✗ |
indexProj = (CORBA::Long)problemSolver()->paths().size(); |
589 |
|
✗ |
if (!pv) { |
590 |
|
✗ |
pv = core::PathVector::create(projPath->outputSize(), |
591 |
|
✗ |
projPath->outputDerivativeSize()); |
592 |
|
✗ |
pv->appendPath(projPath); |
593 |
|
|
} |
594 |
|
✗ |
problemSolver()->addPath(pv); |
595 |
|
✗ |
return success; |
596 |
|
✗ |
} catch (const std::exception& exc) { |
597 |
|
✗ |
throw hpp::Error(exc.what()); |
598 |
|
|
} |
599 |
|
|
} |
600 |
|
|
|
601 |
|
✗ |
void Problem::setTargetState(hpp::ID IDstate) { |
602 |
|
|
try { |
603 |
|
✗ |
graph::GraphComponentPtr_t comp = graph()->get((size_t)IDstate).lock(); |
604 |
|
✗ |
graph::StatePtr_t state = HPP_DYNAMIC_PTR_CAST(graph::State, comp); |
605 |
|
✗ |
if (!state) { |
606 |
|
✗ |
HPP_THROW(Error, "ID " << IDstate << " is not a state."); |
607 |
|
|
} |
608 |
|
✗ |
problemSolver()->setTargetState(state); |
609 |
|
✗ |
} catch (const std::exception& exc) { |
610 |
|
✗ |
throw hpp::Error(exc.what()); |
611 |
|
|
} |
612 |
|
|
} |
613 |
|
|
|
614 |
|
✗ |
ID Problem::edgeAtParam(ULong pathId, Double param, String_out name) { |
615 |
|
|
try { |
616 |
|
✗ |
if (pathId >= problemSolver()->paths().size()) { |
617 |
|
✗ |
HPP_THROW(Error, "Wrong path id: " << pathId << ", number path: " |
618 |
|
|
<< problemSolver()->paths().size() |
619 |
|
|
<< "."); |
620 |
|
|
} |
621 |
|
✗ |
core::PathVectorPtr_t path = problemSolver()->paths()[pathId]; |
622 |
|
|
core::PathVectorPtr_t flat = core::PathVector::create( |
623 |
|
✗ |
path->outputSize(), path->outputDerivativeSize()); |
624 |
|
✗ |
path->flatten(flat); |
625 |
|
|
value_type unused; |
626 |
|
✗ |
std::size_t r = flat->rankAtParam(param, unused); |
627 |
|
✗ |
core::PathPtr_t p = flat->pathAtRank(r); |
628 |
|
|
manipulation::ConstraintSetPtr_t constraint = |
629 |
|
✗ |
HPP_DYNAMIC_PTR_CAST(manipulation::ConstraintSet, p->constraints()); |
630 |
|
✗ |
if (!constraint) { |
631 |
|
✗ |
HPP_THROW(Error, "Path constraint is not of the good type " |
632 |
|
|
<< "at id " << pathId << ", param " << param |
633 |
|
|
<< " (rank: " << r << ")"); |
634 |
|
|
} |
635 |
|
✗ |
if (!constraint->edge()) { |
636 |
|
✗ |
HPP_THROW(Error, "Path constraint does not contain edge information " |
637 |
|
|
<< "at id " << pathId << ", param " << param |
638 |
|
|
<< " (rank: " << r << ")"); |
639 |
|
|
} |
640 |
|
✗ |
if (constraint->edge()->parentGraph()) |
641 |
|
✗ |
name = constraint->edge()->parentGraph()->name().c_str(); |
642 |
|
|
else |
643 |
|
✗ |
name = "Parent graph was destroyed."; |
644 |
|
✗ |
return (ID)constraint->edge()->id(); |
645 |
|
✗ |
} catch (const std::exception& exc) { |
646 |
|
✗ |
throw hpp::Error(exc.what()); |
647 |
|
|
} |
648 |
|
|
} |
649 |
|
|
|
650 |
|
✗ |
manipulation_idl::graph_idl::Validation_ptr Problem::createGraphValidation() { |
651 |
|
✗ |
core::ProblemSolverPtr_t ps = problemSolver(); |
652 |
|
✗ |
graph::ValidationPtr_t validation(new graph::Validation(ps->problem())); |
653 |
|
|
|
654 |
|
|
typedef manipulation_impl::graph_impl::Validation Validation_impl; |
655 |
|
|
|
656 |
|
|
manipulation_idl::graph_idl::Validation_var validation_idl = |
657 |
|
|
makeServantDownCast<Validation_impl>( |
658 |
|
✗ |
server_->parent(), Validation_impl::Storage(validation)); |
659 |
|
✗ |
return validation_idl._retn(); |
660 |
|
|
} |
661 |
|
|
|
662 |
|
✗ |
core_idl::Roadmap_ptr Problem::readRoadmap( |
663 |
|
|
const char* filename, pinocchio_idl::Device_ptr robot, |
664 |
|
|
manipulation_idl::graph_idl::Graph_ptr graph) { |
665 |
|
|
try { |
666 |
|
|
pinocchio::DevicePtr_t device = |
667 |
|
✗ |
reference_to_object<pinocchio::Device>(server_->parent(), robot); |
668 |
|
|
graph::GraphPtr_t _graph = |
669 |
|
✗ |
reference_to_object<graph::Graph>(server_->parent(), graph); |
670 |
|
|
|
671 |
|
✗ |
hpp::core::RoadmapPtr_t roadmap; |
672 |
|
✗ |
std::string fn(filename); |
673 |
|
✗ |
bool xml = (fn.size() >= 4 && fn.compare(fn.size() - 4, 4, ".xml") == 0); |
674 |
|
|
using namespace core::parser; |
675 |
|
✗ |
if (xml) { |
676 |
|
|
typedef hpp::serialization::archive_tpl< |
677 |
|
|
boost::archive::xml_iarchive, |
678 |
|
|
hpp::serialization::remove_duplicate::vector_archive> |
679 |
|
|
archive_type; |
680 |
|
✗ |
serializeRoadmap<archive_type>(roadmap, fn, |
681 |
|
✗ |
make_nvp(device->name(), device.get()), |
682 |
|
✗ |
make_nvp(_graph->name(), _graph.get())); |
683 |
|
|
} else { |
684 |
|
|
typedef hpp::serialization::archive_tpl< |
685 |
|
|
boost::archive::binary_iarchive, |
686 |
|
|
hpp::serialization::remove_duplicate::vector_archive> |
687 |
|
|
archive_type; |
688 |
|
✗ |
serializeRoadmap<archive_type>(roadmap, fn, |
689 |
|
✗ |
make_nvp(device->name(), device.get()), |
690 |
|
✗ |
make_nvp(_graph->name(), _graph.get())); |
691 |
|
|
} |
692 |
|
|
|
693 |
|
|
core_idl::Roadmap_var o = |
694 |
|
✗ |
makeServantDownCast<core_impl::Roadmap>(server_->parent(), roadmap); |
695 |
|
✗ |
return o._retn(); |
696 |
|
✗ |
} catch (const std::exception& exc) { |
697 |
|
✗ |
throw hpp::Error(exc.what()); |
698 |
|
|
} |
699 |
|
|
} |
700 |
|
|
|
701 |
|
✗ |
void Problem::writeRoadmap(const char* filename, core_idl::Roadmap_ptr _roadmap, |
702 |
|
|
pinocchio_idl::Device_ptr robot, |
703 |
|
|
manipulation_idl::graph_idl::Graph_ptr graph) { |
704 |
|
|
try { |
705 |
|
|
pinocchio::DevicePtr_t device = |
706 |
|
✗ |
reference_to_object<pinocchio::Device>(server_->parent(), robot); |
707 |
|
|
graph::GraphPtr_t _graph = |
708 |
|
✗ |
reference_to_object<graph::Graph>(server_->parent(), graph); |
709 |
|
|
core::RoadmapPtr_t roadmap = |
710 |
|
✗ |
reference_to_object<core::Roadmap>(server_->parent(), _roadmap); |
711 |
|
|
|
712 |
|
✗ |
std::string fn(filename); |
713 |
|
✗ |
bool xml = (fn.size() >= 4 && fn.compare(fn.size() - 4, 4, ".xml") == 0); |
714 |
|
|
using namespace core::parser; |
715 |
|
✗ |
if (xml) { |
716 |
|
|
typedef hpp::serialization::archive_tpl< |
717 |
|
|
boost::archive::xml_oarchive, |
718 |
|
|
hpp::serialization::remove_duplicate::vector_archive> |
719 |
|
|
archive_type; |
720 |
|
✗ |
serializeRoadmap<archive_type>(roadmap, fn, |
721 |
|
✗ |
make_nvp(device->name(), device.get()), |
722 |
|
✗ |
make_nvp(_graph->name(), _graph.get())); |
723 |
|
|
} else { |
724 |
|
|
typedef hpp::serialization::archive_tpl< |
725 |
|
|
boost::archive::binary_oarchive, |
726 |
|
|
hpp::serialization::remove_duplicate::vector_archive> |
727 |
|
|
archive_type; |
728 |
|
✗ |
serializeRoadmap<archive_type>(roadmap, fn, |
729 |
|
✗ |
make_nvp(device->name(), device.get()), |
730 |
|
✗ |
make_nvp(_graph->name(), _graph.get())); |
731 |
|
|
} |
732 |
|
✗ |
} catch (const std::exception& exc) { |
733 |
|
✗ |
throw hpp::Error(exc.what()); |
734 |
|
|
} |
735 |
|
|
} |
736 |
|
|
|
737 |
|
✗ |
core_idl::Roadmap_ptr Problem::createRoadmap(core_idl::Distance_ptr distance, |
738 |
|
|
pinocchio_idl::Device_ptr robot) { |
739 |
|
|
core_idl::Roadmap_var o = makeServantDownCast<core_impl::Roadmap>( |
740 |
|
✗ |
server_->parent(), |
741 |
|
✗ |
Roadmap::create( |
742 |
|
✗ |
reference_to_object<core::Distance>(server_->parent(), distance), |
743 |
|
✗ |
reference_to_object<pinocchio::Device>(server_->parent(), robot))); |
744 |
|
✗ |
return o._retn(); |
745 |
|
|
} |
746 |
|
✗ |
core_idl::PathPlanner_ptr Problem::createTransitionPlanner() { |
747 |
|
✗ |
ProblemSolverPtr_t ps = problemSolver(); |
748 |
|
✗ |
core::DistancePtr_t dist(core::WeighedDistance::create(ps->robot())); |
749 |
|
✗ |
core::RoadmapPtr_t roadmap(core::Roadmap::create(dist, ps->robot())); |
750 |
|
✗ |
ProblemPtr_t problem(ps->problem()); |
751 |
|
|
core::PathPlannerPtr_t planner( |
752 |
|
✗ |
manipulation::pathPlanner::TransitionPlanner::createWithRoadmap(problem, |
753 |
|
✗ |
roadmap)); |
754 |
|
|
core_idl::PathPlanner_var o = |
755 |
|
✗ |
makeServantDownCast<core_impl::PathPlanner>(server_->parent(), planner); |
756 |
|
✗ |
return o._retn(); |
757 |
|
|
} |
758 |
|
|
} // namespace impl |
759 |
|
|
} // namespace manipulation |
760 |
|
|
} // namespace hpp |
761 |
|
|
|