Commit 9304b6ce authored by Steve Tonneau's avatar Steve Tonneau
Browse files

onto path validation rbprm

parent be06e21f
......@@ -53,5 +53,31 @@ ADD_SUBDIRECTORY(src)
CONFIG_FILES (include/hpp/corbaserver/rbprm/doc.hh)
SET(CATKIN_PACKAGE_SHARE_DESTINATION
${CMAKE_INSTALL_DATAROOTDIR}/${PROJECT_NAME})
install(FILES
data/package.xml
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
install(FILES
data/urdf/box_rom.urdf
data/urdf/box.urdf
data/urdf/scene.urdf
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/urdf
)
install(FILES
data/srdf/box_rom.srdf
data/srdf/box.srdf
data/srdf/scene.srdf
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/srdf
)
install(FILES
data/meshes/car.stl
data/meshes/box.stl
data/meshes/box_rom.stl
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/meshes
)
SETUP_PROJECT_FINALIZE()
No preview for this file type
......@@ -3,7 +3,7 @@
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<box size="0.05 0.05 0.05"/>
<mesh filename="package://hpp-rbprm-corba/meshes/robot_box.stl"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
......@@ -12,7 +12,7 @@
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<box size="0.05 0.05 0.05"/>
<mesh filename="package://hpp-rbprm-corba/meshes/box.stl"/>
</geometry>
</collision>
</link>
......
......@@ -3,7 +3,7 @@
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<box size="0.1 0.1 0.1"/>
<mesh filename="package://hpp-rbprm-corba/meshes/box_rom.stl"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
......@@ -12,7 +12,7 @@
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<box size="0.1 0.1 0.1"/>
<mesh filename="package://hpp-rbprm-corba/meshes/box_rom.stl"/>
</geometry>
</collision>
</link>
......
......@@ -23,22 +23,8 @@ r = Viewer (ps)
q_init = rbprmBuilder.getCurrentConfig ()
q_goal = q_init [::]
q_init [0:3] = [0, -0.5, 0.4]
#~ rank = rbprmBuilder.rankInConfiguration ['torso_lift_joint']
#~ q_init [rank] = 0.2
r (q_init)
q_goal [0:3] = [1, -0.5, 1]
#~ q_goal [0:3] = [-3.2, 0, 3]
#~ rank = rbprmBuilder.rankInConfiguration ['l_shoulder_lift_joint']
#~ q_goal [rank] = 0.5
#~ rank = rbprmBuilder.rankInConfiguration ['l_elbow_flex_joint']
#~ q_goal [rank] = -0.5
#~ rank = rbprmBuilder.rankInConfiguration ['r_shoulder_lift_joint']
#~ q_goal [rank] = 0.5
#~ rank = rbprmBuilder.rankInConfiguration ['r_elbow_flex_joint']
#~ q_goal [rank] = -0.5
q_init [0:3] = [0, -0.5, 0]
q_goal [0:3] = [1.1, -0.5, 1]
r (q_goal)
r.loadObstacleModel (packageName, "scene", "car")
......@@ -47,7 +33,7 @@ ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
ps.client.problem.selectConFigurationShooter("RbprmShooter")
ps.client.problem.selectPathValidation("RbprmPathValidation",0.05)
ps.solve ()
......
from hpp.corbaserver.rbprm.rbprmbuilder import Builder
from hpp.gepetto import Viewer
rootJointType = 'freeflyer'
packageName = 'hpp-rbprm-corba'
meshPackageName = 'hpp-rbprm-corba'
urdfName = 'box'
urdfNameRom = 'box_rom'
urdfSuffix = ""
srdfSuffix = ""
rbprmBuilder = Builder ()
rbprmBuilder.loadModel(urdfName, urdfNameRom, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
rbprmBuilder.setJointBounds ("base_joint_xyz", [0, 2, -2, 0, 0, 1.5])
#~ from hpp.corbaserver.rbprm. import ProblemSolver
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
ps = ProblemSolver( rbprmBuilder )
r = Viewer (ps)
q_init = rbprmBuilder.getCurrentConfig ()
q_goal = q_init [::]
q_init [0:3] = [0, -0.5, 0.4]
#~ rank = rbprmBuilder.rankInConfiguration ['torso_lift_joint']
#~ q_init [rank] = 0.2
r (q_init)
q_goal [0:3] = [1, -0.5, 1]
#~ q_goal [0:3] = [-3.2, 0, 3]
#~ rank = rbprmBuilder.rankInConfiguration ['l_shoulder_lift_joint']
#~ q_goal [rank] = 0.5
#~ rank = rbprmBuilder.rankInConfiguration ['l_elbow_flex_joint']
#~ q_goal [rank] = -0.5
#~ rank = rbprmBuilder.rankInConfiguration ['r_shoulder_lift_joint']
#~ q_goal [rank] = 0.5
#~ rank = rbprmBuilder.rankInConfiguration ['r_elbow_flex_joint']
#~ q_goal [rank] = -0.5
r (q_goal)
r.loadObstacleModel (packageName, "scene", "car")
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
ps.client.problem.selectConFigurationShooter("RbprmShooter")
ps.solve ()
from hpp.gepetto import PathPlayer
pp = PathPlayer (rbprmBuilder.client.basic, r)
pp (0)
......@@ -49,9 +49,9 @@ class Builder (object):
self.load = load
## Virtual function to load the robot model
def loadModel (self, urdfName, rootJointType, packageName, meshPackageName, urdfSuffix, srdfSuffix):
def loadModel (self, urdfName, urdfNamerom, rootJointType, packageName, meshPackageName, urdfSuffix, srdfSuffix):
self.client.rbprm.rbprm.loadRobotRomModel(urdfName, rootJointType, packageName, urdfName, urdfSuffix, srdfSuffix)
self.client.rbprm.rbprm.loadRobotCompleteModel(urdfName, rootJointType, packageName, urdfName, urdfSuffix, srdfSuffix)
self.client.rbprm.rbprm.loadRobotCompleteModel(urdfNamerom, rootJointType, packageName, urdfName, urdfSuffix, srdfSuffix)
self.name = urdfName
self.displayName = urdfName
self.tf_root = "base_link"
......
......@@ -19,8 +19,11 @@
#include <hpp/util/debug.hh>
#include "rbprmbuilder.impl.hh"
#include "hpp/rbprm/rbprm-device.hh"
#include "hpp/rbprm/rbprm-validation.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>
......@@ -95,6 +98,17 @@ namespace hpp {
}
}
namespace
{
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));
hpp::core::CollisionPathValidationReport defaultValidation;
return hpp::core::DiscretizedCollisionChecking::createWithValidation(robot,val ,defaultValidation, validation);
}
}
void RbprmBuilder::SetProblemSolver (hpp::core::ProblemSolverPtr_t problemSolver)
{
problemSolver_ = problemSolver;
......@@ -104,6 +118,7 @@ namespace hpp {
// add rbprmshooter
problemSolver->addConfigurationShooterType("RbprmShooter",
boost::bind(&BindShooter::create, boost::ref(bindShooter_), _1));
problemSolver->addPathValidationType("RbprmPathValidation", &createPathValidation);
}
} // namespace impl
......
......@@ -33,23 +33,15 @@ namespace hpp {
struct BindShooter
{
BindShooter(const std::size_t shootLimit = 10000,
const std::size_t displacementLimit = 100)
const std::size_t displacementLimit = 10)
: shootLimit_(shootLimit)
, displacementLimit_(displacementLimit) {}
hpp::rbprm::RbPrmShooterPtr_t create (const hpp::model::DevicePtr_t& robot)
{
hpp::model::DevicePtr_t pRobot = robot;
hpp::model::RbPrmDevicePtr_t robotcast = boost::static_pointer_cast<hpp::model::RbPrmDevice>(robot);
hpp::rbprm::RbPrmValidationPtr_t validator = hpp::rbprm::RbPrmValidation::create(robotcast);
const hpp::core::ObjectVector_t& obstacles = problemSolver_->collisionObstacles();
for(hpp::core::ObjectVector_t::const_iterator cit = obstacles.begin();
cit != obstacles.end(); ++cit)
{
validator->addObstacle(*cit);
}
return hpp::rbprm::RbPrmShooter::create
(robotcast,problemSolver_->collisionObstacles(),validator,shootLimit_,displacementLimit_);
(robotcast,problemSolver_->problem ()->collisionObstacles(),shootLimit_,displacementLimit_);
}
hpp::core::ProblemSolverPtr_t problemSolver_;
std::size_t shootLimit_;
......
Markdown is supported
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