Commit 4357c47d authored by Steve Tonneau's avatar Steve Tonneau
Browse files

trying to make darpa scenario work

parent 3f6633d0
...@@ -99,9 +99,11 @@ fullBody.setEndState(q_goal,[rLegId,lLegId,rarmId,larmId]) ...@@ -99,9 +99,11 @@ fullBody.setEndState(q_goal,[rLegId,lLegId,rarmId,larmId])
r(q_init) r(q_init)
# computing the contact sequence # computing the contact sequence
configs = fullBody.interpolate(0.12, 10, 10, True) #~ configs = fullBody.interpolate(0.12, 10, 5, True)
#~ configs = fullBody.interpolate(0.08, 10, 10, True)
#~ configs = fullBody.interpolate(0.11, 7, 10, True) #~ configs = fullBody.interpolate(0.11, 7, 10, True)
#~ configs = fullBody.interpolate(0.1, 1, 5, True) #~ configs = fullBody.interpolate(0.1, 1, 10, True)
configs = fullBody.interpolate(0.04, 1, 10, True)
#~ r.loadObstacleModel ('hpp-rbprm-corba', "darpa", "contact") #~ r.loadObstacleModel ('hpp-rbprm-corba', "darpa", "contact")
...@@ -139,3 +141,74 @@ def saveAll(name): ...@@ -139,3 +141,74 @@ def saveAll(name):
#~ fullBody.exportAll(r, trajec, 'darpa_hyq_t_var_04f_andrea'); #~ fullBody.exportAll(r, trajec, 'darpa_hyq_t_var_04f_andrea');
#~ saveToPinocchio('darpa_hyq_t_var_04f_andrea') #~ saveToPinocchio('darpa_hyq_t_var_04f_andrea')
gui = r.client.gui
scene = "oddct"
r.client.gui.createScene(scene)
resolution = 0.03
i = 0
boxname = scene+"/b"+str(i)
gui.addBox(boxname,resolution,resolution,resolution, [1,1,1,1])
gui.applyConfiguration(boxname,[0,0,0,1,0,0,0])
gui.addSceneToWindow(scene,0)
gui.refresh()
import hpp.corbaserver.rbprm.tools.cwc_trajectory_helper as cwc_trajectory_helper
import time
def applycom():
global gui
global com
c = fullBody.getCenterOfMass()
gui.applyConfiguration(boxname,[c[0],c[1],0,1,0,0,0])
gui.refresh()
def go(dt_framerate=1./24.):
path_player = pp
configs = cwc_trajectory_helper.trajec
for q in configs:
start = time.time()
pp.publisher.robotConfig = q
pp.publisher.publishRobots ()
elapsed = time.time() - start
applycom()
if elapsed < dt_framerate :
time.sleep(dt_framerate-elapsed)
#~ for i in range(9,14):
#~ act(i,verbose=True, use_window=0, numOptim=5, optim_effectors=False, draw=False);go()
#~ saveAll("test"+str(i));
#~ for i in range(14,16):
#~ act(i,verbose=True, use_window=1, numOptim=5, optim_effectors=False, draw=False);go()
#~ saveAll("test"+str(i));
#~ for i in range(16,17):
#~ act(i,verbose=True, use_window=2, numOptim=5, optim_effectors=False, draw=False);go()
#~ saveAll("test"+str(i));
#~ for i in range(17,19):
#~ act(i,verbose=True, use_window=1, numOptim=5, optim_effectors=False, draw=False);go()
#~ saveAll("test"+str(i));
#~ for i in range(19,22):
#~ act(i,verbose=True, use_window=1, numOptim=5, optim_effectors=False, draw=False);go()
#~ saveAll("test"+str(i));
#~ for i in range(23,24):
#~ act(i,verbose=True, use_window=2, numOptim=5, optim_effectors=False, draw=False);go()
#~ saveAll("test"+str(i));
#~ for i in range(24,26):
#~ act(i,verbose=True, use_window=1, numOptim=5, optim_effectors=False, draw=False);go()
#~ saveAll("test"+str(i));
for i in range(16,28):
act(i,verbose=True, use_window=0, numOptim=5, optim_effectors=False, draw=False);go()
saveAll("test"+str(i));
for i in range(28,30):
act(i,verbose=True, use_window=0, numOptim=5, optim_effectors=False, draw=False);go()
saveAll("test"+str(i));
for i in range(30,42):
act(i,verbose=True, use_window=1, numOptim=5, optim_effectors=False, draw=False);go()
saveAll("test"+str(i));
for i in range(32,83):
act(i,verbose=True, use_window=0, numOptim=5, optim_effectors=False, draw=False);go()
saveAll("test"+str(i));
...@@ -96,7 +96,9 @@ reduce_ineq = True, verbose = False, limbsCOMConstraints = None, profile = False ...@@ -96,7 +96,9 @@ reduce_ineq = True, verbose = False, limbsCOMConstraints = None, profile = False
use_window = max(0, min(use_window, (len(states) - 1) - (state_id + 2))) # can't use preview if last state is reached use_window = max(0, min(use_window, (len(states) - 1) - (state_id + 2))) # can't use preview if last state is reached
assert( len(phase_dt) >= 2 + use_window * 2 ), "phase_dt does not describe all phases" 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','end_speed_constraint']
#~ constraints = ['cones_constraint', 'end_reached_constraint', 'com_kinematic_constraint']
constraints = ['cones_constraint', 'end_reached_constraint']
#~ constraints = ['end_reached_constraint'] #~ constraints = ['end_reached_constraint']
#~ constraints = ['cones_constraint', 'end_reached_constraint'] #~ constraints = ['cones_constraint', 'end_reached_constraint']
#~ constraints = ['cones_constraint'] #~ constraints = ['cones_constraint']
...@@ -108,6 +110,7 @@ reduce_ineq = True, verbose = False, limbsCOMConstraints = None, profile = False ...@@ -108,6 +110,7 @@ reduce_ineq = True, verbose = False, limbsCOMConstraints = None, profile = False
if(use_window > 0): if(use_window > 0):
init_waypoint_time = int(np.round(t_end_phases[-1]/ dt)) - 1 init_waypoint_time = int(np.round(t_end_phases[-1]/ dt)) - 1
init_end_com = end_com[:] init_end_com = end_com[:]
constraints = ['cones_constraint', 'end_reached_constraint','end_speed_constraint']
for w in range(1,use_window+1): for w in range(1,use_window+1):
waypoint = end_com[:] waypoint = end_com[:]
waypoint_time = int(np.round(t_end_phases[-1]/ dt)) - 1 waypoint_time = int(np.round(t_end_phases[-1]/ dt)) - 1
...@@ -164,7 +167,9 @@ reduce_ineq = True, verbose = False, limbsCOMConstraints = None, profile = False ...@@ -164,7 +167,9 @@ reduce_ineq = True, verbose = False, limbsCOMConstraints = None, profile = False
print "init speed", lastspeed print "init speed", lastspeed
else: else:
raise ValueError("projection failed, this is bad") raise ValueError("projection failed, this is bad")
lastspeed = np.array([0,0,0]) #~ lastspeed = np.array([0,0,0])
lastspeed = var_final['dc'][-1]
print "end speed", lastspeed
return var_final, params, timeelapsed, cones return var_final, params, timeelapsed, cones
......
...@@ -162,6 +162,7 @@ trackedEffectors = []): ...@@ -162,6 +162,7 @@ trackedEffectors = []):
global trajec_mil global trajec_mil
frame_rate = 1./24. frame_rate = 1./24.
frame_rate_andrea = 1./1000. frame_rate_andrea = 1./1000.
#~ frame_rate_andrea = 1./10.
new_traj = gen_trajectory_to_play(fullBody, pp, trajectory, time_per_path, frame_rate) new_traj = gen_trajectory_to_play(fullBody, pp, trajectory, time_per_path, frame_rate)
new_traj_andrea = gen_trajectory_to_play(fullBody, pp, trajectory, time_per_path,frame_rate_andrea) new_traj_andrea = gen_trajectory_to_play(fullBody, pp, trajectory, time_per_path,frame_rate_andrea)
Ps, Ns, freeEffectorsPerPhase, Ks = genPandNandConesperFrame(fullBody, i, limbsCOMConstraints, cones, pp, trajectory, time_per_path, frame_rate_andrea) Ps, Ns, freeEffectorsPerPhase, Ks = genPandNandConesperFrame(fullBody, i, limbsCOMConstraints, cones, pp, trajectory, time_per_path, frame_rate_andrea)
......
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