SplineTrajectory fits piecewise splines to target positions by minimizing a specified cost function via convex optimization. The constructor sets up all the memory needed for running the optimization in the fit function and querying new points using the evaluate function. More...
Declaration
class SplineTrajectory { ... }
#include <spline_trajectory.hpp>
Public Constructors Index
| SplineTrajectory (size_t num_points, size_t num_coefficients) |
|
The constructor initializes all memory needed for the optimization and evaluation. More...
|
|
Public Member Functions Index
| void | fit (const moveit_pro_controllers::Trajectory &points, const SplineOptimizationParameters &optimization_parameters, double scaled_time_base=1.05) |
|
Fits the piecewise spline to the given trajectory points. More...
|
|
| const moveit_pro_controllers::TimedJointState & | evaluate (double ti) |
|
Evaluates the spline at a given time point. More...
|
|
| const std::vector< double > & | getKnots () const |
|
Returns the vector of knot time points used in the spline trajectory. More...
|
|
Private Member Functions Index
| void | enforceConstraints (const moveit_pro_controllers::Trajectory &points, const SplineOptimizationParameters &optimization_parameters, const double scaled_time_base) |
|
|
|
| void | reparameterizeWithConstraints (const SplineOptimizationParameters &optimization_parameters, double time_scale) |
|
|
|
| double | calculateScaleTimeForConstraints (const SplineOptimizationParameters &optimization_parameters) |
|
|
|
| double | estimateConstraintScaleFactor (double ti, const Eigen::VectorNd &velocity_limits, const Eigen::VectorNd &acceleration_limits) |
|
|
|
| void | validateInputs (const moveit_pro_controllers::Trajectory &points, const SplineOptimizationParameters &optimization_parameters) const |
|
|
|
| void | setupKnots (const moveit_pro_controllers::Trajectory &points) |
|
|
|
| void | setupQueryPoint (const moveit_pro_controllers::Trajectory &points) |
|
|
|
| void | setupCostMatrix (const std::vector< double > &knots, const SplineOptimizationParameters &optimization_parameters) |
|
|
|
| void | setupConstraintMatrixVelocityAcceleration (const std::vector< double > &knots) |
|
|
|
| void | setupConstraintMatrixPosition (const moveit_pro_controllers::Trajectory &points, const SplineOptimizationParameters &optimization_parameters, Eigen::Index dim) |
|
|
|
| std::pair< Eigen::Index, double > | positionPenaltyAnchor (Eigen::Index point_idx) const |
|
|
|
| void | setupBoundaryConstraints (SplineOptimizationParameters optimization_parameters, Eigen::Index dim) |
|
|
|
| void | setupAndRunOptimization (const moveit_pro_controllers::Trajectory &points, const SplineOptimizationParameters &optimization_parameters) |
|
|
|
| void | solveOptimization (const SplineOptimizationParameters &optimization_parameters, Eigen::Index dim) |
|
|
|
| bool | factorizationCacheValid (const SplineOptimizationParameters &optimization_parameters) const |
|
|
|
| void | updateFactorizationCache (const SplineOptimizationParameters &optimization_parameters) |
|
|
|
| void | setPositionBasis (double delta_ti, Eigen::Index segment_index, Eigen::Ref< Eigen::MatrixXd > block) const |
|
|
|
| void | setVelocityBasis (double delta_ti, Eigen::Index segment_index, Eigen::Ref< Eigen::MatrixXd > block) const |
|
|
|
| void | setAccelerationBasis (double delta_ti, Eigen::Index segment_index, Eigen::Ref< Eigen::MatrixXd > block) const |
|
|
|
| void | setJerkBasis (double delta_ti, Eigen::Index segment_index, Eigen::Ref< Eigen::MatrixXd > block) const |
|
|
|
| void | setCostBlock (double delta_ti, Eigen::Index segment_index, int derivative_order, const SplineOptimizationParameters &optimization_parameters, Eigen::Ref< Eigen::MatrixXd > block) const |
|
|
|
Private Member Attributes Index
Description
SplineTrajectory fits piecewise splines to target positions by minimizing a specified cost function via convex optimization. The constructor sets up all the memory needed for running the optimization in the fit function and querying new points using the evaluate function.
The cost function to minimize is the $\int (\dot{x}^2 + \ddot{x}^2 + \dddot{x}^2) \, dt$, where $x = \sum_{i=0}^{\text{num\_coefficients}} x_i \cdot t^i$.
Definition at line 101 of file spline_trajectory.hpp.
Public Constructors
SplineTrajectory()
| SplineTrajectory::SplineTrajectory (size_t num_points, size_t num_coefficients) |
|
The constructor initializes all memory needed for the optimization and evaluation.
- Parameters
-
| num_points | The number of points to fit the spline to. |
| num_coefficients | The number of polynomial terms to use per piecewise spline. |
Declaration at line 109 of file spline_trajectory.hpp, definition at line 17 of file spline_trajectory.cpp.
Public Member Functions
evaluate()
| const moveit_pro_controllers::TimedJointState & SplineTrajectory::evaluate (double ti) |
|
Evaluates the spline at a given time point.
- Parameters
-
| ti | The time from start at which to evaluate the spline. |
- Returns
The joint state (position, velocity, acceleration) at the specified time.
Declaration at line 138 of file spline_trajectory.hpp, definition at line 581 of file spline_trajectory.cpp.
fit()
| void SplineTrajectory::fit (const moveit_pro_controllers::Trajectory & points, const SplineOptimizationParameters & optimization_parameters, double scaled_time_base=1.05) |
|
Fits the piecewise spline to the given trajectory points.
- Exceptions
-
| std::runtime_error | Thrown if:
- The provided points have zero dimensions.
- The number of dimensions in the provided points does not match the dimensions specified in the spline optimization parameters.
- The velocity limits must all be positive and match the number of dimensions in the points.
- The acceleration limits must all be positive and match the number of dimensions in the points.
- The velocity limits, if provided, must each be greater than the corresponding start and end velocity for their dimension.
- The acceleration limits, if provided, must each be greater than the corresponding start and end acceleration for their dimension.
|
- Parameters
-
| points | The trajectory points to fit the spline to. |
| optimization_parameters | Parameters for spline optimization, including weights and constraints. |
| scaled_time_base | The multiplier to iteratively scale time if the inequality dynamic constraints are not met. |
Declaration at line 129 of file spline_trajectory.hpp, definition at line 131 of file spline_trajectory.cpp.
getKnots()
| const std::vector< double > & SplineTrajectory::getKnots () |
|
Returns the vector of knot time points used in the spline trajectory.
- Returns
A const reference to the vector of knot times.
Declaration at line 144 of file spline_trajectory.hpp, definition at line 619 of file spline_trajectory.cpp.
Private Member Functions
calculateScaleTimeForConstraints()
enforceConstraints()
| void SplineTrajectory::enforceConstraints (const moveit_pro_controllers::Trajectory & points, const SplineOptimizationParameters & optimization_parameters, const double scaled_time_base) |
|
estimateConstraintScaleFactor()
| double SplineTrajectory::estimateConstraintScaleFactor (double ti, const Eigen::VectorNd & velocity_limits, const Eigen::VectorNd & acceleration_limits) |
|
factorizationCacheValid()
positionPenaltyAnchor()
| std::pair< Eigen::Index, double > SplineTrajectory::positionPenaltyAnchor (Eigen::Index point_idx) |
|
reparameterizeWithConstraints()
setAccelerationBasis()
| void SplineTrajectory::setAccelerationBasis (double delta_ti, Eigen::Index segment_index, Eigen::Ref< Eigen::MatrixXd > block) |
|
setCostBlock()
| void SplineTrajectory::setCostBlock (double delta_ti, Eigen::Index segment_index, int derivative_order, const SplineOptimizationParameters & optimization_parameters, Eigen::Ref< Eigen::MatrixXd > block) |
|
setJerkBasis()
| void SplineTrajectory::setJerkBasis (double delta_ti, Eigen::Index segment_index, Eigen::Ref< Eigen::MatrixXd > block) |
|
setPositionBasis()
| void SplineTrajectory::setPositionBasis (double delta_ti, Eigen::Index segment_index, Eigen::Ref< Eigen::MatrixXd > block) |
|
setupAndRunOptimization()
| void SplineTrajectory::setupAndRunOptimization (const moveit_pro_controllers::Trajectory & points, const SplineOptimizationParameters & optimization_parameters) |
|
setupBoundaryConstraints()
setupConstraintMatrixPosition()
| void SplineTrajectory::setupConstraintMatrixPosition (const moveit_pro_controllers::Trajectory & points, const SplineOptimizationParameters & optimization_parameters, Eigen::Index dim) |
|
setupConstraintMatrixVelocityAcceleration()
| void SplineTrajectory::setupConstraintMatrixVelocityAcceleration (const std::vector< double > & knots) |
|
setupCostMatrix()
setupKnots()
| void SplineTrajectory::setupKnots (const moveit_pro_controllers::Trajectory & points) |
|
setupQueryPoint()
| void SplineTrajectory::setupQueryPoint (const moveit_pro_controllers::Trajectory & points) |
|
setVelocityBasis()
| void SplineTrajectory::setVelocityBasis (double delta_ti, Eigen::Index segment_index, Eigen::Ref< Eigen::MatrixXd > block) |
|
solveOptimization()
updateFactorizationCache()
validateInputs()
| void SplineTrajectory::validateInputs (const moveit_pro_controllers::Trajectory & points, const SplineOptimizationParameters & optimization_parameters) |
|
Private Member Attributes
| Eigen::MatrixXd SplineTrajectory::A_ |
|
all_coefficients_
| Eigen::VectorXd SplineTrajectory::all_coefficients_ |
|
| Eigen::VectorXd SplineTrajectory::b_ |
|
| Eigen::VectorXd SplineTrajectory::f_ |
|
factorization_cache_
| FactorizationCache SplineTrajectory::factorization_cache_ |
|
| Eigen::MatrixXd SplineTrajectory::H_ |
|
knots_
| std::vector<double> SplineTrajectory::knots_ |
|
num_coefficients_
| const Eigen::Index SplineTrajectory::num_coefficients_ = 0 |
|
num_points_
| const Eigen::Index SplineTrajectory::num_points_ = 0 |
|
query_point_
| moveit_pro_controllers::TimedJointState SplineTrajectory::query_point_ |
|
svd_
| Eigen::JacobiSVD<Eigen::MatrixXd> SplineTrajectory::svd_ |
|
tmp_block_
| Eigen::MatrixXd SplineTrajectory::tmp_block_ |
|
mutable
|
tmp_constraint_tolerance_
| Eigen::MatrixXd SplineTrajectory::tmp_constraint_tolerance_ |
|
mutable
|
tmp_knots_
| std::vector<double> SplineTrajectory::tmp_knots_ |
|
mutable
|
tmp_solution_
| Eigen::MatrixXd SplineTrajectory::tmp_solution_ |
|
mutable
|
tmp_solution_1_
| Eigen::MatrixXd SplineTrajectory::tmp_solution_1_ |
|
mutable
|
tmp_solution_2_
| Eigen::MatrixXd SplineTrajectory::tmp_solution_2_ |
|
mutable
|
total_coefficients_
| const Eigen::Index SplineTrajectory::total_coefficients_ = 0 |
|
total_constraints_
| const Eigen::Index SplineTrajectory::total_constraints_ = 0 |
|
The documentation for this class was generated from the following files:
Generated via doxygen2docusaurus 2.2.2 by Doxygen 1.9.8.