Skip to content
GitLab
Commit 73dcb38a authored by Steve Tonneau's avatar Steve Tonneau
Browse files

stair_bauzil working

parent 2c4f5854
Hide whitespace changes
Inline Side-by-side
CMakeLists.txt
View file @ 73dcb38a
......@@ -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
......
data/stair_bauzil.blend
View file @ 73dcb38a
No preview for this file type
idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
View file @ 73dcb38a
......@@ -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
......
script/loadhrp2stair_bauzil.py
View file @ 73dcb38a
......@@ -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
......
script/loadhrp2truck.py
View file @ 73dcb38a
......@@ -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)
......
script/stair_path_bauzil_hrp2.py
View file @ 73dcb38a
......@@ -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")
......
script/truck_path_hrp2.py
View file @ 73dcb38a
......@@ -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")
src/hpp/corbaserver/rbprm/rbprmfullbody.py
View file @ 73dcb38a
......@@ -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)
......
src/rbprmbuilder.impl.cc
View file @ 73dcb38a
......@@ -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)
......
src/rbprmbuilder.impl.hh
View file @ 73dcb38a
......@@ -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
......
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