From 71c7044ffef6a15b924825f2317bfd8d76c7b234 Mon Sep 17 00:00:00 2001
From: stevet <stevetonneau@gotmail.fr>
Date: Fri, 28 Jun 2019 18:33:55 +0100
Subject: [PATCH] generate one contact
---
idl/hpp/corbaserver/rbprm/rbprmbuilder.idl | 7 ++
.../sandbox/ANYmal/anymal_flatGround.py | 6 +-
script/scenarios/sandbox/run.sh | 4 +-
src/hpp/corbaserver/rbprm/rbprmfullbody.py | 6 +-
src/hpp/corbaserver/rbprm/rbprmstate.py | 13 ++++
.../rbprm/tools/affordance_centroids.py | 67 ++++++++++++++++
src/rbprmbuilder.impl.cc | 77 +++++++++++++++++++
src/rbprmbuilder.impl.hh | 3 +
8 files changed, 175 insertions(+), 8 deletions(-)
create mode 100644 src/hpp/corbaserver/rbprm/tools/affordance_centroids.py
diff --git a/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl b/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
index 3c0dfae5..bd6ce9e9 100644
--- a/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
+++ b/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
@@ -255,6 +255,13 @@ module hpp
floatSeqSeq getContactSamplesProjected(in string name, in floatSeq dofArray, in floatSeq direction,
in unsigned short numSamples) raises (Error);
+ /// Given a contact state and a limb, tries to generate a new contact, and returns the id of the new State if successful
+ /// \param currentState Id of the considered state
+ /// \param name name of the limb used to create a contact
+ /// \param direction desired direction of motion for the robot
+ /// \return transformed configuration for which all possible contacts have been created
+ short generateContactState(in unsigned short currentState, in string name, in floatSeq direction) raises (Error);
+
/// get Ids of limb in an octree cell
/// \param name name of the considered limb
/// \param octreeNodeId considered configuration of the robot
diff --git a/script/scenarios/sandbox/ANYmal/anymal_flatGround.py b/script/scenarios/sandbox/ANYmal/anymal_flatGround.py
index 1cb8c56c..defa4a33 100644
--- a/script/scenarios/sandbox/ANYmal/anymal_flatGround.py
+++ b/script/scenarios/sandbox/ANYmal/anymal_flatGround.py
@@ -75,10 +75,10 @@ v(q_goal)
v.addLandmark('anymal/base',0.3)
v(q_init)
-
+z = [0.,0.,1]
# specify the full body configurations as start and goal state of the problem
-fullBody.setStartState(q_init,fullBody.limbs_names)
-fullBody.setEndState(q_goal,fullBody.limbs_names)
+fullBody.setStartState(q_init,fullBody.limbs_names, [z for _ in range(4)])
+fullBody.setEndState(q_goal,fullBody.limbs_names, [z for _ in range(4)])
print "Generate contact plan ..."
diff --git a/script/scenarios/sandbox/run.sh b/script/scenarios/sandbox/run.sh
index c249455b..6c19fae8 100755
--- a/script/scenarios/sandbox/run.sh
+++ b/script/scenarios/sandbox/run.sh
@@ -1,8 +1,8 @@
#!/bin/bash
-gepetto-viewer-server &
+gepetto-gui &
hpp-rbprm-server &
ipython -i --no-confirm-exit ./$1
-pkill -f 'gepetto-viewer-server'
+pkill -f 'gepetto-gui'
pkill -f 'hpp-rbprm-server'
diff --git a/src/hpp/corbaserver/rbprm/rbprmfullbody.py b/src/hpp/corbaserver/rbprm/rbprmfullbody.py
index 07e43a6a..6b955495 100755
--- a/src/hpp/corbaserver/rbprm/rbprmfullbody.py
+++ b/src/hpp/corbaserver/rbprm/rbprmfullbody.py
@@ -224,7 +224,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)
+ cl.addNewContact(sId, limbName, p, normal, num_max_sample, False)
return sId
## Retrieves the contact candidates configurations given a configuration and a limb
@@ -285,7 +285,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)
@@ -303,7 +303,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
diff --git a/src/hpp/corbaserver/rbprm/rbprmstate.py b/src/hpp/corbaserver/rbprm/rbprmstate.py
index b13c74c6..6c8dce9f 100644
--- a/src/hpp/corbaserver/rbprm/rbprmstate.py
+++ b/src/hpp/corbaserver/rbprm/rbprmstate.py
@@ -202,3 +202,16 @@ class StateHelper(object):
sId = fullBody.generateStateInContact(q,direction,acceleration)
s = State(fullBody,sId=sId)
return s
+
+ @staticmethod
+ def moveAndContact(state,rootOffset,limbName):
+ s, success = StateHelper.removeContact(state, limbName)
+ assert success
+ success = s.projectToRoot((array(s.q()[:3]) + array(rootOffset)).tolist()+s.q()[3:7])
+ if not success:
+ return state, False
+ sId = s.fullBody.clientRbprm.rbprm.generateContactState(s.sId, limbName,rootOffset)
+ if sId < 0:
+ return state, False
+ s = State(s.fullBody,sId=sId)
+ return s, True
diff --git a/src/hpp/corbaserver/rbprm/tools/affordance_centroids.py b/src/hpp/corbaserver/rbprm/tools/affordance_centroids.py
new file mode 100644
index 00000000..7b887559
--- /dev/null
+++ b/src/hpp/corbaserver/rbprm/tools/affordance_centroids.py
@@ -0,0 +1,67 @@
+from numpy import array, cross
+from numpy.linalg import norm
+
+
+## tools to retrive obstacles of a given affordance, indexed by the centroid of their surface.
+## method to import is computeAffordanceCentroids
+
+def flat(pts):
+ return [item for sublist in pts for item in sublist]
+
+__EPS = 1e-5
+
+def __filter_points(points):
+ res = []
+ for el in points:
+ el_arr = array(el)
+ add = True
+ for al in res:
+ if(norm(al - el_arr) < __EPS):
+ add = False
+ break
+ if add:
+ res += [array(el)]
+ return res
+
+def __normal(points):
+ p1 = array(points[0])
+ p2 = array(points[1])
+ p3 = array(points[2])
+ normal = cross((p2 - p1),(p3 - p1))
+ normal /= norm(normal)
+ return normal.tolist()
+
+def __centroid(points):
+ return sum(points) / len(points)
+
+def __centroid_list(list_points):
+ return [[__centroid(__filter_points(flat(pts))).tolist(), __normal(pts[0]) ] for pts in list_points]
+
+def computeAffordanceCentroids(afftool, affordances=["Support","Lean"]):
+ all_points = []
+ for _, el in enumerate(affordances):
+ all_points += afftool.getAffordancePoints(el)
+ return __centroid_list(all_points)
+
+b_id = 0
+
+def draw_centroid(gui, winId, pt, scene="n_name", color = [1,1,1,0.3]):
+ p = pt[0]
+ n = array(pt[0]) + 0.03 * array(pt[1])
+ resolution = 0.01
+ global b_id
+ boxname = scene+"/"+str(b_id)
+ boxnameN = scene+"/"+str(b_id)+"n"
+ b_id += 1
+ gui.addBox(boxname,resolution,resolution,resolution, color)
+ gui.addBox(boxnameN,resolution,resolution,resolution, color)
+ gui.applyConfiguration(boxname,[p[0],p[1],p[2],1,0,0,0])
+ gui.applyConfiguration(boxnameN,[n[0],n[1],n[2],1,0,0,0])
+ gui.addSceneToWindow(scene,winId)
+ gui.refresh()
+
+def draw_centroids(gui, winId, pts_lists, scene="n_name", color = [1,0,0,1]):
+ gui.createScene(scene)
+ for _, pt in enumerate(pts_lists):
+ draw_centroid(gui, winId, pt, scene=scene, color = color)
+
diff --git a/src/rbprmbuilder.impl.cc b/src/rbprmbuilder.impl.cc
index 238e9500..ac4dc272 100644
--- a/src/rbprmbuilder.impl.cc
+++ b/src/rbprmbuilder.impl.cc
@@ -1065,6 +1065,83 @@ namespace hpp {
}
}
+ short RbprmBuilder::generateContactState(::CORBA::UShort cId, const char* name, const ::hpp::floatSeq& direction)
+ throw (hpp::Error)
+ {
+ try
+ {
+ if(lastStatesComputed_.size() <= (std::size_t)(cId))
+ {
+ throw std::runtime_error ("Unexisting state " + std::string(""+(cId+1)));
+ }
+ State& state = lastStatesComputed_[cId];
+ std::string limbname(name);
+ fcl::Vec3f dir;
+ for(std::size_t i =0; i <3; ++i)
+ {
+ dir[i] = direction[(_CORBA_ULong)i];
+ }
+ pinocchio::Configuration_t config = state.configuration_;
+ fullBody()->device_->currentConfiguration(config);
+
+ sampling::T_OctreeReport finalSet;
+ rbprm::T_Limb::const_iterator lit = fullBody()->GetLimbs().find(limbname);
+ if(lit == fullBody()->GetLimbs().end())
+ {
+ throw std::runtime_error ("Impossible to find limb for joint "
+ + std::string(limbname) + " to robot; limb not defined.");
+ }
+ const RbPrmLimbPtr_t& limb = lit->second;
+ pinocchio::Transform3f transformPino = limb->octreeRoot(); // get root transform from configuration
+ fcl::Transform3f transform(transformPino.rotation(),transformPino.translation());
+ // TODO fix as in rbprm-fullbody.cc!!
+ const affMap_t &affMap = problemSolver()->affordanceObjects;
+ if (affMap.map.empty ())
+ {
+ throw hpp::Error ("No affordances found. Unable to interpolate.");
+ }
+ const std::vector<pinocchio::CollisionObjectPtr_t> objects = contact::getAffObjectsForLimb(std::string(limbname), affMap,
+ bindShooter_.affFilter_);
+
+ std::vector<sampling::T_OctreeReport> reports(objects.size());
+ std::size_t i (0);
+ //#pragma omp parallel for
+ for(std::vector<pinocchio::CollisionObjectPtr_t>::const_iterator oit = objects.begin();
+ oit != objects.end(); ++oit, ++i)
+ {
+ sampling::GetCandidates(limb->sampleContainer_, transform, *oit, dir, reports[i], sampling::HeuristicParam());
+ }
+ for(std::vector<sampling::T_OctreeReport>::const_iterator cit = reports.begin();
+ cit != reports.end(); ++cit)
+ {
+ finalSet.insert(cit->begin(), cit->end());
+ }
+ // randomize samples
+ std::random_shuffle(reports.begin(), reports.end());
+ unsigned short num_samples_ok (0);
+ pinocchio::Configuration_t sampleConfig = config;
+ sampling::T_OctreeReport::const_iterator candCit = finalSet.begin();
+ for(std::size_t i=0; i< _CORBA_ULong(finalSet.size()) && num_samples_ok < 100; ++i, ++candCit)
+ {
+ const sampling::OctreeReport& report = *candCit;
+ State state;
+ state.configuration_ = config;
+ hpp::rbprm::projection::ProjectionReport rep =
+ hpp::rbprm::projection::projectSampleToObstacle(fullBody(),std::string(limbname), limb, report, fullBody()->GetCollisionValidation(), sampleConfig, state);
+ if(rep.success_)
+ {
+
+ lastStatesComputed_.push_back(rep.result_);
+ lastStatesComputedTime_.push_back(std::make_pair(-1., rep.result_));
+ return lastStatesComputed_.size() -1;
+ }
+ }
+ return -1;
+ } catch (const std::exception& exc) {
+ throw hpp::Error (exc.what ());
+ }
+ }
+
hpp::floatSeqSeq* RbprmBuilder::getContactSamplesProjected(const char* limbname,
const hpp::floatSeq& configuration,
const hpp::floatSeq& direction,
diff --git a/src/rbprmbuilder.impl.hh b/src/rbprmbuilder.impl.hh
index ab8639b2..b7f07336 100644
--- a/src/rbprmbuilder.impl.hh
+++ b/src/rbprmbuilder.impl.hh
@@ -237,6 +237,9 @@ namespace hpp {
const hpp::floatSeq& direction,
unsigned short numSamples) throw (hpp::Error);
+ virtual short generateContactState(::CORBA::UShort currentState, const char* name, const ::hpp::floatSeq& direction) throw (hpp::Error);
+
+
virtual hpp::floatSeq* getSamplesIdsInOctreeNode(const char* limb,
double octreeNodeId) throw (hpp::Error);
--
GitLab