diff --git a/Code/example_robot.py b/Code/example_robot.py new file mode 100644 index 0000000..74781a3 --- /dev/null +++ b/Code/example_robot.py @@ -0,0 +1,27 @@ +# 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 +import time + +# Create a 7DOF robot. +robot = Leg() + +# 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) diff --git a/Code/robotleg.py b/Code/robotleg.py index 372c1c8..c893b6c 100644 --- a/Code/robotleg.py +++ b/Code/robotleg.py @@ -39,7 +39,7 @@ class Leg: 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.visuals.append( Visual('world/' + prefix + 'thigh',jointId,SE3(eye(3),np.array([0., 0., -.5])))) jointName = prefix + "knee_joint" jointPlacement = SE3(eye(3),np.array( [0, 0, 1.0] )) @@ -49,7 +49,7 @@ class Leg: 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.visuals.append( Visual('world/' + prefix + 'tibia',jointId,SE3(eye(3),np.array([0., 0., -.5])))) jointName = prefix + "ankle_joint" jointPlacement = SE3(eye(3),np.array( [0, 0, 1.0] )) @@ -59,7 +59,7 @@ class Leg: 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.visuals.append( Visual('world/' + prefix + 'feet',jointId,SE3(eye(3),np.array([0., 0., -.25])))) def display(self,q): forwardKinematics(self.model,self.data,q)