GCC Code Coverage Report


Directory: ./
File: src/problem.impl.hh
Date: 2024-09-11 11:37:19
Exec Total Coverage
Lines: 1 2 50.0%
Branches: 0 0 -%

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