diff --git a/bindings/pydrake/planning/BUILD.bazel b/bindings/pydrake/planning/BUILD.bazel index 1e515775e55a..e228d5371825 100644 --- a/bindings/pydrake/planning/BUILD.bazel +++ b/bindings/pydrake/planning/BUILD.bazel @@ -105,6 +105,9 @@ drake_py_unittest( drake_py_unittest( name = "trajectory_optimization_test", + data = [ + "//examples/multibody/cart_pole:models", + ], deps = [ ":planning", "//bindings/pydrake/examples", diff --git a/bindings/pydrake/planning/planning_py_trajectory_optimization.cc b/bindings/pydrake/planning/planning_py_trajectory_optimization.cc index 564a6caa6a80..a3d67c83a4ac 100644 --- a/bindings/pydrake/planning/planning_py_trajectory_optimization.cc +++ b/bindings/pydrake/planning/planning_py_trajectory_optimization.cc @@ -332,6 +332,11 @@ void DefinePlanningTrajectoryOptimization(py::module m) { py::arg("lb"), py::arg("ub"), cls_doc.AddAccelerationBounds.doc) .def("AddJerkBounds", &Class::AddJerkBounds, py::arg("lb"), py::arg("ub"), cls_doc.AddJerkBounds.doc) + .def("AddEffortBoundsAtNormalizedTimes", + &Class::AddEffortBoundsAtNormalizedTimes, py::arg("plant"), + py::arg("s"), py::arg("lb") = std::nullopt, + py::arg("ub") = std::nullopt, py::arg("plant_context") = nullptr, + cls_doc.AddEffortBoundsAtNormalizedTimes.doc) .def("AddDurationCost", &Class::AddDurationCost, py::arg("weight") = 1.0, cls_doc.AddDurationCost.doc) .def("AddPathLengthCost", &Class::AddPathLengthCost, diff --git a/bindings/pydrake/planning/test/trajectory_optimization_test.py b/bindings/pydrake/planning/test/trajectory_optimization_test.py index aba718ac6ced..a8b08beedb01 100644 --- a/bindings/pydrake/planning/test/trajectory_optimization_test.py +++ b/bindings/pydrake/planning/test/trajectory_optimization_test.py @@ -32,6 +32,7 @@ ExpressionConstraint ) from pydrake.multibody.plant import MultibodyPlant +from pydrake.multibody.parsing import Parser import pydrake.solvers as mp from pydrake.symbolic import Variable from pydrake.systems.framework import InputPortSelection @@ -254,6 +255,16 @@ def test_kinematic_trajectory_optimization(self): trajopt.AddAccelerationBounds(lb=b, ub=b) trajopt.AddJerkBounds(lb=b, ub=b) + plant = MultibodyPlant(time_step=0.0) + Parser(plant).AddModelsFromUrl( + url="package://drake/examples/multibody/cart_pole/cart_pole.sdf") + plant.Finalize() + plant_context = plant.CreateDefaultContext() + trajopt.AddEffortBoundsAtNormalizedTimes( + plant=plant, s=[0.1, 0.9], + lb=[-1, -2], ub=[3, 4], + plant_context=plant_context) + trajopt.AddDurationCost(weight=1) trajopt.AddPathLengthCost(weight=1) trajopt.AddPathEnergyCost(weight=1) diff --git a/planning/trajectory_optimization/BUILD.bazel b/planning/trajectory_optimization/BUILD.bazel index 7850e87c0c8d..1f2ea296b34f 100644 --- a/planning/trajectory_optimization/BUILD.bazel +++ b/planning/trajectory_optimization/BUILD.bazel @@ -97,6 +97,7 @@ drake_cc_library( "//math:bspline_basis", "//math:gradient", "//math:matrix_util", + "//multibody/plant", "//solvers:mathematical_program", "//solvers:mathematical_program_result", ], @@ -194,6 +195,7 @@ drake_cc_googletest( deps = [ ":kinematic_trajectory_optimization", "//common/test_utilities:eigen_matrix_compare", + "//multibody/benchmarks/pendulum", "//solvers:constraint", "//solvers:ipopt_solver", "//solvers:osqp_solver", diff --git a/planning/trajectory_optimization/kinematic_trajectory_optimization.cc b/planning/trajectory_optimization/kinematic_trajectory_optimization.cc index ac432551f9ae..374663b3e298 100644 --- a/planning/trajectory_optimization/kinematic_trajectory_optimization.cc +++ b/planning/trajectory_optimization/kinematic_trajectory_optimization.cc @@ -28,6 +28,8 @@ using math::EigenToStdVector; using math::ExtractValue; using math::InitializeAutoDiff; using math::StdVectorToEigen; +using multibody::MultibodyForces; +using multibody::MultibodyPlant; using solvers::Binding; using solvers::BoundingBoxConstraint; using solvers::Constraint; @@ -187,12 +189,74 @@ class DerivativeConstraint : public Constraint { const int derivative_order_; }; +// Constraint of the form tau_lb <= InverseDynamics(q, v, a) <= tau_ub. The +// constraint should be bound to the duration followed by all of the control +// points in the bspline. TODO(russt): The constraint actually only depends on +// a subset of the control points, depending on s, so we could consider +// optimizing this aspect. +class EffortConstraint : public Constraint { + public: + EffortConstraint(const std::shared_ptr>& plant, + std::unique_ptr> plant_context, + const Eigen::VectorXd& lb, const Eigen::VectorXd& ub, + const BsplineTrajectory& bspline, double s) + : Constraint(lb.size(), 1 + bspline.num_control_points() * bspline.rows(), + lb, ub), + plant_(plant), + plant_context_(std::move(plant_context)), + num_control_points_(bspline.num_control_points()) { + // Note: consistency checks on the input arguments already happened in + // AddEffortBoundsAtNormalizedTimes(). + M_q_ = bspline.EvaluateLinearInControlPoints(s, 0).cast(); + M_v_ = bspline.EvaluateLinearInControlPoints(s, 1).cast(); + M_vdot_ = bspline.EvaluateLinearInControlPoints(s, 2).cast(); + } + + void DoEval(const Eigen::Ref& x, + Eigen::VectorXd* y) const override { + AutoDiffVecXd y_t; + Eval(InitializeAutoDiff(x), &y_t); + *y = ExtractValue(y_t); + } + + void DoEval(const Eigen::Ref& x, + AutoDiffVecXd* y) const override { + y->resize(plant_->num_positions()); + AutoDiffXd duration = x[0]; + MatrixX control_points = + x.tail(plant_->num_positions() * num_control_points_) + .reshaped(plant_->num_positions(), num_control_points_); + plant_->SetPositions(plant_context_.get(), control_points * M_q_); + plant_->SetVelocities(plant_context_.get(), + control_points * M_v_ / duration); + VectorX vdot = control_points * M_vdot_ / pow(duration, 2); + MultibodyForces forces_(*plant_); + plant_->CalcForceElementsContribution(*plant_context_, &forces_); + *y = plant_->CalcInverseDynamics(*plant_context_, vdot, forces_); + } + + void DoEval(const Eigen::Ref>&, + VectorX*) const override { + throw std::runtime_error( + "EffortConstraint does not support evaluation with Expression."); + } + + private: + const std::shared_ptr> plant_; + std::unique_ptr> plant_context_; + VectorX M_q_; + VectorX M_v_; + VectorX M_vdot_; + int num_control_points_; +}; + // solvers::LinearConstraint stores linear constraints in the form A*x <= b. -// When constraints are naturally expressed in the form X*a <= b (the decision -// variables on the left), it is still a vector constraint, but we must do some -// work to rewrite it as A*x, and the result is a big sparse A. This method -// does precisely that. For MatrixXDecisionVariable X and VectorXd a, returns -// SparseMatrix A and VectorXDecisionVariable x such that Ax == Xa. +// When constraints are naturally expressed in the form X*a <= b (the +// decision variables on the left), it is still a vector constraint, but we +// must do some work to rewrite it as A*x, and the result is a big sparse A. +// This method does precisely that. 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(); @@ -497,6 +561,47 @@ std::vector> KinematicTrajectoryOptimization::AddJerkBounds( return bindings; } +std::vector> +KinematicTrajectoryOptimization::AddEffortBoundsAtNormalizedTimes( + const MultibodyPlant& plant, + const Eigen::Ref& s, + const std::optional>& lb, + const std::optional>& ub, + const systems::Context* plant_context) { + DRAKE_THROW_UNLESS(s.array().minCoeff() >= 0 && s.array().maxCoeff() <= 1); + const VectorXd effort_lb = lb.value_or(plant.GetEffortLowerLimits()); + const VectorXd effort_ub = ub.value_or(plant.GetEffortUpperLimits()); + DRAKE_THROW_UNLESS(effort_lb.size() == num_positions()); + DRAKE_THROW_UNLESS(effort_ub.size() == num_positions()); + DRAKE_THROW_UNLESS((effort_lb.array() <= effort_ub.array()).all()); + if (plant_context != nullptr) { + plant.ValidateContext(plant_context); + } + + VectorXDecisionVariable vars(1 + num_control_points_ * num_positions_); + vars << duration_, control_points_.reshaped(control_points_.size(), 1); + std::shared_ptr> plant_ad( + systems::System::ToAutoDiffXd(plant)); + std::vector> bindings; + for (int i = 0; i < ssize(s); ++i) { + // Note: we choose to make a separate context for each s so that the + // constraints are thread-safe. + std::unique_ptr> plant_context_ad = + plant_ad->CreateDefaultContext(); + if (plant_context != nullptr) { + plant_context_ad->SetTimeStateAndParametersFrom(*plant_context); + } + bindings.emplace_back( + prog_.AddConstraint(std::make_shared( + plant_ad, std::move(plant_context_ad), + effort_lb, effort_ub, bspline_, s[i]), + vars)); + bindings.back().evaluator()->set_description( + fmt::format("effort bound at s={}", s[i])); + } + return bindings; +} + Binding KinematicTrajectoryOptimization::AddDurationCost( double weight) { auto binding = prog_.AddLinearCost(weight * duration_); diff --git a/planning/trajectory_optimization/kinematic_trajectory_optimization.h b/planning/trajectory_optimization/kinematic_trajectory_optimization.h index 41f610afd537..0f551c7961b5 100644 --- a/planning/trajectory_optimization/kinematic_trajectory_optimization.h +++ b/planning/trajectory_optimization/kinematic_trajectory_optimization.h @@ -9,6 +9,7 @@ #include "drake/common/copyable_unique_ptr.h" #include "drake/common/trajectories/bspline_trajectory.h" +#include "drake/multibody/plant/multibody_plant.h" #include "drake/solvers/binding.h" #include "drake/solvers/mathematical_program.h" #include "drake/solvers/mathematical_program_result.h" @@ -204,6 +205,38 @@ class KinematicTrajectoryOptimization { const Eigen::Ref& lb, const Eigen::Ref& ub); + /** Adds generic (nonlinear) constraints to enforce the effort limits defined + in the plant at a sequence of normalized times, `s`: + @verbatim + B lb ≤ M(q)v̇ + C(q, v)v - τ_g(q) - τ_app ≤ B ub + @endverbatim + where q, v, and v̇ are evaluated at s. B is the plant's actuation matrix, and + M, C, τ_g, and τ_app are the plant's mass matrix, Coriolis force, gravity, + and applied force, respectively. `ub` and `lb` are the upper and lower effort + bounds, respectively; if they are not provided then + plant.GetEffortLowerLimits() and plant.GetEffortUpperLimits() are used. + + Pass `plant_context` if you have non-default parameters in the context. Note + that there are no lifetime requirements on `plant` nsor `plant_context`. + + Note that the convex hull property of the B-splines is not guaranteed to hold + here -- effort limits maybe be violated away from the normalized times `s`. + + @pre plant.is_finalized() + @pre plant.num_positions() == num_positions() + @pre s[i] ∈ [0, 1] for all i + @pre B lb ≤ B ub + + @returns A vector of bindings with one effort limit constraint for each `s`. + */ + std::vector> + AddEffortBoundsAtNormalizedTimes( + const multibody::MultibodyPlant& plant, + const Eigen::Ref& s, + const std::optional>& lb = std::nullopt, + const std::optional>& ub = std::nullopt, + const systems::Context* plant_context = nullptr); + /** Adds a linear cost on the duration of the trajectory. */ solvers::Binding AddDurationCost(double weight = 1.0); @@ -234,6 +267,9 @@ class KinematicTrajectoryOptimization { /* TODO(russt): - Support additional (non-convex) costs/constraints on q(t) directly. + - AddMultibodyPlantConstraints which adds joint, velocity, acceleration, and + effort limits + multibody constraints. Presumably we can't have quaternions + in a fully-actuated system. */ private: diff --git a/planning/trajectory_optimization/test/kinematic_trajectory_optimization_test.cc b/planning/trajectory_optimization/test/kinematic_trajectory_optimization_test.cc index 4322c477a0dc..5ca4b8d0972d 100644 --- a/planning/trajectory_optimization/test/kinematic_trajectory_optimization_test.cc +++ b/planning/trajectory_optimization/test/kinematic_trajectory_optimization_test.cc @@ -10,11 +10,13 @@ #include "drake/common/test_utilities/eigen_matrix_compare.h" #include "drake/common/text_logging.h" #include "drake/math/matrix_util.h" +#include "drake/multibody/benchmarks/pendulum/make_pendulum_plant.h" #include "drake/solvers/ipopt_solver.h" #include "drake/solvers/osqp_solver.h" #include "drake/solvers/solve.h" using Eigen::MatrixXd; +using Eigen::Vector2d; using Eigen::Vector3d; using Eigen::VectorXd; using std::nullopt; @@ -359,6 +361,96 @@ TEST_F(KinematicTrajectoryOptimizationTest, AddJerkBounds) { } } +GTEST_TEST(KinematicTrajectoryOptimizationMultibodyTest, + AddEffortBoundsAtNormalizedTimes) { + multibody::benchmarks::pendulum::PendulumParameters parameters; + auto plant = multibody::benchmarks::pendulum::MakePendulumPlant(parameters); + const int num_control_points = 10; + KinematicTrajectoryOptimization trajopt(plant->num_positions(), + num_control_points); + + const Vector1d tau_lb(-1.0), tau_ub(1.0); + auto bindings = trajopt.AddEffortBoundsAtNormalizedTimes(*plant, Vector1d(0), + tau_lb, tau_ub); + EXPECT_EQ(bindings.size(), 1); + EXPECT_EQ(bindings[0].evaluator()->num_constraints(), 1); + EXPECT_EQ(bindings[0].evaluator()->lower_bound(), tau_lb); + EXPECT_EQ(bindings[0].evaluator()->upper_bound(), tau_ub); + + auto MakeTrajectory = [&](double q0, double v0, double vdot0) { + KinematicTrajectoryOptimization kto(plant->num_positions(), + num_control_points); + kto.AddDurationConstraint(1, 1); + kto.AddPathPositionConstraint(Vector1d(q0), Vector1d(q0), /* s= */ 0); + kto.AddPathVelocityConstraint(Vector1d(v0), Vector1d(v0), /* s= */ 0); + kto.AddPathAccelerationConstraint(Vector1d(vdot0), Vector1d(vdot0), + /* s= */ 0); + auto result = Solve(kto.prog()); + return kto.ReconstructTrajectory(result); + }; + + // Variables for the constraint are [duration, control_points] + VectorXd x(1 + num_control_points); + const double duration = 1.0; + auto vdot_ulimit = [&](double q, double v) { + // ml^2vdot + b*v + mglsin(q) <= 1.0 + return (tau_ub[0] - parameters.damping() * v - + parameters.m() * parameters.g() * parameters.l() * std::sin(q)) / + parameters.m() / parameters.l() / parameters.l(); + }; + + // Initial acceleration below the limit. + double q0 = 0, v0 = 0; + auto traj = MakeTrajectory(q0, v0, /* vdot0 = */ 0.9 * vdot_ulimit(q0, v0)); + x << duration, + math::StdVectorToEigen(traj.control_points()).row(0).transpose(); + EXPECT_TRUE(bindings[0].evaluator()->CheckSatisfied(x)); + + // Initial acceleration above the limit. + traj = MakeTrajectory(q0, v0, /* vdot0 = */ 1.1 * vdot_ulimit(q0, v0)); + x << duration, + math::StdVectorToEigen(traj.control_points()).row(0).transpose(); + EXPECT_FALSE(bindings[0].evaluator()->CheckSatisfied(x)); + + // Confirm that the damping is also considered by setting a (large) non-zero + // velocity. + v0 = -100; + traj = MakeTrajectory(q0, v0, /* vdot0 = */ 0.9 * vdot_ulimit(q0, v0)); + x << duration, + math::StdVectorToEigen(traj.control_points()).row(0).transpose(); + EXPECT_TRUE(bindings[0].evaluator()->CheckSatisfied(x)); + traj = MakeTrajectory(q0, v0, /* vdot0 = */ 1.1 * vdot_ulimit(q0, v0)); + x << duration, + math::StdVectorToEigen(traj.control_points()).row(0).transpose(); + EXPECT_FALSE(bindings[0].evaluator()->CheckSatisfied(x)); + + // Set the damping to zero in the context. + auto context = plant->CreateDefaultContext(); + plant->get_joint(multibody::JointIndex(0)) + .SetDampingVector(context.get(), Vector1d::Zero()); + bindings = trajopt.AddEffortBoundsAtNormalizedTimes( + *plant, Vector1d(0.5), tau_lb, tau_ub, context.get()); + EXPECT_EQ(bindings.size(), 1); + EXPECT_EQ(bindings[0].evaluator()->num_constraints(), 1); + EXPECT_EQ(bindings[0].evaluator()->lower_bound(), tau_lb); + EXPECT_EQ(bindings[0].evaluator()->upper_bound(), tau_ub); + // The torque that tripped do to high damping is now satisfied. + EXPECT_TRUE(bindings[0].evaluator()->CheckSatisfied(x)); + + // Multiple normalized times. + bindings = trajopt.AddEffortBoundsAtNormalizedTimes( + *plant, Vector2d(0.5, 1.0), tau_lb, tau_ub); + EXPECT_EQ(bindings.size(), 2); + + // Default bounds. + bindings = trajopt.AddEffortBoundsAtNormalizedTimes(*plant, Vector1d(0.25)); + EXPECT_EQ(bindings.size(), 1); + EXPECT_TRUE(CompareMatrices(bindings[0].evaluator()->lower_bound(), + plant->GetEffortLowerLimits(), 1e-14)); + EXPECT_TRUE(CompareMatrices(bindings[0].evaluator()->upper_bound(), + plant->GetEffortUpperLimits(), 1e-14)); +} + TEST_F(KinematicTrajectoryOptimizationTest, AddDurationCost) { EXPECT_EQ(trajopt_.prog().linear_costs().size(), 0); auto binding = trajopt_.AddDurationCost(1.0);