BE INSA version 2017

display.py

+# Typical header of a Python script using Pinocchio
+from pinocchio.utils import *
+from pinocchio.explog import exp,log
+from numpy.linalg import pinv,norm
+import pinocchio as se3
+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,
+                                           XYZQUATToViewerConfiguration(se3ToXYZQUAT(M)) )
+        if refresh: self.viewer.gui.refresh()
+

example_display.py

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

example_factor.py

+from pinocchio.utils import *
+from factor import FactorGraph,Factor
+
+f = FactorGraph(1,5)   # Define a factor of 5 variables of dimension 1
+
+M = eye(1)             # M is simply 1 written as a 1x1 matrix.
+
+# Add the constraints that all the variables should be equal (ie x_i = x_i+1)
+for i in range(4):
+    f.addFactorConstraint( [ Factor( i,M ), Factor( i+1,-M ) ], zero(1) )
+
+# The cost is so that x_1 is as close as possible to 10, and x_N as close as possible to 20
+f.addFactor( [ Factor(0,M) ], M*10 )
+f.addFactor( [ Factor(4,M) ], M*20 )
+
+# Guess what is the solution?
+x = f.solve()
+

example_foot_steps.py

+from foot_steps import FootSteps
+from numpy import matrix
+
+footsteps = FootSteps( matrix([0.0,-0.1]).T , matrix([0.0,0.1]).T )
+footsteps.addPhase( .3, 'none' )
+footsteps.addPhase( .7, 'left' , matrix([0.1,+0.1]).T )
+footsteps.addPhase( .1, 'none' )
+footsteps.addPhase( .7, 'right', matrix([0.2,-0.1]).T )
+footsteps.addPhase( .1, 'none' )
+footsteps.addPhase( .7, 'left' , matrix([0.3,+0.1]).T )
+footsteps.addPhase( .1, 'none' )
+footsteps.addPhase( .7, 'right', matrix([0.4,-0.1]).T )
+footsteps.addPhase( .1, 'none' )
+footsteps.addPhase( .7, 'left' , matrix([0.5,+0.1]).T )
+footsteps.addPhase( .1, 'none' )
+footsteps.addPhase( .7, 'right', matrix([0.5,-0.1]).T )
+footsteps.addPhase( .5, 'none' )
+
+refTime = 2.0
+
+print "*** Footstep status at time ",refTime
+print "Current phase: <",footsteps.getPhaseType(refTime), "> foot is flying."
+print "Foot positions: left = ",footsteps.getLeftPosition(refTime).T, \
+      "  -- right = ", footsteps.getRightPosition(refTime).T
+print "Next position of left foot = ", footsteps.getLeftNextPosition(refTime).T
+
+print "Phase timings: ", [ footsteps.getPhaseStart(refTime), \
+                               footsteps.getPhaseStart(refTime)+footsteps.getPhaseDuration(refTime) ] 
+print "Time to impact: ", footsteps.getPhaseRemaining(refTime)
+

example_matplotlib.py

+import numpy as np
+import matplotlib.pyplot as plt
+# In plt, the following functions are the most useful:
+#    ion,plot,draw,show,subplot,figure,title,savefig
+
+# For use in interactive python mode (ipthyon -i)
+interactivePlot = False
+
+if interactivePlot:
+    plt.ion() # Plot functions now instantaneously display, shell is not blocked
+
+
+# Build numpy array for x axis
+x = 1e-3 * np.array (range (100))
+# Build numpy array for y axis
+y = x**2
+
+fig = plt.figure ()
+ax = fig.add_subplot ('111')
+ax.plot (x, y)
+ax.legend (("x^2",))
+
+if not interactivePlot:
+    # Display all the plots and block the shell.
+    # The script will only ends when all windows are closed.
+    plt.show ()
+

example_robot.py

+# Create a 7DOF manipulator robot and moves it along a constant (random) velocity during 10secs.
+from pinocchio.utils import *
+from numpy.linalg import pinv,norm
+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.
+v = rand(robot.model.nv)
+q = np.copy(robot.q0)
+dt = 5e-3
+for i in range(10000):
+    q += v*dt
+    robot.display(q)
+    time.sleep(dt)

example_romeo.py

