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
Guilhem Saurel
pinocchio
Commits
aacfb2e7
Commit
aacfb2e7
authored
Jan 05, 2017
by
Joseph Mirabel
Committed by
Joseph Mirabel
Jan 05, 2017
Browse files
[C++] Clean code of lie groups
parent
e1891980
Changes
6
Hide whitespace changes
Inline
Side-by-side
src/multibody/liegroup/cartesian-product.hpp
View file @
aacfb2e7
...
...
@@ -81,11 +81,11 @@ namespace se3
static
void
randomConfiguration_impl
(
const
Eigen
::
MatrixBase
<
ConfigL_t
>
&
lower
,
const
Eigen
::
MatrixBase
<
ConfigR_t
>
&
upper
,
const
Eigen
::
MatrixBase
<
ConfigOut_t
>
&
qout
)
{
{
ConfigOut_t
&
out
=
const_cast
<
Eigen
::
MatrixBase
<
ConfigOut_t
>&
>
(
qout
).
derived
();
LieGroup1
::
randomConfiguration
(
lower
.
template
head
<
LieGroup1
::
NQ
>(),
upper
.
template
head
<
LieGroup1
::
NQ
>(),
out
.
template
head
<
LieGroup1
::
NQ
>());
LieGroup2
::
randomConfiguration
(
lower
.
template
tail
<
LieGroup2
::
NQ
>(),
upper
.
template
tail
<
LieGroup2
::
NQ
>(),
out
.
template
tail
<
LieGroup2
::
NQ
>());
}
}
template
<
class
ConfigL_t
,
class
ConfigR_t
>
static
bool
isSameConfiguration_impl
(
const
Eigen
::
MatrixBase
<
ConfigL_t
>
&
q0
,
...
...
src/multibody/liegroup/operation-base.hpp
View file @
aacfb2e7
...
...
@@ -309,10 +309,10 @@ namespace se3
protected:
/// Default constructor.
///
///
/// Prevent the construction of derived class.
LieGroupOperationBase
()
{}
/// Copy constructor
///
/// Prevent the copy of derived class.
...
...
src/multibody/liegroup/special-euclidean.hpp
View file @
aacfb2e7
...
...
@@ -111,9 +111,9 @@ namespace se3
static
void
randomConfiguration_impl
(
const
Eigen
::
MatrixBase
<
ConfigL_t
>
&
lower
,
const
Eigen
::
MatrixBase
<
ConfigR_t
>
&
upper
,
const
Eigen
::
MatrixBase
<
ConfigOut_t
>
&
qout
)
{
{
R3crossSO3_t
::
randomConfiguration
(
lower
,
upper
,
qout
);
}
}
template
<
class
ConfigL_t
,
class
ConfigR_t
>
static
bool
isSameConfiguration_impl
(
const
Eigen
::
MatrixBase
<
ConfigL_t
>
&
q0
,
...
...
@@ -150,7 +150,7 @@ namespace se3
SE3
M1
(
SE3
::
Identity
());
forwardKinematics
(
M1
,
q1
);
Motion
nu
(
log6
(
M0
.
inverse
()
*
M1
));
// TODO: optimize implementation
Tangent_t
&
out
=
const_cast
<
Eigen
::
MatrixBase
<
Tangent_t
>&
>
(
d
).
derived
();
out
.
template
head
<
2
>()
=
nu
.
linear
().
head
<
2
>
();
out
(
2
)
=
nu
.
angular
()(
2
);
...
...
@@ -168,7 +168,7 @@ namespace se3
const
double
&
c0
=
q
(
2
),
s0
=
q
(
3
);
Matrix22
R0
;
R0
<<
c0
,
-
s0
,
s0
,
c0
;
const
double
&
t
=
vs
[
2
];
const
double
theta
=
std
::
fabs
(
t
);
...
...
@@ -235,9 +235,9 @@ namespace se3
static
void
randomConfiguration_impl
(
const
Eigen
::
MatrixBase
<
ConfigL_t
>
&
lower
,
const
Eigen
::
MatrixBase
<
ConfigR_t
>
&
upper
,
const
Eigen
::
MatrixBase
<
ConfigOut_t
>
&
qout
)
{
{
R2crossSO1_t
::
randomConfiguration
(
lower
,
upper
,
qout
);
}
}
template
<
class
ConfigL_t
,
class
ConfigR_t
>
static
bool
isSameConfiguration_impl
(
const
Eigen
::
MatrixBase
<
ConfigL_t
>
&
q0
,
...
...
@@ -255,7 +255,7 @@ namespace se3
const
double
&
c_theta
=
q
(
2
),
s_theta
=
q
(
3
);
M
.
rotation
().
topLeftCorner
<
2
,
2
>
()
<<
c_theta
,
-
s_theta
,
s_theta
,
c_theta
;
M
.
translation
().
head
<
2
>
()
=
q
.
template
head
<
2
>();
}
...
...
src/multibody/liegroup/special-orthogonal.hpp
View file @
aacfb2e7
...
...
@@ -109,9 +109,9 @@ namespace se3
static
void
randomConfiguration_impl
(
const
Eigen
::
MatrixBase
<
ConfigL_t
>
&
,
const
Eigen
::
MatrixBase
<
ConfigR_t
>
&
,
const
Eigen
::
MatrixBase
<
ConfigOut_t
>
&
qout
)
{
{
random_impl
(
qout
);
}
}
template
<
class
ConfigL_t
,
class
ConfigR_t
>
static
bool
isSameConfiguration_impl
(
const
Eigen
::
MatrixBase
<
ConfigL_t
>
&
q0
,
...
...
@@ -121,7 +121,7 @@ namespace se3
ConstQuaternionMap_t
quat1
(
q0
.
derived
().
data
());
ConstQuaternionMap_t
quat2
(
q1
.
derived
().
data
());
return
defineSameRotation
(
quat1
,
quat2
);
return
defineSameRotation
(
quat1
,
quat2
,
prec
);
}
};
// struct SpecialOrthogonalOperation
...
...
@@ -146,11 +146,8 @@ namespace se3
const
Eigen
::
MatrixBase
<
ConfigR_t
>
&
q1
,
const
Eigen
::
MatrixBase
<
Tangent_t
>
&
d
)
{
const
Scalar
&
c0
=
q0
(
0
),
&
s0
=
q0
(
1
);
const
Scalar
&
c1
=
q1
(
0
),
&
s1
=
q1
(
1
);
const_cast
<
Eigen
::
MatrixBase
<
Tangent_t
>&
>
(
d
)
[
0
]
=
atan2
(
s1
*
c0
-
s0
*
c1
,
c0
*
c1
+
s0
*
s1
);
=
atan2
(
q0
(
0
)
*
q1
(
1
)
-
q0
(
1
)
*
q1
(
0
),
q0
.
dot
(
q1
)
);
}
template
<
class
ConfigIn_t
,
class
Velocity_t
,
class
ConfigOut_t
>
...
...
@@ -180,28 +177,26 @@ namespace se3
const
Eigen
::
MatrixBase
<
ConfigOut_t
>&
qout
)
{
ConfigOut_t
&
out
=
(
const_cast
<
Eigen
::
MatrixBase
<
ConfigOut_t
>&
>
(
qout
)).
derived
();
const
Scalar
&
c0
=
q0
(
0
),
&
s0
=
q0
(
1
);
const
Scalar
&
c1
=
q1
(
0
),
&
s1
=
q1
(
1
);
assert
(
(
q0
.
norm
()
-
1
)
<
1e-8
&&
"initial configuration not normalized"
);
assert
(
(
q1
.
norm
()
-
1
)
<
1e-8
&&
"final configuration not normalized"
);
Scalar
cosTheta
=
c0
*
c1
+
s0
*
s1
;
Scalar
sinTheta
=
c0
*
s1
-
s0
*
c1
;
Scalar
cosTheta
=
q0
.
dot
(
q1
)
;
Scalar
sinTheta
=
q0
(
0
)
*
q1
(
1
)
-
q0
(
1
)
*
q1
(
0
)
;
Scalar
theta
=
atan2
(
sinTheta
,
cosTheta
);
assert
(
fabs
(
sin
(
theta
)
-
sinTheta
)
<
1e-8
);
if
(
fabs
(
theta
)
>
1e-6
&&
fabs
(
theta
)
<
PI
-
1e-6
)
{
out
=
(
sin
((
1
-
u
)
*
theta
)
/
sinTheta
)
*
q0
+
(
sin
(
u
*
theta
)
/
sinTheta
)
*
q1
;
}
out
=
(
sin
((
1
-
u
)
*
theta
)
/
sinTheta
)
*
q0
+
(
sin
(
u
*
theta
)
/
sinTheta
)
*
q1
;
}
else
if
(
fabs
(
theta
)
<
1e-6
)
// theta = 0
{
out
=
(
1
-
u
)
*
q0
+
u
*
q1
;
}
else
// theta = +-PI
{
double
theta0
=
atan2
(
s0
,
c0
);
double
theta0
=
atan2
(
q0
(
1
),
q0
(
0
)
);
out
<<
cos
(
theta0
+
u
*
theta
),
sin
(
theta0
+
u
*
theta
);
}
...
...
@@ -223,9 +218,9 @@ namespace se3
static
void
randomConfiguration_impl
(
const
Eigen
::
MatrixBase
<
ConfigL_t
>
&
,
const
Eigen
::
MatrixBase
<
ConfigR_t
>
&
,
const
Eigen
::
MatrixBase
<
ConfigOut_t
>
&
qout
)
{
{
random_impl
(
qout
);
}
}
};
// struct SpecialOrthogonal1Operation
}
// namespace se3
...
...
src/multibody/liegroup/vector-space.hpp
View file @
aacfb2e7
...
...
@@ -71,11 +71,11 @@ namespace se3
static
void
randomConfiguration_impl
(
const
Eigen
::
MatrixBase
<
ConfigL_t
>
&
lower_pos_limit
,
const
Eigen
::
MatrixBase
<
ConfigR_t
>
&
upper_pos_limit
,
const
Eigen
::
MatrixBase
<
ConfigOut_t
>
&
qout
)
{
{
ConfigOut_t
&
res
=
const_cast
<
Eigen
::
MatrixBase
<
ConfigOut_t
>&
>
(
qout
).
derived
();
for
(
int
i
=
0
;
i
<
NQ
;
++
i
)
{
if
(
lower_pos_limit
[
i
]
==
-
std
::
numeric_limits
<
Scalar
>::
infinity
()
||
if
(
lower_pos_limit
[
i
]
==
-
std
::
numeric_limits
<
Scalar
>::
infinity
()
||
upper_pos_limit
[
i
]
==
std
::
numeric_limits
<
Scalar
>::
infinity
()
)
{
std
::
ostringstream
error
;
...
...
@@ -85,7 +85,7 @@ namespace se3
}
res
[
i
]
=
lower_pos_limit
[
i
]
+
((
upper_pos_limit
[
i
]
-
lower_pos_limit
[
i
])
*
rand
())
/
RAND_MAX
;
}
}
}
};
// struct VectorSpaceOperation
}
// namespace se3
...
...
unittest/joint-composite.cpp
View file @
aacfb2e7
...
...
@@ -91,6 +91,12 @@ void test_joint_methods(const JointModelBase<JointModel> & jmodel, JointModelCom
BOOST_CHECK
(
jdata
.
Dinv
.
isApprox
(
jdata_composite
.
Dinv
));
BOOST_CHECK
(
jdata
.
UDinv
.
isApprox
(
jdata_composite
.
UDinv
));
/// TODO: Remove me. This is for testing purposes.
Eigen
::
VectorXd
qq
=
q
;
Eigen
::
VectorXd
vv
=
v
;
Eigen
::
VectorXd
res
(
jmodel_composite
.
nq
());
typename
se3
::
IntegrateStep
<
se3
::
LieGroupTpl
>::
ArgsType
args
(
qq
,
vv
,
res
);
se3
::
IntegrateStep
<
se3
::
LieGroupTpl
>::
run
(
jmodel_composite
,
args
);
}
struct
TestJointComposite
{
...
...
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