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

Remove unused files.

parent ea741681
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 ()
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) # 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 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 *
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
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment