rbprmbuilder.impl.hh 23 KB
Newer Older
Steve Tonneau's avatar
Steve Tonneau committed
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
// Copyright (c) 2014 CNRS
// Author: Florent Lamiraux
//
// This file is part of hpp-manipulation-corba.
// hpp-manipulation-corba is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// hpp-manipulation-corba is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Lesser Public License for more details.  You should have
// received a copy of the GNU Lesser General Public License along with
// hpp-manipulation-corba.  If not, see
// <http://www.gnu.org/licenses/>.

#ifndef HPP_RBPRM_CORBA_BUILDER_IMPL_HH
# define HPP_RBPRM_CORBA_BUILDER_IMPL_HH

# include <hpp/core/problem-solver.hh>
22
# include <hpp/core/path.hh>
Steve Tonneau's avatar
Steve Tonneau committed
23
# include "rbprmbuilder.hh"
Steve Tonneau's avatar
Steve Tonneau committed
24
# include <hpp/rbprm/rbprm-device.hh>
Steve Tonneau's avatar
Steve Tonneau committed
25
# include <hpp/rbprm/rbprm-fullbody.hh>
Steve Tonneau's avatar
Steve Tonneau committed
26
27
# include <hpp/rbprm/rbprm-shooter.hh>
# include <hpp/rbprm/rbprm-validation.hh>
28
# include <hpp/rbprm/sampling/analysis.hh>
29
30
31
32
# include <hpp/core/collision-path-validation-report.hh>
# include <hpp/core/problem-solver.hh>
# include <hpp/core/discretized-collision-checking.hh>
# include <hpp/core/straight-path.hh>
33
34
#include <hpp/corbaserver/affordance/server.hh>
# include <hpp/corbaserver/problem-solver-map.hh>
35
# include <hpp/rbprm/rbprm-path-validation.hh>
36
37
# include <hpp/fcl/BVH/BVH_model.h>
# include <hpp/core/config-validations.hh>
38
#include <hpp/rbprm/dynamic/dynamic-path-validation.hh>
39
40
# include "hpp/corbaserver/fwd.hh"

Steve Tonneau's avatar
Steve Tonneau committed
41
42
43
44
namespace hpp {
  namespace rbprm {
    namespace impl {
      using CORBA::Short;
45
			typedef std::map<std::string, std::vector<boost::shared_ptr<model::CollisionObject> > > affMap_t;
Steve Tonneau's avatar
Steve Tonneau committed
46

Steve Tonneau's avatar
Steve Tonneau committed
47
48
49
    struct BindShooter
    {
        BindShooter(const std::size_t shootLimit = 10000,
50
                    const std::size_t displacementLimit = 100)
Steve Tonneau's avatar
Steve Tonneau committed
51
52
53
54
55
56
            : shootLimit_(shootLimit)
            , displacementLimit_(displacementLimit) {}

        hpp::rbprm::RbPrmShooterPtr_t create (const hpp::model::DevicePtr_t& robot)
        {
            hpp::model::RbPrmDevicePtr_t robotcast = boost::static_pointer_cast<hpp::model::RbPrmDevice>(robot);
57
		        if (affMap_.empty ()) {
58
59
    	        throw hpp::Error ("No affordances found. Unable to create shooter object.");
      		  }
Steve Tonneau's avatar
Steve Tonneau committed
60
            rbprm::RbPrmShooterPtr_t shooter = hpp::rbprm::RbPrmShooter::create
61
                    (robotcast, problemSolver_->problem ()->collisionObstacles(), affMap_,
62
										romFilter_,affFilter_,shootLimit_,displacementLimit_);
Steve Tonneau's avatar
Steve Tonneau committed
63
64
65
            if(!so3Bounds_.empty())
                shooter->BoundSO3(so3Bounds_);
            return shooter;
Steve Tonneau's avatar
Steve Tonneau committed
66
        }
67
68
69
70

