From 9e2cc056e3f6210d12c31e736726d361ab76a139 Mon Sep 17 00:00:00 2001
From: t steve <pro@stevetonneau.fr>
Date: Tue, 23 May 2017 13:38:15 +0200
Subject: [PATCH] interpolation methods to bezier_traj
---
.../demos/siggraph_asia/bezier_traj.py | 101 ++++++++++++++++
.../demos/siggraph_asia/darpa_hyq.py | 110 +-----------------
2 files changed, 102 insertions(+), 109 deletions(-)
diff --git a/script/scenarios/demos/siggraph_asia/bezier_traj.py b/script/scenarios/demos/siggraph_asia/bezier_traj.py
index 87c51bd..59f9025 100644
--- a/script/scenarios/demos/siggraph_asia/bezier_traj.py
+++ b/script/scenarios/demos/siggraph_asia/bezier_traj.py
@@ -425,3 +425,104 @@ def init_bezier_traj(robot, r, pplayer, qs, comConstraints):
viewer.client.gui.createScene(scene)
global limbsCOMConstraints
limbsCOMConstraints = comConstraints
+
+com_vel = [0.,0.,0.]
+com_acc = [0.,0.,0.]
+
+vels = []
+accs = []
+
+#~ test_ineq(0,{ rLegId : {'file': "hrp2/RL_com.ineq", 'effector' : 'RLEG_JOINT5'}}, 1000, [1,0,0,1])
+#~ test_ineq(0,{ lLegId : {'file': "hrp2/LL_com.ineq", 'effector' : 'LLEG_JOINT5'}}, 1000, [0,0,1,1])
+#~ gen(0,1)
+
+path = []
+a_s = []
+def go(sid, rg = 2, num_optim = 0, mu = 0.6, window = 2, s = None):
+ global com_vel
+ global com_acc
+ global vels
+ global accs
+ global path
+ global a_s
+ a = []
+ for l in range(sid,sid+rg):
+ print "STATE ", l
+ s = max(norm(array(configs[sid+1]) - array(configs[sid])), 1.) * 1
+ a,com_vel,com_acc = gen_several_states_partial(l,window,mu=mu,num_optim=num_optim, s=s,init_vel=com_vel, init_acc=com_acc, path=True)
+ a_s+=[a]
+ vels += [com_vel[:]]
+ accs += [com_acc[:]]
+ print "STATE ", sid+rg
+ #~ path,com_vel,com_acc = gen_several_states(sid+rg,1,mu=mu,num_optim=num_optim, s=s,init_vel=com_vel, init_acc=com_acc)
+ vels += [com_vel[:]]
+ accs += [com_acc[:]]
+ return a
+
+def go_stop(sid, rg = 2, num_optim = 0, mu = 0.6, window = 2, s = None):
+ global com_vel
+ global com_acc
+ global vels
+ global accs
+ global path
+ global a_s
+ a = []
+ for l in range(sid,sid+rg):
+ print "STATE ", l
+ s = max(norm(array(configs[sid+1]) - array(configs[sid])), 1.) * 1
+ a,com_vel,com_acc = gen_several_states_partial(l,window,mu=mu,num_optim=num_optim, s=s,init_vel=com_vel, init_acc=com_acc, path=True)
+ a_s+=[a]
+ vels += [com_vel[:]]
+ accs += [com_acc[:]]
+ print "STATE ", sid+rg
+ s = max(norm(array(configs[sid+rg+1]) - array(configs[sid+rg])), 1.) * 1
+ a,com_vel,com_acc = gen_several_states(sid+rg,1,mu=mu,num_optim=num_optim, s=s,init_vel=com_vel, init_acc=com_acc)
+ a_s+=[a]
+ vels += [com_vel[:]]
+ accs += [com_acc[:]]
+ return a
+
+def go0(sid, rg, num_optim = 0, mu = 0.6, s =None):
+ global com_vel
+ global com_acc
+ global vels
+ global accs
+ global path
+ if s == None:
+ s = max(norm(array(configs[sid+1]) - array(configs[sid])), 1.) * 1.5
+ print "$$$$$$$$$$$$$$$ S $$$$$$$$ *********************444444444444444444444444444 ", s
+ for i in range(rg):
+ path = gen(sid+i,1,mu=mu,num_optim=num_optim, s=s)
+ return path
+
+def go2(sid, rg = 1, num_optim = 0, mu = 0.5, t =2, s =None):
+ global com_vel
+ global com_acc
+ global vels
+ global accs
+ global path
+ for i in range(rg):
+ if s == None:
+ s = max(norm(array(configs[sid+i+1]) - array(configs[sid+i])), 1.) * 0.6
+ print "$$$$$$$$$$$$$$$ S $$$$$$$$ ", s
+ path,com_vel,com_acc = gen_several_states(sid+i,sid+i+t,mu=mu,num_optim=num_optim, s=s,init_vel=com_vel, init_acc=com_acc)
+ vels += [com_vel[:]]
+ accs += [com_acc[:]]
+ return path
+
+def reset():
+ global com_vel
+ global com_acc
+ global vels
+ global accs
+ global a_s
+ global path
+ com_vel = [0.,0.,0.]
+ com_acc = [0.,0.,0.]
+ clean_path();
+ vels = []
+ accs = []
+ path = []
+ a_s = []
+ for i, config in enumerate(configs):
+ fullBody.setConfigAtState(i,config)
diff --git a/script/scenarios/demos/siggraph_asia/darpa_hyq.py b/script/scenarios/demos/siggraph_asia/darpa_hyq.py
index dbe9ceb..342d0d0 100644
--- a/script/scenarios/demos/siggraph_asia/darpa_hyq.py
+++ b/script/scenarios/demos/siggraph_asia/darpa_hyq.py
@@ -211,113 +211,5 @@ configs = qs
fullBody = fb
tp = ttp
-#~ test_ineq(0,{ rLegId : {'file': "hrp2/RL_com.ineq", 'effector' : 'RLEG_JOINT5'}}, 1000, [1,0,0,1])
-#~ test_ineq(0,{ lLegId : {'file': "hrp2/LL_com.ineq", 'effector' : 'LLEG_JOINT5'}}, 1000, [0,0,1,1])
-#~ gen(0,1)
-
-com_vel = [0.,0.,0.]
-com_acc = [0.,0.,0.]
-
-vels = []
-accs = []
-
-#~ test_ineq(0,{ rLegId : {'file': "hrp2/RL_com.ineq", 'effector' : 'RLEG_JOINT5'}}, 1000, [1,0,0,1])
-#~ test_ineq(0,{ lLegId : {'file': "hrp2/LL_com.ineq", 'effector' : 'LLEG_JOINT5'}}, 1000, [0,0,1,1])
-#~ gen(0,1)
-
-path = []
-a_s = []
-def go(sid, rg = 2, num_optim = 0, mu = 0.6, window = 2, s = None):
- global com_vel
- global com_acc
- global vels
- global accs
- global path
- global a_s
- a = []
- for l in range(sid,sid+rg):
- print "STATE ", l
- s = max(norm(array(configs[sid+1]) - array(configs[sid])), 1.) * 1
- a,com_vel,com_acc = gen_several_states_partial(l,window,mu=mu,num_optim=num_optim, s=s,init_vel=com_vel, init_acc=com_acc, path=True)
- a_s+=[a]
- vels += [com_vel[:]]
- accs += [com_acc[:]]
- print "STATE ", sid+rg
- #~ path,com_vel,com_acc = gen_several_states(sid+rg,1,mu=mu,num_optim=num_optim, s=s,init_vel=com_vel, init_acc=com_acc)
- vels += [com_vel[:]]
- accs += [com_acc[:]]
- return a
-
-def go_stop(sid, rg = 2, num_optim = 0, mu = 0.6, window = 2, s = None):
- global com_vel
- global com_acc
- global vels
- global accs
- global path
- global a_s
- a = []
- for l in range(sid,sid+rg):
- print "STATE ", l
- s = max(norm(array(configs[sid+1]) - array(configs[sid])), 1.) * 1
- a,com_vel,com_acc = gen_several_states_partial(l,window,mu=mu,num_optim=num_optim, s=s,init_vel=com_vel, init_acc=com_acc, path=True)
- a_s+=[a]
- vels += [com_vel[:]]
- accs += [com_acc[:]]
- print "STATE ", sid+rg
- s = max(norm(array(configs[sid+rg+1]) - array(configs[sid+rg])), 1.) * 1
- a,com_vel,com_acc = gen_several_states(sid+rg,1,mu=mu,num_optim=num_optim, s=s,init_vel=com_vel, init_acc=com_acc)
- a_s+=[a]
- vels += [com_vel[:]]
- accs += [com_acc[:]]
- return a
-
-def go0(sid, rg, num_optim = 0, mu = 0.6, s =None):
- global com_vel
- global com_acc
- global vels
- global accs
- global path
- if s == None:
- s = max(norm(array(configs[sid+1]) - array(configs[sid])), 1.) * 1.5
- print "$$$$$$$$$$$$$$$ S $$$$$$$$ *********************444444444444444444444444444 ", s
- for i in range(rg):
- path = gen(sid+i,1,mu=mu,num_optim=num_optim, s=s)
- return path
-
-def go2(sid, rg = 1, num_optim = 0, mu = 0.5, t =2, s =None):
- global com_vel
- global com_acc
- global vels
- global accs
- global path
- for i in range(rg):
- if s == None:
- s = max(norm(array(configs[sid+i+1]) - array(configs[sid+i])), 1.) * 0.6
- print "$$$$$$$$$$$$$$$ S $$$$$$$$ ", s
- path,com_vel,com_acc = gen_several_states(sid+i,sid+i+t,mu=mu,num_optim=num_optim, s=s,init_vel=com_vel, init_acc=com_acc)
- vels += [com_vel[:]]
- accs += [com_acc[:]]
- return path
-
-#~ a = go2(0, s = 1)
-#~ a = go2(0, num_optim=0, s = 1.2, mu=0.6)
-#~ a = go2(2, num_optim=0, s = 1.2, mu=0.6)
-#~ a = go2(4, num_optim=3, mu=0.6)
-
-def reset():
- global com_vel
- global com_acc
- global vels
- global accs
- global a_s
- global path
- com_vel = [0.,0.,0.]
- com_acc = [0.,0.,0.]
- clean_path();
- vels = []
- accs = []
- path = []
- a_s = []
- for i, config in enumerate(configs):
- fullBody.setConfigAtState(i,config)
+
--
GitLab