Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
Stack Of Tasks
pinocchio
Commits
9413d2cc
Commit
9413d2cc
authored
Jan 16, 2018
by
jcarpent
Browse files
[Data] Add explicit data.Dinv quantity for cholesky computations
parent
36cb1069
Changes
3
Hide whitespace changes
Inline
Side-by-side
src/algorithm/cholesky.hxx
View file @
9413d2cc
//
// Copyright (c) 2015-201
7
CNRS
// Copyright (c) 2015-201
8
CNRS
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
...
...
@@ -52,6 +52,7 @@ namespace se3
Eigen
::
MatrixXd
&
M
=
data
.
M
;
Eigen
::
MatrixXd
&
U
=
data
.
U
;
Eigen
::
VectorXd
&
D
=
data
.
D
;
Eigen
::
VectorXd
&
Dinv
=
data
.
Dinv
;
for
(
int
j
=
model
.
nv
-
1
;
j
>=
0
;
--
j
)
{
...
...
@@ -61,10 +62,10 @@ namespace se3
DUt
=
U
.
row
(
j
).
segment
(
j
+
1
,
NVT
).
transpose
()
.
cwiseProduct
(
D
.
segment
(
j
+
1
,
NVT
));
D
[
j
]
=
M
(
j
,
j
)
-
U
.
row
(
j
).
segment
(
j
+
1
,
NVT
)
*
DUt
;
const
double
dj_inv
(
1.
/
D
[
j
]
)
;
D
[
j
]
=
M
(
j
,
j
)
-
U
.
row
(
j
).
segment
(
j
+
1
,
NVT
)
.
dot
(
DUt
)
;
Dinv
[
j
]
=
1.
/
D
[
j
];
for
(
int
_i
=
data
.
parents_fromRow
[(
Model
::
Index
)
j
];
_i
>=
0
;
_i
=
data
.
parents_fromRow
[(
Model
::
Index
)
_i
])
U
(
_i
,
j
)
=
(
M
(
_i
,
j
)
-
U
.
row
(
_i
).
segment
(
j
+
1
,
NVT
).
dot
(
DUt
))
*
dj_inv
;
U
(
_i
,
j
)
=
(
M
(
_i
,
j
)
-
U
.
row
(
_i
).
segment
(
j
+
1
,
NVT
).
dot
(
DUt
))
*
Dinv
[
j
]
;
}
return
data
.
U
;
...
...
@@ -181,8 +182,8 @@ namespace se3
Eigen
::
MatrixBase
<
Mat
>
&
v
)
{
Utv
(
model
,
data
,
v
);
for
(
int
k
=
0
;
k
<
model
.
nv
;
++
k
)
v
.
row
(
k
)
*=
data
.
D
[
k
];
return
Uv
(
model
,
data
,
v
);
v_
.
array
()
*=
data
.
D
.
array
();
}
}
// internal
...
...
@@ -208,8 +209,8 @@ namespace se3
assert
(
model
.
check
(
data
)
&&
"data is not consistent with model."
);
#endif
Uiv
(
model
,
data
,
v
);
for
(
int
k
=
0
;
k
<
model
.
nv
;
++
k
)
v
.
row
(
k
)
/=
data
.
D
[
k
];
return
Utiv
(
model
,
data
,
v
);
v_
.
array
()
*=
data
.
Dinv
.
array
();
}
}
// namespace cholesky
...
...
src/multibody/model.hpp
View file @
9413d2cc
...
...
@@ -498,6 +498,9 @@ namespace se3
/// \brief Diagonal of the joint space intertia matrix obtained by a Cholesky Decomposition.
Eigen
::
VectorXd
D
;
/// \brief Diagonal inverse of the joint space intertia matrix obtained by a Cholesky Decomposition.
Eigen
::
VectorXd
Dinv
;
/// \brief Temporary of size NV used in Cholesky Decomposition.
Eigen
::
VectorXd
tmp
;
...
...
src/multibody/model.hxx
View file @
9413d2cc
...
...
@@ -270,6 +270,7 @@ namespace se3
,
nvSubtree
((
std
::
size_t
)
model
.
njoints
)
,
U
(
model
.
nv
,
model
.
nv
)
,
D
(
model
.
nv
)
,
Dinv
(
model
.
nv
)
,
tmp
(
model
.
nv
)
,
parents_fromRow
((
std
::
size_t
)
model
.
nv
)
,
nvSubtree_fromRow
((
std
::
size_t
)
model
.
nv
)
...
...
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new 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