rbprmbuilder.impl.cc 158 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-idl.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
            if(problemSolver()){
                if(problemSolver()->problem()){
173
174
175
                    double mu = problemSolver()->problem()->getParameter ("FullBody/frictionCoefficient").floatValue();
                    fullBody()->setFriction(mu);
                    hppDout(notice,"fullbody : friction coefficient used  : "<<fullBody()->getFriction());
176
177
178
179
180
                }else{
                    hppDout(warning,"No instance of problem while initializing fullBody");
                }
            }else{
                hppDout(warning,"No instance of problemSolver while initializing fullBody");
181
            }
182
            problemSolver()->pathValidationType ("Discretized",0.05); // reset to avoid conflict with rbprm path
Steve Tonneau's avatar
Steve Tonneau committed
183
            problemSolver()->robot (fullBody()->device_);
184
185
            problemSolver()->resetProblem();
            problemSolver()->robot ()->controlComputation
186
            (flag);
Pierre Fernbach's avatar
Pierre Fernbach committed
187
            refPose_ = fullBody()->device_->currentConfiguration();
Steve Tonneau's avatar
Steve Tonneau committed
188
189
190
        }
        catch (const std::exception& exc)
        {
Steve Tonneau's avatar
Steve Tonneau committed
191
            fullBodyLoaded_ = false;
Steve Tonneau's avatar
Steve Tonneau committed
192
193
194
            hppDout (error, exc.what ());
            throw hpp::Error (exc.what ());
        }
Steve Tonneau's avatar
Steve Tonneau committed
195
        analysisFactory_ = new sampling::AnalysisFactory(fullBody());
Steve Tonneau's avatar
Steve Tonneau committed
196
197
    }

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

218
    hpp::floatSeq* RbprmBuilder::getSampleConfig(const char* limb, unsigned int sampleId) throw (hpp::Error)
Steve Tonneau's avatar
Steve Tonneau committed
219
220
221
    {
        if(!fullBodyLoaded_)
            throw Error ("No full body robot was loaded");
Steve Tonneau's avatar
Steve Tonneau committed
222
        const T_Limb& limbs = fullBody()->GetLimbs();
Steve Tonneau's avatar
Steve Tonneau committed
223
224
225
        T_Limb::const_iterator lit = limbs.find(std::string(limb));
        if(lit == limbs.end())
        {
Steve Tonneau's avatar
Steve Tonneau committed
226
            std::string err("No limb " + std::string(limb) + "was defined for robot" + fullBody()->device_->name());
Steve Tonneau's avatar
Steve Tonneau committed
227
228
229
230
            throw Error (err.c_str());
        }
        const RbPrmLimbPtr_t& limbPtr = lit->second;
        hpp::floatSeq *dofArray;
Steve Tonneau's avatar
Steve Tonneau committed
231
        Eigen::VectorXd config = fullBody()->device_->currentConfiguration ();
Steve Tonneau's avatar
Steve Tonneau committed
232
233
234
235
236
237
        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
238
        config.segment(sample.startRank_, sample.length_) = sample.configuration_;
Steve Tonneau's avatar
Steve Tonneau committed
239
240
241
242
243
244
245
        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
246

247
    hpp::floatSeq* RbprmBuilder::getSamplePosition(const char* limb, unsigned int sampleId) throw (hpp::Error)
Steve Tonneau's avatar
Steve Tonneau committed
248
249
250
    {
        if(!fullBodyLoaded_)
            throw Error ("No full body robot was loaded");
Steve Tonneau's avatar
Steve Tonneau committed
251
        const T_Limb& limbs = fullBody()->GetLimbs();
Steve Tonneau's avatar
Steve Tonneau committed
252
253
254
        T_Limb::const_iterator lit = limbs.find(std::string(limb));
        if(lit == limbs.end())
        {
Steve Tonneau's avatar
Steve Tonneau committed
255
            std::string err("No limb " + std::string(limb) + "was defined for robot" + fullBody()->device_->name());
Steve Tonneau's avatar
Steve Tonneau committed
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
            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
274

275
276
277
278
279
    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,
280
                                                    const rbprm::State& state, const std::vector<std::string> limbSelected = std::vector<std::string>())
281
    {
282
283
        device->device_->currentConfiguration(state.configuration_);
        device->device_->computeForwardKinematics();
284
285
286
287
288
289
290
291
        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;
292
293
294
295
296
            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);
297
                const fcl::Vec3f z = limb->effector_.currentTransformation().rotation() * limb->normal_;
298
                const fcl::Matrix3f alignRotation = tools::GetRotationMatrix(z,normal);
299
                const fcl::Matrix3f rotation = alignRotation * limb->effector_.currentTransformation().rotation();
300
301
302
303
304
305
306
                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)
307
                {
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
                    //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));
334
335
336
                }
                else
                {
337
338
339
340
341
342
343
344
345
346
347
                    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));
                    }
