diff --git a/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl b/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
index 8401ff703c32f2a25d60552d14395526e84af7dc..55dd889e1e1da4740a1628106df853760886c722 100755
--- a/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
+++ b/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
@@ -248,7 +248,8 @@ module hpp
/// \param positions array of positions
/// \param q1 quaternion of 1st rotation
/// \param q2 quaternion of 2nd rotation
- void generateRootPath(in floatSeqSeq rootPositions, in floatSeq q1, in floatSeq q2) raises (Error);
+ /// \return id of the root path computed
+ short generateRootPath(in floatSeqSeq rootPositions, in floatSeq q1, in floatSeq q2) raises (Error);
/// Provided a path has already been computed and interpolated, generate a continuous path
@@ -260,7 +261,8 @@ module hpp
/// \param state1 index of first state.
/// \param state2 index of second state.
/// \param numOptimizations Number of iterations of the shortcut algorithm to apply between each states
- void limbRRT(in double state1, in double state2, in unsigned short numOptimizations) raises (Error);
+ /// \return id of the root path computed
+ short limbRRT(in double state1, in double state2, in unsigned short numOptimizations) raises (Error);
/// Provided a path has already been computed and interpolated, generate a continuous path
/// between two indicated states. The states do not need to be consecutive, but increasing in Id.
@@ -272,7 +274,8 @@ module hpp
/// \param state2 index of second state.
/// \param path index of the path considered for the generation
/// \param numOptimizations Number of iterations of the shortcut algorithm to apply between each states
- void limbRRTFromRootPath(in double state1, in double state2, in unsigned short path, in unsigned short numOptimizations) raises (Error);
+ /// \return id of the root path computed
+ short limbRRTFromRootPath(in double state1, in double state2, in unsigned short path, in unsigned short numOptimizations) raises (Error);
/// Provided a path has already been computed and interpolated, generate a continuous path
/// between two indicated states. The states do not need to be consecutive, but increasing in Id.
@@ -284,7 +287,8 @@ module hpp
/// \param state2 index of second state.
/// \param comPath index of the path considered of the com path
/// \param numOptimizations Number of iterations of the shortcut algorithm to apply between each states
- void comRRT(in double state1, in double state2, in unsigned short comPath, in unsigned short numOptimizations) raises (Error);
+ /// \return id of the root path computed
+ short comRRT(in double state1, in double state2, in unsigned short comPath, in unsigned short numOptimizations) raises (Error);
/// Saves the last computed states by the function interpolate in a filename.
/// Raises an error if interpolate has not been called, or the file could not be opened.
diff --git a/src/hpp/corbaserver/rbprm/rbprmfullbody.py b/src/hpp/corbaserver/rbprm/rbprmfullbody.py
index 1add5f7a9cea14807e8b673197cca8f8038cb2f5..b30adaa5c5b4187ede24e63174ad531bc746903a 100755
--- a/src/hpp/corbaserver/rbprm/rbprmfullbody.py
+++ b/src/hpp/corbaserver/rbprm/rbprmfullbody.py
@@ -295,6 +295,7 @@ class FullBody (object):
# \param positions array of positions
# \param configState1 configuration of 1st rotation
# \param configState2 configuration of 2nd rotation
+ # \return id of the resulting path
def generateRootPathStates(self, positions, configState1, configState2):
return self.client.rbprm.rbprm.generateRootPath(positions, configState1[3:7], configState2[3:7])
@@ -305,6 +306,7 @@ class FullBody (object):
# \param index of first state.
# \param index of second state.
# \param numOptim Number of iterations of the shortcut algorithm to apply between each states
+ # \return id of the resulting path
def limbRRT(self, state1, state2, numOptim = 10):
return self.client.rbprm.rbprm.limbRRT(state1, state2, numOptim)
@@ -318,6 +320,7 @@ class FullBody (object):
# \param state2 index of second state.
# \param path index of the path considered for the generation
# \param numOptimizations Number of iterations of the shortcut algorithm to apply between each states
+ # \return id of the resulting path
def limbRRTFromRootPath(self, state1, state2, path, numOptim = 10):
return self.client.rbprm.rbprm.limbRRTFromRootPath(state1, state2, path, numOptim)
@@ -332,6 +335,7 @@ class FullBody (object):
# \param state2 index of second state.
# \param path index of the com path considered for the generation
# \param numOptimizations Number of iterations of the shortcut algorithm to apply between each states
+ # \return id of the resulting path
def comRRT(self, state1, state2, path, numOptim = 10):
return self.client.rbprm.rbprm.comRRT(state1, state2, path, numOptim)
diff --git a/src/hpp/corbaserver/rbprm/tools/cwc_trajectory.py b/src/hpp/corbaserver/rbprm/tools/cwc_trajectory.py
index a4c3b9751dd1f60bc3e761ffefb6f51de6525712..abf1d550e37dc8dc58dde2a0a6dac6b2fce89eb3 100644
--- a/src/hpp/corbaserver/rbprm/tools/cwc_trajectory.py
+++ b/src/hpp/corbaserver/rbprm/tools/cwc_trajectory.py
@@ -59,3 +59,10 @@ def draw_trajectory(fullBody, states, state_id, computeCones = False, mu = 1, re
plt.show()
return var_final, params
+
+def solve_com_RRT(fullBody, states, state_id, computeCones = False, mu = 1, reduce_ineq = True, num_optims = 0):
+ res = gen_trajectory(fullBody, configs, state_id, computeCones, 0.5, False)
+ c0 = configs[i][0:3]
+ pos = [c0] + [c.tolist() for c in res[0]['c']]
+ pid = fullBody.generateRootPathStates(pos, configs[i], configs[i+1])
+ fullBody.comRRT(i,i+1,pid,num_optims)
diff --git a/src/rbprmbuilder.impl.cc b/src/rbprmbuilder.impl.cc
index 4071a356d2a3b8c62d95c0fca02a1cf35b12d41f..958abecb10a66213a44da0f0ab70add761013a26 100755
--- a/src/rbprmbuilder.impl.cc
+++ b/src/rbprmbuilder.impl.cc
@@ -751,14 +751,14 @@ namespace hpp {
return res;
}
- void RbprmBuilder::generateRootPath(const hpp::floatSeqSeq& rootPositions,
+ CORBA::Short RbprmBuilder::generateRootPath(const hpp::floatSeqSeq& rootPositions,
const hpp::floatSeq& q1Seq, const hpp::floatSeq& q2Seq) throw (hpp::Error)
{
try
{
model::Configuration_t q1 = dofArrayToConfig(4, q1Seq), q2 = dofArrayToConfig(4, q2Seq);
T_Configuration positions = doubleDofArrayToConfig(3, rootPositions);
- problemSolver_->addPath(addRotations(positions,q1,q2,fullBody_->device_->currentConfiguration(),
+ return problemSolver_->addPath(addRotations(positions,q1,q2,fullBody_->device_->currentConfiguration(),
fullBody_->device_,problemSolver_->problem()));
}
catch(std::runtime_error& e)
@@ -918,14 +918,14 @@ namespace hpp {
}
}
- void AddPath(core::PathPtr_t path, core::ProblemSolverPtr_t ps)
+ CORBA::Short AddPath(core::PathPtr_t path, core::ProblemSolverPtr_t ps)
{
core::PathVectorPtr_t resPath = core::PathVector::create(path->outputSize(), path->outputDerivativeSize());
resPath->appendPath(path);
- ps->addPath(resPath);
+ return ps->addPath(resPath);
}
- void RbprmBuilder::limbRRT(double state1, double state2, unsigned short numOptimizations) throw (hpp::Error)
+ CORBA::Short RbprmBuilder::limbRRT(double state1, double state2, unsigned short numOptimizations) throw (hpp::Error)
{
try
{
@@ -938,7 +938,7 @@ namespace hpp {
// /interpolation::TimeConstraintHelper helper(fullBody_, problemSolver_->problem());
core::PathPtr_t path = interpolation::limbRRT(fullBody_,problemSolver_->problem(),
lastStatesComputed_.begin()+s1,lastStatesComputed_.begin()+s2, numOptimizations);
- AddPath(path,problemSolver_);
+ return AddPath(path,problemSolver_);
}
catch(std::runtime_error& e)
{
@@ -946,7 +946,7 @@ namespace hpp {
}
}
- void RbprmBuilder::limbRRTFromRootPath(double state1, double state2, unsigned short path, unsigned short numOptimizations) throw (hpp::Error)
+ CORBA::Short RbprmBuilder::limbRRTFromRootPath(double state1, double state2, unsigned short path, unsigned short numOptimizations) throw (hpp::Error)
{
try
{
@@ -962,7 +962,7 @@ namespace hpp {
}
core::PathPtr_t path = interpolation::limbRRTFromPath(fullBody_,problemSolver_->problem(), problemSolver_->paths()[pathId],
lastStatesComputedTime_.begin()+s1,lastStatesComputedTime_.begin()+s2, numOptimizations);
- AddPath(path,problemSolver_);
+ return AddPath(path,problemSolver_);
}
catch(std::runtime_error& e)
{
@@ -970,7 +970,7 @@ namespace hpp {
}
}
- void RbprmBuilder::comRRT(double state1, double state2, unsigned short path, unsigned short numOptimizations) throw (hpp::Error)
+ CORBA::Short RbprmBuilder::comRRT(double state1, double state2, unsigned short path, unsigned short numOptimizations) throw (hpp::Error)
{
try
{
@@ -988,7 +988,7 @@ assert(s2 == s1 +1);
}
core::PathPtr_t path = interpolation::comRRT(fullBody_,problemSolver_->problem(), problemSolver_->paths()[pathId],
*(lastStatesComputed_.begin()+s1),*(lastStatesComputed_.begin()+s2), numOptimizations);
- AddPath(path,problemSolver_);
+ return AddPath(path,problemSolver_);
}
catch(std::runtime_error& e)
{
diff --git a/src/rbprmbuilder.impl.hh b/src/rbprmbuilder.impl.hh
index 2e07ede06ea59f54bcf367122c22f248bd8bbbe8..104245b6f31a77ca53be2345c712d46b7db8ecf0 100755
--- a/src/rbprmbuilder.impl.hh
+++ b/src/rbprmbuilder.impl.hh
@@ -144,11 +144,11 @@ namespace hpp {
virtual hpp::floatSeqSeq* interpolateConfigs(const hpp::floatSeqSeq& configs, double robustnessTreshold) throw (hpp::Error);
virtual hpp::floatSeqSeq* getContactCone(unsigned short stateId) throw (hpp::Error);
virtual hpp::floatSeqSeq* getContactIntermediateCone(unsigned short stateId) throw (hpp::Error);
- virtual void generateRootPath(const hpp::floatSeqSeq& rootPositions,
+ virtual CORBA::Short generateRootPath(const hpp::floatSeqSeq& rootPositions,
const hpp::floatSeq& q1, const hpp::floatSeq& q2) throw (hpp::Error);
- virtual void limbRRT(double state1, double state2, unsigned short numOptimizations) throw (hpp::Error);
- virtual void limbRRTFromRootPath(double state1, double state2, unsigned short path, unsigned short numOptimizations) throw (hpp::Error);
- virtual void comRRT(double state1, double state2, unsigned short path, unsigned short numOptimizations) throw (hpp::Error);
+ virtual CORBA::Short limbRRT(double state1, double state2, unsigned short numOptimizations) throw (hpp::Error);
+ virtual CORBA::Short limbRRTFromRootPath(double state1, double state2, unsigned short path, unsigned short numOptimizations) throw (hpp::Error);
+ virtual CORBA::Short comRRT(double state1, double state2, unsigned short path, unsigned short numOptimizations) throw (hpp::Error);
virtual void saveComputedStates(const char* filepath) throw (hpp::Error);
virtual void saveLimbDatabase(const char* limbname,const char* filepath) throw (hpp::Error);
virtual hpp::floatSeq* getOctreeBox(const char* limbName, double sampleId) throw (hpp::Error);