Commit ad2b4452 authored by Pierre Fernbach's avatar Pierre Fernbach
Browse files

[contact generation] add method to state api to get the center of the contacts

parent eb274655
......@@ -322,6 +322,12 @@ module hpp
/// \return list of 6d vectors [pox, poy, posz, nprmalx, normaly, normalz]
floatSeqSeq computeContactPointsAtStateForLimb(in unsigned short stateId, in unsigned short isIntermediate, in string limbname) raises (Error);
/// Provided a discrete contact sequence has already been computed, computes
/// the position of the contact center and the normal
/// \param stateId normalized step for generation along the path (ie the path has a length of 1).
/// \return 2 3D vector : the first is the position the second is the normal
floatSeqSeq computeCenterOfContactAtStateForLimb(in unsigned short cId,in unsigned short isIntermediate, in string limbName) raises(Error);
/// Provided a discrete contact sequence has already been computed, computes
/// all the contact positions and normals for a given state
/// \param stateId iD of the considered state
......
......@@ -107,6 +107,9 @@ class State (object):
rawdata = self.cl.computeContactPointsAtStateForLimb(self.sId,0, limbName)
return [[b[i:i+3] for i in range(0, len(b), 6)] for b in rawdata], [[b[i+3:i+6] for i in range(0, len(b), 6)] for b in rawdata]
def getCenterOfContactForLimb(self,limbName):
assert self.isLimbInContact(limbName), "in getContactPosAndNormals: limb " + limbName + "is not in contact at state" + str(stateId)
return self.cl.computeCenterOfContactAtStateForLimb(self.sId,self.isIntermediate, limbName)
## Get position of center of mass
def getCenterOfMass (self):
......
......@@ -1980,6 +1980,59 @@ namespace hpp {
return res;
}
floatSeqSeq* RbprmBuilder::computeCenterOfContactAtStateForLimb(unsigned short cId, unsigned short isIntermediate, const char *limbName) throw (hpp::Error)
{
if(lastStatesComputed_.size() <= cId + isIntermediate)
{
throw std::runtime_error ("Unexisting state " + std::string(""+(cId)));
}
std::string limb(limbName);
State state = lastStatesComputed_[cId];
if(isIntermediate > 0)
{
const State& thirdState = lastStatesComputed_[cId+1];
bool success(false);
State interm = intermediary(state, thirdState, cId, success);
if(success)
state = interm;
}
if(state.contacts_.find(limb) == state.contacts_.end()){
throw std::runtime_error ("Limb : "+limb+" is not in contact at state "+std::string(""+(cId)));
}
hpp::floatSeqSeq *res;
res = new hpp::floatSeqSeq ();
res->length ((_CORBA_ULong)2);
fcl::Vec3f& position = state.contactPositions_.at(limbName);
position += fullBody()->GetLimb(limb)->offset_;
_CORBA_ULong size = (_CORBA_ULong) 3;
double* dofArray = hpp::floatSeq::allocbuf(size);
hpp::floatSeq floats (size, size, dofArray, true);
//convert the config in dofseq
for(std::size_t k =0; k<3; ++k)
{
dofArray[k] = position[k];
}
(*res) [(_CORBA_ULong)0] = floats;
const fcl::Vec3f& normal = state.contactNormals_.at(limbName);
double* dofArray_n = hpp::floatSeq::allocbuf(size);
hpp::floatSeq floats_n (size, size, dofArray_n, true);
//convert the config in dofseq
for(std::size_t k =0; k<3; ++k)
{
dofArray_n[k] = normal[k];
}
(*res) [(_CORBA_ULong)1] = floats_n;
return res;
}
floatSeqSeq* RbprmBuilder::interpolate(double timestep, double path, double robustnessTreshold, unsigned short filterStates) throw (hpp::Error)
{
hppDout(notice,"### Begin interpolate");
......
......@@ -259,6 +259,7 @@ namespace hpp {
virtual hpp::floatSeqSeq* computeContactPointsAtState(unsigned short cId, unsigned short isIntermediate) throw (hpp::Error);
virtual hpp::floatSeqSeq* computeContactPointsForLimb(unsigned short cId, const char* limbName) throw (hpp::Error);
virtual hpp::floatSeqSeq* computeContactPointsAtStateForLimb(unsigned short cId, unsigned short isIntermediate, const char* limbName) throw (hpp::Error);
virtual hpp::floatSeqSeq* computeCenterOfContactAtStateForLimb(unsigned short cId, unsigned short isIntermediate, const char *limbName) 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);
......
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