diff --git a/script/scenarios/demos/siggraph_asia/twister/twister.py b/script/scenarios/demos/siggraph_asia/twister/twister.py
index 28dbf9fcf24df383409982ff179ef75f4f14bab1..789f16aab8a1fb7b61f44fff2ea9d4021ad73337 100644
--- a/script/scenarios/demos/siggraph_asia/twister/twister.py
+++ b/script/scenarios/demos/siggraph_asia/twister/twister.py
@@ -63,60 +63,173 @@ fullBody = fb
tp = ttp
from hpp.corbaserver.rbprm.rbprmstate import State
-from hpp.corbaserver.rbprm.state_alg import addNewContact, isContactReachable, closestTransform, removeContact, addNewContactIfReachable
+from hpp.corbaserver.rbprm.state_alg import addNewContact, isContactReachable, closestTransform, removeContact, addNewContactIfReachable, projectToFeasibleCom
s1 = State(fullBody,q=q_init, limbsIncontact = [rLegId, lLegId])
q0 = s1.q()[:]
-def test(p, n):
- global s1
- t0 = time.clock()
- a, success = addNewContact(s1,rLegId,p, n)
- print "projecttion successfull ? ", success
- t1 = time.clock()
- val, com = isContactReachable(s1, rLegId,p, n, limbsCOMConstraints)
- t2 = time.clock()
- print 'is reachable ? ', val, com
- if(val):
- com[2]+=0.1
- if(success > 0):
- print 'a > 0'
- t3 = time.clock()
- q = fullBody.projectStateToCOM(a.sId, com.tolist())
- t4= time.clock()
- r(a.q())
- else:
- print "using s1 ", s1.sId
- t3 = time.clock()
- q = fullBody.projectStateToCOM(s1.sId, com.tolist())
- t4= time.clock()
- a, succ = addNewContact(s1,rLegId,p, n)
- print "success projection ??", succ
- if (succ):
- r(a.q())
- print "time to addnc", t2 - t1
- print "projectcom succesfull ?", q
- print "time to check reachable", t3- t2
- print "time to project", t4- t3
-
-a = computeAffordanceCentroids(tp.afftool)
-def computeNext(state, limb):
+#~ def test(p, n):
+ #~ global s1
+ #~ t0 = time.clock()
+ #~ a, success = addNewContact(s1,rLegId,p, n)
+ #~ print "projecttion successfull ? ", success
+ #~ t1 = time.clock()
+ #~ val, com = isContactReachable(s1, rLegId,p, n, limbsCOMConstraints)
+ #~ t2 = time.clock()
+ #~ print 'is reachable ? ', val, com
+ #~ if(val):
+ #~ com[2]+=0.1
+ #~ if(success > 0):
+ #~ print 'a > 0'
+ #~ t3 = time.clock()
+ #~ q = fullBody.projectStateToCOM(a.sId, com.tolist())
+ #~ t4= time.clock()
+ #~ r(a.q())
+ #~ else:
+ #~ print "using s1 ", s1.sId
+ #~ t3 = time.clock()
+ #~ q = fullBody.projectStateToCOM(s1.sId, com.tolist())
+ #~ t4= time.clock()
+ #~ a, succ = addNewContact(s1,rLegId,p, n)
+ #~ if (succ):
+ #~ r(a.q())
+ #~ print "time to addnc", t2 - t1
+ #~ print "projectcom succesfull ?", q
+ #~ print "time to check reachable", t3- t2
+ #~ print "time to project", t4- t3
+
+def dist(q0,q1):
+ return norm(array(q0[7:]) - array(q1[7:]) )
+
+def distq_ref(q0):
+ return lambda s: dist(s.q(),q0)
+
+a = computeAffordanceCentroids(tp.afftool, ["Support"])
+def computeNext(state, limb, projectToCom = False, max_num_samples = 10):
global a
t1 = time.clock()
- #~ candidates = [el for el in a if addNewContactIfReachable(state, limb, el[0], el[1], limbsCOMConstraints)[0] ]
+ #~ candidates = [el for el in a if isContactReachable(state, limb, el[0], el[1], limbsCOMConstraints)[0] ]
#~ print "num candidates", len(candidates)
#~ t3 = time.clock()
- results = [addNewContactIfReachable(state, limb, el[0], el[1], limbsCOMConstraints) for el in a]
+ results = [addNewContactIfReachable(state, limb, el[0], el[1], limbsCOMConstraints, projectToCom, max_num_samples) for el in a]
t2 = time.clock()
#~ t4 = time.clock()
resultsFinal = [el[0] for el in results if el[1]]
print "time to filter", t2 - t1
#~ print "time to create", t4 - t3
print "num res", len(resultsFinal)
- return resultsFinal
-
-#~ res = computeNext(s1, rLegId)
-s2 = removeContact(s1,lLegId)[0]
-s3 = computeNext(s2, larmId)[0]
+ #sorting
+ sortedlist = sorted(resultsFinal, key=distq_ref(state.q()))
+ return sortedlist
+
+scene = "bb"
+r.client.gui.createScene(scene)
+b_id = 0
+
+
+def plot_feasible_Kin(state):
+ com = array(state.getCenterOfMass())
+ for i in range(5):
+ for j in range(5):
+ for k in range(10):
+ c = com + array([(i - 2.5)*0.2, (j - 2.5)*0.2, (k-5)*0.2])
+ active_ineq = state.getComConstraint(limbsCOMConstraints,[])
+ if(active_ineq[0].dot( c )<= active_ineq[1]).all():
+ #~ print 'active'
+ createPtBox(r.client.gui, 0, c, color = [0,1,0,1])
+ else:
+ if(active_ineq[0].dot( c )>= active_ineq[1]).all():
+ #~ print "inactive"
+ createPtBox(r.client.gui, 0, c, color = [1,0,0,1])
+ return -1
+
+def compute_w(c, ddc=array([0.,0.,0.]), dL=array([0.,0.,0.]), m = 54., g_vec=array([0.,0.,-9.81])):
+ w1 = m * (ddc - g_vec)
+ return array(w1.tolist() + (cross(c, w1) + dL).tolist())
+
+def plot_feasible_cone(state):
+ com = array(state.getCenterOfMass())
+ #~ H, h = state.getContactCone(0.6)
+ ps = state.getContactPosAndNormals()
+ p = ps[0][0]
+ N = ps[1][0]
+ H = compute_CWC(p, N, state.fullBody.client.basic.robot.getMass(), mu = 0.6, simplify_cones = False)
+ #~ H = comp
+ #~ H = -array(H)
+ #~ h = array(h)
+ #~ print "h", h
+ for i in range(10):
+ for j in range(10):
+ for k in range(1):
+ c = com + array([(i - 5)*0.1, (j - 5)*0.1, k])
+ w = compute_w(c)
+ print "w, " , w
+ if(H.dot( w )<= 0).all():
+ #~ print 'active'
+ createPtBox(r.client.gui, 0, c, color = [0,1,0,1])
+ else:
+ #~ if(H.dot( w )>= 0).all():
+ #~ print "inactive"
+ createPtBox(r.client.gui, 0, c, color = [1,0,0,1])
+ return H
+
+def plot_feasible(state):
+ com = array(state.getCenterOfMass())
+ ps = state.getContactPosAndNormals()
+ p = ps[0][0]
+ N = ps[1][0]
+ H = compute_CWC(p, N, state.fullBody.client.basic.robot.getMass(), mu = 1, simplify_cones = False)
+ for i in range(5):
+ for j in range(5):
+ for k in range(10):
+ c = com + array([(i - 2.5)*0.2, (j - 2.5)*0.2, (k-5)*0.2])
+ w = compute_w(c)
+ active_ineq = state.getComConstraint(limbsCOMConstraints,[])
+ if(active_ineq[0].dot( c )<= active_ineq[1]).all() and (H.dot( w )<= 0).all():
+ #~ print 'active'
+ createPtBox(r.client.gui, 0, c, color = [0,1,0,1])
+ else:
+ if(active_ineq[0].dot( c )>= active_ineq[1]).all():
+ #~ print "inactive"
+ createPtBox(r.client.gui, 0, c, color = [1,0,0,1])
+ return -1
+
+def plot(c):
+ createPtBox(r.client.gui, 0, c, color = [0,1,0,1])
+
+i = 0
+#~ s0 = removeContact(s1,rLegId)[0]
+#~ s_init = computeNext(s0,rLegId, True,20)
+#~ res = computeNext(s0,larmId, True,20)
+#~ s1 = res[0]
+#~ res2 = computeNext(s1,rarmId, True,20)
+#~ s2 = computeNext(s1,rarmId, True,100)[0]
+
+all_states=[]
+#~ all_states=[s1,s2]
+#~ s2 = removeContact(s2,rLegId)[0]
+#~ s3 = computeNext(s2, larmId)[0]
#~ go0(s2.sId,1, s=1)
+#~ plot_feasible(s1)
+from time import sleep
+def play():
+ for i,el in enumerate(all_states):
+ r(el.q())
+ sleep(0.5)
+ i = len(all_states)-1;
+ for j in range(i+1):
+ print "ij,sum", i, j, i-j
+ r(all_states[i-j].q())
+ sleep(0.5)
+
+play()
+
+#~ print "init valid ?", fullBody.isConfigValid(s1.q())
+#~ print "end valid ?", fullBody.isConfigValid(s2.q())
+
+r(q_init)
+#~ path = go0([s2,s1], mu=0.3,num_optim=1)
+s2 = computeNext(s1,rarmId,True,10)[0]; r(s2.q())
+#~ s3 = computeNext(s2,rLegId,True,10)[0]; r(s3.q())
+
diff --git a/script/scenarios/demos/siggraph_asia/twister/twister_spidey.py b/script/scenarios/demos/siggraph_asia/twister/twister_spidey.py
index 71ce3fcfd09fff0ca649cd95f8573c0d0b9a1302..89589f4e92b786c3a081ebf160a3f6b9412d432f 100644
--- a/script/scenarios/demos/siggraph_asia/twister/twister_spidey.py
+++ b/script/scenarios/demos/siggraph_asia/twister/twister_spidey.py
@@ -106,16 +106,16 @@ r.client.gui.createScene(scene)
b_id = 0
-def plot_feasible(state):
+def plot_feasible_Kin(state):
com = array(state.getCenterOfMass())
for i in range(5):
for j in range(5):
for k in range(10):
c = com + array([(i - 2.5)*0.2, (j - 2.5)*0.2, (k-5)*0.2])
- active_ineq = state.getComConstraint(limbsCOMConstraints)
- if(active_ineq[0].dot( c )<= active_ineq[1]).all() and fullBody.isConfigBalanced(state.q(), s1.getLimbsInContact()):
+ active_ineq = state.getComConstraint(limbsCOMConstraints,[])
+ if(active_ineq[0].dot( c )<= active_ineq[1]).all():
#~ print 'active'
- createPtBox(r.client.gui, 0, c, color = [1,0,0,1])
+ createPtBox(r.client.gui, 0, c, color = [0,1,0,1])
else:
if(active_ineq[0].dot( c )>= active_ineq[1]).all():
#~ print "inactive"
@@ -152,6 +152,26 @@ def plot_feasible_cone(state):
createPtBox(r.client.gui, 0, c, color = [1,0,0,1])
return H
+def plot_feasible(state):
+ com = array(state.getCenterOfMass())
+ ps = state.getContactPosAndNormals()
+ p = ps[0][0]
+ N = ps[1][0]
+ H = compute_CWC(p, N, state.fullBody.client.basic.robot.getMass(), mu = 1, simplify_cones = False)
+ for i in range(5):
+ for j in range(5):
+ for k in range(10):
+ c = com + array([(i - 2.5)*0.2, (j - 2.5)*0.2, (k-5)*0.2])
+ w = compute_w(c)
+ active_ineq = state.getComConstraint(limbsCOMConstraints,[])
+ if(active_ineq[0].dot( c )<= active_ineq[1]).all() and (H.dot( w )<= 0).all():
+ #~ print 'active'
+ createPtBox(r.client.gui, 0, c, color = [0,1,0,1])
+ else:
+ if(active_ineq[0].dot( c )>= active_ineq[1]).all():
+ #~ print "inactive"
+ createPtBox(r.client.gui, 0, c, color = [1,0,0,1])
+ return -1
def plot(c):
createPtBox(r.client.gui, 0, c, color = [0,1,0,1])