diff --git a/tesseract/tesseract_common/include/tesseract_common/interpolators.h b/tesseract/tesseract_common/include/tesseract_common/interpolators.h new file mode 100644 index 00000000000..b0958aaf6f3 --- /dev/null +++ b/tesseract/tesseract_common/include/tesseract_common/interpolators.h @@ -0,0 +1,169 @@ +/** + * @file utils.h + * @brief Common Tesseract Utilities for Interpolating + * + * @date January 7, 2020 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2019, Southwest Research Institute + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +#ifndef TESSERACT_COMMON_INTERPOLATORS_H +#define TESSERACT_COMMON_INTERPOLATORS_H + +#include +#include +TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include +#include +TESSERACT_COMMON_IGNORE_WARNINGS_POP + +namespace tesseract_common +{ +/** @brief Uses Eigen/Splines to fit a 3rd order B-Spline to the input data and provides an operator for retrieving + * intermediate points + * + * Note: B-Spline may be lower order than requested for short input vectors */ +class SplineFunction +{ +public: + /** + * @brief SplineFunction Constructor for interpolating while applying restrictions on the derivates of some points + * + * Example: Start/end with 0 velocity, + * Derivatives is a VectorXd of length 2 filled with zeros; + * Indices is then a VectorXi of length 2 filled with 0 and x_vec.size()-1 + * + * **Note: There is a known bug in Eigen as of 1/6/2020. This will give incorrect results unless you patch + * SplineFitting.h.** See stackoverflow.com/questions/48382939 and https://gitlab.com/libeigen/eigen/merge_requests/41 + * @param x_vec Independent variable. Probably time for most motion planning problems + * @param y_vec Dependent variable. Probably joint values for most motion planning problems. + * @param derivatives This is a vector of derivative values. + * @param indices This is a vector of indices of where those derivatives are applied + */ + SplineFunction(const Eigen::Ref& x_vec, + const Eigen::Ref& y_vec, + const Eigen::Ref& derivatives, + const Eigen::Ref& indices) + : x_min_(x_vec.minCoeff()) + , x_max_(x_vec.maxCoeff()) + , + // Scale x values to [0:1] and curve fit with a 3rd order B-Spline. + spline_(Eigen::SplineFitting>::InterpolateWithDerivatives( + y_vec.transpose(), + derivatives.transpose(), + indices, + std::min(static_cast(x_vec.rows()) - 1, 3), + scaledValues(x_vec))) + { + assert(derivatives.size() == indices.size()); + assert(!(x_vec.array().isNaN().any())); + assert(!(x_vec.array().isInf().any())); + assert(!(y_vec.array().isNaN().any())); + assert(!(y_vec.array().isInf().any())); + } + + /** + * @brief Constructor for interpolating with no restrictions on derivatives + * @param x_vec Independent variable. Probably time for most motion planning problems + * @param y_vec Dependent variable. Probably joint values for most motion planning problems. + */ + SplineFunction(const Eigen::Ref& x_vec, const Eigen::Ref& y_vec) + : x_min_(x_vec.minCoeff()) + , x_max_(x_vec.maxCoeff()) + , + // Scale x values to [0:1] and curve fit with a 3rd order B-Spline. + spline_(Eigen::SplineFitting>::Interpolate(y_vec.transpose(), + std::min(x_vec.rows() - 1, 3), + scaledValues(x_vec))) + { + assert(!(x_vec.array().isNaN().any())); + assert(!(x_vec.array().isInf().any())); + assert(!(y_vec.array().isNaN().any())); + assert(!(y_vec.array().isInf().any())); + } + + /** @brief Returns the y value at point x in the spline */ + double operator()(double x) const + { + // x values need to be scaled to [0:1] in extraction as well. + return spline_(scaledValue(x))(0); + } + + /** @brief Returns a vector of interpolated y values for each x in the input vector */ + Eigen::VectorXd operator()(const Eigen::Ref& x_vec) const + { + return x_vec.unaryExpr([this](double x) { return spline_(scaledValue(x))(0); }); + } + +private: + /** @brief Scales value to [0:1] based on x_min and x_max */ + double scaledValue(double x) const { return (x - x_min_) / (x_max_ - x_min_); } + + /** @brief Scales vector such that each value is [0:1] based on x_min and x_max + * + * It is a requirement for Interpolate that x be on the interval [0:1] */ + Eigen::RowVectorXd scaledValues(const Eigen::Ref& x_vec) const + { + return x_vec.unaryExpr([this](double x) { return scaledValue(x); }).transpose(); + } + + /** @brief Minimum value in x_vec. Used to scale inputs to [0:1] */ + double x_min_; + /** @brief Maximum value in x_vec. Used to scale inputs to [0:1] */ + double x_max_; + + /** @brief One dimensional Spline that is used to interpolate the data + * + * Note: operator(double) returns an array of points. In this case the 0th element is the y value*/ + Eigen::Spline spline_; +}; + +/** + * @brief Interpolates each column in a TrajArray using a cubic spline. The resuls are an evenly spaced trajectory with + * result_length size. + * + * Note: While the spline will hit these points, the resulting TrajArray is not guaranteed to include the original + * points unless result_length is of size n * (input_traj.rows() - 1) + 1 + * @param input_traj Input TrajArray. Each column will be interpolated with a cubic spline. + * @param result_length Number of rows in the resulting TrajArray. For best results this should be size n * + * (input_traj.rows() - 1) + 1 + * @return Resulting TrajArray of size results_length x input_traj.cols() + */ +inline TrajArray interpolateCubicSpline(const Eigen::Ref& input_traj, const int& result_length) +{ + TrajArray results(result_length, input_traj.cols()); + for (long ind = 0; ind < input_traj.cols(); ind++) + { + // Fit the spline to the input data + Eigen::VectorXd t_in = + Eigen::VectorXd::LinSpaced(input_traj.rows(), 0.0, static_cast(input_traj.rows() - 1)); + Eigen::VectorXd y_in = input_traj.col(ind); + SplineFunction spline(t_in, y_in); + + // Extract evenly spaced points from spline + Eigen::VectorXd t_out = Eigen::VectorXd::LinSpaced(result_length, 0.0, static_cast(input_traj.rows() - 1)); + Eigen::VectorXd y_out = spline(t_out); + results.col(ind) = y_out; + } + + return results; +} + +} // namespace tesseract_common + +#endif diff --git a/tesseract/tesseract_common/test/tesseract_common_unit.cpp b/tesseract/tesseract_common/test/tesseract_common_unit.cpp index 0dbf987356b..5c03d79306b 100644 --- a/tesseract/tesseract_common/test/tesseract_common_unit.cpp +++ b/tesseract/tesseract_common/test/tesseract_common_unit.cpp @@ -4,6 +4,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH TESSERACT_COMMON_IGNORE_WARNINGS_POP #include +#include TEST(TesseractCommonUnit, isNumeric) // NOLINT { @@ -49,6 +50,96 @@ TEST(TesseractCommonUnit, toNumeric) // NOLINT } } +TEST(TesseractCommonUnit, splineFunctionNoDerivatives) // NOLINT +{ + Eigen::VectorXd xvals(10); + Eigen::VectorXd yvals(xvals.rows()); + + xvals << 0, 1, 2, 3, 4, 5, 6, 7, 8, 9; + yvals = xvals.array().square(); + + // Fit the spline + tesseract_common::SplineFunction spline(xvals, yvals); + + // Check that input knots are hit exactly + for (long ind = 0; ind < xvals.size(); ind++) + EXPECT_NEAR(spline(xvals[ind]), yvals[ind], 1e-8); + + Eigen::VectorXd xvals_interp(8); + xvals_interp << 1.5, 2.5, 3.5, 4.5, 5.5, 6.5, 7.5, 8.5; + // Check that the intermediate points are within 1% for a quadratic (they should be really close) + for (long ind = 0; ind < xvals_interp.size(); ind++) + { + double gt = xvals_interp[ind] * xvals_interp[ind]; + EXPECT_NEAR(spline(xvals_interp[ind]), gt, gt * 0.01 + 1e-8); + } +} + +TEST(TesseractCommonUnit, splineFunctionWithDerivatives) // NOLINT +{ + Eigen::VectorXd xvals(10); + Eigen::VectorXd yvals(xvals.rows()); + + xvals << 0, 1, 2, 3, 4, 5, 6, 7, 8, 9; + yvals = xvals.array().cube(); + + Eigen::VectorXd derivatives(2); + derivatives << 0., 0.; + Eigen::VectorXi indices(2); + indices << 0, static_cast(xvals.size() - 1); + + // Fit the spline + tesseract_common::SplineFunction spline(xvals, yvals, derivatives, indices); + + // Check that input knots are hit exactly + for (long ind = 0; ind < xvals.size(); ind++) + EXPECT_NEAR(spline(xvals[ind]), yvals[ind], 1e-8); + + // Note: Interpolating using derivatives is not tested here because it requires patches to Eigen. See note in class + // documentation +} + +TEST(TesseractCommonUnit, interpolateCubicSpline) // NOLINT +{ + tesseract_common::TrajArray input_traj(10, 5); + Eigen::VectorXd t_in = Eigen::VectorXd::LinSpaced(input_traj.rows(), 0.0, static_cast(input_traj.rows() - 1)); + input_traj.col(0) = t_in.array().square(); + input_traj.col(1) = t_in.array().cube(); + input_traj.col(2) = t_in.array().sin(); + input_traj.col(3) = t_in.array().cos(); + input_traj.col(4) = t_in.array().exp(); + std::cout << "Input Trajectory: \n" << input_traj << std::endl; + + const int result_length = 46; + tesseract_common::TrajArray results = tesseract_common::interpolateCubicSpline(input_traj, result_length); + std::cout << "Spline Results: \n" << results << std::endl; + + EXPECT_EQ(results.rows(), result_length); + EXPECT_EQ(results.cols(), input_traj.cols()); + + // Check that all of the knots are hit exactly + for (long ind = 0; ind < input_traj.rows(); ind++) + { + EXPECT_NEAR(input_traj.col(0)[ind], results.col(0)[ind * 5], 1e-8); + EXPECT_NEAR(input_traj.col(1)[ind], results.col(1)[ind * 5], 1e-8); + EXPECT_NEAR(input_traj.col(2)[ind], results.col(2)[ind * 5], 1e-8); + EXPECT_NEAR(input_traj.col(3)[ind], results.col(3)[ind * 5], 1e-8); + EXPECT_NEAR(input_traj.col(4)[ind], results.col(4)[ind * 5], 1e-8); + } + + // Check that the intermediate points are within a small percentage. The polynomials should be really close + Eigen::VectorXd t_interp = + Eigen::VectorXd::LinSpaced(results.rows(), 0.0, static_cast(input_traj.rows() - 1)); + for (long ind = 0; ind < results.rows(); ind++) + { + EXPECT_NEAR(t_interp.array().square()[ind], results.col(0)[ind], t_interp.array().square()[ind] * 0.01 + 1e-8); + EXPECT_NEAR(t_interp.array().cube()[ind], results.col(1)[ind], t_interp.array().cube()[ind] * 0.01 + 1e-8); + EXPECT_NEAR(t_interp.array().sin()[ind], results.col(2)[ind], 1.0 * 0.05 + 1e-8); + EXPECT_NEAR(t_interp.array().cos()[ind], results.col(3)[ind], 1.0 * 0.05 + 1e-8); + EXPECT_NEAR(t_interp.array().exp()[ind], results.col(4)[ind], t_interp.array().exp()[ind] * 0.10 + 1e-8); + } +} + int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv);