rbprmbuilder.impl.hh 25.7 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
# include "hpp/corbaserver/fwd.hh"
40
# include "hpp/corbaserver/rbprm/server.hh"
41

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
            affMap_ = problemSolver_->affordanceObjects;
stevet's avatar
stevet committed
59
60
            hpp::pinocchio::RbPrmDevicePtr_t robotcast = boost::static_pointer_cast<hpp::pinocchio::RbPrmDevice>(problem.robot());
                if (affMap_.map.empty ()) {
61
62
    	        throw hpp::Error ("No affordances found. Unable to create shooter object.");
      		  }
Steve Tonneau's avatar
Steve Tonneau committed
63
            rbprm::RbPrmShooterPtr_t shooter = hpp::rbprm::RbPrmShooter::create
64
                    (robotcast, problemSolver_->problem ()->collisionObstacles(), affMap_,
65
										romFilter_,affFilter_,shootLimit_,displacementLimit_);
Steve Tonneau's avatar
Steve Tonneau committed
66
67
            if(!so3Bounds_.empty())
                shooter->BoundSO3(so3Bounds_);
68
            shooter->sampleExtraDOF(problem.getParameter("ConfigurationShooter/sampleExtraDOF").boolValue());
69
            shooter->ratioWeighted(problem.getParameter("RbprmShooter/ratioWeighted").floatValue ());
Steve Tonneau's avatar
Steve Tonneau committed
70
            return shooter;
Steve Tonneau's avatar
Steve Tonneau committed
71
        }
72

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

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

          return collisionChecking;
        }

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

Steve Tonneau's avatar
Steve Tonneau committed
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
166
    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
167
168
169
      class RbprmBuilder : public virtual POA_hpp::corbaserver::rbprm::RbprmBuilder
      {
        public:
Steve Tonneau's avatar
Steve Tonneau committed
170
        RbprmBuilder ();
Steve Tonneau's avatar
Steve Tonneau committed
171

172
173
174
175
176
        void setServer (Server* server)
        {
          server_ = server;
        }

Steve Tonneau's avatar
Steve Tonneau committed
177
178
179
180
181
182
183
184
185
186
187
188
189
190
        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
191
192
193
194
195
196

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

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

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

204
        void setReferenceConfig(const hpp::floatSeq &referenceConfig) throw (hpp::Error);
205
        void setPostureWeights(const hpp::floatSeq &postureWeights) throw (hpp::Error);
206
        void setReferenceEndEffector(const char* romName, const hpp::floatSeq &ref) throw(hpp::Error);
207
        void usePosturalTaskContactCreation(const bool usePosturalTaskContactCreation) throw (hpp::Error);
208

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

Steve Tonneau's avatar
Steve Tonneau committed
213

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

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

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

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

stevet's avatar
stevet committed
240
241
242
        virtual short generateContactState(::CORBA::UShort  currentState, const char*  name,  const ::hpp::floatSeq& direction)  throw (hpp::Error);


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

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

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

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

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

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

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

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

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

334
        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
335
        virtual CORBA::Short createState(const hpp::floatSeq& configuration, const hpp::Names_t& contactLimbs) throw (hpp::Error);
336
        virtual hpp::floatSeq* getConfigAtState(unsigned short stateId) throw (hpp::Error);
Steve Tonneau's avatar
Steve Tonneau committed
337
        virtual double setConfigAtState(unsigned short stateId, const hpp::floatSeq& config) throw (hpp::Error);
stevet's avatar
stevet committed
338
339
        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);
340

341
        virtual double projectStateToCOM(unsigned short stateId, const hpp::floatSeq& com, unsigned short max_num_sample) throw (hpp::Error);
342
343
        virtual CORBA::Short cloneState(unsigned short stateId) throw (hpp::Error);
        virtual double projectStateToRoot(unsigned short stateId, const hpp::floatSeq& root, const hpp::floatSeq& offset) throw (hpp::Error);
Steve Tonneau's avatar
Steve Tonneau committed
344
        virtual void saveComputedStates(const char* filepath) throw (hpp::Error);
Steve Tonneau's avatar
Steve Tonneau committed
345
        virtual void saveLimbDatabase(const char* limbname,const char* filepath) throw (hpp::Error);
346
        virtual hpp::floatSeq* getOctreeBox(const char* limbName, double sampleId) throw (hpp::Error);
347
348
        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);
349
        virtual CORBA::Short  computeIntermediary(unsigned short state1, unsigned short state2) throw (hpp::Error);
350
        virtual CORBA::Short  getNumStates() throw (hpp::Error);
351
        virtual hpp::floatSeqSeq* getOctreeBoxes(const char* limbName, const hpp::floatSeq& configuration) throw (hpp::Error);
352
        virtual hpp::floatSeq* getOctreeTransform(const char* limbName, const hpp::floatSeq& configuration) throw (hpp::Error);
353
        virtual CORBA::Short isConfigBalanced(const hpp::floatSeq& config, const hpp::Names_t& contactLimbs, double robustnessTreshold,const hpp::floatSeq& CoM) throw (hpp::Error);
