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

test leg

Browse source
This commit is contained in:
FeedAFish 2022-02-07 16:21:49 +01:00
parent ac02f1130b
commit ae58d03509
2 changed files with 30 additions and 3 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

27
Code/example_robot.py Normal file
View file

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

6
Code/robotleg.py
View file

@ -39,7 +39,7 @@ class Leg:
self.viewer.viewer.gui.addSphere('world/' + prefix + 'hip', 0.3,colorred) self.viewer.viewer.gui.addSphere('world/' + prefix + 'hip', 0.3,colorred)
self.visuals.append( Visual('world/' + prefix + 'hip',jointId,SE3.Identity()) ) self.visuals.append( Visual('world/' + prefix + 'hip',jointId,SE3.Identity()) )
self.viewer.viewer.gui.addBox('world/' + prefix + 'thigh', .1,.1,.5,color) 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" jointName = prefix + "knee_joint"
jointPlacement = SE3(eye(3),np.array( [0, 0, 1.0] )) 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.viewer.viewer.gui.addSphere('world/' + prefix + 'knee', 0.3,colorred)
self.visuals.append( Visual('world/' + prefix + 'knee',jointId,SE3.Identity()) ) self.visuals.append( Visual('world/' + prefix + 'knee',jointId,SE3.Identity()) )
self.viewer.viewer.gui.addBox('world/' + prefix + 'tibia', .1,.1,.5,color) 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" jointName = prefix + "ankle_joint"
jointPlacement = SE3(eye(3),np.array( [0, 0, 1.0] )) 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.viewer.viewer.gui.addSphere('world/' + prefix + 'ankle', 0.3,colorred)
self.visuals.append( Visual('world/' + prefix + 'ankle',jointId,SE3.Identity()) ) self.visuals.append( Visual('world/' + prefix + 'ankle',jointId,SE3.Identity()) )
self.viewer.viewer.gui.addBox('world/' + prefix + 'feet', .25,.5,.1,color) 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): def display(self,q):
forwardKinematics(self.model,self.data,q) forwardKinematics(self.model,self.data,q)