Commit 6a1df7bf authored by Steve Tonneau's avatar Steve Tonneau
Browse files

updated demos

parent 916029ca
......@@ -58,27 +58,27 @@ lLegId = 'lhleg'
rarmId = 'rhleg'
larmId = 'lfleg'
addLimbDb(rLegId, "static")
addLimbDb(lLegId, "static")
addLimbDb(rarmId, "static")
addLimbDb(larmId, "static")
#~ addLimbDb(rLegId, "static")
#~ addLimbDb(lLegId, "static")
#~ addLimbDb(rarmId, "static")
#~ addLimbDb(larmId, "static")
#~ fullBody.addLimb(rLegId,rLeg,rfoot,offset,normal, legx, legy, nbSamples, "jointlimits", 0.1, cType)
fullBody.addLimb(rLegId,rLeg,rfoot,offset,normal, legx, legy, nbSamples, "jointlimits", 0.1, cType)
lLegId = 'lhleg'
lLeg = 'lh_haa_joint'
lfoot = 'lh_foot_joint'
#~ fullBody.addLimb(lLegId,lLeg,lfoot,offset,normal, legx, legy, nbSamples, "jointlimits", 0.05, cType)
fullBody.addLimb(lLegId,lLeg,lfoot,offset,normal, legx, legy, nbSamples, "jointlimits", 0.05, cType)
#~
rarmId = 'rhleg'
rarm = 'rh_haa_joint'
rHand = 'rh_foot_joint'
#~ fullBody.addLimb(rarmId,rarm,rHand,offset,normal, legx, legy, nbSamples, "jointlimits", 0.05, cType)
fullBody.addLimb(rarmId,rarm,rHand,offset,normal, legx, legy, nbSamples, "jointlimits", 0.05, cType)
#~
larmId = 'lfleg'
larm = 'lf_haa_joint'
lHand = 'lf_foot_joint'
#~ fullBody.addLimb(larmId,larm,lHand,offset,normal, legx, legy, nbSamples, "jointlimits", 0.05, cType)
fullBody.addLimb(larmId,larm,lHand,offset,normal, legx, legy, nbSamples, "jointlimits", 0.05, cType)
q_0 = fullBody.getCurrentConfig();
q_init = fullBody.getCurrentConfig(); q_init[0:7] = tp.q_init[0:7]
......@@ -198,17 +198,23 @@ def go(dt_framerate=1./24.):
#~ 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()
for i in range(14,19):
act(i,verbose=True, use_window=0, numOptim=5, optim_effectors=True, draw=False);go()
saveAll("test"+str(i));
for i in range(19,22):
act(i,verbose=True, use_window=1, numOptim=5, optim_effectors=True, draw=False);go()
saveAll("test"+str(i));
for i in range(22,28):
act(i,verbose=True, use_window=0, numOptim=5, optim_effectors=True, 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()
act(i,verbose=True, use_window=0, numOptim=5, optim_effectors=True, 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()
act(i,verbose=True, use_window=1, numOptim=5, optim_effectors=True, 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()
act(i,verbose=True, use_window=0, numOptim=5, optim_effectors=True, draw=False);go()
saveAll("test"+str(i));
......@@ -172,19 +172,7 @@ def contactPlan():
tp.r.client.gui.setVisibility("hyq_trunk", "OFF")
for i in range(1,len(configs)):
r(configs[i]);
time.sleep(0.5)
def interpolate():
tp.cl.problem.selectProblem("default")
r.client.gui.setVisibility("hyq", "ON")
tp.r.client.gui.setVisibility("toto", "OFF")
tp.r.client.gui.setVisibility("hyq_trunk", "OFF")
for i in range(7,20):
act(i,1,optim_effectors=True)
def play(frame_rate = 1./24.):
play_traj(fullBody,pp,frame_rate)
time.sleep(0.5)
def a():
......@@ -207,13 +195,5 @@ def e():
print "displaying contact plan"
contactPlan()
def f():
print "computing feasible com trajectory"
interpolate()
def g():
print "playing feasible trajectory"
play()
print "Root path generated in " + str(tp.t) + " ms."
......@@ -43,7 +43,7 @@ fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 10000, "man
rarmId = 'hrp2_rarm_rom'
rarm = 'RARM_JOINT0'
rHand = 'RARM_JOINT5'
rArmOffset = [-0.05,-0.050,-0.050]
rArmOffset = [-0.040,-0.01,-0.085]
rArmNormal = [1,0,0]
rArmx = 0.024; rArmy = 0.024
fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, 20000, "EFORT", 0.05, "_6_DOF", True)
......@@ -51,7 +51,7 @@ fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, 20000, "
larmId = 'hrp2_larm_rom'
larm = 'LARM_JOINT0'
lHand = 'LARM_JOINT5'
lArmOffset = [-0.05,-0.050,-0.050]
lArmOffset = [-0.040,0.01,-0.085]
lArmNormal = [1,0,0]
lArmx = 0.024; lArmy = 0.024
fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, 20000, "EFORT", 0.05, "_6_DOF", True)
......@@ -158,5 +158,5 @@ def saveAll(name):
saveAllData(fullBody, r, name)
for i in range(10,25): act(i,numOptim = 60, use_window = 0, friction = 1, optim_effectors = True, verbose = False, draw = False);
saveAll('polarisTestAndrea')
#~ for i in range(10,25): act(i,numOptim = 60, use_window = 0, friction = 1, optim_effectors = True, verbose = False, draw = False);
#~ saveAll('polarisTestAndrea')
......@@ -43,7 +43,7 @@ fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 10000, "man
rarmId = 'hrp2_rarm_rom'
rarm = 'RARM_JOINT0'
rHand = 'RARM_JOINT5'
rArmOffset = [-0.05,-0.050,-0.050]
rArmOffset = [-0.045,-0.01,-0.085]
rArmNormal = [1,0,0]
rArmx = 0.024; rArmy = 0.024
#~ fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, 20000, "EFORT", 0.05, "_6_DOF", True)
......@@ -51,7 +51,7 @@ rArmx = 0.024; rArmy = 0.024
larmId = 'hrp2_larm_rom'
larm = 'LARM_JOINT0'
lHand = 'LARM_JOINT5'
lArmOffset = [-0.05,-0.050,-0.050]
lArmOffset = [-0.045,0.01,-0.085]
lArmNormal = [1,0,0]
lArmx = 0.024; lArmy = 0.024
fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, 20000, "EFORT", 0.05, "_6_DOF", True)
......
......@@ -43,7 +43,7 @@ fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 10000, "sta
rarmId = 'hrp2_rarm_rom'
rarm = 'RARM_JOINT0'
rHand = 'RARM_JOINT5'
rArmOffset = [-0.05,-0.050,-0.050]
rArmOffset = [-0.045,-0.01,-0.085]
rArmNormal = [1,0,0]
rArmx = 0.024; rArmy = 0.024
#~ fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, 20000, "EFORT", 0.05, "_6_DOF", True)
......@@ -51,10 +51,10 @@ rArmx = 0.024; rArmy = 0.024
larmId = 'hrp2_larm_rom'
larm = 'LARM_JOINT0'
lHand = 'LARM_JOINT5'
lArmOffset = [-0.05,-0.050,-0.050]
lArmOffset = [-0.045,0.01,-0.085]
lArmNormal = [1,0,0]
lArmx = 0.024; lArmy = 0.024
fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, 20000, "static", 0.05, "_6_DOF", True)
fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, 20000, "EFORT", 0.05, "_6_DOF", True)
#~
fullBody.runLimbSampleAnalysis(rLegId, "jointLimitsDistance", True)
......
......@@ -43,7 +43,7 @@ fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 10000, "sta
rarmId = 'hrp2_rarm_rom'
rarm = 'RARM_JOINT0'
rHand = 'RARM_JOINT5'
rArmOffset = [-0.05,-0.050,-0.050]
rArmOffset = [-0.045,-0.01,-0.085]
rArmNormal = [1,0,0]
rArmx = 0.024; rArmy = 0.024
#~ fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, 20000, "EFORT", 0.05, "_6_DOF", True)
......@@ -51,10 +51,10 @@ rArmx = 0.024; rArmy = 0.024
larmId = 'hrp2_larm_rom'
larm = 'LARM_JOINT0'
lHand = 'LARM_JOINT5'
lArmOffset = [-0.05,-0.050,-0.050]
lArmOffset = [-0.045,0.01,-0.085]
lArmNormal = [1,0,0]
lArmx = 0.024; lArmy = 0.024
#~ fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, 20000, "static", 0.05, "_6_DOF", True)
#~ fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, 20000, "EFORT", 0.05, "_6_DOF", True)
#~
fullBody.runLimbSampleAnalysis(rLegId, "jointLimitsDistance", True)
......
......@@ -126,8 +126,8 @@ def __getTimes(fullBody, configs, i, time_scale):
from hpp import Error as hpperr
import sys, time
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 = []):
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,):
global errorid
global stat_data
fail = 0
......@@ -161,29 +161,29 @@ trackedEffectors = []):
global trajec
global trajec_mil
frame_rate = 1./24.
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_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)
NPeffs = genPEffperFrame(fullBody, freeEffectorsPerPhase, new_traj_andrea, pp, time_per_path, frame_rate_andrea)
com = genComPerFrame(final_state, dt, frame_rate_andrea)
trajec = trajec + new_traj
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))
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))
stat_data["num_success"] += 1
else:
print "TODO, NO CONTACT VARIATION, LINEAR INTERPOLATION REQUIRED"
......
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