Commit 349e4dd0 authored by Pierre Fernbach's avatar Pierre Fernbach
Browse files

add method 'getCOntactsVariations' in python

parent 7137a4a3
......@@ -513,9 +513,14 @@ module hpp
/// \param logFile name of the file where to dump the profiling data
void dumpProfile(in string logFile) raises (Error);
/// return the time at the given state index
/// \param stateId index of the state
double getTimeAtState(in unsigned short stateId) raises (Error);
/// return the time at the given state index
/// \param stateId index of the state
double getTimeAtState(in unsigned short stateId) raises (Error);
/// return the contacts variation between two states
/// \param stateIdFrom : index of the first state
/// \param stateIdTo : index of the second state
Names_t getContactsVariations(in unsigned short stateIdFrom,in unsigned short stateIdTo) raises (Error);
}; // interface Robot
}; // module rbprm
......
......@@ -109,7 +109,7 @@ import fullBodyPlayerHrp2
configs = fullBody.interpolate(0.001,pathId=pId,robustnessTreshold = 1, filterStates = True)
print "number of configs :", len(configs)
r(configs[-1])
......
......@@ -888,3 +888,9 @@ class FullBody (object):
# \param stateId : index of the state
def getTimeAtState(self,stateId):
return self.client.rbprm.rbprm.getTimeAtState(stateId)
## return the contacts variation between two states
# \param stateIdFrom : index of the first state
# \param stateIdTo : index of the second state
def getContactsVariations(self,stateIdFrom,stateIdTo):
return self.client.rbprm.rbprm.getContactsVariations(stateIdFrom,stateIdTo)
......@@ -43,7 +43,7 @@
#include <hpp/rbprm/planner/rbprm-steering-kinodynamic.hh>
#include <hpp/model/configuration.hh>
#include <algorithm> // std::random_shuffle
#include <hpp/rbprm/interpolation/time-constraint-helper.hh>
#ifdef PROFILE
#include "hpp/rbprm/rbprm-profiler.hh"
#endif
......@@ -1113,6 +1113,36 @@ namespace hpp {
}
}
Names_t* RbprmBuilder::getContactsVariations(unsigned short stateIdFrom,unsigned short stateIdTo )throw (hpp::Error){
try
{
if(lastStatesComputed_.size() == 0)
{
throw std::runtime_error ("states not yet computed, call interpolate() first.");
}
if(lastStatesComputedTime_.size() <= stateIdFrom){
throw std::runtime_error ("invalid state id : "+std::string(""+stateIdFrom)+" number of state = "+std::string(""+lastStatesComputedTime_.size()));
}
if(lastStatesComputedTime_.size() <= stateIdTo){
throw std::runtime_error ("invalid state id : "+std::string(""+stateIdTo)+" number of state = "+std::string(""+lastStatesComputedTime_.size()));
}
State stateFrom = lastStatesComputed_[stateIdFrom];
State stateTo = lastStatesComputed_[stateIdTo];
std::vector<std::string> variations_s = stateTo.allVariations(stateFrom,rbprm::interpolation::extractEffectorsName(fullBody_->GetLimbs()));
CORBA::ULong size = (CORBA::ULong) variations_s.size ();
char** nameList = Names_t::allocbuf(size);
Names_t *variations = new Names_t (size,size,nameList);
for (std::size_t i = 0 ; i < variations_s.size() ; ++i){
nameList[i] = (char*) malloc (sizeof(char)*(variations_s[i].length ()+1));
strcpy (nameList [i], variations_s[i].c_str ());
}
return variations;
}
catch(std::runtime_error& e)
{
throw Error(e.what());
}
}
std::vector<State> TimeStatesToStates(const T_StateFrame& ref)
{
......
......@@ -34,6 +34,8 @@
# include <hpp/fcl/BVH/BVH_model.h>
# include <hpp/core/config-validations.hh>
#include <hpp/rbprm/dynamic/dynamic-path-validation.hh>
# include "hpp/corbaserver/fwd.hh"
namespace hpp {
namespace rbprm {
namespace impl {
......@@ -251,6 +253,7 @@ namespace hpp {
virtual hpp::floatSeq* runLimbSampleAnalysis(const char* limbname, const char* analysis, double isstatic) throw (hpp::Error);
virtual void dumpProfile(const char* logFile) throw (hpp::Error);
virtual double getTimeAtState(unsigned short stateId)throw (hpp::Error);
virtual Names_t* getContactsVariations(unsigned short stateIdFrom,unsigned short stateIdTo )throw (hpp::Error);
public:
void SetProblemSolver (hpp::core::ProblemSolverPtr_t problemSolver);
......
Markdown is supported
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