        hpp::core::PathValidationPtr_t createPathValidation (const hpp::model::DevicePtr_t& robot, const hpp::model::value_type& val)
        {
            hpp::model::RbPrmDevicePtr_t robotcast = boost::static_pointer_cast<hpp::model::RbPrmDevice>(robot);
71
                        affMap_ = problemSolver_->map
72
73
74
75
76
							<std::vector<boost::shared_ptr<model::CollisionObject> > > ();
		        if (affMap_.empty ()) {
    	        throw hpp::Error ("No affordances found. Unable to create Path Validaton object.");
      		  }
            hpp::rbprm::RbPrmValidationPtr_t validation
77
              (hpp::rbprm::RbPrmValidation::create(robotcast, romFilter_, affFilter_, affMap_));
78
            hpp::rbprm::RbPrmPathValidationPtr_t collisionChecking = hpp::rbprm::RbPrmPathValidation::create(robot,val);
79
            collisionChecking->add (validation);
80
81
            problemSolver_->problem()->configValidation(core::ConfigValidations::create ());
            problemSolver_->problem()->configValidations()->add(validation);
82
            return collisionChecking;
83
84
        }

85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
        hpp::core::PathValidationPtr_t createDynamicPathValidation (const hpp::model::DevicePtr_t& robot, const hpp::model::value_type& val)
        {
          hpp::model::RbPrmDevicePtr_t robotcast = boost::static_pointer_cast<hpp::model::RbPrmDevice>(robot);
          affMap_ = problemSolver_->map
            <std::vector<boost::shared_ptr<model::CollisionObject> > > ();
          if (affMap_.empty ()) {
            throw hpp::Error ("No affordances found. Unable to create Path Validaton object.");
          }
          hpp::rbprm::RbPrmValidationPtr_t validation
            (hpp::rbprm::RbPrmValidation::create(robotcast, romFilter_, affFilter_, affMap_));
          hpp::rbprm::DynamicPathValidationPtr_t collisionChecking = hpp::rbprm::DynamicPathValidation::create(robot,val);
          collisionChecking->add (validation);
          problemSolver_->problem()->configValidation(core::ConfigValidations::create ());
          problemSolver_->problem()->configValidations()->add(validation);
          // build the dynamicValidation :
          double sizeFootX,sizeFootY,mass,mu;
          bool rectangularContact;
          try {
103
104
105
106
            boost::any value_x = problemSolver_->problem()->get<boost::any> (std::string("sizeFootX"));
            boost::any value_y = problemSolver_->problem()->get<boost::any> (std::string("sizeFootY"));
            sizeFootX = boost::any_cast<double>(value_x)/2.;
            sizeFootY = boost::any_cast<double>(value_y)/2.;
107
108
109
110
111
112
113
114
            rectangularContact = 1;
          } catch (const std::exception& e) {
            hppDout(warning,"Warning : size of foot not definied, use 0 (contact point)");
            sizeFootX =0;
            sizeFootY =0;
            rectangularContact = 0;
          }
          mass = robot->mass();
115
116
117
118
119
120
121
122
          try {
            boost::any value = problemSolver_->problem()->get<boost::any> (std::string("friction"));
            mu = boost::any_cast<double>(value);
            hppDout(notice,"dynamic val : mu define in python : "<<mu);
          } catch (const std::exception& e) {
            mu= 0.5;
            hppDout(notice,"dynamic val : mu not defined, take : "<<mu<<" as default.");
          }
123
124
125
126
127
128
          DynamicValidationPtr_t dynamicVal = DynamicValidation::create(rectangularContact,sizeFootX,sizeFootY,mass,mu);
          collisionChecking->addDynamicValidator(dynamicVal);

          return collisionChecking;
        }

Steve Tonneau's avatar
Steve Tonneau committed
129
        hpp::core::ProblemSolverPtr_t problemSolver_;
Steve Tonneau's avatar
Steve Tonneau committed
130
        std::vector<std::string> romFilter_;
131
        std::map<std::string, std::vector<std::string> > affFilter_;
Steve Tonneau's avatar
Steve Tonneau committed
132
133
        std::size_t shootLimit_;
        std::size_t displacementLimit_;
Steve Tonneau's avatar
Steve Tonneau committed
134
        std::vector<double> so3Bounds_;
135
				affMap_t affMap_;
Steve Tonneau's avatar
Steve Tonneau committed
136
137
    };

Steve Tonneau's avatar
Steve Tonneau committed
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
    class FullBodyMap {
      public:
        typedef std::map<std::string, rbprm::RbPrmFullBodyPtr_t> fMap_t;

