From fdc63ef65e1a3970885ceab9ee1bf06b71e76c9a Mon Sep 17 00:00:00 2001
From: Steve Tonneau <stonneau@axle.laas.fr>
Date: Wed, 3 Aug 2016 23:03:44 +0200
Subject: [PATCH] early attempt at computing contact points
---
idl/hpp/corbaserver/rbprm/rbprmbuilder.idl | 6 ++
src/hpp/corbaserver/rbprm/rbprmfullbody.py | 10 +++
src/rbprmbuilder.impl.cc | 88 ++++++++++++++++++++++
src/rbprmbuilder.impl.hh | 1 +
4 files changed, 105 insertions(+)
diff --git a/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl b/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
index 8c3d0ed6..0c220798 100755
--- a/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
+++ b/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
@@ -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.
diff --git a/src/hpp/corbaserver/rbprm/rbprmfullbody.py b/src/hpp/corbaserver/rbprm/rbprmfullbody.py
index 58b0b13a..ae3ed9e7 100755
--- a/src/hpp/corbaserver/rbprm/rbprmfullbody.py
+++ b/src/hpp/corbaserver/rbprm/rbprmfullbody.py
@@ -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
diff --git a/src/rbprmbuilder.impl.cc b/src/rbprmbuilder.impl.cc
index dacf398a..fb3d7997 100755
--- a/src/rbprmbuilder.impl.cc
+++ b/src/rbprmbuilder.impl.cc
@@ -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
diff --git a/src/rbprmbuilder.impl.hh b/src/rbprmbuilder.impl.hh
index 62580fe7..593c344e 100755
--- a/src/rbprmbuilder.impl.hh
+++ b/src/rbprmbuilder.impl.hh
@@ -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);
--
GitLab