rbprmbuilder.impl.hh 24.2 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>
stevet's avatar
stevet committed
33
# include <hpp/core/problem.hh>
34
35
#include <hpp/corbaserver/affordance/server.hh>
# include <hpp/corbaserver/problem-solver-map.hh>
36
# include <hpp/rbprm/rbprm-path-validation.hh>
37
38
# include <hpp/fcl/BVH/BVH_model.h>
# include <hpp/core/config-validations.hh>
39
#include <hpp/rbprm/dynamic/dynamic-path-validation.hh>
40
41
# include "hpp/corbaserver/fwd.hh"

Steve Tonneau's avatar
Steve Tonneau committed
42
43
44
45
namespace hpp {
  namespace rbprm {
    namespace impl {
      using CORBA::Short;
stevet's avatar
stevet committed
46
47

    typedef hpp::core::Container <hpp::core::AffordanceObjects_t> affMap_t;
Steve Tonneau's avatar
Steve Tonneau committed
48

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

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

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

stevet's avatar
stevet committed
86
        hpp::core::PathValidationPtr_t createDynamicPathValidation (const hpp::pinocchio::DevicePtr_t& robot, const hpp::pinocchio::value_type& val)
87
        {
stevet's avatar
stevet committed
88
89
90
          hpp::pinocchio::RbPrmDevicePtr_t robotcast = boost::static_pointer_cast<hpp::pinocchio::RbPrmDevice>(robot);
          affMap_ = problemSolver_->affordanceObjects;
          if (affMap_.map.empty ()) {
91
92
93
94
95
96
97
98
99
100
101
102
            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 {
stevet's avatar
stevet committed
103
104
105
              sizeFootX = problemSolver_->problem()->getParameter ("sizeFootX").floatValue()/2.;
              sizeFootY = problemSolver_->problem()->getParameter ("sizeFootY").floatValue()/2.;
              rectangularContact = 1;
106
107
          } catch (const std::exception& e) {
            hppDout(warning,"Warning : size of foot not definied, use 0 (contact point)");
stevet's avatar
stevet committed
108
109
            sizeFootX = 0;
            sizeFootY = 0;
110
111
112
            rectangularContact = 0;
          }
          mass = robot->mass();
113
          try {
stevet's avatar
stevet committed
114
115
            mu = problemSolver_->problem()->getParameter ("friction").floatValue();
            hppDout(notice,"mu define in python : "<<mu);
116
117
          } catch (const std::exception& e) {
            mu= 0.5;
stevet's avatar
stevet committed
118
            hppDout(notice,"mu not defined, take : "<<mu<<" as default.");
119
          }
120
121
122
123
124
125
          DynamicValidationPtr_t dynamicVal = DynamicValidation::create(rectangularContact,sizeFootX,sizeFootY,mass,mu);
          collisionChecking->addDynamicValidator(dynamicVal);

          return collisionChecking;
        }

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

Steve Tonneau's avatar
Steve Tonneau committed
135
136
137
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
    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
173
174
175
      class RbprmBuilder : public virtual POA_hpp::corbaserver::rbprm::RbprmBuilder
      {
        public:
Steve Tonneau's avatar
Steve Tonneau committed
176
        RbprmBuilder ();
Steve Tonneau's avatar
Steve Tonneau committed
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191

        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
192
193
194
195
196
197

        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
198
199
                 const char* srdfSuffix,
                 const char* selectedProblem) throw (hpp::Error);
Steve Tonneau's avatar
Steve Tonneau committed
200

201
202
        virtual void loadFullBodyRobotFromExistingRobot () throw (hpp::Error);

203
        void setStaticStability(const bool staticStability) throw (hpp::Error);
204

205
206
207
        void setReferenceConfig(const hpp::floatSeq &referenceConfig) throw (hpp::Error);


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

Steve Tonneau's avatar
Steve Tonneau committed
212

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

221
222
        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
223
        virtual hpp::floatSeq* generateContacts(const hpp::floatSeq& configuration,
224
                                                const hpp::floatSeq& direction, const hpp::floatSeq& acceleration, const double robustnessThreshold) throw (hpp::Error);
225
226
        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
227

228
229
        virtual hpp::floatSeq* generateGroundContact(const hpp::Names_t& contactLimbs) throw (hpp::Error);

230
231
232
233
        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
234
        virtual hpp::floatSeqSeq* getContactSamplesProjected(const char* limb,
Steve Tonneau's avatar
Steve Tonneau committed
235
236
237
238
                                                   const hpp::floatSeq& configuration,
                                                   const hpp::floatSeq& direction,
                                                   unsigned short numSamples) throw (hpp::Error);

239
240
241
        virtual hpp::floatSeq* getSamplesIdsInOctreeNode(const char* limb,
                                                   double octreeNodeId) throw (hpp::Error);

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

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

Steve Tonneau's avatar
Steve Tonneau committed
250
251
        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);
252
253
        virtual void setStartStateId(unsigned short stateId) throw (hpp::Error);
        virtual void setEndStateId(unsigned short stateId) throw (hpp::Error);
254
        virtual hpp::floatSeq*  computeContactForConfig(const hpp::floatSeq& configuration, const char* limbNam) throw (hpp::Error);
255
        virtual hpp::floatSeqSeq* computeContactPoints(unsigned short cId) throw (hpp::Error);
t steve's avatar
t steve committed
256
        virtual hpp::floatSeqSeq* computeContactPointsAtState(unsigned short cId, unsigned short isIntermediate) throw (hpp::Error);
257
        virtual hpp::floatSeqSeq* computeContactPointsForLimb(unsigned short cId, const char* limbName) throw (hpp::Error);
t steve's avatar
t steve committed
258
        virtual hpp::floatSeqSeq* computeContactPointsAtStateForLimb(unsigned short cId, unsigned short isIntermediate, const char* limbName) throw (hpp::Error);
259
        virtual hpp::floatSeqSeq* computeCenterOfContactAtStateForLimb(unsigned short cId, unsigned short isIntermediate, const char *limbName) throw (hpp::Error);
260
261
        virtual hpp::floatSeqSeq* interpolate(double timestep, double path, double robustnessTreshold, unsigned short filterStates, bool testReachability, bool quasiStatic) throw (hpp::Error);
        virtual hpp::floatSeqSeq* interpolateConfigs(const hpp::floatSeqSeq& configs, double robustnessTreshold, unsigned short filterStates, bool testReachability, bool quasiStatic) throw (hpp::Error);
262
263
        virtual hpp::floatSeqSeq* getContactCone(unsigned short stateId, double friction) throw (hpp::Error);
        virtual hpp::floatSeqSeq* getContactIntermediateCone(unsigned short stateId, double friction) throw (hpp::Error);
264
265
        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
266
267

