From 2a2972ce27c10f86562228ed2cc452cdfac68a70 Mon Sep 17 00:00:00 2001
From: Pierre Fernbach <pierre.fernbach@laas.fr>
Date: Fri, 20 Jan 2017 15:20:10 +0100
Subject: [PATCH] correctly compute the initial_guess_velocity if use_windows >
0
---
.../corbaserver/rbprm/tools/cwc_trajectory.py | 25 ++++++++---------
.../rbprm/tools/cwc_trajectory_helper.py | 27 ++++++++++---------
2 files changed, 28 insertions(+), 24 deletions(-)
diff --git a/src/hpp/corbaserver/rbprm/tools/cwc_trajectory.py b/src/hpp/corbaserver/rbprm/tools/cwc_trajectory.py
index 9d4b8a74..a9ffb600 100644
--- a/src/hpp/corbaserver/rbprm/tools/cwc_trajectory.py
+++ b/src/hpp/corbaserver/rbprm/tools/cwc_trajectory.py
@@ -86,16 +86,17 @@ 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))
- dt = 0.01
- nbSteps = int(t_end_phases[3] / 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 p, N, init_com, end_com, t_end_phases, cones, COMConstraints, initial_guess
+ 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.2, phase_dt = [0.4, 1],
reduce_ineq = True, verbose = False, limbsCOMConstraints = None, profile = False, use_window = 0,use_velocity=False, pathId = 0):
@@ -109,7 +110,7 @@ reduce_ineq = True, verbose = False, limbsCOMConstraints = None, profile = False
param_constraints = []
mass = fullBody.getMass()
- p, N, init_com, end_com, t_end_phases, cones, COMConstraints, initial_guess = compute_state_info(fullBody,states, state_id, phase_dt[:2], mu, computeCones, limbsCOMConstraints,pathId)
+ 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):
@@ -120,7 +121,7 @@ reduce_ineq = True, verbose = False, limbsCOMConstraints = None, profile = False
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, initial_guess = compute_state_info(fullBody,states, state_id+w, phase_dt[2*w:], mu, computeCones, limbsCOMConstraints, pathId)
+ 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, pathId)
p+=p1;
N+=N1;
end_com = end_com1;
@@ -128,7 +129,7 @@ reduce_ineq = True, verbose = False, limbsCOMConstraints = None, profile = False
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
diff --git a/src/hpp/corbaserver/rbprm/tools/cwc_trajectory_helper.py b/src/hpp/corbaserver/rbprm/tools/cwc_trajectory_helper.py
index 80562571..75ea7b0a 100644
--- a/src/hpp/corbaserver/rbprm/tools/cwc_trajectory_helper.py
+++ b/src/hpp/corbaserver/rbprm/tools/cwc_trajectory_helper.py
@@ -122,22 +122,23 @@ def __getTimes(fullBody, configs, i, time_scale):
"""
-def __getTimes(fullBody, configs, i, time_scale):
+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
- # TODO : si t = 0
- if t == 0:
- print "WARNING : in getTime, t=0"
- alpha = 0.1
- times = [0.02 , 0]
- times[1] = t - 2*times[0]
- dt = 0.01
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.15
+ use_window = use_window+1
+ times = [0.05 , 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
+ return times, dt, distance,use_window
from hpp import Error as hpperr
@@ -152,10 +153,12 @@ trackedEffectors = [],use_velocity=False,pathId = 0):
if(True):
times = [];
dt = 1000;
- distance = __getTimes(fullBody, configs, i, time_scale)
+ times, dt, distance,use_window = __getTimes(fullBody, configs, i, time_scale,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
for w in range(use_window+1):
- times2, dt2, dist2 = __getTimes(fullBody, configs, i+w, time_scale)
+ times2, dt2, dist2,unused = __getTimes(fullBody, configs, i+w, time_scale)
times += times2
dt = min(dt, dt2)
time_per_path = [times[0]] + [times[1]] + [times [0]]
@@ -177,7 +180,7 @@ trackedEffectors = [],use_velocity=False,pathId = 0):
res = res + [pid]
global trajec
global trajec_mil
- frame_rate = 1./100
+ frame_rate = 1./60
frame_rate_andrea = 1./100.
# frame_rate_andrea = 1./1000.
#~ if(len(trajec) > 0):
--
GitLab