Skip to content
GitLab
Commit e666fab0 authored by Steve Tonneau's avatar Steve Tonneau
Browse files

debugging python ocp

parent ddb8acf2
Hide whitespace changes
Inline Side-by-side
src/hpp/corbaserver/rbprm/tools/cwc_trajectory.py
View file @ e666fab0
......@@ -86,6 +86,7 @@ def compute_state_info(fullBody,states, state_id, phase_dt, mu, computeCones, li
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 cones ', len(cones)
return p, N, init_com, end_com, t_end_phases, cones, COMConstraints
......@@ -95,7 +96,9 @@ reduce_ineq = True, verbose = False, limbsCOMConstraints = None, profile = False
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"
constraints = ['cones_constraint', 'end_reached_constraint','end_speed_constraint']
#~ constraints = ['cones_constraint', 'end_reached_constraint','end_speed_constraint']
#~ constraints = ['cones_constraint', 'end_reached_constraint']
constraints = ['end_reached_constraint']
#~ constraints = ['cones_constraint', 'end_reached_constraint','end_speed_constraint', 'com_kinematic_constraint']
param_constraints = []
mass = fullBody.getMass()
......@@ -152,12 +155,20 @@ reduce_ineq = True, verbose = False, limbsCOMConstraints = None, profile = False
else:
if norm(np.array(var_final['c'][-1]) - np.array(__get_com(fullBody, states[state_id+1]))) > 0.00001:
print "error in com desired: obtained ", __get_com(fullBody, states[state_id+1]), var_final['c'][-1]
print "trying to update com target... ", __get_com(fullBody, states[state_id+1]), var_final['c'][-1]
if (fullBody.projectStateToCOM(state_id+1, (var_final['c'][-1]).tolist())):
#~ print "PROJECTED", init_end_com, var_final['c'][-1]
states[state_id+1] = fullBody.getConfigAtState(state_id+1) #updating config from python side)
lastspeed = var_final['dc'][-1]
print "init speed", lastspeed
else:
raise ValueError("projection failed, this is bad")
lastspeed = np.array([0,0,0])
return var_final, params, timeelapsed
return var_final, params, timeelapsed, cones
def draw_trajectory(fullBody, states, state_id, computeCones = False, mu = 1, dt=0.2, phase_dt = [0.4, 1], reduce_ineq = True, verbose = False, limbsCOMConstraints = None, use_window = 0):
var_final, params, elapsed = gen_trajectory(fullBody, states, state_id, computeCones, mu , dt, phase_dt, reduce_ineq, verbose, limbsCOMConstraints, False, use_window = use_window)
var_final, params, elapsed, cones = gen_trajectory(fullBody, states, state_id, computeCones, mu , dt, phase_dt, reduce_ineq, verbose, limbsCOMConstraints, False, use_window = use_window)
p, N = fullBody.computeContactPoints(state_id)
print "p", p
fig = plt.figure()
......@@ -230,7 +241,7 @@ def draw_trajectory(fullBody, states, state_id, computeCones = False, mu = 1, d
plt.show()
plt.savefig('/tmp/dL'+ str(state_id)+ '.png')
return var_final, params, elapsed
return var_final, params, elapsed, cones
def __cVarPerPhase(var, dt, t, final_state, addValue):
varVals = addValue + [v.tolist() for v in final_state[var]]
......@@ -267,20 +278,20 @@ def __optim__threading_ok(fullBody, states, state_id, computeCones = False, mu =
#now compute com trajectorirs
comTrajIds = [fullBody.generateComTraj(comPosPerPhase[i], comVelPerPhase[i], comAccPerPhase[i], dt) for i in range(0,3)]
return comTrajIds, res[2], final_state #res[2] is timeelapsed
return comTrajIds, res[2], final_state, res[-1] #res[2] is timeelapsed, res[-1] is the cones
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, trackedEffectors = []):
comPosPerPhase, timeElapsed, final_state = __optim__threading_ok(fullBody, states, state_id, computeCones, mu, dt, phase_dt,
comPosPerPhase, timeElapsed, final_state, cones = __optim__threading_ok(fullBody, states, state_id, computeCones, mu, dt, phase_dt,
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, "path ids ", paths_ids[-1], " ," , paths_ids[:-1]
return paths_ids[-1], paths_ids[:-1], timeElapsed, final_state
return paths_ids[-1], paths_ids[:-1], timeElapsed, final_state, cones
def solve_effector_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, trackedEffectors = []):
comPosPerPhase, timeElapsed, final_state = __optim__threading_ok(fullBody, states, state_id, computeCones, mu, dt, phase_dt,
comPosPerPhase, timeElapsed, final_state, cones = __optim__threading_ok(fullBody, states, state_id, computeCones, mu, dt, phase_dt,
reduce_ineq, num_optims, draw, verbose, limbsCOMConstraints, profile, use_window)
print "done. generating state trajectory ",state_id
if(len(trackedEffectors) == 0):
......@@ -290,5 +301,5 @@ reduce_ineq = True, num_optims = 0, draw = False, verbose = False, limbsCOMConst
refPathId = trackedEffectors[0]; path_start = trackedEffectors[1]; path_to = trackedEffectors[2]; effectorstracked = trackedEffectors[3]
paths_ids = [int(el) for el in fullBody.effectorRRTFromPath(state_id, refPathId, path_start, path_to, comPosPerPhase[0],comPosPerPhase[1],comPosPerPhase[2],num_optims, effectorstracked)]
print "done. computing final trajectory to display ",state_id, "path ids ", paths_ids[-1], " ," , paths_ids[:-1]
return paths_ids[-1], paths_ids[:-1], timeElapsed, final_state
return paths_ids[-1], paths_ids[:-1], timeElapsed, final_state, cones
src/hpp/corbaserver/rbprm/tools/cwc_trajectory_helper.py
View file @ e666fab0
......@@ -9,12 +9,13 @@ from numpy import append, array
res = []
trajec = []
trajec_mil = []
#~ contacts = []
contacts = []
pos = []
normals = []
pEffs = []
coms = []
errorid = []
cones_saved = []
def displayComPath(pp, pathId,color=[0.,0.75,0.15,0.9]) :
pathPos=[]
......@@ -32,13 +33,14 @@ def displayComPath(pp, pathId,color=[0.,0.75,0.15,0.9]) :
pp.publisher.client.gui.refresh()
def genPandNperFrame(fullBody, stateid, limbsCOMConstraints, pp, path_ids, times, dt_framerate=1./24.):
def genPandNandConesperFrame(fullBody, stateid, limbsCOMConstraints, cones, pp, path_ids, times, dt_framerate=1./24.):
p, N= fullBody.computeContactPointsPerLimb(stateid, limbsCOMConstraints.keys(), limbsCOMConstraints)
freeEffectors = [ [limbsCOMConstraints[limb]['effector'] for limb in limbsCOMConstraints.keys() if not p[i].has_key(limbsCOMConstraints[limb]['effector'])] for i in range(len(p))]
config_size = len(fullBody.getCurrentConfig())
interpassed = False
pRes = []
nRes = []
cRes = []
for idx, path_id in enumerate(path_ids):
length = pp.client.problem.pathLength (path_id)
num_frames_required = times[idx] / dt_framerate
......@@ -48,7 +50,8 @@ def genPandNperFrame(fullBody, stateid, limbsCOMConstraints, pp, path_ids, times
dt_finals = [dt*i for i in range(int(round(num_frames_required)))]
pRes+= [p[idx] for t in dt_finals]
nRes+= [N[idx] for t in dt_finals]
return pRes, nRes, freeEffectors
cRes+= [cones[idx] for t in dt_finals]
return pRes, nRes, freeEffectors, cRes
def __getPos(effector, fullBody, config):
......@@ -148,9 +151,9 @@ trackedEffectors = []):
else:
comC = None
if(optim_effectors):
pid, trajectory, timeelapsed, final_state = solve_effector_RRT(fullBody, configs, i, True, friction, dt, times, False, optim, draw, verbose, comC, False, use_window=use_window, trackedEffectors = trackedEffectors)
pid, trajectory, timeelapsed, final_state, cones = solve_effector_RRT(fullBody, configs, i, True, friction, dt, times, False, optim, draw, verbose, comC, False, use_window=use_window, trackedEffectors = trackedEffectors)
else :
pid, trajectory, timeelapsed, final_state = solve_com_RRT(fullBody, configs, i, True, friction, dt, times, False, optim, draw, verbose, comC, False, use_window=use_window, trackedEffectors = trackedEffectors)
pid, trajectory, timeelapsed, final_state, cones = solve_com_RRT(fullBody, configs, i, True, friction, dt, times, False, optim, draw, verbose, comC, False, use_window=use_window, trackedEffectors = trackedEffectors)
displayComPath(pp, pid)
#~ pp(pid)
global res
......@@ -159,15 +162,16 @@ trackedEffectors = []):
global trajec_mil
frame_rate = 1./24.
#~ frame_rate_andrea = 1./1000.
frame_rate_andrea = 1./1000.
#~ if(len(trajec) > 0):
#~ frame_rate = 1./25.
#~ frame_rate_andrea = 1./1001.
new_traj = gen_trajectory_to_play(fullBody, pp, trajectory, time_per_path, frame_rate)
#~ new_traj_andrea = gen_trajectory_to_play(fullBody, pp, trajectory, time_per_path,frame_rate_andrea)
new_traj_andrea = gen_trajectory_to_play(fullBody, pp, trajectory, time_per_path,frame_rate_andrea)
#~ new_contacts = gencontactsPerFrame(fullBody, i, limbsCOMConstraints, pp, trajectory, times, frame_rate_andrea)
#~ Ps, Ns, freeEffectorsPerPhase = genPandNperFrame(fullBody, i, limbsCOMConstraints, pp, trajectory, time_per_path, frame_rate_andrea)
#~ NPeffs = genPEffperFrame(fullBody, freeEffectorsPerPhase, new_traj_andrea, pp, time_per_path, frame_rate_andrea)
#~ com = genComPerFrame(final_state, dt, frame_rate_andrea)
Ps, Ns, freeEffectorsPerPhase, Ks = genPandNandConesperFrame(fullBody, i, limbsCOMConstraints, cones, pp, trajectory, time_per_path, frame_rate_andrea)
NPeffs = genPEffperFrame(fullBody, freeEffectorsPerPhase, new_traj_andrea, pp, time_per_path, frame_rate_andrea)
com = genComPerFrame(final_state, dt, frame_rate_andrea)
#~ if(len(trajec) > 0):
#~ new_traj = new_traj[1:]
#~ new_traj_andrea = new_traj_andrea[1:]
......@@ -176,19 +180,21 @@ trackedEffectors = []):
#~ com = com[1:]
#~ NPeffs = NPeffs[1:]
trajec = trajec + new_traj
#~ trajec_mil += new_traj_andrea
trajec_mil += new_traj_andrea
#~ global contacts
#~ contacts += new_contacts
#~ global pos
#~ pos += Ps
#~ global normals
#~ normals+= Ns
#~ global pEffs
#~ pEffs+= NPeffs
#~ global coms
#~ coms+= com
#~ print len(trajec_mil), " ", len(pos), " ", len(normals), " ", len(coms), " ", len(pEffs)
#~ assert(len(trajec_mil) == len(pos) and len(normals) == len(pos) and len(normals) == len(coms) and len(coms) == len(pEffs))
global cones_saved
cones_saved += Ks
global pos
pos += Ps
global normals
normals+= Ns
global pEffs
pEffs+= NPeffs
global coms
coms+= com
print len(trajec_mil), " ", len(pos), " ", len(normals), " ", len(coms), " ", len(pEffs), " ", len(cones_saved)
assert(len(trajec_mil) == len(pos) and len(normals) == len(pos) and len(normals) == len(coms) and len(cones_saved) == len(coms) and len(coms) == len(pEffs))
stat_data["num_success"] += 1
else:
print "TODO, NO CONTACT VARIATION, LINEAR INTERPOLATION REQUIRED"
......@@ -261,7 +267,8 @@ trackedEffectors = []):
#~ stat_data["num_errors"] += 1
#~ errorid += [i]
#~ fail+=1
return fail
#~ return fail
return final_state, cones
def step_profile(fullBody, configs, i, optim, limbsCOMConstraints, friction = 0.5, optim_effectors = True, time_scale = 20., useCOMConstraints = False):
global errorid
......@@ -353,6 +360,7 @@ from pickle import dump
def compressData(data_array, filename):
qs = [data['q'][:] for data in data_array]
C = [data['C'][:] for data in data_array]
cones = [data['cone'][:] for data in data_array]
a = {}
frameswitches = []
for i in range(0,len(pos)):
......@@ -364,6 +372,7 @@ def compressData(data_array, filename):
res = {}
res['Q'] = [data['q'][:] for data in data_array]
res['C'] = [data['C'][:] for data in data_array]
res['cones'] = [data['cone'][:] for data in data_array]
res['fly'] = pEffs
res['frameswitches'] = frameswitches
f1=open(filename+"_compressed", 'w+')
......@@ -379,7 +388,7 @@ def saveToPinocchio(filename):
quat_end = q[4:7]
q[6] = q[3]
q[3:6] = quat_end
data = {'q':q, 'P' : pos[i], 'N' : normals[i], 'C' : coms [i], 'pEffs' : pEffs[i]}
data = {'q':q, 'P' : pos[i], 'N' : normals[i], 'C' : coms [i], 'pEffs' : pEffs[i], 'cone' : cones_saved[i]}
res += [data]
f1=open(filename, 'w+')
dump(res, f1)
......@@ -396,10 +405,12 @@ def clean():
global normals
global pEffs
global coms
global cones_saved
cones_saved = []
res = []
trajec = []
trajec_mil = []
contacts = []
#~ contacts = []
errorid = []
pos = []
normals = []
......@@ -440,6 +451,7 @@ def profile(fullBody, configs, i_start, i_end, limbsCOMConstraints, friction =
write_stats(filename)
def saveAllData(fullBody, r, name):
global trajec
fullBody.exportAll(r, trajec, name)
return saveToPinocchio(name)
......
src/rbprmbuilder.impl.cc
View file @ e666fab0
......@@ -1198,6 +1198,7 @@ namespace hpp {
success = true;
return intermediary;
}
std::cout << "no contact break during intermediary phase " << std::endl;
return firstState;
}
......
Supports Markdown
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