+import pinocchio as se3
+from pinocchio.romeo_wrapper import RomeoWrapper
+from pinocchio.utils import *
+from numpy.linalg import norm
+
+path = '/home/student/models/romeo/'
+urdf = path + 'urdf/romeo.urdf'
+pkgs = [ path, ]
+robot = RomeoWrapper(urdf,pkgs,se3.JointModelFreeFlyer()) # Load urdf model
+                                  # The robot is loaded with the basis fixed to the world
+robot.initDisplay(loadModel=True) # setup the viewer to display the robot
+
+NQ = robot.model.nq               # model configuration size (6)
+NV = robot.model.nv               # model configuration velocity size (6)
+
+q  = rand(NQ)                     # Set up an initial configuration
+q[3:7] /= norm(q[3:7])            # Normalize the quaternion
+robot.display(q)
+
+vq = zero(NV)
+vq[10] = 1                         # Set up a constant robot speed
+
+from time import sleep
+for i in range(10000):            # Move the robot with constant velocity
+    q[7:] += vq[6:]/100
+    robot.display(q)
+    sleep(.01)

example_scipy.py

+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}'.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'
+

example_ur5.py

+import pinocchio as se3
+from pinocchio.robot_wrapper import RobotWrapper
+from pinocchio.utils import *
+
+path = '/home/student/models/'
+urdf = path + 'ur_description/urdf/ur5_gripper.urdf'
+pkgs = [ path, ] # paths where to find the model meshes
+
+robot = RobotWrapper(urdf,pkgs)   # Load urdf model
+                                  # The robot is loaded with the basis fixed to the world
+robot.initDisplay(loadModel=True) # Setup the viewer to display the robot
+
+NQ = robot.model.nq               # model configuration size (6)
+NV = robot.model.nv               # model configuration velocity size (6)
+
+q  = rand(NQ)                     # Set up an initial configuration
+robot.display(q)
+
+vq = zero(NV)
+vq[3] = 1                         # Set up a constant robot speed
+
+from time import sleep
+for i in range(10000):            # Move the robot with constant velocity
+    q += vq/100
+    robot.display(q)
+    sleep(.01)

factor.py

+from pinocchio.utils import *
+import numpy.linalg as npl
+
+'''
+This file implements a sparse linear problem (quadric cost, linear constraints -- LCQP)
+where the decision variables are denoted by x=(x1 ... xn), n being the number of factors.
+The problem can be written:
+  min      Sum_i=1^p  || A_i x - b_i ||^2
+x1...xn
+
+so that    forall j=1:q   C_j x = d_i
+
+Matrices A_i and C_j are block sparse, i.e. they are acting only on some (few) of the variables
+x1 .. xn.
+
+The file implements the main class FactorGraph, which stores the LCQP problem and solve it.
+It also provides a secondary class Factor, used to set up FactorGraph
+'''
+
+
+class Factor:
+    '''
+    A factor is a part of a linear constraint corresponding either a cost ||A x - b|| or
+    a constraint Cx = d.
+    In both cases, we have Ax = sum A_i x_i, where some A_i are null. One object of class
+    Factor stores one of the A_i, along with the correspond <i> index. It is simply a pair
+    (index,matrix).
+
+    This class is used as a arguments of some of the setup functions of FactorGraph.
+    '''
+    def __init__(self, index, matrix):
+        self.index = index
+        self.matrix = matrix
+
+class FactorGraph:
+    '''
+    The class FactorGraph stores a block-sparse linear-constrained quadratic program (LCQP)
+    of variable x=(x1...xn). The size of the problem is set up at construction of the object.
+    Methods addFactor() and addFactorConstraint() are used to set up the problem.
+    Method solve() is used to compute the solution to the problem.
+    '''
+
+
+    def __init__(self,variableSize,nbVariables):
+        '''
+        Initialize a QP sparse problem as min || A x - b || so that C x = d
+        where  x = (x1,..,xn), and dim(xi) = variableSize and n = nbVariables
+        After construction, A,b,C and d are allocated and set to 0.
+        '''
+        self.nx = variableSize
+        self.N = nbVariables
+        self.A = zero([0,self.N*self.nx])
+        self.b = zero(0)
+        self.C = zero([0,self.N*self.nx])
+        self.d = zero(0)
+
+    def matrixFromFactor(self,factors):
+        '''
+        Internal function: not designed to be called by the user.
+        Create a factor matrix [ A1 0 A2 0 A3 ... ] where the Ai's are placed at
+        the indexes of the factors.
+        '''
+        assert( len(factors)>0 )
+        nr = factors[0].matrix.shape[0] # nb rows of the factor
+        nc = self.nx * self.N           # nb cols
+
+        # Define and fill the new rows to be added
+        A = zero([nr,nc])               # new factor to be added to self.A
+        for factor in factors:
+            assert( factor.matrix.shape == (nr,self.nx) )
+            A[:,self.nx*factor.index:self.nx*(factor.index+1)] = factor.matrix
+        return A
+
+    def addFactor(self,factors,reference):
+        '''
+        Add a factor || sum_{i} factor[i].matrix * x_{factor[i].index} - reference ||
+        to the cost.
+        '''
+
+        # Add the new rows to the cost matrix.
+        self.A = np.vstack([ self.A, self.matrixFromFactor(factors) ])
+        self.b = np.vstack([ self.b, reference ])
+
+    def addFactorConstraint(self,factors,reference):
+        '''
+        Add a factor sum_{i} factor[i].matrix * x_{factor[i].index} =  reference
+        to the constraints.
+        '''
+
+        # Add the new rows to the cost matrix.
+        self.C = np.vstack([ self.C, self.matrixFromFactor(factors) ])
+        self.d = np.vstack([ self.d, reference ])
+
+    def solve(self,eps = 1e-8):
+        '''
+        Implement a LCQP solver, with numerical threshold eps.
+        '''
+        Cp = npl.pinv(self.C,eps)
+        xopt = Cp*self.d
+        P = eye(self.nx*self.N) - Cp*self.C
+        xopt += npl.pinv(self.A*P,eps)*(self.b - self.A*xopt)
+        return xopt
+
+

