From a0429d6f027f28c85d144c293c6ffa37fb828081 Mon Sep 17 00:00:00 2001
From: Steve Tonneau <stonneau@axle.laas.fr>
Date: Thu, 11 May 2017 09:38:28 +0200
Subject: [PATCH] scaling scenario

---
 data/meshes/siggraph_asia/scale.stl           | Bin 0 -> 13284 bytes
 data/srdf/siggraph_asia/scale.srdf            |   3 +
 data/urdf/siggraph_asia/scale.urdf            |  19 +
 script/scenarios/demos/siggraph_asia/log.txt  | 346 ++++++++++++++++++
 .../demos/siggraph_asia/scale_hrp2_interp.py  |  18 +-
 5 files changed, 377 insertions(+), 9 deletions(-)
 create mode 100644 data/meshes/siggraph_asia/scale.stl
 create mode 100644 data/srdf/siggraph_asia/scale.srdf
 create mode 100644 data/urdf/siggraph_asia/scale.urdf
 create mode 100644 script/scenarios/demos/siggraph_asia/log.txt

diff --git a/data/meshes/siggraph_asia/scale.stl b/data/meshes/siggraph_asia/scale.stl
new file mode 100644
index 0000000000000000000000000000000000000000..d428086279d69bb1db5c2ab57f0cfb4fb7fd2c35
GIT binary patch
literal 13284
zcmb7~f2f{y8OC2hY=77+lspE(u|LG5S!}5_^t`JWH44%OVfsgNKaNPACQD`gpgpZX
zBAH}J+ZtwuF_^N+T(EHGS^beYrde4~nnDbW#u%8PknMD?`#zuV_j;al-WNK5Y!}aS
zf9~tL@8|peU^{EK-?eqy-CH(KPH)>fGx>=-w|r&umTfCmUVYs)lUIKAo{f_q{=ne>
z`dN0K%9|GGrW+=QtDf1>?t5w15dFgPouQ`>-QP-6+k`s(=Z_Tv4WUjDSuIvo>Lm}f
zs+=w!kxWS-&=CDpBAeE;dtdj%BZ%VelcVv@-f$$L%J<e!mD7`o$(<OzE2_xuG~2Do
zyvh@D-HD{A(j5|x-EMhmo`fy<uA+aOlJ`8(ObrH;YX*ahu0{WHZf<t@vA@ng;mI}9
zsvZpH=5_?Z9Veu@UJ1_cLf_-Z=X;?CgXey+KpdA2i}^qD#QPnz)6z-o*>l+{QS{Xv
zCl<8L0pWLia_=OMX86)mpF1eU9pyz^s^ZZMw@iQAiMWG@?d^D-eATMI9g!D(&0~#i
z*NAJngjcyPMAObZHgR|`|Fgt?qKb{kPjyXua@X&kl=3@hJ0<aClhB{~+4t*a>ipHa
z+f%nUQ|JwMyf^e8E_ke!&Pq)!^}@do7Xl5TP7w9|N>%D5545VBE*|rzIa5I>6-k_i
zh!fv>cJDiq7*gevqw$tUeQzfTRlYY8k}9Vs6_YzLdRJ7D-D$R4lX;aV<hm1eSL7uj
z&x7mCk?$w;@+53w>0KS&eARI0rTf}<th{xIp4t8J&^Mmi-%8_=yIOzllZ8M-s1rmz
zSE^Dkd7xG0bn%FDr6dq&h<>Wvm1p<9?uSS2iYlKRjd%8jBMDW$w|=Ueo>WZkMBNpi
z1oH7zWOthFsy0u&S9wCNJ5hH<UJ{PoZh2~+ge}xvkx1EI|Lw$-KF<%mcJ>Bz^X2=m
zm)0kzwCePeySn8=I|_k@P$!6bu2iL7@<6M~>2x3&?xZ4#(-4uY_3YkvBr&8458nc5
zvo}utRDFWUgNj!pshHe}(YvCG>`r$);x*`1o{;NK)LpSr67oE_&dfOry*vqftGk+i
zeMBNLZ~f<^!}ZO&cK7C=4ACck^kC>Smu+gL(}P#lgAaYa5NHT>N`h*!s!}g`pjG8`
z@rYzf0)d9;rb@19J-hdHzpPy#in~va#=HA|MG~rfZ{1W(PbwyNV)U-4BD>SMUCqE#
z^eRut^-3&E9=jqh3CC`?JT*_k7V55eSCEhTUh(R~?Ty#(kFzBG-K{?k{rZ6ep)(Kf
zSIP^4hJY4P->+1qUh+VTaJqPua|NM<Byk!dykb|ld*6{narep5cxP{%NJ14`;JRj_
zHmCRPlFDg_?yl%db`j7JHJMkD1cJShDk-Yei-gj2a*rU&9r}`xPP$$bNqynjv+=36
z=aJzMJ$Jzsq4f?YtvdbW^lQKM?)Y~G5oid#gG1C^sY<;d=<9SkaN?B&0<FGu!lP22
z-TRItN_L+djdx5PCtZppRB2i?GnGWKHBzaHR1l+gMPITz?UU#CE3fi|Tz4Ypp-Oj1
zC_R!WcRUGmEWImzZ@cjJOWG&y`C5Eem%jMrFNOa3;OfxbUFrKE1R4T5@|W|dD)o{F
zT7=WZBc7sCL7*Y}sd87$K@~~d506ZTyHAeBJA1>Cgetbcb$L^puPRb8xf3MJG;p4Z
z>`r$)Vpm>85{S{e;tmPf8`sH`>D<E;x?lZd7W4ks*2lZg)XAem^j&k0ht^4yP7l5_
zJl)I`0u7;8S`t)?Rh4?l1Fb5j(}VXbC4oRgbW<hQw4UAjx*s0;yD9EIIU4V%jHf3>
z5~?&Ux}Pe~6{%E3Du}u(J_+RERb+QMx2sCNUwM@$<a#AUX~R_M4hhF@w>&jZ!W?y1
zB$CUygSQP={Be2vo9(X*(Qh62eduTIyt0*6UCa?`_quNs0u7-~N%*-^m3qkottzLB
zM?6I(fj~oaQzh55p56PpU)C-V#oZ@I<6WPgD3Va+d+VlRdQvgD6Qg%U71^E6?J5mV
z(W^Wm*DE1P=ZY%bA>r8VmZ#=P*h1ZvdZ$Oe2Ip3Ps{P=#$KvctZ@A-7=shoP2u*Fi
z2K73CKtn)B{)O|HuR-;ahY0#QT|6S0UPTaSc{mLbUhx{l-TRItq8D?>!!_R78z++Z
zuF%XxZGFF@FR36%8y=lqIYAO=ZJ~I?u9%R%PV`fSSL}*AOg?tIW$*GJN#C!Ybh*6i
z^81E+C!U-=e%{$3`i+nLJ@oURIW;R?<_PujnI9Gc4WUj+@QM|yD)o{FT2)RLkJy!x
zK%gP|sYEudXZODDher^_-6u!moxR~mLY42WpDNE4shHe}x~uv8WyfTPAPLuVyO}88
z9hi_*PV`gdJj|h9Bpkcl^3+J>bKtl5T&+Fv+4k#yc{4nuAHVQ)=vOyQhi0!iz2<;G
zLqLZ|XIJVa4-xcrx_Fdx1)<5EKtt4B;qHCiuR}x<=8%VLych3^J4+GT0v`2T(U(;H
zO)uV+6Zk=QJYrW&NM9$Uov6Fw4s)oAPK$_ghbKrvo{g?|ccu41eV*Stb1**pr1wAa
zXlQ+MO6RV42h#f>1R4UmyDL?xmpsrSoG#BJcBNDhXozmA<np^H?%vn^@X+7+y`4Gw
zTflqquDG)lQFp~VkXMnazv;!hA{E)$8}sEHe82K4b_FqdSKJ}t*zK05Mqd)TUw2pf
z-ZpXaowI|RPsVpR>6PDaLpLk$4qfJm6Q=Kj5NHVK?ygj&Uh+VTaJqQJxl$?!G(<O5
za`{~pckk<dS-U_Kcb^=M_u^e~r@yyuDqe$LMJhZ<yLeZ=UHqW6h3>AHkiJfIQ~9pA
zL&CA!El*7zB<cI9d;Q-2={J55?>>j0I)4p%@4mN((mK=9nTOZlCBJ&O5NHUUV~F~l
zK~?G{545VBE*^2Na3>W>oQBZfP3zgc??|G!<Du+m(q?a*`l<Q^lLr-BBNdZ7F?v^2
zk=^NzN9@Y0JR#Se$a$#J9TG~9B+4C6GJaS3J&E>DQ|-pHyW)3aq;K7RUFa`vy*V^{
z&6A?vnSek;K-bMmsY<=%ffnI(@rYe16$BchpDMiK_hfMQzV3%d5b=95%+cQh-q{<D
zB)%(jKUMC^`;rQRw3!AEuOd5pbD}2mDo@CD?Tzk9QPqEHBZ=4*J5iPIjq5d$)QXKq
zrgk6M+1_;3u_^TX-uzzZ>75U>(yG%>?&`LO{!s`tggQag_bXMYmpss_aylJ&Csh&%
zG(<mDepY#Q@9Tbe<R=eRJ~<li?&ly$sPet_Q|0ueVsa<yuK3hcK2Jq<r`fJ*^HayG
zJR#SesJkLB3CC`CNH0&q7V54@q-^T@&!3%}eg5Eo<19(<{pN|#51gK8rB$b&dafYk
z=LEV|OQ}k|<bhUSr&Xt)+?7&6pdtFHl41_3NaB8Bu<%C{yJ8M#wt#o`h9e18PWMyQ
zb43-?I?Z;wuR$kB0u51<F(G}O=%-4GIn;~E+1rvt?8^I+kWTWhiKO)Zj?bQ1*WUZn
zW$}NI(nl|PWlGmK-M1;U>hzP->whO9&=AnIT1r*wB@eU+r&Xt)JXg4riX={Feg4_B
z%+c>Vl87YC0quU$HIY<5mD425JnYK*GA$mXcST>ai-6XA#Us8mc$FvQx)UOESL7uj
zd*eDDL6oQFNti=~boPc*-{}|sHT&q>tMhjl=Jm2omxq4tI~TXo)Yfwaq3qHS=u88r
zD)o{FT7=Wo)^i1+gd}kqBD~^U;qHA$62;voN8_EnaUuy-Y=P^Gcjen9mD3RQ{mQGz
lE`sY2HJMkD1cJRWK~hwy7YU{5<Q_qkJM`^OuIq!r{{VWWxS#+4

literal 0
HcmV?d00001

diff --git a/data/srdf/siggraph_asia/scale.srdf b/data/srdf/siggraph_asia/scale.srdf
new file mode 100644
index 0000000..114223f
--- /dev/null
+++ b/data/srdf/siggraph_asia/scale.srdf
@@ -0,0 +1,3 @@
+<?xml version="1.0"?>
+<robot name="scale">
+</robot>
diff --git a/data/urdf/siggraph_asia/scale.urdf b/data/urdf/siggraph_asia/scale.urdf
new file mode 100644
index 0000000..25c381e
--- /dev/null
+++ b/data/urdf/siggraph_asia/scale.urdf
@@ -0,0 +1,19 @@
+<robot name="scale">
+  <link name="base_link">
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://hpp-rbprm-corba/meshes/scale.stl"/>
+      </geometry>
+      <material name="white">
+        <color rgba="1 1 1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://hpp-rbprm-corba/meshes/scale.stl"/>
+      </geometry>
+    </collision>
+  </link>
+</robot>
diff --git a/script/scenarios/demos/siggraph_asia/log.txt b/script/scenarios/demos/siggraph_asia/log.txt
new file mode 100644
index 0000000..7ae6fd8
--- /dev/null
+++ b/script/scenarios/demos/siggraph_asia/log.txt
@@ -0,0 +1,346 @@
+path computation 27
+path computation 8
+path computation 11
+path computation 20
+path computation 4
+path computation 4
+path computation 4
+path computation 9
+path computation 4
+path computation 4
+path computation 7
+path computation 10
+path computation 4
+path computation 8
+path computation 10
+path computation 8
+path computation 9
+path computation 13
+path computation 10
+path computation 8
+path computation 13
+path computation 9
+path computation 10
+path computation 17
+path computation 11
+path computation 8
+path computation 5
+path computation 9
+path computation 9
+path computation 8
+path computation 13
+path computation 9
+path computation 4
+path computation 7
+path computation 14
+path computation 13
+path computation 8
+path computation 4
+path computation 3
+path computation 4
+path computation 6
+path computation 7
+path computation 2
+path computation 8
+path computation 3
+path computation 2
+path computation 10
+path computation 4
+path computation 9
+path computation 8
+path computation 2
+path computation 4
+path computation 3
+path computation 3
+path computation 6
+path computation 20
+path computation 9
+path computation 15
+path computation 17
+path computation 8
+path computation 7
+path computation 10
+path computation 7
+path computation 11
+path computation 8
+path computation 15
+path computation 10
+path computation 12
+path computation 13
+path computation 18
+path computation 17
+path computation 8
+path computation 14
+path computation 17
+path computation 15
+path computation 10
+path computation 15
+path computation 6
+path computation 8
+path computation 14
+path computation 11
+path computation 8
+path computation 12
+path computation 8
+path computation 9
+path computation 12
+path computation 9
+path computation 4
+path computation 12
+path computation 15
+path computation 13
+path computation 7
+path computation 7
+path computation 4
+path computation 10
+path computation 14
+path computation 12
+path computation 12
+path computation 13
+path computation 8
+path computation 11
+path computation 7
+path computation 15
+path computation 8
+path computation 5
+path computation 4
+path computation 7
+path computation 4
+path computation 9
+path computation 3
+path computation 5
+path computation 10
+path computation 9
+path computation 3
+path computation 5
+path computation 1
+path computation 2
+path computation 2
+path computation 5
+path computation 2
+path computation 4
+path computation 4
+path computation 1
+path computation 7
+path computation 1
+path computation 9
+path computation 2
+path computation 5
+path computation 3
+path computation 3
+path computation 2
+path computation 5
+path computation 5
+path computation 6
+path computation 7
+path computation 3
+path computation 4
+path computation 2
+path computation 3
+path computation 11
+path computation 4
+path computation 2
+path computation 6
+path computation 9
+path computation 6
+path computation 9
+path computation 6
+path computation 12
+path computation 16
+path computation 5
+path computation 10
+path computation 7
+path computation 7
+path computation 5
+path computation 3
+path computation 6
+path computation 5
+path computation 6
+path computation 7
+path computation 9
+path computation 8
+path computation 4
+path computation 8
+path computation 5
+path computation 3
+path computation 6
+path computation 11
+path computation 7
+path computation 4
+path computation 5
+path computation 6
+path computation 9
+path computation 10
+path computation 11
+path computation 7
+path computation 9
+path computation 3
+path computation 3
+path computation 4
+path computation 3
+path computation 3
+path computation 3
+path computation 5
+path computation 3
+path computation 4
+path computation 6
+path computation 7
+path computation 9
+path computation 7
+path computation 4
+path computation 7
+path computation 7
+path computation 5
+path computation 4
+path computation 5
+path computation 7
+path computation 7
+path computation 4
+path computation 4
+path computation 6
+path computation 5
+path computation 9
+path computation 9
+path computation 8
+path computation 6
+path computation 3
+path computation 7
+path computation 7
+path computation 9
+path computation 5
+path computation 6
+path computation 5
+path computation 9
+path computation 9
+path computation 9
+path computation 11
+path computation 6
+path computation 11
+path computation 9
+path computation 4
+path computation 10
+path computation 63
+path computation 3
+path computation 7
+path computation 4
+path computation 4
+path computation 11
+path computation 5
+path computation 5
+path computation 6
+path computation 4
+path computation 4
+path computation 4
+path computation 7
+path computation 4
+path computation 3
+path computation 6
+path computation 5
+path computation 8
+path computation 10
+path computation 16
+path computation 16
+path computation 14
+path computation 5
+path computation 8
+path computation 7
+path computation 4
+path computation 5
+path computation 5
+path computation 4
+path computation 3
+path computation 11
+path computation 5
+path computation 3
+path computation 4
+path computation 3
+path computation 11
+path computation 6
+path computation 7
+path computation 4
+path computation 3
+path computation 5
+path computation 9
+path computation 5
+path computation 8
+path computation 8
+path computation 4
+path computation 7
+path computation 7
+path computation 4
+path computation 3
+path computation 3
+path computation 6
+path computation 3
+path computation 4
+path computation 9
+path computation 5
+path computation 3
+path computation 3
+path computation 7
+path computation 8
+path computation 8
+path computation 3
+path computation 9
+path computation 10
+path computation 4
+path computation 5
+path computation 9
+path computation 5
+path computation 6
+path computation 4
+path computation 7
+path computation 11
+path computation 8
+path computation 4
+path computation 10
+path computation 6
+path computation 4
+path computation 8
+path computation 5
+path computation 8
+path computation 5
+path computation 4
+path computation 6
+path computation 3
+path computation 2
+path computation 3
+path computation 11
+path computation 6
+path computation 4
+path computation 12
+path computation 5
+path computation 6
+path computation 4
+path computation 7
+path computation 7
+path computation 5
+path computation 7
+path computation 10
+path computation 10
+path computation 4
+path computation 10
+path computation 7
+path computation 7
+path computation 11
+path computation 4
+path computation 11
+path computation 5
+path computation 5
+path computation 8
+path computation 8
+path computation 5
+path computation 9
+path computation 8
+path computation 6
+path computation 2
+path computation 5
+path computation 2
+path computation 2
+path computation 8
+path computation 13
+path computation 7
+path computation 10
+path computation 9
+path computation 6
+path computation 7
diff --git a/script/scenarios/demos/siggraph_asia/scale_hrp2_interp.py b/script/scenarios/demos/siggraph_asia/scale_hrp2_interp.py
index 1057901..3f6f8d2 100644
--- a/script/scenarios/demos/siggraph_asia/scale_hrp2_interp.py
+++ b/script/scenarios/demos/siggraph_asia/scale_hrp2_interp.py
@@ -111,8 +111,8 @@ q_goal = fullBody.generateContacts(q_goal, [0,0,1])
 q_init = fullBody.generateContacts(q_init, [0,0,1])
 #~ r(q_goal)
 
-#~ fullBody.setStartState(q_init,[rLegId,lLegId,rarmId]) #,rarmId,larmId])
-fullBody.setStartState(q_init,[rLegId,lLegId]) #,rarmId,larmId])
+fullBody.setStartState(q_init,[rLegId,lLegId,rarmId]) #,rarmId,larmId])
+#~ fullBody.setStartState(q_init,[rLegId,lLegId]) #,rarmId,larmId])
 fullBody.setEndState(q_goal,[rLegId,lLegId])#,rarmId,larmId])
 #~ 
 #~ configs = fullBody.interpolate(0.1)
@@ -274,14 +274,14 @@ def play_all_paths_qs():
         if i % 2 == 0 :
             pp(pid)
 
-def test(stateid = 1, path = False, use_rand = False, just_one_curve = False) :
+def test(stateid = 1, path = False, use_rand = False, just_one_curve = False, num_optim = 0) :
     com_1 = __get_com(fullBody, configs[stateid])
     com_2 = __get_com(fullBody, configs[stateid+1])
     data = gen_sequence_data_from_state(fullBody,stateid,configs, mu = 1.)
     c_bounds_1 = get_com_constraint(fullBody, stateid, configs[stateid], limbsCOMConstraints, interm = False)
     c_bounds_mid = get_com_constraint(fullBody, stateid, configs[stateid], limbsCOMConstraints, interm = True)
     c_bounds_2 = get_com_constraint(fullBody, stateid, configs[stateid+1], limbsCOMConstraints, interm = False)
-    success, c_mid_1, c_mid_2 = solve_quasi_static(data, c_bounds = [c_bounds_1, c_bounds_2, c_bounds_mid], use_rand = use_rand)
+    success, c_mid_1, c_mid_2 = solve_quasi_static(data, c_bounds = [c_bounds_1, c_bounds_2, c_bounds_mid], use_rand = use_rand, mu = 0.8, fullBody = fullBody)
     #~ success, c_mid_1, c_mid_2 = solve_dyn(data, c_bounds = [c_bounds_1, c_bounds_2, c_bounds_mid], use_rand = use_rand)
     #~ success, c_mid_1, c_mid_2 = solve_dyn(data, c_bounds = [c_bounds_1, c_bounds_2])
     
@@ -292,12 +292,12 @@ def test(stateid = 1, path = False, use_rand = False, just_one_curve = False) :
         if just_one_curve:
             bezier_0 = __Bezier([com_1,c_mid_1[0].tolist(),c_mid_2[0].tolist(),com_2])
         
-            p0 = fullBody.generateCurveTrajParts(bezier_0,[0.,0.1,0.9,1.])
+            p0 = fullBody.generateCurveTrajParts(bezier_0,[0.,0.3,0.9,1.])
             print "p0", p0
             #~ pp.displayPath(p0+1)
             #~ pp.displayPath(p0+2)
             pp.displayPath(p0)
-            paths_ids = [int(el) for el in fullBody.comRRTFromPos(stateid,p0+1,p0+2,p0+3)]
+            paths_ids = [int(el) for el in fullBody.comRRTFromPos(stateid,p0+1,p0+2,p0+3,num_optim)]
         else:
             bezier_0 = __Bezier([com_1,c_mid_1[0].tolist()]              , end_acc = c_mid_1[1].tolist() , end_vel = [0.,0.,0.])
             bezier_1 = __Bezier([c_mid_1[0].tolist(),c_mid_2[0].tolist()], end_acc = c_mid_2[1].tolist(), init_acc = c_mid_1[1].tolist(), init_vel = [0.,0.,0.], end_vel = [0.,0.,0.])
@@ -310,12 +310,12 @@ def test(stateid = 1, path = False, use_rand = False, just_one_curve = False) :
             pp.displayPath(p0)
             pp.displayPath(p0+1)
             pp.displayPath(p0+2)
-            paths_ids = [int(el) for el in fullBody.comRRTFromPos(stateid,p0,p0+1,p0+2)]
+            paths_ids = [int(el) for el in fullBody.comRRTFromPos(stateid,p0,p0+1,p0+2,num_optim)]
         #~ paths_ids = []
         global allpaths
         allpaths += paths_ids[:-1]
         #~ allpaths += [paths_ids[-1]]
-        pp(paths_ids[-1])
+        #~ pp(paths_ids[-1])
     
         #~ return success, paths_ids, c_mid_1, c_mid_2
     return success, c_mid_1, c_mid_2, paths_ids
@@ -372,7 +372,7 @@ def gen(ine_curve =False):
 	#~ test(3, True, True, True)
 	test(4, True, True, ine_curve)
 	test(5, True, True, ine_curve)
-	a = gen_trajectory_to_play(fullBody, pp, allpaths, flatten([[0.1, 0.9, 0.1] for _ in range(len(allpaths) / 3)]))
+	a = gen_trajectory_to_play(fullBody, pp, allpaths, flatten([[0.3, 0.6, 0.1] for _ in range(len(allpaths) / 3)]))
 
 #~ pp(29),pp(9),pp(17)
 #~ gen(True)
-- 
GitLab