Commit 84b6dab7 authored by Pierre Fernbach's avatar Pierre Fernbach
Browse files

[contact generation] add method generateEndEffectorBezier

parent de8922e2
......@@ -551,6 +551,17 @@ module hpp
in unsigned short numOptimizations,
in Names_t trackedEffectors) raises (Error);
/// compute and add a trajectory for the end effector between the 2 states
/// represented as a bezier curve.
/// Do not check the kinematic feasability of this trajectory
/// \param state1 index of first state.
/// \param state2 index of end state.
/// \param rootPositions com positions to track
short generateEndEffectorBezier(in double state1,in double state2,
in unsigned short comTraj) raises (Error);
/// Project a given state into a given COM position
/// between two indicated states. The states do not need to be consecutive, but increasing in Id.
/// Will fail if the index of the state does not exist.
......
......@@ -666,6 +666,17 @@ class FullBody (object):
def effectorRRTFromPath(self, state1, refPathId, path_start, path_to, comPos1, comPos2, comPos3, numOptim = 10, trackedEffectors = []):
return self.client.rbprm.rbprm.effectorRRTFromPath(state1, refPathId, path_start, path_to, comPos1, comPos2, comPos3, numOptim, trackedEffectors)
# compute and add a trajectory for the end effector between the 2 states
# represented as a bezier curve.
# Do not check the kinematic feasability of this trajectory
# \param state1 index of first state.
# \param state2 index of end state.
# \param rootPositions com positions to track
def generateEndEffectorBezier(self, state1, state2, comPos):
return self.client.rbprm.rbprm.generateEndEffectorBezier(state1, state2, comPos)
## Project a given state into a given COM position
# between two indicated states. The states do not need to be consecutive, but increasing in Id.
# Will fail if the index of the state does not exist.
......
......@@ -2410,6 +2410,33 @@ assert(s2 == s1 +1);
}
}
CORBA::Short RbprmBuilder::generateEndEffectorBezier(double state1, double state2,
unsigned short cT)throw (hpp::Error){
try
{
hppDout(notice,"Begin generateEndEffectorBezier");
std::size_t s1((std::size_t)state1), s2((std::size_t)state2);
hppDout(notice,"index first state = "<<s1<<" ; index second state : "<<s2);
if(lastStatesComputed_.size () < s1 || lastStatesComputed_.size () < s2 )
{
throw std::runtime_error ("did not find a states at indicated indices: " + std::string(""+s1) + ", " + std::string(""+s2));
}
const core::PathVectors_t& paths = problemSolver()->paths();
if(paths.size() -1 < cT)
{
throw std::runtime_error("in generateEndEffectorBezier, at least one com trajectory is not present in problem solver");
}
State& state1=lastStatesComputed_[s1], state2=lastStatesComputed_[s2];
hppDout(notice,"start generateEndEffectorBezier");
interpolation::generateEndEffectorBezier(fullBody(),problemSolver(),paths[cT],state1,state2);
return problemSolver()->paths().size()-1;
}
catch(std::runtime_error& e)
{
throw Error(e.what());
}
}
hpp::floatSeq* RbprmBuilder::projectToCom(double state, const hpp::floatSeq& targetCom, unsigned short max_num_sample) throw (hpp::Error)
{
try
......
......@@ -313,6 +313,10 @@ namespace hpp {
unsigned short comTraj3,
unsigned short numOptimizations,
const hpp::Names_t& trackedEffectors) throw (hpp::Error);
virtual CORBA::Short generateEndEffectorBezier(double state1, double state2,
unsigned short cT) throw (hpp::Error);
virtual hpp::floatSeq* projectToCom(double state, const hpp::floatSeq& targetCom, unsigned short max_num_sample) throw (hpp::Error);
virtual CORBA::Short createState(const hpp::floatSeq& configuration, const hpp::Names_t& contactLimbs) throw (hpp::Error);
virtual hpp::floatSeq* getConfigAtState(unsigned short stateId) 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