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
Jason Chemin
curves
Commits
0c4a333d
Commit
0c4a333d
authored
Jan 27, 2017
by
Steve Tonneau
Browse files
zero vel/acc also for rotations + added copy constructors for spline to avoid pointers
parent
5cba1d27
Changes
6
Hide whitespace changes
Inline
Side-by-side
include/spline/curve_abc.h
View file @
0c4a333d
...
...
@@ -31,10 +31,10 @@ struct curve_abc : std::unary_function<Time, Point>
/* Constructors - destructors */
public:
///\brief Constructor
curve_abc
(){}
;
curve_abc
(){}
///\brief Destructor
~
curve_abc
(){}
;
~
curve_abc
(){}
/* Constructors - destructors */
/*Operations*/
...
...
include/spline/exact_cubic.h
View file @
0c4a333d
...
...
@@ -65,6 +65,10 @@ struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
exact_cubic
(
const
t_spline_t
&
subSplines
)
:
curve_abc_t
(),
subSplines_
(
subSplines
)
{}
///\brief Copy Constructor
exact_cubic
(
const
exact_cubic
&
other
)
:
curve_abc_t
(),
subSplines_
(
other
.
subSplines_
)
{}
///\brief Destructor
~
exact_cubic
(){}
...
...
@@ -144,8 +148,7 @@ struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
return
subSplines
;
}
private:
exact_cubic
(
const
exact_cubic
&
);
private:
exact_cubic
&
operator
=
(
const
exact_cubic
&
);
/* Constructors - destructors */
...
...
include/spline/helpers/effector_spline_rotation.h
View file @
0c4a333d
...
...
@@ -27,10 +27,34 @@ namespace spline
{
namespace
helpers
{
typedef
Eigen
::
Matrix
<
Numeric
,
4
,
1
>
quat_t
;
typedef
Eigen
::
Ref
<
quat_t
>
quat_ref_t
;
typedef
const
Eigen
::
Ref
<
const
quat_t
>
quat_ref_const_t
;
typedef
Eigen
::
Matrix
<
Numeric
,
7
,
1
>
config_t
;
typedef
Eigen
::
Matrix
<
Numeric
,
1
,
1
>
point_one_dim_t
;
typedef
spline_deriv_constraint
<
Numeric
,
Numeric
,
1
,
false
,
point_one_dim_t
>
spline_deriv_constraint_one_dim
;
typedef
std
::
pair
<
Numeric
,
point_one_dim_t
>
waypoint_one_dim_t
;
typedef
std
::
vector
<
waypoint_one_dim_t
>
t_waypoint_one_dim_t
;
class
time_reparametrization_spline
:
public
spline_deriv_constraint_one_dim
{
public:
time_reparametrization_spline
()
:
spline_deriv_constraint_one_dim
(
computeWayPoints
()){}
~
time_reparametrization_spline
(){}
spline_deriv_constraint_one_dim
computeWayPoints
()
const
{
// initializing time reparametrization for spline
t_waypoint_one_dim_t
waypoints
;
waypoints
.
push_back
(
std
::
make_pair
(
0
,
point_one_dim_t
::
Zero
()));
waypoints
.
push_back
(
std
::
make_pair
(
1
,
point_one_dim_t
::
Ones
()));
return
spline_deriv_constraint_one_dim
(
waypoints
.
begin
(),
waypoints
.
end
());
}
};
/// \class effector_spline_rotation
/// \brief Represents a trajectory for and end effector
...
...
@@ -69,6 +93,7 @@ class effector_spline_rotation
,
to_quat_
(
to_quat
.
data
()),
land_quat_
(
land_quat
.
data
())
,
time_lift_offset_
(
spline_
->
min
()
+
lift_offset_duration
)
,
time_land_offset_
(
spline_
->
max
()
-
land_offset_duration
)
,
rotation_spline_
()
{
// NOTHING
}
...
...
@@ -97,6 +122,7 @@ class effector_spline_rotation
return
res
;
}
/// \brief Interpolates between two quaternions
/// \param t : the time when to evaluate the spline
/// \param quat : quaternion updated as the interpolation result
...
...
@@ -107,8 +133,10 @@ class effector_spline_rotation
if
(
t
>=
time_land_offset_
)
return
land_quat_
.
coeffs
();
//normalize u
Numeric
u
=
(
t
-
time_lift_offset_
)
/
(
time_land_offset_
-
time_lift_offset_
);
return
to_quat_
.
slerp
(
u
,
land_quat_
).
coeffs
();
// reparametrize u
return
to_quat_
.
slerp
(
rotation_spline_
(
u
)[
0
],
land_quat_
).
coeffs
();
}
/*Operations*/
/*Attributes*/
...
...
@@ -118,6 +146,7 @@ class effector_spline_rotation
const
Eigen
::
Quaterniond
land_quat_
;
const
double
time_lift_offset_
;
const
double
time_land_offset_
;
const
time_reparametrization_spline
rotation_spline_
;
// should be static
/*Attributes*/
};
...
...
include/spline/spline_curve.h
View file @
0c4a333d
...
...
@@ -95,8 +95,10 @@ struct spline_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
:
coefficients_
(
other
.
coefficients_
),
dim_
(
other
.
dim_
),
order_
(
other
.
order_
),
t_min_
(
other
.
t_min_
),
t_max_
(
other
.
t_max_
){}
private:
//spline_curve& operator=(const spline_curve& other);
private:
void
safe_check
()
{
if
(
Safe
)
...
...
@@ -162,9 +164,9 @@ struct spline_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
/*Attributes*/
public:
coeff_t
coefficients_
;
std
::
size_t
dim_
;
std
::
size_t
order_
;
coeff_t
coefficients_
;
//const
std
::
size_t
dim_
;
//const
std
::
size_t
order_
;
//const
private:
time_t
t_min_
,
t_max_
;
...
...
include/spline/spline_deriv_constraint.h
View file @
0c4a333d
...
...
@@ -79,6 +79,11 @@ struct spline_deriv_constraint : public exact_cubic<Time, Numeric, Dim, Safe, Po
///\brief Destructor
~
spline_deriv_constraint
(){}
///\brief Copy Constructor
spline_deriv_constraint
(
const
spline_deriv_constraint
&
other
)
:
exact_cubic_t
(
other
.
subSplines_
)
{}
private:
template
<
typename
In
>
void
compute_one_spline
(
In
wayPointsBegin
,
In
wayPointsNext
,
spline_constraints
&
constraints
,
t_spline_t
&
subSplines
)
const
...
...
@@ -140,8 +145,7 @@ struct spline_deriv_constraint : public exact_cubic<Time, Numeric, Dim, Safe, Po
return
subSplines
;
}
private:
spline_deriv_constraint
(
const
spline_deriv_constraint
&
);
private:
spline_deriv_constraint
&
operator
=
(
const
spline_deriv_constraint
&
);
/* Constructors - destructors */
};
...
...
src/tests/spline_test/Main.cpp
View file @
0c4a333d
...
...
@@ -474,6 +474,39 @@ void EffectorSplineRotationRotationTest(bool& error)
ComparePoints
(
q_end
,
eff_traj
(
10
),
errmsg
,
error
);
}
void
TestReparametrization
(
bool
&
error
)
{
helpers
::
time_reparametrization_spline
sp
;
if
(
sp
.
min
()
!=
0
)
{
std
::
cout
<<
"in TestReparametrization; min value is not 0, got "
<<
sp
.
min
()
<<
std
::
endl
;
error
=
true
;
}
if
(
sp
.
max
()
!=
1
)
{
std
::
cout
<<
"in TestReparametrization; max value is not 1, got "
<<
sp
.
max
()
<<
std
::
endl
;
error
=
true
;
}
if
(
sp
(
1
)[
0
]
!=
1.
)
{
std
::
cout
<<
"in TestReparametrization; end value is not 1, got "
<<
sp
(
1
)[
0
]
<<
std
::
endl
;
error
=
true
;
}
if
(
sp
(
0
)[
0
]
!=
0.
)
{
std
::
cout
<<
"in TestReparametrization; init value is not 0, got "
<<
sp
(
0
)[
0
]
<<
std
::
endl
;
error
=
true
;
}
for
(
double
i
=
0
;
i
<
1
;
i
+=
0.002
)
{
if
(
sp
(
i
)[
0
]
>
sp
(
i
+
0.002
)[
0
])
{
std
::
cout
<<
"in TestReparametrization; reparametrization not monotonous "
<<
sp
.
max
()
<<
std
::
endl
;
error
=
true
;
}
}
}
int
main
(
int
/*argc*/
,
char
**
/*argv[]*/
)
{
std
::
cout
<<
"performing tests...
\n
"
;
...
...
@@ -487,6 +520,7 @@ int main(int /*argc*/, char** /*argv[]*/)
EffectorTrajectoryTest
(
error
);
EffectorSplineRotationNoRotationTest
(
error
);
EffectorSplineRotationRotationTest
(
error
);
TestReparametrization
(
error
);
BezierCurveTest
(
error
);
if
(
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