diff --git a/include/spline/bezier_curve.h b/include/spline/bezier_curve.h index 6343de60a85962f5258279e3103b7dbcd2707b98..9459a1c93aef56276da397c584adf27f9819e426 100644 --- a/include/spline/bezier_curve.h +++ b/include/spline/bezier_curve.h @@ -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); diff --git a/include/spline/helpers/effector_spline_rotation.h b/include/spline/helpers/effector_spline_rotation.h index d778d0ed455b088ed965ef524292267aa87f3050..f0377e02b247cce670fd3e2c14479c9aa4cbb996 100644 --- a/include/spline/helpers/effector_spline_rotation.h +++ b/include/spline/helpers/effector_spline_rotation.h @@ -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*/ diff --git a/src/tests/spline_test/Main.cpp b/src/tests/spline_test/Main.cpp index c07187a6d5ceade21bf1d0475e607d64a26067cd..0f0a7ff27ee966170b7e60571eb43e2fb64a50d5 100644 --- a/src/tests/spline_test/Main.cpp +++ b/src/tests/spline_test/Main.cpp @@ -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";