        virtual CORBA::Short straightPath(const hpp::floatSeqSeq& positions) throw (hpp::Error);
t steve's avatar
t steve committed
268
        virtual CORBA::Short generateCurveTraj(const hpp::floatSeqSeq& positions) throw (hpp::Error);
t steve's avatar
t steve committed
269
        virtual CORBA::Short generateCurveTrajParts(const hpp::floatSeqSeq& positions, const hpp::floatSeq& partitions) throw (hpp::Error);
270
        virtual CORBA::Short generateRootPath(const hpp::floatSeqSeq& rootPositions,
271
                                      const hpp::floatSeq& q1, const hpp::floatSeq& q2) throw (hpp::Error);
272
273
        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);
274
        virtual CORBA::Short configToPath(const hpp::floatSeqSeq& configs) throw (hpp::Error);
275
        virtual CORBA::Short comRRT(double state1, double state2, unsigned short path, unsigned short numOptimizations) throw (hpp::Error);
276
277

        typedef core::PathPtr_t (*t_rrt)
278
279
            (RbPrmFullBodyPtr_t, core::ProblemSolverPtr_t, const core::PathPtr_t,
             const  State &, const State &, const  std::size_t, const bool);
280

281
        hpp::floatSeq* rrt(t_rrt functor ,double state1,double state2,
282
283
284
                           unsigned short comTraj1, unsigned short comTraj2, unsigned short comTraj3,
                           unsigned short numOptimizations) throw (hpp::Error);

285
        virtual hpp::floatSeq* comRRTFromPos(double state1,
286
287
288
                                           unsigned short comTraj1,
                                           unsigned short comTraj2,
                                           unsigned short comTraj3,
Steve Tonneau's avatar
Steve Tonneau committed
289
                                           unsigned short numOptimizations) throw (hpp::Error);
290
291
292
293
294
        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
295
296
297
298
299
        virtual hpp::floatSeq* effectorRRTFromPosBetweenState(double state1,double state2,
                                           unsigned short comTraj1,
                                           unsigned short comTraj2,
                                           unsigned short comTraj3,
                                           unsigned short numOptimizations) throw (hpp::Error);
300
        virtual hpp::floatSeq* effectorRRT(double state1,
301
302
303
                                           unsigned short comTraj1,
                                           unsigned short comTraj2,
                                           unsigned short comTraj3,
304
                                           unsigned short numOptimizations) throw (hpp::Error);
305
306
307
308
        virtual hpp::floatSeq* effectorRRTFromPath(double state1,
                                           unsigned short path,
                                           double path_from,
                                           double path_to,
309
310
311
                                           unsigned short comTraj1,
                                           unsigned short comTraj2,
                                           unsigned short comTraj3,
312
313
                                           unsigned short numOptimizations,
                                           const hpp::Names_t& trackedEffectors) throw (hpp::Error);
314
315
316
317
318
319
320
321
322
        virtual hpp::floatSeq* rrtOnePhase(t_rrt functor,double state1,double state2,
                                           unsigned short comTraj,
                                           unsigned short numOptimizations) throw (hpp::Error);
        virtual hpp::floatSeq* effectorRRTOnePhase(double state1,double state2,
                                           unsigned short comTraj,
                                           unsigned short numOptimizations) throw (hpp::Error);
        virtual hpp::floatSeq* comRRTOnePhase(double state1,double state2,
                                           unsigned short comTraj,
                                           unsigned short numOptimizations) throw (hpp::Error);
323
324
325
        virtual hpp::floatSeqSeq* generateEffectorBezierArray(double state1,double state2,
                                           unsigned short comTraj,
                                           unsigned short numOptimizations) throw (hpp::Error);
326
327
328
329

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

330
        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
331
        virtual CORBA::Short createState(const hpp::floatSeq& configuration, const hpp::Names_t& contactLimbs) throw (hpp::Error);
332
        virtual hpp::floatSeq* getConfigAtState(unsigned short stateId) throw (hpp::Error);
Steve Tonneau's avatar
Steve Tonneau committed
333
        virtual double setConfigAtState(unsigned short stateId, const hpp::floatSeq& config) throw (hpp::Error);
stevet's avatar
stevet committed
334
335
        double projectStateToCOMEigen(State& s, const pinocchio::Configuration_t& com_target, unsigned short maxNumeSamples)throw (hpp::Error);
        double projectStateToCOMEigen(unsigned short stateId, const pinocchio::Configuration_t& com_target, unsigned short maxNumeSamples)throw (hpp::Error);
336

337
        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
338
        virtual void saveComputedStates(const char* filepath) throw (hpp::Error);
Steve Tonneau's avatar
Steve Tonneau committed
339
        virtual void saveLimbDatabase(const char* limbname,const char* filepath) throw (hpp::Error);
340
        virtual hpp::floatSeq* getOctreeBox(const char* limbName, double sampleId) throw (hpp::Error);
341
342
        virtual CORBA::Short  isLimbInContact(const char* limbName, double state) throw (hpp::Error);
        virtual CORBA::Short  isLimbInContactIntermediary(const char* limbName, double state) throw (hpp::Error);
343
        virtual CORBA::Short  computeIntermediary(unsigned short state1, unsigned short state2) throw (hpp::Error);
344
        virtual hpp::floatSeqSeq* getOctreeBoxes(const char* limbName, const hpp::floatSeq& configuration) throw (hpp::Error);
345
        virtual hpp::floatSeq* getOctreeTransform(const char* limbName, const hpp::floatSeq& configuration) throw (hpp::Error);
346
        virtual CORBA::Short isConfigBalanced(const hpp::floatSeq& config, const hpp::Names_t& contactLimbs, double robustnessTreshold,const hpp::floatSeq& CoM) throw (hpp::Error);
347
        virtual double isStateBalanced(unsigned short stateId) throw (hpp::Error);
348
        virtual void runSampleAnalysis(const char* analysis, double isstatic) throw (hpp::Error);
349
        virtual hpp::floatSeq* runLimbSampleAnalysis(const char* limbname, const char* analysis, double isstatic) throw (hpp::Error);
350
        virtual hpp::floatSeq* evaluateConfig(const hpp::floatSeq& configuration, const hpp::floatSeq &direction) throw (hpp::Error);
351
        virtual void dumpProfile(const char* logFile) throw (hpp::Error);
352
        virtual double getTimeAtState(unsigned short stateId)throw (hpp::Error);
353
        virtual Names_t* getContactsVariations(unsigned short stateIdFrom,unsigned short stateIdTo )throw (hpp::Error);
354
        virtual Names_t* getAllLimbsNames()throw (hpp::Error);
355
        virtual CORBA::Short addNewContact(unsigned short stateId, const char* limbName,
t steve's avatar
t steve committed
356
                                            const hpp::floatSeq& position, const hpp::floatSeq& normal, unsigned short max_num_sample) throw (hpp::Error);
t steve's avatar
t steve committed
357
        virtual CORBA::Short removeContact(unsigned short stateId, const char* limbName) throw (hpp::Error);
358
        virtual hpp::floatSeq* computeTargetTransform(const char* limbName, const hpp::floatSeq& configuration, const hpp::floatSeq& p, const hpp::floatSeq& n) throw (hpp::Error);
359
        virtual Names_t* getEffectorsTrajectoriesNames(unsigned short pathId)throw (hpp::Error);
360
        virtual hpp::floatSeqSeqSeq* getEffectorTrajectoryWaypoints(unsigned short pathId,const char* effectorName)throw (hpp::Error);
361
362
        virtual hpp::floatSeqSeq* getPathAsBezier(unsigned short pathId)throw (hpp::Error);

363

364
365
        virtual bool areKinematicsConstraintsVerified(const hpp::floatSeq &point)throw (hpp::Error);
        virtual bool areKinematicsConstraintsVerifiedForState(unsigned short stateId,const hpp::floatSeq &point)throw (hpp::Error);
366
        virtual hpp::floatSeq *isReachableFromState(unsigned short stateFrom,unsigned short stateTo)throw (hpp::Error);
367
        virtual hpp::floatSeq* isDynamicallyReachableFromState(unsigned short stateFrom, unsigned short stateTo, bool addPathPerPhase, const hpp::floatSeq &timings, short numPointPerPhase )throw (hpp::Error);
368

369

Steve Tonneau's avatar
Steve Tonneau committed
370
371
372
373
374
375
376
377
378
        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
379
        public:
380
381
        void SetProblemSolverMap (hpp::corbaServer::ProblemSolverMapPtr_t psMap);
        void initNewProblemSolver ();
Steve Tonneau's avatar
Steve Tonneau committed
382
383

