diff --git a/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl b/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl index 7abb2138cf402134d876557d59962129b09db9d2..c7c884081c898f55e5c4c84a7d22f3512f645ff0 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 b3a3a78441198037554add0a9bf34d373135fe26..d8899492590a5af921e1702f42584298908ef641 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 c4403a7366bf9e2b6737e867257e630f422b4432..f91cec44efb6b33aad799b39291c2a9a1f68eda9 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 11723786bf36c589e948326a760af44a2ff76540..ec04f7c74fc45011b24e25e65ccba939f68b0b1f 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 f3d9d89e64a94a4d3d4f7d25b8ddb5b2978ecacf..ae89e89cfcdcd33b4aa488ee2026544e5f7b8ff7 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 3b324399df953e63da44eab818d0c1153e083cc3..53b33a19e4767c57f429054e6e36b2a507642f9d 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: