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


pinocchio
Author(s):
autogenerated on Sun Dec 22 2024 03:41:09