        private:
Steve Tonneau's avatar
Steve Tonneau committed
384
        /// \brief Pointer to hppPlanner object of hpp::corbaServer::Server.
385
386
387
388
389
        corbaServer::ProblemSolverMapPtr_t psMap_;
        core::ProblemSolverPtr_t problemSolver()
        {
            return psMap_->selected();
        }
Steve Tonneau's avatar
Steve Tonneau committed
390
391
392
393
394
395
396
        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
397
398

        private:
stevet's avatar
stevet committed
399
        pinocchio::T_Rom romDevices_;
Steve Tonneau's avatar
Steve Tonneau committed
400
        //rbprm::RbPrmFullBodyPtr_t fullBody_;
Steve Tonneau's avatar
Steve Tonneau committed
401
        bool romLoaded_;
Steve Tonneau's avatar
Steve Tonneau committed
402
        bool fullBodyLoaded_;
Steve Tonneau's avatar
Steve Tonneau committed
403
        BindShooter bindShooter_;
Steve Tonneau's avatar
Steve Tonneau committed
404
405
        rbprm::State startState_;
        rbprm::State endState_;
Steve Tonneau's avatar
Steve Tonneau committed
406
        std::vector<rbprm::State> lastStatesComputed_;
407
        rbprm::T_StateFrame lastStatesComputedTime_;
Steve Tonneau's avatar
Steve Tonneau committed
408
        sampling::AnalysisFactory* analysisFactory_;
stevet's avatar
stevet committed
409
        pinocchio::Configuration_t refPose_;
Steve Tonneau's avatar
Steve Tonneau committed
410
411
412
413
414
415
      }; // class RobotBuilder
    } // namespace impl
  } // namespace manipulation
} // namespace hpp

#endif // HPP_RBPRM_CORBA_BUILDER_IMPL_HH