rbprmbuilder.impl.hh 24.9 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>
23
# include "hpp/corbaserver/rbprm/rbprmbuilder-idl.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
# include <hpp/core/collision-path-validation-report.hh>
# include <hpp/core/problem-solver.hh>
# include <hpp/core/straight-path.hh>
stevet's avatar
stevet committed
32
# include <hpp/core/problem.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;
stevet's avatar
stevet committed
45
46

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

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

stevet's avatar
stevet committed
55
        hpp::rbprm::RbPrmShooterPtr_t create (/*const hpp::pinocchio::DevicePtr_t& robot,*/ const hpp::core::Problem& problem)
Steve Tonneau's avatar
Steve Tonneau committed
56
        {
stevet's avatar
stevet committed
57
            affMap_ = problemSolver_->affordanceObjects;
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
            if(!so3Bounds_.empty())
                shooter->BoundSO3(so3Bounds_);
67
            shooter->sampleExtraDOF(problem.getParameter("ConfigurationShooter/sampleExtraDOF").boolValue());
68
            shooter->ratioWeighted(problem.getParameter("RbprmShooter/ratioWeighted").floatValue ());
Steve Tonneau's avatar
Steve Tonneau committed
69
            return shooter;
Steve Tonneau's avatar
Steve Tonneau committed
70
        }
71

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

stevet's avatar
stevet committed
88
        hpp::core::PathValidationPtr_t createDynamicPathValidation (const hpp::pinocchio::DevicePtr_t& robot, const hpp::pinocchio::value_type& val)
89
        {
stevet's avatar
stevet committed
90
91
92
          hpp::pinocchio::RbPrmDevicePtr_t robotcast = boost::static_pointer_cast<hpp::pinocchio::RbPrmDevice>(robot);
          affMap_ = problemSolver_->affordanceObjects;
          if (affMap_.map.empty ()) {
93
94
95
96
97
98
99
100
101
102
103
            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;
104
105
106
107
108
          sizeFootX = problemSolver_->problem()->getParameter (std::string("DynamicPlanner/sizeFootX")).floatValue()/2.;
          sizeFootY = problemSolver_->problem()->getParameter (std::string("DynamicPlanner/sizeFootY")).floatValue()/2.;
          if(sizeFootX > 0. && sizeFootY > 0.)
            rectangularContact = 1;
          else
109
110
            rectangularContact = 0;
          mass = robot->mass();
111
112
          mu = problemSolver_->problem()->getParameter (std::string("DynamicPlanner/friction")).floatValue();
          hppDout(notice,"mu define in python : "<<mu);
113
          DynamicValidationPtr_t dynamicVal = DynamicValidation::create(rectangularContact,sizeFootX,sizeFootY,mass,mu,robot);
114
115
116
117
118
          collisionChecking->addDynamicValidator(dynamicVal);

          return collisionChecking;
        }

Steve Tonneau's avatar
Steve Tonneau committed
119
        hpp::core::ProblemSolverPtr_t problemSolver_;
Steve Tonneau's avatar
Steve Tonneau committed
120
        std::vector<std::string> romFilter_;
121
        std::map<std::string, std::vector<std::string> > affFilter_;
Steve Tonneau's avatar
Steve Tonneau committed
122
123
        std::size_t shootLimit_;
        std::size_t displacementLimit_;
Steve Tonneau's avatar
Steve Tonneau committed
124
        std::vector<double> so3Bounds_;
stevet's avatar
stevet committed
125
        affMap_t affMap_;
Steve Tonneau's avatar
Steve Tonneau committed
126
127
    };

Steve Tonneau's avatar
Steve Tonneau committed
128
129
130
131
132
133
134
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
    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
