rbprmbuilder.impl.cc 152 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
// Copyright (c) 2012 CNRS
// Author: Florent Lamiraux, Joseph Mirabel
//
// 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/>.

//#include <hpp/fcl/math/transform.h>
#include <hpp/util/debug.hh>
20
#include <hpp/corbaserver/rbprm/rbprmbuilder.hh>
Steve Tonneau's avatar
Steve Tonneau committed
21
22
#include "rbprmbuilder.impl.hh"
#include "hpp/rbprm/rbprm-device.hh"
Steve Tonneau's avatar
Steve Tonneau committed
23
#include "hpp/rbprm/rbprm-validation.hh"
24
#include "hpp/rbprm/interpolation/rbprm-path-interpolation.hh"
Steve Tonneau's avatar
Steve Tonneau committed
25
26
#include "hpp/rbprm/interpolation/limb-rrt.hh"
#include "hpp/rbprm/interpolation/com-rrt.hh"
27
#include "hpp/rbprm/interpolation/com-trajectory.hh"
28
#include "hpp/rbprm/interpolation/spline/effector-rrt.hh"
Steve Tonneau's avatar
Steve Tonneau committed
29
30
#include "hpp/rbprm/projection/projection.hh"
#include "hpp/rbprm/contact_generation/contact_generation.hh"
t steve's avatar
t steve committed
31
#include "hpp/rbprm/contact_generation/algorithm.hh"
Steve Tonneau's avatar
Steve Tonneau committed
32
#include "hpp/rbprm/stability/stability.hh"
Steve Tonneau's avatar
Steve Tonneau committed
33
#include "hpp/rbprm/sampling/sample-db.hh"
34
#include "hpp/core/straight-path.hh"
35
36
37
#include "hpp/core/config-validations.hh"
#include "hpp/core/collision-validation-report.hh"
#include <hpp/core/subchain-path.hh>
Pierre Fernbach's avatar
Pierre Fernbach committed
38
#include <hpp/core/configuration-shooter/uniform.hh>
39
#include <hpp/core/collision-validation.hh>
stevet's avatar
stevet committed
40
#include <hpp/core/problem-solver.hh>
Steve Tonneau's avatar
Steve Tonneau committed
41
#include <fstream>
42
#include <hpp/rbprm/planner/dynamic-planner.hh>
43
#include <hpp/rbprm/planner/rbprm-steering-kinodynamic.hh>
Steve Tonneau's avatar
Steve Tonneau committed
44
#include <algorithm>    // std::random_shuffle
45
#include <hpp/rbprm/interpolation/time-constraint-helper.hh>
Guilhem Saurel's avatar
Guilhem Saurel committed
46
47
#include <hpp/spline/bezier_curve.h>
#include <hpp/rbprm/interpolation/polynom-trajectory.hh>
48
#include <hpp/rbprm/planner/random-shortcut-dynamic.hh>
49
#include <hpp/rbprm/planner/oriented-path-optimizer.hh>
50
#include <hpp/rbprm/sampling/heuristic-tools.hh>
51
#include <hpp/rbprm/contact_generation/reachability.hh>
stevet's avatar
stevet committed
52
#include <hpp/pinocchio/urdf/util.hh>
53
54
55
56
#ifdef PROFILE
    #include "hpp/rbprm/rbprm-profiler.hh"
#endif

Steve Tonneau's avatar
Steve Tonneau committed
57
58


Steve Tonneau's avatar
Steve Tonneau committed
59
60
namespace hpp {
  namespace rbprm {
t steve's avatar
t steve committed
61
62

  typedef spline::bezier_curve<> bezier;
Steve Tonneau's avatar
Steve Tonneau committed
63
64
    namespace impl {

65
66
    const pinocchio::Computation_t flag = static_cast <pinocchio::Computation_t>
  (pinocchio::JOINT_POSITION | pinocchio::JACOBIAN | pinocchio::COM);
67

Steve Tonneau's avatar
Steve Tonneau committed
68
69
70
    RbprmBuilder::RbprmBuilder ()
    : POA_hpp::corbaserver::rbprm::RbprmBuilder()
    , romLoaded_(false)
Steve Tonneau's avatar
Steve Tonneau committed
71
    , fullBodyLoaded_(false)
Steve Tonneau's avatar
Steve Tonneau committed
72
    , bindShooter_()
Steve Tonneau's avatar
Steve Tonneau committed
73
    , analysisFactory_(0)
Steve Tonneau's avatar
Steve Tonneau committed
74
75
76
77
    {
        // NOTHING
    }

78
79
80
81
82
83
84
85
86
87
88
89

    hpp::floatSeq vectorToFloatseq (const hpp::core::vector_t& input)
    {
      CORBA::ULong size = (CORBA::ULong) input.size ();
      double* dofArray = hpp::floatSeq::allocbuf(size);
      hpp::floatSeq floats (size, size, dofArray, true);
      for (std::size_t i=0; i<size; ++i) {
        dofArray [i] = input [i];
      }
      return floats;
    }

Steve Tonneau's avatar
Steve Tonneau committed
90
91
92
93
94
95
96
97
98
    void RbprmBuilder::loadRobotRomModel(const char* robotName,
         const char* rootJointType,
         const char* packageName,
         const char* modelName,
         const char* urdfSuffix,
         const char* srdfSuffix) throw (hpp::Error)
    {
        try
        {
stevet's avatar
stevet committed
99
            hpp::pinocchio::DevicePtr_t romDevice = pinocchio::Device::create (robotName);
Steve Tonneau's avatar
Steve Tonneau committed
100
            romDevices_.insert(std::make_pair(robotName, romDevice));
stevet's avatar
stevet committed
101
102
103
104
105
106
            hpp::pinocchio::urdf::loadRobotModel (romDevice,
                              std::string (rootJointType),
                              std::string (packageName),
                              std::string (modelName),
                              std::string (urdfSuffix),
                              std::string (srdfSuffix));
Steve Tonneau's avatar
Steve Tonneau committed
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
        }
        catch (const std::exception& exc)
        {
            hppDout (error, exc.what ());
            throw hpp::Error (exc.what ());
        }
        romLoaded_ = true;
    }

    void RbprmBuilder::loadRobotCompleteModel(const char* robotName,
         const char* rootJointType,
         const char* packageName,
         const char* modelName,
         const char* urdfSuffix,
         const char* srdfSuffix) throw (hpp::Error)
    {
        if(!romLoaded_)
        {
            std::string err("Rom must be loaded before loading complete model") ;
Pierre Fernbach's avatar
Pierre Fernbach committed
126
            hppDout (error, err );
Steve Tonneau's avatar
Steve Tonneau committed
127
128
129
130
            throw hpp::Error(err.c_str());
        }
        try
        {
stevet's avatar
stevet committed
131
132
            hpp::pinocchio::RbPrmDevicePtr_t device = hpp::pinocchio::RbPrmDevice::create (robotName, romDevices_);
            hpp::pinocchio::urdf::loadRobotModel (device,
Steve Tonneau's avatar
Steve Tonneau committed
133
134
135
136
137
138
                    std::string (rootJointType),
                    std::string (packageName),
                    std::string (modelName),
                    std::string (urdfSuffix),
                    std::string (srdfSuffix));
            // Add device to the planner
139
140
            problemSolver()->robot (device);
            problemSolver()->robot ()->controlComputation
141
            (flag);
Steve Tonneau's avatar
Steve Tonneau committed
142
143
144
145
146
147
148
149
        }
        catch (const std::exception& exc)
        {
            hppDout (error, exc.what ());
            throw hpp::Error (exc.what ());
        }
    }

Steve Tonneau's avatar
Steve Tonneau committed
150
151
152
153
154
    void RbprmBuilder::loadFullBodyRobot(const char* robotName,
         const char* rootJointType,
         const char* packageName,
         const char* modelName,
         const char* urdfSuffix,
Steve Tonneau's avatar
Steve Tonneau committed
155
156
         const char* srdfSuffix,
         const char* selectedProblem) throw (hpp::Error)
Steve Tonneau's avatar
Steve Tonneau committed
157
158
159
    {
        try
        {
stevet's avatar
stevet committed
160
161
            hpp::pinocchio::DevicePtr_t device = pinocchio::Device::create (robotName);
            hpp::pinocchio::urdf::loadRobotModel (device,
Steve Tonneau's avatar
Steve Tonneau committed
162
163
164
165
166
                    std::string (rootJointType),
                    std::string (packageName),
                    std::string (modelName),
                    std::string (urdfSuffix),
                    std::string (srdfSuffix));
Steve Tonneau's avatar
Steve Tonneau committed
167
            std::string name(selectedProblem);
Steve Tonneau's avatar
Steve Tonneau committed
168
169
170
            fullBodyLoaded_ = true;
            fullBodyMap_.map_[name] = rbprm::RbPrmFullBody::create(device);
            fullBodyMap_.selected_ = name;
171
172
173
            if(problemSolver()){
                if(problemSolver()->problem()){
                    try {
stevet's avatar
stevet committed
174
175
                      double mu = problemSolver()->problem()->getParameter ("friction").floatValue();
                      fullBody()->setFriction(mu);
176
177
178
179
180
181
182
183
184
                      hppDout(notice,"fullbody : mu define in python : "<<fullBody()->getFriction());
                    } catch (const std::exception& e) {
                      hppDout(notice,"fullbody : mu not defined, take : "<<fullBody()->getFriction()<<" as default.");
                    }
                }else{
                    hppDout(warning,"No instance of problem while initializing fullBody");
                }
            }else{
                hppDout(warning,"No instance of problemSolver while initializing fullBody");
185
            }
186
            problemSolver()->pathValidationType ("Discretized",0.05); // reset to avoid conflict with rbprm path
Steve Tonneau's avatar
Steve Tonneau committed
187
            problemSolver()->robot (fullBody()->device_);
188
189
            problemSolver()->resetProblem();
            problemSolver()->robot ()->controlComputation
190
            (flag);
Pierre Fernbach's avatar
Pierre Fernbach committed
191
            refPose_ = fullBody()->device_->currentConfiguration();
Steve Tonneau's avatar
Steve Tonneau committed
192
193
194
        }
        catch (const std::exception& exc)
        {
Steve Tonneau's avatar
Steve Tonneau committed
195
            fullBodyLoaded_ = false;
Steve Tonneau's avatar
Steve Tonneau committed
196
197
198
            hppDout (error, exc.what ());
            throw hpp::Error (exc.what ());
        }
Steve Tonneau's avatar
Steve Tonneau committed
199
        analysisFactory_ = new sampling::AnalysisFactory(fullBody());
Steve Tonneau's avatar
Steve Tonneau committed
200
201
    }

202
203
204
205
    void RbprmBuilder::loadFullBodyRobotFromExistingRobot() throw (hpp::Error)
    {
        try
        {
Steve Tonneau's avatar
Steve Tonneau committed
206
            fullBody() = rbprm::RbPrmFullBody::create(problemSolver()->problem()->robot());
207
            problemSolver()->pathValidationType ("Discretized",0.05); // reset to avoid conflict with rbprm path
Steve Tonneau's avatar
Steve Tonneau committed
208
            problemSolver()->robot (fullBody()->device_);
209
210
            problemSolver()->resetProblem();
            problemSolver()->robot ()->controlComputation
211
            (flag);
212
213
214
215
216
217
218
        }
        catch (const std::exception& exc)
        {
            hppDout (error, exc.what ());
            throw hpp::Error (exc.what ());
        }
        fullBodyLoaded_ = true;
Steve Tonneau's avatar
Steve Tonneau committed
219
        analysisFactory_ = new sampling::AnalysisFactory(fullBody());
220
221
    }

222
    hpp::floatSeq* RbprmBuilder::getSampleConfig(const char* limb, unsigned int sampleId) throw (hpp::Error)
Steve Tonneau's avatar
Steve Tonneau committed
223
224
225
    {
        if(!fullBodyLoaded_)
            throw Error ("No full body robot was loaded");
Steve Tonneau's avatar
Steve Tonneau committed
226
        const T_Limb& limbs = fullBody()->GetLimbs();
Steve Tonneau's avatar
Steve Tonneau committed
227
228
229
        T_Limb::const_iterator lit = limbs.find(std::string(limb));
        if(lit == limbs.end())
        {
Steve Tonneau's avatar
Steve Tonneau committed
230
            std::string err("No limb " + std::string(limb) + "was defined for robot" + fullBody()->device_->name());
Steve Tonneau's avatar
Steve Tonneau committed
231
232
233
234
            throw Error (err.c_str());
        }
        const RbPrmLimbPtr_t& limbPtr = lit->second;
        hpp::floatSeq *dofArray;
Steve Tonneau's avatar
Steve Tonneau committed
235
        Eigen::VectorXd config = fullBody()->device_->currentConfiguration ();
Steve Tonneau's avatar
Steve Tonneau committed
236
237
238
239
240
241
        if(sampleId > limbPtr->sampleContainer_.samples_.size())
        {
            std::string err("Limb " + std::string(limb) + "does not have samples.");
            throw Error (err.c_str());
        }
        const sampling::Sample& sample = limbPtr->sampleContainer_.samples_[sampleId];
Steve Tonneau's avatar
Steve Tonneau committed
242
        config.segment(sample.startRank_, sample.length_) = sample.configuration_;
Steve Tonneau's avatar
Steve Tonneau committed
243
244
245
246
247
248
249
        dofArray = new hpp::floatSeq();
        dofArray->length(_CORBA_ULong(config.rows()));
        for(std::size_t i=0; i< _CORBA_ULong(config.rows()); i++)
          (*dofArray)[(_CORBA_ULong)i] = config [i];
        return dofArray;
    }

Steve Tonneau's avatar
Steve Tonneau committed
250

251
    hpp::floatSeq* RbprmBuilder::getSamplePosition(const char* limb, unsigned int sampleId) throw (hpp::Error)
Steve Tonneau's avatar
Steve Tonneau committed
252
253
254
    {
        if(!fullBodyLoaded_)
            throw Error ("No full body robot was loaded");
Steve Tonneau's avatar
Steve Tonneau committed
255
        const T_Limb& limbs = fullBody()->GetLimbs();
Steve Tonneau's avatar
Steve Tonneau committed
256
257
258
        T_Limb::const_iterator lit = limbs.find(std::string(limb));
        if(lit == limbs.end())
        {
Steve Tonneau's avatar
Steve Tonneau committed
259
            std::string err("No limb " + std::string(limb) + "was defined for robot" + fullBody()->device_->name());
Steve Tonneau's avatar
Steve Tonneau committed
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
            throw Error (err.c_str());
        }
        const RbPrmLimbPtr_t& limbPtr = lit->second;
        hpp::floatSeq *dofArray;
        if(sampleId > limbPtr->sampleContainer_.samples_.size())
        {
            std::string err("Limb " + std::string(limb) + "does not have samples.");
            throw Error (err.c_str());
        }
        const sampling::Sample& sample = limbPtr->sampleContainer_.samples_[sampleId];
        const fcl::Vec3f& position = sample.effectorPosition_;
        dofArray = new hpp::floatSeq();
        dofArray->length(_CORBA_ULong(3));
        for(std::size_t i=0; i< 3; i++)
          (*dofArray)[(_CORBA_ULong)i] = position [i];
        return dofArray;
    }

Steve Tonneau's avatar
Steve Tonneau committed
278

279
280
281
282
283
    typedef Eigen::Matrix <value_type, 4, 3, Eigen::RowMajor> Matrix43;
    typedef Eigen::Matrix <value_type, 4, 3, Eigen::RowMajor> Rotation;
    typedef Eigen::Ref<Matrix43> Ref_matrix43;