        std::string selected_;
        fMap_t map_;

        FullBodyMap (const std::string& name = "None") :
          selected_ (name)
          {
            //map_[selected_] = init;
          }

        rbprm::RbPrmFullBodyPtr_t operator-> () {
          return selected();
        }
        operator rbprm::RbPrmFullBodyPtr_t () {
          return selected();
        }
        rbprm::RbPrmFullBodyPtr_t selected () {
          return map_[selected_];
        }
        bool has (const std::string& name) const
        {
          // ProblemMap_t::const_iterator it = map_.find (name);
          // return it != map_.end ();
          return map_.end() != map_.find (name);
        }
        template <typename ReturnType> ReturnType keys () const
        {
          ReturnType l;
          for (fMap_t::const_iterator it = map_.begin ();
              it != map_.end (); ++it)
            l.push_back (it->first);
          return l;
        }
    };

Steve Tonneau's avatar
Steve Tonneau committed
176
177
178
      class RbprmBuilder : public virtual POA_hpp::corbaserver::rbprm::RbprmBuilder
      {
        public:
Steve Tonneau's avatar
Steve Tonneau committed
179
        RbprmBuilder ();
Steve Tonneau's avatar
Steve Tonneau committed
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194

        virtual void loadRobotRomModel (const char* robotName,
                 const char* rootJointType,
                 const char* packageName,
                 const char* modelName,
                 const char* urdfSuffix,
                 const char* srdfSuffix) throw (hpp::Error);

        virtual void loadRobotCompleteModel (const char* robotName,
                 const char* rootJointType,
                 const char* packageName,
                 const char* modelName,
                 const char* urdfSuffix,
                 const char* srdfSuffix) throw (hpp::Error);

Steve Tonneau's avatar
Steve Tonneau committed
195
196
197
198
199
200