foot_steps.py

+from pinocchio.utils import *
+
+class FootSteps:
+    '''
+    The class stores three functions of time: left, right and flyingFoot.
+    Each function is piecewise constant. For each function, the user can ask
+    what is the value of this function at time t.
+
+    The storage is composed of three lists for left, right and flyingFoot, and a list for time.
+    The list of times stores the time intervals, i.e. each element of the list is
+    the start of a time interval. The first element of the list is 0.
+    The value of the functions left,right,flyingFoot one this time interval is stored at
+    the same position is their respective list (i.e. value of left on interval
+    [ time[i],time[i+1] ] is stored in left[i].
+
+    The 4 lists are set up using function addPhase().
+    The values of functions left,right,flyingFoot can be accessed through the function
+    getPhaseType(t), getLeftPosition(t), getRightPosition(t).
+    PhaseType are 'left' (meaning left foot is flying, right foot is fixed), 'right' (ie the opposite)
+    or 'none' (meaning no foot is flying, both are fixed on the ground).
+
+    Additionnally, functions getLeftNextPosition(t),
+    getRightNextPosition(t) can be used to get the next position of the
+    flying foot (in that case, additional work is needed to compute the
+    position of flying foot at time t by interpolating getLeftPosition(t)
+    and getLeftNextPosition(t).
+
+    Functions getPhaseStart(t), getPhaseDuration(t) and getPhaseRemaining(t)
+    can be used to get the starting time, the duration and the remaining time of the
+    current phase at time t.
+    '''
+
+    def __init__( self, right,left ):
+        '''
+        The class is initiated from the initial positions of left and right feet.
+        '''
+
+        self.right = [ right, ]
+        self.left  = [ left,  ]
+        self.time  = [ 0.,    ]
+        self.flyingFoot = []
+
+    def addPhase( self, duration, foot, position = None ):
+        '''
+        Add a phase lasting <duration> where the flyhing foot <foot> (either 'left' or 'right')
+        moves to <position> (being a vector or a SE3 placement).
+        Alternatively, <foot> might be set to 'none' (i.e double support). In that case, <position>
+        is not specified (or is set to None, default).
+        '''
+
+        assert( foot == 'left' or foot == 'right' or foot == 'none' )
+        self.time.append( self.time[-1]+duration )
+        self.right.append( self.right[-1] )
+        self.left.append( self.left[-1] )
+        self.flyingFoot.append(foot)
+        if foot=='left':
+            self.left[-1] = position
+        elif foot=='right':
+            self.right[-1] = position
+
+    def getIndexFromTime(self, t):
+        '''Return the index i of the interval containing t, i.e. t in time[i],time[i+1] '''
+        if t>self.time[-1]: return len(self.time)-1
+        return next( i for i,ti in enumerate(self.time) if ti>t ) - 1
+
+    def getPhaseType(self, t ):
+        i = self.getIndexFromTime(t)
+        return self.flyingFoot[i]
+
+    def getLeftPosition(self,t):
+        i = self.getIndexFromTime(t)
+        return self.left[i]
+
+    def getRightPosition(self,t):
+        i = self.getIndexFromTime(t)
+        return self.right[i]
+
+    def getLeftNextPosition(self,t):
+        i = self.getIndexFromTime(t)
+        i = i+1 if i+1< len(self.time) else i
+        return self.left[i]
+
+    def getRightNextPosition(self,t):
+        i = self.getIndexFromTime(t)
+        i = i+1 if i+1< len(self.time) else i
+        return self.right[i]
+
+    def getPhaseStart(self,t):
+        i = self.getIndexFromTime(t)
+        return self.time[i]
+    def getPhaseDuration(self,t):
+        i = self.getIndexFromTime(t)
+        return self.time[i+1]-self.time[i]
+    def getPhaseRemaining(self,t):
+        i = self.getIndexFromTime(t)
+        return self.time[i+1]-t
+

