Commit c89ae70e authored by Guilhem Saurel's avatar Guilhem Saurel

Merge remote-tracking branch 'main/devel' into devel

parents 7dac08b9 875a0ab4
Pipeline #8335 passed with stage
in 24 minutes and 5 seconds
......@@ -41,7 +41,9 @@ struct Bern {
~Bern() {}
Numeric operator()(const Numeric u) const {
assert(u >= 0. && u <= 1.);
if (!(u >= 0. && u <= 1.)) {
throw std::invalid_argument("u needs to be betwen 0 and 1.");
}
return bin_m_i_ * (pow(u, i_)) * pow((1 - u), m_minus_i);
}
......
......@@ -66,10 +66,12 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
size_(std::distance(PointsBegin, PointsEnd)),
degree_(size_ - 1),
bernstein_(curves::makeBernstein<num_t>((unsigned int)degree_)) {
assert(bernstein_.size() == size_);
if (bernstein_.size() != size_) {
throw std::invalid_argument("Invalid size of polynomial");
}
In it(PointsBegin);
if (Safe && (size_ < 1 || T_max_ <= T_min_)) {
throw std::invalid_argument("can't create bezier min bound is higher than max bound"); // TODO
throw std::invalid_argument("can't create bezier min bound is higher than max bound");
}
for (; it != PointsEnd; ++it) {
control_points_.push_back(*it);
......
......@@ -233,7 +233,9 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Safe, Point> {
//
const Time dt = (t1 - t0);
const Time alpha = (t - t0) / dt;
assert(0. <= alpha && alpha <= 1. && "alpha must be in [0,1]");
if (!(0. <= alpha && alpha <= 1.)) {
throw std::runtime_error("alpha must be in [0,1]");
}
Numeric h00, h10, h01, h11;
evalCoeffs(alpha, h00, h10, h01, h11, degree_derivative);
// std::cout << "for val t="<<t<<" alpha="<<alpha<<" coef : h00="<<h00<<" h10="<<h10<<" h01="<<h01<<"
......
......@@ -37,7 +37,9 @@ typedef exact_cubic_t::spline_t spline_t;
/// \brief Compute time such that the equation from source to offsetpoint is necessarily a line.
Waypoint compute_offset(const Waypoint& source, const Point& normal, const Numeric offset, const Time time_offset) {
Numeric norm = normal.norm();
assert(norm > 0.);
if (norm < 0.) {
throw std::runtime_error("Norm of normal is less than 0!");
}
return std::make_pair(source.first + time_offset, (source.second + normal / norm * offset));
}
......@@ -46,7 +48,9 @@ Waypoint compute_offset(const Waypoint& source, const Point& normal, const Numer
spline_t make_end_spline(const Point& normal, const Point& from, const Numeric offset, const Time init_time,
const Time time_offset) {
Numeric norm = normal.norm();
assert(norm > 0.);
if (norm < 0.) {
throw std::runtime_error("Norm of normal is less than 0!");
}
Point n = normal / norm;
Point d = offset / (time_offset * time_offset * time_offset) * -n;
Point c = -3 * d * time_offset;
......
......@@ -38,7 +38,9 @@ struct SplineOptimizer {
///\brief Initializes optimizer environment.
SplineOptimizer() {
MSKrescodee r_ = MSK_makeenv(&env_, NULL);
assert(r_ == MSK_RES_OK);
if (r_ != MSK_RES_OK) {
throw std::runtime_error("Issue initializing MSK_makeenv");
}
}
///\brief Destructor.
......
......@@ -174,8 +174,12 @@ problem_data<Point, Numeric, Safe> setup_control_points(const problem_definition
// add remaining variables (only if no end_pos constraints)
for (; i < numControlPoints; ++i) variables_.push_back(var_t::Zero(pDef.dim_));
assert(numControlPoints > numConstants);
assert(numControlPoints == variables_.size());
if (numControlPoints <= numConstants) {
throw std::runtime_error("numControlPoints < numConstants");
}
if (numControlPoints != variables_.size()) {
throw std::runtime_error("numControlPoints != variables_.size()");
}
problemData.numControlPoints = numControlPoints;
problemData.numVariables = numControlPoints - numConstants;
......@@ -246,8 +250,12 @@ void initInequalityMatrix(const problem_definition<Point, Numeric>& pDef, proble
// compute sub-bezier curves
T_bezier_t beziers = split<Point, Numeric>(pDef, pData);
assert(pDef.inequalityMatrices_.size() == pDef.inequalityVectors_.size());
assert(pDef.inequalityMatrices_.size() == beziers.size());
if (pDef.inequalityMatrices_.size() != pDef.inequalityVectors_.size()) {
throw std::invalid_argument("The sizes of the inequality matrices and vectors do not match.");
}
if (pDef.inequalityMatrices_.size() != beziers.size()) {
throw std::invalid_argument("The sizes of the inequality matrices and the bezier degree do not match.");
}
long currentRowIdx = 0;
typename problem_definition_t::CIT_matrix_x_t cmit = pDef.inequalityMatrices_.begin();
......@@ -264,7 +272,7 @@ void initInequalityMatrix(const problem_definition<Point, Numeric>& pDef, proble
currentRowIdx += cmit->rows();
}
}
assert(rows == currentRowIdx); // we filled all the constraints
assert(rows == currentRowIdx); // we filled all the constraints - NB: leave assert for Debug tests
}
template <typename Point, typename Numeric, typename In>
......@@ -273,8 +281,9 @@ quadratic_variable<Numeric> bezier_product(In PointsBegin1, In PointsEnd1, In Po
typedef Eigen::Matrix<Numeric, Eigen::Dynamic, 1> vector_x_t;
unsigned int nPoints1 = (unsigned int)(std::distance(PointsBegin1, PointsEnd1)),
nPoints2 = (unsigned int)(std::distance(PointsBegin2, PointsEnd2));
assert(nPoints1 > 0);
assert(nPoints2 > 0);
if (nPoints1 <= 0 || nPoints2 <= 0) {
throw std::runtime_error("This should never happen because an unsigned int cannot go negative without underflowing.");
}
unsigned int deg1 = nPoints1 - 1, deg2 = nPoints2 - 1;
unsigned int newDeg = (deg1 + deg2);
// the integral of the primitive will simply be the last control points of the primitive,
......
......@@ -212,9 +212,9 @@ struct piecewise_curve : public curve_abc<Time, Numeric, Safe, Point, Point_deri
std::size_t num_curves() const { return curves_.size(); }
const curve_ptr_t& curve_at_time(const time_t t) const { return curves_[find_interval(t)]; }
curve_ptr_t curve_at_time(const time_t t) const { return curves_[find_interval(t)]; }
const curve_ptr_t& curve_at_index(const std::size_t idx) const {
curve_ptr_t curve_at_index(const std::size_t idx) const {
if (Safe && idx >= num_curves()) {
throw std::length_error(
"curve_at_index: requested index greater than number of curves in piecewise_curve instance");
......
......@@ -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.");
......
......@@ -35,7 +35,9 @@ struct quadratic_variable {
}
quadratic_variable(const matrix_x_t& A, const point_t& b, const Numeric c = 0) : c_(c), b_(b), A_(A), zero(false) {
assert(A.cols() == b.rows() && A.cols() == A.rows());
if (A.cols() != b.rows() || A.cols() != A.rows()) {
throw std::invalid_argument("The dimensions of A and b are incorrect.");
}
}
quadratic_variable(const point_t& b, const Numeric c = 0)
......@@ -45,7 +47,9 @@ struct quadratic_variable {
// linear evaluation
Numeric operator()(const Eigen::Ref<const point_t>& val) const {
assert(!isZero());
if (isZero()) {
throw std::runtime_error("Not initialized! (isZero)");
}
return val.transpose() * A() * val + b().transpose() * val + c();
}
......@@ -98,15 +102,21 @@ struct quadratic_variable {
}
const matrix_x_t& A() const {
assert(!isZero());
if (isZero()) {
throw std::runtime_error("Not initialized! (isZero)");
}
return A_;
}
const point_t& b() const {
assert(!isZero());
if (isZero()) {
throw std::runtime_error("Not initialized! (isZero)");
}
return b_;
}
const Numeric c() const {
assert(!isZero());
if (isZero()) {
throw std::runtime_error("Not initialized! (isZero)");
}
return c_;
}
bool isZero() const { return zero; }
......
......@@ -25,9 +25,6 @@ struct SE3Curve : public curve_abc<Time, Numeric, Safe, Eigen::Transform<Numeric
typedef Eigen::Transform<Numeric, 3, Eigen::Affine> transform_t;
typedef transform_t point_t;
typedef Eigen::Matrix<Scalar, 6, 1> point_derivate_t;
typedef Eigen::Matrix<Scalar, 3, 1> point3_t;
typedef Eigen::Matrix<Scalar, -1, 1> pointX_t;
typedef Eigen::Matrix<Scalar, 3, 3> matrix3_t;
typedef Eigen::Quaternion<Scalar> Quaternion;
typedef Time time_t;
typedef curve_abc<Time, Numeric, Safe, point_t, point_derivate_t> curve_abc_t; // parent class
......@@ -215,6 +212,10 @@ struct SE3Curve : public curve_abc<Time, Numeric, Safe, Eigen::Transform<Numeric
/// \brief Get the degree of the curve.
/// \return \f$degree\f$, the degree of the curve.
virtual std::size_t degree() const { return translation_curve_->degree(); }
/// \brief const accessor to the translation curve
const curve_ptr_t translation_curve() const {return translation_curve_;}
/// \brief const accessor to the rotation curve
const curve_rotation_ptr_t rotation_curve() const {return rotation_curve_;}
/*Helpers*/
/*Attributes*/
......
......@@ -59,7 +59,9 @@ struct Serializable {
/// \brief Loads a Derived object from an XML file.
template <class Derived>
void loadFromXML(const std::string& filename, const std::string& tag_name) {
assert(!tag_name.empty());
if (tag_name.empty()) {
throw std::invalid_argument("tag_name cannot be empty.");
}
std::ifstream ifs(filename.c_str());
if (ifs) {
boost::archive::xml_iarchive ia(ifs);
......@@ -74,7 +76,9 @@ struct Serializable {
/// \brief Saved a Derived object as an XML file.
template <class Derived>
void saveAsXML(const std::string& filename, const std::string& tag_name) const {
assert(!tag_name.empty());
if (tag_name.empty()) {
throw std::invalid_argument("tag_name cannot be empty.");
}
std::ofstream ofs(filename.c_str());
if (ofs) {
boost::archive::xml_oarchive oa(ofs);
......
......@@ -17,10 +17,8 @@ namespace curves {
///
///
template <typename Time = double, typename Numeric = Time, bool Safe = false>
struct SO3Linear : public curve_abc<Time, Numeric, Safe, Eigen::Matrix<Numeric, 3, 3>, Eigen::Matrix<Numeric, 3, 1> > {
struct SO3Linear : public curve_abc<Time, Numeric, Safe, matrix3_t, point3_t > {
typedef Numeric Scalar;
typedef Eigen::Matrix<Scalar, 3, 1> point3_t;
typedef Eigen::Matrix<Scalar, 3, 3> matrix3_t;
typedef matrix3_t point_t;
typedef point3_t point_derivate_t;
typedef Eigen::Quaternion<Scalar> quaternion_t;
......@@ -40,7 +38,7 @@ struct SO3Linear : public curve_abc<Time, Numeric, Safe, Eigen::Matrix<Numeric,
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();
......@@ -52,7 +50,7 @@ struct SO3Linear : public curve_abc<Time, Numeric, Safe, Eigen::Matrix<Numeric,
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();
......@@ -64,7 +62,7 @@ struct SO3Linear : public curve_abc<Time, Numeric, Safe, Eigen::Matrix<Numeric,
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();
......@@ -76,7 +74,7 @@ struct SO3Linear : public curve_abc<Time, Numeric, Safe, Eigen::Matrix<Numeric,
dim_(3),
init_rot_(quaternion_t(init_rot)),
end_rot_(quaternion_t(end_rot)),
angular_vel_(log3(init_rot.toRotationMatrix().transpose() * end_rot.toRotationMatrix())),
angular_vel_(computeAngularVelocity(init_rot, end_rot, 0., 1.)),
T_min_(0.),
T_max_(1.) {
safe_check();
......@@ -95,12 +93,20 @@ struct SO3Linear : public curve_abc<Time, Numeric, Safe, Eigen::Matrix<Numeric,
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_);
}
......@@ -108,7 +114,7 @@ struct SO3Linear : public curve_abc<Time, Numeric, Safe, Eigen::Matrix<Numeric,
/// \brief Evaluation of the SO3Linear at time t using Eigen slerp.
/// \param t : time when to evaluate the spline.
/// \return \f$x(t)\f$ point corresponding on spline at time t.
virtual matrix3_t operator()(const time_t t) const { return computeAsQuaternion(t).toRotationMatrix(); }
virtual point_t operator()(const time_t t) const { return computeAsQuaternion(t).toRotationMatrix(); }
/**
* @brief isApprox check if other and *this are approximately equals.
......@@ -141,7 +147,7 @@ struct SO3Linear : public curve_abc<Time, Numeric, Safe, Eigen::Matrix<Numeric,
/// \param t : the time when to evaluate the spline.
/// \param order : order of derivative.
/// \return \f$\frac{d^Nx(t)}{dt^N}\f$ point corresponding on derivative spline at time t.
virtual point3_t derivate(const time_t t, const std::size_t order) const {
virtual point_derivate_t derivate(const time_t t, const std::size_t order) const {
if ((t < T_min_ || t > T_max_) && Safe) {
throw std::invalid_argument(
"error in SO3_linear : time t to evaluate derivative should be in range [Tmin, Tmax] of the curve");
......@@ -250,7 +256,9 @@ struct SO3Linear : public curve_abc<Time, Numeric, Safe, Eigen::Matrix<Numeric,
theta = PI_value; // acos((-1-1)/2)
else
theta = acos((tr - Scalar(1)) / Scalar(2));
assert(theta == theta && "theta contains some NaN"); // theta != NaN
if (!std::isfinite(theta)) {
throw std::runtime_error("theta contains some NaN");
}
// From runs of hpp-constraints/tests/logarithm.cc: 1e-6 is too small.
if (theta < PI_value - 1e-2) {
......
......@@ -8,6 +8,15 @@ SET(${PY_NAME}_BINDINGS_SOURCES
namespace.h
)
SET(${PROJECT_NAME}_PYTHON_HEADERS
python_definitions.h
)
INSTALL(FILES
${${PROJECT_NAME}_PYTHON_HEADERS}
DESTINATION include/${PROJECT_NAME}/python
)
ADD_LIBRARY(${PY_NAME} SHARED ${${PY_NAME}_BINDINGS_SOURCES})
SET_TARGET_PROPERTIES(${PY_NAME} PROPERTIES PREFIX "")
TARGET_COMPILE_OPTIONS(${PY_NAME} PRIVATE "-Wno-conversion")
......
This diff is collapsed.
#include "curves/fwd.h"
#include "curves/bezier_curve.h"
#include "curves/polynomial.h"
#include "curves/linear_variable.h"
#include "curves/quadratic_variable.h"
......@@ -25,8 +26,8 @@ typedef std::vector<waypoint_t> t_waypoint_t;
typedef Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic> point_listX_t;
typedef Eigen::Matrix<real, 3, Eigen::Dynamic> point_list3_t;
typedef Eigen::Matrix<real, 6, Eigen::Dynamic> point_list6_t;
typedef polynomial_t::coeff_t coeff_t;
typedef curves::Bern<double> bernstein_t;
template <typename PointList, typename T_Point>
......@@ -45,5 +46,16 @@ T_Point vectorFromEigenVector(const PointList& vector) {
}
return res;
}
template <typename T_point, typename PointList>
PointList vectorToEigenArray(const T_point& vect) {
const size_t nCols = vect.size();
const size_t nRows = vect[0].rows();
PointList res(nRows,nCols);
for (size_t i = 0; i < vect.size(); ++i) {
res.block(0,i,nRows,1) = vect[i];
}
return res;
}
} // namespace curves
#endif //_DEFINITION_PYTHON_BINDINGS
......@@ -5,7 +5,9 @@
namespace curves {
std::vector<linear_variable_t> matrix3DFromEigenArray(const point_list3_t& matrices, const point_list3_t& vectors) {
assert(vectors.cols() * 3 == matrices.cols());
if (vectors.cols() * 3 != matrices.cols()) {
throw std::invalid_argument("vectors.cols() * 3 != matrices.cols()");
}
std::vector<linear_variable_t> res;
for (int i = 0; i < vectors.cols(); ++i) {
res.push_back(linear_variable_t(matrices.block<3, 3>(0, i * 3), vectors.col(i)));
......
......@@ -98,6 +98,14 @@ class TestCurves(unittest.TestCase):
self.assertTrue(norm(a0.derivate(t, 2) - a1.derivate(3 * t, 2) * 9.) < __EPS)
self.assertTrue(norm(prim0(t) - prim1(t * 3) / 3.) < __EPS)
self.assertTrue((prim(0) == array([0., 0., 0.])).all())
# testing accessor to waypoints :
wp_getter = a0.waypoints()
self.assertEqual(wp_getter.shape[0],waypoints.shape[0])
self.assertEqual(wp_getter.shape[1],waypoints.shape[1])
self.assertTrue(array_equal(wp_getter,waypoints))
# check that it return a copy:
a0.waypoints()[1,1] = -15.
self.assertEqual(a0.waypoints()[1,1],waypoints[1,1])
# testing bezier with constraints
c = curve_constraints(3)
c.init_vel = array([[0., 1., 1.]]).transpose()
......@@ -189,6 +197,14 @@ class TestCurves(unittest.TestCase):
self.assertTrue(norm(a0.derivate(t, 2) - a1.derivate(3 * t, 2) * 9.) < __EPS)
self.assertTrue(norm(prim0(t) - prim1(t * 3) / 3.) < __EPS)
self.assertTrue((prim(0) == array([0., 0., 0.])).all())
# testing accessor to waypoints :
wp_getter = a0.waypoints()
self.assertEqual(wp_getter.shape[0],waypoints.shape[0])
self.assertEqual(wp_getter.shape[1],waypoints.shape[1])
self.assertTrue(array_equal(wp_getter,waypoints))
# check that it return a copy:
a0.waypoints()[1,1] = -15.
self.assertEqual(a0.waypoints()[1,1],waypoints[1,1])
# testing bezier with constraints
c = curve_constraints(3)
c.init_vel = array([[0., 1., 1.]]).transpose()
......@@ -344,6 +360,11 @@ class TestCurves(unittest.TestCase):
pc.derivate(0.4, 2)
pc.is_continuous(0)
pc.is_continuous(1)
a0 = pc.curve_at_index(0)
self.assertTrue(array_equal(a0(0.5), pc(0.5)))
self.assertTrue(a0 == a)
a0 = b # should not have any effect
self.assertTrue(array_equal(pc.curve_at_index(0)(0.5), a(0.5)))
# Test serialization
pc.saveAsText("serialization_pc.test")
pc_test = piecewise()
......@@ -397,6 +418,13 @@ class TestCurves(unittest.TestCase):
for i in range(N):
self.assertTrue(isclose(polC0(time_points[i, 0]), points[:, i]).all())
c0 = polC0.curve_at_index(0)
self.assertEqual(c0.min(), time_points[0])
self.assertEqual(c0.max(), time_points[1])
self.assertEqual(c0.dim(), 3)
mid_t = (c0.max() + c0.min()) /2.
self.assertTrue(array_equal(polC0(mid_t), c0(mid_t)))
polC1 = piecewise.FromPointsList(points, points_derivative, time_points)
self.assertEqual(polC1.min(), time_points[0, 0])
self.assertEqual(polC1.max(), time_points[-1, 0])
......@@ -448,6 +476,14 @@ class TestCurves(unittest.TestCase):
pc.derivate(0.4, 2)
pc.is_continuous(0)
pc.is_continuous(1)
# test access to curves :
self.assertTrue(array_equal(pc.curve_at_index(0)(0.5), a(0.5)))
waypoints = array([[3., 4., -3.], [5., 1., 2.]]).transpose()
c = bezier(waypoints, 1.5, 2.)
c0 = pc.curve_at_index(0)
c0 = c # should not have any effect
self.assertTrue(array_equal(pc.curve_at_index(0)(0.5), a(0.5)))
# Test serialization
pc.saveAsText("serialization_pc.test")
pc_test = piecewise()
......@@ -476,6 +512,11 @@ class TestCurves(unittest.TestCase):
pc.derivate(0.4, 2)
pc.is_continuous(0)
pc.is_continuous(1)
a0 = pc.curve_at_index(0)
self.assertTrue(array_equal(a0(0.5), pc(0.5)))
self.assertTrue(a0 == a)
a0 = b # should not have any effect
self.assertTrue(array_equal(pc.curve_at_index(0)(0.5), a(0.5)))
# Test serialization
pc.saveAsText("serialization_pc.test")
pc_test = piecewise()
......@@ -897,6 +938,18 @@ class TestCurves(unittest.TestCase):
self.assertTrue(isclose(d, se3.derivate(max, 1)).all())
self.assertTrue(isclose(se3.derivate(min, 2), zeros((6, 1))).all())
self.assertTrue(isclose(se3.derivate(min, 3), zeros((6, 1))).all())
# test accessor to translation_curve :
tr_se3 = se3.translation_curve()
self.assertTrue(array_equal(tr_se3((max+min)/2.), se3.translation((max+min)/2.)))
# test accessor to rotation :
rot_se3 = se3.rotation_curve()
rot_se3((max+min)/2.)
self.assertTrue(isclose(rot_se3((max+min)/2.),(se3.rotation((max+min)/2.))).all())
# check that it return a CONST reference :
waypoints2 = array([[1., -2., 3.5], [5.6, 5., -6.], [4.,1.2, 0.5]]).transpose()
tr_se3 = bezier3(waypoints2, min, max) # should not have any effect
self.assertFalse(array_equal(tr_se3((max+min)/2.), se3.translation((max+min)/2.)))
# check that errors are correctly raised when necessary :
with self.assertRaises(ValueError):
se3(0.)
......@@ -949,6 +1002,21 @@ class TestCurves(unittest.TestCase):
self.assertTrue(isclose(se3.derivate(t, 1)[0:3], translation.derivate(t, 1)).all())
t += 0.02
# test accessor to translation_curve :
tr_se3 = se3.translation_curve()
self.assertTrue(tr_se3 == translation)
self.assertTrue(array_equal(tr_se3((max+min)/2.), se3.translation((max+min)/2.)))
# test accessor to rotation :
rot_se3 = se3.rotation_curve()
rot_se3((max+min)/2.)
se3.rotation((max+min)/2.)
self.assertTrue(isclose(rot_se3((max+min)/2.), se3.rotation((max+min)/2.)).all())
# check that it return a CONST reference :
waypoints2 = array([[1., -2., 3.5], [5.6, 5., -6.], [4.,1.2, 0.5]]).transpose()
tr_se3 = bezier3(waypoints2, min, max) # should not have any effect
self.assertFalse(array_equal(tr_se3((max+min)/2.), se3.translation((max+min)/2.)))
# test with bezier3
translation = bezier3(waypoints, min, max)
se3 = SE3Curve(translation, init_rot, end_rot)
......@@ -1051,6 +1119,20 @@ class TestCurves(unittest.TestCase):
self.assertTrue(isclose(se3.derivate(t, 1)[3:6], rotation.derivate(t, 1)).all())
t += 0.02
# test accessor to translation_curve :
tr_se3 = se3.translation_curve()
self.assertTrue(tr_se3 == translation)
self.assertTrue(array_equal(tr_se3((max+min)/2.), se3.translation((max+min)/2.)))
# test accessor to rotation :
rot_se3 = se3.rotation_curve()
rot_se3((max+min)/2.)
se3.rotation((max+min)/2.)
self.assertTrue(array_equal(rot_se3((max+min)/2.), se3.rotation((max+min)/2.)))
# check that it return a CONST reference :
waypoints2 = array([[1., -2., 3.5], [5.6, 5., -6.], [4.,1.2, 0.5]]).transpose()
tr_se3 = bezier3(waypoints2, min, max) # should not have any effect
self.assertFalse(array_equal(tr_se3((max+min)/2.), se3.translation((max+min)/2.)))
# check if errors are correctly raised :
rotation = SO3Linear(init_rot, end_rot, min + 0.2, max)
with self.assertRaises(ValueError):
......
......@@ -1855,6 +1855,36 @@ void se3CurveTest(bool& error) {
std::cout << "SE3 curve : second order derivative for rotation should be null" << std::endl;
}
// check accessor to translation curves :
curve_ptr_t translation = cBezier.translation_curve();
if (translation->operator()(min) != cBezier(min).translation()) {
error = true;
std::cout << "SE3 curve : translation curve not equal to se3.translation" << std::endl;
}
if (translation->operator()(max) != cBezier(max).translation()) {
error = true;
std::cout << "SE3 curve : translation curve not equal to se3.translation" << std::endl;
}
if (translation->operator()((max+min)/2.) != cBezier((max+min)/2.).translation()) {
error = true;
std::cout << "SE3 curve : translation curve not equal to se3.translation" << std::endl;
}
// check accessor to rotation curves :
curve_rotation_ptr_t rotation = cBezier.rotation_curve();
if (! rotation->operator()(min).isApprox(cBezier(min).rotation())) {
error = true;
std::cout << "SE3 curve : rotation curve not equal to se3.rotation" << std::endl;
}
if (! rotation->operator()(max).isApprox(cBezier(max).rotation())) {
error = true;
std::cout << "SE3 curve : rotation curve not equal to se3.rotation" << std::endl;
}
if (! rotation->operator()((max+min)/2.).isApprox(cBezier((max+min)/2.).rotation())) {
error = true;
std::cout << "SE3 curve : rotation curve not equal to se3.rotation" << std::endl;
}
// check if errors are correctly raised
try {
cBezier(0.1);
......
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