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

trajectory exporter for blender functional

parent 73e4eec2
......@@ -149,6 +149,11 @@ module hpp
/// \return transformed configuration for which all possible contacts have been created
floatSeq generateContacts(in floatSeq dofArray, in floatSeq direction) raises (Error);
/// Generate an autocollision free configuration with limbs in contact with the ground
/// \param contactLimbs name of the limbs to project in contact
/// \return a sampled contact configuration
floatSeq generateGroundContact(in Names_t contactLimbs) raises (Error);
/// Given a configuration and a limb, returns the id of all samples potentially in contact with the
/// environment, ordered by their efficiency
/// \param name name of the considered limb
......@@ -303,7 +308,7 @@ module hpp
/// \param rootPositions1 com positions to track for 3rd phase
/// \param numOptimizations Number of iterations of the shortcut algorithm to apply between each states
/// \return id of the root path computed
short comRRTFromPos(in double state1,
floatSeq comRRTFromPos(in double state1,
in floatSeqSeq rootPositions1,
in floatSeqSeq rootPositions2,
in floatSeqSeq rootPositions3,
......
......@@ -19,6 +19,7 @@ srdfSuffix = ""
# This time we load the full body model of HyQ
fullBody = FullBody ()
fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
fullBody.setJointBounds ("base_joint_xyz", [-2,5, -1, 1, 0.3, 4])
# Setting a number of sample configurations used
nbSamples = 20000
......@@ -80,7 +81,7 @@ fullBody.setEndState(q_goal,[rLegId,lLegId,rarmId,larmId])
r(q_init)
# computing the contact sequence
configs = fullBody.interpolate(0.1, 1, 0)
configs = fullBody.interpolate(0.1, 1, 5)
#~ r.loadObstacleModel ('hpp-rbprm-corba', "darpa", "contact")
......@@ -93,39 +94,7 @@ pp = PathPlayer (ps.robot.client.basic, r)
from hpp.corbaserver.rbprm.tools.cwc_trajectory import *
pid = 3
def optimize_animate(i):
res = draw_trajectory(fullBody, configs, i, False, 0.5, False)
#~ res = gen_trajectory(fullBody, configs, i, False, 1, False)
pos = [c.tolist() for c in res[0]['c']]
fullBody.generateRootPathStates(pos, configs[i], configs[i+1])
fullBody.interpolateBetweenStatesFromPath(i,i+1,2,0)
fullBody.interpolateBetweenStatesFromPath(i,i+1,2,1)
fullBody.interpolateBetweenStatesFromPath(i,i+1,2,10)
fullBody.interpolateBetweenStatesFromPath(i,i+1,2,50)
global pid
#~ pp(pid);pp(pid+1);pp(pid+2);
pid = pid + 3
print "diff position"
from numpy import array
#~ print (array(pos[-1]) - array(configs[6][0:3]))
#~ print (array(pos[-1]) - array(configs[6][0:3])).norm()
def play(i):
fullBody.setCurrentConfig(configs[i])
r(configs[i])
import time
for j,_ in enumerate (pos):
q=fullBody.getCurrentConfig()
q[0:3] = pos[j]
r(q)
time.sleep(0.25)
q[3:] = configs[i+1][3:]
r(q)
res = []
from hpp.corbaserver.rbprm.tools.path_to_trajectory import *
def displayComPath(pathId,color=[0.,0.75,0.15,0.9]) :
pathPos=[]
......@@ -143,33 +112,35 @@ def displayComPath(pathId,color=[0.,0.75,0.15,0.9]) :
pp.publisher.client.gui.refresh()
res = []
trajec = []
from hpp import Error as hpperr
import sys
numerror = 0
def act(i, optim):
global numerror
try:
pid = solve_com_RRT(fullBody, configs, i, True, 0.3, 0.2, False, optim, False, True)
pid, trajectory = solve_com_RRT(fullBody, configs, i, True, 0.6, 0.2, False, optim, False, True)
displayComPath(pid)
#~ pp(pid)
global res
res = res + [pid]
global trajec
trajec = trajec + gen_trajectory_to_play(fullBody, pp, trajectory, 1)
except hpperr as e:
print "failed at id " + str(i) , e.strerror
numerror+=1
except ValueError as e:
print "failed at id " + str(i) , e
numerror+=1
except IndexError as e:
print "failed at id " + str(i) , e
numerror+=1
except Exception as e:
print e
numerror+=1
except:
return
for i in range(30,47):
act(i, 50)
#~ for i in range(5,35):
#~ act(i, 50)
def displayInSave(pp, pathId, configs):
length = pp.end*pp.client.problem.pathLength (pathId)
t = pp.start*pp.client.problem.pathLength (pathId)
......@@ -178,15 +149,41 @@ def displayInSave(pp, pathId, configs):
configs.append(q)
t += (pp.dt * pp.speed)
#~ fullBody.exportAll(r, respath, 'darpa_hyq_full');
for i in range(11,20):
act(i, 60)
#~ if i % 10 == 0:
#~ respath = []
#~ for p in res:
#~ print p
#~ displayInSave(pp,p, respath)
#~ fullBody.exportAll(r, respath, 'darpa_hyq_full');
pp.setSpeed(2)
for p in res:
pp(p)
import time
respath = []
for p in res:
print p
displayInSave(pp,p, respath)
#~ displayInSave(pp,p, respath)
#~ fullBody.exportAll(r, respath, 'darpa_hyq_full'+str(time()))
#~ for i in range(5,35):
#~ act(i, 50)
for p in res:
pp(p)
fullBody.exportAll(r, respath, 'darpa_hyq_full');
#~ from hpp.gepetto.blender.exportmotion import exportPath
#~ for p in res:
......
......@@ -160,6 +160,12 @@ class FullBody (object):
def generateContacts(self, configuration, direction):
return self.client.rbprm.rbprm.generateContacts(configuration, direction)
## Generate an autocollision free configuration with limbs in contact with the ground
# \param contactLimbs name of the limbs to project in contact
# \return a sampled contact configuration
def generateGroundContact(self, contactLimbs):
return self.client.rbprm.rbprm.generateGroundContact(contactLimbs)
## Retrieves the contact candidates configurations given a configuration and a limb
#
# \param name id of the limb considered
......
......@@ -4,6 +4,8 @@ import numpy as np
import math
import hpp.corbaserver.rbprm.data.com_inequalities as ine
from hpp.corbaserver.rbprm.tools.path_to_trajectory import gen_trajectory_to_play
ineqPath = ine.__path__[0] +"/"
# epsilon for testing whether a number is close to zero
......@@ -39,9 +41,9 @@ def __get_com_constraint(fullBody, state, config, limbsCOMConstraints, interm =
for i, v in limbsCOMConstraints.iteritems():
if not constraintsComLoaded.has_key(i):
constraintsComLoaded[i] = ineq_from_file(ineqPath+v['file'])
print "inter", interm
print "intermed", fullBody.isLimbInContactIntermediary(i, state)
print "inter", fullBody.isLimbInContact(i, state)
#~ print "inter", interm
#~ print "intermed", fullBody.isLimbInContactIntermediary(i, state)
#~ print "inter", fullBody.isLimbInContact(i, state)
contact = (interm and fullBody.isLimbInContactIntermediary(i, state)) or (not interm and fullBody.isLimbInContact(i, state))
if contact:
ineq = constraintsComLoaded[i]
......@@ -50,9 +52,9 @@ def __get_com_constraint(fullBody, state, config, limbsCOMConstraints, interm =
tr[:3,3] = np.array(qEffector[0:3])
ineq_r = rotate_inequalities(ineq, tr)
As.append(ineq_r.A); bs.append(ineq_r.b);
print 'contact', v['effector']
#~ print 'contact', v['effector']
contacts.append(v['effector'])
print 'contacts', contacts
#~ print 'contacts', contacts
return [np.vstack(As), np.hstack(bs)]
......@@ -74,13 +76,13 @@ def gen_trajectory(fullBody, states, state_id, computeCones = False, mu = 1, dt=
COMConstraints = None
if(not (limbsCOMConstraints == None)):
print "retrieving COM constraints"
#~ print "retrieving COM constraints"
COMConstraints = [__get_com_constraint(fullBody, state_id, states[state_id], limbsCOMConstraints)]
if(len(p) > 2):
COMConstraints.append(__get_com_constraint(fullBody, state_id, states[state_id], limbsCOMConstraints, True))
if(len(p) > len(COMConstraints)):
COMConstraints.append(__get_com_constraint(fullBody, state_id + 1, states[state_id + 1], limbsCOMConstraints))
print "num com constraints", len(COMConstraints)
#~ print "num com constraints", len(COMConstraints)
return cone_optimization(p, N, [init_com + [0,0,0], end_com + [0,0,0]], t_end_phases[1:], dt, cones, COMConstraints, mu, mass, 9.81, reduce_ineq, verbose)
......@@ -129,9 +131,8 @@ def solve_com_RRT(fullBody, states, state_id, computeCones = False, mu = 1, dt =
comPosPerPhase[-1].append(comPos[-1])
assert(len(comPos) == len(comPosPerPhase[0]) + len(comPosPerPhase[1]) + len(comPosPerPhase[2]))
#~ assert(comPos == [item for sublist in comPosPerPhase for item in sublist])
print "done. generating state path ",state_id
return fullBody.comRRTFromPos(state_id,comPosPerPhase[0],comPosPerPhase[1],comPosPerPhase[2],num_optims)
print "done. generating state trajectory ",state_id
paths_ids = [int(el) for el in fullBody.comRRTFromPos(state_id,comPosPerPhase[0],comPosPerPhase[1],comPosPerPhase[2],num_optims)]
print "done. computing final trajectory to display ",state_id
return paths_ids[-1], paths_ids[:-1]
__24fps = 1. / 24.
__EPS = 0.1
__EPS = 0.0001
from numpy.linalg import norm
def __linear_interpolation(p0,p1,dist_p0_p1, val):
......@@ -17,43 +17,27 @@ def __gen_frame_positions(com_waypoints, dt, dt_framerate=__24fps):
def __find_q_t(robot, path_player, path_id, t):
u = t
current_t = -1
pp = path_player
length = pp.client.problem.pathLength (path_id)
u = min(t, length)
q = []
a = 0.
b = length
#~ print [pp.client.problem.configAtParam (path_id, i)[-1] for i in [length / 5. *j for j in range(6)]]
while(abs(current_t -t )> __EPS):
current_t = pp.client.problem.configAtParam (path_id, u)[-1]
print "current_t",current_t
print "t",t
if(current_t - t) > __EPS:
print "upb",b
b = u
print "upb",b
elif (t - current_t) > __EPS:
print "upa",a
while(True):
q = pp.client.problem.configAtParam (path_id, u)
current_t = q[-1]
if(a >= b):
print "ERROR, a > b, t does not exist"
if abs(current_t - t) < __EPS:
return q[:-1]
elif(current_t - t) < __EPS:
a = u
print "upa",a
if (pp.client.problem.configAtParam (path_id, a)[-1]) > t and (pp.client.problem.configAtParam (path_id, b)[-1] > t):
print "error, tout au dessus!!!!!!!!!!!!!!!"
return 0
if (pp.client.problem.configAtParam (path_id, a)[-1]) < t and (pp.client.problem.configAtParam (path_id, b)[-1] < t):
print "error, tout en dessous!!!!!!!!!!!!!!!"
return 0
#~ print "cassos"
print "current_l", u
u = (b-a)/2.
print "current_l", u
#~ return 0
#~ print "current_t", current_t
#~ print "t", t
#~ print "a", a
#~ print "b", b
#~ print "length", length
print "gagne"
return q[:-1]
elif (current_t - t) > __EPS:
b = u
u = (b+a)/2.
return u
def linear_interpolate_path(robot, path_player, path_id, total_time, dt_framerate=__24fps):
pp = path_player
......@@ -72,24 +56,32 @@ def follow_trajectory_path(robot, path_player, path_id, total_time, dt_framerate
dt_finals = [dt*i / length for i in range(int(num_frames_required))] + [1]
return[__find_q_t(robot, path_player, path_id, t) for t in dt_finals]
#~ def playCOM(pathId):
#~ length = pp.end*pp.client.problem.pathLength (pathId)
#~ t = pp.start*pp.client.problem.pathLength (pathId)
#~ while t < length :
#~ start = time.time()
#~ q = pp.client.problem.configAtParam (pathId, t)
#~ fullBody.setCurrentConfig(q)
#~ q[0:3] = fullBody.getCenterOfMass()
#~ pp.publisher.robotConfig = q
#~ pp.publisher.publishRobots ()
#~ t += (pp.dt * pp.speed)
#~ elapsed = time.time() - pp.start
#~ if elapsed < pp.dt :
#~ time.sleep(pp.dt-elapsed))
def gen_trajectory_to_play(robot, path_player, path_ids, total_time_per_path, dt_framerate=__24fps):
config_size = len(robot.getCurrentConfig())
res = []
pp = path_player
for path_id in path_ids:
config_size_path = len(path_player.client.problem.configAtParam (path_id, 0))
if(config_size_path > config_size):
res+= follow_trajectory_path(robot, path_player, path_id, total_time_per_path, dt_framerate)
else:
res+= linear_interpolate_path(robot, path_player, path_id, total_time_per_path, dt_framerate)
return res
import time
def play_trajectory(fullBody, path_player, configs, dt_framerate=__24fps):
for q in configs:
start = time.time()
path_player.publisher.robotConfig = q
path_player.publisher.publishRobots ()
elapsed = time.time() - start
if elapsed < dt_framerate :
time.sleep(dt_framerate-elapsed)
import numpy as np
com_waypoints = [np.array([i,i,i]) for i in range(6)]
dt = 0.2
dt_framerate = __24fps
res = __gen_frame_positions(com_waypoints, dt, dt_framerate)
#~ import numpy as np
#~ com_waypoints = [np.array([i,i,i]) for i in range(6)]
#~ dt = 0.2
#~ dt_framerate = __24fps
#~
#~ res = __gen_frame_positions(com_waypoints, dt, dt_framerate)
......@@ -28,9 +28,12 @@
#include "hpp/rbprm/sampling/sample-db.hh"
#include "hpp/model/urdf/util.hh"
#include "hpp/core/straight-path.hh"
#include "hpp/model/joint.hh"
#include "hpp/core/config-validations.hh"
#include "hpp/core/collision-validation-report.hh"
#include <hpp/core/subchain-path.hh>
#include <hpp/core/basic-configuration-shooter.hh>
#include <hpp/core/collision-validation.hh>
#include <fstream>
......@@ -374,6 +377,84 @@ namespace hpp {
}
}
hpp::floatSeq* RbprmBuilder::generateGroundContact(const hpp::Names_t& contactLimbs) throw (hpp::Error)
{
if(!fullBodyLoaded_)
throw Error ("No full body robot was loaded");
try
{
fcl::Vec3f z(0,0,1);
ValidationReportPtr_t report = ValidationReportPtr_t(new CollisionValidationReport);
core::BasicConfigurationShooterPtr_t shooter = core::BasicConfigurationShooter::create(fullBody_->device_);
for(int i =0; i< 1000; ++i)
{
std::vector<std::string> names = stringConversion(contactLimbs);
core::ConfigProjectorPtr_t proj = core::ConfigProjector::create(fullBody_->device_,"proj", 1e-4, 1000);
//hpp::tools::LockJointRec(limb->limb_->name(), body->device_->rootJoint(), proj);
for(std::vector<std::string>::const_iterator cit = names.begin(); cit !=names.end(); ++cit)
{
rbprm::RbPrmLimbPtr_t limb = fullBody_->GetLimbs().at(*cit);
fcl::Transform3f localFrame, globalFrame;
std::vector<bool> posConstraints;
std::vector<bool> rotConstraints;
posConstraints.push_back(false);posConstraints.push_back(false);posConstraints.push_back(true);
rotConstraints.push_back(true);rotConstraints.push_back(true);rotConstraints.push_back(true);
proj->add(core::NumericalConstraint::create (constraints::Position::create("",fullBody_->device_,
limb->effector_,
globalFrame,
localFrame,
posConstraints)));
if(limb->contactType_ == hpp::rbprm::_6_DOF)
{
// rotation matrix around z
value_type theta = 2*(value_type(rand()) / value_type(RAND_MAX) - 0.5) * M_PI;
fcl::Matrix3f r = tools::GetZRotMatrix(theta);
fcl::Matrix3f rotation = r * limb->effector_->initialPosition ().getRotation();
proj->add(core::NumericalConstraint::create (constraints::Orientation::create("",fullBody_->device_,
limb->effector_,
fcl::Transform3f(rotation),
rotConstraints)));
}
}
ConfigurationPtr_t configptr = shooter->shoot();
Configuration_t config = *configptr;
if(proj->apply(config) && config[2]> 0.3)
{
if(problemSolver_->problem()->configValidations()->validate(config,report))
{
State tmp;
for(std::vector<std::string>::const_iterator cit = names.begin(); cit !=names.end(); ++cit)
{
std::string limbId = *cit;
rbprm::RbPrmLimbPtr_t limb = fullBody_->GetLimbs().at(*cit);
tmp.contacts_[limbId] = true;
tmp.contactPositions_[limbId] = limb->effector_->currentTransformation().getTranslation();
tmp.contactRotation_[limbId] = limb->effector_->currentTransformation().getRotation();
tmp.contactNormals_[limbId] = z;
tmp.configuration_ = config;
++tmp.nbContacts;
}
if(stability::IsStable(fullBody_,tmp)>=0)
{
config[0]=0;
config[1]=0;
hpp::floatSeq* dofArray = new hpp::floatSeq();
dofArray->length(_CORBA_ULong(config.rows()));
for(std::size_t j=0; j< config.rows(); j++)
{
(*dofArray)[(_CORBA_ULong)j] = config [j];
}
return dofArray;
}
}
}
}
throw (std::runtime_error("could not generate contact configuration after 1000 trials"));
} catch (const std::exception& exc) {
throw hpp::Error (exc.what ());
}
}
hpp::floatSeq* RbprmBuilder::getContactSamplesIds(const char* limbname,
const hpp::floatSeq& configuration,
const hpp::floatSeq& direction) throw (hpp::Error)
......@@ -400,7 +481,7 @@ namespace hpp {
}
const RbPrmLimbPtr_t& limb = lit->second;
fcl::Transform3f transform = limb->limb_->robot()->rootJoint()->childJoint(0)->currentTransformation (); // get root transform from configuration
// TODO fix as in rbprm-fullbody.cc!!
// TODO fix as in rbprm-fullbody.cc!!
std::vector<sampling::T_OctreeReport> reports(problemSolver_->collisionObstacles().size());
std::size_t i (0);
//#pragma omp parallel for
......@@ -1028,7 +1109,7 @@ assert(s2 == s1 +1);
}
}
CORBA::Short RbprmBuilder::comRRTFromPos(double state1,
hpp::floatSeq* RbprmBuilder::comRRTFromPos(double state1,
const hpp::floatSeqSeq& rootPositions1,
const hpp::floatSeqSeq& rootPositions2,
const hpp::floatSeqSeq& rootPositions3,
......@@ -1036,6 +1117,7 @@ assert(s2 == s1 +1);
{
try
{
std::vector<CORBA::Short> pathsIds;
std::size_t s1((std::size_t)state1), s2((std::size_t)state1+1);
if(lastStatesComputed_.size () < s1 || lastStatesComputed_.size () < s2 )
{
......@@ -1067,7 +1149,6 @@ assert(s2 == s1 +1);
states.push_back(s2Ter);
states.push_back(state2);
std::size_t i =0;
core::PathVectorPtr_t resPath = core::PathVector::create(fullBody_->device_->configSize(), fullBody_->device_->numberDof());
/*for(std::vector<State>::const_iterator cit = states.begin(); cit != states.end();cit=cit+2, ++i)
{
......@@ -1085,7 +1166,6 @@ assert(s2 == s1 +1);
//return -1; //limbRRT(s1, s2, numOptimizations);
}
core::WeighedDistancePtr_t distance = core::WeighedDistance::create(fullBody_->device_);
/*resPath->appendPath(core::StraightPath::create(fullBody_->device_,state1.configuration_,s1Bis.configuration_,
(*distance)(state1.configuration_,s1Bis.configuration_)));
resPath->appendPath(core::StraightPath::create(fullBody_->device_,s1Bis.configuration_,s1Ter.configuration_,
......@@ -1096,12 +1176,12 @@ assert(s2 == s1 +1);
core::PathPtr_t p1 = interpolation::comRRT(fullBody_,problemSolver_->problem(), paths[0],
state1,s1Ter, numOptimizations,false);
resPath->appendPath(p1);
AddPath(p1,problemSolver_);
pathsIds.push_back(AddPath(p1,problemSolver_));
}
core::PathPtr_t p2 =interpolation::comRRT(fullBody_,problemSolver_->problem(), paths[1],
s1Ter,s2Bis, numOptimizations,true);
AddPath(p2,problemSolver_);
pathsIds.push_back(AddPath(p2,problemSolver_));
// reduce path to remove extradof
core::SizeInterval_t interval(0, p2->initial().rows()-1);
core::SizeIntervals_t intervals;
......@@ -1118,14 +1198,21 @@ assert(s2 == s1 +1);
core::PathPtr_t p3= interpolation::comRRT(fullBody_,problemSolver_->problem(), paths[1],
s2Bis,state2, numOptimizations,false);
resPath->appendPath(p3);
AddPath(p3,problemSolver_);
pathsIds.push_back(AddPath(p3,problemSolver_));
}
/*T_State tg; tg.push_back(s1Bis);
tg.push_back(s2Bis);
resPath->appendPath(interpolation::limbRRT(fullBody_,problemSolver_->problem(),
tg.begin(),tg.begin()+1, numOptimizations));*/
return AddPath(reducedPath,problemSolver_);
pathsIds.push_back(AddPath(resPath,problemSolver_));
hpp::floatSeq* dofArray = new hpp::floatSeq();
dofArray->length(pathsIds.size());
for(std::size_t i=0; i< pathsIds.size(); ++i)
{
(*dofArray)[(_CORBA_ULong)i] = pathsIds[i];
}
return dofArray;
}
catch(std::runtime_error& e)
{
......
......@@ -124,6 +124,8 @@ namespace hpp {
virtual hpp::floatSeq* generateContacts(const hpp::floatSeq& configuration,
const hpp::floatSeq& direction) throw (hpp::Error);
virtual hpp::floatSeq* generateGroundContact(const hpp::Names_t& contactLimbs) throw (hpp::Error);
virtual hpp::floatSeq* getContactSamplesIds(const char* limb,
const hpp::floatSeq& configuration,
const hpp::floatSeq& direction) throw (hpp::Error);
......@@ -149,7 +151,7 @@ namespace hpp {
virtual CORBA::Short limbRRT(double state1, double state2, unsigned short numOptimizations) throw (hpp::Error);
virtual CORBA::Short limbRRTFromRootPath(double state1, double state2, unsigned short path, unsigned short numOptimizations) throw (hpp::Error);
virtual CORBA::Short comRRT(double state1, double state2, unsigned short path, unsigned short numOptimizations) throw (hpp::Error);
virtual CORBA::Short comRRTFromPos(double state1,
virtual hpp::floatSeq* comRRTFromPos(double state1,
const hpp::floatSeqSeq& rootPositions1,
const hpp::floatSeqSeq& rootPositions2,
const hpp::floatSeqSeq& rootPositions3,
......
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