    std::vector<fcl::Vec3f> computeRectangleContact(const rbprm::RbPrmFullBodyPtr_t device,
284
                                                    const rbprm::State& state, const std::vector<std::string> limbSelected = std::vector<std::string>())
285
    {
286
287
        device->device_->currentConfiguration(state.configuration_);
        device->device_->computeForwardKinematics();
288
289
290
291
292
293
294
295
        std::vector<fcl::Vec3f> res;
        const rbprm::T_Limb& limbs = device->GetLimbs();
        rbprm::RbPrmLimbPtr_t limb;
        Matrix43 p; Eigen::Matrix3d R;
        for(std::map<std::string, fcl::Vec3f>::const_iterator cit = state.contactPositions_.begin();
            cit != state.contactPositions_.end(); ++cit)
        {
            const std::string& name = cit->first;
296
297
298
299
300
            if(limbSelected.empty() || std::find(limbSelected.begin(), limbSelected.end(), name) != limbSelected.end())
            {
                const fcl::Vec3f& position = cit->second;
                limb = limbs.at(name);
                const fcl::Vec3f& normal = state.contactNormals_.at(name);
301
                const fcl::Vec3f z = limb->effector_.currentTransformation().rotation() * limb->normal_;
302
                const fcl::Matrix3f alignRotation = tools::GetRotationMatrix(z,normal);
303
                const fcl::Matrix3f rotation = alignRotation * limb->effector_.currentTransformation().rotation();
304
305
306
307
308
309
310
                const fcl::Vec3f offset = rotation * limb->offset_;
                const double& lx = limb->x_, ly = limb->y_;
                p << lx,  ly, 0,
                     lx, -ly, 0,
                    -lx, -ly, 0,
                    -lx,  ly, 0;
                if(limb->contactType_ == _3_DOF)
311
                {
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
                    //create rotation matrix from normal
                    fcl::Vec3f z_fcl = state.contactNormals_.at(name);
                    Eigen::Vector3d z,x,y;
                    for(int i =0; i<3; ++i) z[i] = z_fcl[i];
                    x = z.cross(Eigen::Vector3d(0,-1,0));
                    if(x.norm() < 10e-6)
                    {
                        y = z.cross(fcl::Vec3f(1,0,0));
                        y.normalize();
                        x = y.cross(z);
                    }
                    else
                    {
                        x.normalize();
                        y = z.cross(x);
                    }
                    R.block<3,1>(0,0) = x;
                    R.block<3,1>(0,1) = y;
                    R.block<3,1>(0,2) = z;
                    /*for(std::size_t i =0; i<4; ++i)
                    {
                        res.push_back(position + (R*(p.row(i).transpose())) + offset);
                        res.push_back(state.contactNormals_.at(name));
                    }*/
                    res.push_back(position + (R*(offset)));
                    res.push_back(state.contactNormals_.at(name));
338
339
340
                }
                else
                {
341
342
343
344
345
346
347
348
349
350
351
                    const fcl::Matrix3f& fclRotation = state.contactRotation_.at(name);
                    for(int i =0; i< 3; ++i)
                        for(int j =0; j<3;++j)
                            R(i,j) = fclRotation(i,j);
                    fcl::Vec3f z_axis(0,0,1);
                    fcl::Matrix3f rotationLocal = tools::GetRotationMatrix(z_axis, limb->normal_);
                    for(std::size_t i =0; i<4; ++i)
                    {
                        res.push_back(position + (R*(rotationLocal*(p.row(i).transpose() + limb->offset_))));
                        res.push_back(state.contactNormals_.at(name));
                    }
352
                }
353
354
355
356
357
            }
        }
        return res;
    }

358
    std::vector<fcl::Vec3f> computeRectangleContactLocalTr(const rbprm::RbPrmFullBodyPtr_t device,
359
                                                    const rbprm::State& state,
Steve Tonneau's avatar
Steve Tonneau committed
360
                                                    const std::string& limbName)
361
    {
362
363
        device->device_->currentConfiguration(state.configuration_);
        device->device_->computeForwardKinematics();
364
365
366
        std::vector<fcl::Vec3f> res;
        const rbprm::T_Limb& limbs = device->GetLimbs();
        rbprm::RbPrmLimbPtr_t limb;
367
        Matrix43 p; Eigen::Matrix3d cFrame = Eigen::Matrix3d::Identity();
368
369
370
371
372
373
374
375
        for(std::map<std::string, fcl::Vec3f>::const_iterator cit = state.contactPositions_.begin();
            cit != state.contactPositions_.end(); ++cit)
        {
            const std::string& name = cit->first;
            if(limbName == name)
            {
                const fcl::Vec3f& position = cit->second;
                limb = limbs.at(name);
376
                const fcl::Vec3f& normal = state.contactNormals_.at(name);
377
                const fcl::Vec3f z = limb->effector_.currentTransformation().rotation() * limb->normal_;
378
                const fcl::Matrix3f alignRotation = tools::GetRotationMatrix(z,normal);
379
                const fcl::Matrix3f rotation = alignRotation * limb->effector_.currentTransformation().rotation();
380
                const fcl::Vec3f offset = rotation * limb->offset_;
381
382
383
384
385
386
387
388
389
                const double& lx = limb->x_, ly = limb->y_;
                p << lx,  ly, 0,
                     lx, -ly, 0,
                    -lx, -ly, 0,
                    -lx,  ly, 0;
                if(limb->contactType_ == _3_DOF)
                {
                    //create rotation matrix from normal
                    Eigen::Vector3d z,x,y;
390
                    for(int i =0; i<3; ++i) z[i] = normal[i];
391
392
393
394
395
396
397
398
399
400
401
402
                    x = z.cross(Eigen::Vector3d(0,-1,0));
                    if(x.norm() < 10e-6)
                    {
                        y = z.cross(fcl::Vec3f(1,0,0));
                        y.normalize();
                        x = y.cross(z);
                    }
                    else
                    {
                        x.normalize();
                        y = z.cross(x);
                    }
Steve Tonneau's avatar
Steve Tonneau committed
403
404
405
                    cFrame.block<3,1>(0,0) = x;
                    cFrame.block<3,1>(0,1) = y;
                    cFrame.block<3,1>(0,2) = z;
406
                }
407
408
409
410
                fcl::Transform3f roWorld, roEffector;
                roWorld.setRotation(state.contactRotation_.at(name));
                roWorld.setTranslation(position);
                roEffector = roWorld; roEffector.inverse();
411
412
                fcl::Vec3f z_axis(0,0,1);
                fcl::Matrix3f rotationLocal = tools::GetRotationMatrix(z_axis, limb->normal_);
Steve Tonneau's avatar
Steve Tonneau committed
413
                if(limb->contactType_ == _3_DOF)
414
                {
Steve Tonneau's avatar
Steve Tonneau committed
415
416
417
418
419
420
421
                    fcl::Vec3f pworld = position +  offset;
                    res.push_back((roEffector * pworld).getTranslation());
                    res.push_back(roEffector.getRotation() * state.contactNormals_.at(name));
                }
                else
                {
                    for(std::size_t i =0; i<4; ++i)
422
                    {
Steve Tonneau's avatar
Steve Tonneau committed
423
424
425
426
427
428
429
430
431
                        /*if(limb->contactType_ == _3_DOF)
                        {
                            fcl::Vec3f pworld = position + (cFrame*(p.row(i).transpose())) + offset;
                            res.push_back((roEffector * pworld).getTranslation());
                            res.push_back(roEffector.getRotation() * state.contactNormals_.at(name));
                        }
                        else*/
                        {
                            res.push_back(rotationLocal*(p.row(i).transpose()) + limb->offset_);
t steve's avatar
t steve committed
432
433
                            //res.push_back(roEffector.getRotation() * state.contactNormals_.at(name));
                           res.push_back( limb->normal_);
Steve Tonneau's avatar
Steve Tonneau committed
434
                        }
435
436
437
438
439
440
441
442
                    }
                }
                return res;
            }
        }
        return res;
    }

stevet's avatar
stevet committed
443
    pinocchio::Configuration_t dofArrayToConfig (const std::size_t& deviceDim,
444
445
446
447
      const hpp::floatSeq& dofArray)
    {
        std::size_t configDim = (std::size_t)dofArray.length();
        // Fill dof vector with dof array.
stevet's avatar
stevet committed
448
        pinocchio::Configuration_t config; config.resize (configDim);
449
450
451
452
        for (std::size_t iDof = 0; iDof < configDim; iDof++) {
            config [iDof] = (double)dofArray[(_CORBA_ULong)iDof];
        }
        // fill the vector by zero
453
        //hppDout (info, "config dimension: " <<configDim <<",  deviceDim "<<deviceDim);
454
455
456
457
458
459
        if(configDim != deviceDim){
            throw hpp::Error ("dofVector Does not match");
        }
        return config;
    }

stevet's avatar
stevet committed
460
    pinocchio::Configuration_t dofArrayToConfig (const pinocchio::DevicePtr_t& robot,
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
      const hpp::floatSeq& dofArray)
    {
        return dofArrayToConfig(robot->configSize(), dofArray);
    }