348
                }
349
350
351
352
353
            }
        }
        return res;
    }

354
    std::vector<fcl::Vec3f> computeRectangleContactLocalTr(const rbprm::RbPrmFullBodyPtr_t device,
355
                                                    const rbprm::State& state,
Steve Tonneau's avatar
Steve Tonneau committed
356
                                                    const std::string& limbName)
357
    {
358
359
        device->device_->currentConfiguration(state.configuration_);
        device->device_->computeForwardKinematics();
360
361
362
        std::vector<fcl::Vec3f> res;
        const rbprm::T_Limb& limbs = device->GetLimbs();
        rbprm::RbPrmLimbPtr_t limb;
363
        Matrix43 p; Eigen::Matrix3d cFrame = Eigen::Matrix3d::Identity();
364
365
366
367
368
369
370
371
        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);
372
                const fcl::Vec3f& normal = state.contactNormals_.at(name);
373
                const fcl::Vec3f z = limb->effector_.currentTransformation().rotation() * limb->normal_;
374
                const fcl::Matrix3f alignRotation = tools::GetRotationMatrix(z,normal);
375
                const fcl::Matrix3f rotation = alignRotation * limb->effector_.currentTransformation().rotation();
376
                const fcl::Vec3f offset = rotation * limb->offset_;
377
378
379
380
381
382
383
384
385
                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;
386
                    for(int i =0; i<3; ++i) z[i] = normal[i];
387
388
389
390
391
392
393
394
395
396
397
398
                    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
399
400
401
                    cFrame.block<3,1>(0,0) = x;
                    cFrame.block<3,1>(0,1) = y;
                    cFrame.block<3,1>(0,2) = z;
402
                }
403
404
405
406
                fcl::Transform3f roWorld, roEffector;
                roWorld.setRotation(state.contactRotation_.at(name));
                roWorld.setTranslation(position);
                roEffector = roWorld; roEffector.inverse();
407
408
                fcl::Vec3f z_axis(0,0,1);
                fcl::Matrix3f rotationLocal = tools::GetRotationMatrix(z_axis, limb->normal_);
Steve Tonneau's avatar
Steve Tonneau committed
409
                if(limb->contactType_ == _3_DOF)
410
                {
Steve Tonneau's avatar
Steve Tonneau committed
411
412
413
414
415
416
417
                    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)
