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
Guilhem Saurel
ndcurves
Commits
db0ca55b
Commit
db0ca55b
authored
Nov 26, 2019
by
Pierre Fernbach
Browse files
[Tests] so3Linear : correctly tests the angular velocity when constructed from matrix
parent
e9a4e8aa
Changes
1
Hide whitespace changes
Inline
Side-by-side
tests/Main.cpp
View file @
db0ca55b
...
...
@@ -1623,42 +1623,44 @@ void polynomialFromBoundaryConditions(bool& error) {
void
so3LinearTest
(
bool
&
error
)
{
quaternion_t
q0
(
1
,
0
,
0
,
0
);
quaternion_t
q1
(
0.7071
,
0.7071
,
0
,
0
);
SO3Linear_t
so3Traj
(
q0
,
q1
,
0.
,
1.5
);
const
double
tMin
=
0.
;
const
double
tMax
=
1.5
;
SO3Linear_t
so3Traj
(
q0
,
q1
,
tMin
,
tMax
);
if
(
so3Traj
.
min
()
!=
0
)
{
if
(
so3Traj
.
min
()
!=
tMin
)
{
error
=
true
;
std
::
cout
<<
"Min bound not respected"
<<
std
::
endl
;
}
if
(
so3Traj
.
max
()
!=
1.5
)
{
if
(
so3Traj
.
max
()
!=
tMax
)
{
error
=
true
;
std
::
cout
<<
"Max bound not respected"
<<
std
::
endl
;
}
if
(
!
so3Traj
.
computeAsQuaternion
(
0
).
isApprox
(
q0
))
{
if
(
!
so3Traj
.
computeAsQuaternion
(
tMin
).
isApprox
(
q0
))
{
error
=
true
;
std
::
cout
<<
"evaluate at t=0 is not the init quaternion"
<<
std
::
endl
;
}
if
(
so3Traj
(
0
)
!=
q0
.
toRotationMatrix
())
{
if
(
so3Traj
(
tMin
)
!=
q0
.
toRotationMatrix
())
{
error
=
true
;
std
::
cout
<<
"evaluate at t=0 is not the init rotation"
<<
std
::
endl
;
}
if
(
!
so3Traj
.
computeAsQuaternion
(
1.5
).
isApprox
(
q1
))
{
if
(
!
so3Traj
.
computeAsQuaternion
(
tMax
).
isApprox
(
q1
))
{
error
=
true
;
std
::
cout
<<
"evaluate at t=max is not the final quaternion"
<<
std
::
endl
;
}
if
(
so3Traj
(
1.5
)
!=
q1
.
toRotationMatrix
())
{
if
(
so3Traj
(
tMax
)
!=
q1
.
toRotationMatrix
())
{
error
=
true
;
std
::
cout
<<
"evaluate at t=max is not the final rotation"
<<
std
::
endl
;
}
// check derivatives :
if
(
so3Traj
.
derivate
(
0
,
1
)
!=
so3Traj
.
derivate
(
1.
,
1
))
{
if
(
so3Traj
.
derivate
(
tMin
,
1
)
!=
so3Traj
.
derivate
(
1.
,
1
))
{
error
=
true
;
std
::
cout
<<
"first order derivative should be constant."
<<
std
::
endl
;
}
if
(
so3Traj
.
derivate
(
0
,
2
)
!=
point_t
::
Zero
(
3
))
{
if
(
so3Traj
.
derivate
(
tMin
,
2
)
!=
point
3
_t
::
Zero
(
3
))
{
error
=
true
;
std
::
cout
<<
"second order derivative should be null"
<<
std
::
endl
;
}
point3_t
angular_vel
=
so3Traj
.
derivate
(
0
,
1
);
point3_t
angular_vel
=
so3Traj
.
derivate
(
tMin
,
1
);
if
(
angular_vel
[
1
]
!=
0.
||
angular_vel
[
2
]
!=
0
){
error
=
true
;
std
::
cout
<<
"Angular velocity around y and z axis should be null"
<<
std
::
endl
;
...
...
@@ -1682,6 +1684,10 @@ void so3LinearTest(bool& error) {
std
::
cout
<<
"SO3Linear: calling derivate with order = 0 should raise an invalid_argument error"
<<
std
::
endl
;
}
catch
(
std
::
invalid_argument
e
)
{
}
SO3Linear_t
so3TrajMatrix
(
q0
.
toRotationMatrix
(),
q1
.
toRotationMatrix
(),
tMin
,
tMax
);
std
::
string
errmsg
(
"SO3Linear built from quaternion or from matrix are not identical."
);
CompareCurves
(
so3Traj
,
so3TrajMatrix
,
errmsg
,
error
,
1e-3
);
}
void
SO3serializationTest
(
bool
&
error
){
...
...
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