Line |
Branch |
Exec |
Source |
1 |
|
|
// Copyright (C) 2009, 2010 by Florent Lamiraux, Thomas Moulard, JRL. |
2 |
|
|
// |
3 |
|
|
|
4 |
|
|
// Redistribution and use in source and binary forms, with or without |
5 |
|
|
// modification, are permitted provided that the following conditions are |
6 |
|
|
// met: |
7 |
|
|
// |
8 |
|
|
// 1. Redistributions of source code must retain the above copyright |
9 |
|
|
// notice, this list of conditions and the following disclaimer. |
10 |
|
|
// |
11 |
|
|
// 2. Redistributions in binary form must reproduce the above copyright |
12 |
|
|
// notice, this list of conditions and the following disclaimer in the |
13 |
|
|
// documentation and/or other materials provided with the distribution. |
14 |
|
|
// |
15 |
|
|
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
16 |
|
|
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
17 |
|
|
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR |
18 |
|
|
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT |
19 |
|
|
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, |
20 |
|
|
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT |
21 |
|
|
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, |
22 |
|
|
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY |
23 |
|
|
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT |
24 |
|
|
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE |
25 |
|
|
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH |
26 |
|
|
// DAMAGE. |
27 |
|
|
// |
28 |
|
|
// This software is provided "as is" without warranty of any kind, |
29 |
|
|
// either expressed or implied, including but not limited to the |
30 |
|
|
// implied warranties of fitness for a particular purpose. |
31 |
|
|
// |
32 |
|
|
// See the COPYING file for more information. |
33 |
|
|
|
34 |
|
|
#ifndef HPP_CORBASERVER_PROBLEM_IMPL_HH |
35 |
|
|
#define HPP_CORBASERVER_PROBLEM_IMPL_HH |
36 |
|
|
#include <stdlib.h> |
37 |
|
|
|
38 |
|
|
#include <vector> |
39 |
|
|
|
40 |
|
|
#include "hpp/corbaserver/deprecated.hh" |
41 |
|
|
#include "hpp/corbaserver/fwd.hh" |
42 |
|
|
#include "hpp/corbaserver/problem-idl.hh" |
43 |
|
|
#include "hpp/corbaserver/problem-solver-map.hh" |
44 |
|
|
#include "hpp/core_idl/_problem-idl.hh" |
45 |
|
|
#include "hpp/core_idl/distances-idl.hh" |
46 |
|
|
|
47 |
|
|
namespace hpp { |
48 |
|
|
namespace corbaServer { |
49 |
|
|
namespace impl { |
50 |
|
|
using CORBA::Any; |
51 |
|
|
using CORBA::Boolean; |
52 |
|
|
using CORBA::Long; |
53 |
|
|
using CORBA::ULong; |
54 |
|
|
using CORBA::UShort; |
55 |
|
|
|
56 |
|
|
/// \brief Implement CORBA interface ``Problem''. |
57 |
|
|
class Problem : public virtual POA_hpp::corbaserver::Problem { |
58 |
|
|
public: |
59 |
|
|
Problem(); |
60 |
|
|
|
61 |
|
8 |
void setServer(ServerPlugin* server) { server_ = server; } |
62 |
|
|
|
63 |
|
|
virtual Names_t* getAvailable(const char* what); |
64 |
|
|
|
65 |
|
|
virtual Names_t* getSelected(const char* what); |
66 |
|
|
|
67 |
|
|
virtual void setParameter(const char* name, const Any& value); |
68 |
|
|
|
69 |
|
|
virtual Any* getParameter(const char* name); |
70 |
|
|
|
71 |
|
|
virtual char* getParameterDoc(const char* name); |
72 |
|
|
|
73 |
|
|
virtual bool selectProblem(const char* problemName); |
74 |
|
|
|
75 |
|
|
virtual void resetProblem(); |
76 |
|
|
|
77 |
|
|
virtual bool loadPlugin(const char* pluginName); |
78 |
|
|
|
79 |
|
|
virtual void movePathToProblem(ULong pathId, const char* problemName, |
80 |
|
|
const Names_t& jointNames); |
81 |
|
|
|
82 |
|
|
virtual void setMaxNumThreads(UShort n); |
83 |
|
|
|
84 |
|
|
virtual UShort getMaxNumThreads(); |
85 |
|
|
|
86 |
|
✗ |
virtual void setRandomSeed(const Long seed) { srand((int)seed); } |
87 |
|
|
|
88 |
|
|
virtual void setInitialConfig(const hpp::floatSeq& dofArray); |
89 |
|
|
|
90 |
|
|
virtual hpp::floatSeq* getInitialConfig(); |
91 |
|
|
|
92 |
|
|
virtual void addGoalConfig(const hpp::floatSeq& dofArray); |
93 |
|
|
|
94 |
|
|
virtual hpp::floatSeqSeq* getGoalConfigs(); |
95 |
|
|
|
96 |
|
|
virtual void resetGoalConfigs(); |
97 |
|
|
|
98 |
|
|
void setGoalConstraints(const Names_t& constraints); |
99 |
|
|
|
100 |
|
|
void resetGoalConstraints(); |
101 |
|
|
virtual void createOrientationConstraint(const char* constraintName, |
102 |
|
|
const char* joint1Name, |
103 |
|
|
const char* joint2Name, |
104 |
|
|
const Double* p, |
105 |
|
|
const hpp::boolSeq& mask); |
106 |
|
|
|
107 |
|
|
virtual void createTransformationConstraint(const char* constraintName, |
108 |
|
|
const char* joint1Name, |
109 |
|
|
const char* joint2Name, |
110 |
|
|
const Transform_ p, |
111 |
|
|
const hpp::boolSeq& mask); |
112 |
|
|
|
113 |
|
|
virtual void createTransformationConstraint2(const char* constraintName, |
114 |
|
|
const char* joint1Name, |
115 |
|
|
const char* joint2Name, |
116 |
|
|
const Transform_ frame1, |
117 |
|
|
const Transform_ frame2, |
118 |
|
|
const hpp::boolSeq& mask); |
119 |
|
|
|
120 |
|
|
virtual void createTransformationR3xSO3Constraint(const char* constraintName, |
121 |
|
|
const char* joint1Name, |
122 |
|
|
const char* joint2Name, |
123 |
|
|
const Transform_ frame1, |
124 |
|
|
const Transform_ frame2, |
125 |
|
|
const hpp::boolSeq& mask); |
126 |
|
|
|
127 |
|
|
virtual void createLockedJoint(const char* lockedJointName, |
128 |
|
|
const char* jointName, |
129 |
|
|
const hpp::floatSeq& value); |
130 |
|
|
|
131 |
|
|
virtual void createLockedJointWithComp(const char* lockedJointName, |
132 |
|
|
const char* jointName, |
133 |
|
|
const hpp::floatSeq& value, |
134 |
|
|
const hpp::ComparisonTypes_t& comp); |
135 |
|
|
|
136 |
|
|
virtual void createLockedExtraDof(const char* lockedDofName, ULong index, |
137 |
|
|
const hpp::floatSeq& value); |
138 |
|
|
|
139 |
|
|
virtual void createManipulability(const char* name, const char* function); |
140 |
|
|
|
141 |
|
|
void createRelativeComConstraint(const char* constraintName, const char* comn, |
142 |
|
|
const char* jointName, const floatSeq& point, |
143 |
|
|
const hpp::boolSeq& mask); |
144 |
|
|
|
145 |
|
|
void createComBeetweenFeet(const char* constraintName, const char* comn, |
146 |
|
|
const char* jointLName, const char* jointRName, |
147 |
|
|
const floatSeq& pointL, const floatSeq& pointR, |
148 |
|
|
const char* jointRefName, const floatSeq& pRef, |
149 |
|
|
const hpp::boolSeq& mask); |
150 |
|
|
|
151 |
|
|
virtual void createConvexShapeContactConstraint( |
152 |
|
|
const char* constraintName, const Names_t& floorJoints, |
153 |
|
|
const Names_t& objectJoints, const hpp::floatSeqSeq& points, |
154 |
|
|
const hpp::intSeqSeq& objTriangles, const hpp::intSeqSeq& floorTriangles); |
155 |
|
|
|
156 |
|
|
virtual void createPositionConstraint(const char* constraintName, |
157 |
|
|
const char* joint1Name, |
158 |
|
|
const char* joint2Name, |
159 |
|
|
const hpp::floatSeq& point1, |
160 |
|
|
const hpp::floatSeq& point2, |
161 |
|
|
const hpp::boolSeq& mask); |
162 |
|
|
|
163 |
|
|
virtual void createConfigurationConstraint(const char* constraintName, |
164 |
|
|
const hpp::floatSeq& goal, |
165 |
|
|
const hpp::floatSeq& weights); |
166 |
|
|
|
167 |
|
|
virtual void createDistanceBetweenJointConstraint(const char* constraintName, |
168 |
|
|
const char* joint1Name, |
169 |
|
|
const char* joint2Name, |
170 |
|
|
Double distance); |
171 |
|
|
|
172 |
|
|
virtual void createDistanceBetweenJointAndObjects(const char* constraintName, |
173 |
|
|
const char* joint1Name, |
174 |
|
|
const hpp::Names_t& objects, |
175 |
|
|
Double distance); |
176 |
|
|
|
177 |
|
|
virtual void createIdentityConstraint(const char* constraintName, |
178 |
|
|
const Names_t& inJoints, |
179 |
|
|
const hpp::Names_t& outJoints); |
180 |
|
|
|
181 |
|
|
virtual bool applyConstraints(const hpp::floatSeq& input, |
182 |
|
|
hpp::floatSeq_out output, |
183 |
|
|
Double& residualError); |
184 |
|
|
|
185 |
|
|
virtual bool optimize(const hpp::floatSeq& input, hpp::floatSeq_out output, |
186 |
|
|
hpp::floatSeq_out residualError); |
187 |
|
|
|
188 |
|
|
virtual void computeValueAndJacobian(const hpp::floatSeq& config, |
189 |
|
|
hpp::floatSeq_out value, |
190 |
|
|
hpp::floatSeqSeq_out jacobian); |
191 |
|
|
|
192 |
|
|
virtual bool generateValidConfig(ULong maxIter, hpp::floatSeq_out output, |
193 |
|
|
Double& residualError); |
194 |
|
|
|
195 |
|
|
virtual void addPassiveDofs(const char* constraintName, |
196 |
|
|
const hpp::Names_t& dofName); |
197 |
|
|
|
198 |
|
|
virtual void getConstraintDimensions(const char* constraintName, |
199 |
|
|
ULong& inputSize, |
200 |
|
|
ULong& inputDerivativeSize, |
201 |
|
|
ULong& outputSize, |
202 |
|
|
ULong& outputDerivativeSize); |
203 |
|
|
|
204 |
|
|
virtual void setConstantRightHandSide(const char* constraintName, |
205 |
|
|
CORBA::Boolean constant); |
206 |
|
|
|
207 |
|
|
virtual bool getConstantRightHandSide(const char* constraintName); |
208 |
|
|
|
209 |
|
|
virtual floatSeq* getRightHandSide(); |
210 |
|
|
|
211 |
|
|
virtual void setRightHandSide(const hpp::floatSeq& rhs); |
212 |
|
|
|
213 |
|
|
virtual void setRightHandSideFromConfig(const hpp::floatSeq& config); |
214 |
|
|
|
215 |
|
|
virtual void setRightHandSideByName(const char* constraintName, |
216 |
|
|
const hpp::floatSeq& rhs); |
217 |
|
|
|
218 |
|
|
virtual void setRightHandSideFromConfigByName(const char* constraintName, |
219 |
|
|
const hpp::floatSeq& config); |
220 |
|
|
|
221 |
|
|
virtual void resetConstraints(); |
222 |
|
|
virtual void resetConstraintMap(); |
223 |
|
|
virtual void addNumericalConstraints(const char* constraintName, |
224 |
|
|
const hpp::Names_t& constraintNames, |
225 |
|
|
const hpp::intSeq& priorities); |
226 |
|
|
virtual void setNumericalConstraintsLastPriorityOptional(const bool optional); |
227 |
|
|
|
228 |
|
|
virtual void addLockedJointConstraints(const char* configProjName, |
229 |
|
|
const hpp::Names_t& lockedJointNames); |
230 |
|
|
|
231 |
|
|
virtual char* displayConstraints(); |
232 |
|
|
|
233 |
|
|
virtual void filterCollisionPairs(); |
234 |
|
|
virtual Double getErrorThreshold(); |
235 |
|
|
virtual void setErrorThreshold(Double threshold); |
236 |
|
|
virtual void setDefaultLineSearchType(const char* type); |
237 |
|
|
virtual ULong getMaxIterProjection(); |
238 |
|
|
virtual void setMaxIterProjection(ULong iterations); |
239 |
|
|
virtual ULong getMaxIterPathPlanning(); |
240 |
|
|
virtual void setMaxIterPathPlanning(ULong iterations); |
241 |
|
|
virtual void setTimeOutPathPlanning(double timeOut); |
242 |
|
|
virtual double getTimeOutPathPlanning(); |
243 |
|
|
|
244 |
|
|
virtual void addPathOptimizer(const char* pathOptimizerType); |
245 |
|
|
|
246 |
|
|
virtual void clearPathOptimizers(); |
247 |
|
|
|
248 |
|
|
virtual void addConfigValidation(const char* configValidationType); |
249 |
|
|
|
250 |
|
|
virtual void clearConfigValidations(); |
251 |
|
|
|
252 |
|
|
virtual void selectPathValidation(const char* pathValidationType, |
253 |
|
|
Double tolerance); |
254 |
|
|
|
255 |
|
|
virtual void selectPathProjector(const char* pathProjectorType, |
256 |
|
|
Double tolerance); |
257 |
|
|
|
258 |
|
|
virtual void selectPathPlanner(const char* pathPlannerType); |
259 |
|
|
|
260 |
|
|
virtual void selectDistance(const char* distanceType); |
261 |
|
|
|
262 |
|
|
virtual void selectSteeringMethod(const char* steeringMethodType); |
263 |
|
|
|
264 |
|
|
virtual void selectConfigurationShooter(const char* configurationShooterType); |
265 |
|
|
|
266 |
|
|
virtual bool prepareSolveStepByStep(); |
267 |
|
|
virtual bool executeOneStep(); |
268 |
|
|
virtual void finishSolveStepByStep(); |
269 |
|
|
|
270 |
|
|
virtual hpp::intSeq* solve(); |
271 |
|
|
|
272 |
|
|
virtual bool directPath(const hpp::floatSeq& startConfig, |
273 |
|
|
const hpp::floatSeq& endConfig, |
274 |
|
|
CORBA::Boolean validate, ULong& pathId, |
275 |
|
|
CORBA::String_out report); |
276 |
|
|
|
277 |
|
|
virtual bool reversePath(ULong pathId, ULong& reversedPathId); |
278 |
|
|
|
279 |
|
|
virtual void addConfigToRoadmap(const hpp::floatSeq& config); |
280 |
|
|
|
281 |
|
|
virtual void addEdgeToRoadmap(const hpp::floatSeq& config1, |
282 |
|
|
const hpp::floatSeq& config2, ULong pathId, |
283 |
|
|
bool bothEdges); |
284 |
|
|
|
285 |
|
|
virtual void appendDirectPath(ULong pathId, const hpp::floatSeq& config, |
286 |
|
|
Boolean validate); |
287 |
|
|
|
288 |
|
|
virtual void concatenatePath(ULong startId, ULong endId); |
289 |
|
|
|
290 |
|
|
virtual void extractPath(ULong pathId, Double start, Double end); |
291 |
|
|
|
292 |
|
|
virtual void erasePath(ULong pathId); |
293 |
|
|
|
294 |
|
|
virtual bool projectPath(ULong pathId); |
295 |
|
|
|
296 |
|
|
virtual void interruptPathPlanning(); |
297 |
|
|
|
298 |
|
|
virtual Long numberPaths(); |
299 |
|
|
|
300 |
|
|
virtual hpp::intSeq* optimizePath(ULong pathId); |
301 |
|
|
|
302 |
|
|
virtual Double pathLength(ULong pathId); |
303 |
|
|
|
304 |
|
|
virtual hpp::floatSeq* configAtParam(ULong pathId, Double atDistance); |
305 |
|
|
|
306 |
|
|
virtual hpp::floatSeq* derivativeAtParam(ULong pathId, ULong order, |
307 |
|
|
Double atDistance); |
308 |
|
|
|
309 |
|
|
virtual hpp::floatSeqSeq* getWaypoints(ULong inPathId, floatSeq_out times); |
310 |
|
|
virtual hpp::floatSeqSeq* nodes(); |
311 |
|
|
virtual Long numberEdges(); |
312 |
|
|
virtual void edge(ULong edgeId, hpp::floatSeq_out q1, hpp::floatSeq_out q2); |
313 |
|
|
virtual Long connectedComponentOfEdge(ULong edgeId); |
314 |
|
|
virtual hpp::floatSeq* node(ULong nodeId); |
315 |
|
|
virtual Long connectedComponentOfNode(ULong nodeId); |
316 |
|
|
virtual Long numberNodes(); |
317 |
|
|
virtual Long numberConnectedComponents(); |
318 |
|
|
virtual hpp::floatSeqSeq* nodesConnectedComponent(ULong connectedComponentId); |
319 |
|
|
|
320 |
|
|
virtual hpp::floatSeq* getNearestConfig(const hpp::floatSeq& config, |
321 |
|
|
const Long connectedComponentId, |
322 |
|
|
hpp::core::value_type& distance); |
323 |
|
|
|
324 |
|
|
virtual void clearRoadmap(); |
325 |
|
|
virtual void resetRoadmap(); |
326 |
|
|
virtual void saveRoadmap(const char* filename); |
327 |
|
|
virtual void loadRoadmap(const char* filename); |
328 |
|
|
|
329 |
|
|
virtual void savePath(hpp::core_idl::Path_ptr path, const char* filename); |
330 |
|
|
virtual hpp::core_idl::Path_ptr loadPath(const char* filename); |
331 |
|
|
|
332 |
|
|
virtual void scCreateScalarMultiply(const char* outName, Double scalar, |
333 |
|
|
const char* inName); |
334 |
|
|
|
335 |
|
|
hpp::core_idl::Distance_ptr getDistance(); |
336 |
|
|
|
337 |
|
|
void setDistance(hpp::core_idl::Distance_ptr distance); |
338 |
|
|
|
339 |
|
|
hpp::core_idl::Path_ptr getPath(ULong pathId); |
340 |
|
|
|
341 |
|
|
ULong addPath(hpp::core_idl::PathVector_ptr _path); |
342 |
|
|
|
343 |
|
|
hpp::core_idl::SteeringMethod_ptr getSteeringMethod(); |
344 |
|
|
|
345 |
|
|
hpp::core_idl::PathValidation_ptr getPathValidation(); |
346 |
|
|
|
347 |
|
|
hpp::core_idl::PathPlanner_ptr getPathPlanner(); |
348 |
|
|
|
349 |
|
|
hpp::core_idl::Problem_ptr getProblem(); |
350 |
|
|
|
351 |
|
|
hpp::constraints_idl::Implicit_ptr getConstraint(const char* name); |
352 |
|
|
|
353 |
|
|
void setRobot(hpp::pinocchio_idl::Device_ptr robot); |
354 |
|
|
|
355 |
|
|
pinocchio_idl::CollisionObject_ptr getObstacle(const char* name); |
356 |
|
|
|
357 |
|
|
core_idl::Problem_ptr createProblem(pinocchio_idl::Device_ptr robot); |
358 |
|
|
|
359 |
|
|
core_idl::Roadmap_ptr createRoadmap(core_idl::Distance_ptr distance, |
360 |
|
|
pinocchio_idl::Device_ptr robot); |
361 |
|
|
core_idl::Roadmap_ptr readRoadmap(const char* filename, |
362 |
|
|
pinocchio_idl::Device_ptr robot); |
363 |
|
|
void writeRoadmap(const char* filename, core_idl::Roadmap_ptr roadmap, |
364 |
|
|
pinocchio_idl::Device_ptr robot); |
365 |
|
|
core_idl::PathPlanner_ptr createPathPlanner(const char* type, |
366 |
|
|
core_idl::Problem_ptr _problem, |
367 |
|
|
core_idl::Roadmap_ptr roadmap); |
368 |
|
|
core_idl::PathOptimizer_ptr createPathOptimizer( |
369 |
|
|
const char* type, core_idl::Problem_ptr _problem); |
370 |
|
|
|
371 |
|
|
core_idl::PathProjector_ptr createPathProjector(const char* type, |
372 |
|
|
core_idl::Problem_ptr robot, |
373 |
|
|
value_type parameter); |
374 |
|
|
core_idl::PathValidation_ptr createPathValidation( |
375 |
|
|
const char* type, pinocchio_idl::Device_ptr robot, value_type parameter); |
376 |
|
|
core_idl::ConfigValidation_ptr createConfigValidation( |
377 |
|
|
const char* type, pinocchio_idl::Device_ptr robot) override; |
378 |
|
|
core_idl::ConfigurationShooter_ptr createConfigurationShooter( |
379 |
|
|
const char* type, core_idl::Problem_ptr _problem); |
380 |
|
|
core_idl::Distance_ptr createDistance(const char* type, |
381 |
|
|
core_idl::Problem_ptr _problem); |
382 |
|
|
core_idl::SteeringMethod_ptr createSteeringMethod( |
383 |
|
|
const char* type, core_idl::Problem_ptr _problem); |
384 |
|
|
|
385 |
|
|
core_idl::Constraint_ptr createConstraintSet(pinocchio_idl::Device_ptr robot, |
386 |
|
|
const char* name); |
387 |
|
|
core_idl::Constraint_ptr createConfigProjector( |
388 |
|
|
pinocchio_idl::Device_ptr robot, const char* name, Double threshold, |
389 |
|
|
ULong iterations); |
390 |
|
|
|
391 |
|
|
private: |
392 |
|
|
/// Return the selected problem solver |
393 |
|
|
core::ProblemSolverPtr_t problemSolver(); |
394 |
|
|
/// \brief Pointer to the ServerPlugin owning this object. |
395 |
|
|
ServerPlugin* server_; |
396 |
|
|
}; |
397 |
|
|
} // end of namespace impl. |
398 |
|
|
} // end of namespace corbaServer. |
399 |
|
|
} // end of namespace hpp. |
400 |
|
|
|
401 |
|
|
#endif //! HPP_CORBASERVER_OBSTACLE_IMPL_HH |
402 |
|
|
|