diff --git a/CMakeLists.txt b/CMakeLists.txt
index d23620f95bbd7bbbfa2eebd7cd611467bbad5b72..b49b0dd27ba2642b2e3c8a0f1b6a42057003ea3a 100755
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -68,6 +68,7 @@ install(FILES
data/urdf/scene.urdf
data/urdf/scene_wall.urdf
data/urdf/truck.urdf
+ data/urdf/stair_bauzil.urdf
#~ data/urdf/scene2.urdf
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/urdf
)
@@ -79,6 +80,7 @@ install(FILES
data/srdf/scene.srdf
data/srdf/scene_wall.srdf
data/srdf/truck.srdf
+ data/srdf/stair_bauzil.srdf
#~ data/srdf/scene2.srdf
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/srdf
)
@@ -95,6 +97,7 @@ install(FILES
data/meshes/ground_table.stl
data/meshes/truck.stl
data/meshes/truck_vision.stl
+ data/meshes/stair_bauzil.stl
#~ data/meshes/pedal.stl
#~ data/meshes/board.stl
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/meshes
diff --git a/data/meshes/ground_table.stl b/data/meshes/ground_table.stl
index 3eebf92166c9b83512d71e0b6cf5413ae4f96b09..a76a9ac850a7ceee19ee737ba016625982d5a5dc 100644
Binary files a/data/meshes/ground_table.stl and b/data/meshes/ground_table.stl differ
diff --git a/data/meshes/ground_tablec.stl b/data/meshes/ground_tablec.stl
new file mode 100644
index 0000000000000000000000000000000000000000..4ab21725fac44de98481beceac2ea224501bfdb3
Binary files /dev/null and b/data/meshes/ground_tablec.stl differ
diff --git a/data/meshes/stair_bauzil.stl b/data/meshes/stair_bauzil.stl
index 670fe94e40ea3dab777f06a194e00d6791e45923..a824a8a3937d2cc6e347831e926f12964ce705d9 100644
Binary files a/data/meshes/stair_bauzil.stl and b/data/meshes/stair_bauzil.stl differ
diff --git a/data/stair.blend1 b/data/stair.blend1
new file mode 100644
index 0000000000000000000000000000000000000000..f0c0501e63fbb995c93b18aa3c654b3820c9fc75
Binary files /dev/null and b/data/stair.blend1 differ
diff --git a/data/stair.blend2 b/data/stair.blend2
new file mode 100644
index 0000000000000000000000000000000000000000..769b6f256642c86e0e5a6dd0558e695e78e5377f
Binary files /dev/null and b/data/stair.blend2 differ
diff --git a/data/stair_bauzil.blend b/data/stair_bauzil.blend
index e7e64c04631a938ca51bb2b8a080e8eaf3176a83..68e279f99e0c7405949ba4accfefce239d2e4b42 100644
Binary files a/data/stair_bauzil.blend and b/data/stair_bauzil.blend differ
diff --git a/data/stair_bauzil.blend1 b/data/stair_bauzil.blend1
new file mode 100644
index 0000000000000000000000000000000000000000..130293e28e20fd89325b9d61049f2b5682e24eae
Binary files /dev/null and b/data/stair_bauzil.blend1 differ
diff --git a/data/stair_bauzil.blend2 b/data/stair_bauzil.blend2
new file mode 100644
index 0000000000000000000000000000000000000000..f46c14f49d3ef5d438f8cbf8da06aef15f1d01b7
Binary files /dev/null and b/data/stair_bauzil.blend2 differ
diff --git a/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl b/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
index feb19dfb9d15b6d53cc0e3c16519ea0d51011b78..8cff3863531602bea2e4415590c61864e60b0bec 100755
--- a/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
+++ b/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
@@ -100,6 +100,7 @@ module hpp
void setStartState(in floatSeq dofArray, in Names_t contactLimbs) raises (Error);
void setEndState(in floatSeq dofArray, in Names_t contactLimbs) raises (Error);
floatSeqSeq interpolate(in double timestep) raises (Error);
+ void saveComputedStates(in string filename) raises (Error);
}; // interface Robot
}; // module rbprm
diff --git a/script/loadhrp2stair_bauzil.py b/script/loadhrp2stair_bauzil.py
index 97267a694aeef50c7359700105b239b6818a7afd..4d404e4eb32c3ca3d3b8bd119ada5d2986f15bee 100644
--- a/script/loadhrp2stair_bauzil.py
+++ b/script/loadhrp2stair_bauzil.py
@@ -46,7 +46,7 @@ rHand = 'RARM_JOINT5'
rArmOffset = [-0.05,-0.050,-0.050]
rArmNormal = [1,0,0]
rArmx = 0.024; rArmy = 0.024
-fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, 30000, 0.01)
+fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, 10000, 0.1)
#~ AFTER loading obstacles
diff --git a/script/loadhrp2truck.py b/script/loadhrp2truck.py
index a1d8439636009cd0cf348add02f1e1c666e74043..f38d754e5769ee3b7a695990d32f7e86b946d353 100644
--- a/script/loadhrp2truck.py
+++ b/script/loadhrp2truck.py
@@ -18,7 +18,7 @@ srdfSuffix = ""
fullBody = FullBody ()
fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
-fullBody.setJointBounds ("base_joint_xyz", [-1,1, -4, -1, 1, 2.2])
+fullBody.setJointBounds ("base_joint_xyz", [0,4, -1, 1, 1, 2.5])
#~ from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
@@ -39,14 +39,14 @@ rLeg = 'RLEG_JOINT0'
rLegOffset = [0,-0.105,0,]
rLegNormal = [0,1,0]
rLegx = 0.09; rLegy = 0.05
-fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, 20000, 0.01)
+fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, 20000, 0.03)
lLegId = '8lLeg'
lLeg = 'LLEG_JOINT0'
lLegOffset = [0,-0.105,0]
lLegNormal = [0,1,0]
lLegx = 0.09; lLegy = 0.05
-fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 20000, 0.01)
+fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 20000, 0.03)
rarmId = '3Rarm'
rarm = 'RARM_JOINT0'
@@ -97,11 +97,9 @@ q_init = fullBody.getCurrentConfig (); r (q_init)
#~ fullBody.client.basic.robot.setJointConfig('base_joint_SO3',[0.7316888688738209, 0, -0.6816387600233341, 0]); q_init = fullBody.getCurrentConfig (); r (q_init)
-q_init = fullBody.getCurrentConfig (); r (q_init)
-q_init [0:6] = [0.0, -2.1, 2.0, 0.7316888688738209, 0.0, 0.6816387600233341]; fullBody.setCurrentConfig (q_init); r (q_init)
-q_goal = q_init [::]
-q_goal [0:7] = [0.0, -4.0, 2.0, 1.0, 0.0, 0.0, 0.0]
-
+q_0 = fullBody.getCurrentConfig();
+q_init = fullBody.getCurrentConfig(); q_init[0:7] = tp.q_init[0:7]
+q_goal = fullBody.getCurrentConfig(); q_goal[0:7] = tp.q_goal[0:7]
q_init = fullBody.generateContacts(q_init, [0,0,-1])
r (q_init)
diff --git a/script/stair_path_bauzil_hrp2.py b/script/stair_path_bauzil_hrp2.py
index 2d61fe2271aa44a6e6d9997556cdb9fd75f72d3a..eb8801fa72bb845b48b42b25a6cdd7ccb9391ae8 100644
--- a/script/stair_path_bauzil_hrp2.py
+++ b/script/stair_path_bauzil_hrp2.py
@@ -21,13 +21,12 @@ ps = ProblemSolver( rbprmBuilder )
r = Viewer (ps)
-q_init = rbprmBuilder.getCurrentConfig (); r (q_init)
-q_goal = q_init [::]
q_init = rbprmBuilder.getCurrentConfig ();
-q_init [0:3] = [0, -0.6, 0.6]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
-
+q_init [0:3] = [0, -0.7, 0.6]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
+q_init [3:7] = [ 0.98877108, 0. , 0.14943813, 0. ]
+q_goal = q_init [::]
q_goal [0:3] = [1.49, -0.6, 1.25]; r (q_goal)
#~ ps.addPathOptimizer("GradientBased")
diff --git a/script/stair_path_bauzil_hrp2.pyc b/script/stair_path_bauzil_hrp2.pyc
index c12e1b13acc5852de60688c0820fe674a62d8dda..a8ad9281dc2bd105d226a3c51836799c146f30c4 100644
Binary files a/script/stair_path_bauzil_hrp2.pyc and b/script/stair_path_bauzil_hrp2.pyc differ
diff --git a/script/truck_path_hrp2.py b/script/truck_path_hrp2.py
index 9691b746b952cd7ae2d6ceb78884594902fe969c..6839c0fb80dea6a3e709d0c14ce6d4831619a3b3 100644
--- a/script/truck_path_hrp2.py
+++ b/script/truck_path_hrp2.py
@@ -12,7 +12,7 @@ srdfSuffix = ""
rbprmBuilder = Builder ()
rbprmBuilder.loadModel(urdfName, urdfNameRom, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
-rbprmBuilder.setJointBounds ("base_joint_xyz", [-1,1, -4, -1, 1, 2.2])
+rbprmBuilder.setJointBounds ("base_joint_xyz", [0,4, -1, 1, 1, 2.5])
#~ from hpp.corbaserver.rbprm. import ProblemSolver
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
@@ -26,9 +26,18 @@ q_init = rbprmBuilder.getCurrentConfig (); r (q_init)
#~ fullBody.client.basic.robot.setJointConfig('base_joint_SO3',[0.7316888688738209, 0, -0.6816387600233341, 0]); q_init = rbprmBuilder.getCurrentConfig (); r (q_init)
q_init = rbprmBuilder.getCurrentConfig (); r (q_init)
-q_init [0:6] = [0.0, -2.2, 2.0, 0.7316888688738209, 0.0, 0.6816387600233341]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
+q_init [0:6] = [0.0, -2.2, 2.0, 0.7316888688738209, 0.0, 0.6816387600233341];
+q_init = [2.5, -0.1, 1.55, 1.0, 0.0, 0.0, 0.0]; r (q_init) # 1 scale
+#~ q_init [0:3] = [2.2, 0, 2]; r (q_init)
+#~ q_init [0:3] = [2., -0.1, 1.8]; r (q_init) # 0.9 scale
+#~ q_init [0:3] = [2., -0.1, 1.6]; r (q_init) # 0.8 scale
-q_goal = [0.0, -4.0, 2.0, 1.0, 0.0, 0.0, 0.0]
+rbprmBuilder.setCurrentConfig (q_init); r (q_init)
+
+q_goal = [4.0, -0.1, 1.8, 1.0, 0.0, 0.0, 0.0]
+#~ q_goal [0:3] = [4.0, 0, 2.0]; r (q_goal) # 1 scale
+#~ q_goal [0:3] = [3., -0.1, 1.9]; r (q_goal) # 0.9 scale
+#~ q_goal [0:3] = [3., -0.1, 1.5]; r (q_goal) # 0.8 scale
ps.addPathOptimizer("RandomShortcut")
@@ -46,8 +55,8 @@ from hpp.gepetto import PathPlayer
pp = PathPlayer (rbprmBuilder.client.basic, r)
#~ pp (0)
-#~ pp (1)
-#~ pp.fromFile("/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/truck.path")
pp (1)
+#~ pp.fromFile("/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/truck.path")
+#~ pp (1)
#~ pp.toFile(1, "/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/truck.path")
diff --git a/script/truck_path_hrp2.pyc b/script/truck_path_hrp2.pyc
index ecfc6b2ae01637fad21a2b1ceee8633e3495e1bb..860eb5c39a6674004c1b4270fbd150603bb527a8 100644
Binary files a/script/truck_path_hrp2.pyc and b/script/truck_path_hrp2.pyc differ
diff --git a/src/hpp/corbaserver/rbprm/rbprmfullbody.py b/src/hpp/corbaserver/rbprm/rbprmfullbody.py
index 42a7e2bf4d656b03caff5dda4536fa35038c440e..73f44bbbc59ed049d7dfdf3b9ea322ab2c083578 100755
--- a/src/hpp/corbaserver/rbprm/rbprmfullbody.py
+++ b/src/hpp/corbaserver/rbprm/rbprmfullbody.py
@@ -92,6 +92,9 @@ class FullBody (object):
def setEndState(self, configuration, contacts):
return self.client.rbprm.rbprm.setEndState(configuration, contacts)
+ def saveComputedStates(self, filename):
+ return self.client.rbprm.rbprm.saveComputedStates(filename)
+
def interpolate(self, stepsize):
return self.client.rbprm.rbprm.interpolate(stepsize)
diff --git a/src/rbprmbuilder.impl.cc b/src/rbprmbuilder.impl.cc
index 7712d0716c75a399bf15ff42453dde7b3ac7cd94..cb72ca2713eff1e1a000aef5e17b2e4c8d5a45cd 100755
--- a/src/rbprmbuilder.impl.cc
+++ b/src/rbprmbuilder.impl.cc
@@ -26,6 +26,7 @@
#include <hpp/core/problem-solver.hh>
#include <hpp/core/discretized-collision-checking.hh>
#include <hpp/core/straight-path.hh>
+#include <fstream>
@@ -118,7 +119,9 @@ namespace hpp {
std::string (urdfSuffix),
std::string (srdfSuffix));
fullBody_ = rbprm::RbPrmFullBody::create(device);
+ problemSolver_->pathValidationType ("Discretized",0.05); // reset to avoid conflict with rbprm path
problemSolver_->robot (fullBody_->device_);
+ problemSolver_->resetProblem();
problemSolver_->robot ()->controlComputation
(model::Device::JOINT_POSITION);
}
@@ -233,11 +236,9 @@ namespace hpp {
dir[i] = direction[i];
}
model::Configuration_t config = dofArrayToConfig (fullBody_->device_, configuration);
-std::cout << "configuration loaded " << config << std::endl;
rbprm::State state = rbprm::ComputeContacts(fullBody_,config,
problemSolver_->collisionObstacles(), dir);
hpp::floatSeq* dofArray = new hpp::floatSeq();
-std::cout << "configuration out " << state.configuration_ << std::endl;
dofArray->length(_CORBA_ULong(state.configuration_.rows()));
for(std::size_t i=0; i< _CORBA_ULong(config.rows()); i++)
(*dofArray)[(_CORBA_ULong)i] = state.configuration_ [i];
@@ -342,8 +343,8 @@ std::cout << "configuration out " << state.configuration_ << std::endl;
state.contactNormals_[*cit] = fcl::Vec3f(rot(0,2),rot(1,2), rot(2,2));
state.contacts_[*cit] = true;
state.contactOrder_.push(*cit);
- state.nbContacts++;
- }
+ }
+ state.nbContacts = state.contactNormals_.size() ;
state.configuration_ = config;
fullBody->device_->currentConfiguration(old);
}
@@ -392,21 +393,21 @@ std::cout << "configuration out " << state.configuration_ << std::endl;
}
hpp::rbprm::RbPrmInterpolationPtr_t interpolator = rbprm::RbPrmInterpolation::create(problemSolver_->paths().back(),fullBody_,startState_,endState_);
- std::vector<State> states = interpolator->Interpolate(problemSolver_->collisionObstacles(),timestep);
+ lastStatesComputed_ = interpolator->Interpolate(problemSolver_->collisionObstacles(),timestep);
hpp::floatSeqSeq *res;
res = new hpp::floatSeqSeq ();
- for(std::vector<State>::const_iterator cit = states.begin(); cit != states.end(); ++cit)
+ for(std::vector<State>::const_iterator cit = lastStatesComputed_.begin(); cit != lastStatesComputed_.end(); ++cit)
{
const core::Configuration_t config = cit->configuration_;
hpp::floatSeq dofArray;
dofArray.length (config.size());
}
- res->length (states.size ());
+ res->length (lastStatesComputed_.size ());
std::size_t i=0;
std::size_t id = 0;
- for(std::vector<State>::const_iterator cit = states.begin(); cit != states.end(); ++cit, ++id)
+ for(std::vector<State>::const_iterator cit = lastStatesComputed_.begin(); cit != lastStatesComputed_.end(); ++cit, ++id)
{
std::cout << "ID " << id;
cit->print();
@@ -429,6 +430,29 @@ std::cout << "configuration out " << state.configuration_ << std::endl;
}
}
+ void RbprmBuilder::saveComputedStates(const char* outfilename) throw (hpp::Error)
+ {
+ std::stringstream ss;
+ ss << lastStatesComputed_.size() << "\n";
+ for(std::vector<rbprm::State>::const_iterator cit = lastStatesComputed_.begin();
+ cit != lastStatesComputed_.end(); ++cit)
+ {
+ cit->print(ss);
+ }
+ std::ofstream outfile;
+ outfile.open(outfilename);
+ if (outfile.is_open())
+ {
+ outfile << ss.rdbuf();
+ outfile.close();
+ }
+ else
+ {
+ std::string error("Can not open outfile " + std::string(outfilename));
+ throw Error(error.c_str());
+ }
+ }
+
namespace
{
hpp::core::PathValidationPtr_t createPathValidation (const hpp::model::DevicePtr_t& robot, const hpp::model::value_type& val)
diff --git a/src/rbprmbuilder.impl.hh b/src/rbprmbuilder.impl.hh
index 001e79ca0a1b7152e9876020ae8ee55a8755fc76..0a49cfe79c26810d24227a0c743481eb5952c905 100755
--- a/src/rbprmbuilder.impl.hh
+++ b/src/rbprmbuilder.impl.hh
@@ -90,6 +90,7 @@ namespace hpp {
virtual void setStartState(const hpp::floatSeq& configuration, const hpp::Names_t& contactLimbs) throw (hpp::Error);
virtual void setEndState(const hpp::floatSeq& configuration, const hpp::Names_t& contactLimbs) throw (hpp::Error);
virtual hpp::floatSeqSeq* interpolate(double timestep) throw (hpp::Error);
+ virtual void saveComputedStates(const char* filepath) throw (hpp::Error);
public:
void SetProblemSolver (hpp::core::ProblemSolverPtr_t problemSolver);
@@ -106,6 +107,7 @@ namespace hpp {
BindShooter bindShooter_;
rbprm::State startState_;
rbprm::State endState_;
+ std::vector<rbprm::State> lastStatesComputed_;
}; // class RobotBuilder
} // namespace impl
} // namespace manipulation