From 12ba267c882344ccb64e917b36653a838517596b Mon Sep 17 00:00:00 2001 From: Guilhem Saurel Date: Thu, 31 Jan 2019 23:02:13 +0100 Subject: [PATCH] remove .orig files --- .../rbprm/tools/cwc_trajectory.py.orig | 392 ------------- .../rbprm/tools/cwc_trajectory_helper.py.orig | 529 ------------------ 2 files changed, 921 deletions(-) delete mode 100644 src/hpp/corbaserver/rbprm/tools/cwc_trajectory.py.orig delete mode 100644 src/hpp/corbaserver/rbprm/tools/cwc_trajectory_helper.py.orig diff --git a/src/hpp/corbaserver/rbprm/tools/cwc_trajectory.py.orig b/src/hpp/corbaserver/rbprm/tools/cwc_trajectory.py.orig deleted file mode 100644 index 57ae7f69..00000000 --- a/src/hpp/corbaserver/rbprm/tools/cwc_trajectory.py.orig +++ /dev/null @@ -1,392 +0,0 @@ -import matplotlib -#~ matplotlib.use('Agg') -import matplotlib.pyplot as plt -from mpl_toolkits.mplot3d import Axes3D -from cwc import cone_optimization -from obj_to_constraints import ineq_from_file, rotate_inequalities -import numpy as np -import math -from numpy.linalg import norm -import time - -import hpp.corbaserver.rbprm.data.com_inequalities as ine - -ineqPath = ine.__path__[0] +"/" - -# epsilon for testing whether a number is close to zero -_EPS = np.finfo(float).eps * 4.0 - -def quaternion_matrix(quaternion): - q = np.array(quaternion, dtype=np.float64, copy=True) - n = np.dot(q, q) - if n < _EPS: - return np.identity(4) - q *= math.sqrt(2.0 / n) - q = np.outer(q, q) - return np.array([ - [1.0-q[2, 2]-q[3, 3], q[1, 2]-q[3, 0], q[1, 3]+q[2, 0], 0.0], - [ q[1, 2]+q[3, 0], 1.0-q[1, 1]-q[3, 3], q[2, 3]-q[1, 0], 0.0], - [ q[1, 3]-q[2, 0], q[2, 3]+q[1, 0], 1.0-q[1, 1]-q[2, 2], 0.0], - [ 0.0, 0.0, 0.0, 1.0]]) - -def __get_com(robot, config): - save = robot.getCurrentConfig() - robot.setCurrentConfig(config) - com = robot.getCenterOfMass() - robot.setCurrentConfig(save) - return com - -constraintsComLoaded = {} - -lastspeed = np.array([0,0,0]) - -def __get_com_constraint(fullBody, state, config, limbsCOMConstraints, interm = False): - global constraintsLoaded - As = []; bs = [] - fullBody.setCurrentConfig(config) - contacts = [] - for i, v in limbsCOMConstraints.iteritems(): - if not constraintsComLoaded.has_key(i): - constraintsComLoaded[i] = ineq_from_file(ineqPath+v['file']) - contact = (interm and fullBody.isLimbInContactIntermediary(i, state)) or (not interm and fullBody.isLimbInContact(i, state)) - if contact: - ineq = constraintsComLoaded[i] - qEffector = fullBody.getJointPosition(v['effector']) - tr = quaternion_matrix(qEffector[3:7]) - tr[:3,3] = np.array(qEffector[0:3]) - ineq_r = rotate_inequalities(ineq, tr) - As.append(ineq_r.A); bs.append(ineq_r.b); - contacts.append(v['effector']) - return [np.vstack(As), np.hstack(bs)] - -def compute_state_info(fullBody,states, state_id, phase_dt, mu, computeCones, limbsCOMConstraints, pathId = 0): - print "phase dt in compute_state_info:",phase_dt - init_com = __get_com(fullBody, states[state_id]) - end_com = __get_com(fullBody, states[state_id+1]) - p, N = fullBody.computeContactPoints(state_id) - fly_time = phase_dt [1] - support_time = phase_dt [0] - t_end_phases = [0] - [t_end_phases.append(t_end_phases[-1]+fly_time) for _ in range(len(p))] - if(len(t_end_phases) == 4): - t_end_phases[1] = support_time - 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)(round(el*100.))) / 100. for el in 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]) - COMConstraints = None - if(not (limbsCOMConstraints == None)): - 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 cones ', len(cones) - return p, N, init_com, end_com, t_end_phases, cones, COMConstraints - -def compute_initial_guess(fullBody,t_end_phases,pathId,state_id,dt=0.01): - nbSteps = int(t_end_phases[-1] / dt) # FIXME : prendre dt en parametre - initial_guess = np.zeros(nbSteps*6).tolist() - t_init = fullBody.getTimeAtState(state_id) - for i in range(0,nbSteps): - initial_guess[i*3:3+i*3] = fullBody.client.basic.problem.configAtParam(pathId,t_init+i*dt)[-3:] # acceleration - initial_guess[(nbSteps*3) + i*3: (nbSteps*3) + 3+ i*3] = [0,0,0] # angular momentum - #print "initial guess velocity = ",initial_guess - return initial_guess - -def gen_trajectory(fullBody, states, state_id, computeCones = False, mu = 1, dt=0.01, phase_dt = [0.4, 1], -reduce_ineq = True, verbose = False, limbsCOMConstraints = None, profile = False, use_window = 0,use_velocity=False, pathId = 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 -<<<<<<< HEAD - #print "phase dt : ", phase_dt - #assert( len(phase_dt) >= 2 + use_window * 2 ), "phase_dt does not describe all phases" - configSize = len(states[state_id]) - #constraints = ['cones_constraint', 'end_reached_constraint','end_speed_constraint'] - constraints = ['end_reached_constraint','cones_constraint'] -======= - 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', 'com_kinematic_constraint'] - constraints = ['cones_constraint', 'end_reached_constraint'] - #~ constraints = ['end_reached_constraint'] - #~ constraints = ['cones_constraint', 'end_reached_constraint'] - #~ constraints = ['cones_constraint'] ->>>>>>> rewrite_p2 - #~ constraints = ['cones_constraint', 'end_reached_constraint','end_speed_constraint', 'com_kinematic_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,pathId) - if(not use_velocity): - initial_guess = [] - if(use_window > 0): - init_waypoint_time = int(np.round(t_end_phases[-1]/ dt)) - 1 - init_end_com = end_com[:] - constraints = ['cones_constraint', 'end_reached_constraint','end_speed_constraint'] - for w in range(1,use_window+1): - waypoint = end_com[:] - waypoint_time = int(np.round(t_end_phases[-1]/ dt)) - 1 - # trying not to apply constraint - #~ 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], mu, computeCones, limbsCOMConstraints, pathId) - 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:]] - initial_guess = compute_initial_guess(fullBody,t_end_phases,pathId,state_id) - if (not profile): - print "num cones ", len(cones) - print "end_phases", t_end_phases - - timeelapsed = 0 - if (profile): - start = time.clock() - if(use_velocity): - init_vel = states[state_id][configSize-6 : configSize-3] - end_vel = states[state_id+1][configSize-6 : configSize-3] - else: - init_vel=[0,0,0] - end_vel=[0,0,0] - print "init_vel =" - print init_vel - print "end_vel = " - print end_vel - var_final, params = cone_optimization(p, N, [init_com + init_vel, end_com + end_vel], t_end_phases[1:], dt, cones, COMConstraints, mu, mass, 9.81, reduce_ineq, verbose, - constraints, param_constraints,initial_guess = initial_guess) - #~ print "end_com ", end_com , "computed end come", var_final['c'][-1], var_final['c_end'] - if (profile): - end = time.clock() - timeelapsed = (end - start) * 1000 - #~ print "solving time", timeelapsed - if(use_window > 0): - var_final['c_old'] = var_final['c'][:] - var_final['dc_old'] = var_final['dc'][:] - var_final['ddc_old'] = var_final['ddc'][:] - var_final['c'] = var_final['c'][:init_waypoint_time+1] - var_final['dc'] = var_final['dc'][:init_waypoint_time+1] - var_final['ddc'] = var_final['ddc'][:init_waypoint_time+1] - params["t_init_phases"] = params["t_init_phases"][:-3*use_window] - 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())): - #~ 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'][init_waypoint_time] - print "init speed", lastspeed - else: - use_window = 0 - print "reached com is not good, restarting problem with window = ",use_window - return gen_trajectory(fullBody, states, state_id, computeCones, mu, dt, phase_dt, reduce_ineq, verbose, limbsCOMConstraints, profile, use_window = use_window, pathId = pathId) - 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] -<<<<<<< HEAD - print "restarting problem with windows = ",use_window+1 - return gen_trajectory(fullBody, states, state_id, computeCones, mu, dt, phase_dt ,reduce_ineq, verbose, limbsCOMConstraints, profile , use_window + 1,use_velocity, pathId) - lastspeed = np.array([0,0,0]) -======= - 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]) - lastspeed = var_final['dc'][-1] - print "end speed", lastspeed ->>>>>>> rewrite_p2 - - return var_final, params, timeelapsed, cones - -<<<<<<< HEAD -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,use_velocity=False, pathId = 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,use_velocity=use_velocity, pathId = pathId) -======= -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, cones = gen_trajectory(fullBody, states, state_id, computeCones, mu , dt, phase_dt, reduce_ineq, verbose, limbsCOMConstraints, False, use_window = use_window) ->>>>>>> rewrite_p2 - p, N = fullBody.computeContactPoints(state_id) - print "p", p - fig = plt.figure() - ax = fig.add_subplot(111, projection='3d') - n = 100 - points = var_final['x'] - xs = [points[i] for i in range(0,len(points),6)] - ys = [points[i] for i in range(1,len(points),6)] - zs = [points[i] for i in range(2,len(points),6)] - ax.scatter(xs, ys, zs, c='b') - - colors = ["r", "b", "g"] - #print contact points of first phase - for id_c, points in enumerate(p): - xs = [point[0] for point in points] - ys = [point[1] for point in points] - zs = [point[2] for point in points] - ax.scatter(xs, ys, zs, c=colors[id_c]) - - ax.set_xlabel('X Label') - ax.set_ylabel('Y Label') - ax.set_zlabel('Z Label') - -<<<<<<< HEAD - #plt.show() - plt.savefig('/tmp/figCWC/c'+ str(state_id)+ '.png') -======= - plt.show() - #~ plt.savefig('/tmp/c'+ str(state_id)+ '.png') ->>>>>>> rewrite_p2 - - print "plotting speed " - print "end target ", params['x_end'] - fig = plt.figure() - ax = fig.add_subplot(111) - if(use_window > 0): - #~ points = var_final['dc_old'] - points = var_final['dc'] - else: - points = var_final['dc'] - - #~ print "points", points - ys = [norm(el) * el[0] / abs(el[0]+ _EPS) for el in points] - xs = [i * params['dt'] for i in range(0,len(points))] - ax.scatter(xs, ys, c='b') - - -<<<<<<< HEAD - #~ plt.show() - plt.savefig('/tmp/figCWC/dc'+ str(state_id)+ '.png') -======= - plt.show() - #~ plt.savefig('/tmp/dc'+ str(state_id)+ '.png') ->>>>>>> rewrite_p2 - - print "plotting acceleration " - fig = plt.figure() - ax = fig.add_subplot(111) - if(use_window > 0): - #~ points = var_final['ddc_old'] - points = var_final['ddc'] - else: - points = var_final['ddc'] - ys = [norm(el) * el[0] / abs(el[0]+ _EPS) for el in points] - xs = [i * params['dt'] for i in range(0,len(points))] - ax.scatter(xs, ys, c='b') - - -<<<<<<< HEAD - #~ plt.show() - plt.savefig('/tmp/figCWC/ddc'+ str(state_id)+ '.png') -======= - plt.show() - plt.savefig('/tmp/ddc'+ str(state_id)+ '.png') ->>>>>>> rewrite_p2 - - print "plotting Dl " - fig = plt.figure() - ax = fig.add_subplot(111) - points = var_final['dL'] - ys = [norm(el) * el[0] / abs(el[0]+ _EPS) for el in points] - xs = [i * params['dt'] for i in range(0,len(points))] - ax.scatter(xs, ys, c='b') - - -<<<<<<< HEAD - #~ plt.show() - plt.savefig('/tmp/figCWC/dL'+ str(state_id)+ '.png') - return var_final, params, elapsed -======= - plt.show() - plt.savefig('/tmp/dL'+ str(state_id)+ '.png') - return var_final, params, elapsed, cones ->>>>>>> rewrite_p2 - -def __cVarPerPhase(var, dt, t, final_state, addValue): - varVals = addValue + [v.tolist() for v in final_state[var]] - print "cVarPerPhase : t = ", t - varPerPhase = [[varVals[(int)(round(t_id/dt)) ] for t_id in np.arange(t[index],t[index+1]-_EPS,dt)] for index, _ in enumerate(t[:-1]) ] - #print "varperPhase =" - #print varPerPhase - varPerPhase[2].append(varVals[-1]) - if(not var == "ddc"): - assert len(varVals) == len(varPerPhase[0]) + len(varPerPhase[1]) + len(varPerPhase[2]), mess - - if var == "dc": - varPerPhase[2] = varPerPhase[2][:-1] # not relevant for computation - else: - varPerPhase[0].append(varPerPhase[1][0]) # end pos of state is the same as the previous one - varPerPhase[1].append(varPerPhase[2][0]) - if var == "ddc": #acceleration: remove first - varPerPhase = [v[1:] for v in varPerPhase] - assert len(final_state[var]) == len(varPerPhase[0]) + len(varPerPhase[1]) + len(varPerPhase[2]), "incorrect num of ddc" - return varPerPhase - -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 = 0,use_velocity=False, pathId = 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,use_velocity, pathId) - else: - res = gen_trajectory(fullBody, states, state_id, computeCones, mu, dt, phase_dt, reduce_ineq, verbose, limbsCOMConstraints, profile, use_window,use_velocity, pathId) - alpha = res[1]['alpha'] - print "t in optim_threading :",res[1]["t_init_phases"] - t = [ti * alpha for ti in res[1]["t_init_phases"]] - print "t after alpha in optim_threading :",t - dt = res[1]["dt"] * alpha - final_state = res[0] - c0 = res[1]["x_init"][0:3] - dc0 = res[1]["x_init"][3:7] - comPosPerPhase = __cVarPerPhase('c' , dt, t, final_state, [c0]) - comVelPerPhase = __cVarPerPhase('dc' , dt, t, final_state, [dc0]) - comAccPerPhase = __cVarPerPhase('ddc', dt, t, final_state, [[0,0,0]]) - - #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[-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], -<<<<<<< HEAD -reduce_ineq = True, num_optims = 0, draw = False, verbose = False, limbsCOMConstraints = None, profile = False, use_window = 0, trackedEffectors = [],use_velocity=False,pathId = 0): - comPosPerPhase, timeElapsed, final_state = __optim__threading_ok(fullBody, states, state_id, computeCones, mu, dt, phase_dt, - reduce_ineq, num_optims, draw, verbose, limbsCOMConstraints, profile, use_window,use_velocity, pathId) -======= -reduce_ineq = True, num_optims = 0, draw = False, verbose = False, limbsCOMConstraints = None, profile = False, use_window = 0, trackedEffectors = []): - 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) ->>>>>>> rewrite_p2 - 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, cones - -def solve_effector_RRT(fullBody, states, state_id, computeCones = False, mu = 1, dt =0.1, phase_dt = [0.4, 1], -<<<<<<< HEAD -reduce_ineq = True, num_optims = 0, draw = False, verbose = False, limbsCOMConstraints = None, profile = False, use_window = 0, trackedEffectors = [],use_velocity=False,pathId = 0): - comPosPerPhase, timeElapsed, final_state = __optim__threading_ok(fullBody, states, state_id, computeCones, mu, dt, phase_dt, - reduce_ineq, num_optims, draw, verbose, limbsCOMConstraints, profile, use_window,use_velocity, pathId) -======= -reduce_ineq = True, num_optims = 0, draw = False, verbose = False, limbsCOMConstraints = None, profile = False, use_window = 0, trackedEffectors = []): - 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) ->>>>>>> rewrite_p2 - print "done. generating state trajectory ",state_id - if(len(trackedEffectors) == 0): - paths_ids = [int(el) for el in fullBody.effectorRRT(state_id,comPosPerPhase[0],comPosPerPhase[1],comPosPerPhase[2],num_optims)] - else: - print "handling extra effector constraints" - 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, cones - diff --git a/src/hpp/corbaserver/rbprm/tools/cwc_trajectory_helper.py.orig b/src/hpp/corbaserver/rbprm/tools/cwc_trajectory_helper.py.orig deleted file mode 100644 index ebf34a0d..00000000 --- a/src/hpp/corbaserver/rbprm/tools/cwc_trajectory_helper.py.orig +++ /dev/null @@ -1,529 +0,0 @@ - -from hpp.corbaserver.rbprm.tools.cwc_trajectory import * -from hpp.corbaserver.rbprm.tools.path_to_trajectory import * -from cwc import OptimError, cone_optimization -from hpp.corbaserver.rbprm.tools.path_to_trajectory import gen_trajectory_to_play -from numpy import append, array - -#global variables -res = [] -trajec = [] -trajec_mil = [] -contacts = [] -pos = [] -normals = [] -pEffs = [] -coms = [] -errorid = [] -cones_saved = [] - -def displayComPath(pp, 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() - - -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 - #~ print "dt_framerate", dt_framerate - #~ print "num_frames_required", times[idx], " ", num_frames_required - dt = float(length) / num_frames_required - 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] - cRes+= [cones[idx] for t in dt_finals] - return pRes, nRes, freeEffectors, cRes - - -def __getPos(effector, fullBody, config): - fullBody.setCurrentConfig (config) - q = fullBody.getJointPosition(effector) - quat_end = q[4:7] - q[6] = q[3] - q[3:6] = quat_end - return q - -def genPEffperFrame(fullBody, freeEffectorsPerPhase, qs, pp, times, dt_framerate): - res = [] - for idx, phase in enumerate(freeEffectorsPerPhase): - num_frames_required = int(times[idx] / dt_framerate) - qid = len(res) - for q in qs[qid:num_frames_required+qid]: - p = {} - for effector in phase: - p[effector] = __getPos(effector, fullBody, q) - res.append(p) - return res - - -def genComPerFrame(final_state, dt, dt_framerate = 1./1000.): - num_frames_per_dt = int(round(dt / dt_framerate)) - inc = 1./((float)(num_frames_per_dt)) - c = [array(final_state['x_init'][:3])] + final_state['c'] - dc = [array(final_state['x_init'][3:])] + final_state['dc'] - ddc = final_state['ddc'] - cs = [] - for i in range(0,len(c)-1): - for j in range(num_frames_per_dt): - ddt = j * inc * dt - cs.append(c[i] + ddt *dc[i] + ddt *ddt * 0.5 * ddc[i]) - return cs - -stat_data = { -"error_com_proj" : 0, -"error_optim_fail" : 0, -"error_unknown" : 0, -"num_errors" : 0, -"num_success" : 0, -"num_trials" : 0, -"time_cwc" : { "min" : 10000000., "avg" : 0., "max" : 0., "totaltime" : 0., "numiter" : 0 }, -} - -def __update_cwc_time(t): - global stat_data - stat_data["time_cwc"]["min"] = min(stat_data["time_cwc"]["min"], t) - stat_data["time_cwc"]["max"] = max(stat_data["time_cwc"]["max"], t) - stat_data["time_cwc"]["totaltime"] += t - stat_data["time_cwc"]["numiter"] += 1 - -""" -def __getTimes(fullBody, configs, i, time_scale): - 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 - -""" - -def __getTimes(fullBody, configs, i, time_scale,use_window=0): - t = fullBody.getTimeAtState(i+1) - fullBody.getTimeAtState(i) - dt = 0.01 - print "t = ",t - t = time_scale*t - print "after scale, t = ",t - 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) - # TODO : si t = 0, hardcoded ... - if t <= dt: - print "WARNING : in getTime, t=0" - t = 0.1 - use_window = use_window+1 - times = [0.02 , 0] #FIXME : hardcoded value depend on interpolation step choosen (not available here) - times[1] = t - 2*times[0] - print "times : ",times - return times, dt, distance,use_window - - -from hpp import Error as hpperr -import sys, time -<<<<<<< HEAD -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, -trackedEffectors = [],use_velocity=False,pathId = 0): - print "##########################################" -======= -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,trackedEffectors = [], saveForSim=False,): ->>>>>>> rewrite_p2 - global errorid - global stat_data - fail = 0 - #~ try: - print "Use window = ",use_window - if(True): - times = []; - dt = 1000; - times, dt, distance,use_window = __getTimes(fullBody, configs, i, time_scale,use_window) - print "Use window = ",use_window - if distance == 0: - use_window = use_window+1 - use_window = max(0, min(use_window, (len(configs) - 1) - (i + 2))) # can't use preview if last state is reached - w = 0 - while w < (use_window+1): - times2, dt2, dist2,use_window = __getTimes(fullBody, configs, i+w, time_scale,use_window) - print "Use window = ",use_window - times += times2 - dt = min(dt, dt2) - w = w+1 - time_per_path = [times[0]] + [times[1]] + [times [0]] - print 'time per path', times, time_per_path - print 'dt', dt - if(distance > 0.0001): - stat_data["num_trials"] += 1 - if(useCOMConstraints): - comC = limbsCOMConstraints - else: - comC = None - if(optim_effectors): -<<<<<<< HEAD - 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,use_velocity=use_velocity,pathId = pathId) - 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,use_velocity=use_velocity,pathId = pathId) -======= - 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, cones = solve_com_RRT(fullBody, configs, i, True, friction, dt, times, False, optim, draw, verbose, comC, False, use_window=use_window, trackedEffectors = trackedEffectors) ->>>>>>> rewrite_p2 - displayComPath(pp, pid) - #~ pp(pid) - global res - res = res + [pid] - global trajec - global trajec_mil -<<<<<<< HEAD - frame_rate = 0.01 - frame_rate_andrea = 1./100. -# frame_rate_andrea = 1./1000. - #~ if(len(trajec) > 0): - #~ frame_rate = 1./25. - #~ frame_rate_andrea = 1./1001. - print "first traj :" - new_traj = gen_trajectory_to_play(fullBody, pp, trajectory, time_per_path, frame_rate) - print "traj 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) - #~ if(len(trajec) > 0): - #~ new_traj = new_traj[1:] - #~ new_traj_andrea = new_traj_andrea[1:] - #~ Ps = Ps[1:] - #~ Ns = Ns[1:] - #~ com = com[1:] - #~ NPeffs = NPeffs[1:] - trajec = trajec + new_traj - 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)) -======= - frame_rate = 1./24. - new_traj = gen_trajectory_to_play(fullBody, pp, trajectory, time_per_path, frame_rate) - trajec = trajec + new_traj - if(saveForSim): - frame_rate_sim = 1./1000. - new_traj_andrea = gen_trajectory_to_play(fullBody, pp, trajectory, time_per_path,frame_rate_sim) - 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) - trajec_mil += new_traj_andrea - #~ global contacts - #~ contacts += new_contacts - 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)) ->>>>>>> rewrite_p2 - stat_data["num_success"] += 1 - else: - print "TODO, NO CONTACT VARIATION, LINEAR INTERPOLATION REQUIRED" - #~ except hpperr as e: - #~ print "hpperr failed at id " + str(i) , e.strerror - #~ if (use_window == 0 and (len(configs) - 1) - (i + 2) > 0): - #~ print "could not project com, trying to increase velocity " - #~ try: - #~ return step(fullBody, configs, i, optim, pp, limbsCOMConstraints, friction, optim_effectors, time_scale, useCOMConstraints, 1, verbose, draw, trackedEffectors = trackedEffectors) - #~ except: - #~ if ((len(configs) - 1) - (i + 3) > 0): - #~ print "could not project com, trying to increase velocity more " - #~ step(fullBody, configs, i, optim, pp, limbsCOMConstraints, friction, optim_effectors, time_scale, useCOMConstraints, 2, verbose, draw, trackedEffectors = trackedEffectors) - #~ else: - #~ print "In hpperr and window != 0" - #~ print "hpperr failed at id " + str(i) , e.strerror - #~ stat_data["error_com_proj"] += 1 - #~ stat_data["num_errors"] += 1 - #~ errorid += [i] - #~ fail+=1 - #~ except OptimError as e: - #~ print "OptimError failed at id " + str(i) , e - #~ stat_data["error_optim_fail"] += 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: - #~ print e - #~ if (use_window == 0 and (len(configs) - 1) - (i + 2) > 0): - #~ print "could not project com, trying to increase velocity " - #~ try: - #~ return step(fullBody, configs, i, optim, pp, limbsCOMConstraints, friction, optim_effectors, time_scale, useCOMConstraints, 1, verbose, draw, trackedEffectors = trackedEffectors) - #~ except: - #~ print "faile twice" - #~ if ((len(configs) - 1) - (i + 3) > 0): - #~ print "could not project com, trying to increase velocity more " - #~ step(fullBody, configs, i, optim, pp, limbsCOMConstraints, friction, optim_effectors, time_scale, useCOMConstraints, 2, verbose, draw, trackedEffectors = trackedEffectors) - #~ else: - #~ print "In Exception and window != 0" - #~ stat_data["error_unknown"] += 1 - #~ stat_data["num_errors"] += 1 - #~ print e - #~ errorid += [i] - #~ fail+=1 - #~ except: - #~ print "unknown" - #~ if (use_window == 0 and (len(configs) - 1) - (i + 2) > 0): - #~ print "could not project com, trying to increase velocity " - #~ try: - #~ return step(fullBody, configs, i, optim, pp, limbsCOMConstraints, friction, optim_effectors, time_scale, useCOMConstraints, 1, verbose, draw, trackedEffectors = trackedEffectors) - #~ except: - #~ if ((len(configs) - 1) - (i + 3) > 0): - #~ print "could not project com, trying to increase velocity more " - #~ step(fullBody, configs, i, optim, pp, limbsCOMConstraints, friction, optim_effectors, time_scale, useCOMConstraints, 2, verbose, draw, trackedEffectors = trackedEffectors) - #~ else: - #~ print "In unknown and window != 0" - #~ stat_data["error_unknown"] += 1 - #~ stat_data["num_errors"] += 1 - #~ errorid += [i] - #~ fail+=1 - #~ 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 - global stat_data - fail = 0 - try: - 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 - if(distance > 0.0001): - stat_data["num_trials"] += 1 - if(useCOMConstraints): - comC = limbsCOMConstraints - else: - comC = None - if(optim_effectors): - pid, trajectory, timeelapsed, final_state = solve_effector_RRT(fullBody, configs, i, True, friction, dt, times, False, optim, False, False, comC, True) - else : - pid, trajectory, timeelapsed, final_state = solve_com_RRT(fullBody, configs, i, True, friction, dt, times, False, optim, False, False, comC, True) - __update_cwc_time(timeelapsed) - stat_data["num_success"] += 1 - else: - print "TODO, NO CONTACT VARIATION, LINEAR INTERPOLATION REQUIRED" - except hpperr as e: - print "hpperr failed at id " + str(i) , e.strerror - stat_data["error_com_proj"] += 1 - stat_data["num_errors"] += 1 - errorid += [i] - fail+=1 - except OptimError as e: - print "OptimError failed at id " + str(i) , e.strerror - stat_data["error_optim_fail"] += 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 - except: - stat_data["error_unknown"] += 1 - stat_data["num_errors"] += 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) - - - -import time - -def __isDiff(P0, P1): - return len(set(P0.keys()) - set(P1.keys())) != 0 or len(set(P1.keys()) - set(P0.keys())) - -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)): - if i == 0 or __isDiff(pos[i], pos[i-1]): - a = {} - for effector in pos[i].keys(): - a[effector] = {'P' : pos[i][effector], 'N' : normals[i][effector]} - frameswitches.append([i,a]) - 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+') - dump(res, f1) - f1.close() - return res - -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, '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) - f1.close() - return compressData(res,filename) - -def clean(): - global res - global trajec - global trajec_mil - global contacts - global errorid - global pos - global normals - global pEffs - global coms - global cones_saved - cones_saved = [] - res = [] - trajec = [] - trajec_mil = [] - #~ contacts = [] - errorid = [] - pos = [] - normals = [] - pEffs = [] - coms = [] - -import copy - -def stats(): - global stat_data - stat_data["error_id"] = errorid - stat_data_copy = copy.deepcopy(stat_data) - return stat_data_copy - -def write_stats(filename): - global stat_data - sd = copy.deepcopy(stat_data) - f = open(filename, 'a') - f.write("optim_error_com_proj " + str(sd["error_com_proj"]) + "\n") - f.write("optim_error_optim_fail " + str(sd["error_optim_fail"]) + "\n") - f.write("optim_error_unknown " + str(sd["error_unknown"]) + "\n") - 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 - -def profile(fullBody, configs, i_start, i_end, limbsCOMConstraints, friction = 0.5, optim_effectors = False, time_scale = 20., useCOMConstraints = False, filename ="log.txt"): - global stat_data - if(i_end > len(configs)-1): - print "ERROR: i_end < len_configs ", i_end, len(configs)-1 - return # no point in trying optim, path was not fully computed - for i in range(i_start, i_end): - step_profile(fullBody, configs, i, 0, limbsCOMConstraints, friction, optim_effectors, time_scale, useCOMConstraints) - stat_data["time_cwc"]["avg"] = stat_data["time_cwc"]["totaltime"] / float(stat_data["time_cwc"]["numiter"]) - write_stats(filename) - -def saveAllData(fullBody, r, name): - global trajec - fullBody.exportAll(r, trajec, name) - return saveToPinocchio(name) - -def play_traj(fullBody,pp,frame_rate): - global trajec - return play_trajectory(fullBody,pp,trajec,frame_rate) - -#~ fullBody.exportAll(r, trajec, 'darpa_hyq_t_var_04f_andrea'); -#~ saveToPinocchio('darpa_hyq_t_var_04f_andrea')