 pinocchio  2.6.21 A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives factor.py
1 from pinocchio.utils import zero, eye
2 import numpy as np
3 import numpy.linalg as npl
4
5 '''
6 This file implements a sparse linear problem (quadric cost, linear constraints -- LCQP)
7 where the decision variables are denoted by x=(x1 ... xn), n being the number of factors.
8 The problem can be written:
9  min Sum_i=1^p || A_i x - b_i ||^2
10 x1...xn
11
12 so that forall j=1:q C_j x = d_i
13
14 Matrices A_i and C_j are block sparse, i.e. they are acting only on some (few) of the variables
15 x1 .. xn.
16
17 The file implements the main class FactorGraph, which stores the LCQP problem and solve it.
18 It also provides a secondary class Factor, used to set up FactorGraph
19 '''
20
21
22 class Factor(object):
23  '''
24  A factor is a part of a linear constraint corresponding either a cost ||A x - b|| or
25  a constraint Cx = d.
26  In both cases, we have Ax = sum A_i x_i, where some A_i are null. One object of class
27  Factor stores one of the A_i, along with the correspond <i> index. It is simply a pair
28  (index, matrix).
29
30  This class is used as a arguments of some of the setup functions of FactorGraph.
31  '''
32  def __init__(self, index, matrix):
33  self.index = index
34  self.matrix = matrix
35
36
37 class FactorGraph(object):
38  '''
39  The class FactorGraph stores a block-sparse linear-constrained quadratic program (LCQP)
40  of variable x=(x1...xn). The size of the problem is set up at construction of the object.
41  Methods add_factor() and add_factor_constraint() are used to set up the problem.
42  Method solve() is used to compute the solution to the problem.
43  '''
44  def __init__(self, variableSize, nbVariables):
45  '''
46  Initialize a QP sparse problem as min || A x - b || so that C x = d
47  where x = (x1, .., xn), and dim(xi) = variableSize and n = nbVariables
48  After construction, A, b, C and d are allocated and set to 0.
49  '''
50  self.nx = variableSize
51  self.N = nbVariables
52  self.A = zero([0, self.N * self.nx])
53  self.b = zero(0)
54  self.C = zero([0, self.N * self.nx])
55  self.d = zero(0)
56
57  def matrix_form_factor(self, factors):
58  '''
59  Internal function: not designed to be called by the user.
60  Create a factor matrix [ A1 0 A2 0 A3 ... ] where the Ai's are placed at
61  the indexes of the factors.
62  '''
63  assert(len(factors) > 0)
64  nr = factors.matrix.shape # nb rows of the factor
65  nc = self.nx * self.N # nb cols
66
67  # Define and fill the new rows to be added
68  A = zero([nr, nc]) # new factor to be added to self.A
69  for factor in factors:
70  assert(factor.matrix.shape == (nr, self.nx))
71  A[:, self.nx * factor.index:self.nx * (factor.index + 1)] = factor.matrix
72  return A
73
75  '''
76  Add a factor || sum_{i} factor[i].matrix * x_{factor[i].index} - reference ||
77  to the cost.
78  '''
79  # Add the new rows to the cost matrix.
80  self.A = np.vstack([self.A, self.matrix_form_factor(factors)])
81  self.b = np.vstack([self.b, reference])
82
84  '''
85  Add a factor sum_{i} factor[i].matrix * x_{factor[i].index} = reference
86  to the constraints.
87  '''
88  # Add the new rows to the cost matrix.
89  self.C = np.vstack([self.C, self.matrix_form_factor(factors)])
90  self.d = np.vstack([self.d, reference])
91
92  def solve(self, eps=1e-8):
93  '''
94  Implement a LCQP solver, with numerical threshold eps.
95  '''
96  Cp = npl.pinv(self.C, eps)
97  xopt = Cp * self.d
98  P = eye(self.nx * self.N) - Cp * self.C
99  xopt += npl.pinv(self.A * P, eps) * (self.b - self.A * xopt)
100  return xopt
factor.FactorGraph.N
N
Definition: factor.py:51
factor.FactorGraph.matrix_form_factor
def matrix_form_factor(self, factors)
Definition: factor.py:57
factor.FactorGraph.d
d
Definition: factor.py:55
factor.Factor.index
index
Definition: factor.py:33
factor.Factor.matrix
matrix
Definition: factor.py:34
Definition: factor.py:83
utils
factor.FactorGraph.nx
nx
Definition: factor.py:50
factor.Factor
Definition: factor.py:22
factor.FactorGraph
Definition: factor.py:37
factor.FactorGraph.C
C
Definition: factor.py:54
factor.FactorGraph.solve
def solve(self, eps=1e-8)
Definition: factor.py:92
factor.FactorGraph.A
A
Definition: factor.py:52
factor.FactorGraph.b
b
Definition: factor.py:53
factor.FactorGraph.__init__
def __init__(self, variableSize, nbVariables)
Definition: factor.py:44