rbprmbuilder.impl.cc

// 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>
#include <hpp/corbaserver/rbprm/rbprmbuilder.hh>
#include "rbprmbuilder.impl.hh"
#include "hpp/rbprm/rbprm-device.hh"
#include "hpp/rbprm/rbprm-validation.hh"
#include "hpp/rbprm/interpolation/rbprm-path-interpolation.hh"
#include "hpp/rbprm/interpolation/limb-rrt.hh"
#include "hpp/rbprm/interpolation/com-rrt.hh"
#include "hpp/rbprm/interpolation/com-trajectory.hh"
#include "hpp/rbprm/interpolation/spline/effector-rrt.hh"
#include "hpp/rbprm/projection/projection.hh"
#include "hpp/rbprm/contact_generation/contact_generation.hh"
#include "hpp/rbprm/contact_generation/algorithm.hh"
#include "hpp/rbprm/stability/stability.hh"
#include "hpp/rbprm/sampling/sample-db.hh"
#include "hpp/core/straight-path.hh"
#include "hpp/core/config-validations.hh"
#include "hpp/core/collision-validation-report.hh"
#include <hpp/core/subchain-path.hh>
#include <hpp/core/configuration-shooter/uniform.hh>
#include <hpp/core/collision-validation.hh>
#include <hpp/core/problem-solver.hh>
#include <fstream>
#include <hpp/rbprm/planner/dynamic-planner.hh>
#include <hpp/rbprm/planner/rbprm-steering-kinodynamic.hh>
#include <algorithm>    // std::random_shuffle
#include <hpp/rbprm/interpolation/time-constraint-helper.hh>
#include <hpp/spline/bezier_curve.h>
#include <hpp/rbprm/interpolation/polynom-trajectory.hh>
#include <hpp/rbprm/planner/random-shortcut-dynamic.hh>
#include <hpp/rbprm/planner/oriented-path-optimizer.hh>
#include <hpp/rbprm/sampling/heuristic-tools.hh>
#include <hpp/rbprm/contact_generation/reachability.hh>
#include <hpp/pinocchio/urdf/util.hh>
#ifdef PROFILE
    #include "hpp/rbprm/rbprm-profiler.hh"
#endif



namespace hpp {
  namespace rbprm {

  typedef spline::bezier_curve<> bezier;
    namespace impl {

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

    RbprmBuilder::RbprmBuilder ()
    : POA_hpp::corbaserver::rbprm::RbprmBuilder()
    , romLoaded_(false)
    , fullBodyLoaded_(false)
    , bindShooter_()
    , analysisFactory_(0)
    {
        // NOTHING
    }


    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;
    }

    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
        {
            hpp::pinocchio::DevicePtr_t romDevice = pinocchio::Device::create (robotName);
            romDevices_.insert(std::make_pair(robotName, romDevice));
            hpp::pinocchio::urdf::loadRobotModel (romDevice,
                              std::string (rootJointType),
                              std::string (packageName),
                              std::string (modelName),
                              std::string (urdfSuffix),
                              std::string (srdfSuffix));
        }
        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") ;
            hppDout (error, err );
            throw hpp::Error(err.c_str());
        }
        try
        {
            hpp::pinocchio::RbPrmDevicePtr_t device = hpp::pinocchio::RbPrmDevice::create (robotName, romDevices_);
            hpp::pinocchio::urdf::loadRobotModel (device,
                    std::string (rootJointType),
                    std::string (packageName),
                    std::string (modelName),
                    std::string (urdfSuffix),
                    std::string (srdfSuffix));
            // Add device to the planner
            problemSolver()->robot (device);
            problemSolver()->robot ()->controlComputation
            (flag);
        }
        catch (const std::exception& exc)
        {
            hppDout (error, exc.what ());
            throw hpp::Error (exc.what ());
        }
    }

    void RbprmBuilder::loadFullBodyRobot(const char* robotName,
         const char* rootJointType,
         const char* packageName,
         const char* modelName,
         const char* urdfSuffix,
         const char* srdfSuffix,
         const char* selectedProblem) throw (hpp::Error)
    {
        try
        {
            hpp::pinocchio::DevicePtr_t device = pinocchio::Device::create (robotName);
            hpp::pinocchio::urdf::loadRobotModel (device,
                    std::string (rootJointType),
                    std::string (packageName),
                    std::string (modelName),
                    std::string (urdfSuffix),
                    std::string (srdfSuffix));
            std::string name(selectedProblem);
            fullBodyLoaded_ = true;
            fullBodyMap_.map_[name] = rbprm::RbPrmFullBody::create(device);
            fullBodyMap_.selected_ = name;
            if(problemSolver()){
                if(problemSolver()->problem()){
                    try {
                      double mu = problemSolver()->problem()->getParameter ("friction").floatValue();
                      fullBody()->setFriction(mu);
                      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");
            }
            problemSolver()->pathValidationType ("Discretized",0.05); // reset to avoid conflict with rbprm path
            problemSolver()->robot (fullBody()->device_);
            problemSolver()->resetProblem();
            problemSolver()->robot ()->controlComputation
            (flag);
            refPose_ = fullBody()->device_->currentConfiguration();
        }
        catch (const std::exception& exc)
        {
            fullBodyLoaded_ = false;
            hppDout (error, exc.what ());
            throw hpp::Error (exc.what ());
        }
        analysisFactory_ = new sampling::AnalysisFactory(fullBody());
    }

    void RbprmBuilder::loadFullBodyRobotFromExistingRobot() throw (hpp::Error)
    {
        try
        {
            fullBody() = rbprm::RbPrmFullBody::create(problemSolver()->problem()->robot());
            problemSolver()->pathValidationType ("Discretized",0.05); // reset to avoid conflict with rbprm path
            problemSolver()->robot (fullBody()->device_);
            problemSolver()->resetProblem();
            problemSolver()->robot ()->controlComputation
            (flag);
        }
        catch (const std::exception& exc)
        {
            hppDout (error, exc.what ());
            throw hpp::Error (exc.what ());
        }
        fullBodyLoaded_ = true;
        analysisFactory_ = new sampling::AnalysisFactory(fullBody());
    }

    hpp::floatSeq* RbprmBuilder::getSampleConfig(const char* limb, unsigned int sampleId) throw (hpp::Error)
    {
        if(!fullBodyLoaded_)
            throw Error ("No full body robot was loaded");
        const T_Limb& limbs = fullBody()->GetLimbs();
        T_Limb::const_iterator lit = limbs.find(std::string(limb));
        if(lit == limbs.end())
        {
            std::string err("No limb " + std::string(limb) + "was defined for robot" + fullBody()->device_->name());
            throw Error (err.c_str());
        }
        const RbPrmLimbPtr_t& limbPtr = lit->second;
        hpp::floatSeq *dofArray;
        Eigen::VectorXd config = fullBody()->device_->currentConfiguration ();
        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];
        config.segment(sample.startRank_, sample.length_) = sample.configuration_;
        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;
    }


    hpp::floatSeq* RbprmBuilder::getSamplePosition(const char* limb, unsigned int sampleId) throw (hpp::Error)
    {
        if(!fullBodyLoaded_)
            throw Error ("No full body robot was loaded");
        const T_Limb& limbs = fullBody()->GetLimbs();
        T_Limb::const_iterator lit = limbs.find(std::string(limb));
        if(lit == limbs.end())
        {
            std::string err("No limb " + std::string(limb) + "was defined for robot" + fullBody()->device_->name());
            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;
    }


    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,
                                                    const rbprm::State& state, const std::vector<std::string> limbSelected = std::vector<std::string>())
    {
        device->device_->currentConfiguration(state.configuration_);
        device->device_->computeForwardKinematics();
        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;
            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);
                const fcl::Vec3f z = limb->effector_.currentTransformation().rotation() * limb->normal_;
                const fcl::Matrix3f alignRotation = tools::GetRotationMatrix(z,normal);
                const fcl::Matrix3f rotation = alignRotation * limb->effector_.currentTransformation().rotation();
                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)
                {
                    //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));
                }
                else
                {
                    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));
                    }
                }
            }
        }
        return res;
    }

    std::vector<fcl::Vec3f> computeRectangleContactLocalTr(const rbprm::RbPrmFullBodyPtr_t device,
                                                    const rbprm::State& state,
                                                    const std::string& limbName)
    {
        device->device_->currentConfiguration(state.configuration_);
        device->device_->computeForwardKinematics();
        std::vector<fcl::Vec3f> res;
        const rbprm::T_Limb& limbs = device->GetLimbs();
        rbprm::RbPrmLimbPtr_t limb;
        Matrix43 p; Eigen::Matrix3d cFrame = Eigen::Matrix3d::Identity();
        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);
                const fcl::Vec3f& normal = state.contactNormals_.at(name);
                const fcl::Vec3f z = limb->effector_.currentTransformation().rotation() * limb->normal_;
                const fcl::Matrix3f alignRotation = tools::GetRotationMatrix(z,normal);
                const fcl::Matrix3f rotation = alignRotation * limb->effector_.currentTransformation().rotation();
                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)
                {
                    //create rotation matrix from normal
                    Eigen::Vector3d z,x,y;
                    for(int i =0; i<3; ++i) z[i] = normal[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);
                    }
                    cFrame.block<3,1>(0,0) = x;
                    cFrame.block<3,1>(0,1) = y;
                    cFrame.block<3,1>(0,2) = z;
                }
                fcl::Transform3f roWorld, roEffector;
                roWorld.setRotation(state.contactRotation_.at(name));
                roWorld.setTranslation(position);
                roEffector = roWorld; roEffector.inverse();
                fcl::Vec3f z_axis(0,0,1);
                fcl::Matrix3f rotationLocal = tools::GetRotationMatrix(z_axis, limb->normal_);
                if(limb->contactType_ == _3_DOF)
                {
                    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)
                    {
                        /*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_);
                            //res.push_back(roEffector.getRotation() * state.contactNormals_.at(name));
                           res.push_back( limb->normal_);
                        }
                    }
                }
                return res;
            }
        }
        return res;
    }

    pinocchio::Configuration_t dofArrayToConfig (const std::size_t& deviceDim,
      const hpp::floatSeq& dofArray)
    {
        std::size_t configDim = (std::size_t)dofArray.length();
        // Fill dof vector with dof array.
        pinocchio::Configuration_t config; config.resize (configDim);
        for (std::size_t iDof = 0; iDof < configDim; iDof++) {
            config [iDof] = (double)dofArray[(_CORBA_ULong)iDof];
        }
        // fill the vector by zero
        //hppDout (info, "config dimension: " <<configDim <<",  deviceDim "<<deviceDim);
        if(configDim != deviceDim){
            throw hpp::Error ("dofVector Does not match");
        }
        return config;
    }

    pinocchio::Configuration_t dofArrayToConfig (const pinocchio::DevicePtr_t& robot,
      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;
    }

    T_Configuration doubleDofArrayToConfig (const pinocchio::DevicePtr_t& robot,
      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);
            const RbPrmLimbPtr_t limb = fullBody()->GetLimbs().at(limbName);
            pinocchio::Configuration_t config = dofArrayToConfig (fullBody()->device_, configuration);
            fullBody()->device_->currentConfiguration(config);
            fullBody()->device_->computeForwardKinematics();
            State state;
            state.configuration_ = config;
            state.contacts_[limbName] = true;
            const fcl::Vec3f position = limb->effector_.currentTransformation().translation();
            state.contactPositions_[limbName] = position;
            state.contactNormals_[limbName] = fcl::Vec3f(0,0,1);
            state.contactRotation_[limbName] = limb->effector_.currentTransformation().rotation();
            std::vector<fcl::Vec3f> poss = (computeRectangleContact(fullBody(), state));

            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 ());
        }
    }


    CORBA::UShort RbprmBuilder::getNumSamples(const char* limb) throw (hpp::Error)
    {
        const T_Limb& limbs = fullBody()->GetLimbs();
        T_Limb::const_iterator lit = limbs.find(std::string(limb));
        if(lit == limbs.end())
        {
            std::string err("No limb " + std::string(limb) + "was defined for robot" + fullBody()->device_->name());
            throw Error (err.c_str());
        }
        return (CORBA::UShort)(lit->second->sampleContainer_.samples_.size());
    }

    floatSeq *RbprmBuilder::getOctreeNodeIds(const char* limb) throw (hpp::Error)
    {
        const T_Limb& limbs = fullBody()->GetLimbs();
        T_Limb::const_iterator lit = limbs.find(std::string(limb));
        if(lit == limbs.end())
        {
            std::string err("No limb " + std::string(limb) + "was defined for robot" + fullBody()->device_->name());
            throw Error (err.c_str());
        }
        const sampling::T_VoxelSampleId& ids =  lit->second->sampleContainer_.samplesInVoxels_;
        hpp::floatSeq* dofArray = new hpp::floatSeq();
        dofArray->length((_CORBA_ULong)ids.size());
        sampling::T_VoxelSampleId::const_iterator it = ids.begin();
        for(std::size_t i=0; i< _CORBA_ULong(ids.size()); ++i, ++it)
        {
          (*dofArray)[(_CORBA_ULong)i] = (double)it->first;
        }
        return dofArray;
    }

    double RbprmBuilder::getSampleValue(const char* limb, const char* valueName, unsigned int sampleId) throw (hpp::Error)
    {
        const T_Limb& limbs = fullBody()->GetLimbs();
        T_Limb::const_iterator lit = limbs.find(std::string(limb));
        if(lit == limbs.end())
        {
            std::string err("No limb " + std::string(limb) + "was defined for robot" + fullBody()->device_->name());
            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];
    }


    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());
        }
    }

    std::vector<std::string> stringConversion(const hpp::Names_t& dofArray)
    {
        std::vector<std::string> res;
        std::size_t dim = (std::size_t)dofArray.length();
        for (_CORBA_ULong iDof = 0; iDof < dim; iDof++)
        {
            res.push_back(std::string(dofArray[iDof]));
        }
        return res;
    }

    std::vector<double> doubleConversion(const hpp::floatSeq& dofArray)
    {
        std::vector<double> res;
        std::size_t dim = (std::size_t)dofArray.length();
        for (_CORBA_ULong iDof = 0; iDof < dim; iDof++)
        {
            res.push_back(dofArray[iDof]);
        }
        return res;
    }

    void RbprmBuilder::setStaticStability(const bool staticStability) throw (hpp::Error){
      if(!fullBodyLoaded_)
        throw Error ("No full body robot was loaded");
      fullBody()->staticStability(staticStability);
    }

    void RbprmBuilder::setReferenceConfig(const hpp::floatSeq& referenceConfig) throw (hpp::Error){
      if(!fullBodyLoaded_)
        throw Error ("No full body robot was loaded");
      Configuration_t config(dofArrayToConfig (fullBody()->device_, referenceConfig));
      refPose_ = config;
      fullBody()->referenceConfig(config);
    }

    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);
    }



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


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

    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;

    }

    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;
    }

    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;
    }

    double RbprmBuilder::projectStateToCOMEigen(unsigned short stateId, const pinocchio::Configuration_t& com_target, unsigned short maxNumeSamples) throw (hpp::Error){
        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;
    }

    double RbprmBuilder::projectStateToCOMEigen(State& s, const pinocchio::Configuration_t& com_target, unsigned short maxNumeSamples) throw (hpp::Error)
    {
        try
        {
//            /hpp::pinocchio::Configuration_t c = rbprm::interpolation::projectOnCom(fullBody(), problemSolver()->problem(),s,com_target,succes);
            hppDout(notice,"ProjectStateToComEigen, init config in state : "<<pinocchio::displayConfig(s.configuration_));
            rbprm::projection::ProjectionReport rep = rbprm::projection::projectToComPosition(fullBody(),com_target,s);
            hpp::pinocchio::Configuration_t& c = rep.result_.configuration_;
            ValidationReportPtr_t rport (ValidationReportPtr_t(new CollisionValidationReport));
            CollisionValidationPtr_t val = fullBody()->GetCollisionValidation();
            if(!rep.success_){
                hppDout(notice,"Projection failed for state with config = "<<pinocchio::displayConfig(c));
            }
            if(rep.success_){
                hppDout(notice,"Projection successfull for state without collision check.");
                rep.success_ =  rep.success_ &&  val->validate(rep.result_.configuration_,rport);
                if(!rep.success_){
                    hppDout(notice,"Projection failed after collision check for state with config = "<<pinocchio::displayConfig(c));
                    hppDout(notice,"report : "<<*rport);
                }else{
                    hppDout(notice,"Success after collision check");
                }
            }
            if (! rep.success_ && maxNumeSamples>0)
            {
                hppDout(notice,"Projection for state failed, try to randomly sample other initial point : ");
                Configuration_t head = s.configuration_.head<7>();
                size_t ecs_size = fullBody()->device_->extraConfigSpace().dimension ();
                Configuration_t ecs = s.configuration_.tail(ecs_size);
                core::configurationShooter::UniformPtr_t shooter = core::configurationShooter::Uniform::create(fullBody()->device_);
                for(std::size_t i =0; !rep.success_ && i< maxNumeSamples; ++i)
                {
                    s.configuration_ = *shooter->shoot();
                    s.configuration_.head<7>() = head;
                    s.configuration_.tail(ecs_size) = ecs;
                    //c = rbprm::interpolation::projectOnCom(fullBody(), problemSolver()->problem(),s,com_target,succes);
                    hppDout(notice,"Sample before prjection : r(["<<pinocchio::displayConfig(s.configuration_)<<"])");
                    rep = rbprm::projection::projectToComPosition(fullBody(),com_target,s);
                    hppDout(notice,"Sample after prjection : r(["<<pinocchio::displayConfig(rep.result_.configuration_)<<"])");
                    if(!rep.success_){
                        hppDout(notice,"Projection failed on iter "<<i);
                    }
                    if(rep.success_){
                        hppDout(notice,"Projection successfull on iter "<<i<<" without collision check.");
                        rep.success_ =  rep.success_ &&  val->validate(rep.result_.configuration_,rport);
                        if(!rep.success_){
                            hppDout(notice,"Projection failed on iter "<<i<<" after collision check");
                            hppDout(notice,"report : "<<*rport);
                        }else{
                            hppDout(notice,"Success after collision check");
                        }
                    }
                    c = rep.result_.configuration_;
                }
            }
            if(rep.success_)
            {
                hppDout(notice,"Valid configuration found after projection : r(["<<pinocchio::displayConfig(c)<<"])");
                hpp::pinocchio::Configuration_t trySave = c;
                rbprm::T_Limb fLimbs = GetFreeLimbs(fullBody(), s);
                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];
                    s.configuration_.segment(sample.startRank_, sample.length_) = refPose_.segment(sample.startRank_, sample.length_) ;
                    rep = rbprm::projection::projectToComPosition(fullBody(),com_target,s);
                    rep.success_ = rep.success_ && val->validate(rep.result_.configuration_,rport);
                    if(rep.success_)
                    {
                        trySave = rep.result_.configuration_;
                    }
                    else
                    {
                        //
                    }
                }
                s.configuration_ = trySave;
                hppDout(notice,"ProjectoToComEigen success");
                return 1.;
            }
            hppDout(notice,"No valid configurations can be found after projection : r(["<<pinocchio::displayConfig(c)<<"])");
            hppDout(notice,"ProjectoToComEigen failure");
            return 0;
        }
        catch(std::runtime_error& e)
        {
            std::cout << "ERREUR " << std::endl;
            throw Error(e.what());
        }
    }

    CORBA::Short RbprmBuilder::createState(const hpp::floatSeq& configuration, const hpp::Names_t& contactLimbs) throw (hpp::Error)
    {
        pinocchio::Configuration_t config = dofArrayToConfig (fullBody()->device_, configuration);
        fullBody()->device_->currentConfiguration(config);
        fullBody()->device_->computeForwardKinematics();
        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)
        {
            rbprm::RbPrmLimbPtr_t limb = fullBody()->GetLimbs().at(*cit);
            const std::string& limbName = *cit;
            state.contacts_[limbName] = true;
            const fcl::Vec3f position = limb->effector_.currentTransformation().translation();
            state.contactPositions_[limbName] = position;
            state.contactNormals_[limbName] = limb->effector_.currentTransformation().rotation() * limb->normal_;
            state.contactRotation_[limbName] = limb->effector_.currentTransformation().rotation();
            state.contactOrder_.push(limbName);
        }
        state.nbContacts = state.contactNormals_.size();
        lastStatesComputed_.push_back(state);
        lastStatesComputedTime_.push_back(std::make_pair(-1., state));
        return (CORBA::Short)(lastStatesComputed_.size()-1);
    }

    double RbprmBuilder::projectStateToCOM(unsigned short stateId, const hpp::floatSeq& com, unsigned short max_num_sample) throw (hpp::Error)
    {
        pinocchio::Configuration_t com_target = dofArrayToConfig (3, com);
        return projectStateToCOMEigen(stateId, com_target, max_num_sample);
    }


    rbprm::State RbprmBuilder::generateContacts_internal(const hpp::floatSeq& configuration,
      const hpp::floatSeq& direction,const hpp::floatSeq& acceleration, const double robustnessThreshold ) throw (hpp::Error)
    {
        if(!fullBodyLoaded_)
            throw Error ("No full body robot was loaded");
        try
        {
            fcl::Vec3f dir,acc;
            for(std::size_t i =0; i <3; ++i)
            {
                dir[i] = direction[(_CORBA_ULong)i];
                acc[i] = acceleration[(_CORBA_ULong)i];
            }
            dir.normalize();

            const affMap_t &affMap = problemSolver()->affordanceObjects;
                if (affMap.map.empty ()) {
    	        throw hpp::Error ("No affordances found. Unable to generate Contacts.");
              }
            pinocchio::Configuration_t config = dofArrayToConfig (fullBody()->device_, configuration);
            rbprm::State state = rbprm::contact::ComputeContacts(fullBody(),config,
              affMap, bindShooter_.affFilter_, dir,robustnessThreshold,acc);
            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);
            hpp::floatSeq* dofArray = new hpp::floatSeq();
            dofArray->length(_CORBA_ULong(state.configuration_.rows()));
            for(std::size_t i=0; i< _CORBA_ULong(state.configuration_.rows()); i++)
              (*dofArray)[(_CORBA_ULong)i] = state.configuration_ [i];
            return dofArray;
        } catch (const std::exception& exc) {
        throw hpp::Error (exc.what ());
        }
    }

    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));
            return (CORBA::Short)(lastStatesComputed_.size()-1);
        } catch (const std::exception& exc) {
        throw hpp::Error (exc.what ());
        }
    }

    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);
            core::configurationShooter::UniformPtr_t  shooter = core::configurationShooter::Uniform::create(fullBody()->device_);
            for(int i =0; i< 1000; ++i)
            {
                core::DevicePtr_t device = fullBody()->device_->clone();
                std::vector<std::string> names = stringConversion(contactLimbs);
                core::ConfigProjectorPtr_t proj = core::ConfigProjector::create(device,"proj", 1e-4, 100);
                //hpp::tools::LockJointRec(limb->limb_->name(), body->device_->rootJoint(), proj);
                for(std::vector<std::string>::const_iterator cit = names.begin(); cit !=names.end(); ++cit)
                {
                    rbprm::RbPrmLimbPtr_t limb = fullBody()->GetLimbs().at(*cit);
                    pinocchio::Transform3f localFrame(1), globalFrame(1);
                    localFrame.translation(-limb->offset_);
                    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);
                    const pinocchio::Frame effectorFrame = device->getFrameByName(limb->effector_.name());
                    pinocchio::JointPtr_t effectorJoint= limb->effector_.joint();
                    proj->add(core::NumericalConstraint::create (constraints::Position::create("",fullBody()->device_,
                                                                                               effectorJoint,
                                                                                               effectorFrame.pinocchio().placement * globalFrame,
                                                                                               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);
                        // TODO not assume identity matrix for effector frame
                        fcl::Matrix3f rotation = r * effectorFrame.pinocchio().placement.rotation().transpose();// * limb->effector_->initialPosition ().getRotation();
                        proj->add(core::NumericalConstraint::create (constraints::Orientation::create("",fullBody()->device_,
                                                                                                      effectorJoint,
                                                                                                      pinocchio::Transform3f(rotation,fcl::Vec3f::Zero()),
                                                                                                      rotConstraints)));
                    }
                }
                ConfigurationPtr_t configptr = shooter->shoot();
                Configuration_t config = *configptr;
                if(proj->apply(config) && config[2]> 0.3)
                {
                    if(problemSolver()->problem()->configValidations()->validate(config,report))
                    {
                        State tmp;
                        for(std::vector<std::string>::const_iterator cit = names.begin(); cit !=names.end(); ++cit)
                        {
                            std::string limbId = *cit;
                            rbprm::RbPrmLimbPtr_t limb = fullBody()->GetLimbs().at(*cit);
                            tmp.contacts_[limbId] = true;
                            tmp.contactPositions_[limbId] = limb->effector_.currentTransformation().translation();
                            tmp.contactRotation_[limbId] = limb->effector_.currentTransformation().rotation();
                            tmp.contactNormals_[limbId] = z;
                            tmp.configuration_ = config;
                            ++tmp.nbContacts;
                        }
                        if(stability::IsStable(fullBody(),tmp)>=0)
                        {
                            config[0]=0;
                            config[1]=0;
                            hpp::floatSeq* dofArray = new hpp::floatSeq();
                            dofArray->length(_CORBA_ULong(config.rows()));
                            for(size_type j=0; j< config.rows(); j++)
                            {
                              (*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 ());
        }
    }

    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
        {
            fcl::Vec3f dir;
            for(std::size_t i =0; i <3; ++i)
            {
                dir[i] = direction[(_CORBA_ULong)i];
            }
            pinocchio::Configuration_t config = dofArrayToConfig (fullBody()->device_, configuration);
            pinocchio::Configuration_t save = fullBody()->device_->currentConfiguration();
            fullBody()->device_->currentConfiguration(config);

            sampling::T_OctreeReport finalSet;
            rbprm::T_Limb::const_iterator lit = fullBody()->GetLimbs().find(std::string(limbname));
            if(lit == fullBody()->GetLimbs().end())
            {
                throw std::runtime_error ("Impossible to find limb for joint "
                                          + std::string(limbname) + " to robot; limb not defined.");
            }
            const RbPrmLimbPtr_t& limb = lit->second;
            pinocchio::Transform3f transformPin = limb->limb_->robot()->rootJoint()->childJoint(0)->currentTransformation (); // get root transform from configuration
            fcl::Transform3f transform(transformPin.rotation(), transformPin.translation());
                        // TODO fix as in rbprm-fullbody.cc!!
            std::vector<sampling::T_OctreeReport> reports(problemSolver()->collisionObstacles().size());
            std::size_t i (0);
            //#pragma omp parallel for
            for(core::ObjectStdVector_t::const_iterator oit = problemSolver()->collisionObstacles().begin();
                oit != problemSolver()->collisionObstacles().end(); ++oit, ++i)
            {
                sampling::GetCandidates(limb->sampleContainer_, transform, *oit, dir, reports[i], sampling::HeuristicParam());