footsteps.py

+from pinocchio.utils import *
+
+class FootSteps:
+    '''
+    The class stores three functions of time: left, right and flyingFoot.
+    Each function is piecewise constant. For each function, the user can ask
+    what is the value of this function at time t.
+
+    The storage is composed of three lists for left, right and flyingFoot, and a list for time.
+    The list of times stores the time intervals, i.e. each element of the list is
+    the start of a time interval. The first element of the list is 0.
+    The value of the functions left,right,flyingFoot one this time interval is stored at
+    the same position is their respective list (i.e. value of left on interval
+    [ time[i],time[i+1] ] is stored in left[i].
+
+    The 4 lists are set up using function addPhase().
+    The values of functions left,right,flyingFoot can be accessed through the function
+    getPhaseType(t), getLeftPosition(t), getRightPosition(t).
+    PhaseType are 'left' (meaning left foot is flying, right foot is fixed), 'right' (ie the opposite)
+    or 'none' (meaning no foot is flying, both are fixed on the ground).
+
+    Additionnally, functions getLeftNextPosition(t),
+    getRightNextPosition(t) can be used to get the next position of the
+    flying foot (in that case, additional work is needed to compute the
+    position of flying foot at time t by interpolating getLeftPosition(t)
+    and getLeftNextPosition(t).
+
+    Functions getPhaseStart(t), getPhaseDuration(t) and getPhaseRemaining(t)
+    can be used to get the starting time, the duration and the remaining time of the
+    current phase at time t.
+    '''
+
+    def __init__( self, right,left ):
+        '''
+        The class is initiated from the initial positions of left and right feet.
+        '''
+
+        self.right = [ right, ]
+        self.left  = [ left,  ]
+        self.time  = [ 0.,    ]
+        self.flyingFoot = []
+
+    def addPhase( self, duration, foot, position = None ):
+        '''
+        Add a phase lasting <duration> where the flyhing foot <foot> (either 'left' or 'right')
+        moves to <position> (being a vector or a SE3 placement).
+        Alternatively, <foot> might be set to 'none' (i.e double support). In that case, <position>
+        is not specified (or is set to None, default).
+        '''
+
+        assert( foot == 'left' or foot == 'right' or foot == 'none' )
+        self.time.append( self.time[-1]+duration )
+        self.right.append( self.right[-1] )
+        self.left.append( self.left[-1] )
+        self.flyingFoot.append(foot)
+        if foot=='left':
+            self.left[-1] = position
+        elif foot=='right':
+            self.right[-1] = position
+
+    def getIndexFromTime(self, t):
+        '''Return the index i of the interval containing t, i.e. t in time[i],time[i+1] '''
+        if t>self.time[-1]: return len(self.time)-1
+        return next( i for i,ti in enumerate(self.time) if ti>t ) - 1
+
+    def getPhaseType(self, t ):
+        i = self.getIndexFromTime(t)
+        return self.flyingFoot[i]
+
+    def getLeftPosition(self,t):
+        i = self.getIndexFromTime(t)
+        return self.left[i]
+
+    def getRightPosition(self,t):
+        i = self.getIndexFromTime(t)
+        return self.right[i]
+
+    def getLeftNextPosition(self,t):
+        i = self.getIndexFromTime(t)
+        i = i+1 if i+1< len(self.time) else i
+        return self.left[i]
+
+    def getRightNextPosition(self,t):
+        i = self.getIndexFromTime(t)
+        i = i+1 if i+1< len(self.time) else i
+        return self.right[i]
+
+    def getPhaseStart(self,t):
+        i = self.getIndexFromTime(t)
+        return self.time[i]
+    def getPhaseDuration(self,t):
+        i = self.getIndexFromTime(t)
+        return self.time[i+1]-self.time[i]
+    def getPhaseRemaining(self,t):
+        i = self.getIndexFromTime(t)
+        return self.time[i+1]-t
+

robot_arm.py

+from pinocchio.utils import *
+from pinocchio.explog import exp,log
+from numpy.linalg import pinv,norm
+import pinocchio as se3
+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):