354
        virtual double isStateBalanced(unsigned short stateId) throw (hpp::Error);
355
        virtual void runSampleAnalysis(const char* analysis, double isstatic) throw (hpp::Error);
356
        virtual hpp::floatSeq* runLimbSampleAnalysis(const char* limbname, const char* analysis, double isstatic) throw (hpp::Error);
357
        virtual hpp::floatSeq* evaluateConfig(const hpp::floatSeq& configuration, const hpp::floatSeq &direction) throw (hpp::Error);
358
        virtual void dumpProfile(const char* logFile) throw (hpp::Error);
359
        virtual double getTimeAtState(unsigned short stateId)throw (hpp::Error);
360
361
        virtual Names_t* getContactsVariations(unsigned short stateIdFrom,unsigned short stateIdTo )throw (hpp::Error);        
        virtual Names_t* getCollidingObstacleAtConfig(const ::hpp::floatSeq& configuration,const char* limbName)throw (hpp::Error);
362
363
        virtual floatSeqSeq* getContactSurfacesAtConfig(const ::hpp::floatSeq& configuration,const char* limbName)throw (hpp::Error);

364
        virtual Names_t* getAllLimbsNames()throw (hpp::Error);
365
        virtual CORBA::Short addNewContact(unsigned short stateId, const char* limbName,
366
                                            const hpp::floatSeq& position, const hpp::floatSeq& normal, unsigned short max_num_sample, bool lockOtherJoints, const floatSeq &rotation) throw (hpp::Error);
t steve's avatar
t steve committed
367
        virtual CORBA::Short removeContact(unsigned short stateId, const char* limbName) throw (hpp::Error);
368
        virtual hpp::floatSeq* computeTargetTransform(const char* limbName, const hpp::floatSeq& configuration, const hpp::floatSeq& p, const hpp::floatSeq& n) throw (hpp::Error);
369
        virtual Names_t* getEffectorsTrajectoriesNames(unsigned short pathId)throw (hpp::Error);
370
        virtual hpp::floatSeqSeqSeq* getEffectorTrajectoryWaypoints(unsigned short pathId,const char* effectorName)throw (hpp::Error);
371
372
        virtual hpp::floatSeqSeq* getPathAsBezier(unsigned short pathId)throw (hpp::Error);

373
        virtual bool toggleNonContactingLimb(const char* limbName)throw (hpp::Error);
374
375
        virtual bool areKinematicsConstraintsVerified(const hpp::floatSeq &point)throw (hpp::Error);
        virtual bool areKinematicsConstraintsVerifiedForState(unsigned short stateId,const hpp::floatSeq &point)throw (hpp::Error);
376
        virtual hpp::floatSeq *isReachableFromState(unsigned short stateFrom,unsigned short stateTo,const bool useIntermediateState)throw (hpp::Error);
377
        virtual hpp::floatSeq* isDynamicallyReachableFromState(unsigned short stateFrom, unsigned short stateTo, bool addPathPerPhase, const hpp::floatSeq &timings, short numPointPerPhase )throw (hpp::Error);
378

379

Steve Tonneau's avatar
Steve Tonneau committed
380
381
382
383
384
385
386
387
388
        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
389
        public:
390
391
        void SetProblemSolverMap (hpp::corbaServer::ProblemSolverMapPtr_t psMap);
        void initNewProblemSolver ();
Steve Tonneau's avatar
Steve Tonneau committed
392
393

        private:
394
395
        core::ProblemSolverPtr_t problemSolver()
        {
396
            return server_->problemSolver();
397
        }
398
399

        Server* server_;
Steve Tonneau's avatar
Steve Tonneau committed
400
401
402
403
404
405
406
        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
407
408

        private:
stevet's avatar
stevet committed
409
        pinocchio::T_Rom romDevices_;
Steve Tonneau's avatar
Steve Tonneau committed
410
        //rbprm::RbPrmFullBodyPtr_t fullBody_;
Steve Tonneau's avatar
Steve Tonneau committed
411
        bool romLoaded_;
Steve Tonneau's avatar
Steve Tonneau committed
412
        bool fullBodyLoaded_;
Steve Tonneau's avatar
Steve Tonneau committed
413
        BindShooter bindShooter_;
Steve Tonneau's avatar
Steve Tonneau committed
414
415
        rbprm::State startState_;
        rbprm::State endState_;
Steve Tonneau's avatar
Steve Tonneau committed
416
        std::vector<rbprm::State> lastStatesComputed_;
417
        rbprm::T_StateFrame lastStatesComputedTime_;
Steve Tonneau's avatar
Steve Tonneau committed
418
        sampling::AnalysisFactory* analysisFactory_;
stevet's avatar
stevet committed
419
        pinocchio::Configuration_t refPose_;
Steve Tonneau's avatar
Steve Tonneau committed
420
421
422
423
424
425
      }; // class RobotBuilder
    } // namespace impl
  } // namespace manipulation
} // namespace hpp

#endif // HPP_RBPRM_CORBA_BUILDER_IMPL_HH