Commit f7e432f7 authored by Steve Tonneau's avatar Steve Tonneau
Browse files

finished integration of multiple roms

parent cf58a886
......@@ -27,3 +27,5 @@
*.exe
*.out
*.app
build/
......@@ -5,14 +5,15 @@ rootJointType = 'freeflyer'
packageName = 'hpp-rbprm-corba'
meshPackageName = 'hpp-rbprm-corba'
urdfName = 'hrp2_trunk'
urdfNameRom = 'hrp2_rom'
urdfNameRoms = ['hrp2_larm_rom','hrp2_rarm_rom','hrp2_lleg_rom','hrp2_rleg_rom']
urdfSuffix = ""
srdfSuffix = ""
rbprmBuilder = Builder ()
rbprmBuilder.loadModel(urdfName, urdfNameRom, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
rbprmBuilder.loadModel(urdfName, urdfNameRoms, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
rbprmBuilder.setJointBounds ("base_joint_xyz", [-1, 1, -0.5, 0.5, 0, 5])
rbprmBuilder.setFilter(['hrp2_lleg_rom','hrp2_rleg_rom'])
#~ from hpp.corbaserver.rbprm. import ProblemSolver
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
......@@ -27,7 +28,7 @@ q_goal = rbprmBuilder.getCurrentConfig ();
q_init = rbprmBuilder.getCurrentConfig ();
#~ q_init = [-0.15, -0.2, 0.6, 0.99500416527802582, 0.0, 0.099833416646828155, 0.0]
q_init = [-0.3, -0.2, 0.6, 1, 0.0, 0., 0.0]; r (q_init)
q_init = [-1, -0.2, 0.6, 1, 0.0, 0., 0.0]; r (q_init)
rbprmBuilder.setCurrentConfig (q_init); r (q_init)
#~ q_goal = [0, 0, 2, 0.99500416527802582, 0.0, 0.099833416646828155, 0.0]; r (q_goal)
......@@ -39,7 +40,7 @@ ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
ps.client.problem.selectConFigurationShooter("RbprmShooter")
ps.client.problem.selectPathValidation("RbprmPathValidation",0.05)
ps.client.problem.selectPathValidation("RbprmPathValidation",0.5)
r.loadObstacleModel (packageName, "stepladder", "planning")
ps.solve ()
......
......@@ -49,8 +49,12 @@ class Builder (object):
self.load = load
## Virtual function to load the robot model
def loadModel (self, urdfName, urdfNamerom, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix):
self.client.rbprm.rbprm.loadRobotRomModel(urdfNamerom, rootJointType, packageName, urdfNamerom, urdfSuffix, srdfSuffix)
def loadModel (self, urdfName, urdfNameroms, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix):
if(isinstance(urdfNameroms, list)):
for urdfNamerom in urdfNameroms:
self.client.rbprm.rbprm.loadRobotRomModel(urdfNamerom, rootJointType, packageName, urdfNamerom, urdfSuffix, srdfSuffix)
else:
self.client.rbprm.rbprm.loadRobotRomModel(urdfNameroms, rootJointType, packageName, urdfNameroms, urdfSuffix, srdfSuffix)
self.client.rbprm.rbprm.loadRobotCompleteModel(urdfName, rootJointType, packageName, urdfName, urdfSuffix, srdfSuffix)
self.name = urdfName
self.displayName = urdfName
......@@ -73,6 +77,8 @@ class Builder (object):
self.rankInVelocity [j] = rankInVelocity
rankInVelocity += self.client.basic.robot.getJointNumberDof (j)
## Init RbprmShooter
#
# \param jointName name of the joint,
......
......@@ -22,10 +22,6 @@
#include "hpp/rbprm/rbprm-validation.hh"
#include "hpp/rbprm/rbprm-path-interpolation.hh"
#include "hpp/model/urdf/util.hh"
#include <hpp/core/collision-path-validation-report.hh>
#include <hpp/core/problem-solver.hh>
#include <hpp/core/discretized-collision-checking.hh>
#include <hpp/core/straight-path.hh>
#include <fstream>
......@@ -459,7 +455,7 @@ namespace hpp {
}
}
namespace
/*namespace
{
hpp::core::PathValidationPtr_t createPathValidation (const hpp::model::DevicePtr_t& robot, const hpp::model::value_type& val)
{
......@@ -468,7 +464,7 @@ namespace hpp {
hpp::core::CollisionPathValidationReport defaultValidation;
return hpp::core::DiscretizedCollisionChecking::createWithValidation(robot,val ,defaultValidation, validation);
}
}
}*/
void RbprmBuilder::SetProblemSolver (hpp::core::ProblemSolverPtr_t problemSolver)
{
......@@ -479,7 +475,8 @@ namespace hpp {
// add rbprmshooter
problemSolver->addConfigurationShooterType("RbprmShooter",
boost::bind(&BindShooter::create, boost::ref(bindShooter_), _1));
problemSolver->addPathValidationType("RbprmPathValidation", &createPathValidation);
problemSolver->addPathValidationType("RbprmPathValidation",
boost::bind(&BindShooter::createPathValidation, boost::ref(bindShooter_), _1, _2));
}
} // namespace impl
......
......@@ -24,6 +24,10 @@
# include <hpp/rbprm/rbprm-fullbody.hh>
# include <hpp/rbprm/rbprm-shooter.hh>
# include <hpp/rbprm/rbprm-validation.hh>
# include <hpp/core/collision-path-validation-report.hh>
# include <hpp/core/problem-solver.hh>
# include <hpp/core/discretized-collision-checking.hh>
# include <hpp/core/straight-path.hh>
namespace hpp {
namespace rbprm {
......@@ -43,6 +47,15 @@ namespace hpp {
return hpp::rbprm::RbPrmShooter::create
(robotcast,problemSolver_->problem ()->collisionObstacles(),romFilter_,shootLimit_,displacementLimit_);
}
hpp::core::PathValidationPtr_t createPathValidation (const hpp::model::DevicePtr_t& robot, const hpp::model::value_type& val)
{
hpp::model::RbPrmDevicePtr_t robotcast = boost::static_pointer_cast<hpp::model::RbPrmDevice>(robot);
hpp::rbprm::RbPrmValidationPtr_t validation(hpp::rbprm::RbPrmValidation::create(robotcast, romFilter_));
hpp::core::CollisionPathValidationReport defaultValidation;
return hpp::core::DiscretizedCollisionChecking::createWithValidation(robot,val ,defaultValidation, validation);
}
hpp::core::ProblemSolverPtr_t problemSolver_;
std::vector<std::string> romFilter_;
std::size_t shootLimit_;
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment