tkpham/Projet4MA
1
0
Fork 0
Code Issues Pull requests Releases Wiki Activity

test leggedrobot

Browse source
This commit is contained in:
FeedAFish 2022-02-08 00:24:50 +01:00
parent 3b889f0b42
commit 35b96e504e
3 changed files with 112 additions and 19 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

BIN
Code/ImageExplain/leg.png Normal file
View file

Binary file not shown.
Side by side

After

Width:  |  Height:  |  Size: 1.8 KiB

92
Code/leggedrobot.py Normal file
View file

@ -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()

39
Code/robotleg.py
View file

@ -15,8 +15,9 @@ class Visual:
def place(self,display,oMjoint): def place(self,display,oMjoint):
oMbody = oMjoint*self.placement oMbody = oMjoint*self.placement
display.place(self.name,oMbody,False) display.place(self.name,oMbody,False)
class Leg: class Leg():
def __init__(self): def __init__(self,name=''):
self.name = name
self.viewer = Display() self.viewer = Display()
self.visuals = [] self.visuals = []
self.model = Model () self.model = Model ()
@ -24,7 +25,7 @@ class Leg:
self.data = self.model.createData() self.data = self.model.createData()
self.q0 = zero(self.model.nq) 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] color = [red,green,blue,transparency] = [1,1,0.78,1.0]
colorred = [1.0,0.0,0.0,1.0] colorred = [1.0,0.0,0.0,1.0]
@ -32,37 +33,37 @@ class Leg:
jointId = rootId jointId = rootId
jointName = prefix + "hip_joint" jointName = self.name+"hip_joint"
joint = JointModelRX() joint = JointModelRX()
jointId = self.model.addJoint(jointId,joint,jointPlacement,jointName) jointId = self.model.addJoint(jointId,joint,jointPlacement,jointName)
self.model.appendBodyToJoint(jointId,Inertia.Random(),SE3.Identity()) self.model.appendBodyToJoint(jointId,Inertia.Random(),SE3.Identity())
self.viewer.viewer.gui.addSphere('world/' + prefix + 'hip', 0.3,colorred) self.viewer.viewer.gui.addSphere('world/' +self.name+ 'hip', 0.3,colorred)
self.visuals.append( Visual('world/' + prefix + 'hip',jointId,SE3.Identity()) ) self.visuals.append( Visual('world/' + self.name+'hip',jointId,SE3.Identity()) )
self.viewer.viewer.gui.addBox('world/' + prefix + 'thigh', .1,.1,.5,color) self.viewer.viewer.gui.addBox('world/' +self.name+ 'thigh', .1,.1,.5,color)
self.visuals.append( Visual('world/' + prefix + 'thigh',jointId,SE3(eye(3),np.array([0., 0., -.5])))) 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] )) jointPlacement = SE3(eye(3),np.array( [0, 0, -1.0] ))
joint = JointModelRX() joint = JointModelRX()
jointId = self.model.addJoint(jointId,joint,jointPlacement,jointName) jointId = self.model.addJoint(jointId,joint,jointPlacement,jointName)
self.model.appendBodyToJoint(jointId,Inertia.Random(),SE3.Identity()) self.model.appendBodyToJoint(jointId,Inertia.Random(),SE3.Identity())
self.viewer.viewer.gui.addSphere('world/' + prefix + 'knee', 0.3,colorred) self.viewer.viewer.gui.addSphere('world/' +self.name+ 'knee', 0.3,colorred)
self.visuals.append( Visual('world/' + prefix + 'knee',jointId,SE3.Identity()) ) self.visuals.append( Visual('world/' + self.name+'knee',jointId,SE3.Identity()) )
self.viewer.viewer.gui.addBox('world/' + prefix + 'tibia', .1,.1,.5,color) self.viewer.viewer.gui.addBox('world/' +self.name+ 'tibia', .1,.1,.5,color)
self.visuals.append( Visual('world/' + prefix + 'tibia',jointId,SE3(eye(3),np.array([0., 0., -.5])))) 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] )) jointPlacement = SE3(eye(3),np.array( [0, 0, -1.0] ))
joint = JointModelRX() joint = JointModelRX()
jointId = self.model.addJoint(jointId,joint,jointPlacement,jointName) jointId = self.model.addJoint(jointId,joint,jointPlacement,jointName)
self.model.appendBodyToJoint(jointId,Inertia.Random(),SE3.Identity()) self.model.appendBodyToJoint(jointId,Inertia.Random(),SE3.Identity())
self.viewer.viewer.gui.addSphere('world/' + prefix + 'ankle', 0.3,colorred) self.viewer.viewer.gui.addSphere('world/' + self.name+'ankle', 0.3,colorred)
self.visuals.append( Visual('world/' + prefix + 'ankle',jointId,SE3.Identity()) ) self.visuals.append( Visual('world/' + self.name+'ankle',jointId,SE3.Identity()) )
self.viewer.viewer.gui.addBox('world/' + prefix + 'feet', .25,.5,.1,color) self.viewer.viewer.gui.addBox('world/' +self.name+ 'feet', .25,.5,.1,color)
self.visuals.append( Visual('world/' + prefix + 'feet',jointId,SE3(eye(3),np.array([0., 0., -.25])))) self.visuals.append( Visual('world/' +self.name+ 'feet',jointId,SE3(eye(3),np.array([0., .1, -.25]))))
def display(self,q): def display(self,q):
forwardKinematics(self.model,self.data,q) forwardKinematics(self.model,self.data,q)
for visual in self.visuals: for visual in self.visuals:
visual.place( self.viewer,self.data.oMi[visual.jointParent] ) visual.place( self.viewer,self.data.oMi[visual.jointParent] )
self.viewer.viewer.gui.refresh() self.viewer.viewer.gui.refresh()