Unverified Commit 7884ae26 authored by Fernbach Pierre's avatar Fernbach Pierre Committed by GitHub
Browse files

Merge pull request #32 from pFernbach/devel

Fix when t_min == t_max
parents 8d201862 7f8ae09c
......@@ -106,6 +106,7 @@ struct polynomial : public curve_abc<Time, Numeric, Safe, Point> {
///
polynomial(const Point& init, const Point& end, const time_t min, const time_t max)
: dim_(init.size()), degree_(1), T_min_(min), T_max_(max) {
if (T_min_ >= T_max_) throw std::invalid_argument("T_min must be strictly lower than T_max");
if (init.size() != end.size()) throw std::invalid_argument("init and end points must have the same dimensions.");
t_point_t coeffs;
coeffs.push_back(init);
......@@ -127,6 +128,7 @@ struct polynomial : public curve_abc<Time, Numeric, Safe, Point> {
polynomial(const Point& init, const Point& d_init, const Point& end, const Point& d_end, const time_t min,
const time_t max)
: dim_(init.size()), degree_(3), T_min_(min), T_max_(max) {
if (T_min_ >= T_max_) throw std::invalid_argument("T_min must be strictly lower than T_max");
if (init.size() != end.size()) throw std::invalid_argument("init and end points must have the same dimensions.");
if (init.size() != d_init.size())
throw std::invalid_argument("init and d_init points must have the same dimensions.");
......@@ -170,6 +172,7 @@ struct polynomial : public curve_abc<Time, Numeric, Safe, Point> {
polynomial(const Point& init, const Point& d_init, const Point& dd_init, const Point& end, const Point& d_end,
const Point& dd_end, const time_t min, const time_t max)
: dim_(init.size()), degree_(5), T_min_(min), T_max_(max) {
if (T_min_ >= T_max_) throw std::invalid_argument("T_min must be strictly lower than T_max");
if (init.size() != end.size()) throw std::invalid_argument("init and end points must have the same dimensions.");
if (init.size() != d_init.size())
throw std::invalid_argument("init and d_init points must have the same dimensions.");
......
......@@ -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_min)),
angular_vel_(computeAngularVelocity(init_rot.toRotationMatrix(), end_rot.toRotationMatrix(), t_min, t_max)),
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_min)),
angular_vel_(computeAngularVelocity(init_rot, end_rot, t_min, t_max)),
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,12 +93,20 @@ 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
}
if (t > T_max_) return end_rot_;
if (t < T_min_) return init_rot_;
if (t >= T_max_) return end_rot_;
if (t <= T_min_) return init_rot_;
Scalar u = (t - T_min_) / (T_max_ - T_min_);
return init_rot_.slerp(u, end_rot_);
}
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment