Commit f5d7beae authored by Steve Tonneau's avatar Steve Tonneau

finished effector_rotation_spline

parent c0022298
......@@ -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);
......
......@@ -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*/
......
......@@ -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";
......
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