166
167
168
      class RbprmBuilder : public virtual POA_hpp::corbaserver::rbprm::RbprmBuilder
      {
        public:
Steve Tonneau's avatar
Steve Tonneau committed
169
        RbprmBuilder ();
Steve Tonneau's avatar
Steve Tonneau committed
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184

        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
185
186
187
188
189
190

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

194
195
        virtual void loadFullBodyRobotFromExistingRobot () throw (hpp::Error);

196
        void setStaticStability(const bool staticStability) throw (hpp::Error);
197

198
        void setReferenceConfig(const hpp::floatSeq &referenceConfig) throw (hpp::Error);
199
        void setPostureWeights(const hpp::floatSeq &postureWeights) throw (hpp::Error);
200
        void setReferenceEndEffector(const char* romName, const hpp::floatSeq &ref) throw(hpp::Error);
201
        void usePosturalTaskContactCreation(const bool usePosturalTaskContactCreation) throw (hpp::Error);
202

Steve Tonneau's avatar
Steve Tonneau committed
203
        virtual void setFilter(const hpp::Names_t& roms) throw (hpp::Error);
204
				virtual void setAffordanceFilter(const char* romName, const hpp::Names_t& affordances) throw (hpp::Error);
Steve Tonneau's avatar
Steve Tonneau committed
205
206
        virtual void boundSO3(const hpp::floatSeq& limitszyx) throw (hpp::Error);

Steve Tonneau's avatar
Steve Tonneau committed
207

208
209
        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);
210
        virtual hpp::floatSeqSeq* getEffectorPosition(const char* limb, const hpp::floatSeq& configuration) throw (hpp::Error);
Steve Tonneau's avatar
Steve Tonneau committed
211
        virtual CORBA::UShort getNumSamples(const char* limb) throw (hpp::Error);
212
        virtual hpp::floatSeq* getOctreeNodeIds(const char* limb) throw (hpp::Error);
213
        virtual double getSampleValue(const char* limb, const char* valueName, unsigned int sampleId) throw (hpp::Error);
214
        virtual double getEffectorDistance(unsigned short  state1, unsigned short  state2) throw (hpp::Error);
Steve Tonneau's avatar
Steve Tonneau committed
215

216
217
        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
218
        virtual hpp::floatSeq* generateContacts(const hpp::floatSeq& configuration,
219
                                                const hpp::floatSeq& direction, const hpp::floatSeq& acceleration, const double robustnessThreshold) throw (hpp::Error);
220
221
        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
222

223
224
        virtual hpp::floatSeq* generateGroundContact(const hpp::Names_t& contactLimbs) throw (hpp::Error);

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

234
235
236
        virtual hpp::floatSeq* getSamplesIdsInOctreeNode(const char* limb,
                                                   double octreeNodeId) throw (hpp::Error);

237
        virtual void addLimb(const char* id, const char* limb, const char* effector, const hpp::floatSeq& offset, const hpp::floatSeq& normal, double x, double y,
238
                             unsigned int samples, const char *heuristicName, double resolution, const char *contactType,
239
                             double disableEffectorCollision, double grasp,const hpp::floatSeq& limbOffset,const char* kinematicConstraintsPath, double kinematicConstraintsMin) throw (hpp::Error);
240
241
        virtual void addNonContactingLimb(const char* id, const char* limb, const char* effector, unsigned int samples) throw (hpp::Error);

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

Steve Tonneau's avatar
Steve Tonneau committed
245
246
        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);
247
248
        virtual void setStartStateId(unsigned short stateId) throw (hpp::Error);
        virtual void setEndStateId(unsigned short stateId) throw (hpp::Error);
249
        virtual hpp::floatSeq*  computeContactForConfig(const hpp::floatSeq& configuration, const char* limbNam) throw (hpp::Error);
250
        virtual hpp::floatSeqSeq* computeContactPoints(unsigned short cId) throw (hpp::Error);
t steve's avatar
t steve committed
251
        virtual hpp::floatSeqSeq* computeContactPointsAtState(unsigned short cId, unsigned short isIntermediate) throw (hpp::Error);
252
        virtual hpp::floatSeqSeq* computeContactPointsForLimb(unsigned short cId, const char* limbName) throw (hpp::Error);
