From 35b96e504ef2147d05e2fa6354dd795f0e2af9ab Mon Sep 17 00:00:00 2001
From: FeedAFish <phamtuankietdn2000@gmail.com>
Date: Tue, 8 Feb 2022 00:24:50 +0100
Subject: [PATCH] test leggedrobot

---
 Code/ImageExplain/leg.png | Bin 0 -> 1829 bytes
 Code/leggedrobot.py       |  92 ++++++++++++++++++++++++++++++++++++++
 Code/robotleg.py          |  39 ++++++++--------
 3 files changed, 112 insertions(+), 19 deletions(-)
 create mode 100644 Code/ImageExplain/leg.png
 create mode 100644 Code/leggedrobot.py

diff --git a/Code/ImageExplain/leg.png b/Code/ImageExplain/leg.png
new file mode 100644
index 0000000000000000000000000000000000000000..49b00f13e31a046eaab948388078adbc1c31947e
GIT binary patch
literal 1829
zcmeAS@N?(olHy`uVBq!ia0vp^D;XFVtvT3$tYth4F9RvY;vjb?hIQv;UIIBR>5jgR
z3=A9lx&I`x0{NT;9+AZi419+{nDKc2iWCM0wq2esjv*CsZ|~Y?$USE~{?UCMTWL2N
z@B0T2+4%DhGI2_6XyegWZ044|q}Y7C?DLE~-7U?$ydDz{w((q>71*pOX;q<_;o;cW
z;&mb5p5o!X3c-?D8`>OJP3DXG@0sMO`2PJnJ9&${#p^3{4dUertM2WN{&9G|TurpX
zjj3h)8(q9?CJOm4IU=n2k$7fi`=3Ag*2`~S(7Choan?n>8Gihy=DuC~=}k(__wDbe
zuacTyo6O*HOSS)2WXY#@H|rGs?0P>*?OY@4Edf%j!K3zj6W5mg|5x1E^>y{P7pbLp
z?`(ThxjEAMh2liETmPT?-Td{StolpUw(YOWuGrVzZO=>#oTnc2sB=n%lIlE<PAKzj
z<1V{A_rEHa9A2*uwO*un$zbzw+qnCC6wpItcaWI>zFS4xx~s14zdHG>&n@P#ISJ=n
zq?WB)o?3Hv(~JMOLr?0-i-M|M@$s_r-^GX5{`j<3;kOHp&~}{V?Orj{<M5ibbMv>Z
zy!f2G*A#b@uw~AB_{1?;A7t~b!rJ|(i=8oz-80|!-oLN&ZyOeG+x`1P%B={U{{Mfs
z)cowvjI^2Q#K%8>Pf>Jaq+RX#%ggUiS^w|q`~6e(?)*I6oDbHqJ9yfx=b8H!*sZyJ
z@w1!u;-J`Q?bE0K#(9ZqeiSn^cl9z;_aiOc;EB+QZPIu5N2~4sA3M{J|5kXL`;v!h
zmop`|K3*u4xg+}h9i7|T@9T$}=EY0OO>Eo!^W5&#%ma;PxWflIP3I?mcJ|Lyn>`aX
z`jJusf@#Mpnvnng;>qjksjr)Fr|a)oU}ux(e%|`Wrx#DE-#Yfs^t4XcvgymITn)J0
zneA`i`rZ!Q-?`<mTlng-EBT-_#(XAi^QL`PJ&_-k;0jT_FSTIZ`TcDFW93`kDT5N6
z;<4+x-F4#VX%7^^R_WrAxmhxig^!kR+h>*MzQ+R^*Lzy4zu%c(GH;gf^11Eu)7|U$
zUf=hr>f87EQ<|I8-v;iV=?P2gKqo!VF_S)K>!uG%D47jgw|>^|JSWHh_`E+aHamgk
z!nw%U>bA@Lf|>4bYt81E)=w-83FS@y28@0*FP*#jCHuO?UjdLgj5dc~{sv{Vw+j?)
z!?!2`b6v?fvwM32r-`{Q-~9M>j`Nd$OO6;%Mvfrt<xl>@d+v6uu4cv<_Tkjq_-Z}h
zsdtCxe&d+P_Ur7mu%C~Y&!2a!%fmQ!^{G!M^Gb4S%4`bs<Q3-;lN6AP3Pb|g+rQ;s
z)vdl4tM0{L+Ov6c?6>_p?)ZwWT9xJgynN<&vsJH>uHWll7x(VD+RNG{-{0TYJ%0TB
zsm=dowy)<oo?@gaoqso{e^<7^M6<lT=9-URi_f0=?VFytaQ(mUf6GehrvJU!yY9Hv
z3=ebj{OMn>KD@gv?fYi4Px<#WzCOIUA$!i<?{|*PetdcD>2*4~(=I*l{a<<YzrQ#n
zi1r+Gu{tt&**=5GLhG+j^waJ9)-<z3p|9_jBReoY_V#Vr6*aZDSNg4B?A6~hzZ}z7
zU%v5DEH}U0m;W!7&vIT#`PBXGMa{J7({Eio+7qVt{h$14_4!xcuBc`c%#_zL=P%pU
nqBq|HXwtcNz(jRu>Zf?o+g@r1`S;BPR&5NPu6{1-oD!M<z(ie~

literal 0
HcmV?d00001

diff --git a/Code/leggedrobot.py b/Code/leggedrobot.py
new file mode 100644
index 0000000..5fed7cd
--- /dev/null
+++ b/Code/leggedrobot.py
@@ -0,0 +1,92 @@
+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
+import gepetto.corbaserver
+from display import Display
+import eigenpy
+eigenpy.switchToNumpyArray()
+
+class Visual:
+    def __init__(self,name,jointParent,placement):
+        self.name = name                  # Name in gepetto viewer
+        self.jointParent = jointParent    # ID (int) of the joint 
+        self.placement = placement        # placement of the body wrt joint, i.e. bodyMjoint
+    def place(self,display,oMjoint):
+        oMbody = oMjoint*self.placement
+        display.place(self.name,oMbody,False)
+class Robot():
+    def __init__(self):
+        self.thigh = 1.
+        self.tibia = 1.
+        self.feetsize = (.2,.5,.1)
+        self.feet = 0.2
+        self.leglen=1.+1.+.25
+        self.legdist=0.5
+
+        self.viewer = Display()
+        self.visuals = []
+        self.model = Model ()
+        self.Create()
+        self.data = self.model.createData()
+        self.q0 = zero(self.model.nq)
+
+    def CreateLeg(self,rootId=0,name='',jointPlacement):
+        color   = [red,green,blue,transparency] = [1,1,0.78,1.0]
+        colorred = [1.0,0.0,0.0,1.0]
+
+        jointId = rootId
+
+        jointId = rootId
+
+        jointName          = name+"hip_joint"
+        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.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]))))
+
+        jointName          = name+"knee_joint"
+        jointPlacement     = SE3(eye(3),np.array( [0, 0, -1.0] ))
+        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.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]))))
+
+        jointName          = name+"ankle_joint"
+        jointPlacement     = SE3(eye(3),np.array( [0, 0, -1.0] ))
+        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.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])))
+    def Crright(self):
+        self.CreateLeg(name='right_',jointPlacement=pin.SE3(np.eye(3), np.array([0, -self.legdist/2, 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
diff --git a/Code/robotleg.py b/Code/robotleg.py
index 45d7482..b5fd175 100644
--- a/Code/robotleg.py
+++ b/Code/robotleg.py
@@ -15,8 +15,9 @@ class Visual:
     def place(self,display,oMjoint):
         oMbody = oMjoint*self.placement
         display.place(self.name,oMbody,False)
-class Leg:
-    def __init__(self):
+class Leg():
+    def __init__(self,name=''):
+        self.name = name
         self.viewer = Display()
         self.visuals = []
         self.model = Model ()
@@ -24,7 +25,7 @@ class Leg:
         self.data = self.model.createData()
         self.q0 = zero(self.model.nq)
 
-    def CreateLeg(self,rootId=0, prefix='', jointPlacement=SE3.Identity()):
+    def CreateLeg(self,rootId=0,jointPlacement=SE3.Identity()):
         color   = [red,green,blue,transparency] = [1,1,0.78,1.0]
         colorred = [1.0,0.0,0.0,1.0]
 
@@ -32,37 +33,37 @@ class Leg:
 
         jointId = rootId
 
-        jointName          = prefix + "hip_joint"
+        jointName          = self.name+"hip_joint"
         joint              = JointModelRX()
         jointId = self.model.addJoint(jointId,joint,jointPlacement,jointName)
         self.model.appendBodyToJoint(jointId,Inertia.Random(),SE3.Identity())
-        self.viewer.viewer.gui.addSphere('world/' + prefix + 'hip', 0.3,colorred)
-        self.visuals.append( Visual('world/' + prefix + 'hip',jointId,SE3.Identity()) )
-        self.viewer.viewer.gui.addBox('world/' + prefix + 'thigh', .1,.1,.5,color)
-        self.visuals.append( Visual('world/' + prefix + 'thigh',jointId,SE3(eye(3),np.array([0., 0., -.5]))))
+        self.viewer.viewer.gui.addSphere('world/'  +self.name+ 'hip', 0.3,colorred)
+        self.visuals.append( Visual('world/'  + self.name+'hip',jointId,SE3.Identity()) )
+        self.viewer.viewer.gui.addBox('world/'  +self.name+ 'thigh', .1,.1,.5,color)
+        self.visuals.append( Visual('world/' + self.name+'thigh',jointId,SE3(eye(3),np.array([0., 0., -.5]))))
 
-        jointName          = prefix + "knee_joint"
+        jointName          = self.name+"knee_joint"
         jointPlacement     = SE3(eye(3),np.array( [0, 0, -1.0] ))
         joint              = JointModelRX()
         jointId = self.model.addJoint(jointId,joint,jointPlacement,jointName)
         self.model.appendBodyToJoint(jointId,Inertia.Random(),SE3.Identity())
-        self.viewer.viewer.gui.addSphere('world/' + prefix + 'knee', 0.3,colorred)
-        self.visuals.append( Visual('world/' + prefix + 'knee',jointId,SE3.Identity()) )
-        self.viewer.viewer.gui.addBox('world/' + prefix + 'tibia', .1,.1,.5,color)
-        self.visuals.append( Visual('world/' + prefix + 'tibia',jointId,SE3(eye(3),np.array([0., 0., -.5]))))
+        self.viewer.viewer.gui.addSphere('world/'  +self.name+ 'knee', 0.3,colorred)
+        self.visuals.append( Visual('world/'  + self.name+'knee',jointId,SE3.Identity()) )
+        self.viewer.viewer.gui.addBox('world/'  +self.name+ 'tibia', .1,.1,.5,color)
+        self.visuals.append( Visual('world/'  +self.name+ 'tibia',jointId,SE3(eye(3),np.array([0., 0., -.5]))))
 
-        jointName          = prefix + "ankle_joint"
+        jointName          = self.name+"ankle_joint"
         jointPlacement     = SE3(eye(3),np.array( [0, 0, -1.0] ))
         joint              = JointModelRX()
         jointId = self.model.addJoint(jointId,joint,jointPlacement,jointName)
         self.model.appendBodyToJoint(jointId,Inertia.Random(),SE3.Identity())
-        self.viewer.viewer.gui.addSphere('world/' + prefix + 'ankle', 0.3,colorred)
-        self.visuals.append( Visual('world/' + prefix + 'ankle',jointId,SE3.Identity()) )
-        self.viewer.viewer.gui.addBox('world/' + prefix + 'feet', .25,.5,.1,color)
-        self.visuals.append( Visual('world/' + prefix + 'feet',jointId,SE3(eye(3),np.array([0., 0., -.25]))))
+        self.viewer.viewer.gui.addSphere('world/'  + self.name+'ankle', 0.3,colorred)
+        self.visuals.append( Visual('world/' + self.name+'ankle',jointId,SE3.Identity()) )
+        self.viewer.viewer.gui.addBox('world/' +self.name+ 'feet', .25,.5,.1,color)
+        self.visuals.append( Visual('world/' +self.name+ 'feet',jointId,SE3(eye(3),np.array([0., .1, -.25]))))
 
     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()