hpp-corbaserver  4.10.1
Corba server for Humanoid Path Planner applications
problem.impl.hh
Go to the documentation of this file.
1 // Copyright (C) 2009, 2010 by Florent Lamiraux, Thomas Moulard, JRL.
2 //
3 // This file is part of the hpp-corbaserver.
4 //
5 // This software is provided "as is" without warranty of any kind,
6 // either expressed or implied, including but not limited to the
7 // implied warranties of fitness for a particular purpose.
8 //
9 // See the COPYING file for more information.
10 
11 #ifndef HPP_CORBASERVER_PROBLEM_IMPL_HH
12 # define HPP_CORBASERVER_PROBLEM_IMPL_HH
13 # include <vector>
14 # include <stdlib.h>
15 
16 # include "hpp/corbaserver/fwd.hh"
19 
22 
24 
25 namespace hpp
26 {
27  namespace corbaServer
28  {
29  namespace impl
30  {
31  using CORBA::Any;
32  using CORBA::Boolean;
33  using CORBA::Long;
34  using CORBA::ULong;
35  using CORBA::UShort;
36 
38  class Problem : public virtual POA_hpp::corbaserver::Problem
39  {
40  public:
41  Problem ();
42 
43  void setServer (ServerPlugin* server)
44  {
45  server_ = server;
46  }
47 
48  virtual Names_t* getAvailable (const char* what);
49 
50  virtual Names_t* getSelected (const char* what);
51 
52  virtual void setParameter (const char* name, const Any& value);
53 
54  virtual Any* getParameter (const char* name);
55 
56  virtual char* getParameterDoc (const char* name);
57 
58  virtual bool selectProblem (const char* problemName);
59 
60  virtual void resetProblem ();
61 
62  virtual bool loadPlugin (const char* pluginName);
63 
64  virtual void movePathToProblem (ULong pathId, const char* problemName,
65  const Names_t& jointNames);
66 
67  virtual void setMaxNumThreads (UShort n);
68 
69  virtual UShort getMaxNumThreads ();
70 
71  virtual void
72  setRandomSeed (const Long seed) {
73  srand ((int) seed);
74  }
75 
76  virtual void
77  setInitialConfig (const hpp::floatSeq& dofArray);
78 
79  virtual hpp::floatSeq*
81 
82  virtual void
83  addGoalConfig (const hpp::floatSeq& dofArray);
84 
85  virtual hpp::floatSeqSeq* getGoalConfigs ();
86 
87  virtual void
88  resetGoalConfigs () ;
89 
90  virtual void createOrientationConstraint
91  (const char* constraintName, const char* joint1Name,
92  const char* joint2Name, const Double* p, const hpp::boolSeq& mask);
93 
95  (const char* constraintName, const char* joint1Name,
96  const char* joint2Name, const Transform_ p, const hpp::boolSeq& mask);
97 
99  (const char* constraintName, const char* joint1Name,
100  const char* joint2Name, const Transform_ frame1,
101  const Transform_ frame2, const hpp::boolSeq& mask);
102 
104  (const char* constraintName, const char* joint1Name,
105  const char* joint2Name, const Transform_ frame1,
106  const Transform_ frame2, const hpp::boolSeq& mask);
107 
108  virtual void createLockedJoint (const char* lockedJointName,
109  const char* jointName,
110  const hpp::floatSeq& value);
111 
112  virtual void createLockedJointWithComp
113  (const char* lockedJointName, const char* jointName,
114  const hpp::floatSeq& value, const hpp::ComparisonTypes_t& comp);
115 
116  virtual void createLockedExtraDof (const char* lockedDofName,
117  ULong index,
118  const hpp::floatSeq& value);
119 
120  virtual void createManipulability (const char* name, const char* function);
121 
122  void createRelativeComConstraint (const char* constraintName,
123  const char* comn, const char* jointName, const floatSeq& point,
124  const hpp::boolSeq& mask);
125 
126  void createComBeetweenFeet (const char* constraintName, const char* comn,
127  const char* jointLName, const char* jointRName,
128  const floatSeq& pointL, const floatSeq& pointR,
129  const char* jointRefName, const floatSeq& pRef,
130  const hpp::boolSeq& mask);
131 
133  (const char* constraintName, const Names_t& floorJoints,
134  const Names_t& objectJoints,
135  const hpp::floatSeqSeq& points, const hpp::intSeqSeq& objTriangles,
136  const hpp::intSeqSeq& floorTriangles);
137 
139  const char* constraintName, const hpp::Names_t& jointNames,
140  const hpp::floatSeqSeq& points, const hpp::floatSeqSeq& normals,
141  const char* comRootJointName);
142 
143  virtual void createPositionConstraint (const char* constraintName,
144  const char* joint1Name,
145  const char* joint2Name,
146  const hpp::floatSeq& point1,
147  const hpp::floatSeq& point2,
148  const hpp::boolSeq& mask);
149 
150  virtual void createConfigurationConstraint (const char* constraintName,
151  const hpp::floatSeq& goal,
152  const hpp::floatSeq& weights);
153 
155  (const char* constraintName, const char* joint1Name,
156  const char* joint2Name, Double distance);
157 
159  (const char* constraintName, const char* joint1Name,
160  const hpp::Names_t& objects, Double distance);
161 
162  virtual void createIdentityConstraint
163  (const char* constraintName, const Names_t& inJoints,
164  const hpp::Names_t& outJoints);
165 
166  virtual bool applyConstraints (const hpp::floatSeq& input,
167  hpp::floatSeq_out output,
168  Double& residualError);
169 
170  virtual bool optimize (const hpp::floatSeq& input,
171  hpp::floatSeq_out output,
172  hpp::floatSeq_out residualError);
173 
174  virtual void computeValueAndJacobian
175  (const hpp::floatSeq& config, hpp::floatSeq_out value,
176  hpp::floatSeqSeq_out jacobian);
177 
178  virtual bool generateValidConfig (ULong maxIter,
179  hpp::floatSeq_out output,
180  Double& residualError);
181 
182  virtual void addPassiveDofs (const char* constraintName,
183  const hpp::Names_t& dofName);
184 
185  virtual void getConstraintDimensions (const char* constraintName,
186  ULong& inputSize , ULong& inputDerivativeSize,
187  ULong& outputSize, ULong& outputDerivativeSize);
188 
189  virtual void setConstantRightHandSide (const char* constraintName,
190  CORBA::Boolean constant);
191 
192  virtual bool getConstantRightHandSide (const char* constraintName);
193 
194  virtual floatSeq* getRightHandSide ();
195 
196  virtual void setRightHandSide (const hpp::floatSeq& rhs);
197 
198  virtual void setRightHandSideFromConfig (const hpp::floatSeq& config);
199 
200  virtual void setRightHandSideByName (const char* constraintName,
201  const hpp::floatSeq& rhs);
202 
203  virtual void setRightHandSideFromConfigByName (const char* constraintName,
204  const hpp::floatSeq& config);
205 
206  virtual void resetConstraints ();
207  virtual void resetConstraintMap ();
208  virtual void addNumericalConstraints
209  (const char* constraintName, const hpp::Names_t& constraintNames,
210  const hpp::intSeq& priorities);
212  (const bool optional);
213 
214  virtual void addLockedJointConstraints
215  (const char* configProjName,const hpp::Names_t& lockedJointNames);
216 
217  virtual char* displayConstraints ();
218 
219  virtual void filterCollisionPairs ();
220  virtual Double getErrorThreshold ();
221  virtual void setErrorThreshold (Double threshold);
222  virtual void setDefaultLineSearchType (const char* type);
223  virtual ULong getMaxIterProjection ();
224  virtual void setMaxIterProjection (ULong iterations);
225  virtual ULong getMaxIterPathPlanning ();
226  virtual void setMaxIterPathPlanning (ULong iterations);
227  virtual void setTimeOutPathPlanning (double timeOut);
228  virtual double getTimeOutPathPlanning ();
229 
230 
231  virtual void addPathOptimizer (const char* pathOptimizerType);
232 
233  virtual void clearPathOptimizers ();
234 
235  virtual void addConfigValidation (const char* configValidationType);
236 
237  virtual void clearConfigValidations ();
238 
239  virtual void selectPathValidation (const char* pathValidationType,
240  Double tolerance);
241 
242  virtual void selectPathProjector (const char* pathProjectorType,
243  Double tolerance);
244 
245  virtual void selectPathPlanner (const char* pathPlannerType);
246 
247  virtual void selectDistance (const char* distanceType);
248 
249  virtual void selectSteeringMethod (const char* steeringMethodType);
250 
251  virtual void selectConfigurationShooter (const char* configurationShooterType);
252 
253  virtual bool prepareSolveStepByStep ();
254  virtual bool executeOneStep ();
255  virtual void finishSolveStepByStep ();
256 
257  virtual hpp::intSeq* solve ();
258 
259  virtual bool directPath (const hpp::floatSeq& startConfig,
260  const hpp::floatSeq& endConfig,
261  CORBA::Boolean validate,
262  ULong& pathId,
263  CORBA::String_out report);
264 
265  virtual bool reversePath(ULong pathId, ULong& reversedPathId);
266 
267  virtual void addConfigToRoadmap (const hpp::floatSeq& config);
268 
269  virtual void addEdgeToRoadmap (const hpp::floatSeq& config1,
270  const hpp::floatSeq& config2,
271  ULong pathId, bool bothEdges);
272 
273  virtual void appendDirectPath (ULong pathId,
274  const hpp::floatSeq& config,
275  Boolean validate);
276 
277  virtual void concatenatePath (ULong startId, ULong endId);
278 
279  virtual void extractPath (ULong pathId, Double start, Double end);
280 
281  virtual void erasePath (ULong pathId);
282 
283  virtual bool projectPath (ULong pathId);
284 
285  virtual void interruptPathPlanning ();
286 
287  virtual Long numberPaths ();
288 
289  virtual hpp::intSeq* optimizePath (ULong pathId);
290 
291  virtual Double pathLength (ULong pathId);
292 
293  virtual hpp::floatSeq* configAtParam (ULong pathId,
294  Double atDistance);
295 
296  virtual hpp::floatSeq* derivativeAtParam (ULong pathId,
297  ULong order,
298  Double atDistance);
299 
300  virtual hpp::floatSeqSeq* getWaypoints (ULong inPathId, floatSeq_out times);
301  virtual hpp::floatSeqSeq* nodes ();
302  virtual Long numberEdges ();
303  virtual void edge (ULong edgeId, hpp::floatSeq_out q1,
304  hpp::floatSeq_out q2);
305  virtual Long connectedComponentOfEdge (ULong edgeId);
306  virtual hpp::floatSeq* node (ULong nodeId);
307  virtual Long connectedComponentOfNode (ULong nodeId);
308  virtual Long numberNodes ();
309  virtual Long numberConnectedComponents ();
310  virtual hpp::floatSeqSeq*
311  nodesConnectedComponent (ULong connectedComponentId);
312 
313  virtual hpp::floatSeq*
314  getNearestConfig (const hpp::floatSeq& config, const Long connectedComponentId,
315  hpp::core::value_type& distance);
316 
317  virtual void clearRoadmap ();
318  virtual void resetRoadmap ();
319  virtual void saveRoadmap (const char* filename);
320  virtual void readRoadmap (const char* filename);
321 
322  virtual void scCreateScalarMultiply (const char* outName, Double scalar, const char* inName);
323 
325 
327 
328  hpp::core_idl::Path_ptr getPath (ULong pathId);
329 
331 
333 
335 
337 
339 
341 
343 
344  private:
346  core::ProblemSolverPtr_t problemSolver ();
348  ServerPlugin* server_;
349  };
350  } // end of namespace impl.
351  } // end of namespace corbaServer.
352 } // end of namespace hpp.
353 
354 #endif
virtual void setRightHandSideFromConfigByName(const char *constraintName, const hpp::floatSeq &config)
virtual hpp::intSeq * solve()
virtual void edge(ULong edgeId, hpp::floatSeq_out q1, hpp::floatSeq_out q2)
Definition: server-plugin.hh:44
_objref_PathPlanner * PathPlanner_ptr
Definition: path_planners-idl.hh:242
virtual void addLockedJointConstraints(const char *configProjName, const hpp::Names_t &lockedJointNames)
virtual void addPathOptimizer(const char *pathOptimizerType)
virtual void extractPath(ULong pathId, Double start, Double end)
hpp::core_idl::Problem_ptr getProblem()
virtual hpp::floatSeq * node(ULong nodeId)
virtual ULong getMaxIterProjection()
virtual void setMaxNumThreads(UShort n)
virtual void scCreateScalarMultiply(const char *outName, Double scalar, const char *inName)
virtual hpp::floatSeq * derivativeAtParam(ULong pathId, ULong order, Double atDistance)
virtual bool prepareSolveStepByStep()
hpp::core_idl::PathPlanner_ptr getPathPlanner()
virtual void addGoalConfig(const hpp::floatSeq &dofArray)
_objref_SteeringMethod * SteeringMethod_ptr
Definition: _problem-idl.hh:161
virtual void createPositionConstraint(const char *constraintName, const char *joint1Name, const char *joint2Name, const hpp::floatSeq &point1, const hpp::floatSeq &point2, const hpp::boolSeq &mask)
virtual void setRandomSeed(const Long seed)
Definition: problem.impl.hh:72
Implement CORBA interface ``Obstacle&#39;&#39;.
Definition: basic-server.hh:27
virtual ULong getMaxIterPathPlanning()
virtual void setParameter(const char *name, const Any &value)
virtual void setDefaultLineSearchType(const char *type)
virtual void createLockedJointWithComp(const char *lockedJointName, const char *jointName, const hpp::floatSeq &value, const hpp::ComparisonTypes_t &comp)
virtual Long connectedComponentOfEdge(ULong edgeId)
virtual bool reversePath(ULong pathId, ULong &reversedPathId)
virtual hpp::floatSeq * getNearestConfig(const hpp::floatSeq &config, const Long connectedComponentId, hpp::core::value_type &distance)
virtual hpp::intSeq * optimizePath(ULong pathId)
Implement CORBA interface ``Problem&#39;&#39;.
Definition: problem.impl.hh:38
virtual void selectConfigurationShooter(const char *configurationShooterType)
_objref_PathValidation * PathValidation_ptr
Definition: _problem-idl.hh:187
void setServer(ServerPlugin *server)
Definition: problem.impl.hh:43
virtual void setNumericalConstraintsLastPriorityOptional(const bool optional)
virtual bool selectProblem(const char *problemName)
ULong addPath(hpp::core_idl::PathVector_ptr _path)
sequence< floatSeq > floatSeqSeq
Definition: common.idl:33
virtual void createOrientationConstraint(const char *constraintName, const char *joint1Name, const char *joint2Name, const Double *p, const hpp::boolSeq &mask)
virtual bool optimize(const hpp::floatSeq &input, hpp::floatSeq_out output, hpp::floatSeq_out residualError)
virtual void addEdgeToRoadmap(const hpp::floatSeq &config1, const hpp::floatSeq &config2, ULong pathId, bool bothEdges)
virtual hpp::floatSeq * getInitialConfig()
virtual Long connectedComponentOfNode(ULong nodeId)
virtual bool applyConstraints(const hpp::floatSeq &input, hpp::floatSeq_out output, Double &residualError)
virtual void createConfigurationConstraint(const char *constraintName, const hpp::floatSeq &goal, const hpp::floatSeq &weights)
virtual hpp::floatSeqSeq * nodes()
virtual bool generateValidConfig(ULong maxIter, hpp::floatSeq_out output, Double &residualError)
virtual void appendDirectPath(ULong pathId, const hpp::floatSeq &config, Boolean validate)
_objref_Problem * Problem_ptr
Definition: problem-idl.hh:111
void createStaticStabilityConstraint(const char *constraintName, const hpp::Names_t &jointNames, const hpp::floatSeqSeq &points, const hpp::floatSeqSeq &normals, const char *comRootJointName)
virtual void setRightHandSide(const hpp::floatSeq &rhs)
virtual Long numberConnectedComponents()
sequence< string > Names_t
Sequence of names.
Definition: common.idl:22
virtual Names_t * getAvailable(const char *what)
sequence< long > intSeq
Definition: common.idl:29
virtual hpp::floatSeqSeq * nodesConnectedComponent(ULong connectedComponentId)
virtual UShort getMaxNumThreads()
virtual bool getConstantRightHandSide(const char *constraintName)
virtual void createDistanceBetweenJointAndObjects(const char *constraintName, const char *joint1Name, const hpp::Names_t &objects, Double distance)
virtual Any * getParameter(const char *name)
virtual void createLockedJoint(const char *lockedJointName, const char *jointName, const hpp::floatSeq &value)
virtual Names_t * getSelected(const char *what)
virtual bool directPath(const hpp::floatSeq &startConfig, const hpp::floatSeq &endConfig, CORBA::Boolean validate, ULong &pathId, CORBA::String_out report)
Definition: common-idl.hh:73
virtual void setErrorThreshold(Double threshold)
virtual void setConstantRightHandSide(const char *constraintName, CORBA::Boolean constant)
virtual void createIdentityConstraint(const char *constraintName, const Names_t &inJoints, const hpp::Names_t &outJoints)
virtual void createLockedExtraDof(const char *lockedDofName, ULong index, const hpp::floatSeq &value)
virtual void selectDistance(const char *distanceType)
virtual void addNumericalConstraints(const char *constraintName, const hpp::Names_t &constraintNames, const hpp::intSeq &priorities)
sequence< boolean > boolSeq
Definition: common.idl:28
virtual void setTimeOutPathPlanning(double timeOut)
virtual void interruptPathPlanning()
virtual bool projectPath(ULong pathId)
void setDistance(hpp::core_idl::Distance_ptr distance)
virtual void getConstraintDimensions(const char *constraintName, ULong &inputSize, ULong &inputDerivativeSize, ULong &outputSize, ULong &outputDerivativeSize)
virtual double getTimeOutPathPlanning()
virtual char * displayConstraints()
virtual void readRoadmap(const char *filename)
virtual void saveRoadmap(const char *filename)
ProblemServant< POA_hpp::core_idl::Problem, core::ProblemPtr_t > Problem
Definition: problem.hh:116
virtual void selectSteeringMethod(const char *steeringMethodType)
_objref_PathVector * PathVector_ptr
Definition: paths-idl.hh:82
virtual hpp::floatSeqSeq * getGoalConfigs()
virtual void setRightHandSideFromConfig(const hpp::floatSeq &config)
virtual void selectPathProjector(const char *pathProjectorType, Double tolerance)
virtual void concatenatePath(ULong startId, ULong endId)
void setRobot(hpp::pinocchio_idl::Device_ptr robot)
virtual void addPassiveDofs(const char *constraintName, const hpp::Names_t &dofName)
virtual void movePathToProblem(ULong pathId, const char *problemName, const Names_t &jointNames)
double Transform_[7]
Element of SE(3) represented by a vector and a unit quaternion.
Definition: common.idl:36
virtual hpp::floatSeqSeq * getWaypoints(ULong inPathId, floatSeq_out times)
virtual void createManipulability(const char *name, const char *function)
Definition: common-idl.hh:649
virtual void addConfigValidation(const char *configValidationType)
virtual void setInitialConfig(const hpp::floatSeq &dofArray)
hpp::core_idl::Path_ptr getPath(ULong pathId)
void createRelativeComConstraint(const char *constraintName, const char *comn, const char *jointName, const floatSeq &point, const hpp::boolSeq &mask)
virtual void setRightHandSideByName(const char *constraintName, const hpp::floatSeq &rhs)
virtual Double getErrorThreshold()
_objref_Implicit * Implicit_ptr
Definition: constraints-idl.hh:200
hpp::core_idl::SteeringMethod_ptr getSteeringMethod()
virtual void setMaxIterProjection(ULong iterations)
virtual void selectPathValidation(const char *pathValidationType, Double tolerance)
virtual Double pathLength(ULong pathId)
virtual void createDistanceBetweenJointConstraint(const char *constraintName, const char *joint1Name, const char *joint2Name, Double distance)
virtual void erasePath(ULong pathId)
virtual void computeValueAndJacobian(const hpp::floatSeq &config, hpp::floatSeq_out value, hpp::floatSeqSeq_out jacobian)
virtual void finishSolveStepByStep()
hpp::constraints_idl::Implicit_ptr getConstraint(const char *name)
virtual void createTransformationConstraint2(const char *constraintName, const char *joint1Name, const char *joint2Name, const Transform_ frame1, const Transform_ frame2, const hpp::boolSeq &mask)
::CORBA::Double value_type
Definition: common-idl.hh:61
Definition: common-idl.hh:570
sequence< ComparisonType > ComparisonTypes_t
Definition: common.idl:48
virtual void clearConfigValidations()
virtual char * getParameterDoc(const char *name)
virtual void createTransformationSE3Constraint(const char *constraintName, const char *joint1Name, const char *joint2Name, const Transform_ frame1, const Transform_ frame2, const hpp::boolSeq &mask)
virtual void addConfigToRoadmap(const hpp::floatSeq &config)
virtual floatSeq * getRightHandSide()
_objref_Path * Path_ptr
Definition: path_planners-idl.hh:83
sequence< double > floatSeq
Robot configuration is defined by a sequence of dof value.
Definition: common.idl:32
virtual hpp::floatSeq * configAtParam(ULong pathId, Double atDistance)
_objref_Device * Device_ptr
Definition: _problem-idl.hh:103
void createComBeetweenFeet(const char *constraintName, const char *comn, const char *jointLName, const char *jointRName, const floatSeq &pointL, const floatSeq &pointR, const char *jointRefName, const floatSeq &pRef, const hpp::boolSeq &mask)
virtual bool loadPlugin(const char *pluginName)
virtual void setMaxIterPathPlanning(ULong iterations)
_objref_Distance * Distance_ptr
Definition: _problem-idl.hh:135
sequence< intSeq > intSeqSeq
Definition: common.idl:30
virtual void createConvexShapeContactConstraint(const char *constraintName, const Names_t &floorJoints, const Names_t &objectJoints, const hpp::floatSeqSeq &points, const hpp::intSeqSeq &objTriangles, const hpp::intSeqSeq &floorTriangles)
hpp::core_idl::PathValidation_ptr getPathValidation()
virtual void createTransformationConstraint(const char *constraintName, const char *joint1Name, const char *joint2Name, const Transform_ p, const hpp::boolSeq &mask)
virtual void selectPathPlanner(const char *pathPlannerType)
hpp::core_idl::Distance_ptr getDistance()