Commit c4bc8db4 authored by Steve Tonneau's avatar Steve Tonneau
Browse files

interpolate now takes extra optional param to filter states

parent 43f6f3dd
......@@ -234,7 +234,9 @@ module hpp
/// to this function. If these conditions are not met an error is raised.
/// \param timestep normalized step for generation along the path (ie the path has a length of 1).
/// \param path path computed.
floatSeqSeq interpolate(in double timestep, in double path, in double robustnessTreshold) raises (Error);
/// \param robustnessTreshold minimum value of the static equilibrium robustness criterion required to accept the configuration (0 by default).
/// \param filterStates If different than 0, the resulting state list will be filtered to remove unnecessary states
floatSeqSeq interpolate(in double timestep, in double path, in double robustnessTreshold, in unsigned short filterStates) raises (Error);
/// Provided a path has already been computed, interpolates it and generates the statically stable
/// constact configurations along it. setStartState and setEndState must have been called prior
......@@ -242,7 +244,8 @@ module hpp
/// \param timestep normalized step for generation along the path (ie the path has a length of 1).
/// \param path path computed.
/// \param robustnessTreshold minimum value of the static equilibrium robustness criterion required to accept the configuration (0 by default).
floatSeqSeq interpolateConfigs(in floatSeqSeq configs, in double robustnessTreshold) raises (Error);
/// \param filterStates If different than 0, the resulting state list will be filtered to remove unnecessary states
floatSeqSeq interpolateConfigs(in floatSeqSeq configs, in double robustnessTreshold, in unsigned short filterStates) raises (Error);
/// returns the CWC of the robot at a given state
......
......@@ -110,6 +110,7 @@ INSTALL(
${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/tools//generateROMs.py
${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/tools/plot_analytics.py
${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/tools/cwc_trajectory.py
${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/tools/cwc_trajectory_helper.py
${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/tools/path_to_trajectory.py
${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/tools/obj_to_constraints.py
DESTINATION ${PYTHON_SITELIB}/hpp/corbaserver/rbprm/tools
......
......@@ -254,8 +254,13 @@ class FullBody (object):
# \param stepSize discretization step
# \param pathId Id of the path to compute from
# \param robustnessTreshold minimum value of the static equilibrium robustness criterion required to accept the configuration (0 by default).
def interpolate(self, stepsize, pathId = 1, robustnessTreshold = 0):
return self.client.rbprm.rbprm.interpolate(stepsize, pathId, robustnessTreshold)
# \param filterStates If different than 0, the resulting state list will be filtered to remove unnecessary states
def interpolate(self, stepsize, pathId = 1, robustnessTreshold = 0, filterStates = False):
if(filterStates):
filt = 1
else:
filt = 0
return self.client.rbprm.rbprm.interpolate(stepsize, pathId, robustnessTreshold, filt)
## Provided a discrete contact sequence has already been computed, computes
# all the contact positions and normals for a given state, the next one, and the intermediate between them.
......@@ -272,8 +277,14 @@ class FullBody (object):
#
# \param stepSize discretization step
# \param pathId Id of the path to compute from
def interpolateConfigs(self, configs):
return self.client.rbprm.rbprm.interpolateConfigs(configs)
# \param robustnessTreshold minimum value of the static equilibrium robustness criterion required to accept the configuration (0 by default).
# \param filterStates If different than 0, the resulting state list will be filtered to remove unnecessary states
def interpolateConfigs(self, configs, robustnessTreshold = 0, filterStates = False):
if(filterStates):
filt = 1
else:
filt = 0
return self.client.rbprm.rbprm.interpolateConfigs(configs, robustnessTreshold, filt)
##
#
......
......@@ -217,7 +217,8 @@ def saveAllData(fullBody, r, name):
saveToPinocchio(name)
def play_traj(fullBody,pp,frame_rate):
return play_trajectory(fullBody,pp,frame_rate)
global trajec
return play_trajectory(fullBody,pp,trajec,frame_rate)
#~ fullBody.exportAll(r, trajec, 'darpa_hyq_t_var_04f_andrea');
#~ saveToPinocchio('darpa_hyq_t_var_04f_andrea')
......@@ -783,7 +783,7 @@ namespace hpp {
return res;
}
floatSeqSeq* RbprmBuilder::interpolateConfigs(const hpp::floatSeqSeq& configs, double robustnessTreshold) throw (hpp::Error)
floatSeqSeq* RbprmBuilder::interpolateConfigs(const hpp::floatSeqSeq& configs, double robustnessTreshold, unsigned short filterStates) throw (hpp::Error)
{
try
{
......@@ -803,7 +803,7 @@ namespace hpp {
throw hpp::Error ("No affordances found. Unable to interpolate.");
}
hpp::rbprm::interpolation::RbPrmInterpolationPtr_t interpolator = rbprm::interpolation::RbPrmInterpolation::create(fullBody_,startState_,endState_);
lastStatesComputedTime_ = interpolator->Interpolate(affMap, bindShooter_.affFilter_,configurations,robustnessTreshold);
lastStatesComputedTime_ = interpolator->Interpolate(affMap, bindShooter_.affFilter_,configurations,robustnessTreshold, filterStates != 0);
lastStatesComputed_ = TimeStatesToStates(lastStatesComputedTime_);
hpp::floatSeqSeq *res;
res = new hpp::floatSeqSeq ();
......@@ -1047,7 +1047,7 @@ namespace hpp {
return res;
}
floatSeqSeq* RbprmBuilder::interpolate(double timestep, double path, double robustnessTreshold) throw (hpp::Error)
floatSeqSeq* RbprmBuilder::interpolate(double timestep, double path, double robustnessTreshold, unsigned short filterStates) throw (hpp::Error)
{
try
{
......@@ -1075,7 +1075,7 @@ namespace hpp {
hpp::rbprm::interpolation::RbPrmInterpolationPtr_t interpolator =
rbprm::interpolation::RbPrmInterpolation::create(fullBody_,startState_,endState_,problemSolver_->paths()[pathId]);
lastStatesComputedTime_ = interpolator->Interpolate(affMap, bindShooter_.affFilter_,
timestep,robustnessTreshold);
timestep,robustnessTreshold, filterStates != 0);
lastStatesComputed_ = TimeStatesToStates(lastStatesComputedTime_);
hpp::floatSeqSeq *res;
......
......@@ -144,8 +144,8 @@ namespace hpp {
virtual void setStartState(const hpp::floatSeq& configuration, const hpp::Names_t& contactLimbs) throw (hpp::Error);
virtual void setEndState(const hpp::floatSeq& configuration, const hpp::Names_t& contactLimbs) throw (hpp::Error);
virtual hpp::floatSeqSeq* computeContactPoints(unsigned short cId) throw (hpp::Error);
virtual hpp::floatSeqSeq* interpolate(double timestep, double path, double robustnessTreshold) throw (hpp::Error);
virtual hpp::floatSeqSeq* interpolateConfigs(const hpp::floatSeqSeq& configs, double robustnessTreshold) throw (hpp::Error);
virtual hpp::floatSeqSeq* interpolate(double timestep, double path, double robustnessTreshold, unsigned short filterStates) throw (hpp::Error);
virtual hpp::floatSeqSeq* interpolateConfigs(const hpp::floatSeqSeq& configs, double robustnessTreshold, unsigned short filterStates) throw (hpp::Error);
virtual hpp::floatSeqSeq* getContactCone(unsigned short stateId, double friction) throw (hpp::Error);
virtual hpp::floatSeqSeq* getContactIntermediateCone(unsigned short stateId, double friction) throw (hpp::Error);
virtual CORBA::Short generateRootPath(const hpp::floatSeqSeq& rootPositions,
......
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