    T_Configuration doubleDofArrayToConfig (const std::size_t& deviceDim,
      const hpp::floatSeqSeq& doubleDofArray)
    {
        std::size_t configsDim = (std::size_t)doubleDofArray.length();
        T_Configuration res;
        for (_CORBA_ULong iConfig = 0; iConfig < configsDim; iConfig++)
        {
            res.push_back(dofArrayToConfig(deviceDim, doubleDofArray[iConfig]));
        }
        return res;
    }

stevet's avatar
stevet committed
478
    T_Configuration doubleDofArrayToConfig (const pinocchio::DevicePtr_t& robot,
479
480
481
482
483
484
485
486
487
488
      const hpp::floatSeqSeq& doubleDofArray)
    {
        return doubleDofArrayToConfig(robot->configSize(), doubleDofArray);
    }

    hpp::floatSeqSeq* RbprmBuilder::getEffectorPosition(const char* lb, const hpp::floatSeq& configuration) throw (hpp::Error)
    {
        try
        {
            const std::string limbName(lb);
Steve Tonneau's avatar
Steve Tonneau committed
489
            const RbPrmLimbPtr_t limb = fullBody()->GetLimbs().at(limbName);
stevet's avatar
stevet committed
490
            pinocchio::Configuration_t config = dofArrayToConfig (fullBody()->device_, configuration);
Steve Tonneau's avatar
Steve Tonneau committed
491
492
            fullBody()->device_->currentConfiguration(config);
            fullBody()->device_->computeForwardKinematics();
493
494
495
            State state;
            state.configuration_ = config;
            state.contacts_[limbName] = true;
496
            const fcl::Vec3f position = limb->effector_.currentTransformation().translation();
497
            state.contactPositions_[limbName] = position;
498
            state.contactNormals_[limbName] = fcl::Vec3f(0,0,1);
499
            state.contactRotation_[limbName] = limb->effector_.currentTransformation().rotation();
Pierre Fernbach's avatar
Pierre Fernbach committed
500
            std::vector<fcl::Vec3f> poss = (computeRectangleContact(fullBody(), state));
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525

            hpp::floatSeqSeq *res;
            res = new hpp::floatSeqSeq ();
            res->length ((_CORBA_ULong)poss.size ());
            for(std::size_t i = 0; i < poss.size(); ++i)
            {
                _CORBA_ULong size = (_CORBA_ULong) (3);
                double* dofArray = hpp::floatSeq::allocbuf(size);
                hpp::floatSeq floats (size, size, dofArray, true);
                //convert the config in dofseq
                for(std::size_t j=0; j<3; j++)
                {
                    dofArray[j] = poss[i][j];
                }
                (*res) [(_CORBA_ULong)i] = floats;
            }
            return res;
        }
        catch (const std::exception& exc)
        {
            throw hpp::Error (exc.what ());
        }
    }


Steve Tonneau's avatar
Steve Tonneau committed
526
527
    CORBA::UShort RbprmBuilder::getNumSamples(const char* limb) throw (hpp::Error)
    {
Steve Tonneau's avatar
Steve Tonneau committed
528
        const T_Limb& limbs = fullBody()->GetLimbs();
Steve Tonneau's avatar
Steve Tonneau committed
529
530
531
        T_Limb::const_iterator lit = limbs.find(std::string(limb));
        if(lit == limbs.end())
        {
Steve Tonneau's avatar
Steve Tonneau committed
532
            std::string err("No limb " + std::string(limb) + "was defined for robot" + fullBody()->device_->name());
Steve Tonneau's avatar
Steve Tonneau committed
533
534
            throw Error (err.c_str());
        }
535
        return (CORBA::UShort)(lit->second->sampleContainer_.samples_.size());
Steve Tonneau's avatar
Steve Tonneau committed
536
537
    }

538
539
    floatSeq *RbprmBuilder::getOctreeNodeIds(const char* limb) throw (hpp::Error)
    {
Steve Tonneau's avatar
Steve Tonneau committed
540
        const T_Limb& limbs = fullBody()->GetLimbs();
541
542
543
        T_Limb::const_iterator lit = limbs.find(std::string(limb));
        if(lit == limbs.end())
        {
Steve Tonneau's avatar
Steve Tonneau committed
544
            std::string err("No limb " + std::string(limb) + "was defined for robot" + fullBody()->device_->name());
545
546
547
548
            throw Error (err.c_str());
        }
        const sampling::T_VoxelSampleId& ids =  lit->second->sampleContainer_.samplesInVoxels_;
        hpp::floatSeq* dofArray = new hpp::floatSeq();
549
        dofArray->length((_CORBA_ULong)ids.size());
550
551
552
        sampling::T_VoxelSampleId::const_iterator it = ids.begin();
        for(std::size_t i=0; i< _CORBA_ULong(ids.size()); ++i, ++it)
        {
553
          (*dofArray)[(_CORBA_ULong)i] = (double)it->first;
554
555
556
557
        }
        return dofArray;
    }

558
    double RbprmBuilder::getSampleValue(const char* limb, const char* valueName, unsigned int sampleId) throw (hpp::Error)
Steve Tonneau's avatar
Steve Tonneau committed
559
    {
Steve Tonneau's avatar
Steve Tonneau committed
560
        const T_Limb& limbs = fullBody()->GetLimbs();
Steve Tonneau's avatar
Steve Tonneau committed
561
562
563
        T_Limb::const_iterator lit = limbs.find(std::string(limb));
        if(lit == limbs.end())
        {
Steve Tonneau's avatar
Steve Tonneau committed
564
            std::string err("No limb " + std::string(limb) + "was defined for robot" + fullBody()->device_->name());
Steve Tonneau's avatar
Steve Tonneau committed
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
            throw Error (err.c_str());
        }
        const sampling::SampleDB& database = lit->second->sampleContainer_;
        if (database.samples_.size() <= sampleId)
        {
            std::string err("unexisting sample id " + sampleId);
            throw Error (err.c_str());
        }
        sampling::T_Values::const_iterator cit = database.values_.find(std::string(valueName));
        if(cit == database.values_.end())
        {
            std::string err("value not existing in database " + std::string(valueName));
            throw Error (err.c_str());
        }
        return cit->second[sampleId];
    }

582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599

