.

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/example_display.py

@@ -0,0 +1,30 @@
+# Example of creation of an object of class Display (implemented in the previous example, inside
+#  a file 'display.py'). Do not forget to start Gepetto-viewer server in another terminal before 
+#  creating the client.
+
+import pinocchio as se3
+from pinocchio.utils import *
+
+from display import Display
+display = Display()
+
+
+# Example of use of the class Display to create a box visual object.
+boxid   = 147
+name    = 'box' + str(boxid)
+[w,h,d] = [1.0,1.0,1.0]
+color   = [red,green,blue,transparency] = [1,1,0.78,1.0]
+display.viewer.gui.addBox('world/'+name, w,h,d,color)
+
+# Example of use of the class Display to create a sphere visual object.
+display.viewer.gui.addSphere('world/sphere', 1.0,color)
+
+# Example of use of the class Display to create a cylinder visual object.
+radius = 1.0
+height = 1.0
+display.viewer.gui.addCylinder('world/cylinder', radius,height,color)
+
+# Example of use of the class display to place the previously-create object at random SE3 placements.
+display.place("world/box147",se3.SE3.Random(),False)
+display.place("world/sphere",se3.SE3.Random(),False)
+display.place("world/cylinder",se3.SE3.Random())

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 robot_arm import Robot
+import time
+
+# Create a 7DOF robot.
+robot = Robot()
+
+# 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)

Code/example_scipy.py

@@ -0,0 +1,39 @@
+import numpy as np
+from scipy.optimize import fmin_bfgs, fmin_slsqp
+
+def cost(x):
+     '''Cost f(x,y) = x^2 + 2y^2 - 2xy - 2x '''
+     x0 = x[0]
+     x1 = x[1]
+     return -1*(2*x0*x1 + 2*x0 - x0**2 - 2*x1**2)
+
+def constraint_eq(x):
+     ''' Constraint x^3 = y '''
+     return np.array([ x[0]**3-x[1] ])
+
+def constraint_ineq(x):
+     '''Constraint x>=2, y>=2'''
+     return np.array([ x[0]-2,x[1]-2 ])
+
+class CallbackLogger:
+     def __init__(self):
+          self.nfeval = 1
+     def __call__(self,x):
+          print('===CBK=== {0:4d}   {1: 3.6f}   {2: 3.6f}    {3: 3.6f}'.format(self.nfeval, x[0], x[1], cost(x)))
+          self.nfeval += 1
+
+x0 = np.array([0.0,0.0])
+# Optimize cost without any constraints in BFGS, with traces.
+xopt_bfgs = fmin_bfgs(cost, x0, callback=CallbackLogger())
+print('\n *** Xopt in BFGS = ',xopt_bfgs,'\n\n\n\n')
+
+# Optimize cost without any constraints in CLSQ
+xopt_lsq = fmin_slsqp(cost,[-1.0,1.0], iprint=2, full_output=1)
+print('\n *** Xopt in LSQ = ',xopt_lsq,'\n\n\n\n')
+
+# Optimize cost with equality and inequality constraints in CLSQ
+xopt_clsq = fmin_slsqp(cost,[-1.0,1.0],
+                       f_eqcons=constraint_eq, f_ieqcons=constraint_ineq,
+                       iprint=2, full_output=1)
+print('\n *** Xopt in c-lsq = ',xopt_clsq,'\n\n\n\n')
+

Code/inverse_kinematics.py

@@ -0,0 +1,37 @@
+import numpy as np
+import numpy.linalg
+from scipy.optimize import fmin_bfgs
+from pinocchio import forwardKinematics, log, neutral
+import eigenpy
+eigenpy.switchToNumpyMatrix()
+
+class CallbackLogger:
+     def __init__(self, ik):
+          self.nfeval = 1
+          self.ik = ik
+     def __call__(self,x):
+          print '===CBK=== {0:4d}   {1}'.format(self.nfeval,
+                                                self.ik.latestCost)
+          self.nfeval += 1
+
+class InverseKinematics (object):
+    leftFootJoint = 'left_leg_6_joint'
+    rightFootJoint = 'right_leg_6_joint'
+    waistJoint = 'waist_joint'
+
+    def __init__ (self, robot):
+        self.q = neutral (robot.model)
+        forwardKinematics (robot.model, robot.data, self.q)
+        self.robot = robot
+        # Initialize references of feet and center of mass with initial values
+        self.leftFootRefPose = robot.data.oMi [robot.leftFootJointId].copy ()
+        self.rightFootRefPose = robot.data.oMi [robot.rightFootJointId].copy ()
+        self.waistRefPose = robot.data.oMi [robot.waistJointId].copy ()
+
+    def cost (self, q):
+        # Write your code here
+        pass
+
+    def solve (self, q):
+        # Write your code here
+        pass

