Commit 32d4ac58 authored by Steve Tonneau's avatar Steve Tonneau
Browse files

added bindings from com projection and added friction as a cone parameter

parent 4aee73d7
......@@ -229,14 +229,15 @@ module hpp
/// returns the CWC of the robot at a given state
///
/// \param stateId The considered state
/// \param friction The friction coefficient
/// \return H matrix, such that H w <= h. h is added as the last column
floatSeqSeq getContactCone(in unsigned short stateId) raises (Error);
floatSeqSeq getContactCone(in unsigned short stateId, in double friction) raises (Error);
/// returns the CWC of the robot between two states
///
/// \param stateId The considered state
/// \return H matrix, such that H w <= h. h is added as the last column
floatSeqSeq getContactIntermediateCone(in unsigned short stateId) raises (Error);
floatSeqSeq getContactIntermediateCone(in unsigned short stateId, in double friction) raises (Error);
/// Create a path for the root given
/// a set of 3d key points
......@@ -290,6 +291,14 @@ module hpp
/// \return id of the root path computed
short comRRT(in double state1, in double state2, in unsigned short comPath, in unsigned short numOptimizations) 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.
/// \param state index of first state.
/// \param targetCom 3D vector for the com position
/// \return projected configuration
floatSeq projectToCom(in double state, in floatSeq targetCom) raises (Error);
/// Saves the last computed states by the function interpolate in a filename.
/// Raises an error if interpolate has not been called, or the file could not be opened.
/// \param filename name of the file used to save the contacts.
......
......@@ -257,8 +257,8 @@ class FullBody (object):
#
# \param stateId The considered state
# \return H matrix and h column, such that H w <= h
def getContactCone(self, stateId):
H_h = array(self.client.rbprm.rbprm.getContactCone(stateId))
def getContactCone(self, stateId, friction = 0.5):
H_h = array(self.client.rbprm.rbprm.getContactCone(stateId, friction))
# now decompose cone
return H_h[:,:-1], H_h[:, -1]
......@@ -266,8 +266,8 @@ class FullBody (object):
#
# \param stateId The first considered state
# \return H matrix and h column, such that H w <= h
def getContactIntermediateCone(self, stateId):
H_h = array(self.client.rbprm.rbprm.getContactIntermediateCone(stateId))
def getContactIntermediateCone(self, stateId, friction = 0.5):
H_h = array(self.client.rbprm.rbprm.getContactIntermediateCone(stateId, friction))
# now decompose cone
return H_h[:,:-1], H_h[:, -1]
......@@ -340,6 +340,15 @@ class FullBody (object):
return self.client.rbprm.rbprm.comRRT(state1, state2, path, numOptim)
## 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.
# \param state index of first state.
# \param targetCom 3D vector for the com position
# \return projected configuration
def projectToCom(self, state, targetCom):
return self.client.rbprm.rbprm.projectToCom(state, targetCom)
## Given start and goal states
# generate a contact sequence over a list of configurations
#
......
......@@ -5,9 +5,7 @@ def __get_com(robot, config):
save = robot.getCurrentConfig()
robot.setCurrentConfig(config)
com = robot.getCenterOfMass()
print 'debut', com
print 'debut c ', config[0:3]
print 'z_diff', config[2] - com[2]
print 'COM', com
#~ com = config[0:3] #assimilate root and com
robot.setCurrentConfig(save)
return com
......@@ -22,11 +20,11 @@ def gen_trajectory(fullBody, states, state_id, computeCones = False, mu = 1, red
dt = 0.1
cones = None
if(computeCones):
cones = [fullBody.getContactCone(state_id)[0]]
cones = [fullBody.getContactCone(state_id, mu)[0]]
if(len(p) > 2):
cones.append(fullBody.getContactIntermediateCone(state_id)[0])
cones.append(fullBody.getContactIntermediateCone(state_id, mu)[0])
if(len(p) > len(cones)):
cones.append(fullBody.getContactCone(state_id+1)[0])
cones.append(fullBody.getContactCone(state_id+1, mu)[0])
return cone_optimization(p, N, [init_com + [0,0,0], end_com + [0,0,0]], t_end_phases[1:], dt, cones, mu, mass, 9.81, reduce_ineq)
def draw_trajectory(fullBody, states, state_id, computeCones = False, mu = 1, reduce_ineq = True):
......@@ -60,9 +58,27 @@ def draw_trajectory(fullBody, states, state_id, computeCones = False, mu = 1, re
plt.show()
return var_final, params
def solve_com_RRT(fullBody, states, state_id, computeCones = False, mu = 1, reduce_ineq = True, num_optims = 0):
res = gen_trajectory(fullBody, configs, state_id, computeCones, 0.5, False)
c0 = configs[i][0:3]
pos = [c0] + [c.tolist() for c in res[0]['c']]
pid = fullBody.generateRootPathStates(pos, configs[i], configs[i+1])
fullBody.comRRT(i,i+1,pid,num_optims)
def solve_com_RRT(fullBody, states, state_id, computeCones = False, mu = 1, reduce_ineq = True, num_optims = 0, draw = False):
print "callgin gen ",state_id
if(draw):
res = draw_trajectory(fullBody, states, state_id, computeCones, mu, reduce_ineq)
else:
res = gen_trajectory(fullBody, states, state_id, computeCones, mu, reduce_ineq)
t = res[1]["t_init_phases"];
dt = res[1]["dt"];
final_state = res[0]
c0 = res[1]["x_init"][0:3]
print "c0", c0
comPos = [c0] + [c.tolist() for c in final_state['c']]
comPosPerPhase = [[comPos[(int)(t_id/dt)] for t_id in np.arange(t[index],t[index+1],dt)] for index, _ in enumerate(t[:-1]) ]
comPosPerPhase[-1].append(comPos[-1])
assert(len(comPos) == len(comPosPerPhase[0]) + len(comPosPerPhase[1]) + len(comPosPerPhase[2]))
assert(comPos == [item for sublist in comPosPerPhase for item in sublist])
print "done. generating state path ",state_id
pid = fullBody.generateRootPathStates(comPos, states[state_id], states[state_id+1])
print "done. shortcuting ",state_id
return fullBody.comRRT(state_id,state_id+1,pid,num_optims)
......@@ -624,12 +624,12 @@ namespace hpp {
}
}
hpp::floatSeqSeq* contactCone(RbPrmFullBodyPtr_t& fullBody, State& state)
hpp::floatSeqSeq* contactCone(RbPrmFullBodyPtr_t& fullBody, State& state, const double friction)
{
hpp::floatSeqSeq *res;
res = new hpp::floatSeqSeq ();
std::pair<stability::MatrixXX, stability::VectorX> cone = stability::ComputeCentroidalCone(fullBody, state);
std::pair<stability::MatrixXX, stability::VectorX> cone = stability::ComputeCentroidalCone(fullBody, state, friction);
res->length ((_CORBA_ULong)cone.first.rows());
_CORBA_ULong size = (_CORBA_ULong) cone.first.cols()+1;
for(int i=0; i < cone.first.rows(); ++i)
......@@ -647,7 +647,7 @@ namespace hpp {
return res;
}
hpp::floatSeqSeq* RbprmBuilder::getContactCone(unsigned short stateId) throw (hpp::Error)
hpp::floatSeqSeq* RbprmBuilder::getContactCone(unsigned short stateId, double friction) throw (hpp::Error)
{
try
{
......@@ -655,7 +655,7 @@ namespace hpp {
{
throw std::runtime_error ("Unexisting state " + std::string(""+(stateId)));
}
return contactCone(fullBody_, lastStatesComputed_[stateId]);
return contactCone(fullBody_, lastStatesComputed_[stateId],friction);
}
catch(std::runtime_error& e)
{
......@@ -682,7 +682,7 @@ namespace hpp {
return firstState;
}
hpp::floatSeqSeq* RbprmBuilder::getContactIntermediateCone(unsigned short stateId) throw (hpp::Error)
hpp::floatSeqSeq* RbprmBuilder::getContactIntermediateCone(unsigned short stateId, double friction) throw (hpp::Error)
{
try
{
......@@ -696,7 +696,7 @@ namespace hpp {
{
throw std::runtime_error ("No contact breaks, hence no intermediate state from state " + std::string(""+(stateId)));
}
return contactCone(fullBody_,intermediaryState);
return contactCone(fullBody_,intermediaryState, friction);
}
catch(std::runtime_error& e)
{
......@@ -996,6 +996,31 @@ assert(s2 == s1 +1);
}
}
hpp::floatSeq* RbprmBuilder::projectToCom(double state, const hpp::floatSeq& targetCom) throw (hpp::Error)
{
try
{
if(lastStatesComputed_.size () < state)
{
throw std::runtime_error ("did not find a states at indicated index: " + std::string(""+(std::size_t)(state)));
}
model::Configuration_t config = dofArrayToConfig (std::size_t(3), targetCom);
fcl::Vec3f comTarget; for(int i =0; i<3; ++i) comTarget[i] = config[i];
model::Configuration_t res = interpolation::projectOnCom(fullBody_,problemSolver_->problem(), lastStatesComputed_[state],comTarget);
hpp::floatSeq* dofArray = new hpp::floatSeq();
dofArray->length(res.rows());
for(std::size_t i=0; i< res.rows(); ++i)
{
(*dofArray)[(_CORBA_ULong)i] = res[i];
}
return dofArray;
}
catch(std::runtime_error& e)
{
throw Error(e.what());
}
}
void RbprmBuilder::saveComputedStates(const char* outfilename) throw (hpp::Error)
{
std::stringstream ss;
......
......@@ -142,13 +142,14 @@ namespace hpp {
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* getContactCone(unsigned short stateId) throw (hpp::Error);
virtual hpp::floatSeqSeq* getContactIntermediateCone(unsigned short stateId) 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,
const hpp::floatSeq& q1, const hpp::floatSeq& q2) throw (hpp::Error);
virtual CORBA::Short limbRRT(double state1, double state2, unsigned short numOptimizations) throw (hpp::Error);
virtual CORBA::Short limbRRTFromRootPath(double state1, double state2, unsigned short path, unsigned short numOptimizations) throw (hpp::Error);
virtual CORBA::Short comRRT(double state1, double state2, unsigned short path, unsigned short numOptimizations) throw (hpp::Error);
virtual hpp::floatSeq* projectToCom(double state, const hpp::floatSeq& targetCom) 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);
......
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