Commit d1063e96 authored by t steve's avatar t steve
Browse files

updated chair and stairs

parent e15d62f7
......@@ -92,6 +92,7 @@ install(FILES
data/urdf/darpa.urdf
data/urdf/car.urdf
data/urdf/polaris.urdf
data/urdf/siggraph_asia/chair.urdf
data/urdf/siggraph_asia/down.urdf
data/urdf/siggraph_asia/scale.urdf
data/urdf/siggraph_asia/scale_spidey.urdf
......@@ -139,6 +140,7 @@ install(FILES
data/srdf/darpa.srdf
data/srdf/car.srdf
data/srdf/polaris.srdf
data/srdf/siggraph_asia/chair.srdf
data/srdf/siggraph_asia/down.srdf
data/srdf/siggraph_asia/scale.srdf
data/srdf/siggraph_asia/scale_spidey.srdf
......@@ -188,13 +190,13 @@ install(FILES
data/meshes/stair_bauzil_reduced.stl
data/meshes/climb.stl
data/meshes/stepladder.stl
data/meshes/chair.stl
data/meshes/car.stl
data/meshes/polaris.stl
data/meshes/polaris_reduced.stl
data/meshes/siggraph_asia/twister.stl
data/meshes/siggraph_asia/down.stl
data/meshes/siggraph_asia/scale.stl
data/meshes/siggraph_asia/chair.stl
data/meshes/siggraph_asia/scale_spidey.stl
data/meshes/siggraph_asia/mountain.stl
data/meshes/siggraph_asia/grasp.stl
......
......@@ -138,11 +138,11 @@ def onepath2(states_subset, ctxt=1, nopt=1, mu=1, effector = False):
global states
#~ print "ctxt", ctxt
#~ print "q", len(states[ol+1].q())
#~ s = max(norm(array(states_subset[1].q()) - array(states_subset[0].q())), 1.) * 0.4
s = max(norm(array(states_subset[1].q()) - array(states_subset[0].q())), 1.) * 0.4
#~ print "s",s
#~ if(ol > len(path) -1):
path = all_paths[ctxt][:]
path += [go2(states_subset, num_optim=nopt, mu=mu, use_kin = True, s=None, effector = effector)]
path += [go2(states_subset, num_optim=nopt, mu=mu, use_kin = True, s=s, effector = effector)]
#~ else:
#~ path[ol]=go2(states_subset, num_optim=nopt, mu=mu, use_kin = False, s=s, effector = effector)
all_paths[ctxt] = path
......@@ -232,16 +232,16 @@ states+=[s]
#~ lc()
#~ le = min(38, len(states)-10)
#~ lc()
lc()
#~ path = []
#~ all_paths = [[],[]]
states = list(reversed(states))
#~ states = list(reversed(states))
#~ for i in range(0,4):
#~ print i
#~ onepath(i,0,nopt=1,effector=False,mu=1);
#~
#~
#~ onepath2(states [0:-2],nopt=3,mu=1,effector=False)
#~ onepath2(states [2:-2],nopt=0,mu=1,effector=False)
#~ e(0.01)
......@@ -240,6 +240,9 @@ def gen_several_states(states, num_optim = 0, ine_curve =True, s = 1., effector
com_acc = init_acc[:]
start = states[0].sId
len_con = len(states)
print "AAAAAAAAAAAAAAAAAAAAAAAAAAAAA com_vel", com_vel
print "AAAAAAAAAAAAAAAAAAAAAAAAAAAA com_acc", com_acc
print "going from, to ", com_1, "->", com_2
print "going from, to ", start, "->", start + len_con
allpoints = [com_1]
......@@ -261,7 +264,7 @@ def gen_several_states(states, num_optim = 0, ine_curve =True, s = 1., effector
allpoints+=[res[1][0],res[2][0]]
step = (1./ len_con)
idx = step * (i - start)
all_partitions += [idx +0.3*step,idx+0.7*step,idx+step]
all_partitions += [idx +0.3*step,idx+0.7*step,idx+step+0.002]
break
if not res[0]:
n_fail += 1
......@@ -270,7 +273,7 @@ def gen_several_states(states, num_optim = 0, ine_curve =True, s = 1., effector
step = (1./ len_con)
#~ idx = step * (i - start)
idx = step * i
all_partitions += [idx +0.2*step,idx+0.8*step,idx+step]
all_partitions += [idx +0.4*step,idx+0.9*step,idx+step]
all_partitions = [0.] + all_partitions
print "n_fail ", n_fail
print "generating super curve"
......@@ -525,16 +528,24 @@ def go2(states, one_curve = True, num_optim = 0, mu = 0.6, s =None, use_kin = T
init_acc =com_acc
path = []
sc = s
for i, el in enumerate(states[:-1]):
print "************ one call to ", i
if s == None:
sc = max(norm(array(states[i+1].q()) - array(el.q())), 1.) * 0.3
print "states idds ", i, " ", i+2, " ", len (states[i:i+2])
a, ve, ac = gen_several_states(states[i:i+2],mu=mu,num_optim=num_optim, s=sc, ine_curve = one_curve, use_Kin = use_kin, effector = effector, init_vel =com_vel, init_acc = com_acc)
try:
for i, el in enumerate(states[:-2]):
print "************ one call to ", i
if s == None:
sc = max(norm(array(states[i+1].q()) - array(el.q())), 1.) * 0.6
print "states idds ", i, " ", i+2, " ", len (states[i:i+2])
a, ve, ac = gen_several_states(states[i:i+2],mu=mu,num_optim=num_optim, s=sc, ine_curve = one_curve, use_Kin = use_kin, effector = effector, init_vel =com_vel, init_acc = com_acc)
com_vel = ve
com_acc = ac
clean_path();
path += a
a, ve, ac = gen_several_states(states[-2:],mu=mu,num_optim=num_optim, s=sc, ine_curve = one_curve, use_Kin = use_kin, effector = effector, init_vel =com_vel, init_acc = com_acc)
com_vel = ve
com_acc = ac
clean_path();
path += a
except:
print "FAILT"
return path
print "path", len(path)
return path
......
......@@ -59,7 +59,7 @@ rArmNormal = [0,0,1]
rArmx = 0.024; rArmy = 0.024
#disabling collision for hook
#~ fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, 10000, "manipulability", 0.1, "_6_DOF", False,grasp = True)
fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, 10000, "manipulability", 0.1, "_6_DOF", True)
#~ fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, 10000, "manipulability", 0.1, "_6_DOF", True)
#~
......@@ -346,6 +346,7 @@ i=0
d(0.1); e(0.01); states = planToStates(fullBody,configs)
#~ lc()
onepath2(states [0:18],nopt=0,mu=1,effector=False,init_acc=[0,0,0.], init_vel=[0.,0.,0.])
onepath2(states [0:18],nopt=3,mu=1,effector=False,init_acc=[0,0,0.], init_vel=[0.,0.,0.])
plall()
#~ e(0.01)
......@@ -73,8 +73,8 @@ afftool.loadObstacleModel (packageName, "stairs_lower", "planning", r)
#~ afftool.visualiseAffordances('Support', r, [0.25, 0.5, 0.5])
#~ afftool.visualiseAffordances('Lean', r, [0, 0, 0.9])
ps.client.problem.selectConFigurationShooter("RbprmShooter")
ps.client.problem.selectPathValidation("RbprmPathValidation",0.05)
#~ ps.client.problem.selectConFigurationShooter("RbprmShooter")
#~ ps.client.problem.selectPathValidation("RbprmPathValidation",0.05)
#~ ps.solve ()
t = ps.solve ()
......
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