Commit 0b0cbd4c authored by Florent Lamiraux's avatar Florent Lamiraux Committed by Florent Lamiraux florent@laas.fr
Browse files

BE INSA version 2017

parents
# 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 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())
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()
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)
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 ()
# 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)
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)
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'
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)
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
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
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
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):