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
f5d7beae
Commit
f5d7beae
authored
Jan 27, 2017
by
Steve Tonneau
Browse files
finished effector_rotation_spline
parent
c0022298
Changes
3
Hide whitespace changes
Inline
Side-by-side
include/spline/bezier_curve.h
View file @
f5d7beae
...
...
@@ -43,7 +43,7 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
In
it
(
PointsBegin
);
if
(
Safe
&&
(
size_
<=
1
||
minBound
==
maxBound
))
{
throw
;
// TODO
throw
std
::
out_of_range
(
"TODO"
)
;
// TODO
}
for
(;
it
!=
PointsEnd
;
++
it
)
{
...
...
@@ -72,8 +72,8 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
num_t
nT
=
(
t
-
minBound_
)
/
(
maxBound_
-
minBound_
);
if
(
Safe
&!
(
0
<=
nT
&&
nT
<=
1
))
{
//throw
; // TODO
}
throw
std
::
out_of_range
(
"TODO"
)
;
// TODO
}
else
{
num_t
dt
=
(
1
-
nT
);
...
...
include/spline/helpers/effector_spline_rotation.h
View file @
f5d7beae
...
...
@@ -101,13 +101,13 @@ class effector_spline_rotation
/// \param t : the time when to evaluate the spline
/// \param quat : quaternion updated as the interpolation result
///
quat_t
interpolate_quat
(
time_t
t
)
const
quat_t
interpolate_quat
(
Numeric
t
)
const
{
if
(
t
<=
time_lift_offset_
)
return
quat_t
(
to_quat_
.
coeffs
()
.
data
())
;
if
(
t
>=
time_land_offset_
)
return
quat_t
(
land_quat_
.
coeffs
()
.
data
())
;
if
(
t
<=
time_lift_offset_
)
return
to_quat_
.
coeffs
();
if
(
t
>=
time_land_offset_
)
return
land_quat_
.
coeffs
();
//normalize u
Numeric
u
=
(
t
-
time_lift_offset_
)
/
(
time_land_offset_
-
time_lift_offset_
);
return
quat_t
(
to_quat_
.
slerp
(
u
,
land_quat_
).
coeffs
()
.
data
())
;
return
to_quat_
.
slerp
(
u
,
land_quat_
).
coeffs
();
}
/*Operations*/
...
...
src/tests/spline_test/Main.cpp
View file @
f5d7beae
...
...
@@ -451,19 +451,43 @@ void EffectorSplineRotationNoRotationTest(bool& error)
ComparePoints
(
q_end
,
eff_traj
(
10
),
errmsg
,
error
);
}
void
EffectorSplineRotationRotationTest
(
bool
&
error
)
{
// create arbitrary trajectory
spline
::
T_Waypoint
waypoints
;
for
(
double
i
=
0
;
i
<=
10
;
i
=
i
+
2
)
{
waypoints
.
push_back
(
std
::
make_pair
(
i
,
point_t
(
i
,
i
,
i
)));
}
helpers
::
quat_t
init_quat
=
GetXRotQuat
(
M_PI
);
helpers
::
effector_spline_rotation
eff_traj
(
waypoints
.
begin
(),
waypoints
.
end
(),
init_quat
);
helpers
::
config_t
q_init
=
helpers
::
config_t
::
Zero
();
q_init
.
tail
<
4
>
()
=
init_quat
;
helpers
::
config_t
q_end
;
q_end
<<
10.
,
10.
,
10.
,
0.
,
0.
,
0.
,
1.
;
helpers
::
config_t
q_to
=
q_init
;
q_to
(
2
)
+=
0.02
;
helpers
::
config_t
q_land
=
q_end
;
q_land
(
2
)
+=
0.02
;
helpers
::
quat_t
q_mod
=
GetXRotQuat
(
M_PI_2
);;
std
::
string
errmsg
(
"Error in EffectorSplineRotationNoRotationTest; while checking waypoints (expected / obtained)"
);
ComparePoints
(
q_init
,
eff_traj
(
0
),
errmsg
,
error
);
ComparePoints
(
q_to
,
eff_traj
(
0.02
),
errmsg
,
error
);
ComparePoints
(
q_land
,
eff_traj
(
9.98
),
errmsg
,
error
);
ComparePoints
(
q_mod
,
eff_traj
(
5
).
tail
<
4
>
(),
errmsg
,
error
);
ComparePoints
(
q_end
,
eff_traj
(
10
),
errmsg
,
error
);
}
int
main
(
int
/*argc*/
,
char
**
/*argv[]*/
)
{
std
::
cout
<<
"performing tests...
\n
"
;
bool
error
=
false
;
std
::
cout
<<
"performing tests...
\n
"
;
bool
error
=
false
;
CubicFunctionTest
(
error
);
ExactCubicNoErrorTest
(
error
);
ExactCubicPointsCrossedTest
(
error
);
// checks that given wayPoints are crossed
ExactCubicTwoPointsTest
(
error
);
ExactCubicNoErrorTest
(
error
);
ExactCubicPointsCrossedTest
(
error
);
// checks that given wayPoints are crossed
ExactCubicTwoPointsTest
(
error
);
ExactCubicOneDimTest
(
error
);
ExactCubicVelocityConstraintsTest
(
error
);
EffectorTrajectoryTest
(
error
);
EffectorSplineRotationNoRotationTest
(
error
);
//BezierCurveTest(error);
EffectorSplineRotationRotationTest
(
error
);
BezierCurveTest
(
error
);
if
(
error
)
{
std
::
cout
<<
"There were some errors
\n
"
;
...
...
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