test leg..

Code/INSTALL

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

Code/config.sh

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

Code/display.py

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

Code/robotleg.py

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