From 6fad9ec825b0aef199937dbb68e878fc547965e6 Mon Sep 17 00:00:00 2001 From: Pham Tuan Kiet Date: Sun, 13 Feb 2022 16:00:08 +0100 Subject: [PATCH] ball add --- Code/__pycache__/ball.cpython-39.pyc | Bin 0 -> 2992 bytes Code/__pycache__/display.cpython-39.pyc | Bin 0 -> 2480 bytes Code/__pycache__/leggedrobot.cpython-39.pyc | Bin 0 -> 3789 bytes Code/__pycache__/robot_arm.cpython-39.pyc | Bin 0 -> 4001 bytes Code/__pycache__/robotleg.cpython-39.pyc | Bin 0 -> 3060 bytes Code/ball.py | 76 +++++++++++++ Code/example_ball.py | 14 +++ Code/example_robot.py | 25 ++--- Code/leggedrobot.py | 34 +++--- Code/robot.py | 113 ++++++++++++++++++++ 10 files changed, 227 insertions(+), 35 deletions(-) create mode 100644 Code/__pycache__/ball.cpython-39.pyc create mode 100644 Code/__pycache__/display.cpython-39.pyc create mode 100644 Code/__pycache__/leggedrobot.cpython-39.pyc create mode 100644 Code/__pycache__/robot_arm.cpython-39.pyc create mode 100644 Code/__pycache__/robotleg.cpython-39.pyc create mode 100644 Code/ball.py create mode 100644 Code/example_ball.py create mode 100644 Code/robot.py diff --git a/Code/__pycache__/ball.cpython-39.pyc b/Code/__pycache__/ball.cpython-39.pyc new file mode 100644 index 0000000000000000000000000000000000000000..10682003fdff4e2cc154c50f96d1c3d150e8195c GIT binary patch literal 2992 zcmZuzOOG5$5zfrYdUp4`+#bIc5U{&)t;7uA!*W<*uWc3?G%ah#LRM*4t(wksPxaQL zC#!mf9;#36I~RUHx8Y)m13v%>iC=)=mRAliH#opywZcSXP1|Fyvy@R0k&nuV_%fp6 zPA4GHes$eH_ziG>N8w_#K)3_F{2dTM1Wm~!e6y5hEMl1zSrla}<(VDXnG-oi=Bb-` zk(c?Ap9N8nwW3zmj@nr#>SSxtTDBgon|XG+5pBYHC*8^}MVHQX-RLst>~uTYHt#FZ z6}i2*x+IkRln7UNPl@p6EV>4gF9MJPkgf}RpMMl!uRBlcFAc+`XTa<20TQ@Gut}1K|$z@*NaVB+Zgw{NamCP&c!>I-` z12*KEmoj~#wqO@^2}u{obE0tDFF)8lEHb%UeK9_av)#KBCF9BN$5k3v>3)*!-UH|C z9>i(7GoEU%-%s+S>i0XavUGqD>cDfl-dM1+8Xq8@Rt-(Dp&L`a2@D!BB3&}GYPtZk zMy$33T^fWzWLfW%&@yM#wz0yTJml&!tkte)JRZ5lo-sIbioJtEOpQ4WxdIc_H6WYN zOXF4=(9`z$Db1$6&_;XEbN#*!`u(gBlN9-OzkdYbXhzi4)m|6&N5?7ikqzM5tB-ip z;|eujLj$_w4o~VgFbqRNkw7OYktDOxzO)HFaH8$2#N0*9l&nKN@i$DYxdSJRm7312#ph&(p0XEC?bj^;d9dWL#`)SB_vIS*?Q6^~UhP*tMxY{# z){@}$0DT*t&SDKwQG4gQ^~FZDDR91-Ik%P+6D;5)=!N_Y?N;AI^ZtADvtR%DKmWY_ z4-laJ>o#1f9bnF7n6?X&QAIX7@3Hz8C?F7K4%E%3Sk;Q(yLn>iI+9gXm zv)-cP!m7F8AK^M1#^J(`+$GG1%xk~YdKa`XxAJRi4p6F@;Mbsbf5Fx70oIVUyW~SM zLyp56zP|`+hM=oji?(oa9Hk>=K`Up@c#U-u3k-ZuCe=D($Xw6~uA$$-)T=j?wk zUc5NNR1tPm3tk2o>W9eP0s^=>HYq?`gQ7GUL0e^#LxE6G^VD^m!>a_Upk1a?jzhbt z7EDx{9%*{4+3^qFj7|uvK7?N4azZ)Oh%Uy6Zo`8)1hWX#jljglWT*kcf7LM0Vi7c< z`2?6IEZ{^|(|L1Wt^4HjcfeI_!O`_NHgak2 zno<8`Qx#>(^R z+6!Ci1`I0veNo>)g2B*ioHP{?ue3tvDa5!!SDGkX_|~gLjYS_*x%r?+yX~#Z-nv3< zcq3Ye-?d^eI82J2NtL9f-gu>ezdHD*Q|M@gmq54kNj9GDq)8s9Lxp!kVFciJXSGS= zHndFnNDx#Fq|A{`)-h@3(ol|NRTVpfLLJ1VRL5Ah&XtWVrlQ&Ala2YVOolQaPZhdP zZF03@atu6?hU=%ANnrN{Vd2^!&crNw(UH~!aN#AmTqM$!_G5@wrTj@ zq2W7kxJp|GPB;1+bUIzvxVO8W@!8wb_?Z|39hAH{F+m|@4&uuPPwoxG&##9+zIrs! zRhhB<6n9>H&iuRsT;>d2>I^;V4t=^xeHy%XhXHNT_Iqd8qH8pwE5K>f4tTL{!ac6a zESb&!>=1Z%vqUM<@E(#UZ<8EM5-}nPNv(_{BWH!?#n>`A&}1Ye(Gy0Vv65*mc1}2( zGD#GZCrtLp07QAh3u_~1dLk(5#2t-RMYh3>lx^PuZlYnQR#sjL#Wfd2UkaTdQ9E;Q zkDPE-B^eP%$1K%^7vPObS;2bNidlg;iZq>Yk$M-Rg~1Bp-Gx{E4#Rv?JIC$^@6=g1 zv47|kH(UpQI-u@5*M2?xzIF=lt~0;AaE|@s)`#}OJ#~NeV3ogcuRAoj?lh}g_ys)= zYCX;RuwLKHBHBKMRn2ULwin*1x3tl4JE!0U4G}B}0L!t*BJd0*T#->#q&6I>C<+X~ zl;LS8bA(v%leOwTur`3dlgByU28HlEknX_h9LWC@)S_LeKH?hp>~R^$yU zZQN}dD=85O3X;Tn_Iw!*VZus0;4~ZY`!L(LQWvDBaF=jz^&Sp^ex-?FP}c$?*?)MEXqw7 z%sPPgC$9Jr&$yn+&w|T zwa2^##PXO6SNjWSOxQLSzWk=)2lluw9M?;}`*hQ=y;hN18~`{;iZN5#^D@(mO>252nPJuqmYQ)An4qTLd)&0nM-F^X0OsbQ)cs=0sm0o|B3Wb z34+qB6;)o&dKoX0Y%CF@vW)}!ZsTG;YdTo(LIablC`RM|=bnkatx0pd=skN^Mx literal 0 HcmV?d00001 diff --git a/Code/__pycache__/leggedrobot.cpython-39.pyc b/Code/__pycache__/leggedrobot.cpython-39.pyc new file mode 100644 index 0000000000000000000000000000000000000000..ccafe0edae8624539ebf442ba0232c22efb2560b GIT binary patch literal 3789 zcmcInOK%*<5uTp+zPKbsy_LjyIMF%?X~)SSfnidQ*r9APf+z$@!@*{_-CPbev$O0O zN+z)j1fZ@#&dF(4@I^l%Cjq(T=5wD6`WH=R9|w~)wEPnQVF{L4cd54%mfGA-9quriJBgcm+)I7#o4K0= zX~@I0!YgT&SJN7=rFC9U8@!Ru@wv3gn}*Iy=J^8ld&weSMDHhO(j~q$!JXx24elI2 zFVBuHj4fupZ;3#J4=oWM+x#MC6;Z{kirFRMZCS0_Q=HUlG0om++1imG_qCH`U5wp+ z+`F&cUZ&Dks26rJ^&nE>gSaQtsE9kc4%T~86>+5Nx3aibY-BvqfY+$vmfepw84xlZpE$cDcY&noMN~NytkuC3}Ug z;8Y!%V%T!Eo6BTJErN?WLtqKuv89OZf8JWz%~H8i-0SZ~>B{v3C8L9tYef{(0J&>651q}#Ixj$mWsXfUw7`CiL0N7bGY#t6`I)miM-fhZNvgKWdd&HZd+I}?XS zsuG+Bp}GjLfR-EWvMM{OPflr=ZnivuS55X5t{BalPH%hgC|9x-jK6!=Lo0 z%tz)is!u->vd~qu@`6*)5!Q(8*8rzEg>*;W*xGS~C;W$u`{fY0`LgPQ=MesyGcp{)z_{SFdpi3|;DrQz4 zSfd(UNm!^ls*efRDCTIk*E}Y!^W=#wfAjP5<6o|}oX`LES9<=rI$8T{b+YpJRrMOk zx;y5v*VK10Yb|K6*p0ip=;G}-;^B^zMIIkX?b1m5@W4d&bbua)FBD>!rTzP{d?1w$ z?wk9QD>8xhU_qH^{{}p{khd*02suvynZh4(hn~wXq8tT5$%|!njM}kAks9_Xg`B-pZ&7 ztJ>!gZqfWfWrC}X>P6!j2K&vi(KXiLxhDS=Rm*HB8JYO6eIT!A zAEP=sln6L|?L|sOhpmQ*IowxSR;&w?*Yunbt$q#$Pbse%=Xh+FMa?xVJPy?Rv`&^%s|4O9utwlV1V~o( z76FsDPm(uf3S#~NP&u)-P$EnvBE-wZA?JOWI5iZ@-z1Q)knbiEDXIrbcVDN`Edr+< zHMpV7wha(W&)S%t)nHyYEJ9}+m_%j(BK{041O zMujsvqBPqviUXb=y5Ce4n$Nz6K|xw7D?D~w<|*f<^@l4%doVFR@b;cRu=j$C));2r zKZg|$Z#b$vf6%H5z|q`a^%EM=wKG-Ty7(5GN2z#;9Zj@5sWvxTHFX(SMOD4}76DTj8mIdOO^pveQzfa*aGv?v ziRUS*g2^m76)vbL7M5NQZM>erv@Y`|er2*wXE)B44~jU+_58C1{PEy_N0G7>6_&c* zJ4pM7%SqgelCGjsTagv;D==kJs*dC8Si)Zy!_54y98*hkdg_JoybTlMF~zeCaWxpcNJR*wHA$fy2>-{?Z>#f z$Ie(YC+EP8Gn~e`?VrGP{{kd#eL~^@If1l-11El8b&ov`2{|BYRMqwT>U;j`8^6`6 zD=3X`|K@+Uq$q!;!Fpp8v8iWv9@0f3~>;y7qRM4I+OO&^_}-!{8xm=Az+> zr!vLUnCj_FWBN&Nm3eqI^!z8;W>EDv) z6F=rNS|FWv4|tGgE_Q{CyPxp^&q3@ckEDh(_htsxx;#pc+a-y>fQ@E_`eeMvsM!Z-#_Cw2$&*%sZVG9M3E?ifZ9^vRL}g z`fNR*V4M|_$3&I#`~V%2PfTWQWD*1TO<#uuhS{&olcyxVMP6A zr*nuvs8;MwhowQMBd*Md&Rs-m6!NwfdW26>D~@W|jv;6+K)qCmn}?{`Ptd$8EV((g z&Xj$fSD=f$`C$+zXoA0BZ%T4 zqX4)#8S}Wy1^mAZ$}%3{kh54XBlsSetPJS#B=85>5bj3M;=8>|u?|^63P(g;0LRQF z@XXM`PnavMND&#&;usd>(Y?39z&uvXb~8Ws%gs5D2dVJI$UX8!=yxNY(Y3%c8OPnx z8zlXxmkJ<5oXu!efnAc50goH~D3S+nl&d!gIcEZZJYp3lkvn9N-F7*!6Xv-DLy}mR zLqpO=eo~QeOJ6Rb#P6m@yn+(BZI_^La2GM_;$3lZiofvVK_rnDLRF{(q(3jzKS!PW z805;SLWt9&R?BrcGRDf(q=yetYmkp(&CVSf9n?+uu9VxcBLgj zaZ;GlT1Bfg?jsRz!c~iUIoyVA3WF?E7$;n$g_ax^+ToqHDnVc_0%h<~Xx>E4=srbt z07(vhP4#3!#=2Xophby6Fv zllmC%=T&9WAO%gD5M7;WXl|StSf?3%T+at^goOK8m#>SZ`*XPe;mComj$VAk9X}S#l8?V$e z^;E0qX{;$_xV)!T?wRQSvuq3RlnpEIk!|t6%7)eV%C_`>m2LT*vSH;tvaM9It-jT6 zSp5&#*5>U-!PUO9(>7j{CyN_sdjGun)o;H2$6vSpf&t3kxA0U_G*n}3Yhxs*yoK3Q z(2;NFiFKTch;4YzY`(WC%OAHqXE+#epkmzn-EXPQ{4(mCnrLmy!E}W-~ve482!GUWNeuEVk*4`mR#?c7_rn+7g4F1jvpljegEUHorwh(J zvxj6f@r-UX;&C`IQMChWtR2`NDrcIi9DYEv_&L)WJscO$LtWUQC~YY|rXHC-jmN(UN72EevGjak&T|(~nk$Tc9g{`uWopw!J!>ag@ znvbaY9yKz{BYzR3VBu0j^ycokNcyg0fsV?Hko2`2XYEIVX4_Z1Mf|@@gP%%;Z zGH>Z(0)GSXCs2@j1to+vGGCTe$g>y#g Scc9iK_!_;kcx$zVrGEoCtM7OK literal 0 HcmV?d00001 diff --git a/Code/__pycache__/robotleg.cpython-39.pyc b/Code/__pycache__/robotleg.cpython-39.pyc new file mode 100644 index 0000000000000000000000000000000000000000..f23f2daf332b2e8c218720a146d637c28730390d GIT binary patch literal 3060 zcmcImy>A>v6rb7e-8` z58u{xuLr7>&t2d}}dHv_MPrK&Qy-v5}a8nOK3Pcq6tGCvXxs za1$@^l1flXszFuFnekLG4c}PtOi~MKBiU>)%V!63Lqf?XgxSn_M3}Ru1@pkU%mdB? zZh@Jr#IL-B-4LHj`hl-Woj>eIJbT(;ZCr<*9|zK-J<=dO4JhsDKs82I$rHNb>)=M1 za9P>Fjot|bPYdb7u9`f7p>Ied=kdCj0V|@0WERL1BIe=s$6~#mCA?ld=(NM6eyuBb z*sWhJ;;@KUqojTv{H}{^Eh}Q)TI}pdr_qSgsAx1Q@O5qhA=H9rcc%1WaXjBdNQ@i0 zV?tN%oP_~8vOyL|U+>WYxVEA7bVi2?Bye!r%1vKa`;?~gMD5-;#2kDpZB|A-u(M_5 zbYNx6YZ=>7E|p{f6a@wj-Og2LsYiFKqg^VaD{%DS4@R%ikY1yaWUL$GxY}rJ!jY6S z;@H^MgnbO2P>aCtK=9{Ki#XIV1WH~*1cu`Zs$B%Kj|h)xOn*cH69zX1=8&vw%w!fw ztPf~25(1Od`Qsuzah%pMt7_|rhF&4E3{Ij3-u z+wk@fTonYAmUaTpZ!#sP51z92nT%$CRKje+XE(MMRUZVwMKJcdUm=5Wlwr`WLs`BQr)>Nt0*Zu=9r{CZu%pK|F%n@F~ z{J-$B_CI?$`vxyz?np1^;QM)~Le4*&CCtC(<-+7y;xFhQTk(yTh;;EDkk;=P?tk;_ zw_lfD00ZsS5~RWRVKCxB?E-L06u9lE(@-gK*V@iR%<6#-qShtVep(95q8+u`FTVR0 z+mA~vv`EO$OD{1!LCRI8kc?-Z2Ht~|^U^3NsC^+BDZF|b7^nc2qqQguDq;EnQfQf$ zO~nMpI_Jem|M}8*75~7<^QZoNnHgLVig$oWCuHn4Rx~*qb~-#|*C4^)%alkssA1g= zQa&Vn@7KNu2)BYA4oRvb%}|JN z2XY_Qn7|TQR@`F2wDRQ6xSC3{nZ=oqy5LNjEx~yzjkUPTgK8ndH1A;fZSDk)QbQ$M z830SJ8mvIE2~0Jds3xkAoP%}ox{zN6Lg)f@Gy@(FlHnXZNzc;r@O%jT$s;7YQTP4h!Feosol@*xol_^fYLB2ZE*Ls)&p*}#@HmshuVV@#HjgrmdD22xv zxL8pR>V_w5u&B;YoW~J{T~*=HsbtGhc?GG%gtP@;7d&qRB2-%m@O(#{gqJvl8 z5g2v2myL~-p+bLz0zH=163X)}-r+@&EjBZ;7Uo=RVfh^i%dM-*Z1bqaQ@E4O<=auw sY~RaPaM7y>V})_G^S^UbI+x3<&PVts=jgBlL9Nj-2): + q+=v*dt + v+=v1 + self.display(q) + time.sleep(10*dt) + + def display(self,q): + forwardKinematics(self.model,self.data,q) + for visual in self.visuals: + visual.place( self.viewer,self.data.oMi[visual.jointParent] ) + self.viewer.viewer.gui.refresh() + + diff --git a/Code/example_ball.py b/Code/example_ball.py new file mode 100644 index 0000000..8779f49 --- /dev/null +++ b/Code/example_ball.py @@ -0,0 +1,14 @@ +from numpy.linalg import pinv,norm +from pinocchio import neutral +from pinocchio.explog import exp,log +from display import Display +import eigenpy +from ball import Ball +import eigenpy +import numpy as np +import time + +# Create a 7DOF robot. +robot = Ball() + + diff --git a/Code/example_robot.py b/Code/example_robot.py index 74781a3..4ff39df 100644 --- a/Code/example_robot.py +++ b/Code/example_robot.py @@ -1,27 +1,20 @@ # Create a 7DOF manipulator robot and moves it along a constant (random) velocity during 10secs. -import numpy as np from numpy.linalg import pinv,norm from pinocchio import neutral -from robotleg import Leg +from pinocchio.explog import exp,log +from display import Display +import eigenpy +from leggedrobot import Robot +import eigenpy import time # Create a 7DOF robot. -robot = Leg() +robot = Robot() # Hide the floor. robot.viewer.viewer.gui.setVisibility('world/floor','OFF') # Move the robot during 10secs at velocity v. -dt = 1e-3 -for j in range (robot.model.nv): - v = np.array (robot.model.nv * [0]) - v [j] = 1 - q = neutral (robot.model) - for i in range(1000): - q += v*dt - robot.display(q) - time.sleep(dt) - for i in range(1000): - q -= v*dt - robot.display(q) - time.sleep(dt) +dt = 2e-3 +q = neutral (robot.model) +robot.display(q) diff --git a/Code/leggedrobot.py b/Code/leggedrobot.py index 5fed7cd..bc31a82 100644 --- a/Code/leggedrobot.py +++ b/Code/leggedrobot.py @@ -1,19 +1,12 @@ from pinocchio.utils import * from pinocchio.explog import exp,log from numpy.linalg import pinv,norm -from pinocchio import forwardKinematics, Inertia, JointModelRX, Model, SE3 -import gepetto.corbaserver -from display import Display -import eigenpy -eigenpy.switchToNumpyArray() - -from pinocchio.utils import * -from pinocchio.explog import exp,log -from numpy.linalg import pinv,norm -from pinocchio import forwardKinematics, Inertia, JointModelRX, Model, SE3 +from pinocchio import forwardKinematics, Inertia, JointModelRX,JointModelPY,JointModelPZ, Model, SE3,neutral import gepetto.corbaserver +import time from display import Display import eigenpy +import numpy as np eigenpy.switchToNumpyArray() class Visual: @@ -30,7 +23,7 @@ class Robot(): self.tibia = 1. self.feetsize = (.2,.5,.1) self.feet = 0.2 - self.leglen=1.+1.+.25 + self.leglen=1.+1.+.3 self.legdist=0.5 self.viewer = Display() @@ -39,8 +32,10 @@ class Robot(): self.Create() self.data = self.model.createData() self.q0 = zero(self.model.nq) + #print(self.model) + #self.Walk(10) - def CreateLeg(self,rootId=0,name='',jointPlacement): + def CreateLeg(self,rootId=0,name='',jointPlacement=SE3.Identity()): color = [red,green,blue,transparency] = [1,1,0.78,1.0] colorred = [1.0,0.0,0.0,1.0] @@ -52,7 +47,7 @@ class Robot(): joint = JointModelRX() jointId = self.model.addJoint(jointId,joint,jointPlacement,jointName) self.model.appendBodyToJoint(jointId,Inertia.Random(),SE3.Identity()) - self.viewer.viewer.gui.addSphere('world/' +name+ 'hip', 0.3,colorred) + self.viewer.viewer.gui.addSphere('world/' +name+ 'hip', 0.1,colorred) self.visuals.append( Visual('world/' + name+'hip',jointId,SE3.Identity()) ) self.viewer.viewer.gui.addBox('world/' +name+ 'thigh', .1,.1,.5,color) self.visuals.append( Visual('world/' + name+'thigh',jointId,SE3(eye(3),np.array([0., 0., -.5])))) @@ -62,7 +57,7 @@ class Robot(): joint = JointModelRX() jointId = self.model.addJoint(jointId,joint,jointPlacement,jointName) self.model.appendBodyToJoint(jointId,Inertia.Random(),SE3.Identity()) - self.viewer.viewer.gui.addSphere('world/' +name+ 'knee', 0.3,colorred) + self.viewer.viewer.gui.addSphere('world/' +name+ 'knee', 0.1,colorred) self.visuals.append( Visual('world/' + name+'knee',jointId,SE3.Identity()) ) self.viewer.viewer.gui.addBox('world/' +name+ 'tibia', .1,.1,.5,color) self.visuals.append( Visual('world/' +name+ 'tibia',jointId,SE3(eye(3),np.array([0., 0., -.5])))) @@ -72,21 +67,22 @@ class Robot(): joint = JointModelRX() jointId = self.model.addJoint(jointId,joint,jointPlacement,jointName) self.model.appendBodyToJoint(jointId,Inertia.Random(),SE3.Identity()) - self.viewer.viewer.gui.addSphere('world/' + name+'ankle', 0.3,colorred) + self.viewer.viewer.gui.addSphere('world/' + name+'ankle', 0.1,colorred) self.visuals.append( Visual('world/' + name+'ankle',jointId,SE3.Identity()) ) self.viewer.viewer.gui.addBox('world/' +name+ 'feet', .25,.5,.1,color) self.visuals.append( Visual('world/' +name+ 'feet',jointId,SE3(eye(3),np.array([0., .1, -.25])))) def Crleft(self): - self.CreateLeg(name='left_',jointPlacement=pin.SE3(np.eye(3), np.array([0, self.legdist/2, self.leglen]))) + self.CreateLeg(name='left_',jointPlacement=SE3(np.eye(3), np.array([self.legdist/2,0, self.leglen]))) def Crright(self): - self.CreateLeg(name='right_',jointPlacement=pin.SE3(np.eye(3), np.array([0, -self.legdist/2, self.leglen]))) + self.CreateLeg(name='right_',jointPlacement=SE3(np.eye(3), np.array([-self.legdist/2,0, self.leglen]))) def Create(self): self.Crleft() self.Crright() - + + def display(self,q): forwardKinematics(self.model,self.data,q) for visual in self.visuals: visual.place( self.viewer,self.data.oMi[visual.jointParent] ) - self.viewer.viewer.gui.refresh() \ No newline at end of file + self.viewer.viewer.gui.refresh() diff --git a/Code/robot.py b/Code/robot.py new file mode 100644 index 0000000..7d325c0 --- /dev/null +++ b/Code/robot.py @@ -0,0 +1,113 @@ +from pinocchio.utils import * +from pinocchio.explog import exp,log +from numpy.linalg import pinv,norm +from pinocchio import forwardKinematics, Inertia, JointModelRX, Model, SE3 +import gepetto.corbaserver +from display import Display +import pinocchio as pin +import eigenpy +import hppfcl +from pinocchio.visualize import GepettoVisualizer, MeshcatVisualizer + +eigenpy.switchToNumpyArray() + +color = [red,green,blue,transparency] = [1,1,0.78,1.0] +red = [1.0,0.0,0.0,1.0] + +class Joint: + def __init__(self,name,parent=None,rootid=0, **kargs): + self.name=name + self.joint_models = kwargs.get("joint_models", [pin.JointModelRX()]) + self.placement = kwargs.get("placement", self.__calculate_placement(parent)) + self.ids = [] + self.geo_ids = [] + self.inertias = kwargs.get("inertias") + + #check if parent =0 + + if not self.inertias: + self.inertias = [None] * len(self.joint_models) + for i in range(len(self.joint_models)): + self.inertias[i] = pin.Inertia.Random() + for i, (joint_model, inertia) in enumerate( + zip(self.joint_models, self.inertias) + ): + current_parent = None + current_placement = None + if i == 0: + current_parent = parent.id if parent else root_id + current_placement = self.placement + else: + current_parent = self.ids[-1] + current_placement = pin.SE3.Identity() + self.ids.append( + model.addJoint( + current_parent, + joint_model, + current_placement, + self.name, + ) + model.appendBodyToJoint(self.ids[-1], inertia, pin.SE3.Identity()) + self.id = self.ids[-1] + self.sphere_radius = kwargs.get("sphere_radius", 0.1) + self.sphere = None + if self.sphere_radius: + self.sphere_name = kwargs.get("sphere_name", self.name) + self.sphere_color = kwargs.get("sphere_color", red) + self.sphere_placement = kwargs.get("sphere_placement", pin.SE3.Identity()) + self.sphere = self.__create_geo_obj( + self.sphere_name, + self.sphere_placement, + self.sphere_color, + hppfcl.Sphere, + self.sphere_radius, + ) + self.geo_ids.append(geo_model.addGeometryObject(self.sphere)) + self.box_x = kwargs.get("box_x", 0.1) + self.box_y = kwargs.get("box_y", 0.1) + self.box_z = kwargs.get("box_z", 0) + self.box = None + if self.box_x and self.box_y and self.box_z: + self.box_name = kwargs.get("box_name", f"box_{name}") + self.box_color = kwargs.get("box_color", WHITE) + # Placement of the geometry with respect to the joint frame + self.box_placement = kwargs.get( + "box_placement", pin.SE3(np.eye(3), np.array([0, 0, self.box_z / 2])) + ) + self.box = self.__create_geo_obj( + self.box_name, + self.box_placement, + self.box_color, + hppfcl.Box, + self.box_x, + self.box_y, + self.box_z, + ) + self.geo_ids.append(geo_model.addGeometryObject(self.box)) + def __create_geo_obj(self, name, placement, color, factory, *nargs, **kwargs): + return pin.GeometryObject( + name=name, + parent_joint=self.id, + collision_geometry=factory(*nargs, **kwargs), + placement=placement, + mesh_path="", + mesh_scale=np.ones(3), + override_material=False, + mesh_color=color, + ) + def __calculate_placement(self, parent=None): + if parent is None: + return pin.SE3.Identity() + else: + parent_z = 0 + if parent.box_z != 0: + parent_z = parent.box_z + return pin.SE3(np.eye(3), np.array([0, 0, parent_z])) + +class Robot: + def __init__(self, root_id=0, show_origin=True, add_plane=True): + self.viewer = Display() + self.visuals = [] + self.model = Model() + collision_model = pin.GeometryModel() + visual_model = pin.GeometryModel()