From 67566cfecc8a9decacbd77d28b8e93d0a2526335 Mon Sep 17 00:00:00 2001 From: Russ Tedrake Date: Sun, 26 Jan 2025 21:22:58 -0500 Subject: [PATCH] Convert kintrajopt to use AsLinearInControlPoints Resolves #22533. --- .../kinematic_trajectory_optimization.cc | 278 ++++++++++-------- .../kinematic_trajectory_optimization.h | 26 +- .../kinematic_trajectory_optimization_test.cc | 13 +- 3 files changed, 174 insertions(+), 143 deletions(-) diff --git a/planning/trajectory_optimization/kinematic_trajectory_optimization.cc b/planning/trajectory_optimization/kinematic_trajectory_optimization.cc index d593eb8509b2..23f5d202479f 100644 --- a/planning/trajectory_optimization/kinematic_trajectory_optimization.cc +++ b/planning/trajectory_optimization/kinematic_trajectory_optimization.cc @@ -14,6 +14,7 @@ #include "drake/math/matrix_util.h" using Eigen::MatrixXd; +using Eigen::SparseMatrix; using Eigen::VectorXd; using std::nullopt; using std::optional; @@ -35,6 +36,7 @@ using solvers::LinearConstraint; using solvers::LinearCost; using solvers::MathematicalProgram; using solvers::MathematicalProgramResult; +using solvers::MatrixXDecisionVariable; using solvers::VectorXDecisionVariable; using symbolic::Expression; using symbolic::Variables; @@ -42,6 +44,8 @@ using trajectories::BsplineTrajectory; namespace { +const double kInf = std::numeric_limits::infinity(); + class PathConstraint : public Constraint { public: PathConstraint(std::shared_ptr wrapped_constraint, @@ -88,25 +92,24 @@ class PathConstraint : public Constraint { /* Implements a constraint of the form wrapped_constraint([q, v]), where duration = x[0] - q = M_pos * x[1:num_pos_vars+1] - v = M_vel * x[-num_vel_vars:] / duration - - TODO(russt): M_pos and M_vel are predictably sparse, and we could handle that - here if performance demands it. + control_points = x[1:num_control_points].reshaped(num_positions, + num_control_points) q = control_points * a_pos v = control_points * a_vel */ class WrappedVelocityConstraint : public Constraint { public: WrappedVelocityConstraint(std::shared_ptr wrapped_constraint, - Eigen::MatrixXd M_pos, Eigen::MatrixXd M_vel) + VectorXd a_pos, VectorXd a_vel, int num_positions, + int num_control_points) : Constraint(wrapped_constraint->num_outputs(), - M_pos.cols() + M_vel.cols() + 1, + 1 + num_positions * num_control_points, wrapped_constraint->lower_bound(), wrapped_constraint->upper_bound()), wrapped_constraint_(wrapped_constraint), - M_pos_{std::move(M_pos)}, - M_vel_{std::move(M_vel)} { - DRAKE_DEMAND(M_pos_.rows() + M_vel_.rows() == - wrapped_constraint_->num_vars()); + a_pos_{std::move(a_pos)}, + a_vel_{std::move(a_vel)}, + num_positions_{num_positions}, + num_control_points_{num_control_points} { + DRAKE_DEMAND(2 * num_positions_ == wrapped_constraint_->num_vars()); } void DoEval(const Eigen::Ref& x, @@ -119,9 +122,12 @@ class WrappedVelocityConstraint : public Constraint { void DoEval(const Eigen::Ref& x, AutoDiffVecXd* y) const override { AutoDiffXd duration = x[0]; + MatrixX control_points = + x.tail(num_positions_ * num_control_points_) + .reshaped(num_positions_, num_control_points_); VectorX qv(wrapped_constraint_->num_vars()); - qv << M_pos_ * x.segment(1, M_pos_.cols()), - M_vel_ * x.tail(M_vel_.cols()) / duration; + qv.head(num_positions_) = control_points * a_pos_.cast(); + qv.tail(num_positions_) = control_points * (a_vel_ / duration); wrapped_constraint_->Eval(qv, y); } @@ -134,21 +140,24 @@ class WrappedVelocityConstraint : public Constraint { private: std::shared_ptr wrapped_constraint_; - const Eigen::MatrixXd M_pos_; - const Eigen::MatrixXd M_vel_; + const VectorXd a_pos_; + const VectorXd a_vel_; + const int num_positions_; + const int num_control_points_; }; /* Implements a constraint of the form: duration = x[0] - lb <= M * x[1:] / duration^order <= ub + control_points.T = x[1:num_control_points] + lb <= A * control_points.T / duration^order <= ub */ class DerivativeConstraint : public Constraint { public: - DerivativeConstraint(const Eigen::MatrixXd& M, int derivative_order, - const Eigen::Ref& lb, - const Eigen::Ref& ub) - : Constraint(M.rows(), M.cols() + 1, lb, ub), - M_{M}, + DerivativeConstraint(const SparseMatrix& A, int derivative_order, + double lb, double ub) + : Constraint(A.rows(), A.cols() + 1, VectorXd::Constant(A.rows(), lb), + VectorXd::Constant(A.rows(), ub)), + A_{A}, derivative_order_{derivative_order} { DRAKE_DEMAND(derivative_order >= 1); } @@ -163,7 +172,7 @@ class DerivativeConstraint : public Constraint { void DoEval(const Eigen::Ref& x, AutoDiffVecXd* y) const override { AutoDiffXd duration = x[0]; - *y = M_ * x.tail(M_.cols()) / pow(duration, derivative_order_); + *y = A_ * x.tail(A_.cols()) / pow(duration, derivative_order_); } void DoEval(const Eigen::Ref>&, @@ -173,10 +182,35 @@ class DerivativeConstraint : public Constraint { } private: - const Eigen::MatrixXd M_; + const SparseMatrix A_; const int derivative_order_; }; +// For MatrixXDecisionVariable X and VectorXd a, returns SparseMatrix A and +// VectorXDecisionVariable x such that Ax == Xa. +std::pair, VectorXDecisionVariable> RewriteXa( + const MatrixXDecisionVariable& X, const VectorXd& a) { + const int num_nonzeros = (a.array() != 0).count(); + std::vector> triplets(X.rows() * num_nonzeros); + VectorXDecisionVariable x(X.rows() * num_nonzeros); + // A * x = [aᵀ, 0, 0, ..., 0 ][Xᵀ(:,0)] + // [0, aᵀ, 0, ..., 0 ][Xᵀ(:,1)] + // [ ... ][ ... ] + // [0, 0, 0, ...., aᵀ][Xᵀ(:,m)] + int n = 0; + for (int row = 0; row < X.rows(); ++row) { + for (int col = 0; col < X.cols(); ++col) { + if (a(col) != 0) { + triplets.push_back(Eigen::Triplet(row, n, a(col))); + x(n++) = X(row, col); + } + } + } + SparseMatrix A(X.rows(), n); + A.setFromTriplets(triplets.begin(), triplets.end()); + return std::make_pair(A, x); +} + } // namespace KinematicTrajectoryOptimization::KinematicTrajectoryOptimization( @@ -193,13 +227,14 @@ KinematicTrajectoryOptimization::KinematicTrajectoryOptimization( const trajectories::BsplineTrajectory& trajectory) : num_positions_(trajectory.rows()), num_control_points_(trajectory.num_control_points()) { - // basis_ = trajectory.basis() normalized to s∈[0,1]. + // basis = trajectory.basis() normalized to s∈[0,1]. const double duration = trajectory.end_time() - trajectory.start_time(); std::vector normalized_knots = trajectory.basis().knots(); for (auto& knot : normalized_knots) { knot /= duration; } - basis_ = BsplineBasis(trajectory.basis().order(), normalized_knots); + BsplineBasis basis(trajectory.basis().order(), normalized_knots); + bspline_ = BsplineTrajectory(basis, trajectory.control_points()); control_points_ = prog_.NewContinuousVariables( num_positions_, num_control_points_, "control_point"); @@ -216,7 +251,7 @@ KinematicTrajectoryOptimization::KinematicTrajectoryOptimization( // positions and its derivatives. // TODO(russt): Consider computing these only the first time they are used. sym_r_ = std::make_unique>( - basis_, EigenToStdVector(control_points_.cast())); + basis, EigenToStdVector(control_points_.cast())); sym_rdot_ = dynamic_pointer_cast_or_throw>( sym_r_->MakeDerivative()); @@ -240,13 +275,13 @@ BsplineTrajectory KinematicTrajectoryOptimization::ReconstructTrajectory( const MathematicalProgramResult& result) const { const double duration = result.GetSolution(duration_); - std::vector scaled_knots = basis_.knots(); + std::vector scaled_knots = basis().knots(); for (auto& knot : scaled_knots) { knot *= duration; } return BsplineTrajectory( - BsplineBasis(basis_.order(), scaled_knots), + BsplineBasis(basis().order(), scaled_knots), EigenToStdVector(result.GetSolution(control_points_))); } @@ -257,10 +292,12 @@ KinematicTrajectoryOptimization::AddPathPositionConstraint( DRAKE_DEMAND(lb.size() == num_positions()); DRAKE_DEMAND(ub.size() == num_positions()); DRAKE_DEMAND(0 <= s && s <= 1); - const VectorX sym_r_value = sym_r_->value(s); - auto binding = - prog_.AddLinearConstraint(lb <= sym_r_value && sym_r_value <= ub); - binding.evaluator()->set_description("path position constraint"); + const VectorXd M = bspline_.EvaluateLinearInControlPoints(s); + // lb <= control_points_ * M <= ub + auto [A, x] = RewriteXa(control_points_, M); + auto binding = prog_.AddLinearConstraint(A, lb, ub, x); + binding.evaluator()->set_description( + fmt::format("path position constraint at s={}", s)); return binding; } @@ -269,9 +306,9 @@ Binding KinematicTrajectoryOptimization::AddPathPositionConstraint( DRAKE_DEMAND(constraint->num_vars() == num_positions_); DRAKE_DEMAND(0 <= s && s <= 1); std::vector basis_function_values; - basis_function_values.reserve(basis_.order()); + basis_function_values.reserve(basis().order()); std::vector active_control_point_indices = - basis_.ComputeActiveBasisFunctionIndices(s); + basis().ComputeActiveBasisFunctionIndices(s); const int num_active_control_points = static_cast(active_control_point_indices.size()); VectorXDecisionVariable var_vector(num_active_control_points * @@ -279,14 +316,15 @@ Binding KinematicTrajectoryOptimization::AddPathPositionConstraint( for (int i = 0; i < num_active_control_points; ++i) { const int control_point_index = active_control_point_indices[i]; basis_function_values.push_back( - basis_.EvaluateBasisFunctionI(control_point_index, s)); + basis().EvaluateBasisFunctionI(control_point_index, s)); var_vector.segment(i * num_positions(), num_positions()) = control_points_.col(control_point_index); } auto binding = prog_.AddConstraint( std::make_shared(constraint, basis_function_values), var_vector); - binding.evaluator()->set_description("path position constraint"); + binding.evaluator()->set_description( + fmt::format("path position constraint at s={}", s)); return binding; } @@ -297,10 +335,13 @@ KinematicTrajectoryOptimization::AddPathVelocityConstraint( DRAKE_DEMAND(lb.size() == num_positions()); DRAKE_DEMAND(ub.size() == num_positions()); DRAKE_DEMAND(0 <= s && s <= 1); - const VectorX sym_rdot_value = sym_rdot_->value(s); - auto binding = - prog_.AddLinearConstraint(lb <= sym_rdot_value && sym_rdot_value <= ub); - binding.evaluator()->set_description("path velocity constraint"); + const VectorXd M = + bspline_.EvaluateLinearInControlPoints(s, /* derivative_order= */ 1); + // lb <= control_points_ * M <= ub + auto [A, x] = RewriteXa(control_points_, M); + auto binding = prog_.AddLinearConstraint(A, lb, ub, x); + binding.evaluator()->set_description( + fmt::format("path velocity constraint at s={}", s)); return binding; } @@ -310,24 +351,18 @@ KinematicTrajectoryOptimization::AddVelocityConstraintAtNormalizedTime( DRAKE_DEMAND(constraint->num_vars() == 2 * num_positions_); DRAKE_DEMAND(0 <= s && s <= 1); - VectorX r = sym_r_->value(s); - VectorX rdot = sym_rdot_->value(s); - VectorXDecisionVariable vars_pos, vars_vel; - std::unordered_map unused_map; - std::tie(vars_pos, unused_map) = symbolic::ExtractVariablesFromExpression(r); - std::tie(vars_vel, unused_map) = - symbolic::ExtractVariablesFromExpression(rdot); - Eigen::MatrixXd M_pos(num_positions(), vars_pos.size()); - Eigen::MatrixXd M_vel(num_positions(), vars_vel.size()); - symbolic::DecomposeLinearExpressions(r, vars_pos, &M_pos); - symbolic::DecomposeLinearExpressions(rdot, vars_vel, &M_vel); - VectorXDecisionVariable duration_and_vars(1 + vars_pos.size() + - vars_vel.size()); - duration_and_vars << duration_, vars_pos, vars_vel; + const VectorXd a_pos = + bspline_.EvaluateLinearInControlPoints(s, /* derivative_order= */ 0); + const VectorXd a_vel = + bspline_.EvaluateLinearInControlPoints(s, /* derivative_order= */ 1); + VectorXDecisionVariable vars(1 + control_points_.size()); + vars << duration_, control_points_.reshaped(control_points_.size(), 1); auto wrapped_constraint = std::make_shared( - constraint, std::move(M_pos), std::move(M_vel)); - auto binding = prog_.AddConstraint(wrapped_constraint, duration_and_vars); - binding.evaluator()->set_description("velocity constraint"); + constraint, std::move(a_pos), std::move(a_vel), num_positions_, + num_control_points_); + auto binding = prog_.AddConstraint(wrapped_constraint, vars); + binding.evaluator()->set_description( + fmt::format("velocity constraint at s={}", s)); return binding; } @@ -338,10 +373,13 @@ KinematicTrajectoryOptimization::AddPathAccelerationConstraint( DRAKE_DEMAND(lb.size() == num_positions()); DRAKE_DEMAND(ub.size() == num_positions()); DRAKE_DEMAND(0 <= s && s <= 1); - const VectorX sym_rddot_value = sym_rddot_->value(s); - auto binding = - prog_.AddLinearConstraint(lb <= sym_rddot_value && sym_rddot_value <= ub); - binding.evaluator()->set_description("path acceleration constraint"); + const VectorXd M = + bspline_.EvaluateLinearInControlPoints(s, /* derivative_order= */ 2); + // lb <= control_points_ * M <= ub + auto [A, x] = RewriteXa(control_points_, M); + auto binding = prog_.AddLinearConstraint(A, lb, ub, x); + binding.evaluator()->set_description( + fmt::format("path acceleration constraint at s={}", s)); return binding; } @@ -389,88 +427,82 @@ KinematicTrajectoryOptimization::AddVelocityBounds( // the control points satisfy these convex constraints and the curve is // inside the convex hull of these constraints, then the curve satisfies the // constraints for all t. - std::vector> binding; - for (int i = 0; i < sym_rdot_->num_control_points(); ++i) { - binding.emplace_back(prog_.AddLinearConstraint( - sym_rdot_->control_points()[i] >= duration_ * lb && - sym_rdot_->control_points()[i] <= duration_ * ub)); - binding[i].evaluator()->set_description( - fmt::format("velocity bound {}", i)); + std::vector> bindings; + const SparseMatrix M = + bspline_.AsLinearInControlPoints(/* derivative_order = */ 1); + // duration * lb <= control_points * M <= duration * ub. + VectorXDecisionVariable vars(1 + num_control_points_); + vars[0] = duration_; + SparseMatrix A(M.cols(), M.rows() + 1); + A.rightCols(M.rows()) = M.transpose(); + for (int i = 0; i < num_positions_; ++i) { + vars.tail(num_control_points_) = control_points_.row(i).transpose(); + // lb(i)*duration <= control_points(i,:) * M, but transposed + A.col(0) = VectorXd::Constant(M.cols(), -lb(i)).sparseView(); + bindings.emplace_back(prog_.AddLinearConstraint( + A, /* lb= */ VectorXd::Zero(M.cols()), + /* ub= */ VectorXd::Constant(M.cols(), kInf), vars)); + bindings.back().evaluator()->set_description( + fmt::format("velocity lower bound for position {}", i)); + // ub(i)*duration >= control_points(i,:) * M, but transposed + A.col(0) = VectorXd::Constant(M.cols(), -ub(i)).sparseView(); + bindings.emplace_back(prog_.AddLinearConstraint( + A, /* lb= */ VectorXd::Constant(M.cols(), -kInf), + /* ub= */ VectorXd::Zero(M.cols()), vars)); + bindings.back().evaluator()->set_description( + fmt::format("velocity upper bound for position {}", i)); } - return binding; + return bindings; } -std::vector>> +std::vector> KinematicTrajectoryOptimization::AddAccelerationBounds( const Eigen::Ref& lb, const Eigen::Ref& ub) { DRAKE_DEMAND(lb.size() == num_positions()); DRAKE_DEMAND(ub.size() == num_positions()); - // We have t = duration * s. So dsdt = 1/duration, d²sdt² = 0. Then q̈(t) = - // r̈(s) * dsdt^2. - - // This again leverages the convex hull property to enforce the guarantee ∀t - // by only constraining the values at the control points. - Eigen::RowVectorXd M; - VectorXDecisionVariable vars, duration_and_vars; - std::unordered_map unused_map; - std::vector>> binding( - sym_rddot_->num_control_points()); - for (int i = 0; i < sym_rddot_->num_control_points(); ++i) { - for (int j = 0; j < num_positions(); ++j) { - std::tie(vars, unused_map) = symbolic::ExtractVariablesFromExpression( - sym_rddot_->control_points()[i](j)); - M.resize(vars.size()); - // TODO(russt): Avoid symbolic here and throughout. - symbolic::DecomposeLinearExpressions( - Vector1(sym_rddot_->control_points()[i](j)), vars, &M); - auto con = std::make_shared(M, 2, lb.segment<1>(j), - ub.segment<1>(j)); - duration_and_vars.resize(vars.size() + 1); - duration_and_vars << duration_, vars; - binding[i].emplace_back(prog_.AddConstraint(con, duration_and_vars)); - binding[i][j].evaluator()->set_description( - fmt::format("acceleration bound {}, {}", i, j)); - } + // These are nonconvex constraints, but they again leverage the convex hull + // property to enforce the guarantee ∀t by only constraining the values at + // the control points. + const SparseMatrix M = + bspline_.AsLinearInControlPoints(/* derivative_order= */ 2); + SparseMatrix A = M.transpose(); + VectorXDecisionVariable vars(1 + num_control_points_); + vars[0] = duration_; + std::vector> bindings; + for (int i = 0; i < num_positions_; ++i) { + vars.tail(num_control_points_) = control_points_.row(i).transpose(); + bindings.emplace_back(prog_.AddConstraint( + std::make_shared(A, 2, lb(i), ub(i)), vars)); + bindings.back().evaluator()->set_description( + fmt::format("acceleration bound for position {}", i)); } - return binding; + return bindings; } -std::vector>> -KinematicTrajectoryOptimization::AddJerkBounds( +std::vector> KinematicTrajectoryOptimization::AddJerkBounds( const Eigen::Ref& lb, const Eigen::Ref& ub) { DRAKE_DEMAND(lb.size() == num_positions()); DRAKE_DEMAND(ub.size() == num_positions()); - // Following the derivations above, we have d³qdt³(t) = d³rds³(s) * dsdt*3. - // This again leverages the convex hull property to enforce the guarantee ∀t // by only constraining the values at the control points. - Eigen::RowVectorXd M; - VectorXDecisionVariable vars, duration_and_vars; - std::unordered_map map_var_to_index; - std::vector>> binding( - sym_rddot_->num_control_points()); - for (int i = 0; i < sym_rdddot_->num_control_points(); ++i) { - for (int j = 0; j < num_positions(); ++j) { - std::tie(vars, map_var_to_index) = - symbolic::ExtractVariablesFromExpression( - sym_rdddot_->control_points()[i](j)); - M.resize(vars.size()); - symbolic::DecomposeLinearExpressions( - Vector1(sym_rdddot_->control_points()[i](j)), vars, &M); - auto con = std::make_shared(M, 3, lb.segment<1>(j), - ub.segment<1>(j)); - duration_and_vars.resize(vars.size() + 1); - duration_and_vars << duration_, vars; - binding[i].emplace_back(prog_.AddConstraint(con, duration_and_vars)); - binding[i][j].evaluator()->set_description( - fmt::format("jerk bound {}, {}", i, j)); - } + const SparseMatrix M = + bspline_.AsLinearInControlPoints(/* derivative_order= */ 3); + SparseMatrix A = M.transpose(); + VectorXDecisionVariable vars(1 + num_control_points_); + vars[0] = duration_; + std::vector> bindings; + for (int i = 0; i < num_positions_; ++i) { + vars.tail(num_control_points_) = control_points_.row(i).transpose(); + bindings.emplace_back(prog_.AddConstraint( + std::make_shared(A, 3, lb(i), ub(i)), vars)); + bindings.back().evaluator()->set_description( + fmt::format("jerk bound for position {}", i)); } - return binding; + return bindings; } Binding KinematicTrajectoryOptimization::AddDurationCost( diff --git a/planning/trajectory_optimization/kinematic_trajectory_optimization.h b/planning/trajectory_optimization/kinematic_trajectory_optimization.h index fb99ffe57ce8..030a0b5ae405 100644 --- a/planning/trajectory_optimization/kinematic_trajectory_optimization.h +++ b/planning/trajectory_optimization/kinematic_trajectory_optimization.h @@ -74,7 +74,7 @@ class KinematicTrajectoryOptimization { int num_control_points() const { return num_control_points_; } /** Returns the basis used to represent the path, r(s), over s∈[0,1]. */ - const math::BsplineBasis& basis() const { return basis_; } + const math::BsplineBasis& basis() const { return bspline_.basis(); } /** Returns the control points defining the path as an M-by-N matrix, where M is the number of positions and N is the number of control points. */ @@ -170,8 +170,8 @@ class KinematicTrajectoryOptimization { trajectory, q̇(t). These bounds will be respected at all times. Note this does NOT directly constrain ṙ(s). - @returns A vector of bindings with the ith element adding a constraint to the - ith control point of the derivative trajectory. */ + @returns A vector of bindings with interleaved lower and then upper bounds, + constraining all of the control points for one position at a time. */ std::vector> AddVelocityBounds( const Eigen::Ref& lb, const Eigen::Ref& ub); @@ -180,22 +180,20 @@ class KinematicTrajectoryOptimization { bounds to the acceleration trajectory, q̈(t). These constraints will be respected at all times. Note that this does NOT directly constrain r̈(s). - @returns A vector of bindings with the ith element is itself a vector of - constraints (one per dof) adding a constraint to the ith control point of the - acceleration trajectory. */ - std::vector>> - AddAccelerationBounds(const Eigen::Ref& lb, - const Eigen::Ref& ub); + @returns A vector of bindings constraining all of the control points for one + position at a time. */ + std::vector> AddAccelerationBounds( + const Eigen::Ref& lb, + const Eigen::Ref& ub); /** Adds generic (nonlinear) constraints to enforce the upper and lower bounds to the jerk trajectory, d³qdt³(t). These constraints will be respected at all times. Note that this does NOT directly constrain d³rds³(s). - @returns A vector of bindings with the ith element is itself a vector of - constraints (one per dof) adding a constraint to the ith control point of the - jerk trajectory. */ - std::vector>> AddJerkBounds( + @returns A vector of bindings constraining all of the control points for one + position at a time. */ + std::vector> AddJerkBounds( const Eigen::Ref& lb, const Eigen::Ref& ub); @@ -236,7 +234,7 @@ class KinematicTrajectoryOptimization { int num_positions_{}; int num_control_points_{}; - math::BsplineBasis basis_; + trajectories::BsplineTrajectory bspline_; solvers::MatrixXDecisionVariable control_points_; symbolic::Variable duration_; diff --git a/planning/trajectory_optimization/test/kinematic_trajectory_optimization_test.cc b/planning/trajectory_optimization/test/kinematic_trajectory_optimization_test.cc index f555d5a001fb..4322c477a0dc 100644 --- a/planning/trajectory_optimization/test/kinematic_trajectory_optimization_test.cc +++ b/planning/trajectory_optimization/test/kinematic_trajectory_optimization_test.cc @@ -251,9 +251,10 @@ TEST_F(KinematicTrajectoryOptimizationTest, AddVelocityBounds) { EXPECT_EQ(trajopt_.prog().linear_constraints().size(), 0); auto binding = trajopt_.AddVelocityBounds(-VectorXd::Ones(num_positions_), VectorXd::Ones(num_positions_)); - EXPECT_THAT(binding[0].to_string(), HasSubstr("velocity bound")); + EXPECT_THAT(binding[0].to_string(), HasSubstr("velocity lower bound")); + EXPECT_THAT(binding[1].to_string(), HasSubstr("velocity upper bound")); EXPECT_EQ(trajopt_.prog().linear_constraints().size(), - trajopt_.num_control_points() - 1); + trajopt_.num_positions() * 2); result = Solve(trajopt_.prog()); EXPECT_TRUE(result.is_success()); @@ -292,9 +293,9 @@ TEST_F(KinematicTrajectoryOptimizationTest, AddAccelerationBounds) { EXPECT_EQ(trajopt_.prog().generic_constraints().size(), 0); auto binding = trajopt_.AddAccelerationBounds(-Vector3d::Ones(), Vector3d::Ones()); - EXPECT_THAT(binding[0][0].to_string(), HasSubstr("acceleration bound")); + EXPECT_THAT(binding[0].to_string(), HasSubstr("acceleration bound")); EXPECT_EQ(trajopt_.prog().generic_constraints().size(), - trajopt_.num_positions() * (trajopt_.num_control_points() - 2)); + trajopt_.num_positions()); result = Solve(trajopt_.prog()); EXPECT_TRUE(result.is_success()); @@ -340,9 +341,9 @@ TEST_F(KinematicTrajectoryOptimizationTest, AddJerkBounds) { EXPECT_EQ(trajopt_.prog().generic_constraints().size(), 0); auto binding = trajopt_.AddJerkBounds(-Vector3d::Ones(), Vector3d::Ones()); - EXPECT_THAT(binding[0][0].to_string(), HasSubstr("jerk bound")); + EXPECT_THAT(binding[0].to_string(), HasSubstr("jerk bound")); EXPECT_EQ(trajopt_.prog().generic_constraints().size(), - trajopt_.num_positions() * (trajopt_.num_control_points() - 3)); + trajopt_.num_positions()); result = Solve(trajopt_.prog()); EXPECT_TRUE(result.is_success());