Code/legged_robot.py

@@ -0,0 +1,53 @@
+from pinocchio.utils import *
+from pinocchio.explog import exp,log
+from numpy.linalg import pinv,norm
+from pinocchio import SE3, Model, Inertia, JointModelFreeFlyer, JointModelRX, \
+    JointModelRY, JointModelRZ, forwardKinematics, neutral
+import gepetto.corbaserver
+from display import Display
+
+class Visual:
+    '''
+    Class representing one 3D mesh of the robot, to be attached to a joint. The class contains:
+    * the name of the 3D objects inside Gepetto viewer.
+    * the ID of the joint in the kinematic tree to which the body is attached.
+    * the placement of the body with respect to the joint frame.
+    This class is only used in the list Robot.visuals (see below).
+    '''
+    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 Robot:
+    '''
+    Define a class Robot representing a biped robot
+    The members of the class are:
+    * viewer: a display encapsulating a gepetto viewer client to create 3D
+      objects and place them.
+    * model: the kinematic tree of the robot.
+    * data: the temporary variables to be used by the kinematic algorithms.
+    * visuals: the list of all the 'visual' 3D objects to render the robot,
+      each element of the list being an object Visual (see above).
+    '''
+
+    def __init__(self):
+        self.viewer = Display()
+        self.visuals = []
+        self.model = Model ()
+        self.createLeggedRobot ()
+        self.data = self.model.createData()
+        self.q0 = neutral (self.model)
+
+    def createLeggedRobot (self,rootId=0, prefix=''):
+        # Write your code here
+        pass
+
+    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()

Code/robot_arm.py

@@ -0,0 +1,90 @@
+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:
+    '''
+    Class representing one 3D mesh of the robot, to be attached to a joint. The class contains:
+    * the name of the 3D objects inside Gepetto viewer.
+    * the ID of the joint in the kinematic tree to which the body is attached.
+    * the placement of the body with respect to the joint frame.
+    This class is only used in the list Robot.visuals (see below).
+    '''
+    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 Robot:
+    '''
+    Define a class Robot with 7DOF (shoulder=3 + elbow=1 + wrist=3). 
+    The configuration is nq=7. The velocity is the same. 
+    The members of the class are:
+    * viewer: a display encapsulating a gepetto viewer client to create 3D objects and place them.
+    * model: the kinematic tree of the robot.
+    * data: the temporary variables to be used by the kinematic algorithms.
+    * visuals: the list of all the 'visual' 3D objects to render the robot, each element of the list being
+    an object Visual (see above).
+    
+    See tp1.py for an example of use.
+    '''
+
+    def __init__(self):
+        self.viewer = Display()
+        self.visuals = []
+        self.model = Model ()
+        self.createArm3DOF()
+        self.data = self.model.createData()
+        self.q0 = zero(self.model.nq)
+
+    def createArm3DOF(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 + "shoulder_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 + 'shoulder', 0.3,colorred)
+        self.visuals.append( Visual('world/' + prefix + 'shoulder',jointId,SE3.Identity()) )
+        self.viewer.viewer.gui.addBox('world/' + prefix + 'upperarm', .1,.1,.5,color)
+        self.visuals.append( Visual('world/' + prefix + 'upperarm',jointId,SE3(eye(3),np.array([0., 0., .5]))))
+
+        jointName          = prefix + "elbow_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 + 'elbow', 0.3,colorred)
+        self.visuals.append( Visual('world/' + prefix + 'elbow',jointId,SE3.Identity()) )
+        self.viewer.viewer.gui.addBox('world/' + prefix + 'lowerarm', .1,.1,.5,color)
+        self.visuals.append( Visual('world/' + prefix + 'lowerarm',jointId,SE3(eye(3),np.array([0., 0., .5]))))
+
+        jointName          = prefix + "wrist_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 + 'wrist', 0.3,colorred)
+        self.visuals.append( Visual('world/' + prefix + 'wrist',jointId,SE3.Identity()) )
+        self.viewer.viewer.gui.addBox('world/' + prefix + 'hand', .1,.1,.25,color)
+        self.visuals.append( Visual('world/' + prefix + 'hand',jointId,SE3(eye(3),np.array([0., 0., .25]))))
+
+
+    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()
+
+