418
                    {
Steve Tonneau's avatar
Steve Tonneau committed
419
420
421
422
423
424
425
426
427
                        /*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
428
429
                            //res.push_back(roEffector.getRotation() * state.contactNormals_.at(name));
                           res.push_back( limb->normal_);
Steve Tonneau's avatar
Steve Tonneau committed
430
                        }
431
432
433
434
435
436
437
438
                    }
                }
                return res;
            }
        }
        return res;
    }

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

stevet's avatar
stevet committed
456
    pinocchio::Configuration_t dofArrayToConfig (const pinocchio::DevicePtr_t& robot,
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
      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
474
    T_Configuration doubleDofArrayToConfig (const pinocchio::DevicePtr_t& robot,
475
476
477
478
479
480
481
482
483
484
      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
485
            const RbPrmLimbPtr_t limb = fullBody()->GetLimbs().at(limbName);
stevet's avatar
stevet committed
486
            pinocchio::Configuration_t config = dofArrayToConfig (fullBody()->device_, configuration);
Steve Tonneau's avatar
Steve Tonneau committed
487
488
            fullBody()->device_->currentConfiguration(config);
            fullBody()->device_->computeForwardKinematics();
489
490
491
            State state;
            state.configuration_ = config;
            state.contacts_[limbName] = true;
492
            const fcl::Vec3f position = limb->effector_.currentTransformation().translation();
493
            state.contactPositions_[limbName] = position;
494
            state.contactNormals_[limbName] = fcl::Vec3f(0,0,1);
495
            state.contactRotation_[limbName] = limb->effector_.currentTransformation().rotation();
Pierre Fernbach's avatar
Pierre Fernbach committed
496
            std::vector<fcl::Vec3f> poss = (computeRectangleContact(fullBody(), state));
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521

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

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

554
    double RbprmBuilder::getSampleValue(const char* limb, const char* valueName, unsigned int sampleId) throw (hpp::Error)
Steve Tonneau's avatar
Steve Tonneau committed
555
    {
Steve Tonneau's avatar
Steve Tonneau committed
556
        const T_Limb& limbs = fullBody()->GetLimbs();
Steve Tonneau's avatar
Steve Tonneau committed
557
558
559
        T_Limb::const_iterator lit = limbs.find(std::string(limb));
        if(lit == limbs.end())
        {
Steve Tonneau's avatar
Steve Tonneau committed
560
            std::string err("No limb " + std::string(limb) + "was defined for robot" + fullBody()->device_->name());
Steve Tonneau's avatar
Steve Tonneau committed
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
            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];
    }

578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595

    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
596
597
598
599
    std::vector<std::string> stringConversion(const hpp::Names_t& dofArray)
    {
        std::vector<std::string> res;
        std::size_t dim = (std::size_t)dofArray.length();
600
        for (_CORBA_ULong iDof = 0; iDof < dim; iDof++)
Steve Tonneau's avatar
Steve Tonneau committed
601
602
603
604
605
        {
            res.push_back(std::string(dofArray[iDof]));
        }
        return res;
    }
Steve Tonneau's avatar
Steve Tonneau committed
606

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

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

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

632
633
634
635
636
637
638
    void RbprmBuilder::setPostureWeights(const hpp::floatSeq& postureWeights) throw (hpp::Error){
      if(!fullBodyLoaded_)
        throw Error ("No full body robot was loaded");
      Configuration_t config(dofArrayToConfig (fullBody()->device_->numberDof(), postureWeights));
      fullBody()->postureWeights(config);
    }

639
640
641
642
643
644
645
646
647
648
649
    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);
    }

650
651
652
653
654
    void RbprmBuilder::usePosturalTaskContactCreation(const bool usePosturalTaskContactCreation) throw (hpp::Error){
      if(!fullBodyLoaded_)
        throw Error ("No full body robot was loaded");
      fullBody()->usePosturalTaskContactCreation(usePosturalTaskContactCreation);
    }
Steve Tonneau's avatar
Steve Tonneau committed
655
656
657
658
659
660

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

661

662
    void RbprmBuilder::setAffordanceFilter(const char* romName, const hpp::Names_t& affordances) throw (hpp::Error)
663
    {
664
665
666
667
        std::string name (romName);
				std::vector<std::string> affNames = stringConversion(affordances);
        bindShooter_.affFilter_.erase(name);
        bindShooter_.affFilter_.insert(std::make_pair(name,affNames));
668
669
    }

Steve Tonneau's avatar
Steve Tonneau committed
670
671
672
673
674
675
676
677
678
679
680
    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;

    }

681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
    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
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
    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
715
    double RbprmBuilder::projectStateToCOMEigen(unsigned short stateId, const pinocchio::Configuration_t& com_target, unsigned short maxNumeSamples) throw (hpp::Error){
716
717
718
719
720
721
722
723
724
725
726
        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
727
    double RbprmBuilder::projectStateToCOMEigen(State& s, const pinocchio::Configuration_t& com_target, unsigned short maxNumeSamples) throw (hpp::Error)
728
729
730
    {
        try
        {
stevet's avatar
stevet committed
731
//            /hpp::pinocchio::Configuration_t c = rbprm::interpolation::projectOnCom(fullBody(), problemSolver()->problem(),s,com_target,succes);
Pierre Fernbach's avatar
Pierre Fernbach committed
732
            hppDout(notice,"ProjectStateToComEigen, init config in state : "<<pinocchio::displayConfig(s.configuration_));
Steve Tonneau's avatar
Steve Tonneau committed
733
            rbprm::projection::ProjectionReport rep = rbprm::projection::projectToComPosition(fullBody(),com_target,s);
stevet's avatar
stevet committed
734
            hpp::pinocchio::Configuration_t& c = rep.result_.configuration_;
735
            ValidationReportPtr_t rport (ValidationReportPtr_t(new CollisionValidationReport));
Steve Tonneau's avatar
Steve Tonneau committed
736
            CollisionValidationPtr_t val = fullBody()->GetCollisionValidation();
737
            if(!rep.success_){
stevet's avatar
stevet committed
738
                hppDout(notice,"Projection failed for state with config = "<<pinocchio::displayConfig(c));
739
740
            }
            if(rep.success_){
741
                hppDout(notice,"Projection successfull for state without collision check.");
742
743
                rep.success_ =  rep.success_ &&  val->validate(rep.result_.configuration_,rport);
                if(!rep.success_){
stevet's avatar
stevet committed
744
                    hppDout(notice,"Projection failed after collision check for state with config = "<<pinocchio::displayConfig(c));
745
                    hppDout(notice,"report : "<<*rport);
746
747
                }else{
                    hppDout(notice,"Success after collision check");
748
749
                }
            }
750
751
            if (! rep.success_ && maxNumeSamples>0)
            {
752
                hppDout(notice,"Projection for state failed, try to randomly sample other initial point : ");
753
                Configuration_t head = s.configuration_.head<7>();
754
755
                size_t ecs_size = fullBody()->device_->extraConfigSpace().dimension ();
                Configuration_t ecs = s.configuration_.tail(ecs_size);
Pierre Fernbach's avatar
Pierre Fernbach committed
756
                core::configurationShooter::UniformPtr_t shooter = core::configurationShooter::Uniform::create(fullBody()->device_);
757
758
                for(std::size_t i =0; !rep.success_ && i< maxNumeSamples; ++i)
                {
759
                    shooter->shoot(s.configuration_);
760
                    s.configuration_.head<7>() = head;
761
                    s.configuration_.tail(ecs_size) = ecs;
Steve Tonneau's avatar
Steve Tonneau committed
762
                    //c = rbprm::interpolation::projectOnCom(fullBody(), problemSolver()->problem(),s,com_target,succes);
stevet's avatar
stevet committed
763
                    hppDout(notice,"Sample before prjection : r(["<<pinocchio::displayConfig(s.configuration_)<<"])");
Steve Tonneau's avatar
Steve Tonneau committed
764
                    rep = rbprm::projection::projectToComPosition(fullBody(),com_target,s);
stevet's avatar
stevet committed
765
                    hppDout(notice,"Sample after prjection : r(["<<pinocchio::displayConfig(rep.result_.configuration_)<<"])");
766
                    if(!rep.success_){
767
                        hppDout(notice,"Projection failed on iter "<<i);
768
769
                    }
                    if(rep.success_){
770
                        hppDout(notice,"Projection successfull on iter "<<i<<" without collision check.");
771
772
                        rep.success_ =  rep.success_ &&  val->validate(rep.result_.configuration_,rport);
                        if(!rep.success_){
773
                            hppDout(notice,"Projection failed on iter "<<i<<" after collision check");
774
                            hppDout(notice,"report : "<<*rport);
775
776
                        }else{
                            hppDout(notice,"Success after collision check");
777
778
                        }
                    }
779
780
781
782
                    c = rep.result_.configuration_;
                }
            }
            if(rep.success_)
783
            {
stevet's avatar
stevet committed
784
785
                hppDout(notice,"Valid configuration found after projection : r(["<<pinocchio::displayConfig(c)<<"])");
                hpp::pinocchio::Configuration_t trySave = c;
786
                rbprm::T_Limb fLimbs = GetFreeLimbs(fullBody(), s);
t steve's avatar
t steve committed
787
788
789
790
791
                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
792
                    s.configuration_.segment(sample.startRank_, sample.length_) = refPose_.segment(sample.startRank_, sample.length_) ;
t steve's avatar
t steve committed
793
794
                    rep = rbprm::projection::projectToComPosition(fullBody(),com_target,s);
                    rep.success_ = rep.success_ && val->validate(rep.result_.configuration_,rport);
795
796
797
798
799
800
                    if(rep.success_)
                    {
                        trySave = rep.result_.configuration_;
                    }
                    else
                    {
stevet's avatar
stevet committed
801
                        //
802
                    }
t steve's avatar
t steve committed
803
                }
804
805
                s.configuration_ = trySave;
                hppDout(notice,"ProjectoToComEigen success");
806
807
                return 1.;
            }
stevet's avatar
stevet committed
808
            hppDout(notice,"No valid configurations can be found after projection : r(["<<pinocchio::displayConfig(c)<<"])");
809
            hppDout(notice,"ProjectoToComEigen failure");
810
811
812
813
            return 0;
        }
        catch(std::runtime_error& e)
        {
814
            std::cout << "ERREUR " << std::endl;
815
816
817
818
            throw Error(e.what());
        }
    }

Steve Tonneau's avatar
Steve Tonneau committed
819
820
    CORBA::Short RbprmBuilder::createState(const hpp::floatSeq& configuration, const hpp::Names_t& contactLimbs) throw (hpp::Error)
    {
stevet's avatar
stevet committed
821
        pinocchio::Configuration_t config = dofArrayToConfig (fullBody()->device_, configuration);
Steve Tonneau's avatar
Steve Tonneau committed
822
823
        fullBody()->device_->currentConfiguration(config);
        fullBody()->device_->computeForwardKinematics();
Steve Tonneau's avatar
Steve Tonneau committed
824
825
826
827
828
        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
829
            rbprm::RbPrmLimbPtr_t limb = fullBody()->GetLimbs().at(*cit);
Steve Tonneau's avatar
Steve Tonneau committed
830
831
            const std::string& limbName = *cit;
            state.contacts_[limbName] = true;
832
            const fcl::Vec3f position = limb->effector_.currentTransformation().translation();
Steve Tonneau's avatar
Steve Tonneau committed
833
            state.contactPositions_[limbName] = position;
834
835
            state.contactNormals_[limbName] = limb->effector_.currentTransformation().rotation() * limb->normal_;
            state.contactRotation_[limbName] = limb->effector_.currentTransformation().rotation();
836
            state.contactOrder_.push(limbName);
Steve Tonneau's avatar
Steve Tonneau committed
837
        }
838
        state.nbContacts = state.contactNormals_.size();
Steve Tonneau's avatar
Steve Tonneau committed
839
        lastStatesComputed_.push_back(state);
Steve Tonneau's avatar
Steve Tonneau committed
840
        lastStatesComputedTime_.push_back(std::make_pair(-1., state));
841
        return (CORBA::Short)(lastStatesComputed_.size()-1);
Steve Tonneau's avatar
Steve Tonneau committed
842
843
    }

844
    double RbprmBuilder::projectStateToCOM(unsigned short stateId, const hpp::floatSeq& com, unsigned short max_num_sample) throw (hpp::Error)
845
    {
stevet's avatar
stevet committed
846
        pinocchio::Configuration_t com_target = dofArrayToConfig (3, com);
847
        return projectStateToCOMEigen(stateId, com_target, max_num_sample);
848
849
    }

850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
    double RbprmBuilder::projectStateToRoot(unsigned short stateId, const hpp::floatSeq& root) throw (hpp::Error)
    {
        pinocchio::Configuration_t root_target = dofArrayToConfig (7, root);
        if(lastStatesComputed_.size() <= stateId)
        {
            throw std::runtime_error ("Unexisting state " + std::string(""+(stateId)));
        }
        State s = lastStatesComputed_[stateId];
        projection::ProjectionReport rep = projection::projectToRootConfiguration(fullBody(),root_target,s);
        double success = 0.;
        if(rep.success_){
          ValidationReportPtr_t rport (ValidationReportPtr_t(new CollisionValidationReport));
          CollisionValidationPtr_t val = fullBody()->GetCollisionValidation();
          if(val->validate(rep.result_.configuration_,rport)){
            lastStatesComputed_[stateId] = rep.result_;
            lastStatesComputedTime_[stateId].second = rep.result_;
            success = 1.;
          }
        }
        return success;
    }

872

873
    rbprm::State RbprmBuilder::generateContacts_internal(const hpp::floatSeq& configuration,
874
      const hpp::floatSeq& direction,const hpp::floatSeq& acceleration, const double robustnessThreshold ) throw (hpp::Error)
Steve Tonneau's avatar
Steve Tonneau committed
875
876
877
878
879
    {
        if(!fullBodyLoaded_)
            throw Error ("No full body robot was loaded");
        try
        {
880
            fcl::Vec3f dir,acc;
Steve Tonneau's avatar
Steve Tonneau committed
881
882
            for(std::size_t i =0; i <3; ++i)
            {
883
                dir[i] = direction[(_CORBA_ULong)i];
884
                acc[i] = acceleration[(_CORBA_ULong)i];
Steve Tonneau's avatar
Steve Tonneau committed
885
            }
stevet's avatar
stevet committed
886
            dir.normalize();
887

stevet's avatar
stevet committed
888
889
            const affMap_t &affMap = problemSolver()->affordanceObjects;
                if (affMap.map.empty ()) {
890
    	        throw hpp::Error ("No affordances found. Unable to generate Contacts.");
891
              }
stevet's avatar
stevet committed
892
            pinocchio::Configuration_t config = dofArrayToConfig (fullBody()->device_, configuration);
Steve Tonneau's avatar
Steve Tonneau committed
893
            rbprm::State state = rbprm::contact::ComputeContacts(fullBody(),config,
Pierre Fernbach's avatar
Pierre Fernbach committed
894
              affMap, bindShooter_.affFilter_, dir,robustnessThreshold,acc);
895
896
897
898
899
900
901
902
903
904
905
906
            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
907
            hpp::floatSeq* dofArray = new hpp::floatSeq();
Steve Tonneau's avatar
stair    
Steve Tonneau committed
908
            dofArray->length(_CORBA_ULong(state.configuration_.rows()));
909
            for(std::size_t i=0; i< _CORBA_ULong(state.configuration_.rows()); i++)
Steve Tonneau's avatar
Steve Tonneau committed
910
911
912
913
914
915
916
              (*dofArray)[(_CORBA_ULong)i] = state.configuration_ [i];
            return dofArray;
        } catch (const std::exception& exc) {
        throw hpp::Error (exc.what ());
        }
    }

917
918
919
920
921
922
923
924
    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));
925
            return (CORBA::Short)(lastStatesComputed_.size()-1);
926
927
928
929
930
        } catch (const std::exception& exc) {
        throw hpp::Error (exc.what ());
        }
    }

931
932
933
934
935
936
937
938
    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
939
            core::configurationShooter::UniformPtr_t  shooter = core::configurationShooter::Uniform::create(fullBody()->device_);
940
            Configuration_t config;
941
942
943
            for(int i =0; i< 1000; ++i)
            {
                std::vector<std::string> names = stringConversion(contactLimbs);
944
                core::ConfigProjectorPtr_t proj = core::ConfigProjector::create(fullBody()->device_,"proj", 1e-4, 100);
945
946
947
                //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
948
                    rbprm::RbPrmLimbPtr_t limb = fullBody()->GetLimbs().at(*cit);
949
                    pinocchio::Transform3f localFrame(1), globalFrame(1);
stevet's avatar
stevet committed
950
                    localFrame.translation(-limb->offset_);
951
952
953
954
                    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);
955
                    const pinocchio::Frame effectorFrame = fullBody()->device_->getFrameByName(limb->effector_.name());
956
                    pinocchio::JointPtr_t effectorJoint= limb->effector_.joint();
Steve Tonneau's avatar
Steve Tonneau committed
957
                    proj->add(core::NumericalConstraint::create (constraints::Position::create("",fullBody()->device_,
958
                                                                                               effectorJoint,
959
                                                                                               effectorFrame.pinocchio().placement * globalFrame,
960
961
962
963
964
965
966
                                                                                               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
967
                        // TODO not assume identity matrix for effector frame
968
                        fcl::Matrix3f rotation = r * effectorFrame.pinocchio().placement.rotation().transpose();// * limb->effector_->initialPosition ().getRotation();
Steve Tonneau's avatar
Steve Tonneau committed
969
                        proj->add(core::NumericalConstraint::create (constraints::Orientation::create("",fullBody()->device_,
970
                                                                                                      effectorJoint,
stevet's avatar
stevet committed
971
                                                                                                      pinocchio::Transform3f(rotation,fcl::Vec3f::Zero()),
972
973
974
                                                                                                      rotConstraints)));
                    }
                }
975
                shooter->shoot (config);
976
977
                if(proj->apply(config) && config[2]> 0.3)
                {
978
                    if(problemSolver()->problem()->configValidations()->validate(config,report))
979
980
981
982
983
                    {
                        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
984
                            rbprm::RbPrmLimbPtr_t limb = fullBody()->GetLimbs().at(*cit);
985
                            tmp.contacts_[limbId] = true;
986
987
                            tmp.contactPositions_[limbId] = limb->effector_.currentTransformation().translation();
                            tmp.contactRotation_[limbId] = limb->effector_.currentTransformation().rotation();
988
989
990
991
                            tmp.contactNormals_[limbId] = z;
                            tmp.configuration_ = config;
                            ++tmp.nbContacts;
                        }
Steve Tonneau's avatar
Steve Tonneau committed
992
                        if(stability::IsStable(fullBody(),tmp)>=0)
993
994
995
996
997
                        {
                            config[0]=0;
                            config[1]=0;
                            hpp::floatSeq* dofArray = new hpp::floatSeq();
                            dofArray->length(_CORBA_ULong(config.rows()));
998
                            for(size_type j=0; j< config.rows(); j++)
999
1000
1001
1002
1003
1004
1005
1006
1007
1008
1009
1010
1011
1012
                            {
                              (*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 ());
        }
    }

1013
1014
1015
1016
1017
1018
1019
1020
    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
        {
1021
            fcl::Vec3f dir;
1022
1023
            for(std::size_t i =0; i <3; ++i)
            {
1024
                dir[i] = direction[(_CORBA_ULong)i];
1025
            }
stevet's avatar
stevet committed
1026
1027
            pinocchio::Configuration_t config = dofArrayToConfig (fullBody()->device_, configuration);
            pinocchio::Configuration_t save = fullBody()->device_->currentConfiguration();
Steve Tonneau's avatar
Steve Tonneau committed
1028
            fullBody()->device_->currentConfiguration(config);
1029
1030

            sampling::T_OctreeReport finalSet;
Steve Tonneau's avatar
Steve Tonneau committed
1031
1032
            rbprm::T_Limb::const_iterator lit = fullBody()->GetLimbs().find(std::string(limbname));
            if(lit == fullBody()->GetLimbs().end())
1033
1034
            {
                throw std::runtime_error ("Impossible to find limb for joint "
Steve Tonneau's avatar
Steve Tonneau committed
1035
                                          + std::string(limbname) + " to robot; limb not defined.");
1036
1037
            }