t steve's avatar
t steve committed
253
        virtual hpp::floatSeqSeq* computeContactPointsAtStateForLimb(unsigned short cId, unsigned short isIntermediate, const char* limbName) throw (hpp::Error);
254
        virtual hpp::floatSeqSeq* computeCenterOfContactAtStateForLimb(unsigned short cId, unsigned short isIntermediate, const char *limbName) throw (hpp::Error);
255
256
        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);
257
258
        virtual hpp::floatSeqSeq* getContactCone(unsigned short stateId, double friction) throw (hpp::Error);
        virtual hpp::floatSeqSeq* getContactIntermediateCone(unsigned short stateId, double friction) throw (hpp::Error);
259
260
        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
261
262

        virtual CORBA::Short straightPath(const hpp::floatSeqSeq& positions) throw (hpp::Error);
t steve's avatar
t steve committed
263
        virtual CORBA::Short generateCurveTraj(const hpp::floatSeqSeq& positions) throw (hpp::Error);
t steve's avatar
t steve committed
264
        virtual CORBA::Short generateCurveTrajParts(const hpp::floatSeqSeq& positions, const hpp::floatSeq& partitions) throw (hpp::Error);
265
        virtual CORBA::Short generateRootPath(const hpp::floatSeqSeq& rootPositions,
266
                                      const hpp::floatSeq& q1, const hpp::floatSeq& q2) throw (hpp::Error);
267
268
        virtual CORBA::Short limbRRT(unsigned short state1, unsigned short state2, unsigned short numOptimizations) throw (hpp::Error);
        virtual CORBA::Short limbRRTFromRootPath(unsigned short state1, unsigned short state2, unsigned short path, unsigned short numOptimizations) throw (hpp::Error);
269
        virtual CORBA::Short configToPath(const hpp::floatSeqSeq& configs) throw (hpp::Error);
270
        virtual CORBA::Short comRRT(unsigned short state1, unsigned short state2, unsigned short path, unsigned short numOptimizations) throw (hpp::Error);
271
272

        typedef core::PathPtr_t (*t_rrt)
273
274
            (RbPrmFullBodyPtr_t, core::ProblemSolverPtr_t, const core::PathPtr_t,
             const  State &, const State &, const  std::size_t, const bool);
275

276
        hpp::floatSeq* rrt(t_rrt functor ,unsigned short state1,unsigned short state2,
277
278
279
                           unsigned short comTraj1, unsigned short comTraj2, unsigned short comTraj3,
                           unsigned short numOptimizations) throw (hpp::Error);

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

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

325
        virtual hpp::floatSeq* projectToCom(unsigned short state, const hpp::floatSeq& targetCom, unsigned short max_num_sample) throw (hpp::Error);
Steve Tonneau's avatar
Steve Tonneau committed
326
        virtual CORBA::Short createState(const hpp::floatSeq& configuration, const hpp::Names_t& contactLimbs) throw (hpp::Error);
327
        virtual hpp::floatSeq* getConfigAtState(unsigned short stateId) throw (hpp::Error);
Steve Tonneau's avatar
Steve Tonneau committed
328
        virtual double setConfigAtState(unsigned short stateId, const hpp::floatSeq& config) throw (hpp::Error);
stevet's avatar
stevet committed
329
330
        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);
331

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

359

360
361
        virtual bool areKinematicsConstraintsVerified(const hpp::floatSeq &point)throw (hpp::Error);
        virtual bool areKinematicsConstraintsVerifiedForState(unsigned short stateId,const hpp::floatSeq &point)throw (hpp::Error);
362
        virtual hpp::floatSeq *isReachableFromState(unsigned short stateFrom,unsigned short stateTo,const bool useIntermediateState)throw (hpp::Error);
363
        virtual hpp::floatSeq* isDynamicallyReachableFromState(unsigned short stateFrom, unsigned short stateTo, bool addPathPerPhase, const hpp::floatSeq &timings, short numPointPerPhase )throw (hpp::Error);
364

365

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

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

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

#endif // HPP_RBPRM_CORBA_BUILDER_IMPL_HH