From 6f6b37bc6ef965e54902d7303bb85fb1774508f5 Mon Sep 17 00:00:00 2001
From: Steve Tonneau <stonneau@laas.fr>
Date: Wed, 8 Jul 2015 18:09:51 +0200
Subject: [PATCH] generate contacts no collision
---
idl/hpp/corbaserver/rbprm/rbprmbuilder.idl | 2 +
script/loadfullbody.py | 30 +++++++------
src/hpp/corbaserver/rbprm/rbprmbuilder.py | 2 +-
src/hpp/corbaserver/rbprm/rbprmfullbody.py | 4 +-
src/rbprmbuilder.impl.cc | 51 +++++++++++++++++++++-
src/rbprmbuilder.impl.hh | 3 ++
6 files changed, 75 insertions(+), 17 deletions(-)
diff --git a/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl b/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
index 7abb213..c7c8840 100644
--- a/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
+++ b/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
@@ -91,6 +91,8 @@ module hpp
/// \return dofArray Array of degrees of freedom.
floatSeq getSampleConfig(in string sampleName, in unsigned short sampleId) raises (Error);
+ floatSeq generateContacts(in floatSeq dofArray, in floatSeq direction) raises (Error);
+
void addLimb(in string limb, in unsigned short samples, in double resolution) raises (Error);
}; // interface Robot
diff --git a/script/loadfullbody.py b/script/loadfullbody.py
index b3a3a78..d889949 100644
--- a/script/loadfullbody.py
+++ b/script/loadfullbody.py
@@ -2,11 +2,13 @@ from hpp.corbaserver.rbprm.rbprmbuilder import Builder
from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
from hpp.gepetto import Viewer
-rootJointType = 'freeflyer'
-packageName = 'hpp-rbprm-corba'
-meshPackageName = 'hpp-rbprm-corba'
-urdfName = 'box'
-urdfNameRom = 'box_rom'
+
+packageName = "hpp_tutorial"
+meshPackageName = "pr2_description"
+rootJointType = "freeflyer"
+##
+# Information to retrieve urdf and srdf files.
+urdfName = "pr2"
urdfSuffix = ""
srdfSuffix = ""
@@ -14,21 +16,23 @@ fullBody = FullBody ()
fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
fullBody.setJointBounds ("base_joint_xyz", [0, 2, -2, 0, -1, 1.5])
-fullBody.addLimb("base_joint_SO3", 100, 0.1)
+fullBody.addLimb("r_shoulder_pan_joint", 1000, 0.001)
-#~ from hpp.corbaserver.rbprm. import ProblemSolver
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
ps = ProblemSolver( fullBody )
-
r = Viewer (ps)
+r.loadObstacleModel ('hpp-rbprm-corba', "scene", "car")
-fullBody.getSample('base_joint_SO3', 1)
-fullBody.client.rbprm.rbprm.getSampleConfig('base_joint_SO3', 1)
-q_init = fullBody.client.rbprm.rbprm.getSampleConfig('base_joint_SO3', 1)
+q_init = fullBody.getCurrentConfig ()
+q_init [0:3] = [-0.6, -0.5, -0.4]
r (q_init)
+fullBody.setCurrentConfig (q_init)
-ps.setInitialConfig (q_init)
-ps.addGoalConfig (q_goal)
+q_init = fullBody.generateContacts(q_init, [0,1,0])
+r (q_init)
+
+#~ q_init = fullBody.getSample('r_shoulder_pan_joint', 1)
+#~ r (q_init)
diff --git a/src/hpp/corbaserver/rbprm/rbprmbuilder.py b/src/hpp/corbaserver/rbprm/rbprmbuilder.py
index c4403a7..f91cec4 100644
--- a/src/hpp/corbaserver/rbprm/rbprmbuilder.py
+++ b/src/hpp/corbaserver/rbprm/rbprmbuilder.py
@@ -49,7 +49,7 @@ class Builder (object):
self.load = load
## Virtual function to load the robot model
- def loadModel (self, urdfName, urdfNamerom, rootJointType, packageName, meshPackageName, urdfSuffix, srdfSuffix):
+ def loadModel (self, urdfName, urdfNamerom, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix):
self.client.rbprm.rbprm.loadRobotRomModel(urdfNamerom, rootJointType, packageName, urdfNamerom, urdfSuffix, srdfSuffix)
self.client.rbprm.rbprm.loadRobotCompleteModel(urdfName, rootJointType, packageName, urdfName, urdfSuffix, srdfSuffix)
self.name = urdfName
diff --git a/src/hpp/corbaserver/rbprm/rbprmfullbody.py b/src/hpp/corbaserver/rbprm/rbprmfullbody.py
index 1172378..ec04f7c 100644
--- a/src/hpp/corbaserver/rbprm/rbprmfullbody.py
+++ b/src/hpp/corbaserver/rbprm/rbprmfullbody.py
@@ -48,7 +48,7 @@ class FullBody (object):
self.client = CorbaClient ()
self.load = load
- def loadFullBodyModel (self, urdfName, rootJointType, packageName, meshPackageName, urdfSuffix, srdfSuffix):
+ def loadFullBodyModel (self, urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix):
self.client.rbprm.rbprm.loadFullBodyRobot(urdfName, rootJointType, packageName, urdfName, urdfSuffix, srdfSuffix)
self.name = urdfName
self.displayName = urdfName
@@ -77,6 +77,8 @@ class FullBody (object):
def getSample(self, name, idsample):
return self.client.rbprm.rbprm.getSampleConfig(name,idsample)
+ def generateContacts(self, configuration, direction):
+ return self.client.rbprm.rbprm.generateContacts(configuration, direction)
## \name Degrees of freedom
# \{
diff --git a/src/rbprmbuilder.impl.cc b/src/rbprmbuilder.impl.cc
index f3d9d89..ae89e89 100644
--- a/src/rbprmbuilder.impl.cc
+++ b/src/rbprmbuilder.impl.cc
@@ -149,8 +149,7 @@ namespace hpp {
throw Error (err.c_str());
}
const sampling::Sample& sample = limbPtr->sampleContainer_.samples_[sampleId];
- size_t ric = sample.startRank_;
- config.tail(config.rows() - ric) = sample.configuration_;
+ 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++)
@@ -158,6 +157,54 @@ namespace hpp {
return dofArray;
}
+ model::Configuration_t dofArrayToConfig (const model::DevicePtr_t& robot,
+ const hpp::floatSeq& dofArray)
+ {
+ std::size_t configDim = (std::size_t)dofArray.length();
+ // Get robot
+ if (!robot) {
+ throw hpp::Error ("No robot in problem solver.");
+ }
+ std::size_t deviceDim = robot->configSize ();
+ // Fill dof vector with dof array.
+ model::Configuration_t config; config.resize (configDim);
+ for (std::size_t iDof = 0; iDof < configDim; iDof++) {
+ config [iDof] = dofArray[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;
+ }
+
+
+ hpp::floatSeq* RbprmBuilder::generateContacts(const hpp::floatSeq& configuration, const hpp::floatSeq& direction) throw (hpp::Error)
+ {
+ if(!fullBodyLoaded_)
+ throw Error ("No full body robot was loaded");
+ try
+ {
+ Eigen::Vector3d dir;
+ for(std::size_t i =0; i <3; ++i)
+ {
+ dir[i] = direction[i];
+ }
+ model::Configuration_t config = dofArrayToConfig (fullBody_->device_, configuration);
+ rbprm::State state = rbprm::ComputeContacts(fullBody_,config,
+ problemSolver_->collisionObstacles(), dir);
+ hpp::floatSeq* 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] = state.configuration_ [i];
+ return dofArray;
+ } catch (const std::exception& exc) {
+ throw hpp::Error (exc.what ());
+ }
+ }
+
void RbprmBuilder::addLimb(const char* limb, unsigned short samples, double resolution) throw (hpp::Error)
{
if(!fullBodyLoaded_)
diff --git a/src/rbprmbuilder.impl.hh b/src/rbprmbuilder.impl.hh
index 3b32439..53b33a1 100644
--- a/src/rbprmbuilder.impl.hh
+++ b/src/rbprmbuilder.impl.hh
@@ -77,6 +77,9 @@ namespace hpp {
virtual hpp::floatSeq* getSampleConfig(const char* limb, unsigned short sampleId) throw (hpp::Error);
+ virtual hpp::floatSeq* generateContacts(const hpp::floatSeq& configuration,
+ const hpp::floatSeq& direction) throw (hpp::Error);
+
virtual void addLimb(const char* limb, unsigned short samples, double resolution) throw (hpp::Error);
public:
--
GitLab