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
Stack Of Tasks
pinocchio
Commits
06c7ae89
Verified
Commit
06c7ae89
authored
Nov 14, 2019
by
Justin Carpentier
Browse files
liegroup: fix identiation
parent
3849da15
Changes
2
Hide whitespace changes
Inline
Side-by-side
src/multibody/liegroup/cartesian-product.hpp
View file @
06c7ae89
...
...
@@ -48,15 +48,16 @@ namespace pinocchio
CartesianProductOperation
()
:
lg1_
(),
lg2_
()
{
}
//
/
Get dimension of Lie Group vector representation
//
/
//
/
For instance, for SO(3), the dimension of the vector representation is
//
/
4 (quaternion) while the dimension of the tangent space is 3.
// Get dimension of Lie Group vector representation
//
// For instance, for SO(3), the dimension of the vector representation is
// 4 (quaternion) while the dimension of the tangent space is 3.
Index
nq
()
const
{
return
lg1_
.
nq
()
+
lg2_
.
nq
();
}
/// Get dimension of Lie Group tangent space
// Get dimension of Lie Group tangent space
Index
nv
()
const
{
return
lg1_
.
nv
()
+
lg2_
.
nv
();
...
...
@@ -79,17 +80,17 @@ namespace pinocchio
template
<
class
ConfigL_t
,
class
ConfigR_t
,
class
Tangent_t
>
void
difference_impl
(
const
Eigen
::
MatrixBase
<
ConfigL_t
>
&
q0
,
const
Eigen
::
MatrixBase
<
ConfigR_t
>
&
q1
,
const
Eigen
::
MatrixBase
<
Tangent_t
>
&
d
)
const
const
Eigen
::
MatrixBase
<
ConfigR_t
>
&
q1
,
const
Eigen
::
MatrixBase
<
Tangent_t
>
&
d
)
const
{
lg1_
.
difference
(
Q1
(
q0
),
Q1
(
q1
),
Vo1
(
d
));
lg2_
.
difference
(
Q2
(
q0
),
Q2
(
q1
),
Vo2
(
d
));
}
template
<
ArgumentPosition
arg
,
class
ConfigL_t
,
class
ConfigR_t
,
class
JacobianOut_t
>
void
dDifference
(
const
Eigen
::
MatrixBase
<
ConfigL_t
>
&
q0
,
const
Eigen
::
MatrixBase
<
ConfigR_t
>
&
q1
,
const
Eigen
::
MatrixBase
<
JacobianOut_t
>&
J
)
const
void
dDifference
(
const
Eigen
::
MatrixBase
<
ConfigL_t
>
&
q0
,
const
Eigen
::
MatrixBase
<
ConfigR_t
>
&
q1
,
const
Eigen
::
MatrixBase
<
JacobianOut_t
>
&
J
)
const
{
J12
(
J
).
setZero
();
J21
(
J
).
setZero
();
...
...
@@ -100,8 +101,8 @@ namespace pinocchio
template
<
class
ConfigIn_t
,
class
Velocity_t
,
class
ConfigOut_t
>
void
integrate_impl
(
const
Eigen
::
MatrixBase
<
ConfigIn_t
>
&
q
,
const
Eigen
::
MatrixBase
<
Velocity_t
>
&
v
,
const
Eigen
::
MatrixBase
<
ConfigOut_t
>
&
qout
)
const
const
Eigen
::
MatrixBase
<
Velocity_t
>
&
v
,
const
Eigen
::
MatrixBase
<
ConfigOut_t
>
&
qout
)
const
{
lg1_
.
integrate
(
Q1
(
q
),
V1
(
v
),
Qo1
(
qout
));
lg2_
.
integrate
(
Q2
(
q
),
V2
(
v
),
Qo2
(
qout
));
...
...
src/multibody/liegroup/special-orthogonal.hpp
View file @
06c7ae89
...
...
@@ -127,9 +127,9 @@ namespace pinocchio
}
template
<
ArgumentPosition
arg
,
class
ConfigL_t
,
class
ConfigR_t
,
class
JacobianOut_t
>
void
dDifference_impl
(
const
Eigen
::
MatrixBase
<
ConfigL_t
>
&
q0
,
const
Eigen
::
MatrixBase
<
ConfigR_t
>
&
q1
,
const
Eigen
::
MatrixBase
<
JacobianOut_t
>&
J
)
const
void
dDifference_impl
(
const
Eigen
::
MatrixBase
<
ConfigL_t
>
&
q0
,
const
Eigen
::
MatrixBase
<
ConfigR_t
>
&
q1
,
const
Eigen
::
MatrixBase
<
JacobianOut_t
>
&
J
)
const
{
Matrix2
R
;
// R0.transpose() * R1;
R
(
0
,
0
)
=
R
(
1
,
1
)
=
q0
.
dot
(
q1
);
...
...
@@ -271,6 +271,7 @@ namespace pinocchio
{
return
NQ
;
}
/// Get dimension of Lie Group tangent space
static
Index
nv
()
{
...
...
@@ -306,7 +307,7 @@ namespace pinocchio
template
<
ArgumentPosition
arg
,
class
ConfigL_t
,
class
ConfigR_t
,
class
JacobianOut_t
>
void
dDifference_impl
(
const
Eigen
::
MatrixBase
<
ConfigL_t
>
&
q0
,
const
Eigen
::
MatrixBase
<
ConfigR_t
>
&
q1
,
const
Eigen
::
MatrixBase
<
JacobianOut_t
>&
J
)
const
const
Eigen
::
MatrixBase
<
JacobianOut_t
>
&
J
)
const
{
ConstQuaternionMap_t
p0
(
q0
.
derived
().
data
());
ConstQuaternionMap_t
p1
(
q1
.
derived
().
data
());
...
...
@@ -369,7 +370,7 @@ namespace pinocchio
template
<
class
Config_t
,
class
Tangent_t
,
class
JacobianOut_t
>
static
void
dIntegrate_dq_impl
(
const
Eigen
::
MatrixBase
<
Config_t
>
&
/*q*/
,
const
Eigen
::
MatrixBase
<
Tangent_t
>
&
v
,
const
Eigen
::
MatrixBase
<
JacobianOut_t
>&
J
)
const
Eigen
::
MatrixBase
<
JacobianOut_t
>
&
J
)
{
JacobianOut_t
&
Jout
=
PINOCCHIO_EIGEN_CONST_CAST
(
JacobianOut_t
,
J
);
Jout
=
exp3
(
-
v
);
...
...
@@ -387,7 +388,7 @@ namespace pinocchio
static
void
interpolate_impl
(
const
Eigen
::
MatrixBase
<
ConfigL_t
>
&
q0
,
const
Eigen
::
MatrixBase
<
ConfigR_t
>
&
q1
,
const
Scalar
&
u
,
const
Eigen
::
MatrixBase
<
ConfigOut_t
>&
qout
)
const
Eigen
::
MatrixBase
<
ConfigOut_t
>
&
qout
)
{
ConstQuaternionMap_t
p0
(
q0
.
derived
().
data
());
ConstQuaternionMap_t
p1
(
q1
.
derived
().
data
());
...
...
@@ -406,7 +407,7 @@ namespace pinocchio
}
template
<
class
Config_t
>
static
void
normalize_impl
(
const
Eigen
::
MatrixBase
<
Config_t
>&
qout
)
static
void
normalize_impl
(
const
Eigen
::
MatrixBase
<
Config_t
>
&
qout
)
{
Config_t
&
qout_
=
PINOCCHIO_EIGEN_CONST_CAST
(
Config_t
,
qout
);
qout_
.
normalize
();
...
...
Write
Preview
Markdown
is supported
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