diff --git a/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl b/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl index 329e3dc32a69d1309d8f732dbda7296b47673482..978bd5780dbaec9a4ba0aad792ab9aecbceeaa75 100755 --- a/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl +++ b/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl @@ -180,7 +180,7 @@ module hpp /// \param dofArray initial configuration of the robot /// \param direction desired direction of motion for the robot /// \return transformed configuration for which all possible contacts have been created - floatSeq generateContacts(in floatSeq dofArray, in floatSeq direction) raises (Error); + floatSeq generateContacts(in floatSeq dofArray, in floatSeq direction, in floatSeq acceleration) raises (Error); /// Generate an autocollision free configuration with limbs in contact with the ground /// \param contactLimbs name of the limbs to project in contact diff --git a/src/hpp/corbaserver/rbprm/rbprmfullbody.py b/src/hpp/corbaserver/rbprm/rbprmfullbody.py index 67e524f044edd77f5da051764a2290e498d066b0..2bdb61d6833ee5ad1c22c502567d70609428b783 100755 --- a/src/hpp/corbaserver/rbprm/rbprmfullbody.py +++ b/src/hpp/corbaserver/rbprm/rbprmfullbody.py @@ -201,8 +201,8 @@ class FullBody (object): # # \param configuration the initial robot configuration # \param direction a 3d vector specifying the desired direction of motion - def generateContacts(self, configuration, direction): - return self.client.rbprm.rbprm.generateContacts(configuration, direction) + def generateContacts(self, configuration, direction,acceleration = [0,0,0]): + return self.client.rbprm.rbprm.generateContacts(configuration, direction, acceleration) ## Generate an autocollision free configuration with limbs in contact with the ground # \param contactLimbs name of the limbs to project in contact diff --git a/src/rbprmbuilder.impl.cc b/src/rbprmbuilder.impl.cc index d2ac240b3c904be45d82d7a645bb14e6be3c8e2f..8ca1b6cdb75f93780b2e26d15714d8da615eeaec 100755 --- a/src/rbprmbuilder.impl.cc +++ b/src/rbprmbuilder.impl.cc @@ -38,7 +38,6 @@ #include <hpp/core/collision-validation.hh> #include <fstream> #include <hpp/rbprm/planner/dynamic-planner.hh> -#include <hpp/rbprm/planner/parabola-planner.hh> #include <hpp/rbprm/planner/rbprm-steering-kinodynamic.hh> #ifdef PROFILE @@ -651,16 +650,17 @@ namespace hpp { } hpp::floatSeq* RbprmBuilder::generateContacts(const hpp::floatSeq& configuration, - const hpp::floatSeq& direction) throw (hpp::Error) + const hpp::floatSeq& direction,const hpp::floatSeq& acceleration) throw (hpp::Error) { if(!fullBodyLoaded_) throw Error ("No full body robot was loaded"); try { - fcl::Vec3f dir; + 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]; } const affMap_t &affMap = problemSolver_->map <std::vector<boost::shared_ptr<model::CollisionObject> > > (); @@ -668,8 +668,9 @@ namespace hpp { throw hpp::Error ("No affordances found. Unable to generate Contacts."); } model::Configuration_t config = dofArrayToConfig (fullBody_->device_, configuration); + rbprm::State state = rbprm::ComputeContacts(fullBody_,config, - affMap, bindShooter_.affFilter_, dir); + affMap, bindShooter_.affFilter_, dir,0,acc); hpp::floatSeq* dofArray = new hpp::floatSeq(); dofArray->length(_CORBA_ULong(state.configuration_.rows())); for(std::size_t i=0; i< _CORBA_ULong(config.rows()); i++) @@ -2085,7 +2086,6 @@ assert(s2 == s1 +1); problemSolver->add<core::PathValidationBuilder_t>("RbprmPathValidation", boost::bind(&BindShooter::createPathValidation, boost::ref(bindShooter_), _1, _2)); problemSolver->add<core::PathPlannerBuilder_t>("DynamicPlanner",DynamicPlanner::createWithRoadmap); - problemSolver->add<core::PathPlannerBuilder_t>("ParabolaPlanner",ParabolaPlanner::createWithRoadmap); problemSolver->add <core::SteeringMethodBuilder_t> ("RBPRMKinodynamic", SteeringMethodKinodynamic::create); diff --git a/src/rbprmbuilder.impl.hh b/src/rbprmbuilder.impl.hh index 38c225d25cb7b08935d3ea4de7fc99f5e7e5b1ad..a12ab7fe4d1eec52659a8db72e15d86195ee3b0a 100755 --- a/src/rbprmbuilder.impl.hh +++ b/src/rbprmbuilder.impl.hh @@ -130,7 +130,7 @@ namespace hpp { virtual double getEffectorDistance(unsigned short state1, unsigned short state2) throw (hpp::Error); virtual hpp::floatSeq* generateContacts(const hpp::floatSeq& configuration, - const hpp::floatSeq& direction) throw (hpp::Error); + const hpp::floatSeq& direction, const hpp::floatSeq& acceleration) throw (hpp::Error); virtual hpp::floatSeq* generateGroundContact(const hpp::Names_t& contactLimbs) throw (hpp::Error);