        virtual void loadFullBodyRobot (const char* robotName,
                 const char* rootJointType,
                 const char* packageName,
                 const char* modelName,
                 const char* urdfSuffix,
Steve Tonneau's avatar
Steve Tonneau committed
201
202
                 const char* srdfSuffix,
                 const char* selectedProblem) throw (hpp::Error);
Steve Tonneau's avatar
Steve Tonneau committed
203

204
205
        virtual void loadFullBodyRobotFromExistingRobot () throw (hpp::Error);

206
        void setStaticStability(const bool staticStability) throw (hpp::Error);
207

208
209
210
        void setReferenceConfig(const hpp::floatSeq &referenceConfig) throw (hpp::Error);


Steve Tonneau's avatar
Steve Tonneau committed
211
        virtual void setFilter(const hpp::Names_t& roms) throw (hpp::Error);
212
				virtual void setAffordanceFilter(const char* romName, const hpp::Names_t& affordances) throw (hpp::Error);
Steve Tonneau's avatar
Steve Tonneau committed
213
214
        virtual void boundSO3(const hpp::floatSeq& limitszyx) throw (hpp::Error);

Steve Tonneau's avatar
Steve Tonneau committed
215

Steve Tonneau's avatar
Steve Tonneau committed
216
        virtual hpp::floatSeq* getSampleConfig(const char* limb, unsigned short sampleId) throw (hpp::Error);
Steve Tonneau's avatar
Steve Tonneau committed
217
        virtual hpp::floatSeq* getSamplePosition(const char* limb, unsigned short sampleId) throw (hpp::Error);
218
        virtual hpp::floatSeqSeq* getEffectorPosition(const char* limb, const hpp::floatSeq& configuration) throw (hpp::Error);
Steve Tonneau's avatar
Steve Tonneau committed
219
        virtual CORBA::UShort getNumSamples(const char* limb) throw (hpp::Error);
220
        virtual hpp::floatSeq* getOctreeNodeIds(const char* limb) throw (hpp::Error);
Steve Tonneau's avatar
Steve Tonneau committed
221
        virtual double getSampleValue(const char* limb, const char* valueName, unsigned short sampleId) throw (hpp::Error);
222
        virtual double getEffectorDistance(unsigned short  state1, unsigned short  state2) throw (hpp::Error);
Steve Tonneau's avatar
Steve Tonneau committed
223

224
225
        rbprm::State generateContacts_internal(const hpp::floatSeq& configuration,
          const hpp::floatSeq& direction,const hpp::floatSeq& acceleration, const double robustnessThreshold ) throw (hpp::Error);
Steve Tonneau's avatar
Steve Tonneau committed
226
        virtual hpp::floatSeq* generateContacts(const hpp::floatSeq& configuration,
227
                                                const hpp::floatSeq& direction, const hpp::floatSeq& acceleration, const double robustnessThreshold) throw (hpp::Error);
228
229
        virtual CORBA::Short generateStateInContact(const hpp::floatSeq& configuration,
                                                const hpp::floatSeq& direction, const hpp::floatSeq& acceleration, const double robustnessThreshold) throw (hpp::Error);
Steve Tonneau's avatar
Steve Tonneau committed
230

231
232
        virtual hpp::floatSeq* generateGroundContact(const hpp::Names_t& contactLimbs) throw (hpp::Error);

233
234
235
236
        virtual hpp::floatSeq* getContactSamplesIds(const char* limb,
                                                   const hpp::floatSeq& configuration,
                                                   const hpp::floatSeq& direction) throw (hpp::Error);

Steve Tonneau's avatar
Steve Tonneau committed
237
        virtual hpp::floatSeqSeq* getContactSamplesProjected(const char* limb,
Steve Tonneau's avatar
Steve Tonneau committed
238
239
240
241
                                                   const hpp::floatSeq& configuration,
                                                   const hpp::floatSeq& direction,
                                                   unsigned short numSamples) throw (hpp::Error);

242
243
244
        virtual hpp::floatSeq* getSamplesIdsInOctreeNode(const char* limb,
                                                   double octreeNodeId) throw (hpp::Error);

245
        virtual void addLimb(const char* id, const char* limb, const char* effector, const hpp::floatSeq& offset, const hpp::floatSeq& normal, double x, double y,
246
                             unsigned int samples, const char *heuristicName, double resolution, const char *contactType,
247
                             double disableEffectorCollision, double grasp,const hpp::floatSeq& limbOffset) throw (hpp::Error);
248
249
        virtual void addNonContactingLimb(const char* id, const char* limb, const char* effector, unsigned int samples) throw (hpp::Error);

250
        virtual void addLimbDatabase(const char* databasePath, const char* id, const char* heuristicName, double loadValues,
t steve's avatar
t steve committed
251
                                     double disableEffectorCollision, double grasp) throw (hpp::Error);
Steve Tonneau's avatar
Steve Tonneau committed
252

Steve Tonneau's avatar
Steve Tonneau committed
253
254
        virtual void setStartState(const hpp::floatSeq& configuration, const hpp::Names_t& contactLimbs) throw (hpp::Error);
        virtual void setEndState(const hpp::floatSeq& configuration, const hpp::Names_t& contactLimbs) throw (hpp::Error);
255
256
        virtual void setStartStateId(unsigned short stateId) throw (hpp::Error);
        virtual void setEndStateId(unsigned short stateId) throw (hpp::Error);
257
        virtual hpp::floatSeq*  computeContactForConfig(const hpp::floatSeq& configuration, const char* limbNam) throw (hpp::Error);
258
        virtual hpp::floatSeqSeq* computeContactPoints(unsigned short cId) throw (hpp::Error);
t steve's avatar
t steve committed
259
        virtual hpp::floatSeqSeq* computeContactPointsAtState(unsigned short cId, unsigned short isIntermediate) throw (hpp::Error);
260
        virtual hpp::floatSeqSeq* computeContactPointsForLimb(unsigned short cId, const char* limbName) throw (hpp::Error);
t steve's avatar
t steve committed
261
        virtual hpp::floatSeqSeq* computeContactPointsAtStateForLimb(unsigned short cId, unsigned short isIntermediate, const char* limbName) throw (hpp::Error);
262
        virtual hpp::floatSeqSeq* computeCenterOfContactAtStateForLimb(unsigned short cId, unsigned short isIntermediate, const char *limbName) throw (hpp::Error);
263
264
        virtual hpp::floatSeqSeq* interpolate(double timestep, double path, double robustnessTreshold, unsigned short filterStates) throw (hpp::Error);
        virtual hpp::floatSeqSeq* interpolateConfigs(const hpp::floatSeqSeq& configs, double robustnessTreshold, unsigned short filterStates) throw (hpp::Error);
265
266
        virtual hpp::floatSeqSeq* getContactCone(unsigned short stateId, double friction) throw (hpp::Error);
        virtual hpp::floatSeqSeq* getContactIntermediateCone(unsigned short stateId, double friction) throw (hpp::Error);
267
268
        virtual CORBA::Short generateComTraj(const hpp::floatSeqSeq& positions, const hpp::floatSeqSeq& velocities,
                                             const hpp::floatSeqSeq& accelerations, const double dt) throw (hpp::Error);
t steve's avatar
t steve committed
269
270