    double RbprmBuilder::getEffectorDistance(unsigned short state1, unsigned short state2) throw (hpp::Error)
    {
        try
        {
            std::size_t s1((std::size_t)state1), s2((std::size_t)state2);
            if(lastStatesComputed_.size () < s1 || lastStatesComputed_.size () < s2 )
            {
                throw std::runtime_error ("did not find a states at indicated indices: " + std::string(""+s1) + ", " + std::string(""+s2));
            }
            return rbprm::effectorDistance(lastStatesComputed_[s1], lastStatesComputed_[s2]);
        }
        catch(std::runtime_error& e)
        {
            throw Error(e.what());
        }
    }

Steve Tonneau's avatar
Steve Tonneau committed
600
601
602
603
    std::vector<std::string> stringConversion(const hpp::Names_t& dofArray)
    {
        std::vector<std::string> res;
        std::size_t dim = (std::size_t)dofArray.length();
604
        for (_CORBA_ULong iDof = 0; iDof < dim; iDof++)
Steve Tonneau's avatar
Steve Tonneau committed
605
606
607
608
609
        {
            res.push_back(std::string(dofArray[iDof]));
        }
        return res;
    }
Steve Tonneau's avatar
Steve Tonneau committed
610

Steve Tonneau's avatar
Steve Tonneau committed
611
612
613
614
    std::vector<double> doubleConversion(const hpp::floatSeq& dofArray)
    {
        std::vector<double> res;
        std::size_t dim = (std::size_t)dofArray.length();
615
        for (_CORBA_ULong iDof = 0; iDof < dim; iDof++)
Steve Tonneau's avatar
Steve Tonneau committed
616
617
618
619
620
621
        {
            res.push_back(dofArray[iDof]);
        }
        return res;
    }

622
    void RbprmBuilder::setStaticStability(const bool staticStability) throw (hpp::Error){
623
624
      if(!fullBodyLoaded_)
        throw Error ("No full body robot was loaded");
Pierre Fernbach's avatar
Pierre Fernbach committed
625
      fullBody()->staticStability(staticStability);
626
627
    }

628
629
630
    void RbprmBuilder::setReferenceConfig(const hpp::floatSeq& referenceConfig) throw (hpp::Error){
      if(!fullBodyLoaded_)
        throw Error ("No full body robot was loaded");
631
632
      Configuration_t config(dofArrayToConfig (fullBody()->device_, referenceConfig));
      refPose_ = config;
Pierre Fernbach's avatar
Pierre Fernbach committed
633
      fullBody()->referenceConfig(config);
634
635
    }

636
637
638
639
640
641
642
643
644
645
646
    void RbprmBuilder::setReferenceEndEffector(const char* romName, const hpp::floatSeq &ref) throw(hpp::Error){
      std::string name (romName);
      hpp::pinocchio::RbPrmDevicePtr_t device = boost::dynamic_pointer_cast<hpp::pinocchio::RbPrmDevice>(problemSolver()->robot ());
      if(!device)
        throw Error ("No rbprmDevice in problemSolver");
      if(device->robotRoms_.find(name) == device->robotRoms_.end())
        throw Error("Device do not contain this rom ");
      Configuration_t config(dofArrayToConfig (3, ref));
      device->setEffectorReference(name,config);
    }

647

Steve Tonneau's avatar
Steve Tonneau committed
648
649
650
651
652
653

