Commit c6ca9ec9 authored by stevet's avatar stevet
Browse files

[Feat] add clone state method, projectroot can now take offset, added helper...

[Feat] add clone state method, projectroot can now take offset, added helper method to move limbs into non contacting
parent 74995639
......@@ -154,11 +154,18 @@ module hpp
/// \param com target com
double projectStateToCOM(in unsigned short stateId, in floatSeq com, in unsigned short max_num_sample) raises (Error);
/// Project a state into a COM
///
/// \param stateId target state
/// \param com target com
short cloneState(in unsigned short stateId) raises (Error);
/// Project a state into a given root position and orientation
///
/// \param stateId target state
/// \param root the root configuration (size 7)
double projectStateToRoot(in unsigned short stateId, in floatSeq root) raises (Error);
/// \param offset specific point to be projected in root frame. If different than 0 root orientation is ignored
double projectStateToRoot(in unsigned short stateId, in floatSeq root, in floatSeq offset) raises (Error);
/// Project a state into a COM
......@@ -799,6 +806,8 @@ module hpp
floatSeqSeq getPathAsBezier(in unsigned short pathId)raises (Error);
boolean toggleNonContactingLimb(in string limbname)raises (Error);
boolean areKinematicsConstraintsVerified(in floatSeq point)raises (Error);
boolean areKinematicsConstraintsVerifiedForState(in unsigned short stateFrom,in floatSeq point)raises (Error);
......
SCENE="multicontact/ground"
INIT_CONFIG_ROOT = [0,0,0.465,0,0,0,1]
INIT_CONFIG_WB = None
ROOT_BOUNDS = [-20,20, -20, 20, 0.4, 0.5]
ROOT_BOUNDS = [-20,20, -20, 20, 0.1, 0.6]
......@@ -12,13 +12,14 @@ n_goal = q_init[:7][:]
n_goal[0] += 2
n_goal[1] += 1
n_goal[3:7] = [0.,0.,0.7071,0.7071]
#~ sos.fullBody.toggleNonContactingLimb(sos.fullBody.prongFrontId)
states, configs = fsp.goToQuasiStatic(initState,n_goal, displayGuidePath = True)
#equivalent to
# fsp.goToQuasiStatic(self, initState, n_goal, stepsize = 0.002, goalLimbsInContact = None, goalNormals = None, displayGuidePath = False)
#display computed States:
sos.dispContactPlan(states,0.051) #2nd argument is frame rateue
#~ sos.dispContactPlan(states,0.051) #2nd argument is frame rateue
s = states[-1] #last state computed
......@@ -29,3 +30,14 @@ viewer(s.q())
#some helpers:
s.q() # configuration associated to state
#~ initState. # configuration associated to state
from hpp.corbaserver.rbprm import rbprmstate
target = sos.fullBody.getJointPosition(sos.fullBody.prongFrontId)[:3]
target[2] = 0.
s = rbprmstate.StateHelper.cloneState(states[-1])[0]
fb = sos.fullBody
#~ sos.fullBody.toggleNonContactingLimb(sos.fullBody.prongFrontId)
#~ rbprmstate.StateHelper.addNewContact(s,sos.fullBody.prongFrontId,target,[0.,0.,1.])
#!/bin/bash
gepetto-gui &
ipython -i --no-confirm-exit ./$1
pkill -f 'gepetto-gui'
......@@ -27,6 +27,7 @@ rbprmBuilder.setJointBounds ("root_joint", root_bounds)
rbprmBuilder.setFilter(rbprmBuilder.urdfNameRom)
for rom in rbprmBuilder.urdfNameRom :
rbprmBuilder.setAffordanceFilter(rom, ['Support'])
#~ rbprmBuilder.setAffordanceFilter("front_prong", ['Support'])
# We also bound the rotations of the torso. (z, y, x)
#~ rbprmBuilder.boundSO3([-1.7,1.7,-0.1,0.1,-0.1,0.1])
......@@ -103,7 +104,8 @@ pp = PathPlayer (v)
###### WHOLE BODY INIT
from hpp.corbaserver.rbprm.anymal import Robot
#~ from hpp.corbaserver.rbprm.anymal import Robot
from hpp.corbaserver.rbprm.anymal_prong import Robot
#~ from hpp.gepetto import Viewer
from tools.display_tools import *
import time
......@@ -195,7 +197,7 @@ fullBody.setCurrentConfig (q_init)
v(q_init)
v.addLandmark('anymal/base_0',0.3)
#~ v.addLandmark('anymal/base_0',0.3)
v(q_init)
#fullBody.setReferenceConfig(fullBody.referenceConfig_legsApart[::]+[0]*6)
......
......@@ -93,6 +93,7 @@ INSTALL(
${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/tools/generateROMs.py
${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/tools/com_constraints.py
${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/tools/plot_analytics.py
${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/tools/affordance_centroids.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
......
......@@ -290,7 +290,7 @@ class FullBody (Robot):
num_max_sample = 1
for (limbName, normal) in zip(contacts, normals):
p = cl.getEffectorPosition(limbName,configuration)[0]
cl.addNewContact(sId, limbName, p, normal, num_max_sample, True)
cl.addNewContact(sId, limbName, p, normal, num_max_sample, False)
return cl.setStartStateId(sId)
......@@ -308,7 +308,7 @@ class FullBody (Robot):
num_max_sample = 1
for (limbName, normal) in zip(contacts, normals):
p = cl.getEffectorPosition(limbName,configuration)[0]
cl.addNewContact(sId, limbName, p, normal, num_max_sample, True)
cl.addNewContact(sId, limbName, p, normal, num_max_sample, False)
return cl.setEndStateId(sId)
## Initialize the first state of the path interpolation
......@@ -745,9 +745,10 @@ class FullBody (Robot):
# and update the state configuration.
# \param state index of first state.
# \param root : root configuration (size 7)
# \param offset specific point to be projected in root frame. If different than 0 root orientation is ignored
# \return whether the projection was successful
def projectStateToRoot(self, state, root):
return self.clientRbprm.rbprm.projectStateToRoot(state, root) > 0
def projectStateToRoot(self, state, root, offset = [0,0,0.]):
return self.clientRbprm.rbprm.projectStateToRoot(state, root, offset) > 0
## Project a given state into a given COM position
# and update the state configuration.
......@@ -990,4 +991,7 @@ class FullBody (Robot):
def isDynamicallyReachableFromState(self,stateFrom,stateTo,addPathPerPhase = False,timings=[],numPointsPerPhases=5):
return self.clientRbprm.rbprm.isDynamicallyReachableFromState(stateFrom,stateTo,addPathPerPhase,timings,numPointsPerPhases)
def toggleNonContactingLimb(self,limbName):
return self.clientRbprm.rbprm.toggleNonContactingLimb(limbName)
......@@ -141,8 +141,14 @@ class State (object):
else:
return self.fullBody.projectStateToCOM(self.sId, targetCom,maxNumSample) > 0
def projectToRoot(self,targetRoot):
return self.fullBody.projectStateToRoot(self.sId,targetRoot) > 0
## Project a state into a given root position and orientation
#
# \param stateId target state
# \param root the root configuration (size 7)
# \param offset specific point to be projected in root frame. If different than 0 root orientation is ignored
# \return Whether the projection has been successful or not
def projectToRoot(self,targetRoot, offset = [0., 0., 0.]):
return self.fullBody.projectStateToRoot(self.sId,targetRoot, offset) > 0
def getComConstraint(self, limbsCOMConstraints, exceptList = []):
return get_com_constraint(self.fullBody, self.sId, self.cl.getConfigAtState(self.sId), limbsCOMConstraints, interm = self.isIntermediate, exceptList = exceptList)
......@@ -181,6 +187,13 @@ class StateHelper(object):
if(sId != -1):
return State(state.fullBody, sId = sId), True
return state, False
@staticmethod
def cloneState(state):
sId = state.cl.cloneState(state.sId)
if(sId != -1):
return State(state.fullBody, sId = sId), True
return state, False
## tries to remove a new contact from a state
## if the limb is already in contact, replace the
......
......@@ -18,9 +18,13 @@
from __future__ import print_function
from hpp.corbaserver.rbprm.rbprmstate import State
from lp_find_point import find_valid_c_cwc, find_valid_c_cwc_qp, lp_ineq_4D
from hpp.corbaserver.rbprm.tools.com_constraints import *
from CWC_methods import compute_CWC, is_stable
try:
from hpp.corbaserver.rbprm.tools.com_constraints import *
from CWC_methods import compute_CWC, is_stable
from lp_find_point import find_valid_c_cwc, find_valid_c_cwc_qp, lp_ineq_4D
except:
print "WARNING: in state_alg, some optinal dependencies were not found"
pass
## algorithmic methods on state
......
......@@ -841,21 +841,40 @@ namespace hpp {
return (CORBA::Short)(lastStatesComputed_.size()-1);
}
CORBA::Short RbprmBuilder::cloneState(unsigned short stateId) throw (hpp::Error)
{
try
{
if(lastStatesComputed_.size() <= stateId){
throw std::runtime_error ("Can't clone state: invalid state id : "+std::string(""+stateId)+" number of state = "+std::string(""+lastStatesComputed_.size()));
}
State newState = lastStatesComputed_[stateId];
lastStatesComputed_.push_back(newState);
lastStatesComputedTime_.push_back(std::make_pair(-1., newState));
return (CORBA::Short)(lastStatesComputed_.size()-1);
}
catch(std::runtime_error& e)
{
throw Error(e.what());
}
}
double RbprmBuilder::projectStateToCOM(unsigned short stateId, const hpp::floatSeq& com, unsigned short max_num_sample) throw (hpp::Error)
{
pinocchio::Configuration_t com_target = dofArrayToConfig (3, com);
return projectStateToCOMEigen(stateId, com_target, max_num_sample);
}
double RbprmBuilder::projectStateToRoot(unsigned short stateId, const hpp::floatSeq& root) throw (hpp::Error)
double RbprmBuilder::projectStateToRoot(unsigned short stateId, const hpp::floatSeq& root, const hpp::floatSeq& offset) throw (hpp::Error)
{
pinocchio::Configuration_t root_target = dofArrayToConfig (7, root);
pinocchio::Configuration_t offset_target = dofArrayToConfig (3, offset);
if(lastStatesComputed_.size() <= stateId)
{
throw std::runtime_error ("Unexisting state " + std::string(""+(stateId)));
}
State s = lastStatesComputed_[stateId];
projection::ProjectionReport rep = projection::projectToRootConfiguration(fullBody(),root_target,s);
projection::ProjectionReport rep = projection::projectToRootConfiguration(fullBody(),root_target,s,offset_target);
double success = 0.;
if(rep.success_){
ValidationReportPtr_t rport (ValidationReportPtr_t(new CollisionValidationReport));
......@@ -3461,6 +3480,15 @@ namespace hpp {
return limbsNames;
}
bool RbprmBuilder::toggleNonContactingLimb(const char* limbName)throw (hpp::Error)
{
if(!fullBodyLoaded_){
throw std::runtime_error ("fullBody not loaded");
}
const std::string limb (limbName);
return fullBody()->toggleNonContactingLimb(limb);
}
bool RbprmBuilder::areKinematicsConstraintsVerified(const hpp::floatSeq &point)throw (hpp::Error){
if(!fullBodyLoaded_){
throw std::runtime_error ("fullBody not loaded");
......
......@@ -339,7 +339,8 @@ namespace hpp {
double projectStateToCOMEigen(unsigned short stateId, const pinocchio::Configuration_t& com_target, unsigned short maxNumeSamples)throw (hpp::Error);
virtual double projectStateToCOM(unsigned short stateId, const hpp::floatSeq& com, unsigned short max_num_sample) throw (hpp::Error);
virtual double projectStateToRoot(unsigned short stateId, const hpp::floatSeq& root) throw (hpp::Error);
virtual CORBA::Short cloneState(unsigned short stateId) throw (hpp::Error);
virtual double projectStateToRoot(unsigned short stateId, const hpp::floatSeq& root, const hpp::floatSeq& offset) 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);
......@@ -366,7 +367,7 @@ namespace hpp {
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 toggleNonContactingLimb(const char* limbName)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);
virtual hpp::floatSeq *isReachableFromState(unsigned short stateFrom,unsigned short stateTo,const bool useIntermediateState)throw (hpp::Error);
......
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