Commit 39789ced authored by Steve Tonneau's avatar Steve Tonneau
Browse files

try to increase window to stop if com projection fails

parent d5809b31
......@@ -7,6 +7,11 @@ from hpp.gepetto import Viewer
#calling script darpa_hyq_path to compute root path
import darpa_hyq_path as tp
from os import environ
ins_dir = environ['DEVEL_DIR']
db_dir = ins_dir+"/install/share/hyq-rbprm/database/hyq_"
packageName = "hyq_description"
meshPackageName = "hyq_description"
rootJointType = "freeflyer"
......@@ -45,22 +50,35 @@ normal = [0,1,0]
# Specifying the rectangular contact surface length
legx = 0.02; legy = 0.02
# remaining parameters are the chosen heuristic (here, manipulability), and the resolution of the octree (here, 10 cm).
fullBody.addLimb(rLegId,rLeg,rfoot,offset,normal, legx, legy, nbSamples, "manipulability", 0.1, cType)
def addLimbDb(limbId, heuristicName, loadValues = True, disableEffectorCollision = False):
fullBody.addLimbDatabase(str(db_dir+limbId+'.db'), limbId, heuristicName,loadValues, disableEffectorCollision)
lLegId = 'lhleg'
rarmId = 'rhleg'
larmId = 'lfleg'
addLimbDb(rLegId, "static")
addLimbDb(lLegId, "static")
addLimbDb(rarmId, "static")
addLimbDb(larmId, "static")
#~ fullBody.addLimb(rLegId,rLeg,rfoot,offset,normal, legx, legy, nbSamples, "jointlimits", 0.1, cType)
lLegId = 'lhleg'
lLeg = 'lh_haa_joint'
lfoot = 'lh_foot_joint'
fullBody.addLimb(lLegId,lLeg,lfoot,offset,normal, legx, legy, nbSamples, "manipulability", 0.05, cType)
#~ fullBody.addLimb(lLegId,lLeg,lfoot,offset,normal, legx, legy, nbSamples, "jointlimits", 0.05, cType)
#~
rarmId = 'rhleg'
rarm = 'rh_haa_joint'
rHand = 'rh_foot_joint'
fullBody.addLimb(rarmId,rarm,rHand,offset,normal, legx, legy, nbSamples, "manipulability", 0.05, cType)
#~ fullBody.addLimb(rarmId,rarm,rHand,offset,normal, legx, legy, nbSamples, "jointlimits", 0.05, cType)
larmId = 'lfleg'
larm = 'lf_haa_joint'
lHand = 'lf_foot_joint'
fullBody.addLimb(larmId,larm,lHand,offset,normal, legx, legy, nbSamples, "forward", 0.05, cType)
#~ fullBody.addLimb(larmId,larm,lHand,offset,normal, legx, legy, nbSamples, "jointlimits", 0.05, cType)
q_0 = fullBody.getCurrentConfig();
q_init = fullBody.getCurrentConfig(); q_init[0:7] = tp.q_init[0:7]
......@@ -81,7 +99,9 @@ fullBody.setEndState(q_goal,[rLegId,lLegId,rarmId,larmId])
r(q_init)
# computing the contact sequence
configs = fullBody.interpolate(0.12, 1, 10)
configs = fullBody.interpolate(0.12, 10, 10, False)
#~ configs = fullBody.interpolate(0.11, 7, 10, True)
#~ configs = fullBody.interpolate(0.1, 1, 5, True)
#~ r.loadObstacleModel ('hpp-rbprm-corba', "darpa", "contact")
......@@ -89,27 +109,14 @@ configs = fullBody.interpolate(0.12, 1, 10)
i = 0;
fullBody.draw(configs[i],r); i=i+1; i-1
from hpp.gepetto import PathPlayer
pp = PathPlayer (ps.robot.client.basic, r)
from hpp.corbaserver.rbprm.tools.cwc_trajectory import *
from hpp.corbaserver.rbprm.tools.path_to_trajectory import *
def displayComPath(pathId,color=[0.,0.75,0.15,0.9]) :
pathPos=[]
length = pp.end*pp.client.problem.pathLength (pathId)
t = pp.start*pp.client.problem.pathLength (pathId)
while t < length :
q = pp.client.problem.configAtParam (pathId, t)
pp.publisher.robot.setCurrentConfig(q)
q = pp.publisher.robot.getCenterOfMass()
pathPos = pathPos + [q[:3]]
t += pp.dt
nameCurve = "path_"+str(pathId)+"_com"
pp.publisher.client.gui.addCurve(nameCurve,pathPos,color)
pp.publisher.client.gui.addToGroup(nameCurve,pp.publisher.sceneName)
pp.publisher.client.gui.refresh()
pp = PathPlayer (fullBody.client.basic, r)
from hpp.corbaserver.rbprm.tools.cwc_trajectory_helper import step, clean,stats, saveAllData, play_traj
limbsCOMConstraints = { rLegId : {'file': "hyq/"+rLegId+"_com.ineq", 'effector' : rfoot},
lLegId : {'file': "hyq/"+lLegId+"_com.ineq", 'effector' : lfoot},
......@@ -117,240 +124,18 @@ limbsCOMConstraints = { rLegId : {'file': "hyq/"+rLegId+"_com.ineq", 'effector'
larmId : {'file': "hyq/"+larmId+"_com.ineq", 'effector' : lHand} }
def act(i, numOptim = 0, use_window = 0, friction = 0.5, optim_effectors = True, verbose = False, draw = False):
return step(fullBody, configs, i, numOptim, pp, limbsCOMConstraints, 0.4, optim_effectors = optim_effectors, time_scale = 20., useCOMConstraints = False, use_window = use_window,
verbose = verbose, draw = draw)
res = []
trajec = []
trajec_mil = []
contacts = []
pos = []
normals = []
errorid = []
def getContactPerPhase(stateid):
contacts = [[],[],[]]
global limbsCOMConstraints
for k, v in limbsCOMConstraints.iteritems():
if(fullBody.isLimbInContact(k, stateid)):
contacts[0]+=[v['effector']]
if(fullBody.isLimbInContactIntermediary(k, stateid)):
contacts[1]+=[v['effector']]
if(fullBody.isLimbInContact(k, stateid+1)):
contacts[2]+=[v['effector']]
return contacts
def gencontactsPerFrame(stateid, path_ids, times, dt_framerate=1./24.):
contactsPerPhase = getContactPerPhase(stateid)
config_size = len(fullBody.getCurrentConfig())
interpassed = False
res = []
for path_id in path_ids:
length = pp.client.problem.pathLength (path_id)
num_frames_required_fly = times[1] / dt_framerate
num_frames_required_support = times[0] / dt_framerate
dt_fly = float(length) / num_frames_required_fly
dt_support = float(length) / num_frames_required_support
dt_finals_fly = [dt_fly*i for i in range(int(num_frames_required_fly))] + [1]
dt_finals_support = [dt_support*i for i in range(int(num_frames_required_support))] + [1]
config_size_path = len(pp.client.problem.configAtParam (path_id, 0))
if(config_size_path > config_size):
interpassed = True
res+= [contactsPerPhase[1] for t in dt_finals_fly]
elif interpassed:
res+= [contactsPerPhase[2] for t in dt_finals_support]
else:
res+= [contactsPerPhase[0] for t in dt_finals_support]
return res
def genPandNperFrame(stateid, path_ids, times, dt_framerate=1./24.):
p, N= fullBody.computeContactPoints(stateid)
config_size = len(fullBody.getCurrentConfig())
interpassed = False
pRes = []
nRes = []
for path_id in path_ids:
length = pp.client.problem.pathLength (path_id)
num_frames_required_fly = times[1] / dt_framerate
num_frames_required_support = times[0] / dt_framerate
dt_fly = float(length) / num_frames_required_fly
dt_support = float(length) / num_frames_required_support
dt_finals_fly = [dt_fly*i for i in range(int(num_frames_required_fly))] + [1]
dt_finals_support = [dt_support*i for i in range(int(num_frames_required_support))] + [1]
config_size_path = len(pp.client.problem.configAtParam (path_id, 0))
if(config_size_path > config_size):
interpassed = True
pRes+= [p[1] for t in dt_finals_fly]
nRes+= [N[1] for t in dt_finals_fly]
elif interpassed:
pRes+= [p[2] for t in dt_finals_support]
nRes+= [N[2] for t in dt_finals_support]
else:
pRes+= [p[0] for t in dt_finals_support]
nRes+= [N[0] for t in dt_finals_support]
return pRes, nRes
from hpp import Error as hpperr
import sys
numerror = 0
def act(i, optim, time_scale = 20.):
global numerror
global errorid
fail = 0
try:
print "distance", fullBody.getEffectorDistance(i,i+1)
trunk_distance = np.linalg.norm(np.array(configs[i+1][0:3]) - np.array(configs[i][0:3]))
distance = max(fullBody.getEffectorDistance(i,i+1), trunk_distance)
dist = int(distance * time_scale)#heuristic
while(dist %4 != 0):
dist +=1
total_time_flying_path = max(float(dist)/10., 0.3)
total_time_support_path = float((int)(math.ceil(min(total_time_flying_path /2., 0.2)*10.))) / 10.
times = [total_time_support_path, total_time_flying_path]
if(total_time_flying_path>= 1.):
dt = 0.1
elif total_time_flying_path<= 0.3:
dt = 0.05
else:
dt = 0.1
print 'time per path', times
print 'dt', dt
#~ total_time_per_path = 1.2
#~ pid, trajectory = solve_com_RRT(fullBody, configs, i, True, 1, 0.2, total_time_per_path, False, optim, False, False)
if(distance > 0.0001):
#~ pid, trajectory = solve_com_RRT(fullBody, configs, i, True, 0.4, dt, times, False, optim, False, False)
pid, trajectory = solve_effector_RRT(fullBody, configs, i, True, 0.4, dt, times, False, optim, False, False)
displayComPath(pid)
#~ pp(pid)
global res
res = res + [pid]
global trajec
global trajec_mil
trajec = trajec + gen_trajectory_to_play(fullBody, pp, trajectory, times)
trajec_mil = trajec_mil + gen_trajectory_to_play(fullBody, pp, trajectory, times, 1./1000.)
global contacts
contacts += gencontactsPerFrame(i, trajectory, times, 1./1000.)
Ps, Ns = genPandNperFrame(i, trajectory, times, 1./1000.)
global pos
pos += Ps
global normals
normals+= Ns
assert(len(contacts) == len(trajec_mil) and len(contacts) == len(pos) and len(normals) == len(pos))
else:
print "TODO, NO CONTACT VARIATION, LINEAR INTERPOLATION REQUIRED"
except hpperr as e:
print "hpperr failed at id " + str(i) , e.strerror
numerror+=1
errorid += [i]
fail+=1
#~ except ValueError as e:
#~ print "ValueError failed at id " + str(i) , e
#~ numerror+=1
#~ errorid += [i]
#~ fail+=1
#~ except IndexError as e:
#~ print "IndexError failed at id " + str(i) , e
#~ numerror+=1
#~ errorid += [i]
#~ fail+=1
#~ except Exception as e:
#~ print e
#~ numerror+=1
#~ errorid += [i]
#~ fail+=1
#~ except:
#~ numerror+=1
#~ errorid += [i]
#~ fail+=1
return fail
def play(frame_rate = 1./24.):
play_traj(fullBody,pp,frame_rate)
def actu(i, optim):
global numerror
global errorid
fail = 0
print "distance", fullBody.getEffectorDistance(i,i+1)
trunk_distance = np.linalg.norm(np.array(configs[i+1][0:3]) - np.array(configs[i][0:3]))
distance = max(fullBody.getEffectorDistance(i,i+1), trunk_distance)
dist = int(distance * 20.)#heuristic
while(dist %4 != 0):
dist +=1
total_time_flying_path = max(float(dist)/10., 0.3)
total_time_support_path = float((int)(math.ceil(min(total_time_flying_path /2., 0.4)*10.))) / 10.
times = [total_time_support_path, total_time_flying_path]
if(total_time_flying_path>= 1.):
dt = 0.2
elif total_time_flying_path<= 0.3:
dt = 0.05
else:
dt = 0.1
print 'time per path', times
print 'dt', dt
#~ total_time_per_path = 1.2
#~ pid, trajectory = solve_com_RRT(fullBody, configs, i, True, 1, 0.2, total_time_per_path, False, optim, False, False)
if(distance > 0.0001):
pid, trajectory = solve_com_RRT(fullBody, configs, i, True, 0.4, dt, times, False, optim, False, False)
displayComPath(pid)
#~ pp(pid)
global res
res = res + [pid]
global trajec
global trajec_mil
trajec = trajec + gen_trajectory_to_play(fullBody, pp, trajectory, times)
trajec_mil = trajec_mil + gen_trajectory_to_play(fullBody, pp, trajectory, times, 1./1000.)
global contacts
contacts += gencontactsPerFrame(i, trajectory, times, 1./1000.)
Ps, Ns = genPandNperFrame(i, trajectory, times, 1./1000.)
global pos
pos += Ps
global normals
normals+= Ns
assert(len(contacts) == len(trajec_mil) and len(contacts) == len(pos) and len(normals) == len(pos))
else:
print "TODO, NO CONTACT VARIATION, LINEAR INTERPOLATION REQUIRED"
def displayInSave(pp, pathId, configs):
length = pp.end*pp.client.problem.pathLength (pathId)
t = pp.start*pp.client.problem.pathLength (pathId)
while t < length :
q = pp.client.problem.configAtParam (pathId, t)
configs.append(q)
t += (pp.dt * pp.speed)
import time
#~ play_trajectory(fullBody,pp,trajec)
from pickle import dump
def saveToPinocchio(filename):
res = []
for i, q_gep in enumerate(trajec_mil):
#invert to pinocchio config:
q = q_gep[:]
quat_end = q[4:7]
q[6] = q[3]
q[3:6] = quat_end
data = {'q':q, 'contacts': contacts[i], 'P' : pos[i], 'N' : normals[i]}
res += [data]
f1=open(filename, 'w+')
dump(res, f1)
f1.close()
def clean():
global res
global trajec
global trajec_mil
global contacts
global errorid
global pos
global normals
res = []
trajec = []
trajec_mil = []
contacts = []
errorid = []
pos = []
normals = []
def saveAll(name):
saveAllData(fullBody, r, name)
#~ fullBody.exportAll(r, trajec, 'hole_hyq_t_var_04f_andrea');
#~ fullBody.exportAll(r, configs, 'hole_hyq_t_var_04f_andrea_contact_planning');
#~ saveToPinocchio('obstacle_hyq_t_var_04f_andrea')
#~ fullBody.exportAll(r, trajec, 'darpa_hyq_t_var_04f_andrea');
#~ saveToPinocchio('darpa_hyq_t_var_04f_andrea')
......@@ -67,3 +67,6 @@ pp = PathPlayer (rbprmBuilder.client.basic, r)
q_far = q_init [::]
q_far [0:3] = [-2, -3, 0.63];
r(q_far)
for i in range(1,10):
rbprmBuilder.client.basic.problem.optimizePath(i)
......@@ -326,7 +326,7 @@ class FullBody (object):
# \return H matrix and h column, such that H w <= h
def getContactCone(self, stateId, friction = 0.5):
H_h = array(self.client.rbprm.rbprm.getContactCone(stateId, friction))
#~ print "H_h", len(H_h)
print "H_h", H_h.shape
# now decompose cone
return H_h[:,:-1], H_h[:, -1]
......@@ -336,6 +336,7 @@ class FullBody (object):
# \return H matrix and h column, such that H w <= h
def getContactIntermediateCone(self, stateId, friction = 0.5):
H_h = array(self.client.rbprm.rbprm.getContactIntermediateCone(stateId, friction))
print "H_h", H_h.shape
# now decompose cone
return H_h[:,:-1], H_h[:, -1]
......
......@@ -97,7 +97,7 @@ def gen_trajectory(fullBody, states, state_id, computeCones = False, mu = 1, dt=
reduce_ineq = True, verbose = False, limbsCOMConstraints = None, profile = False, use_window = 0):
global lastspeed
use_window = max(0, min(use_window, (len(states) - 1) - (state_id + 2))) # can't use preview if last state is reached
assert( len(phase_dt) == 2 + use_window * 2 ), "phase_dt does not describe all phases"
assert( len(phase_dt) >= 2 + use_window * 2 ), "phase_dt does not describe all phases"
constraints = ['cones_constraint', 'end_reached_constraint','end_speed_constraint']
param_constraints = []
......@@ -141,7 +141,7 @@ reduce_ineq = True, verbose = False, limbsCOMConstraints = None, profile = False
var_final['c'] = var_final['c'][:init_waypoint_time+1]
params["t_init_phases"] = params["t_init_phases"][:-3*use_window]
lastspeed = var_final['dc'][init_waypoint_time]
#~ print "trying to project on com (from, to) ", init_end_com, var_final['c'][-1]
print "trying to project on com (from, to) ", init_end_com, var_final['c'][-1]
if (fullBody.projectStateToCOM(state_id+1, (var_final['c'][-1]).tolist())):
states[state_id+1] = fullBody.getConfigAtState(state_id+1) #updating config from python side)
else:
......@@ -229,7 +229,7 @@ def __optim__threading_ok(fullBody, states, state_id, computeCones = False, mu =
def solve_com_RRT(fullBody, states, state_id, computeCones = False, mu = 1, dt =0.1, phase_dt = [0.4, 1],
reduce_ineq = True, num_optims = 0, draw = False, verbose = False, limbsCOMConstraints = None, profile = False, use_window = 0):
comPosPerPhase, timeElapsed = __optim__threading_ok(fullBody, states, state_id, computeCones, mu, dt, phase_dt,
reduce_ineq, num_optims, draw, verbose, limbsCOMConstraints, profile)
reduce_ineq, num_optims, draw, verbose, limbsCOMConstraints, profile, use_window)
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
......
......@@ -200,29 +200,35 @@ def step(fullBody, configs, i, optim, pp, limbsCOMConstraints, friction = 0.5,
stat_data["num_errors"] += 1
errorid += [i]
fail+=1
except ValueError as e:
print "ValueError failed at id " + str(i) , e
stat_data["error_unknown"] += 1
stat_data["num_errors"] += 1
errorid += [i]
fail+=1
except IndexError as e:
print "IndexError failed at id " + str(i) , e
stat_data["error_unknown"] += 1
stat_data["num_errors"] += 1
errorid += [i]
fail+=1
#~ except ValueError as e:
#~ print "ValueError failed at id " + str(i) , e
#~ stat_data["error_unknown"] += 1
#~ stat_data["num_errors"] += 1
#~ errorid += [i]
#~ fail+=1
#~ except IndexError as e:
#~ print "IndexError failed at id " + str(i) , e
#~ stat_data["error_unknown"] += 1
#~ stat_data["num_errors"] += 1
#~ errorid += [i]
#~ fail+=1
except Exception as e:
stat_data["error_unknown"] += 1
stat_data["num_errors"] += 1
print e
errorid += [i]
fail+=1
if (use_window == 0 and (len(configs) - 1) - (i + 2) > 0):
print "could not project com, trying ti increase velocity "
return step(fullBody, configs, i, optim, pp, limbsCOMConstraints, friction, optim_effectors, time_scale, useCOMConstraints, 1, verbose, draw)
except:
stat_data["error_unknown"] += 1
stat_data["num_errors"] += 1
errorid += [i]
fail+=1
if (use_window == 0 and (len(configs) - 1) - (i + 2) > 0):
print "could not project com, trying ti increase velocity "
return step(fullBody, configs, i, optim, pp, limbsCOMConstraints, friction, optim_effectors, time_scale, useCOMConstraints, 1, verbose, draw)
return fail
def step_profile(fullBody, configs, i, optim, limbsCOMConstraints, friction = 0.5, optim_effectors = True, time_scale = 20., useCOMConstraints = False):
......@@ -343,6 +349,7 @@ import copy
def stats():
global stat_data
stat_data["error_id"] = errorid
stat_data_copy = copy.deepcopy(stat_data)
return stat_data_copy
......
......@@ -485,8 +485,10 @@ namespace hpp {
throw std::runtime_error ("Unexisting state " + std::string(""+(stateId)));
}
model::Configuration_t com_target = dofArrayToConfig (3, com);
State& s = lastStatesComputed_[stateId];
State s = lastStatesComputed_[stateId];
bool succes (false);
std::cout << com_target << std::endl;
s.print();
hpp::model::Configuration_t c = rbprm::interpolation::projectOnCom(fullBody_, problemSolver_->problem(),s,com_target,succes);
if(succes)
{
......
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