        virtual CORBA::Short straightPath(const hpp::floatSeqSeq& positions) throw (hpp::Error);
t steve's avatar
t steve committed
271
        virtual CORBA::Short generateCurveTraj(const hpp::floatSeqSeq& positions) throw (hpp::Error);
t steve's avatar
t steve committed
272
        virtual CORBA::Short generateCurveTrajParts(const hpp::floatSeqSeq& positions, const hpp::floatSeq& partitions) throw (hpp::Error);
273
        virtual CORBA::Short generateRootPath(const hpp::floatSeqSeq& rootPositions,
274
                                      const hpp::floatSeq& q1, const hpp::floatSeq& q2) throw (hpp::Error);
275
276
        virtual CORBA::Short limbRRT(double state1, double state2, unsigned short numOptimizations) throw (hpp::Error);
        virtual CORBA::Short limbRRTFromRootPath(double state1, double state2, unsigned short path, unsigned short numOptimizations) throw (hpp::Error);
277
        virtual CORBA::Short configToPath(const hpp::floatSeqSeq& configs) throw (hpp::Error);
278
        virtual CORBA::Short comRRT(double state1, double state2, unsigned short path, unsigned short numOptimizations) throw (hpp::Error);
279
280

        typedef core::PathPtr_t (*t_rrt)
281
282
            (RbPrmFullBodyPtr_t, core::ProblemSolverPtr_t, const core::PathPtr_t,
             const  State &, const State &, const  std::size_t, const bool);
283

284
        hpp::floatSeq* rrt(t_rrt functor ,double state1,double state2,
285
286
287
                           unsigned short comTraj1, unsigned short comTraj2, unsigned short comTraj3,
                           unsigned short numOptimizations) throw (hpp::Error);

288
        virtual hpp::floatSeq* comRRTFromPos(double state1,
289
290
291
                                           unsigned short comTraj1,
                                           unsigned short comTraj2,
                                           unsigned short comTraj3,
Steve Tonneau's avatar
Steve Tonneau committed
292
                                           unsigned short numOptimizations) throw (hpp::Error);
293
294
295
296
297
        virtual hpp::floatSeq* comRRTFromPosBetweenState(double state1,double state2,
                                           unsigned short comTraj1,
                                           unsigned short comTraj2,
                                           unsigned short comTraj3,
                                           unsigned short numOptimizations) throw (hpp::Error);
Steve Tonneau's avatar
Steve Tonneau committed
298
299
300
301
302
        virtual hpp::floatSeq* effectorRRTFromPosBetweenState(double state1,double state2,
                                           unsigned short comTraj1,
                                           unsigned short comTraj2,
                                           unsigned short comTraj3,
                                           unsigned short numOptimizations) throw (hpp::Error);
303
        virtual hpp::floatSeq* effectorRRT(double state1,
304
305
306
                                           unsigned short comTraj1,
                                           unsigned short comTraj2,
                                           unsigned short comTraj3,
307
                                           unsigned short numOptimizations) throw (hpp::Error);
308
309
310
311
        virtual hpp::floatSeq* effectorRRTFromPath(double state1,
                                           unsigned short path,
                                           double path_from,
                                           double path_to,
312
313
314
                                           unsigned short comTraj1,
                                           unsigned short comTraj2,
                                           unsigned short comTraj3,
315
316
                                           unsigned short numOptimizations,
                                           const hpp::Names_t& trackedEffectors) throw (hpp::Error);
317
318
319
320

