Skip to content
GitLab
Menu
Projects
Groups
Snippets
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
Gepetto
beinsa
Commits
044b8b2b
Commit
044b8b2b
authored
Oct 19, 2018
by
Florent Lamiraux
Committed by
Florent Lamiraux florent@laas.fr
Oct 19, 2018
Browse files
Add file factor_graph.py
parent
5577288d
Changes
1
Hide whitespace changes
Inline
Sidebyside
factor_graph.py
0 → 100644
View file @
044b8b2b
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 blocksparse linearconstrained
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
=
1e8
):
'''
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
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Attach a file
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment