From 061c26b38984a218447e658df89d679d4b3a6ba6 Mon Sep 17 00:00:00 2001
From: Pierre Fernbach <pierre.fernbach@laas.fr>
Date: Wed, 14 Dec 2016 15:01:43 +0100
Subject: [PATCH] propagate acceleration through 'computeContact'
---
idl/hpp/corbaserver/rbprm/rbprmbuilder.idl | 2 +-
src/hpp/corbaserver/rbprm/rbprmfullbody.py | 4 ++--
src/rbprmbuilder.impl.cc | 10 +++++-----
src/rbprmbuilder.impl.hh | 2 +-
4 files changed, 9 insertions(+), 9 deletions(-)
diff --git a/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl b/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
index 329e3dc3..978bd578 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 67e524f0..2bdb61d6 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 d2ac240b..8ca1b6cd 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 38c225d2..a12ab7fe 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);
--
GitLab