diff --git a/tesseract_motion_planners/trajopt/src/trajopt_utils.cpp b/tesseract_motion_planners/trajopt/src/trajopt_utils.cpp index 90408dba4d8..323a40cfd2e 100644 --- a/tesseract_motion_planners/trajopt/src/trajopt_utils.cpp +++ b/tesseract_motion_planners/trajopt/src/trajopt_utils.cpp @@ -190,7 +190,7 @@ trajopt::TermInfo::Ptr createCollisionTermInfo(int start_index, trajopt::TermInfo::Ptr createSmoothVelocityTermInfo(int start_index, int end_index, int n_joints, double coeff, trajopt::TermType type) { - if ((end_index - start_index) < 2) + if ((end_index - start_index) < 1) throw std::runtime_error("TrajOpt JointVelTermInfo requires at least two states!"); std::shared_ptr jv = std::make_shared(); @@ -208,7 +208,7 @@ trajopt::TermInfo::Ptr createSmoothVelocityTermInfo(int start_index, const Eigen::Ref& coeff, trajopt::TermType type) { - if ((end_index - start_index) < 2) + if ((end_index - start_index) < 1) throw std::runtime_error("TrajOpt JointVelTermInfo requires at least two states!"); std::shared_ptr jv = std::make_shared(); @@ -224,7 +224,7 @@ trajopt::TermInfo::Ptr createSmoothVelocityTermInfo(int start_index, trajopt::TermInfo::Ptr createSmoothAccelerationTermInfo(int start_index, int end_index, int n_joints, double coeff, trajopt::TermType type) { - if ((end_index - start_index) < 3) + if ((end_index - start_index) < 2) throw std::runtime_error("TrajOpt JointAccTermInfo requires at least three states!"); std::shared_ptr ja = std::make_shared(); @@ -242,7 +242,7 @@ trajopt::TermInfo::Ptr createSmoothAccelerationTermInfo(int start_index, const Eigen::Ref& coeff, trajopt::TermType type) { - if ((end_index - start_index) < 3) + if ((end_index - start_index) < 2) throw std::runtime_error("TrajOpt JointAccTermInfo requires at least three states!"); std::shared_ptr ja = std::make_shared(); @@ -258,7 +258,7 @@ trajopt::TermInfo::Ptr createSmoothAccelerationTermInfo(int start_index, trajopt::TermInfo::Ptr createSmoothJerkTermInfo(int start_index, int end_index, int n_joints, double coeff, trajopt::TermType type) { - if ((end_index - start_index) < 5) + if ((end_index - start_index) < 4) throw std::runtime_error("TrajOpt JointJerkTermInfo requires at least five states!"); std::shared_ptr jj = std::make_shared(); @@ -276,7 +276,7 @@ trajopt::TermInfo::Ptr createSmoothJerkTermInfo(int start_index, const Eigen::Ref& coeff, trajopt::TermType type) { - if ((end_index - start_index) < 5) + if ((end_index - start_index) < 4) throw std::runtime_error("TrajOpt JointJerkTermInfo requires at least five states!"); std::shared_ptr jj = std::make_shared();