Commit 412788bb authored by Pierre Fernbach's avatar Pierre Fernbach
Browse files

[transition test] add a method to retrieve the Bezier curve in python from the path index

parent 44394474
......@@ -717,6 +717,12 @@ module hpp
/// Throw an error if there is no trajectory computed for the given id/name
floatSeqSeqSeq getEffectorTrajectoryWaypoints(in unsigned short pathId,in string effectorName) raises (Error);
/// Return the waypoints of the bezier curve at given index
/// \param pathId the path index
/// \return a list of lenght degree+ 2 : the first item is the time of the curve, the others are the waypoints
floatSeqSeq getPathAsBezier(in unsigned short pathId)raises (Error);
boolean areKinematicsConstraintsVerified(in floatSeq point)raises (Error);
boolean areKinematicsConstraintsVerifiedForState(in unsigned short stateFrom,in floatSeq point)raises (Error);
......
......@@ -1080,6 +1080,15 @@ class FullBody (object):
res +=[curve]
return res
## Return the bezier curve corresponding to a given path index
def getPathAsBezier(self,pathId):
l = self.client.rbprm.rbprm.getPathAsBezier(pathId)
t = l[0][0]
wps = matrix(l[1:]).transpose()
curve = bezier(wps,t)
return curve
## return the contacts variation between two states
# \param stateIdFrom : index of the first state
# \param stateIdTo : index of the second state
......
......@@ -1589,6 +1589,46 @@ namespace hpp {
}
}
hpp::floatSeqSeq* RbprmBuilder::getPathAsBezier(unsigned short pathId)throw (hpp::Error){
try{
if(!fullBodyLoaded_)
throw std::runtime_error ("No Fullbody loaded");
if(problemSolver()->paths().size() <= pathId)
throw std::runtime_error ("No path at index "+ boost::lexical_cast<std::string>(pathId));
PathVectorPtr_t pathVector = problemSolver()->paths()[pathId];
PathPtr_t path = pathVector->pathAtRank(0);
// try to cast path as BezierPath :
BezierPathPtr_t bezierPath = boost::dynamic_pointer_cast<BezierPath>(path);
if(!bezierPath)
throw std::runtime_error ("Not a bezier path at index "+ boost::lexical_cast<std::string>(pathId));
const bezier_t::t_point_t waypoints = bezierPath->getWaypoints();
// build the floatSeqSeq : first value is the time, the others are the waypoints
hpp::floatSeqSeq *res;
res = new hpp::floatSeqSeq ();
_CORBA_ULong size = (_CORBA_ULong)waypoints.size()+1;
res->length (size); // +1 because the first value is the length (time)
{ // add the time at the first index :
double* dofArray = hpp::floatSeq::allocbuf(1);
hpp::floatSeq floats (1, 1, dofArray, true);
dofArray[0] = bezierPath->length();
(*res) [(_CORBA_ULong)0] = floats; // Always assume the curve start at 0. There isn't any ways to create it otherwise in python
}
// now add the waypoints :
std::size_t i=1;
for(bezier_t::t_point_t::const_iterator wit = waypoints.begin(); wit != waypoints.end(); ++wit,++i)
{
(*res) [(_CORBA_ULong)i] = vectorToFloatseq(*wit);
}
return res;
}
catch(std::runtime_error& e)
{
std::cout << "ERROR " << e.what() << std::endl;
throw Error(e.what());
}
}
......
......@@ -347,6 +347,8 @@ namespace hpp {
virtual hpp::floatSeq* computeTargetTransform(const char* limbName, const hpp::floatSeq& configuration, const hpp::floatSeq& p, const hpp::floatSeq& n) throw (hpp::Error);
virtual Names_t* getEffectorsTrajectoriesNames(unsigned short pathId)throw (hpp::Error);
virtual hpp::floatSeqSeqSeq* getEffectorTrajectoryWaypoints(unsigned short pathId,const char* effectorName)throw (hpp::Error);
virtual hpp::floatSeqSeq* getPathAsBezier(unsigned short pathId)throw (hpp::Error);
virtual bool areKinematicsConstraintsVerified(const hpp::floatSeq &point)throw (hpp::Error);
virtual bool areKinematicsConstraintsVerifiedForState(unsigned short stateId,const hpp::floatSeq &point)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