From 631a36429f7165c3e4c1f90d128d4b08d772c462 Mon Sep 17 00:00:00 2001 From: FeedAFish Date: Mon, 7 Feb 2022 16:06:40 +0100 Subject: [PATCH] test leg ver 1 --- Code/robotleg.py | 68 ++++++++++++++++++++++++++ {Code => CodeEx}/INSTALL | 0 {Code => CodeEx}/config.sh | 0 {Code => CodeEx}/display.py | 0 {Code => CodeEx}/example_display.py | 0 {Code => CodeEx}/example_robot.py | 0 {Code => CodeEx}/example_scipy.py | 0 {Code => CodeEx}/inverse_kinematics.py | 0 {Code => CodeEx}/legged_robot.py | 0 {Code => CodeEx}/robot_arm.py | 4 +- 10 files changed, 70 insertions(+), 2 deletions(-) create mode 100644 Code/robotleg.py rename {Code => CodeEx}/INSTALL (100%) rename {Code => CodeEx}/config.sh (100%) rename {Code => CodeEx}/display.py (100%) rename {Code => CodeEx}/example_display.py (100%) rename {Code => CodeEx}/example_robot.py (100%) rename {Code => CodeEx}/example_scipy.py (100%) rename {Code => CodeEx}/inverse_kinematics.py (100%) rename {Code => CodeEx}/legged_robot.py (100%) rename {Code => CodeEx}/robot_arm.py (97%) diff --git a/Code/robotleg.py b/Code/robotleg.py new file mode 100644 index 0000000..fa74f8f --- /dev/null +++ b/Code/robotleg.py @@ -0,0 +1,68 @@ +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 Leg: + def __init__(self): + self.viewer = Display() + self.visuals = [] + self.model = Model () + self.createArm() + self.data = self.model.createData() + self.q0 = zero(self.model.nq) + + def CreateLeg(self,rootId=0, prefix='', jointPlacement=SE3.Identity()): + color = [red,green,blue,transparency] = [1,1,0.78,1.0] + colorred = [1.0,0.0,0.0,1.0] + + jointId = rootId + + jointName = prefix + "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,1,color) + self.visuals.append( Visual('world/' + prefix + 'thigh',jointId,SE3(eye(3),np.array([0., 0., -1])))) + + jointId = rootId + + jointName = prefix + "knee_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 + 'knee', 0.3,colorred) + self.visuals.append( Visual('world/' + prefix + 'knee',jointId,SE3.Identity()) ) + self.viewer.viewer.gui.addBox('world/' + prefix + 'tibia', .1,.1,1,color) + self.visuals.append( Visual('world/' + prefix + 'tibia',jointId,SE3(eye(3),np.array([0., 0., -1])))) + + jointId = rootId + + jointName = prefix + "ankle_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 + 'ankle', 0.3,colorred) + self.visuals.append( Visual('world/' + prefix + 'ankle',jointId,SE3.Identity()) ) + self.viewer.viewer.gui.addBox('world/' + prefix + 'feet', .5,1,.2,color) + self.visuals.append( Visual('world/' + prefix + 'feet',jointId,SE3(eye(3),np.array([0., 0., -.2])))) + + 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/INSTALL b/CodeEx/INSTALL similarity index 100% rename from Code/INSTALL rename to CodeEx/INSTALL diff --git a/Code/config.sh b/CodeEx/config.sh similarity index 100% rename from Code/config.sh rename to CodeEx/config.sh diff --git a/Code/display.py b/CodeEx/display.py similarity index 100% rename from Code/display.py rename to CodeEx/display.py diff --git a/Code/example_display.py b/CodeEx/example_display.py similarity index 100% rename from Code/example_display.py rename to CodeEx/example_display.py diff --git a/Code/example_robot.py b/CodeEx/example_robot.py similarity index 100% rename from Code/example_robot.py rename to CodeEx/example_robot.py diff --git a/Code/example_scipy.py b/CodeEx/example_scipy.py similarity index 100% rename from Code/example_scipy.py rename to CodeEx/example_scipy.py diff --git a/Code/inverse_kinematics.py b/CodeEx/inverse_kinematics.py similarity index 100% rename from Code/inverse_kinematics.py rename to CodeEx/inverse_kinematics.py diff --git a/Code/legged_robot.py b/CodeEx/legged_robot.py similarity index 100% rename from Code/legged_robot.py rename to CodeEx/legged_robot.py diff --git a/Code/robot_arm.py b/CodeEx/robot_arm.py similarity index 97% rename from Code/robot_arm.py rename to CodeEx/robot_arm.py index e45245b..664fc06 100644 --- a/Code/robot_arm.py +++ b/CodeEx/robot_arm.py @@ -41,11 +41,11 @@ class Robot: self.viewer = Display() self.visuals = [] self.model = Model () - self.createArm3DOF() + self.createArm() self.data = self.model.createData() self.q0 = zero(self.model.nq) - def createArm3DOF(self,rootId=0, prefix='', jointPlacement=SE3.Identity()): + def createArm(self,rootId=0, prefix='', jointPlacement=SE3.Identity()): color = [red,green,blue,transparency] = [1,1,0.78,1.0] colorred = [1.0,0.0,0.0,1.0]