Skip to content

GitLab

Commit 98a74de9 authored by Steve Tonneau's avatar Steve Tonneau
Browse files

prototype for sliding window planning

parent 7ea8c33d
Hide whitespace changes
Inline Side-by-side
script/scenarios/ground_crouch_hyq_interp.py
View file @ 98a74de9
......@@ -3,7 +3,12 @@ from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
from hpp.gepetto import Viewer
import ground_crouch_hyq_path as tp
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
packageName = "hyq_description"
meshPackageName = "hyq_description"
......@@ -44,22 +49,37 @@ 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, "forward", 0.1, cType)
def addLimbDb(limbId, heuristicName, loadValues = True, disableEffectorCollision = False):
fullBody.addLimbDatabase(str(db_dir+limbId+'.db'), limbId, heuristicName,loadValues, disableEffectorCollision)
lLegId = 'lhleg'
lLeg = 'lh_haa_joint'
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, "forward", 0.1, cType)
#~ lLegId = 'lhleg'
#~ lLeg = 'lh_haa_joint'
lfoot = 'lh_foot_joint'
fullBody.addLimb(lLegId,lLeg,lfoot,offset,normal, legx, legy, nbSamples, "backward", 0.05, cType)
#~ fullBody.addLimb(lLegId,lLeg,lfoot,offset,normal, legx, legy, nbSamples, "backward", 0.05, cType)
rarmId = 'rhleg'
rarm = 'rh_haa_joint'
#~ rarmId = 'rhleg'
#~ rarm = 'rh_haa_joint'
rHand = 'rh_foot_joint'
fullBody.addLimb(rarmId,rarm,rHand,offset,normal, legx, legy, nbSamples, "backward", 0.05, cType)
#~ fullBody.addLimb(rarmId,rarm,rHand,offset,normal, legx, legy, nbSamples, "backward", 0.05, cType)
larmId = 'lfleg'
larm = 'lf_haa_joint'
#~ 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, "forward", 0.05, cType)
q_0 = fullBody.getCurrentConfig();
q_init = fullBody.getCurrentConfig(); q_init[0:7] = tp.q_init[0:7]
......@@ -77,7 +97,7 @@ fullBody.setEndState(q_goal,[rLegId,lLegId,rarmId,larmId])
r(q_init)
configs = fullBody.interpolate(0.1,1,10) #hole
configs = fullBody.interpolate(0.1,1,10, False) #hole
#~ configs = fullBody.interpolate(0.08,1,5) # bridge
r.loadObstacleModel ('hpp-rbprm-corba', "groundcrouch", "contact")
......@@ -90,191 +110,24 @@ from hpp.gepetto import PathPlayer
pp = PathPlayer (fullBody.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()
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},
rarmId : {'file': "hyq/"+rarmId+"_com.ineq", 'effector' : rHand},
larmId : {'file': "hyq/"+larmId+"_com.ineq", 'effector' : lHand} }
lLegId : {'file': "hyq/"+lLegId+"_com.ineq", 'effector' : lfoot},
rarmId : {'file': "hyq/"+rarmId+"_com.ineq", 'effector' : rHand},
larmId : {'file': "hyq/"+larmId+"_com.ineq", 'effector' : lHand} }
res = []
trajec = []
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, total_time_per_path, 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 = total_time_per_path / dt_framerate
dt = float(length) / num_frames_required
dt_finals = [dt*i for i in range(int(num_frames_required))] + [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]
elif interpassed:
res+= [contactsPerPhase[2] for t in dt_finals]
else:
res+= [contactsPerPhase[0] for t in dt_finals]
return res
def genPandNperFrame(stateid, path_ids, total_time_per_path, 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 = total_time_per_path / dt_framerate
dt = float(length) / num_frames_required
dt_finals = [dt*i for i in range(int(num_frames_required))] + [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]
nRes+= [N[1] for t in dt_finals]
elif interpassed:
pRes+= [p[2] for t in dt_finals]
nRes+= [N[2] for t in dt_finals]
else:
pRes+= [p[0] for t in dt_finals]
nRes+= [N[0] for t in dt_finals]
return pRes, nRes
from hpp import Error as hpperr
import sys
numerror = 0
def act(i, optim):
global numerror
global errorid
fail = 0
try:
total_time_per_path = 1
pid, trajectory = solve_com_RRT(fullBody, configs, i, True, 0.5, 0.2, total_time_per_path, False, optim, False, False)
displayComPath(pid)
#~ pp(pid)
global res
res = res + [pid]
global trajec
trajec = trajec + gen_trajectory_to_play(fullBody, pp, trajectory, total_time_per_path)
global contacts
contacts += gencontactsPerFrame(i, trajectory, total_time_per_path)
Ps, Ns = genPandNperFrame(i, trajectory, total_time_per_path)
global pos
pos += Ps
global normals
normals+= Ns
assert(len(contacts) == len(trajec) and len(contacts) == len(pos) and len(normals) == len(pos))
except hpperr as e:
print "failed at id " + str(i) , e.strerror
numerror+=1
errorid += [i]
fail+=1
except ValueError as e:
print "failed at id " + str(i) , e
numerror+=1
errorid += [i]
fail+=1
except IndexError as e:
print "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 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)
def act(i, numOptim = 0):
return step(fullBody, configs, i, numOptim, pp, limbsCOMConstraints, 0.6, optim_effectors = True, time_scale = 20., useCOMConstraints = False)
def play(frame_rate = 1./24.):
play_traj(fullBody,pp,frame_rate)
import time
#~ play_trajectory(fullBody,pp,trajec)
from pickle import dump
def saveToPinocchio(filename):
res = []
for i, q_gep in enumerate(trajec):
#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 contacts
global errorid
global pos
global normals
res = []
trajec = []
contacts = []
errorid = []
pos = []
normals = []
#~ saveToPinocchio('darpahyq_andrea')
#~ fullBody.exportAll(r, trajec, 'bridge_hyq_1s_05f_andrea');
#~ saveToPinocchio('bridge_hyq_1s_05f_andrea')
#~ fullBody.exportAll(r, trajec, 'hole_hyq_1s_05f_andrea');
#~ saveToPinocchio('hole_hyq_1s_05f_andrea')
#~ print "don"
def saveAll(name):
saveAllData(fullBody, r, name)
#~ fullBody.exportAll(r, trajec, 'obstacle_hyq_t_var_04f_andrea');
#~ saveToPinocchio('obstacle_hyq_t_var_04f_andrea')
src/hpp/corbaserver/rbprm/rbprmfullbody.py
View file @ 98a74de9
......@@ -104,7 +104,7 @@ class FullBody (object):
# \param heuristicName: name of the selected heuristic for configuration evaluation
# \param loadValues: whether values computed, other than the static ones, should be loaded in memory
# \param disableEffectorCollision: whether collision detection should be disabled for end effector bones
def addLimbDatabase(self, databasepath, limbId, heuristicName, loadValues = True, disableEffectorCollision = False):
def addLimbDatabase(self, databasepath, limbId, heuristicName, loadValues = True, disableEffectorCollision = False):
boolVal = 0.
boolValEff = 0.
if(loadValues):
......
src/hpp/corbaserver/rbprm/tools/cwc_trajectory.py
View file @ 98a74de9
......@@ -35,6 +35,8 @@ def __get_com(robot, config):
constraintsComLoaded = {}
lastspeed = np.array([0,0,0])
def __get_com_constraint(fullBody, state, config, limbsCOMConstraints, interm = False):
global constraintsLoaded
As = []; bs = []
......@@ -59,12 +61,10 @@ def __get_com_constraint(fullBody, state, config, limbsCOMConstraints, interm =
#~ print 'contacts', contacts
return [np.vstack(As), np.hstack(bs)]
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):
def compute_state_info(fullBody,states, state_id, phase_dt, mu, computeCones, limbsCOMConstraints):
init_com = __get_com(fullBody, states[state_id])
end_com = __get_com(fullBody, states[state_id+1])
p, N = fullBody.computeContactPoints(state_id)
mass = fullBody.getMass()
fly_time = phase_dt [1]
support_time = phase_dt [0]
t_end_phases = [0]
......@@ -74,18 +74,13 @@ def gen_trajectory(fullBody, states, state_id, computeCones = False, mu = 1, dt=
t_end_phases[2] = t_end_phases[1] + fly_time
t_end_phases[3] = t_end_phases[2] + support_time
t_end_phases = [float((int)(math.ceil(el*10.))) / 10. for el in t_end_phases]
if (not profile):
print "end_phases", t_end_phases
cones = None
if(computeCones):
cones = [fullBody.getContactCone(state_id, mu)[0]]
if(len(p) > 2):
cones.append(fullBody.getContactIntermediateCone(state_id, mu)[0])
if(len(p) > len(cones)):
cones.append(fullBody.getContactCone(state_id+1, mu)[0])
if (not profile):
print "num cones ", len(cones)
cones.append(fullBody.getContactCone(state_id+1, mu)[0])
COMConstraints = None
if(not (limbsCOMConstraints == None)):
#~ print "retrieving COM constraints"
......@@ -95,17 +90,59 @@ def gen_trajectory(fullBody, states, state_id, computeCones = False, mu = 1, dt=
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)
timeelapsed = 0
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"
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):
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)
p+=p1;
N+=N1;
end_com = end_com1;
cones += cones1;
if(COMConstraints != None and COMConstraints1 != None):
COMConstraints += COMConstraints1;
t_end_phases += [t_end_phases[-1] + t for t in t_end_phases1[1:]]
if (not profile):
print "num cones ", len(cones)
print "end_phases", t_end_phases
timeelapsed = 0
if (profile):
start = time.clock()
var_final, params = 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)
print "init x", init_com + lastspeed.tolist()
var_final, params = cone_optimization(p, N, [init_com + lastspeed.tolist(), end_com + [0,0,0]], t_end_phases[1:], dt, cones, COMConstraints, mu, mass, 9.81, reduce_ineq, verbose,
constraints, param_constraints)
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"]
global lastspeed
lastspeed = var_final['dc'][waypoint_time]
print "lastspeed", lastspeed
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):
var_final, params, elapsed = gen_trajectory(fullBody, states, state_id, computeCones, mu , dt, phase_dt, reduce_ineq, verbose, limbsCOMConstraints, 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 = False):
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
import matplotlib.pyplot as plt
......@@ -138,7 +175,7 @@ def draw_trajectory(fullBody, states, state_id, computeCones = False, mu = 1, d
fig = plt.figure()
ax = fig.add_subplot(111)
points = var_final['dc']
print "points", points
#~ print "points", points
ys = [norm(el) for el in points]
xs = [i * params['dt'] for i in range(0,len(points))]
ax.scatter(xs, ys, c='b')
......@@ -162,32 +199,41 @@ 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):
num_optims = 0, draw = False, verbose = False, limbsCOMConstraints = None, profile = False, use_window = False):
print "callgin gen ",state_id
if(draw):
res = draw_trajectory(fullBody, states, state_id, computeCones, mu, dt, phase_dt, reduce_ineq, verbose, limbsCOMConstraints)
res = draw_trajectory(fullBody, states, state_id, computeCones, mu, dt, phase_dt, reduce_ineq, verbose, limbsCOMConstraints, use_window)
else:
res = gen_trajectory(fullBody, states, state_id, computeCones, mu, dt, phase_dt, reduce_ineq, verbose, limbsCOMConstraints, profile)
res = gen_trajectory(fullBody, states, state_id, computeCones, mu, dt, phase_dt, reduce_ineq, verbose, limbsCOMConstraints, profile, use_window)
t = res[1]["t_init_phases"];
dt = res[1]["dt"];
final_state = res[0]
c0 = res[1]["x_init"][0:3]
comPos = [c0] + [c.tolist() for c in final_state['c']]
comPosPerPhase = [[comPos[(int)(t_id/dt)] for t_id in np.arange(t[index],t[index+1]-_EPS,dt)] for index, _ in enumerate(t[:-1]) ]
comPosPerPhase = [[comPos[(int)(t_id/dt) ] for t_id in np.arange(t[index],t[index+1]-_EPS,dt)] for index, _ in enumerate(t[:-1]) ]
comPosPerPhase[-1].append(comPos[-1])
print "(len(comPos)", len(comPos)
print "(len(0)", len(comPosPerPhase[0])
print "(len(1)", len(comPosPerPhase[1])
print "(len(2)", len(comPosPerPhase[2])
print "(len(sum)", len(comPosPerPhase[0]) + len(comPosPerPhase[1]) + len(comPosPerPhase[2])
assert(len(comPos) == len(comPosPerPhase[0]) + len(comPosPerPhase[1]) + len(comPosPerPhase[2]))
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):
comPosPerPhase, timeElapsed = __optim__threading_ok(fullBody, states, state_id, computeCones, mu, dt, phase_dt, reduce_ineq, num_optims, draw, verbose, limbsCOMConstraints, profile)
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):
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
print "comePhaseLength", len(comPosPerPhase[0]), len(comPosPerPhase[1]), len(comPosPerPhase[2])
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], 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):
comPosPerPhase, timeElapsed = __optim__threading_ok(fullBody, states, state_id, computeCones, mu, dt, phase_dt, reduce_ineq, num_optims, draw, verbose, limbsCOMConstraints, profile)
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):
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
print "comePhaseLength", len(comPosPerPhase[0]), len(comPosPerPhase[1]), len(comPosPerPhase[2])
paths_ids = [int(el) for el in fullBody.effectorRRT(state_id,comPosPerPhase[0],comPosPerPhase[1],comPosPerPhase[2],num_optims)]
......
src/hpp/corbaserver/rbprm/tools/cwc_trajectory_helper.py
View file @ 98a74de9
......@@ -108,28 +108,39 @@ def __update_cwc_time(t):
stat_data["time_cwc"]["numiter"] += 1
def __getTimes(fullBody, configs, i, time_scale):
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
return times, dt, distance
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):
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):
global errorid
global stat_data
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
#~ 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)
times += times2
dt = min(dt, dt2)
print 'time per path', times
print 'dt', dt
if(distance > 0.0001):
......@@ -139,20 +150,29 @@ def step(fullBody, configs, i, optim, pp, limbsCOMConstraints, friction = 0.5,
else:
comC = None
if(optim_effectors):
pid, trajectory, timeelapsed = solve_effector_RRT(fullBody, configs, i, True, friction, dt, times, False, optim, False, False, comC)
pid, trajectory, timeelapsed = solve_effector_RRT(fullBody, configs, i, True, friction, dt, times, False, optim, draw, verbose, comC, False, use_window=use_window)
else :
pid, trajectory, timeelapsed = solve_com_RRT(fullBody, configs, i, True, friction, dt, times, False, optim, False, False, comC)
pid, trajectory, timeelapsed = solve_com_RRT(fullBody, configs, i, True, friction, dt, times, False, optim, draw, verbose, comC, False, use_window=use_window)
displayComPath(pp, 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.)
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.)
if(len(trajec) > 0):
new_traj = new_traj[1:]
new_traj_andrea = new_traj_andrea[1:]
new_contacts = new_contacts[1:]
Ps = Ps[1:]
Ns = Ns[1:]
trajec = trajec + new_traj
trajec_mil = new_traj_andrea
global contacts
contacts += gencontactsPerFrame(fullBody, i, limbsCOMConstraints, pp, trajectory, times, 1./1000.)
Ps, Ns = genPandNperFrame(fullBody, i, pp, trajectory, times, 1./1000.)
contacts += new_contacts
global pos
pos += Ps
global normals
......@@ -329,6 +349,7 @@ def write_stats(filename):
f.write("optim_num_success " + str(sd["num_success"]) + "\n")
f.write("optim_num_trials " + str(sd["num_trials"]) + "\n")
f.write("num_errors " + str(sd["num_errors"]) + "\n")
f.write("error_id " + str(errorid) + "\n")
f.write("time_cwc " + str(sd["time_cwc"]["min"]) + " " + str(sd["time_cwc"]["avg"]) + " " + str(sd["time_cwc"]["max"]) + " " + str(sd["time_cwc"]["totaltime"]) + " " + str(sd["time_cwc"]["numiter"]) + " " + "\n")
f.close()
return sd
......
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