        virtual CORBA::Short generateEndEffectorBezier(double state1, double state2,
        unsigned short cT) throw (hpp::Error);

321
        virtual hpp::floatSeq* projectToCom(double state, const hpp::floatSeq& targetCom, unsigned short max_num_sample) throw (hpp::Error);
Steve Tonneau's avatar
Steve Tonneau committed
322
        virtual CORBA::Short createState(const hpp::floatSeq& configuration, const hpp::Names_t& contactLimbs) throw (hpp::Error);
323
        virtual hpp::floatSeq* getConfigAtState(unsigned short stateId) throw (hpp::Error);
Steve Tonneau's avatar
Steve Tonneau committed
324
        virtual double setConfigAtState(unsigned short stateId, const hpp::floatSeq& config) throw (hpp::Error);
325
326
        double projectStateToCOMEigen(unsigned short stateId, const model::Configuration_t& com_target, unsigned short maxNumeSamples)throw (hpp::Error);
        virtual double projectStateToCOM(unsigned short stateId, const hpp::floatSeq& com, unsigned short max_num_sample) throw (hpp::Error);
Steve Tonneau's avatar
Steve Tonneau committed
327
        virtual void saveComputedStates(const char* filepath) throw (hpp::Error);
Steve Tonneau's avatar
Steve Tonneau committed
328
        virtual void saveLimbDatabase(const char* limbname,const char* filepath) throw (hpp::Error);
329
        virtual hpp::floatSeq* getOctreeBox(const char* limbName, double sampleId) throw (hpp::Error);
330
331
        virtual CORBA::Short  isLimbInContact(const char* limbName, double state) throw (hpp::Error);
        virtual CORBA::Short  isLimbInContactIntermediary(const char* limbName, double state) throw (hpp::Error);
332
        virtual CORBA::Short  computeIntermediary(unsigned short state1, unsigned short state2) throw (hpp::Error);
333
        virtual hpp::floatSeqSeq* getOctreeBoxes(const char* limbName, const hpp::floatSeq& configuration) throw (hpp::Error);
334
        virtual hpp::floatSeq* getOctreeTransform(const char* limbName, const hpp::floatSeq& configuration) throw (hpp::Error);
335
        virtual CORBA::Short isConfigBalanced(const hpp::floatSeq& config, const hpp::Names_t& contactLimbs, double robustnessTreshold) throw (hpp::Error);
336
        virtual double isStateBalanced(unsigned short stateId) throw (hpp::Error);
337
        virtual void runSampleAnalysis(const char* analysis, double isstatic) throw (hpp::Error);
338
        virtual hpp::floatSeq* runLimbSampleAnalysis(const char* limbname, const char* analysis, double isstatic) throw (hpp::Error);
339
        virtual hpp::floatSeq* evaluateConfig(const hpp::floatSeq& configuration, const hpp::floatSeq &direction) throw (hpp::Error);
340
        virtual void dumpProfile(const char* logFile) throw (hpp::Error);
341
        virtual double getTimeAtState(unsigned short stateId)throw (hpp::Error);
342
        virtual Names_t* getContactsVariations(unsigned short stateIdFrom,unsigned short stateIdTo )throw (hpp::Error);
343
        virtual Names_t* getAllLimbsNames()throw (hpp::Error);
344
        virtual CORBA::Short addNewContact(unsigned short stateId, const char* limbName,
t steve's avatar
t steve committed
345
                                            const hpp::floatSeq& position, const hpp::floatSeq& normal, unsigned short max_num_sample) throw (hpp::Error);
t steve's avatar
t steve committed
346
        virtual CORBA::Short removeContact(unsigned short stateId, const char* limbName) throw (hpp::Error);
347
        virtual hpp::floatSeq* computeTargetTransform(const char* limbName, const hpp::floatSeq& configuration, const hpp::floatSeq& p, const hpp::floatSeq& n) throw (hpp::Error);
348
        virtual Names_t* getEffectorsTrajectoriesNames(unsigned short pathId)throw (hpp::Error);
349
        virtual hpp::floatSeqSeqSeq* getEffectorTrajectoryWaypoints(unsigned short pathId,const char* effectorName)throw (hpp::Error);
350

351
352
        virtual bool areKinematicsConstraintsVerified(const hpp::floatSeq &point)throw (hpp::Error);
        virtual bool areKinematicsConstraintsVerifiedForState(unsigned short stateId,const hpp::floatSeq &point)throw (hpp::Error);
353
        virtual bool isReachableFromState(unsigned short stateFrom,unsigned short stateTo)throw (hpp::Error);
354
        virtual CORBA::Short isDynamicallyReachableFromState(unsigned short stateFrom, unsigned short stateTo, const hpp::floatSeq &timings, unsigned short numPointPerPhase )throw (hpp::Error);
355

356

Steve Tonneau's avatar
Steve Tonneau committed
357
358
359
360
361
362
363
364
365
        void selectFullBody (const char* name) throw (hpp::Error)
        {
          std::string psName (name);
          bool has = fullBodyMap_.has (psName);
          if (!has)
              throw hpp::Error("unknown fullBody Problem");
          fullBodyMap_.selected_ = psName;
        }

Steve Tonneau's avatar
Steve Tonneau committed
366
        public:
367
368
        void SetProblemSolverMap (hpp::corbaServer::ProblemSolverMapPtr_t psMap);
        void initNewProblemSolver ();
Steve Tonneau's avatar
Steve Tonneau committed
369
370

