Commit 55d35499 authored by Steve Tonneau's avatar Steve Tonneau
Browse files

implementing sliding window of any number of states forward

parent 98a74de9
......@@ -7,8 +7,8 @@ from os import environ
ins_dir = environ['DEVEL_DIR']
db_dir = ins_dir+"/install/share/hyq-rbprm/database/hyq_"
#~ import ground_crouch_hyq_path as tp
import ground_crouch_hyq_path_bridge as tp
import ground_crouch_hyq_path as tp
#~ import ground_crouch_hyq_path_bridge as tp
packageName = "hyq_description"
meshPackageName = "hyq_description"
......@@ -97,7 +97,7 @@ fullBody.setEndState(q_goal,[rLegId,lLegId,rarmId,larmId])
r(q_init)
configs = fullBody.interpolate(0.1,1,10, False) #hole
configs = fullBody.interpolate(0.1,1,10, True) #hole
#~ configs = fullBody.interpolate(0.08,1,5) # bridge
r.loadObstacleModel ('hpp-rbprm-corba', "groundcrouch", "contact")
......@@ -120,14 +120,16 @@ limbsCOMConstraints = { rLegId : {'file': "hyq/"+rLegId+"_com.ineq", 'effector'
def act(i, numOptim = 0):
return step(fullBody, configs, i, numOptim, pp, limbsCOMConstraints, 0.6, optim_effectors = True, time_scale = 20., useCOMConstraints = False)
def act(i, numOptim = 0, use_window = False, verbose = False, draw = False):
return step(fullBody, configs, i, numOptim, pp, limbsCOMConstraints, 0.4, optim_effectors = True, time_scale = 20., useCOMConstraints = False, use_window = use_window,
verbose = verbose, draw = draw)
def play(frame_rate = 1./24.):
play_traj(fullBody,pp,frame_rate)
def saveAll(name):
saveAllData(fullBody, r, name)
#~ fullBody.exportAll(r, trajec, 'obstacle_hyq_t_var_04f_andrea');
#~ fullBody.exportAll(r, trajec, 'hole_hyq_t_var_04f_andrea');
#~ fullBody.exportAll(r, configs, 'obstacle_hyq_t_var_04f_contact_planning');
#~ saveToPinocchio('obstacle_hyq_t_var_04f_andrea')
......@@ -93,21 +93,23 @@ def compute_state_info(fullBody,states, state_id, phase_dt, mu, computeCones, li
return p, N, init_com, end_com, t_end_phases, cones, COMConstraints
def gen_trajectory(fullBody, states, state_id, computeCones = False, mu = 1, dt=0.2, phase_dt = [0.4, 1],
reduce_ineq = True, verbose = False, limbsCOMConstraints = None, profile = False, use_window = False):
use_window = use_window and state_id + 2 < len(states) - 1 # can't use preview if last state is reached
assert( len(phase_dt) == 4 or not use_window ), "phase_dt does not describe all phases"
reduce_ineq = True, verbose = False, limbsCOMConstraints = None, profile = False, use_window = 0):
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']
param_constraints = []
mass = fullBody.getMass()
p, N, init_com, end_com, t_end_phases, cones, COMConstraints = compute_state_info(fullBody,states, state_id, phase_dt[:2], mu, computeCones, limbsCOMConstraints)
if(use_window):
if(use_window > 0):
init_waypoint_time = int(np.round(t_end_phases[-1]/ dt)) - 1
for w in range(1,use_window+1):
waypoint = end_com[:]
waypoint_time = int(np.round(t_end_phases[-1]/ dt)) - 1
print "waypoint_time", waypoint_time
param_constraints = [("waypoint_reached_constraint",(waypoint_time, waypoint))]
p1, N1, init_com1, end_com1, t_end_phases1, cones1, COMConstraints1 = compute_state_info(fullBody,states, state_id+1, phase_dt[2:], mu, computeCones, limbsCOMConstraints)
param_constraints += [("waypoint_reached_constraint",(waypoint_time, waypoint))]
p1, N1, init_com1, end_com1, t_end_phases1, cones1, COMConstraints1 = compute_state_info(fullBody,states, state_id+w, phase_dt[2*w:], mu, computeCones, limbsCOMConstraints)
p+=p1;
N+=N1;
end_com = end_com1;
......@@ -129,19 +131,14 @@ reduce_ineq = True, verbose = False, limbsCOMConstraints = None, profile = False
if (profile):
end = time.clock()
timeelapsed = (end - start) * 1000
if(use_window):
print "wtf", var_final['c']
var_final['c'] = var_final['c'][:waypoint_time+1]
print "wtf2", var_final['c']
print "t_init_phases", params["t_init_phases"]
params["t_init_phases"] = params["t_init_phases"][:-3]
print "t_init_phases", params["t_init_phases"]
if(use_window > 0):
var_final['c'] = var_final['c'][:init_waypoint_time+1]
params["t_init_phases"] = params["t_init_phases"][:-3*use_window]
global lastspeed
lastspeed = var_final['dc'][waypoint_time]
print "lastspeed", lastspeed
lastspeed = var_final['dc'][init_waypoint_time]
return var_final, params, timeelapsed
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 = False):
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)
p, N = fullBody.computeContactPoints(state_id)
from mpl_toolkits.mplot3d import Axes3D
......@@ -176,7 +173,7 @@ def draw_trajectory(fullBody, states, state_id, computeCones = False, mu = 1, d
ax = fig.add_subplot(111)
points = var_final['dc']
#~ print "points", points
ys = [norm(el) for el in points]
ys = [el for el in points]
xs = [i * params['dt'] for i in range(0,len(points))]
ax.scatter(xs, ys, c='b')
......@@ -187,7 +184,7 @@ def draw_trajectory(fullBody, states, state_id, computeCones = False, mu = 1, d
fig = plt.figure()
ax = fig.add_subplot(111)
points = var_final['ddc']
ys = [norm(el) for el in points]
ys = [el for el in points]
xs = [i * params['dt'] for i in range(0,len(points))]
ax.scatter(xs, ys, c='b')
......@@ -199,7 +196,7 @@ def draw_trajectory(fullBody, states, state_id, computeCones = False, mu = 1, d
return var_final, params, elapsed
def __optim__threading_ok(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 = False):
num_optims = 0, draw = False, verbose = False, limbsCOMConstraints = None, profile = False, use_window = 0):
print "callgin gen ",state_id
if(draw):
res = draw_trajectory(fullBody, states, state_id, computeCones, mu, dt, phase_dt, reduce_ineq, verbose, limbsCOMConstraints, use_window)
......@@ -221,7 +218,7 @@ def __optim__threading_ok(fullBody, states, state_id, computeCones = False, mu =
return comPosPerPhase, res[2] #res[2] is timeelapsed
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 = False):
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)
print "done. generating state trajectory ",state_id
......@@ -231,7 +228,7 @@ reduce_ineq = True, num_optims = 0, draw = False, verbose = False, limbsCOMConst
return paths_ids[-1], paths_ids[:-1], timeElapsed
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 = False):
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, use_window)
print "done. generating state trajectory ",state_id
......
......@@ -129,16 +129,18 @@ def __getTimes(fullBody, configs, i, time_scale):
from hpp import Error as hpperr
import sys, time
def step(fullBody, configs, i, optim, pp, limbsCOMConstraints, friction = 0.5, optim_effectors = True, time_scale = 20., useCOMConstraints = False, use_window = False, verbose = False, draw = False):
def step(fullBody, configs, i, optim, pp, limbsCOMConstraints, friction = 0.5, optim_effectors = True, time_scale = 20., useCOMConstraints = False, use_window = 0, verbose = False, draw = False):
global errorid
global stat_data
fail = 0
#~ try:
if(True):
times, dt, distance = __getTimes(fullBody, configs, i, time_scale)
use_window = use_window and i + 2 < len(configs) - 1 # can't use preview if last state is reached
if(use_window):
times2, dt2, dist2 = __getTimes(fullBody, configs, i+1, time_scale)
try:
#~ if(True):
times = [];
dt = 1000;
distance = __getTimes(fullBody, configs, i, time_scale)
use_window = max(0, min(use_window, (len(configs) - 1) - (i + 2))) # can't use preview if last state is reached
for w in range(use_window+1):
times2, dt2, dist2 = __getTimes(fullBody, configs, i+w, time_scale)
times += times2
dt = min(dt, dt2)
print 'time per path', times
......@@ -158,11 +160,16 @@ def step(fullBody, configs, i, optim, pp, limbsCOMConstraints, friction = 0.5,
global res
res = res + [pid]
global trajec
global trajec_mil
new_traj = gen_trajectory_to_play(fullBody, pp, trajectory, times)
new_traj_andrea = gen_trajectory_to_play(fullBody, pp, trajectory, times, 1./1000.)
new_contacts = gencontactsPerFrame(fullBody, i, limbsCOMConstraints, pp, trajectory, times, 1./1000.)
Ps, Ns = genPandNperFrame(fullBody, i, pp, trajectory, times, 1./1000.)
global trajec_mil
frame_rate = 1./24.
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, times, frame_rate)
new_traj_andrea = gen_trajectory_to_play(fullBody, pp, trajectory, times,frame_rate_andrea)
new_contacts = gencontactsPerFrame(fullBody, i, limbsCOMConstraints, pp, trajectory, times, frame_rate_andrea)
Ps, Ns = genPandNperFrame(fullBody, i, pp, trajectory, times, frame_rate_andrea)
if(len(trajec) > 0):
new_traj = new_traj[1:]
new_traj_andrea = new_traj_andrea[1:]
......
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