    void RbprmBuilder::setFilter(const hpp::Names_t& roms) throw (hpp::Error)
    {
        bindShooter_.romFilter_ = stringConversion(roms);
    }

654

655
    void RbprmBuilder::setAffordanceFilter(const char* romName, const hpp::Names_t& affordances) throw (hpp::Error)
656
    {
657
658
659
660
        std::string name (romName);
				std::vector<std::string> affNames = stringConversion(affordances);
        bindShooter_.affFilter_.erase(name);
        bindShooter_.affFilter_.insert(std::make_pair(name,affNames));
661
662
    }

Steve Tonneau's avatar
Steve Tonneau committed
663
664
665
666
667
668
669
670
671
672
673
    void RbprmBuilder::boundSO3(const hpp::floatSeq& limitszyx) throw (hpp::Error)
    {
        std::vector<double> limits = doubleConversion(limitszyx);
        if(limits.size() !=6)
        {
            throw Error ("Can not bound SO3, array of 6 double required");
        }
        bindShooter_.so3Bounds_ = limits;

    }

674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
    rbprm::T_Limb GetFreeLimbs(const RbPrmFullBodyPtr_t fullBody, const hpp::rbprm::State &from, const hpp::rbprm::State &to)
    {
        rbprm::T_Limb res;
        std::vector<std::string> fixedContacts = to.fixedContacts(from);
        std::vector<std::string> variations = to.contactVariations(from);
        for(rbprm::CIT_Limb cit = fullBody->GetLimbs().begin();
            cit != fullBody->GetLimbs().end(); ++cit)
        {
            if(std::find(fixedContacts.begin(), fixedContacts.end(), cit->first) == fixedContacts.end())
            {
                if(std::find(variations.begin(), variations.end(), cit->first) != variations.end())
                {
                    res.insert(*cit);
                }
            }
        }
        return res;
    }

t steve's avatar
t steve committed
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
    rbprm::T_Limb GetFreeLimbs(const RbPrmFullBodyPtr_t fullBody, const hpp::rbprm::State &state)
    {
        rbprm::T_Limb res;
        std::vector<std::string> fixedContacts = state.fixedContacts(state);
        for(rbprm::CIT_Limb cit = fullBody->GetLimbs().begin();
            cit != fullBody->GetLimbs().end(); ++cit)
        {
            if(std::find(fixedContacts.begin(), fixedContacts.end(), cit->first) == fixedContacts.end())
            {
                    res.insert(*cit);
            }
        }
        return res;
    }

stevet's avatar
stevet committed
708
    double RbprmBuilder::projectStateToCOMEigen(unsigned short stateId, const pinocchio::Configuration_t& com_target, unsigned short maxNumeSamples) throw (hpp::Error){
709
710
711
712
713
714
715
716
717
718
719
        if(lastStatesComputed_.size() <= stateId)
        {
            throw std::runtime_error ("Unexisting state " + std::string(""+(stateId)));
        }
        State s = lastStatesComputed_[stateId];
        double success =  projectStateToCOMEigen(s,com_target,maxNumeSamples);
        lastStatesComputed_[stateId] = s;
        lastStatesComputedTime_[stateId].second = s;
        return success;
    }

stevet's avatar
stevet committed
720
    double RbprmBuilder::projectStateToCOMEigen(State& s, const pinocchio::Configuration_t& com_target, unsigned short maxNumeSamples) throw (hpp::Error)
721
722
723
    {
        try
        {
stevet's avatar
stevet committed
724
//            /hpp::pinocchio::Configuration_t c = rbprm::interpolation::projectOnCom(fullBody(), problemSolver()->problem(),s,com_target,succes);
Pierre Fernbach's avatar
Pierre Fernbach committed
725
            hppDout(notice,"ProjectStateToComEigen, init config in state : "<<pinocchio::displayConfig(s.configuration_));
Steve Tonneau's avatar
Steve Tonneau committed
726
            rbprm::projection::ProjectionReport rep = rbprm::projection::projectToComPosition(fullBody(),com_target,s);
stevet's avatar
stevet committed
727
            hpp::pinocchio::Configuration_t& c = rep.result_.configuration_;
728
            ValidationReportPtr_t rport (ValidationReportPtr_t(new CollisionValidationReport));
Steve Tonneau's avatar
Steve Tonneau committed
729
            CollisionValidationPtr_t val = fullBody()->GetCollisionValidation();
730
            if(!rep.success_){
stevet's avatar
stevet committed
731
                hppDout(notice,"Projection failed for state with config = "<<pinocchio::displayConfig(c));
732
733
            }
            if(rep.success_){
734
                hppDout(notice,"Projection successfull for state without collision check.");
735
736
                rep.success_ =  rep.success_ &&  val->validate(rep.result_.configuration_,rport);
                if(!rep.success_){
stevet's avatar
stevet committed
737
                    hppDout(notice,"Projection failed after collision check for state with config = "<<pinocchio::displayConfig(c));
738
                    hppDout(notice,"report : "<<*rport);
739
740
                }else{
                    hppDout(notice,"Success after collision check");
741
742
                }
            }
743
744
            if (! rep.success_ && maxNumeSamples>0)
            {
745
                hppDout(notice,"Projection for state failed, try to randomly sample other initial point : ");
746
                Configuration_t head = s.configuration_.head<7>();
747
748
                size_t ecs_size = fullBody()->device_->extraConfigSpace().dimension ();
                Configuration_t ecs = s.configuration_.tail(ecs_size);
Pierre Fernbach's avatar
Pierre Fernbach committed
749
                core::configurationShooter::UniformPtr_t shooter = core::configurationShooter::Uniform::create(fullBody()->device_);
750
751
752
753
                for(std::size_t i =0; !rep.success_ && i< maxNumeSamples; ++i)
                {
                    s.configuration_ = *shooter->shoot();
                    s.configuration_.head<7>() = head;
754
                    s.configuration_.tail(ecs_size) = ecs;
Steve Tonneau's avatar
Steve Tonneau committed
755
                    //c = rbprm::interpolation::projectOnCom(fullBody(), problemSolver()->problem(),s,com_target,succes);
stevet's avatar
stevet committed
756
                    hppDout(notice,"Sample before prjection : r(["<<pinocchio::displayConfig(s.configuration_)<<"])");
Steve Tonneau's avatar
Steve Tonneau committed
757
                    rep = rbprm::projection::projectToComPosition(fullBody(),com_target,s);
stevet's avatar
stevet committed
758
                    hppDout(notice,"Sample after prjection : r(["<<pinocchio::displayConfig(rep.result_.configuration_)<<"])");
759
                    if(!rep.success_){
760
                        hppDout(notice,"Projection failed on iter "<<i);
761
762
                    }
                    if(rep.success_){
763
                        hppDout(notice,"Projection successfull on iter "<<i<<" without collision check.");
764
765
                        rep.success_ =  rep.success_ &&  val->validate(rep.result_.configuration_,rport);
                        if(!rep.success_){
766
                            hppDout(notice,"Projection failed on iter "<<i<<" after collision check");
767
                            hppDout(notice,"report : "<<*rport);
768
769
                        }else{
                            hppDout(notice,"Success after collision check");
770
771
                        }
                    }
772
773
774
775
                    c = rep.result_.configuration_;
                }
            }
            if(rep.success_)
776
            {
stevet's avatar
stevet committed
777
778
                hppDout(notice,"Valid configuration found after projection : r(["<<pinocchio::displayConfig(c)<<"])");
                hpp::pinocchio::Configuration_t trySave = c;
779
                rbprm::T_Limb fLimbs = GetFreeLimbs(fullBody(), s);
t steve's avatar
t steve committed
780
781
782
783
784
                for(rbprm::CIT_Limb cit = fLimbs.begin(); cit != fLimbs.end() && rep.success_; ++cit)
                {
                    // get part of reference configuration that concerns the limb.
                    RbPrmLimbPtr_t limb = cit->second;
                    const sampling::Sample& sample = limb->sampleContainer_.samples_[0];
Pierre Fernbach's avatar
Pierre Fernbach committed
785
                    s.configuration_.segment(sample.startRank_, sample.length_) = refPose_.segment(sample.startRank_, sample.length_) ;
t steve's avatar
t steve committed
786
787
                    rep = rbprm::projection::projectToComPosition(fullBody(),com_target,s);
                    rep.success_ = rep.success_ && val->validate(rep.result_.configuration_,rport);
788
789
790
791
792
793
                    if(rep.success_)
                    {
                        trySave = rep.result_.configuration_;
                    }
                    else
                    {
stevet's avatar
stevet committed
794
                        //
795
                    }
t steve's avatar
t steve committed
796
                }
797
798
                s.configuration_ = trySave;
                hppDout(notice,"ProjectoToComEigen success");
799
800
                return 1.;
            }
stevet's avatar
stevet committed
801
            hppDout(notice,"No valid configurations can be found after projection : r(["<<pinocchio::displayConfig(c)<<"])");
802
            hppDout(notice,"ProjectoToComEigen failure");
803
804
805
806
            return 0;
        }
        catch(std::runtime_error& e)
        {
807
            std::cout << "ERREUR " << std::endl;
808
809
810
811
            throw Error(e.what());
        }
    }

Steve Tonneau's avatar
Steve Tonneau committed
812
813
    CORBA::Short RbprmBuilder::createState(const hpp::floatSeq& configuration, const hpp::Names_t& contactLimbs) throw (hpp::Error)
    {
stevet's avatar
stevet committed
814
        pinocchio::Configuration_t config = dofArrayToConfig (fullBody()->device_, configuration);
Steve Tonneau's avatar
Steve Tonneau committed
815
816
        fullBody()->device_->currentConfiguration(config);
        fullBody()->device_->computeForwardKinematics();
Steve Tonneau's avatar
Steve Tonneau committed
817
818
819
820
821
        State state;
        state.configuration_ = config;
        std::vector<std::string> names = stringConversion(contactLimbs);
        for(std::vector<std::string>::const_iterator cit = names.begin(); cit != names.end(); ++cit)
        {
Steve Tonneau's avatar
Steve Tonneau committed
822
            rbprm::RbPrmLimbPtr_t limb = fullBody()->GetLimbs().at(*cit);
Steve Tonneau's avatar
Steve Tonneau committed
823
824
            const std::string& limbName = *cit;
            state.contacts_[limbName] = true;
825
            const fcl::Vec3f position = limb->effector_.currentTransformation().translation();
Steve Tonneau's avatar
Steve Tonneau committed
826
            state.contactPositions_[limbName] = position;
827
828
            state.contactNormals_[limbName] = limb->effector_.currentTransformation().rotation() * limb->normal_;
            state.contactRotation_[limbName] = limb->effector_.currentTransformation().rotation();
829
            state.contactOrder_.push(limbName);
Steve Tonneau's avatar
Steve Tonneau committed
830
        }
831
        state.nbContacts = state.contactNormals_.size();
Steve Tonneau's avatar
Steve Tonneau committed
832
        lastStatesComputed_.push_back(state);
Steve Tonneau's avatar
Steve Tonneau committed
833
        lastStatesComputedTime_.push_back(std::make_pair(-1., state));
834
        return (CORBA::Short)(lastStatesComputed_.size()-1);
Steve Tonneau's avatar
Steve Tonneau committed
835
836
    }

837
    double RbprmBuilder::projectStateToCOM(unsigned short stateId, const hpp::floatSeq& com, unsigned short max_num_sample) throw (hpp::Error)
838
    {
stevet's avatar
stevet committed
839
        pinocchio::Configuration_t com_target = dofArrayToConfig (3, com);
840
        return projectStateToCOMEigen(stateId, com_target, max_num_sample);
841
842
    }

843

844
    rbprm::State RbprmBuilder::generateContacts_internal(const hpp::floatSeq& configuration,
845
      const hpp::floatSeq& direction,const hpp::floatSeq& acceleration, const double robustnessThreshold ) throw (hpp::Error)
Steve Tonneau's avatar
Steve Tonneau committed
846
847
848
849
850
    {
        if(!fullBodyLoaded_)
            throw Error ("No full body robot was loaded");
        try
        {
851
            fcl::Vec3f dir,acc;
Steve Tonneau's avatar
Steve Tonneau committed
852
853
            for(std::size_t i =0; i <3; ++i)
            {
854
                dir[i] = direction[(_CORBA_ULong)i];
855
                acc[i] = acceleration[(_CORBA_ULong)i];
Steve Tonneau's avatar
Steve Tonneau committed
856
            }
stevet's avatar
stevet committed
857
            dir.normalize();
858

stevet's avatar
stevet committed
859
860
            const affMap_t &affMap = problemSolver()->affordanceObjects;
                if (affMap.map.empty ()) {
861
    	        throw hpp::Error ("No affordances found. Unable to generate Contacts.");
862
              }
stevet's avatar
stevet committed
863
            pinocchio::Configuration_t config = dofArrayToConfig (fullBody()->device_, configuration);
Steve Tonneau's avatar
Steve Tonneau committed
864
            rbprm::State state = rbprm::contact::ComputeContacts(fullBody(),config,
Pierre Fernbach's avatar
Pierre Fernbach committed
865
              affMap, bindShooter_.affFilter_, dir,robustnessThreshold,acc);
866
867
868
869
870
871
872
873
874
875
876
877
            return state;
        } catch (const std::exception& exc) {
        throw hpp::Error (exc.what ());
        }
    }

