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

early attempt at computing contact points

parent 51fdee2d
......@@ -204,6 +204,12 @@ module hpp
/// \param contactLimbs ids of the limb in contact for the state
void setEndState(in floatSeq dofArray, in Names_t contactLimbs) raises (Error);
/// 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.
/// \param stateId normalized step for generation along the path (ie the path has a length of 1).
/// \return list of 2 or 3 lists of 6d vectors [pox, poy, posz, nprmalx, normaly, normalz]
floatSeqSeq computeContactPoints(in unsigned short stateId) 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
/// to this function. If these conditions are not met an error is raised.
......
......@@ -233,6 +233,16 @@ class FullBody (object):
# \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)
## Discretizes a path return by a motion planner into a discrete
# sequence of balanced, contact configurations and returns
# the sequence as an array of configurations
#
# \param stateId id of the first state
# \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 computeContactPoints(self, stateId):
return self.client.rbprm.rbprm.computeContactPoints(stateId)
## Given start and goal states
# generate a contact sequence over a list of configurations
......
......@@ -616,6 +616,94 @@ namespace hpp {
}
}
typedef Eigen::Matrix <value_type, 4, 3, Eigen::RowMajor> Matrix43;
typedef Eigen::Matrix <value_type, 4, 3, Eigen::RowMajor> Rotation;
typedef Eigen::Ref<Matrix43> Ref_matrix43;
std::vector<fcl::Vec3f> computeRectangleContact(const rbprm::RbPrmFullBodyPtr_t device,
const rbprm::State& state)
{
std::vector<fcl::Vec3f> res;
const rbprm::T_Limb& limbs = device->GetLimbs();
rbprm::RbPrmLimbPtr_t limb;
Matrix43 p; Eigen::Matrix3d R;
for(std::map<std::string, fcl::Vec3f>::const_iterator cit = state.contactPositions_.begin();
cit != state.contactPositions_.end(); ++cit)
{
const std::string& name = cit->first;
const fcl::Vec3f& position = cit->second;
limb = limbs.at(name);
const double& lx = limb->x_, ly = limb->y_;
p << lx, ly, 0,
lx, -ly, 0,
-lx, -ly, 0,
-lx, ly, 0;
const fcl::Matrix3f& fclRotation = state.contactRotation_.at(name);
for(int i =0; i< 3; ++i)
for(int j =0; j<3;++j)
R(i,j) = fclRotation(i,j);
for(std::size_t i =0; i<4; ++i)
{
res.push_back(position + (R*p.row(i).transpose()));
res.push_back(state.contactNormals_.at(name));
}
}
return res;
}
floatSeqSeq* RbprmBuilder::computeContactPoints(unsigned short cId) throw (hpp::Error)
{
if(lastStatesComputed_.size() <= cId + 1)
{
throw std::runtime_error ("Unexisting state " + std::string(""+(cId+1)));
}
const State& firstState = lastStatesComputed_[cId], thirdState = lastStatesComputed_[cId+1];
std::vector<std::vector<fcl::Vec3f> > allStates;
allStates.push_back(computeRectangleContact(fullBody_, firstState));
std::vector<std::string> variations =thirdState.contactVariations(firstState);
if(variations.size() >1)
{
throw std::runtime_error ("too many state variation between states" + std::string(""+cId) +
"and " + std::string(""+(cId + 1)));
}
if(!variations.empty())
{
std::cout << "variation " << variations[0] << std::endl;
State intermediary(firstState);
intermediary.RemoveContact(*variations.begin());
allStates.push_back(computeRectangleContact(fullBody_, intermediary));
}
allStates.push_back(computeRectangleContact(fullBody_, thirdState));
hpp::floatSeqSeq *res;
res = new hpp::floatSeqSeq ();
// compute array of contact positions
res->length ((_CORBA_ULong)allStates.size());
std::size_t i=0;
for(std::vector<std::vector<fcl::Vec3f> >::const_iterator cit = allStates.begin();
cit != allStates.end(); ++cit, ++i)
{
const std::vector<fcl::Vec3f>& positions = *cit;
_CORBA_ULong size = (_CORBA_ULong) positions.size () * 3;
double* dofArray = hpp::floatSeq::allocbuf(size);
hpp::floatSeq floats (size, size, dofArray, true);
//convert the config in dofseq
for(std::size_t h = 0; h<positions.size(); ++h)
{
for(std::size_t k =0; k<3; ++k)
{
model::size_type j (h*3 + k);
dofArray[j] = positions[h][k];
}
}
(*res) [(_CORBA_ULong)i] = floats;
}
return res;
}
floatSeqSeq* RbprmBuilder::interpolate(double timestep, double path, double robustnessTreshold) throw (hpp::Error)
{
try
......
......@@ -139,6 +139,7 @@ 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 void interpolateBetweenStates(double state1, double state2, unsigned short numOptimizations) 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