Commit 71c7044f authored by stevet's avatar stevet
Browse files

generate one contact

parent 8c11ad1d
......@@ -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
......
......@@ -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 ..."
......
#!/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'
......@@ -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
......
......@@ -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
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)
......@@ -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,
......
......@@ -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);
......
Markdown is supported
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