    hpp::floatSeq* RbprmBuilder::generateContacts(const hpp::floatSeq& configuration,
      const hpp::floatSeq& direction,const hpp::floatSeq& acceleration, const double robustnessThreshold ) throw (hpp::Error)
    {
        try
        {
            rbprm::State state = generateContacts_internal(configuration,direction,acceleration,robustnessThreshold);
Steve Tonneau's avatar
Steve Tonneau committed
878
            hpp::floatSeq* dofArray = new hpp::floatSeq();
Steve Tonneau's avatar
stair    
Steve Tonneau committed
879
            dofArray->length(_CORBA_ULong(state.configuration_.rows()));
880
            for(std::size_t i=0; i< _CORBA_ULong(state.configuration_.rows()); i++)
Steve Tonneau's avatar
Steve Tonneau committed
881
882
883
884
885
886
887
              (*dofArray)[(_CORBA_ULong)i] = state.configuration_ [i];
            return dofArray;
        } catch (const std::exception& exc) {
        throw hpp::Error (exc.what ());
        }
    }

888
889
890
891
892
893
894
895
    CORBA::Short RbprmBuilder::generateStateInContact(const hpp::floatSeq& configuration,
      const hpp::floatSeq& direction,const hpp::floatSeq& acceleration, const double robustnessThreshold ) throw (hpp::Error)
    {
        try
        {
            rbprm::State state = generateContacts_internal(configuration,direction,acceleration,robustnessThreshold);
            lastStatesComputed_.push_back(state);
            lastStatesComputedTime_.push_back(std::make_pair(-1., state));
896
            return (CORBA::Short)(lastStatesComputed_.size()-1);
897
898
899
900
901
        } catch (const std::exception& exc) {
        throw hpp::Error (exc.what ());
        }
    }

902
903
904
905
906
907
908
909
    hpp::floatSeq* RbprmBuilder::generateGroundContact(const hpp::Names_t& contactLimbs) throw (hpp::Error)
    {
        if(!fullBodyLoaded_)
            throw Error ("No full body robot was loaded");
        try
        {
            fcl::Vec3f z(0,0,1);
            ValidationReportPtr_t report = ValidationReportPtr_t(new CollisionValidationReport);
Pierre Fernbach's avatar
Pierre Fernbach committed
910
            core::configurationShooter::UniformPtr_t  shooter = core::configurationShooter::Uniform::create(fullBody()->device_);
911
912
            for(int i =0; i< 1000; ++i)
            {
Steve Tonneau's avatar
Steve Tonneau committed
913
                core::DevicePtr_t device = fullBody()->device_->clone();
914
                std::vector<std::string> names = stringConversion(contactLimbs);
915
                core::ConfigProjectorPtr_t proj = core::ConfigProjector::create(device,"proj", 1e-4, 100);
916
917
918
                //hpp::tools::LockJointRec(limb->limb_->name(), body->device_->rootJoint(), proj);
                for(std::vector<std::string>::const_iterator cit = names.begin(); cit !=names.end(); ++cit)
                {
Steve Tonneau's avatar
Steve Tonneau committed
919
                    rbprm::RbPrmLimbPtr_t limb = fullBody()->GetLimbs().at(*cit);
920
                    pinocchio::Transform3f localFrame(1), globalFrame(1);
stevet's avatar
stevet committed
921
                    localFrame.translation(-limb->offset_);
922
923
924
925
                    std::vector<bool> posConstraints;
                    std::vector<bool> rotConstraints;
                    posConstraints.push_back(false);posConstraints.push_back(false);posConstraints.push_back(true);
                    rotConstraints.push_back(true);rotConstraints.push_back(true);rotConstraints.push_back(true);
926
                    const pinocchio::Frame effectorFrame = device->getFrameByName(limb->effector_.name());
927
                    pinocchio::JointPtr_t effectorJoint= limb->effector_.joint();
Steve Tonneau's avatar
Steve Tonneau committed
928
                    proj->add(core::NumericalConstraint::create (constraints::Position::create("",fullBody()->device_,
929
                                                                                               effectorJoint,
930
                                                                                               effectorFrame.pinocchio().placement * globalFrame,
931
932
933
934
935
936
937
                                                                                               localFrame,
                                                                                               posConstraints)));
                    if(limb->contactType_ == hpp::rbprm::_6_DOF)
                    {
                        // rotation matrix around z
                        value_type theta = 2*(value_type(rand()) / value_type(RAND_MAX) - 0.5) * M_PI;
                        fcl::Matrix3f r = tools::GetZRotMatrix(theta);
stevet's avatar
stevet committed
938
                        // TODO not assume identity matrix for effector frame
939
                        fcl::Matrix3f rotation = r * effectorFrame.pinocchio().placement.rotation().transpose();// * limb->effector_->initialPosition ().getRotation();
Steve Tonneau's avatar
Steve Tonneau committed
940
                        proj->add(core::NumericalConstraint::create (constraints::Orientation::create("",fullBody()->device_,
941
                                                                                                      effectorJoint,
stevet's avatar
stevet committed
942
                                                                                                      pinocchio::Transform3f(rotation,fcl::Vec3f::Zero()),
943
944
945
946
947
948
949
                                                                                                      rotConstraints)));
                    }
                }
                ConfigurationPtr_t configptr = shooter->shoot();
                Configuration_t config = *configptr;
                if(proj->apply(config) && config[2]> 0.3)
                {
950
                    if(problemSolver()->problem()->configValidations()->validate(config,report))
951
952
953
954
955
                    {
                        State tmp;
                        for(std::vector<std::string>::const_iterator cit = names.begin(); cit !=names.end(); ++cit)
                        {
                            std::string limbId = *cit;
Steve Tonneau's avatar
Steve Tonneau committed
956
                            rbprm::RbPrmLimbPtr_t limb = fullBody()->GetLimbs().at(*cit);
957
                            tmp.contacts_[limbId] = true;
958
959
                            tmp.contactPositions_[limbId] = limb->effector_.currentTransformation().translation();
                            tmp.contactRotation_[limbId] = limb->effector_.currentTransformation().rotation();
960
961
962
963
                            tmp.contactNormals_[limbId] = z;
                            tmp.configuration_ = config;
                            ++tmp.nbContacts;
                        }
Steve Tonneau's avatar
Steve Tonneau committed
964
                        if(stability::IsStable(fullBody(),tmp)>=0)
965
966
967
968
969
                        {
                            config[0]=0;
                            config[1]=0;
                            hpp::floatSeq* dofArray = new hpp::floatSeq();
                            dofArray->length(_CORBA_ULong(config.rows()));
970
                            for(size_type j=0; j< config.rows(); j++)
971
972
973
974
975
976
977
978
979
980
981
982
983
984
                            {
                              (*dofArray)[(_CORBA_ULong)j] = config [j];
                            }
                            return dofArray;
                        }
                    }
                }
            }
            throw (std::runtime_error("could not generate contact configuration after 1000 trials"));
        } catch (const std::exception& exc) {
        throw hpp::Error (exc.what ());
        }
    }

985
986
987
988
989
990
991
992
    hpp::floatSeq* RbprmBuilder::getContactSamplesIds(const char* limbname,
                                        const hpp::floatSeq& configuration,
                                        const hpp::floatSeq& direction) throw (hpp::Error)
    {
        if(!fullBodyLoaded_)
            throw Error ("No full body robot was loaded");
        try
        {
993
            fcl::Vec3f dir;
994
995
            for(std::size_t i =0; i <3; ++i)
            {
996
                dir[i] = direction[(_CORBA_ULong)i];
997
            }
stevet's avatar
stevet committed
998
999
            pinocchio::Configuration_t config = dofArrayToConfig (fullBody()->device_, configuration);
            pinocchio::Configuration_t save = fullBody()->device_->currentConfiguration();
Steve Tonneau's avatar
Steve Tonneau committed
1000
            fullBody()->device_->currentConfiguration(config);
1001
1002

            sampling::T_OctreeReport finalSet;
Steve Tonneau's avatar
Steve Tonneau committed
1003
1004
            rbprm::T_Limb::const_iterator lit = fullBody()->GetLimbs().find(std::string(limbname));
            if(lit == fullBody()->GetLimbs().end())
1005
1006
            {
                throw std::runtime_error ("Impossible to find limb for joint "
Steve Tonneau's avatar
Steve Tonneau committed
1007
                                          + std::string(limbname) + " to robot; limb not defined.");
1008
1009
            }
            const RbPrmLimbPtr_t& limb = lit->second;
stevet's avatar
stevet committed
1010
1011
            pinocchio::Transform3f transformPin = limb->limb_->robot()->rootJoint()->childJoint(0)->currentTransformation (); // get root transform from configuration
            fcl::Transform3f transform(transformPin.rotation(), transformPin.translation());
1012
                        // TODO fix as in rbprm-fullbody.cc!!
1013
            std::vector<sampling::T_OctreeReport> reports(problemSolver()->collisionObstacles().size());
1014
1015
            std::size_t i (0);
            //#pragma omp parallel for
stevet's avatar
stevet committed
1016
            for(core::ObjectStdVector_t::const_iterator oit = problemSolver()->collisionObstacles().begin();
1017
                oit != problemSolver()->collisionObstacles().end(); ++oit, ++i)
1018
            {
Raphael Lefevre's avatar
Raphael Lefevre committed
1019
                sampling::GetCandidates(limb->sampleContainer_, transform, *oit, dir, reports[i], sampling::HeuristicParam());
1020
1021