From 06f003be7410b41732e46cb6a70bc1d68bd0a5e9 Mon Sep 17 00:00:00 2001
From: paLeziart <paleziart@laas.fr>
Date: Thu, 19 Mar 2020 17:21:14 +0100
Subject: [PATCH] Gamepad implementation with a new script + 2 test functions
for profiling + moving camera in PyBullet
---
Joystick.py | 21 ++++---
TSID_Debug_controller_four_legs_fb_vel.py | 26 ++++++--
bauzil_stairs.stl | Bin 0 -> 32684 bytes
bauzil_stairs.urdf | 24 ++++++++
gamepadClient.py | 72 ++++++++++++++++++++++
main.py | 13 ++--
utils.py | 3 +
7 files changed, 143 insertions(+), 16 deletions(-)
create mode 100644 bauzil_stairs.stl
create mode 100644 bauzil_stairs.urdf
create mode 100644 gamepadClient.py
diff --git a/Joystick.py b/Joystick.py
index c60e2891..ee9e888a 100644
--- a/Joystick.py
+++ b/Joystick.py
@@ -2,6 +2,7 @@
import numpy as np
from time import clock
+import gamepadClient as gC
# import inputs
@@ -21,9 +22,11 @@ class Joystick:
self.vX = 0.
self.vY = 0.
self.vYaw = 0.
- self.VxScale = 0.1/32768
- self.VyScale = 0.1/32768
- self.vYawScale = 0.4/32768
+ self.VxScale = 0.1
+ self.VyScale = 0.1
+ self.vYawScale = 0.4
+
+ self.gp = gC.GamepadClient()
def update_v_ref(self, k_loop):
"""events = inputs.get_gamepad()
@@ -36,14 +39,18 @@ class Joystick:
self.vY = event.state * self.VyScale
if event.code == 'ABS_RX':
self.vYaw = event.state * self.vYawScale
- print(- self.vY, - self.vX, - self.vYaw)
+ print(- self.vY, - self.vX, - self.vYaw)"""
- self.v_ref = np.array([[- self.vY, - self.vX, 0.0, 0.0, 0.0, - self.vYaw]]).T"""
+ self.vX = self.gp.leftJoystickX.value * self.VxScale
+ self.vY = self.gp.leftJoystickY.value * self.VxScale
+ self.vYaw = self.gp.rightJoystickX.value * self.vYawScale
+ # print(- self.vY, - self.vX, - self.vYaw)
+ self.v_ref = np.array([[- self.vY, - self.vX, 0.0, 0.0, 0.0, - self.vYaw]]).T
# Change reference velocity during the simulation (in trunk frame)
# Moving forwards
- if k_loop == 200:
- self.v_ref = np.array([[0.1, 0.0, 0.0, 0.0, 0.0, 0.0]]).T
+ """if k_loop == 200:
+ self.v_ref = np.array([[0.1, 0.0, 0.0, 0.0, 0.0, 0.0]]).T"""
"""
# Turning
if k_loop == 1500:
diff --git a/TSID_Debug_controller_four_legs_fb_vel.py b/TSID_Debug_controller_four_legs_fb_vel.py
index 78b4a4af..242935b9 100644
--- a/TSID_Debug_controller_four_legs_fb_vel.py
+++ b/TSID_Debug_controller_four_legs_fb_vel.py
@@ -91,7 +91,7 @@ class controller:
self.memory_contacts = R @ self.shoulders.copy()
# Foot trajectory generator
- max_height_feet = 0.04
+ max_height_feet = 0.05
t_lock_before_touchdown = 0.05
self.ftgs = [ftg.Foot_trajectory_generator(max_height_feet, t_lock_before_touchdown) for i in range(4)]
@@ -509,11 +509,14 @@ class controller:
def update_footsteps(self, k_simu, k_loop, looping, sequencer, mpc_interface):
- self.t_remaining[0, [1, 2]] = np.max((0.0, 0.16 * (looping*0.5 - k_loop) * 0.001))
+ """# self.t_remaining[0, [1, 2]] = np.max((0.0, 0.16 * (looping*0.5 - k_loop) * 0.001))
+ self.t_remaining[0, [1, 2]] = 0.16 * (looping*0.5 - k_loop) * 0.001
+ (self.t_remaining[0, [1, 2]])[self.t_remaining[0, [1, 2]] < 0.0] = 0.0
if k_loop < int(looping*0.5):
self.t_remaining[0, [0, 3]] = 0.0
else:
- self.t_remaining[0, [0, 3]] = 0.16 * (looping - k_loop) * 0.001
+ self.t_remaining[0, [0, 3]] = 0.16 * (looping - k_loop) * 0.001"""
+ self.test_tmp1(k_loop, looping)
# Get PyBullet velocity in local frame
"""self.vu_m[0:2, 0:1] = mpc_interface.lV[0:2, 0:1]
@@ -527,8 +530,23 @@ class controller:
"""for i in range(4):
self.footsteps[:, i:(i+1)] = mpc_interface.o_shoulders[0:2, i:(i+1)] + \
(mpc_interface.oMl.rotation @ self.fstep_planner.footsteps_tsid[:, i]).T[0:2, :]"""
- self.footsteps = np.array(mpc_interface.o_shoulders + (mpc_interface.oMl.rotation @ self.fstep_planner.footsteps_tsid))[0:2, :]
+ # self.footsteps = np.array(mpc_interface.o_shoulders + (mpc_interface.oMl.rotation @ self.fstep_planner.footsteps_tsid))[0:2, :]
+ self.test_tmp2(mpc_interface)
+
+ return 0
+ def test_tmp1(self, k_loop, looping):
+ # self.t_remaining[0, [1, 2]] = np.max((0.0, 0.16 * (looping*0.5 - k_loop) * 0.001))
+ self.t_remaining[0, [1, 2]] = 0.16 * (looping*0.5 - k_loop) * 0.001
+ (self.t_remaining[0, [1, 2]])[self.t_remaining[0, [1, 2]] < 0.0] = 0.0
+ if k_loop < int(looping*0.5):
+ self.t_remaining[0, [0, 3]] = 0.0
+ else:
+ self.t_remaining[0, [0, 3]] = 0.16 * (looping - k_loop) * 0.001
+ return 0
+
+ def test_tmp2(self, mpc_interface):
+ self.footsteps = np.array(mpc_interface.o_shoulders + (mpc_interface.oMl.rotation @ self.fstep_planner.footsteps_tsid))[0:2, :]
return 0
def update_ref_forces(self, mpc_interface, mpc):
diff --git a/bauzil_stairs.stl b/bauzil_stairs.stl
new file mode 100644
index 0000000000000000000000000000000000000000..c75c11b9c75eccda0502c24f645bec841a864c69
GIT binary patch
literal 32684
zcmb8136xz`nT0PX5Yl0Ki7XTiL8dT;5LlU@-bsLfA|M12SVVzPfK7x15M@ra8XB1*
zgFqo58ni*$s0^kIow`qGF(oAmNLOe@2~dK94Xt5PI_bObsqdWs-1lA)?^@Q|@BDk8
zfA9a?`zn<b&N_d={Bu5cYUjAq&Y6GaxC1_W>RFwqp0nqq2~(zw8-MP3bH`2GHT#FI
zt2WHCQP(}$G5XUl)PH-^Dek%DwZ{K^V5S>2=?<42Rcp-LWTrc5>>cjDeTwy;^tm&y
z`=raVZBIPZowfGL`qm#Cm9T1C*BYBHoZ*hU@f#JztUE4pjVJG3alowcy_R+9?zP6T
z_d3_pcf0%b*jnR_DSfUn?-NCj{~Yn%`lQ=d^<Mk<vAJbUy}Q=<?Of+>yYO~*<5y~p
zbN;^4^;~dtq4?F9arwL*?(1E9?WEkYR<Ely-0z+1f8%!d_}jI{^a*`#%q;b|;-rOn
z|2f%;o{<YzSk{djk8bR6<qY?kZ+*jkXT<2nge&{ps%6I%iZO>y&yU#Rw)zp9eaKms
zo!dQqwZ^_r_PNguXjaE>+%s*$vz_^7pBk)NS2mN~Da*dH<!e9ge5KPJxW{0xO=!KR
z=h1)PhV`eHJYV1W&AHBYx2{sfE=w<QSDblK$5Z#L>97@hHtBghxyhz(^TU49+dF?|
zZYxQS<UIB|;>F%;x9{)$_m3Zz+Z=WZHdB_}G4GQ6*^~ce#n*niL9eaYvq{h6_$xn}
zchA4N-ZOTy<fQCG?L@Nd=aYW(<E__p=Ks3cVBMy*ce~A$WjlOf{L~j->CBJZZLn@x
zcDlA-mc4V$qA8F4p)>#H0fTj0u{rD%vTWj`mrNaawKKnJY}&)BQ&*RL;&0RPr#sz)
zpC3$SW#^M7pWpTOY1cg4>8?Iuu-BfsJxzN~S$6XyU+Fpf_nmIUNrSz1FRQlalw~{5
zJ<v^i@cR0u8;o%aPp>szxpSsle!=PPM<Z*E?q_GX{C`h%+Z<kNT>k1xH*v(;dX}v?
z{Af4xeP8U|Z;K6b%ewHCT4VpWoV#rMPWPKFYK_@vt#m8ay<I4dTzysVA!DBDeP!^3
z+_KL9d#$m}!86>r6`k&+8;af^Sm|y!_02*t>bgtw7e2XP$90c9)L~hN{IS;9_JW!2
zxDTK1-q>Pv<K}Z$x-ED6TcOC#8N2MRKX&HRrwrDu!aB6-EPLVNj^2g8d$xYo<|jK_
zu~T^D(OTn)H7nitNlO0A;Pjr4ztrh&-G8vx`n68d<p1{{9UWJEbg(}6#!hGVvOUu2
z-gfsqxA5Y9>-7unt=o#tk@nbf@6qmwKQHfHc=mp|t=LIhw^_F5*5mT+_qezB`o|{a
zwqoaIGiBMoeeU#p*(S5<`yTyp-B#?}Y`)@?;ihe;{>z^`^Z(u@JrC>3X3Da!uDN~c
zOV4%Yd(IxL+xgfYsp7J)Z$9m$;&b;?#pkZ=Zs%qvl4U=<Vn)wvgPrdE2d4_lviY*?
z=qLYH-)_$n-L>;aP5bLjwZ@k=J;3d}_Z@EY9cqn7Hk|2pE$|b?_rbRdyz>vPEAEo|
zZI`;g{^HjiM?ZPxG|Sq4n_A<WCpouUlOlU{s$2fvz4L8<GI_dXJ#}iW@uzL;ZeZda
z?wz9F>{sh<Y}uoGt3|!5_W43S>FFiYE$hZLwZ?4^)ZLCnk6GQd#u*#b-MCV5#+jGq
zTix~CiV-)>o@!aQzGrmfgvV#NT?)n48;x%C{BedGUn+KTTj!6?`ncQU*v2%=vQtPE
zYd8MovIDO_v?t5Dob6#V*`2a%_s4IUyx$hbPg%EaVQ&3?@ts=Z!Hu2UzHpLG_vOiN
z)xZ7XiEi|irBnC0rq+1r?U`<ef@NK$iXT0;(9L~p?20e%HEFr6SXb$EmtOs1ebwyF
z{J3WqFSo8N%T7JZCjWF)e)JQ^<mcaVWTLQBu$i*#j1v~-8*Fy%iXVRJzoyxWRixA1
z_>*gU51#O8cgMLSmfMO=ZYPpuTRrvcv~6zv;De8?TbP`O^=_w-W&7oq_dId;{3$v?
zTe0`C?U!XAIO2E9W`Ae<sZ!XAJ!iX9mgV=nzHE<qtJEX6$!(@I$AM4(V%b&)t}PYz
z1nqg)bIP*&H=H|V)ctE8P!GG8RomXhbAI#5?ydLyrGEcsKmTL*vEqBtFZ<k|jyleL
zeal+o;MZ5WmkYf2s9NLDftBuu51qGvmR-H`k?w<=yf44yv@@q$)-Gq&8f#wtnR~5C
z@%_*Lq4$*T&V1R1bEjKYU-1m~U$(MTEO@i{o_NwqcU9Tr_}wndcR%s?`uC3c?sUs~
zcy+BYrqSpAT=Y2ity<&Wul2dNOT}@|oHFIT@5{>*v^}gttIo2W4mozkXTH7AJ^#U}
z({060;mi|ijpJ7Lx$Daw-@j{c^4f_vEYoz?T~hq{{EeTv?BVnFx8BoP^}Tk2+j@id
zxU1efdAZ%o_DB^!ShtURV8rD7Z>zhO+ltL(dlx?&T-V>b;CpA}Pw%j3nypxcb(m#~
z*DlQWo-pv>;Wx}%ZYy@u>2z;D^qW&Y^v_q9)3vj*3fnKsUVi?~DIeMV_EKTfTD3i!
zEW7Hl`Mrx)pYJw0@%rVqyH(g37WeMi8!y{&+fB>!unNnvGt9DYPw%^b&GdccDz^&D
zvJ=sds9nX+M*cdxV`Sr{;+YsfXK{z{#DRws3{S6JxxQM~FZxFX1y~$io-IU0J%s58
z7Kh6_!<AMo3#^?~YtErn)HajWxzZe3ZIvly>z%r5J{H?dDD;}<z=@KhW(tul^ziF<
z<y3?@53o2~J_i-`xM<AMpa7?Ay`zGHep<(2P?dghg&GzL?W9^f532x+g<JM7RpotU
zD_|Tl^~e??hs7vxsG2VCtm*_-`y9G6-dXa4@Jt-E=$-`o-Tl{iU%}EHQ^hFK=fkq-
zC$%sNy~g{Bie-VD&x0J=11y%XdTX64&7oCTQjjTS>z%EDQQ$VH%8pehu-fO)35unC
zg_#?e{q-834_G?qDn=2{qspS6)^QkAWe!|v)v~~Ht6KVeuor5ZNo=k(hgL}|1#VNe
z-q{Km1-*kR$$=AC?Q`e^_ZnE@mL76)UHA(<w<nnW^&0OhSUTq_MiI}W%A%jvg;A6}
z!hJ=>vcPg1*+S&7*k;l?SDHhsHaTunw%*waSUqrsLa*^TC#`jeqB)1|3>3ml7o7CS
z^^S`6(mA65qlo801qJ=I&eVe{+*edA3*0^j4(Wkg_SdU-W-DMEDiez49ICX+ghH?J
z`-gQ>&D5Mjr}pCOmxpI!uYoTmIO$QIhd*b}^5>jl6fsAYML(?zqtI)-uc%lSxP1=o
z(JGVHxx6!SXceq|ahtOB&Q`!Ea2r%*s;U!M?Q`hP5KH?Ca~@#!*K545VEOyip9dI4
zJP#`BAxu9oin2$U16NwHEU<QpY$2-v+f3qcr8%@pS{y20%GNt|rvgU7q=PESffHEm
zbLa%GU45aubHR%8`^WZoE=Vx@>owk2uyoE<j3S;#l|?_T3!^A|l<#n*70Uw4ZDb2s
z1=wa1hbzsYRVmg#hZL<cX`L%R=cL7z80~ZD&OjlID=_=(HQraSbk0?bBA!Q;ML(^#
zJ`Y@JFUtZqpNCa|#d`M~>H$j%+-hA{a}KS-LNR;}oWN?IL#MXkcC*4Waoy_a307~e
zcwfQNIahJY*1IZ;ep(ksq1Sj{QL!wr@<p}~IV=`S>vE`dt~7^MVM&48l&yER0!D$`
zpbF19X|2QdIdo^B5N5hy_SdV=K}Ak<&M3er()-G?=qE+%9Jtb6mIYRC)%@uRSp`@u
z+=|1M=FloEn<-`MolOWvF?<f3z-pgEr*>=4Sl2UlBPYDaMPm|-E4{}13YN~fic!S#
zsIus%bzu}`k8od6u`ICMMz#<+EVh}n&XwlSs!fjDl&yER0#*-Pq0no5&Pi(>qG+At
z@`F$A#ruw97bQ69k?WoIg{6I~7)5#>mPJ3Qg;D4==AdF(;PyGRhaA!aw_4|l=OI^G
zg~fDPSFJgRR;$TztD3Gf2h)YsK8K!EybCiou;1N(_0FiMhn_nWU=%S26%_Q-Iu3)X
zyzg<PRm%cvC)Lu=EA~Py7H-AnN^@ux77kmV2RWnwqoB8Hy1cWh6IjFN;E9r&Pam+~
z-G7ZaU}@hfMv<O}WzkP+K9zzhoCg)l0=GU76!K1wch8~Du$Zpg21hxFw0aLlL2tP6
zJggH~?Q`rtXL0Xg+bt@7hiM}xyy_z-Cz$>9>L+Yj|1_{9D8MM<c~DUgVfuklls(Eh
z;7Ti&1=dcHEo2p7n@Jq5G>2AOWlGt4r|z1M#WoWPz54U0I#F`eOd+y`>LYK;sR(l(
zU~#y74k~i;#`s%<0-Un-jtUC;X&r|_Rn8e#s9~YdPO8PvE2{vDg<JM7Rk^;}3K)k>
zJ+g(!VKE9Es;0|3t2%)-e2#5?+24Wpi9b6p!G3rDHJ%47?OVkt;`32u(NF8bDD)b0
zP_Znq+(x#LlA{3IOyY2*IkYOp`sa|MRVJ--r8#hd!}dA!M2RJZFs{JtuUDUgiWGW+
zQGijz^Pqx)ep<(2P?dAWl~yeatesSg&xcijZ6>j~(i~cCl__QG9XTwvnNaA}JF7Y&
zZTlR5I%=xZQ>1-``Md%rJ<8{tiWC=)nGh6U6!AQ$prD`Dd4hwgoHMSpYFS|Iq*^==
zs{q?fV*5+K-z7v2i*eZc^NJi&fKjA5iXKT0oWN?IW4Ae5gzvAyoCjFFx%`Bw4D|?#
zB47A!Cq+CDDkwM)t>ZBCD0ARStCj`UPO8Q8unMqPxD}f#&7oCTIBb0$<d6c4V)z_5
zfi-*%K0oA&Pam*)bHyC6v~LxsY`v?p=%;m_;GhcULB+Db$|ps79w_9U9`Bw*`@mwl
z%A|E&>3v0|RVL5(U%j)c6CAeBQB)k>h0SzJob<@`&a(8}p#Y<ZIjAVDF#W(N^y+iq
zN-LHHZaxpI01r*KlEZ#pnd$xmIdEl@GilYb%zj>x7FVjb&+*3i--hoW!hBwVrRMSz
zrlPd<kpn>iPT6`#1qGj1TIUH4s`9?a6>3;0n$N>3z+&N+{q>sW&<ZRZw*I^#hZJBG
zI8;rSzptuJV71S&X8fFR9>Sal*zfMY`W#f`<jXG~6%=3;@%f;FLar=Ep;w;+S6Y#p
z9AK#<TgWQFV&PUCt~7^MVd1Lvd5}X2FbW*1rYp^X6IjFN;E9qeo<XqR-G7ZaU}@hf
zMv<O}WzkRS^f|XVs8|+Q`P9d!7Ls2SV5Xqn;&7!ov`Sj0YZc8oq^KsRw`#iLd8mRD
zSnYG@i9#XF+`xW!|J6IAq8@tgP=Hay98^%yPwO}gs_=P5#j?QKNs4$L<j^WuEZl0H
zE6t%*SU7Bb9^{Y$i~_f+>GIC1PGGgq@zCopxp$WQs{A((;e!@Enqa@X|LSvC*2sb9
zg93~qo(C0q7p5N=g<gFQTxrF!z}hLYg{%TB7H-AiN^@ux77jIql&yE_P6doYnNa98
z&4CkG!{^xdnge=KtohQs1SdUmy_0uX+P8{Pq~~E-^pjc`g<fM0DwYM7+sGD@D->Xx
zNgS?t9&)ACR+&<^-jUW~^}rPhy{0*Ef<qLoa*UrLmRF3Q!Q38t-Q%Rk(0N4GX{YTR
z6krtbJgBILF#W(N^hyrzN-LHHZl6O6d8fy_=RiSn_28UTYtErnST>W^xzZd=m-DFR
z@GAXDTz+{5#j=>Eiyr>Z)nf69gMxlix1JRW`dLNXpY)bxdw@}-v!Z*|VKqmb6%`cP
z7fcS{i&>$-p;dS$U-dJHSR}XmqNay;Bqa)q*KbxR>=Zan)tYCeRk^YX@~NgPofS@C
z*-ViwG-vTcZl@0)?m9*e)P)~-;eZ5-!{wb>R`L4=K><$LdPhY)gy{!Hp;w;+S6Z<w
zuy%@UA*%q}OyY2*Ikeg;Q_9vmb=Q0>wwX}qHO+w&B}dH^B3r0nhWQ9{9$;~}d=4tb
zYkkqnj|T-fW$PUk6gbg34uh)ns~%J=3#^@_h|h;rfW;DKhM`CK{liwkQs6da>z%ED
zQQ$VHk{n{G2dv?9jCYTf&x8~U#`Ptb{q-8p1D5u!VicaOcU2bsv@VQ7uYSUELdCMc
z$`{!}Rsj}ESPsSEN^@uxmK3;6*?MOyU=+9ws&F2pRS#J0bLdG$A<T5a?5|g!gNmH!
z2}S`%5zm7P3i@dshe1{5z?D`l3#^?~i_eEufW^YC*j#B2t-`{gJ;CN2QnbpXb*}h%
zMOs{m(LP7<FK*>iG;&}`U0Bby#q3}HnIzXc%PRhTFetz%Vh$?WS6EN2#VF$6C*n#K
z6c$UZ_whWe0=#}Xv`?!{TG!Q_L#wSaY29D^y=c`5pI2mR&M{E@H(ty~nDYSBL9gB!
z745ZM@n5u|0HcUGsGy*q)^QkA<@13nty&hieGVy5i-lXQ`%8b~6S5UB4()fG<{VPD
z%A|FFaUNACq#Ztoo+!NQNwt^`dX47+OZ!$aiZq91(NF5upI6F3#j?P1SUM_CPsl33
zt#jD(kSo;IA?sz2^7MpAYq5IZO6&fz=RsOriGiXyhn^@D!ngttH7H)lK}CD%xkCX)
z5zm7P3i@fCse2VUaHUnt0+S=Ca;v~$RUO$vmIcOP>+>K7``YBg=g_{^VKqnm+#S1U
zUiq1!|MEpxPpZW}ga7I$Y*_^}D8MM<c~H^5!g^{gMxj^l3|Cq~VX@SD-<(6MsKvsq
z*16IgT7`wH)}L2$LIsS1PomJHeD11FU`fpsq=-2dbX*ZC!kh<~{YzE(<WiB7p0SI9
z0*oS_2Ne`J(K-%;s?32aty&gXJE_*3L#wF8!mZZ1yfa$?<B+LK(VRo8tukqy%R8$&
zA?@%v^sM1s&!ENZuh*CZmiDb;6lo62qMy|1bIyd7gNkK=<*;;Ao}Q3ZfY&bvYU@x~
z>$;kAXtkOgx2oxi=RsOrsoFk=o+uQ;xB^Sf<(*Mc+Twpf0Z!R^M+F7_wB9-gu2925
zp`BEV&xcij#lo%FTxkxi!jeKgTAzolfKlKu^eE4>>I9b5!ct2SbIdOOI}$xbiywNu
zLs(C$#q3|Ia(%U|=Pn!*6krtbzM`Uih3N-Ip;zw=S6Z<wuy%@UA*%qdUk=nHSC7`8
zSGEGSnY0sEngb_%UbW9LX6YeeKEk*Hv%g;bgsG^<oeMgG0*oS_2Ne{Yht_czRONkz
zE3H}<SUaf}?<=bSi-lXUxzZe3g@wb`=RpoBz$k{#ffHE6=g^abLQks2>dh5%z|y`|
zoU-+<%A%jvd4hu~oCg)l0?VNk>3N{gbm<{%KYdD7`JCGdSo`8OI4aqCXDeV7^bV>d
z2NQ+WK8K!dybCj3F#GG(p9dB7&~t|Zj3PcCR8Y`Q>o^RmG6$}-YFS|Iq*^==s{o6I
zTd}#)99o5iLwkbFIizTnN$Xtk^NO^%5~F<%{iISpMFUIr7cQSvi)j#k9<;38<}411
zfQJ+@N2nD03ezuO+oL>>;Hs!tR=~wh62<edih$QIN3l=KOlnYEEpimAa>A^FtCFuT
z{9Idg!sk^`H03Dnh%g^vdIW5HluvG`6niZ_WOYykJfw&@LS;z7d1xJnUPO-Is#vzH
zfZOLN6hU3Eg4-ci@$VUIC19IrNYNt4kSk^tT-hGweN}Zr+MsBeqx{Sm@?Jiv7TX@-
zJYZ?xCfqWIWzkRS^z+J|2NlZ-xP6YIyQV-7;j(x897780&??gNpi)haTi>QB$IwJ!
zwa=j^3h%<W3b-jps1!Z)+@T0~ND*^{%8-J7TE}6UBe*J-Ei2%r^RSA53s!L3K1Wdr
zESqUa(IUr?E1UdXatt|vRn399u6)koZ{=5=_MP%46XBhT&t(5I&Q=XDY`v=LE8X4Q
z#lQA=1z0-?qwt^aQc$6vRW$!pRAtEtimEF=FYClQ1cx5wGcJF)s-{I@bNEj=tH5DZ
zO>+M|1cmmp`=Yiz(pj-@b#5U?`FW^r?4=5L{bq%N>EbZ-Xqpu{xMNV*9P;GvtH1R3
zyIqU_r#iX1vbZyZr2vb`<?kz1Zti}6nZsi3r1i)a(q2^5ql(o}y!fY#D$Dk$x{CJ|
zEWFDJIP^$nMRLmm+Z^$}LV?5fv(jg;_9Cs_7q#sX?<;y(HHwfUxvx;b(!RpO&q{m2
zTEAJrqMyx_Wd|H|&^|gx?yH@P6D|Hlw{&vpKKqndii?ME@#phnp1N;HQO-)0(@y(C
zP=K|QaJl#W+$q{?$DxXHzyhmxsYqsJS#m`W9F~6eRR$sL1q<&|fJ2YuzLG0dEDP*&
z9CyqyB+perVZEcEN9nBi;-5j1!y*Uy#O`XE6|PX*ex;&mR#L#C$MCa4QJot(;&)Dx
zGf}2sGszW~|Gd&&qTf7|s}bGZ<?oJ40fwztKPy$byWbxaVC^J~B0hsu)T4@<e@m~*
zs`f)sItup{EWFDJ7)3fOl3Nzo=J0(~L4iZ<i-I08JqqhY7`5$@&We54e^yf1eTAzh
z7@t8Zu&hIJG|vhaQy{s`;b%3!<BHzBZs{)mWoaWPoMksm#vzzFm5S24D%uMLSUdS1
z8)sd`bM9v)?|Tka=w}rnN4OuWtZF|LK7+nmC+dd+j3UkoSJkvAd=BmHFaHcGhZH!h
zs$uK3tDF_d?Y^jek2UKSHqDBCQKQhh-<wXh0%p3^6ZGlDq5_rzhoMLL#HtUog2fb2
z_#8MYlII^O{=d_>uY~D$w&@-DniUo8g#z6AzLJ9Z(9bGDj&fq^R%LOQ&<};UepXbp
zuXPAU5$`KpsbX1R?WFy=x^k<)VO0%VuYOh}xBH^@J?wpDdw|z(R&rwZWg;Xm9hG;A
zt$^*^P&Cg97Wb9SlqApZT<}b|uY|d;=tmD!UCF(KLjBlDILqwkmG)w~mIbDV|8!F)
zD(uUBrF9$zN9C-jNP%~YLywXzEv{5SVX@Ca7oI^TibJcQ2O2*slGB4s@fo!DRn--0
z+fVy9&r024(PQ{op<uc=BuBijaK%KK0*dBY&F+~nqqqC%^3PYoeBYHrFmv*GsAw+~
zVC`gk`1Ddxp`TTR9K}83uPRGUP*h#{^wx=W2o62MeT6Hg%f2XVjxu@q4BA<NQP6`F
z`k5dmheZx164^p1a0QEr&?DYgwgRRHnL>{;J+snYU@34IdNj=nRyDcS>7m!Ia#j=H
zo>=})dwt|GXYtJ!uPWCQMe*OFE_14^Teq(GeK@ZWmMf`+%e_M<RZv(KnBR`}j`WpE
zwFmuB#3zah3i{y|MRbNMRV)ik4k>i`SuqnFR@I*Czxw;C>O}jZrboO>tShipBst1+
zXD?O2QZNw|&9lNgr$%xVW$N-2t4^2#3UZLXE2q2urgmV-SIavCEVVFf-$yJ}`uqEX
zLYSR|qaz`@S6L`*51*b&wTE?B_9!PSL<NO)Sk2*25m(i;HlKFp^0Q(u99Gq^_1YDl
z!Ky3NTK7zJ))iPR6zylF3M~5RjzJOcD`tgvu~3i?Kk2M+0;~P3Rt?=t`a9O=F8oA!
zUxB6O?_Ogcu~gCDv7!)WC;sj=K7*>Duq?3c;nP#8_Mjh%I4deB=!aVr@f2~Tie-Vx
z5t8Rtfy1gAw(o<Y>IyYInrEebtr|s=qkO7t1x!EPF({g6g?F(~RL|Tyt2%+zepaez
z-{Ri`i{J6tB*9W^J)M<Us`U3CR!1SsPQrew=;&T$p|CxCdMec(^g|J6MFoX*Sk2*Q
zg)5T7viV3*=E<!BhgEf=a44#-P;0&1yZmk=ghKm*#X>>y`0wYo0;V4)TlNT_?aT`A
zVxeFPQuu$1(reWTtoE~_NAa1s|8|S!)zt$mwXk;P>T+VK^7H=w4iv)dBrJA(2I*d9
zp|CytokFGBgMKLDeMJR@by&^eo#CpQ*5)I1nJ2dj99Gqd!l9_TLap_3?{HtC(7s@?
zP>|d^(pOsn(~r}PY$0Zacd<|mKP#NTYCEgq|9kw_8QXp4lo{%`^pKeiiyzwS>2Qy$
SuB)p_0c+Pe-<Sc;vi}G7dLKam
literal 0
HcmV?d00001
diff --git a/bauzil_stairs.urdf b/bauzil_stairs.urdf
new file mode 100644
index 00000000..022d7ba9
--- /dev/null
+++ b/bauzil_stairs.urdf
@@ -0,0 +1,24 @@
+<robot name="bauzil_stairs.urdf">
+ <link concave="yes" name="baseLink">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <mass value="0.0"/>
+ <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 3.1415" xyz="-0.9 1.8 -0.01"/>
+ <geometry>
+ <mesh filename="bauzil_stairs.stl" scale="1 1 0.3"/>
+ </geometry>
+ <material name="red">
+ <color rgba="1 0 0 1"/>
+ </material>
+ </visual>
+ <collision concave="yes">
+ <origin rpy="0 0 3.1415" xyz="-0.9 1.8 -0.01"/>
+ <geometry>
+ <mesh filename="bauzil_stairs.stl" scale="1 1 0.3"/>
+ </geometry>
+ </collision>
+ </link>
+</robot>
\ No newline at end of file
diff --git a/gamepadClient.py b/gamepadClient.py
new file mode 100644
index 00000000..c2e3f65f
--- /dev/null
+++ b/gamepadClient.py
@@ -0,0 +1,72 @@
+'''
+Simple python script to get Asyncronous gamepad inputs
+Thomas FLAYOLS - LAAS CNRS
+From https://github.com/thomasfla/solopython
+
+Use:
+To display data, run "python gamepadClient.py"
+'''
+import inputs
+import time
+from multiprocessing import Process
+from multiprocessing.sharedctypes import Value
+from ctypes import c_double, c_bool
+
+
+class GamepadClient():
+ def __init__(self):
+ self.running = Value(c_bool, lock=True)
+ self.startButton = Value(c_bool, lock=True)
+ self.leftJoystickX = Value(c_double, lock=True)
+ self.leftJoystickY = Value(c_double, lock=True)
+ self.rightJoystickX = Value(c_double, lock=True)
+ self.rightJoystickY = Value(c_double, lock=True)
+
+ self.startButton.value = False
+ self.leftJoystickX.value = 0.0
+ self.leftJoystickY.value = 0.0
+ self.rightJoystickX.value = 0.0
+ self.rightJoystickY.value = 0.0
+
+ args = (self.running, self.startButton, self.leftJoystickX,
+ self.leftJoystickY, self.rightJoystickX, self.rightJoystickY)
+ self.process = Process(target=self.run, args=args)
+ self.process.start()
+ time.sleep(0.2)
+
+ def run(self, running, startButton, leftJoystickX, leftJoystickY, rightJoystickX, rightJoystickY):
+ running.value = True
+ while(running.value):
+ events = inputs.get_gamepad()
+ for event in events:
+ #print(event.ev_type, event.code, event.state)
+ if event.ev_type == 'Absolute':
+ if event.code == 'ABS_X':
+ leftJoystickX.value = event.state / 32768.0
+ if event.code == 'ABS_Y':
+ leftJoystickY.value = event.state / 32768.0
+ if event.code == 'ABS_RX':
+ rightJoystickX.value = event.state / 32768.0
+ if event.code == 'ABS_RY':
+ rightJoystickY.value = event.state / 32768.0
+ if (event.ev_type == 'Key'):
+ if event.code == 'BTN_START':
+ startButton.value = event.state
+ print (event.state)
+ def stop(self):
+ self.running.value = False
+ self.process.terminate()
+ self.process.join()
+
+
+if __name__ == "__main__":
+ gp = GamepadClient()
+ for i in range(1000):
+ print("LX = ", gp.leftJoystickX.value, end=" ; ")
+ print("LY = ", gp.leftJoystickY.value, end=" ; ")
+ print("RX = ", gp.rightJoystickX.value, end=" ; ")
+ print("RY = ", gp.rightJoystickY.value, end=" ; ")
+ print("start = ",gp.startButton.value)
+ time.sleep(0.1)
+
+ gp.stop()
diff --git a/main.py b/main.py
index d3a7b332..b6c72d41 100644
--- a/main.py
+++ b/main.py
@@ -28,8 +28,8 @@ N_SIMULATION = 1000 # number of time steps simulated
# Initialize the error for the simulation time
time_error = False
-t_list_mpc = []
-t_list_tsid = []
+t_list_mpc = [0] * int(N_SIMULATION)
+t_list_tsid = [0] * int(N_SIMULATION)
# Enable/Disable Gepetto viewer
enable_gepetto_viewer = False
@@ -66,7 +66,7 @@ for k in range(int(N_SIMULATION)):
time_start = time.time()
- if (k % 100) == 0:
+ if (k % 1000) == 0:
print("Iteration: ", k)
####################################################################
@@ -101,6 +101,9 @@ for k in range(int(N_SIMULATION)):
pyb.resetBaseVelocity(pyb_sim.sphereId2, linearVelocity=[-3.0, 0.0, 2.0])
flag_sphere2 = False
+ pyb.resetDebugVisualizerCamera(cameraDistance=0.75, cameraYaw=50, cameraPitch=-35,
+ cameraTargetPosition=[qmes12[0, 0],qmes12[1, 0] + 0.0, 0.0])
+
########################
# Update MPC interface #
########################
@@ -211,7 +214,7 @@ for k in range(int(N_SIMULATION)):
time_spent = time.time() - time_mpc
# Logging the time spent
- t_list_mpc.append(time_spent)
+ t_list_mpc[k] = time_spent
# Visualisation with gepetto viewer
if enable_gepetto_viewer:
@@ -315,7 +318,7 @@ for k in range(int(N_SIMULATION)):
time_spent = time.time() - time_start
# Logging the time spent
- t_list_tsid.append(time_spent)
+ t_list_tsid[k] = time_spent
############
# PYBULLET #
diff --git a/utils.py b/utils.py
index 018b8665..cdbe39dc 100644
--- a/utils.py
+++ b/utils.py
@@ -210,6 +210,9 @@ class pybullet_simulator:
# Load horizontal plane
pyb.setAdditionalSearchPath(pybullet_data.getDataPath())
self.planeId = pyb.loadURDF("plane.urdf")
+ """self.stairsId = pyb.loadURDF("../../../../../Documents/Git-Repositories/mpc-tsid/bauzil_stairs.urdf")#,
+ #basePosition=[-1.25, 3.5, -0.1],
+ #baseOrientation=pyb.getQuaternionFromEuler([0.0, 0.0, 3.1415]))"""
mesh_scale = [1.0, 0.1, 0.02]
visualShapeId = pyb.createVisualShape(shapeType=pyb.GEOM_MESH,
--
GitLab