Commit 50a3c3c1 authored by Steve Tonneau's avatar Steve Tonneau
Browse files

working on effector trajhectories

parent 49f5ca89
import matplotlib
#~ matplotlib.use('Agg')
import matplotlib.pyplot as plt
from hpp.corbaserver.rbprm.rbprmbuilder import Builder
from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
......@@ -147,32 +150,11 @@ from numpy import matlib
#~ for i in range(6,25):
#~ act(i, 60, optim_effectors = True)
#~ pid = act(8,0,optim_effectors = False)
from hpp.corbaserver.rbprm.tools.cwc_trajectory_helper import res
pid = res[0]-2
dt = 0.001
#~ qs = [ pp.client.problem.configAtParam (pid, i*dt) for i in range(600,700)]
#~ cs = []
#~ res = zeros((3, 100))
#~ for i in range(len(qs)):
#~ q = qs[i]
#~ fullBody.setCurrentConfig(q[:-1])
#~ c = fullBody.getCenterOfMass()
#~ cs += [c[0]]
#~ res[:,i] = c
#~ for i in range(8,9): act(i,0,optim_effectors=False, draw=False)
#~ ddc = [2 *(cs[i-1] - c[i]) for i in range(1, len(c))]
#~ ddcx = [ddci[0] for ddci in ddc]
#~ res = zeros((3, 100))
for i in range(7,8):
act(i,0, use_window=0, verbose=True, optim_effectors=False, draw=False)
#~ for i,ci in enumerate(c):
#~ res[:,i] = ci
#~ m = matrix(res)
#~ pid = act(8,0,optim_effectors = False)
from derivative_filters import *
......@@ -188,6 +170,7 @@ import plot_utils
from hpp.corbaserver.rbprm.tools.cwc_trajectory_helper import trajec_mil
from hpp.corbaserver.rbprm.tools.cwc_trajectory_helper import trajec
from hpp.corbaserver.rbprm.tools.cwc_trajectory_helper import coms
#~ plt.plot(x[0,:].A.squeeze())
#~ plt.plot(dx[0,:].A.squeeze())
......
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
......@@ -155,9 +159,6 @@ reduce_ineq = True, verbose = False, limbsCOMConstraints = None, profile = 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
import matplotlib.pyplot as plt
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
n = 100
......@@ -179,14 +180,16 @@ def draw_trajectory(fullBody, states, state_id, computeCones = False, mu = 1, d
ax.set_ylabel('Y Label')
ax.set_zlabel('Z Label')
plt.show()
#~ plt.show()
plt.savefig('/tmp/c'+ str(state_id)+ '.png')
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_old']
points = var_final['dc']
else:
points = var_final['dc']
......@@ -196,13 +199,15 @@ def draw_trajectory(fullBody, states, state_id, computeCones = False, mu = 1, d
ax.scatter(xs, ys, c='b')
plt.show()
#~ plt.show()
plt.savefig('/tmp/dc'+ str(state_id)+ '.png')
print "plotting acceleration "
fig = plt.figure()
ax = fig.add_subplot(111)
if(use_window > 0):
points = var_final['ddc_old']
#~ 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]
......@@ -210,7 +215,8 @@ def draw_trajectory(fullBody, states, state_id, computeCones = False, mu = 1, d
ax.scatter(xs, ys, c='b')
plt.show()
#~ plt.show()
plt.savefig('/tmp/ddc'+ str(state_id)+ '.png')
print "plotting Dl "
fig = plt.figure()
......@@ -221,7 +227,8 @@ def draw_trajectory(fullBody, states, state_id, computeCones = False, mu = 1, d
ax.scatter(xs, ys, c='b')
plt.show()
#~ plt.show()
plt.savefig('/tmp/dL'+ str(state_id)+ '.png')
return var_final, params, elapsed
def __cVarPerPhase(var, dt, t, final_state, addValue):
......
......@@ -74,11 +74,15 @@ def genPEffperFrame(fullBody, freeEffectorsPerPhase, qs, pp, times, dt_framerate
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 = []
[cs.append(c[i] + ddt*dt *dc[i] + ddt*dt *ddt* dt * 0.5 * ddc[i]) for ddt in range(num_frames_per_dt) for i in range(0,len(ddc))]
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 = {
......@@ -164,14 +168,13 @@ trackedEffectors = []):
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:]
#~ new_contacts = new_contacts[1:]
Ps = Ps[1:]
Ns = Ns[1:]
com = com[1:]
pEffs = pEffs[1:]
#~ 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
......
......@@ -66,10 +66,8 @@ def gen_trajectory_to_play(robot, path_player, path_ids, total_time_per_paths, d
config_size_path = len(path_player.client.problem.configAtParam (path_id, 0))
if(config_size_path > config_size):
#~ if(i == 1 ):
print "a traj"
res+= follow_trajectory_path(robot, path_player, path_id, total_time_per_paths[i], dt_framerate)
else:
print "a path"
res+= linear_interpolate_path(robot, path_player, path_id, total_time_per_paths[i], dt_framerate)
activeid +=1
return res
......
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