diff --git a/Code/ImageExplain/leg.png b/Code/ImageExplain/leg.png new file mode 100644 index 0000000..49b00f1 Binary files /dev/null and b/Code/ImageExplain/leg.png differ 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()