Commit 38e8cad1 authored by Pierre Fernbach's avatar Pierre Fernbach
Browse files

Fix computation of t_init_phase

parent 438b7d92
......@@ -19,7 +19,7 @@ pId = tp.ps.numberPaths() -1
fullBody = FullBody ()
fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
fullBody.setJointBounds ("base_joint_xyz", [-2,5, 0, 2, 0.45, 1.8])
fullBody.setJointBounds ("base_joint_xyz", [-2,5, 0.5, 1.5, 0.3, 1.8])
fullBody.client.basic.robot.setDimensionExtraConfigSpace(tp.extraDof)
ps = tp.ProblemSolver( fullBody )
......
......@@ -106,12 +106,21 @@ r(q_init)
#r.solveAndDisplay("rm",1,0.01)
t = ps.solve ()
from hpp.gepetto import PathPlayer
pp = PathPlayer (rbprmBuilder.client.basic, r)
pp.dt=0.03
pp.displayVelocityPath(0)
r.client.gui.setVisibility("path_0_root","ALWAYS_ON_TOP")
"""
if isinstance(t, list):
t = t[0]* 3600000 + t[1] * 60000 + t[2] * 1000 + t[3]
f = open('log.txt', 'a')
f.write("path computation " + str(t) + "\n")
f.close()
"""
"""
for i in range(0,9):
......@@ -156,11 +165,6 @@ for i in range(0,ps.numberNodes()):
r.client.gui.removeFromGroup("vecRM"+str(i),r.sceneName)
"""
from hpp.gepetto import PathPlayer
pp = PathPlayer (rbprmBuilder.client.basic, r)
pp.dt=0.03
pp.displayVelocityPath(0)
r.client.gui.setVisibility("path_0_root","ALWAYS_ON_TOP")
"""
......
......@@ -131,7 +131,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)
initial_guess = compute_initial_guess(fullBody,t_end_phases,pathId,state_id,dt)
if (not profile):
print "num cones ", len(cones)
print "end_phases", t_end_phases
......@@ -163,7 +163,7 @@ reduce_ineq = True, verbose = False, limbsCOMConstraints = None, profile = False
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]
params["t_init_phases"] = params["t_init_phases"][:4]
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]
......
......@@ -133,12 +133,15 @@ def __getTimes(fullBody, configs, i, time_scale,use_window=0):
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:
if t <= dt*6.:
print "WARNING : in getTime, t=0"
t = 0.1
t = dt*6.
use_window = use_window+1
times = [0.02 , 0] #FIXME : hardcoded value depend on interpolation step choosen (not available here)
times = [dt*2. , 0] #FIXME : hardcoded value depend on interpolation step choosen (not available here)
if t > dt*14.:
times = [dt*4. , 0]
times[1] = t - 2*times[0]
times[1] = float((int)(math.floor(times[1]*100.))) / 100.
print "times : ",times
return times, dt, distance,use_window
......
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