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
Pierre Fernbach
curves
Commits
2ada9cb7
Commit
2ada9cb7
authored
Feb 06, 2020
by
Pierre Fernbach
Browse files
[SO3Linear fix the computation of the angular velocity when t_min == t_max
parent
41f0d266
Changes
1
Hide whitespace changes
Inline
Side-by-side
include/curves/so3_linear.h
View file @
2ada9cb7
...
...
@@ -38,7 +38,7 @@ struct SO3Linear : public curve_abc<Time, Numeric, Safe, matrix3_t, point3_t > {
dim_
(
3
),
init_rot_
(
init_rot
),
end_rot_
(
end_rot
),
angular_vel_
(
log3
(
init_rot
.
toRotationMatrix
()
.
transpose
()
*
end_rot
.
toRotationMatrix
()
)
/
(
t_max
-
t_m
in
)),
angular_vel_
(
computeAngularVelocity
(
init_rot
.
toRotationMatrix
()
,
end_rot
.
toRotationMatrix
()
,
t_min
,
t_m
ax
)),
T_min_
(
t_min
),
T_max_
(
t_max
)
{
safe_check
();
...
...
@@ -50,7 +50,7 @@ struct SO3Linear : public curve_abc<Time, Numeric, Safe, matrix3_t, point3_t > {
dim_
(
3
),
init_rot_
(
quaternion_t
(
init_rot
)),
end_rot_
(
quaternion_t
(
end_rot
)),
angular_vel_
(
log3
(
init_rot
.
transpose
()
*
end_rot
)
/
(
t_max
-
t_m
in
)),
angular_vel_
(
computeAngularVelocity
(
init_rot
,
end_rot
,
t_min
,
t_m
ax
)),
T_min_
(
t_min
),
T_max_
(
t_max
)
{
safe_check
();
...
...
@@ -62,7 +62,7 @@ struct SO3Linear : public curve_abc<Time, Numeric, Safe, matrix3_t, point3_t > {
dim_
(
3
),
init_rot_
(
init_rot
),
end_rot_
(
end_rot
),
angular_vel_
(
log3
(
init_rot
.
toRotationMatrix
()
.
transpose
()
*
end_rot
.
toRotationMatrix
())),
angular_vel_
(
computeAngularVelocity
(
init_rot
.
toRotationMatrix
()
,
end_rot
.
toRotationMatrix
()
,
0.
,
1.
)),
T_min_
(
0.
),
T_max_
(
1.
)
{
safe_check
();
...
...
@@ -74,7 +74,7 @@ struct SO3Linear : public curve_abc<Time, Numeric, Safe, matrix3_t, point3_t > {
dim_
(
3
),
init_rot_
(
quaternion_t
(
init_rot
)),
end_rot_
(
quaternion_t
(
end_rot
)),
angular_vel_
(
log3
(
init_rot
.
transpose
()
*
end_rot
)),
angular_vel_
(
computeAngularVelocity
(
init_rot
,
end_rot
,
0.
,
1.
)),
T_min_
(
0.
),
T_max_
(
1.
)
{
safe_check
();
...
...
@@ -93,6 +93,14 @@ struct SO3Linear : public curve_abc<Time, Numeric, Safe, matrix3_t, point3_t > {
T_min_
(
other
.
T_min_
),
T_max_
(
other
.
T_max_
)
{}
point3_t
computeAngularVelocity
(
const
matrix3_t
&
init_rot
,
const
matrix3_t
&
end_rot
,
const
double
t_min
,
const
double
t_max
){
if
(
t_min
==
t_max
){
return
point3_t
::
Zero
();
}
else
{
return
log3
(
init_rot
.
transpose
()
*
end_rot
)
/
(
t_max
-
t_min
);
}
}
quaternion_t
computeAsQuaternion
(
const
time_t
t
)
const
{
if
(
Safe
&
!
(
T_min_
<=
t
&&
t
<=
T_max_
))
{
throw
std
::
invalid_argument
(
"can't evaluate bezier curve, time t is out of range"
);
// TODO
...
...
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