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:18:30 +01:00
parent 6e0030f172
commit ac02f1130b
4 changed files with 100 additions and 10 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

23
Code/INSTALL Normal file
View file

@ -0,0 +1,23 @@
INSTALLATION INSTRUCTIONS
Robotpkg
sudo apt-get install cmake cmake-curses-gui cmake-qt-gui curl doxygen g++ ipython libassimp-dev assimp-utils libboost-dev omniidl-python omniorb omniorb-idl omniorb-nameserver python python-matplotlib python-numpy python-scipy python2.7 qgit libbz2-dev zlib1g-dev libssl-dev pax tar libeigen3-dev libtinyxml-dev liburdfdom-dev libboost-all-dev libpcre3-dev libopenscenegraph-dev libqt4-dev python-qt4-dev libomniorb4-dev libqt4-opengl-dev python-omniorb python-scipy
git clone https://git.openrobots.org/robots/robotpkg.git
cd robotpkg
git clone git://git.openrobots.org/robots/robotpkg/robotpkg-wip wip
export INSTALL_PREFIX=/usr/local/insa/openrobots
./bootstrap/bootstrap --prefix=$INSTALL_PREFIX
cd math/pinocchio; make install
cd graphics/collada-dom; make install
cd wip/osg-dae; make install
cd graphics/gepetto-viewer-corba; make install
Anaconda
- dependencies:
- pinocchio,
- gepetto-viewer-corba,
- numpy, scipy,
- matplotlib

6
Code/config.sh Normal file
View file

@ -0,0 +1,6 @@
export PYTHONPATH=/usr/local/insa/openrobots/lib/python2.7/site-packages
export LD_LIBRARY_PATH=/usr/local/insa/openrobots/lib
export PATH=/usr/local/insa/openrobots/bin:/usr/local/insa/openrobots/sbin:/bin
alias python='/usr/bin/python'
alias ipython='/usr/bin/ipython'

61
Code/display.py Normal file
View file

@ -0,0 +1,61 @@
# Typical header of a Python script using Pinocchio
from pinocchio.utils import *
from pinocchio.explog import exp,log
from numpy.linalg import pinv,norm
from pinocchio import SE3ToXYZQUATtuple
import gepetto.corbaserver
# Example of a class Display that connect to Gepetto-viewer and implement a
# 'place' method to set the position/rotation of a 3D visual object in a scene.
class Display():
'''
Class Display: Example of a class implementing a client for the Gepetto-viewer server. The main
method of the class is 'place', that sets the position/rotation of a 3D visual object in a scene.
'''
def __init__(self,windowName = "pinocchio" ):
'''
This function connect with the Gepetto-viewer server and open a window with the given name.
If the window already exists, it is kept in the current state. Otherwise, the newly-created
window is set up with a scene named 'world'.
'''
# Create the client and connect it with the display server.
try:
self.viewer=gepetto.corbaserver.Client()
except:
print("Error while starting the viewer client. ")
print("Check whether Gepetto-viewer is properly started")
# Open a window for displaying your model.
try:
# If the window already exists, do not do anything.
windowID = self.viewer.gui.getWindowID (windowName)
print("Warning: window '"+windowName+"' already created.")
print("The previously created objects will not be destroyed and do not have to be created again.")
except:
# Otherwise, create the empty window.
windowID = self.viewer.gui.createWindow (windowName)
# Start a new "scene" in this window, named "world", with just a floor.
self.viewer.gui.createSceneWithFloor("world")
self.viewer.gui.addSceneToWindow("world",windowID)
# Finally, refresh the layout to obtain your first rendering.
self.viewer.gui.refresh()
def nofloor(self):
'''
This function will hide the floor.
'''
self.viewer.gui.setVisibility('world/floor',"OFF")
self.viewer.gui.refresh()
def place(self,objName,M,refresh=True):
'''
This function places (ie changes both translation and rotation) of the object
names "objName" in place given by the SE3 object "M". By default, immediately refresh
the layout. If multiple objects have to be placed at the same time, do the refresh
only at the end of the list.
'''
self.viewer.gui.applyConfiguration(objName, SE3ToXYZQUATtuple(M))
if refresh: self.viewer.gui.refresh()

20
Code/robotleg.py
View file

@ -30,36 +30,36 @@ class Leg:
jointId = rootId
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
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]))))
jointName = prefix + "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,1,color)
self.visuals.append( Visual('world/' + prefix + 'tibia',jointId,SE3(eye(3),np.array([0., 0., -1]))))
jointId = rootId
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]))))
jointName = prefix + "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', .5,1,.2,color)
self.visuals.append( Visual('world/' + prefix + 'feet',jointId,SE3(eye(3),np.array([0., 0., -.2]))))
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]))))
def display(self,q):
forwardKinematics(self.model,self.data,q)