Commit 4aee73d7 authored by Steve Tonneau's avatar Steve Tonneau
Browse files

limbRRT, comRRT and generateRootPath return id of the path created

parent c2927c08
......@@ -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.
......
......@@ -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)
......
......@@ -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)
......@@ -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)
{
......
......@@ -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);
......
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