        private:
Steve Tonneau's avatar
Steve Tonneau committed
371
        /// \brief Pointer to hppPlanner object of hpp::corbaServer::Server.
372
373
374
375
376
        corbaServer::ProblemSolverMapPtr_t psMap_;
        core::ProblemSolverPtr_t problemSolver()
        {
            return psMap_->selected();
        }
Steve Tonneau's avatar
Steve Tonneau committed
377
378
379
380
381
382
383
        FullBodyMap fullBodyMap_;
        rbprm::RbPrmFullBodyPtr_t fullBody()
        {
            if(!fullBodyLoaded_)
                throw Error ("No full body robot was loaded");
            return fullBodyMap_.selected();
        }
Steve Tonneau's avatar
Steve Tonneau committed
384
385

        private:
Steve Tonneau's avatar
Steve Tonneau committed
386
        model::T_Rom romDevices_;
Steve Tonneau's avatar
Steve Tonneau committed
387
        //rbprm::RbPrmFullBodyPtr_t fullBody_;
Steve Tonneau's avatar
Steve Tonneau committed
388
        bool romLoaded_;
Steve Tonneau's avatar
Steve Tonneau committed
389
        bool fullBodyLoaded_;
Steve Tonneau's avatar
Steve Tonneau committed
390
        BindShooter bindShooter_;
Steve Tonneau's avatar
Steve Tonneau committed
391
392
        rbprm::State startState_;
        rbprm::State endState_;
Steve Tonneau's avatar
Steve Tonneau committed
393
        std::vector<rbprm::State> lastStatesComputed_;
394
        rbprm::T_StateFrame lastStatesComputedTime_;
Steve Tonneau's avatar
Steve Tonneau committed
395
        sampling::AnalysisFactory* analysisFactory_;
Pierre Fernbach's avatar
Pierre Fernbach committed
396
        model::Configuration_t refPose_;
Steve Tonneau's avatar
Steve Tonneau committed
397
398
399
400
401
402
      }; // class RobotBuilder
    } // namespace impl
  } // namespace manipulation
} // namespace hpp

#endif // HPP_RBPRM_CORBA_BUILDER_IMPL_HH