diff --git a/script/scenarios/demos/siggraph_asia/twister/bezier_traj.py b/script/scenarios/demos/siggraph_asia/twister/bezier_traj.py
index e076b7c730bb67cb9f4eab43826dfcfe91cac6ed..c01a204a353e8c42c6afb33a6168e6f9372107f7 100644
--- a/script/scenarios/demos/siggraph_asia/twister/bezier_traj.py
+++ b/script/scenarios/demos/siggraph_asia/twister/bezier_traj.py
@@ -67,7 +67,7 @@ def test(s1,s2, path = False, use_rand = False, just_one_curve = False, num_opti
createPtBox(viewer.client.gui, 0, c_mid_1[0].tolist(), 0.01, [0,1,0,1.])
createPtBox(viewer.client.gui, 0, c_mid_2[0].tolist(), 0.01, [0,1,0,1.])
- partions = [0.,0.1,0.9,1.]
+ partions = [0.,0.2,0.8,1.]
p0 = fullBody.generateCurveTrajParts(bezier_0,partions)
#testing intermediary configurations
print 'paritions:', partions[1], " "
@@ -450,7 +450,7 @@ def go0(states, num_optim = 0, mu = 0.6, s =None):
sc = s
for i, el in enumerate(states[:-1]):
if s == None:
- sc = max(norm(array(states[i+1].q()) - array(el.q())), 1.) * 1
+ sc = max(norm(array(states[i+1].q()) - array(el.q())), 1.) * 0.5
path = gen(el,states[i+1],mu=mu,num_optim=num_optim, s=sc)
return path
diff --git a/script/scenarios/demos/siggraph_asia/twister/spidey_model.py b/script/scenarios/demos/siggraph_asia/twister/spidey_model.py
index 8c06bf09a28eb7cca4541aa37df3c4c753d983e9..6e78ca07a2e8eed2b4b4926a1da2330a118e5088 100644
--- a/script/scenarios/demos/siggraph_asia/twister/spidey_model.py
+++ b/script/scenarios/demos/siggraph_asia/twister/spidey_model.py
@@ -39,7 +39,7 @@ fullBody.runLimbSampleAnalysis(lLegId, "jointLimitsDistance", True)
limbsCOMConstraints = { rLegId : {'file': "spiderman/RL_com.ineq", 'effector' : rfoot},
- lLegId : {'file': "spiderman/LL_com.ineq", 'effector' : rHand},
+ lLegId : {'file': "spiderman/LL_com.ineq", 'effector' : lfoot},
rarmId : {'file': "spiderman/RA_com.ineq", 'effector' : rHand},
larmId : {'file': "spiderman/LA_com.ineq", 'effector' : lHand} }
diff --git a/script/scenarios/demos/siggraph_asia/twister/twister_spidey.py b/script/scenarios/demos/siggraph_asia/twister/twister_spidey.py
index 5ebb8f1c9c1d023f2b24083af71f8054f60815e7..42881a54bcac70d56c4dfeaad582a6f8a1badaf5 100644
--- a/script/scenarios/demos/siggraph_asia/twister/twister_spidey.py
+++ b/script/scenarios/demos/siggraph_asia/twister/twister_spidey.py
@@ -3,6 +3,8 @@ from hpp.corbaserver.rbprm.rbprmbuilder import Builder
from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
from hpp.gepetto import Viewer
from hpp.gepetto import PathPlayer
+from numpy import array, sort
+from numpy.linalg import norm
import twister_spidey_path as path_planner
import spidey_model as model
@@ -22,7 +24,7 @@ init_plan_execute(model.fullBody, r, path_planner, pp)
#~ base_joint_xyz_limits = tp.base_joint_xyz_limits
q_0 = fullBody.getCurrentConfig(); r(q_0)
-
+q_0[2] = 1.05
q_init = fullBody.getCurrentConfig(); q_init[0:7] = path_planner.q_init[0:7]
q_goal = fullBody.getCurrentConfig(); q_goal[0:7] = path_planner.q_goal[0:7]
@@ -41,7 +43,7 @@ from hpp.corbaserver.rbprm.rbprmstate import State
from hpp.corbaserver.rbprm.state_alg import addNewContact, isContactReachable, closestTransform, removeContact, addNewContactIfReachable, projectToFeasibleCom
#~ s1 = State(fullBody,q=q_init, limbsIncontact = [rLegId, lLegId])
-s1 = State(fullBody,q=q_init, limbsIncontact = [rLegId])
+s1 = State(fullBody,q=q_0, limbsIncontact = [rLegId, lLegId])
q0 = s1.q()[:]
@@ -75,6 +77,12 @@ q0 = s1.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
@@ -89,7 +97,9 @@ def computeNext(state, limb, projectToCom = False, max_num_samples = 10):
print "time to filter", t2 - t1
#~ print "time to create", t4 - t3
print "num res", len(resultsFinal)
- return resultsFinal
+ #sorting
+ sortedlist = sorted(resultsFinal, key=distq_ref(state.q()))
+ return sortedlist
scene = "bb"
r.client.gui.createScene(scene)
@@ -97,29 +107,34 @@ b_id = 0
def plot_feasible(state):
- for i in range(10):
- for j in range(10):
- for k in range(20):
- c = array([(i - 5)*0.1, (j - 5.)*0.1, (k-10)*0.1])
+ 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():
+ if(active_ineq[0].dot( c )<= active_ineq[1]).all() and fullBody.isConfigBalanced(state.q(), s1.getLimbsInContact()):
#~ print 'active'
- createPtBox(r.client.gui, 0, c, color = [0,1,0,1])
+ createPtBox(r.client.gui, 0, c, color = [1,0,0,1])
else:
if(active_ineq[0].dot( c )>= active_ineq[1]).all():
#~ print "inactive"
- createPtBox(r.client.gui, 0, c)
+ 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]
+#~ 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]
+#~ s2 = computeNext(s1,rarmId, True,100)[0]
-all_states=[s1,s2]
+all_states=[]
+#~ all_states=[s1,s2]
#~ s2 = removeContact(s2,rLegId)[0]
#~ s3 = computeNext(s2, larmId)[0]
#~ go0(s2.sId,1, s=1)
@@ -137,10 +152,11 @@ def play():
play()
-print "init valid ?", fullBody.isConfigValid(s1.q())
-print "end valid ?", fullBody.isConfigValid(s2.q())
+#~ 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)
-
+#~ 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())