diff --git a/lcmtypes/lcmt_c3_forces.lcm b/lcmtypes/lcmt_c3_forces.lcm new file mode 100644 index 0000000000..f9122bd72a --- /dev/null +++ b/lcmtypes/lcmt_c3_forces.lcm @@ -0,0 +1,11 @@ +package dairlib; + +/* lcmtype containing information to visualize forces in meshcat +*/ +struct lcmt_c3_forces +{ + int64_t utime; + + int32_t num_forces; + lcmt_force forces[num_forces]; +} \ No newline at end of file diff --git a/lcmtypes/lcmt_c3_intermediates.lcm b/lcmtypes/lcmt_c3_intermediates.lcm new file mode 100644 index 0000000000..c709decfb9 --- /dev/null +++ b/lcmtypes/lcmt_c3_intermediates.lcm @@ -0,0 +1,12 @@ +package dairlib; + +struct lcmt_c3_intermediates +{ + int32_t num_points; + int32_t num_total_variables; + + float time_vec[num_points]; + float z_sol[num_total_variables][num_points]; + float delta_sol[num_total_variables][num_points]; + float w_sol[num_total_variables][num_points]; +} \ No newline at end of file diff --git a/lcmtypes/lcmt_c3_output.lcm b/lcmtypes/lcmt_c3_output.lcm new file mode 100644 index 0000000000..b82b674801 --- /dev/null +++ b/lcmtypes/lcmt_c3_output.lcm @@ -0,0 +1,9 @@ +package dairlib; + +struct lcmt_c3_output +{ + int64_t utime; + + lcmt_c3_solution c3_solution; + lcmt_c3_intermediates c3_intermediates; +} diff --git a/lcmtypes/lcmt_c3_solution.lcm b/lcmtypes/lcmt_c3_solution.lcm new file mode 100644 index 0000000000..9648477032 --- /dev/null +++ b/lcmtypes/lcmt_c3_solution.lcm @@ -0,0 +1,14 @@ +package dairlib; + +struct lcmt_c3_solution +{ + int32_t num_points; + int32_t num_state_variables; + int32_t num_contact_variables; + int32_t num_input_variables; + + float time_vec[num_points]; + float x_sol[num_state_variables][num_points]; + float lambda_sol[num_contact_variables][num_points]; + float u_sol[num_input_variables][num_points]; +} diff --git a/lcmtypes/lcmt_c3_state.lcm b/lcmtypes/lcmt_c3_state.lcm new file mode 100644 index 0000000000..1d1f4a2101 --- /dev/null +++ b/lcmtypes/lcmt_c3_state.lcm @@ -0,0 +1,10 @@ +package dairlib; + +struct lcmt_c3_state +{ + int64_t utime; + int32_t num_states; + + float state [num_states]; + string state_names [num_states]; +} \ No newline at end of file diff --git a/lcmtypes/lcmt_force.lcm b/lcmtypes/lcmt_force.lcm new file mode 100644 index 0000000000..935c9635aa --- /dev/null +++ b/lcmtypes/lcmt_force.lcm @@ -0,0 +1,12 @@ +package dairlib; + +/* lcmtype containing information to visualize forces in meshcat +*/ +struct lcmt_force +{ + int64_t utime; + + // These are all expressed in the world frame. + double contact_point[3]; + double contact_force[3]; +} \ No newline at end of file diff --git a/lcmtypes/lcmt_object_state.lcm b/lcmtypes/lcmt_object_state.lcm new file mode 100644 index 0000000000..aa5d1cffa2 --- /dev/null +++ b/lcmtypes/lcmt_object_state.lcm @@ -0,0 +1,16 @@ +package dairlib; + +struct lcmt_object_state +{ + int64_t utime; + string object_name; + + int32_t num_positions; + int32_t num_velocities; + + string position_names [num_positions]; + double position [num_positions]; + + string velocity_names [num_velocities]; + double velocity [num_velocities]; +} diff --git a/lcmtypes/lcmt_timestamped_saved_traj.lcm b/lcmtypes/lcmt_timestamped_saved_traj.lcm new file mode 100644 index 0000000000..7a49bd7c2b --- /dev/null +++ b/lcmtypes/lcmt_timestamped_saved_traj.lcm @@ -0,0 +1,10 @@ +package dairlib; + +/* lcmtype analog for LcmTrajectory WITH TIMESTAMP to enable using a + LcmSerializer to save/load trajectories +*/ + +struct lcmt_timestamped_saved_traj { + int64_t utime; + lcmt_saved_traj saved_traj; +} diff --git a/multibody/geom_geom_collider.cc b/multibody/geom_geom_collider.cc index 60d79fe659..c8c023a068 100644 --- a/multibody/geom_geom_collider.cc +++ b/multibody/geom_geom_collider.cc @@ -1,17 +1,17 @@ #include "multibody/geom_geom_collider.h" -#include "multibody/geom_geom_collider.h" #include "drake/math/rotation_matrix.h" -using Eigen::Vector3d; -using Eigen::Matrix; using drake::EigenPtr; using drake::MatrixX; +using drake::VectorX; using drake::geometry::GeometryId; using drake::geometry::SignedDistancePair; using drake::multibody::JacobianWrtVariable; using drake::multibody::MultibodyPlant; using drake::systems::Context; +using Eigen::Matrix; +using Eigen::Vector3d; namespace dairlib { namespace multibody { @@ -22,8 +22,7 @@ GeomGeomCollider::GeomGeomCollider( const drake::SortedPair geometry_pair) : plant_(plant), geometry_id_A_(geometry_pair.first()), - geometry_id_B_(geometry_pair.second()) { -} + geometry_id_B_(geometry_pair.second()) {} template std::pair> GeomGeomCollider::Eval(const Context& context, @@ -35,22 +34,21 @@ template std::pair> GeomGeomCollider::EvalPolytope( const Context& context, int num_friction_directions, JacobianWrtVariable wrt) { - if (num_friction_directions == 1) { throw std::runtime_error( - "GeomGeomCollider cannot specificy 1 friction direction unless " - "using EvalPlanar."); + "GeomGeomCollider cannot specify 1 friction direction unless " + "using EvalPlanar."); } // Build friction basis - Matrix force_basis( - 2 * num_friction_directions + 1, 3); + Matrix force_basis(2 * num_friction_directions + 1, + 3); force_basis.row(0) << 1, 0, 0; for (int i = 0; i < num_friction_directions; i++) { double theta = (M_PI * i) / num_friction_directions; - force_basis.row(2*i + 1) = Vector3d(0, cos(theta), sin(theta)); - force_basis.row(2*i + 2) = -force_basis.row(2*i + 1); + force_basis.row(2 * i + 1) = Vector3d(0, cos(theta), sin(theta)); + force_basis.row(2 * i + 2) = -force_basis.row(2 * i + 1); } return DoEval(context, force_basis, wrt); } @@ -62,12 +60,41 @@ std::pair> GeomGeomCollider::EvalPlanar( return DoEval(context, planar_normal.transpose(), wrt, true); } - // } +template +std::pair, VectorX> +GeomGeomCollider::CalcWitnessPoints(const Context& context) { + const auto& query_port = plant_.get_geometry_query_input_port(); + const auto& query_object = + query_port.template Eval>(context); + + const SignedDistancePair signed_distance_pair = + query_object.ComputeSignedDistancePairClosestPoints(geometry_id_A_, + geometry_id_B_); + const auto& inspector = query_object.inspector(); + const auto frame_A_id = inspector.GetFrameId(geometry_id_A_); + const auto frame_B_id = inspector.GetFrameId(geometry_id_B_); + const auto& frameA = plant_.GetBodyFromFrameId(frame_A_id)->body_frame(); + const auto& frameB = plant_.GetBodyFromFrameId(frame_B_id)->body_frame(); + + const Vector3d& p_ACa = + inspector.GetPoseInFrame(geometry_id_A_).template cast() * + signed_distance_pair.p_ACa; + const Vector3d& p_BCb = + inspector.GetPoseInFrame(geometry_id_B_).template cast() * + signed_distance_pair.p_BCb; + Vector3d p_WCa = Vector3d::Zero(); + Vector3d p_WCb = Vector3d::Zero(); + plant_.CalcPointsPositions(context, frameA, p_ACa, plant_.world_frame(), + &p_WCa); + plant_.CalcPointsPositions(context, frameB, p_BCb, plant_.world_frame(), + &p_WCb); + return std::pair, VectorX>(p_WCa, p_WCb); +} + template std::pair> GeomGeomCollider::DoEval( const Context& context, Matrix force_basis, JacobianWrtVariable wrt, bool planar) { - const auto& query_port = plant_.get_geometry_query_input_port(); const auto& query_object = query_port.template Eval>(context); @@ -84,27 +111,25 @@ std::pair> GeomGeomCollider::DoEval( const Vector3d& p_ACa = inspector.GetPoseInFrame(geometry_id_A_).template cast() * - signed_distance_pair.p_ACa; + signed_distance_pair.p_ACa; const Vector3d& p_BCb = inspector.GetPoseInFrame(geometry_id_B_).template cast() * - signed_distance_pair.p_BCb; - + signed_distance_pair.p_BCb; - int n_cols = (wrt == JacobianWrtVariable::kV) ? plant_.num_velocities() : - plant_.num_positions(); + int n_cols = (wrt == JacobianWrtVariable::kV) ? plant_.num_velocities() + : plant_.num_positions(); Matrix Jv_WCa(3, n_cols); Matrix Jv_WCb(3, n_cols); - plant_.CalcJacobianTranslationalVelocity(context, wrt, - frameA, p_ACa, plant_.world_frame(), - plant_.world_frame(), &Jv_WCa); - plant_.CalcJacobianTranslationalVelocity(context, wrt, - frameB, p_BCb, plant_.world_frame(), - plant_.world_frame(), &Jv_WCb); + plant_.CalcJacobianTranslationalVelocity(context, wrt, frameA, p_ACa, + plant_.world_frame(), + plant_.world_frame(), &Jv_WCa); + plant_.CalcJacobianTranslationalVelocity(context, wrt, frameB, p_BCb, + plant_.world_frame(), + plant_.world_frame(), &Jv_WCb); - const auto& R_WC = - drake::math::RotationMatrix::MakeFromOneVector( - signed_distance_pair.nhat_BA_W, 0); + const auto& R_WC = drake::math::RotationMatrix::MakeFromOneVector( + signed_distance_pair.nhat_BA_W, 0); // if this is a planar problem, then the basis has one row and encodes // the planar normal direction. @@ -116,7 +141,8 @@ std::pair> GeomGeomCollider::DoEval( force_basis.resize(3, 3); // First row is the contact normal, projected to the plane - force_basis.row(0) = signed_distance_pair.nhat_BA_W - + force_basis.row(0) = + signed_distance_pair.nhat_BA_W - planar_normal * planar_normal.dot(signed_distance_pair.nhat_BA_W); force_basis.row(0).normalize(); @@ -125,7 +151,7 @@ std::pair> GeomGeomCollider::DoEval( force_basis.row(1).normalize(); force_basis.row(2) = -force_basis.row(1); } - // Standard case + // Standard case auto J = force_basis * R_WC.matrix().transpose() * (Jv_WCa - Jv_WCb); return std::pair>(signed_distance_pair.distance, J); } @@ -133,5 +159,4 @@ std::pair> GeomGeomCollider::DoEval( } // namespace multibody } // namespace dairlib - template class dairlib::multibody::GeomGeomCollider; \ No newline at end of file diff --git a/multibody/geom_geom_collider.h b/multibody/geom_geom_collider.h index 5e32b95290..459417582a 100644 --- a/multibody/geom_geom_collider.h +++ b/multibody/geom_geom_collider.h @@ -31,7 +31,6 @@ class GeomGeomCollider { drake::multibody::JacobianWrtVariable wrt = drake::multibody::JacobianWrtVariable::kV); - /// Calculates the distance and contact frame Jacobian. /// Jacobian is ordered [J_n; J_t], and has shape //// (2*num_friction_directions + 1) x (nq or nv), depending @@ -48,12 +47,10 @@ class GeomGeomCollider { /// @param num_friction_directions /// @return A pair with std::pair> EvalPolytope( - const drake::systems::Context& context, - int num_friction_directions, + const drake::systems::Context& context, int num_friction_directions, drake::multibody::JacobianWrtVariable wrt = drake::multibody::JacobianWrtVariable::kV); - /// Calculates the distance and contact frame Jacobian for a 2D planar problem /// Jacobian is ordered [J_n; +J_t; -J_t], and has shape 3 x (nq). /// J_t refers to the (contact_normal x planar_normal) direction @@ -66,6 +63,15 @@ class GeomGeomCollider { drake::multibody::JacobianWrtVariable wrt = drake::multibody::JacobianWrtVariable::kV); + /// Returns the position expressed in the World frame of the closest points on + /// each Geometry. The distance between the two points should match the + /// distance returned by Eval. Note the order of the points return depend on the + /// order that the geometries were added to the MultibodyPlant + /// @param context The context for the MultibodyPlant + /// @return A pair of positions in sorted order as determined by SortedPair + std::pair, drake::VectorX> CalcWitnessPoints( + const drake::systems::Context& context); + private: std::pair> DoEval( const drake::systems::Context& context, diff --git a/solvers/BUILD.bazel b/solvers/BUILD.bazel index 638e9a6106..cca75370e4 100644 --- a/solvers/BUILD.bazel +++ b/solvers/BUILD.bazel @@ -19,6 +19,92 @@ cc_library( ], ) +cc_library( + name = "c3", + srcs = [ + "c3.cc", + "c3_qp.cc", + ], + hdrs = [ + "c3.h", + "c3_options.h", + "c3_qp.h", + ], + copts = [ + "-fopenmp", + ], + linkopts = [ + "-fopenmp", + ], + deps = [ + ":c3_miqp", + ":lcs", + "//solvers:fast_osqp_solver", + "@drake//:drake_shared_library", + ], +) + +cc_library( + name = "c3_miqp", + srcs = + select({ + "//tools:with_gurobi": [ + ":c3_miqp.cc", + ], + "//conditions:default": [ + ":c3_miqp_no_gurobi.cc", + ], + }), + hdrs = [ + "c3.h", + "c3_miqp.h", + "c3_options.h", + ], + deps = + select({ + "//tools:with_gurobi": [ + ":lcs", + "@drake//:drake_shared_library", + "@gurobi//:gurobi_cxx", + ], + "//conditions:default": [ + ":lcs", + "@drake//:drake_shared_library", + ], + }), +) + +cc_library( + name = "c3_output", + srcs = [ + "c3_output.cc", + ], + hdrs = [ + "c3_output.h", + ], + deps = [ + "//lcmtypes:lcmt_robot", + "@drake//:drake_shared_library", + ], +) + +cc_library( + name = "lcs", + srcs = [ + "lcs.cc", + "lcs_factory.cc", + ], + hdrs = [ + "lcs.h", + "lcs_factory.h", + ], + deps = [ + "//multibody:geom_geom_collider", + "//multibody/kinematic", + "@drake//:drake_shared_library", + ], +) + cc_library( name = "constraint_factory", srcs = [ diff --git a/solvers/c3.cc b/solvers/c3.cc new file mode 100644 index 0000000000..425663ed1b --- /dev/null +++ b/solvers/c3.cc @@ -0,0 +1,447 @@ +#include "solvers/c3.h" + +#include +#include + +#include +#include + +#include "solvers/lcs.h" + +#include "drake/solvers/mathematical_program.h" +#include "drake/solvers/moby_lcp_solver.h" +#include "drake/solvers/osqp_solver.h" +#include "drake/solvers/solve.h" + +namespace dairlib { +namespace solvers { + +using Eigen::MatrixXd; +using Eigen::VectorXd; +using std::vector; + +using drake::solvers::MathematicalProgram; +using drake::solvers::MathematicalProgramResult; +using drake::solvers::SolutionResult; +using drake::solvers::SolverOptions; + +using drake::solvers::OsqpSolver; +using drake::solvers::OsqpSolverDetails; +using drake::solvers::Solve; + +C3::CostMatrices::CostMatrices(const std::vector& Q, + const std::vector& R, + const std::vector& G, + const std::vector& U) { + this->Q = Q; + this->R = R; + this->G = G; + this->U = U; +} + +C3::C3(const LCS& lcs, const C3::CostMatrices& costs, + const vector& x_desired, const C3Options& options) + : warm_start_(options.warm_start), + N_((lcs.A_).size()), + n_x_((lcs.A_)[0].cols()), + n_lambda_((lcs.D_)[0].cols()), + n_u_((lcs.B_)[0].cols()), + lcs_(lcs), + cost_matrices_(costs), + x_desired_(x_desired), + options_(options), + h_is_zero_(lcs.H_[0].isZero(0)), + prog_(MathematicalProgram()), + osqp_(OsqpSolver()) { + if (warm_start_) { + warm_start_delta_.resize(options_.admm_iter + 1); + warm_start_binary_.resize(options_.admm_iter + 1); + warm_start_x_.resize(options_.admm_iter + 1); + warm_start_lambda_.resize(options_.admm_iter + 1); + warm_start_u_.resize(options_.admm_iter + 1); + for (size_t iter = 0; iter < options_.admm_iter + 1; ++iter) { + warm_start_delta_[iter].resize(N_); + for (size_t i = 0; i < N_; i++) { + warm_start_delta_[iter][i] = VectorXd::Zero(n_x_ + n_lambda_ + n_u_); + } + warm_start_binary_[iter].resize(N_); + for (size_t i = 0; i < N_; i++) { + warm_start_binary_[iter][i] = VectorXd::Zero(n_lambda_); + } + warm_start_x_[iter].resize(N_ + 1); + for (size_t i = 0; i < N_ + 1; i++) { + warm_start_x_[iter][i] = VectorXd::Zero(n_x_); + } + warm_start_lambda_[iter].resize(N_); + for (size_t i = 0; i < N_; i++) { + warm_start_lambda_[iter][i] = VectorXd::Zero(n_lambda_); + } + warm_start_u_[iter].resize(N_); + for (size_t i = 0; i < N_; i++) { + warm_start_u_[iter][i] = VectorXd::Zero(n_u_); + } + } + } + + auto Dn = lcs.D_.at(0).norm(); + auto An = lcs.A_.at(0).norm(); + AnDn_ = An / Dn; + + for (int i = 0; i < N_; ++i) { + lcs_.D_.at(i) *= AnDn_; + lcs_.E_.at(i) /= AnDn_; + lcs_.c_.at(i) /= AnDn_; + lcs_.H_.at(i) /= AnDn_; + } + + x_ = vector(); + u_ = vector(); + lambda_ = vector(); + + z_sol_ = std::make_unique>(); + x_sol_ = std::make_unique>(); + lambda_sol_ = std::make_unique>(); + u_sol_ = std::make_unique>(); + w_sol_ = std::make_unique>(); + delta_sol_ = std::make_unique>(); + for (int i = 0; i < N_; ++i) { + z_sol_->push_back(Eigen::VectorXd::Zero(n_x_ + n_lambda_ + n_u_)); + x_sol_->push_back(Eigen::VectorXd::Zero(n_x_)); + lambda_sol_->push_back(Eigen::VectorXd::Zero(n_lambda_)); + u_sol_->push_back(Eigen::VectorXd::Zero(n_u_)); + w_sol_->push_back(Eigen::VectorXd::Zero(n_x_ + n_lambda_ + n_u_)); + delta_sol_->push_back(Eigen::VectorXd::Zero(n_x_ + n_lambda_ + n_u_)); + } + + for (int i = 0; i < N_ + 1; i++) { + x_.push_back(prog_.NewContinuousVariables(n_x_, "x" + std::to_string(i))); + if (i < N_) { + u_.push_back(prog_.NewContinuousVariables(n_u_, "k" + std::to_string(i))); + lambda_.push_back(prog_.NewContinuousVariables( + n_lambda_, "lambda" + std::to_string(i))); + } + } + + // initialize the constraint bindings + initial_state_constraint_ = nullptr; + dynamics_constraints_.resize(N_); + target_cost_.resize(N_ + 1); + + MatrixXd LinEq(n_x_, 2 * n_x_ + n_lambda_ + n_u_); + LinEq.block(0, n_x_ + n_lambda_ + n_u_, n_x_, n_x_) = + -1 * MatrixXd::Identity(n_x_, n_x_); + for (int i = 0; i < N_; i++) { + LinEq.block(0, 0, n_x_, n_x_) = lcs.A_.at(i); + LinEq.block(0, n_x_, n_x_, n_lambda_) = lcs.D_.at(i); + LinEq.block(0, n_x_ + n_lambda_, n_x_, n_u_) = lcs.B_.at(i); + + dynamics_constraints_[i] = + prog_ + .AddLinearEqualityConstraint( + LinEq, -lcs.d_.at(i), + {x_.at(i), lambda_.at(i), u_.at(i), x_.at(i + 1)}) + .evaluator() + .get(); + } + input_costs_.resize(N_); + for (int i = 0; i < N_ + 1; i++) { + target_cost_[i] = + prog_ + .AddQuadraticCost(2 * cost_matrices_.Q.at(i), -2 * cost_matrices_.Q.at(i) * x_desired_.at(i), + x_.at(i), 1) + .evaluator() + .get(); + if (i < N_) { + input_costs_[i] = + prog_ + .AddQuadraticCost(2 * cost_matrices_.R.at(i), VectorXd::Zero(n_u_), u_.at(i), 1) + .evaluator(); + } + } +} + +void C3::UpdateLCS(const LCS& lcs) { + DRAKE_DEMAND(lcs.A_.size() == N_); + DRAKE_DEMAND(lcs.A_[0].rows() == n_x_); + DRAKE_DEMAND(lcs.A_[0].cols() == n_x_); + DRAKE_DEMAND(lcs.D_[0].cols() == n_lambda_); + DRAKE_DEMAND(lcs.B_[0].cols() == n_u_); + + lcs_ = lcs; + h_is_zero_ = lcs_.H_[0].isZero(0); + + // Scaling dynamics matrices constraint for better numerics + // This only scales lambda + auto Dn = lcs_.D_[0].norm(); + auto An = lcs_.A_[0].norm(); + AnDn_ = An / Dn; + + for (int i = 0; i < N_; ++i) { + lcs_.D_.at(i) *= AnDn_; + lcs_.E_.at(i) /= AnDn_; + lcs_.c_.at(i) /= AnDn_; + lcs_.H_.at(i) /= AnDn_; + } + + MatrixXd LinEq = MatrixXd::Zero(n_x_, 2 * n_x_ + n_lambda_ + n_u_); + LinEq.block(0, n_x_ + n_u_ + n_lambda_, n_x_, n_x_) = + -1 * MatrixXd::Identity(n_x_, n_x_); + for (int i = 0; i < N_; i++) { + LinEq.block(0, 0, n_x_, n_x_) = lcs_.A_.at(i); + LinEq.block(0, n_x_, n_x_, n_lambda_) = lcs_.D_.at(i); + LinEq.block(0, n_x_ + n_lambda_, n_x_, n_u_) = lcs_.B_.at(i); + + dynamics_constraints_[i]->UpdateCoefficients(LinEq, -lcs.d_.at(i)); + } +} + +void C3::UpdateTarget(const std::vector& x_des) { + x_desired_ = x_des; + for (int i = 0; i < N_ + 1; i++) { + target_cost_[i]->UpdateCoefficients(2 * cost_matrices_.Q.at(i), + -2 * cost_matrices_.Q.at(i) * x_desired_.at(i)); + } +} + +void C3::Solve(const VectorXd& x0) { + auto start = std::chrono::high_resolution_clock::now(); + if (initial_state_constraint_) { + initial_state_constraint_->UpdateCoefficients( + MatrixXd::Identity(n_x_, n_x_), x0); + } else { + initial_state_constraint_ = + prog_ + .AddLinearEqualityConstraint(MatrixXd::Identity(n_x_, n_x_), x0, + x_[0]) + .evaluator(); + } + VectorXd delta_init = VectorXd::Zero(n_x_ + n_lambda_ + n_u_); + if (options_.delta_option == 1) { + delta_init.head(n_x_) = x0; + } + std::vector delta(N_, delta_init); + std::vector w(N_, VectorXd::Zero(n_x_ + n_lambda_ + n_u_)); + vector Gv = cost_matrices_.G; + + for (int i = 0; i < N_; ++i) { + input_costs_[i]->UpdateCoefficients(2 * cost_matrices_.R.at(i), + -2 * cost_matrices_.R.at(i) * u_sol_->at(i)); + } + + for (int iter = 0; iter < options_.admm_iter; iter++) { + ADMMStep(x0, &delta, &w, &Gv, iter); + } + + vector WD(N_, VectorXd::Zero(n_x_ + n_lambda_ + n_u_)); + for (int i = 0; i < N_; i++) { + WD.at(i) = delta.at(i) - w.at(i); + } + + vector zfin = SolveQP(x0, Gv, WD, options_.admm_iter, true); + + *w_sol_ = w; + *delta_sol_ = delta; + + if (!options_.end_on_qp_step) { + *z_sol_ = delta; + z_sol_->at(0).segment(0, n_x_) = x0; + x_sol_->at(0) = x0; + for (int i = 1; i < N_; ++i) { + z_sol_->at(i).segment(0, n_x_) = + lcs_.A_.at(i - 1) * x_sol_->at(i - 1) + + lcs_.B_.at(i - 1) * u_sol_->at(i - 1) + + lcs_.D_.at(i - 1) * lambda_sol_->at(i - 1) + lcs_.d_.at(i - 1); + } + } + + // Undoing to scaling to put variables back into correct units + // This only scales lambda + for (int i = 0; i < N_; ++i) { + lambda_sol_->at(i) *= AnDn_; + z_sol_->at(i).segment(n_x_, n_lambda_) *= AnDn_; + } + + auto finish = std::chrono::high_resolution_clock::now(); + auto elapsed = finish - start; + solve_time_ = + std::chrono::duration_cast(elapsed).count() / + 1e6; +} + +void C3::ADMMStep(const VectorXd& x0, vector* delta, + vector* w, vector* Gv, + int admm_iteration) { + vector WD(N_, VectorXd::Zero(n_x_ + n_lambda_ + n_u_)); + + for (int i = 0; i < N_; i++) { + WD.at(i) = delta->at(i) - w->at(i); + } + + vector z = SolveQP(x0, *Gv, WD, admm_iteration, true); + + vector ZW(N_, VectorXd::Zero(n_x_ + n_lambda_ + n_u_)); + for (int i = 0; i < N_; i++) { + ZW[i] = w->at(i) + z[i]; + } + + if (cost_matrices_.U[0].isZero(0)) { + *delta = SolveProjection(*Gv, ZW, admm_iteration); + + } else { + *delta = SolveProjection(cost_matrices_.U, ZW, admm_iteration); + } + + for (int i = 0; i < N_; i++) { + w->at(i) = w->at(i) + z[i] - delta->at(i); + w->at(i) = w->at(i) / options_.rho_scale; + Gv->at(i) = Gv->at(i) * options_.rho_scale; + } +} + +vector C3::SolveQP(const VectorXd& x0, const vector& G, + const vector& WD, int admm_iteration, + bool is_final_solve) { + if (h_is_zero_ == 1) { // No dependence on u, so just simulate passive system + drake::solvers::MobyLCPSolver LCPSolver; + VectorXd lambda0; + LCPSolver.SolveLcpLemke(lcs_.F_[0], lcs_.E_[0] * x0 + lcs_.c_[0], &lambda0); + constraints_.push_back(prog_.AddLinearConstraint(lambda_[0] == lambda0)); + } + + for (auto& cost : costs_) { + prog_.RemoveCost(cost); + } + costs_.clear(); + + for (int i = 0; i < N_ + 1; i++) { + if (i < N_) { + costs_.push_back(prog_.AddQuadraticCost( + 2 * G.at(i).block(0, 0, n_x_, n_x_), + -2 * G.at(i).block(0, 0, n_x_, n_x_) * WD.at(i).segment(0, n_x_), + x_.at(i), 1)); + costs_.push_back(prog_.AddQuadraticCost( + 2 * G.at(i).block(n_x_, n_x_, n_lambda_, n_lambda_), + -2 * G.at(i).block(n_x_, n_x_, n_lambda_, n_lambda_) * + WD.at(i).segment(n_x_, n_lambda_), + lambda_.at(i), 1)); + costs_.push_back(prog_.AddQuadraticCost( + 2 * G.at(i).block(n_x_ + n_lambda_, n_x_ + n_lambda_, n_u_, n_u_), + -2 * G.at(i).block(n_x_ + n_lambda_, n_x_ + n_lambda_, n_u_, n_u_) * + WD.at(i).segment(n_x_ + n_lambda_, n_u_), + u_.at(i), 1)); + } + } + + // /// initialize decision variables to warm start + if (warm_start_) { + int index = solve_time_ / lcs_.dt_; + double weight = (solve_time_ - index * lcs_.dt_) / lcs_.dt_; + for (int i = 0; i < N_ - 1; i++) { + prog_.SetInitialGuess(x_[i], + (1 - weight) * warm_start_x_[admm_iteration][i] + + weight * warm_start_x_[admm_iteration][i + 1]); + prog_.SetInitialGuess( + lambda_[i], (1 - weight) * warm_start_lambda_[admm_iteration][i] + + weight * warm_start_lambda_[admm_iteration][i + 1]); + prog_.SetInitialGuess(u_[i], + (1 - weight) * warm_start_u_[admm_iteration][i] + + weight * warm_start_u_[admm_iteration][i + 1]); + } + prog_.SetInitialGuess(x_[0], x0); + prog_.SetInitialGuess(x_[N_], warm_start_x_[admm_iteration][N_]); + } + + MathematicalProgramResult result = osqp_.Solve(prog_); + + if (result.is_success()) { + for (int i = 0; i < N_; i++) { + if (is_final_solve) { + x_sol_->at(i) = result.GetSolution(x_[i]); + lambda_sol_->at(i) = result.GetSolution(lambda_[i]); + u_sol_->at(i) = result.GetSolution(u_[i]); + } + z_sol_->at(i).segment(0, n_x_) = result.GetSolution(x_[i]); + z_sol_->at(i).segment(n_x_, n_lambda_) = result.GetSolution(lambda_[i]); + z_sol_->at(i).segment(n_x_ + n_lambda_, n_u_) = result.GetSolution(u_[i]); + + if (warm_start_) { + // update warm start parameters + warm_start_x_[admm_iteration][i] = result.GetSolution(x_[i]); + warm_start_lambda_[admm_iteration][i] = result.GetSolution(lambda_[i]); + warm_start_u_[admm_iteration][i] = result.GetSolution(u_[i]); + } + } + if (warm_start_) { + warm_start_x_[admm_iteration][N_] = result.GetSolution(x_[N_]); + } + } + + return *z_sol_; +} + +vector C3::SolveProjection(const vector& G, + vector& WZ, int admm_iteration) { + vector deltaProj(N_, VectorXd::Zero(n_x_ + n_lambda_ + n_u_)); + int i; + + if (options_.num_threads > 0) { + omp_set_dynamic(0); // Explicitly disable dynamic teams + omp_set_num_threads(options_.num_threads); // Set number of threads + omp_set_nested(1); + } + +#pragma omp parallel for num_threads(options_.num_threads) + for (i = 0; i < N_; i++) { + if (warm_start_) { + if (i == N_ - 1) { + deltaProj[i] = + SolveSingleProjection(G[i], WZ[i], lcs_.E_[i], lcs_.F_[i], + lcs_.H_[i], lcs_.c_[i], admm_iteration, -1); + } else { + deltaProj[i] = SolveSingleProjection(G[i], WZ[i], lcs_.E_[i], + lcs_.F_[i], lcs_.H_[i], lcs_.c_[i], + admm_iteration, i + 1); + } + } else { + deltaProj[i] = + SolveSingleProjection(G[i], WZ[i], lcs_.E_[i], lcs_.F_[i], lcs_.H_[i], + lcs_.c_[i], admm_iteration, -1); + } + } + + return deltaProj; +} + +void C3::AddLinearConstraint(Eigen::MatrixXd& A, VectorXd& lower_bound, + VectorXd& upper_bound, int constraint) { + if (constraint == 1) { + for (int i = 1; i < N_; i++) { + user_constraints_.push_back( + prog_.AddLinearConstraint(A, lower_bound, upper_bound, x_.at(i))); + } + } + + if (constraint == 2) { + for (int i = 0; i < N_; i++) { + user_constraints_.push_back( + prog_.AddLinearConstraint(A, lower_bound, upper_bound, u_.at(i))); + } + } + + if (constraint == 3) { + for (int i = 0; i < N_; i++) { + user_constraints_.push_back(prog_.AddLinearConstraint( + A, lower_bound, upper_bound, lambda_.at(i))); + } + } +} + +void C3::RemoveConstraints() { + for (auto& userconstraint : user_constraints_) { + prog_.RemoveConstraint(userconstraint); + } + user_constraints_.clear(); +} + +} // namespace solvers +} // namespace dairlib diff --git a/solvers/c3.h b/solvers/c3.h new file mode 100644 index 0000000000..db90502224 --- /dev/null +++ b/solvers/c3.h @@ -0,0 +1,157 @@ +#pragma once + +#include + +#include + +#include "solvers/c3_options.h" +#include "solvers/lcs.h" + +#include "drake/solvers/mathematical_program.h" +#include "drake/solvers/osqp_solver.h" +#include "drake/solvers/solve.h" + +namespace dairlib { +namespace solvers { + +class C3 { + public: + struct CostMatrices { + CostMatrices(const std::vector& Q, + const std::vector& R, + const std::vector& G, + const std::vector& U); + std::vector Q; + std::vector R; + std::vector G; + std::vector U; + }; + /// @param LCS LCS parameters + /// @param Q, R, G, U Cost function parameters + C3(const LCS& LCS, const CostMatrices& costs, + const std::vector& x_desired, const C3Options& options); + + virtual ~C3() = default; + + /// Solve the MPC problem + /// @param x0 The initial state of the system + /// @param delta A pointer to the copy variable solution + /// @param w A pointer to the scaled dual variable solution + /// @return The first control action to take, u[0] + void Solve(const Eigen::VectorXd& x0); + + /// Update the LCS without needing to reconstruct the C3 object + void UpdateLCS(const LCS& lcs); + /// Update the target without needing to reconstruct the C3 object + void UpdateTarget(const std::vector& x_des); + + /// allow users to add constraints (adds for all timesteps) + /// @param A, lower_bound, upper_bound lower_bound <= A x <= upper_bound + /// @param constraint inputconstraint, stateconstraint, forceconstraint + void AddLinearConstraint(Eigen::MatrixXd& A, Eigen::VectorXd& lower_bound, + Eigen::VectorXd& upper_bound, int constraint); + + /// allow user to remove all constraints added by AddLinearConstraint + void RemoveConstraints(); + + /// Solve a single ADMM step + /// @param x0 The initial state of the system + /// @param delta The copy variables from the previous step + /// @param w The scaled dual variables from the previous step + /// @param G A pointer to the G variables from previous step + void ADMMStep(const Eigen::VectorXd& x0, std::vector* delta, + std::vector* w, + std::vector* G, int admm_iteration); + + /// Solve a single QP + /// @param x0 The initial state of the system + /// @param WD A pointer to the (w - delta) variables + /// @param G A pointer to the G variables from previous step + std::vector SolveQP(const Eigen::VectorXd& x0, + const std::vector& G, + const std::vector& WD, + int admm_iteration, + bool is_final_solve = false); + + /// Solve the projection problem for all timesteps + /// @param WZ A pointer to the (z + w) variables + /// @param G A pointer to the G variables from previous step + std::vector SolveProjection( + const std::vector& G, std::vector& WZ, + int admm_iteration); + + /// Solve a single projection step + /// @param E, F, H, c LCS parameters + /// @param U A pointer to the U variables + /// @param delta_c A pointer to the copy of (z + w) variables + virtual Eigen::VectorXd SolveSingleProjection( + const Eigen::MatrixXd& U, const Eigen::VectorXd& delta_c, + const Eigen::MatrixXd& E, const Eigen::MatrixXd& F, + const Eigen::MatrixXd& H, const Eigen::VectorXd& c, + const int admm_iteration, const int& warm_start_index) = 0; + + void SetOsqpSolverOptions(const drake::solvers::SolverOptions& options) { + prog_.SetSolverOptions(options); + } + + std::vector GetFullSolution() { return *z_sol_; } + std::vector GetStateSolution() { return *x_sol_; } + std::vector GetForceSolution() { return *lambda_sol_; } + std::vector GetInputSolution() { return *u_sol_; } + std::vector GetDualDeltaSolution() { return *delta_sol_; } + std::vector GetDualWSolution() { return *w_sol_; } + + protected: + std::vector> warm_start_delta_; + std::vector> warm_start_binary_; + std::vector> warm_start_x_; + std::vector> warm_start_lambda_; + std::vector> warm_start_u_; + bool warm_start_; + const int N_; + const int n_x_; // n + const int n_lambda_; // m + const int n_u_; // k + + private: + LCS lcs_; + double AnDn_ = 1.0; // Scaling factor for lambda + const CostMatrices cost_matrices_; + std::vector x_desired_; + const C3Options options_; + double solve_time_ = 0; + bool h_is_zero_; + + drake::solvers::MathematicalProgram prog_; + // QP step variables + drake::solvers::OsqpSolver osqp_; + std::vector x_; + std::vector u_; + std::vector lambda_; + // QP step constraints + std::shared_ptr initial_state_constraint_; + std::vector dynamics_constraints_; + std::vector> + constraints_; + std::vector> + user_constraints_; + + /// Projection step variables are defined outside of the MathematicalProgram + /// interface + + std::vector target_cost_; + std::vector> costs_; + std::vector> input_costs_; + + // Solutions + std::unique_ptr> x_sol_; + std::unique_ptr> lambda_sol_; + std::unique_ptr> u_sol_; + + std::unique_ptr> z_sol_; + std::unique_ptr> delta_sol_; + std::unique_ptr> w_sol_; +}; + +} // namespace solvers +} // namespace dairlib diff --git a/solvers/c3_miqp.cc b/solvers/c3_miqp.cc new file mode 100644 index 0000000000..02d1f57186 --- /dev/null +++ b/solvers/c3_miqp.cc @@ -0,0 +1,126 @@ +#include "solvers/c3_miqp.h" + +#include "gurobi_c++.h" + +namespace dairlib { +namespace solvers { + +using Eigen::MatrixXd; +using Eigen::VectorXd; +using std::vector; + +C3MIQP::C3MIQP(const LCS& LCS, const CostMatrices& costs, + const vector& xdesired, const C3Options& options) + : C3(LCS, costs, xdesired, options), M_(options.M) {} + +VectorXd C3MIQP::SolveSingleProjection(const MatrixXd& U, + const VectorXd& delta_c, + const MatrixXd& E, const MatrixXd& F, + const MatrixXd& H, const VectorXd& c, + const int admm_iteration, + const int& warm_start_index) { + // Create an environment + GRBEnv env; + // env.set("LogToConsole", "0"); + env.set("OutputFlag", "0"); + env.set("Threads", "0"); + env.start(); + // set up linear term in cost + VectorXd cost_lin = -2 * delta_c.transpose() * U; + + // set up for constraints (Ex + F \lambda + Hu + c >= 0) + MatrixXd Mcons1(n_lambda_, n_x_ + n_lambda_ + n_u_); + Mcons1 << E, F, H; + + // set up for constraints (\lambda >= 0) + MatrixXd MM1 = MatrixXd::Zero(n_lambda_, n_x_); + MatrixXd MM2 = MatrixXd::Identity(n_lambda_, n_lambda_); + MatrixXd MM3 = MatrixXd::Zero(n_lambda_, n_u_); + MatrixXd Mcons2(n_lambda_, n_x_ + n_lambda_ + n_u_); + Mcons2 << MM1, MM2, MM3; + + GRBModel model = GRBModel(env); + + GRBVar delta_k[n_x_ + n_lambda_ + n_u_]; + GRBVar binary[n_lambda_]; + + for (int i = 0; i < n_lambda_; i++) { + binary[i] = model.addVar(0.0, 1.0, 0.0, GRB_BINARY); + if (warm_start_index != -1) { + binary[i].set(GRB_DoubleAttr_Start, + warm_start_binary_[admm_iteration][warm_start_index](i)); + } + } + + for (int i = 0; i < n_x_ + n_lambda_ + n_u_; i++) { + delta_k[i] = model.addVar(-kVariableBounds, kVariableBounds, 0.0, GRB_CONTINUOUS); + if (warm_start_index != -1) { + delta_k[i].set(GRB_DoubleAttr_Start, + warm_start_delta_[admm_iteration][warm_start_index](i)); + } + } + + GRBQuadExpr obj = 0; + + for (int i = 0; i < n_x_ + n_lambda_ + n_u_; i++) { + obj.addTerm(cost_lin(i), delta_k[i]); + obj.addTerm(U(i, i), delta_k[i], delta_k[i]); + } + + model.setObjective(obj, GRB_MINIMIZE); + + double coeff[n_x_ + n_lambda_ + n_u_]; + double coeff2[n_x_ + n_lambda_ + n_u_]; + + for (int i = 0; i < n_lambda_; i++) { + GRBLinExpr lambda_expr = 0; + + /// convert VectorXd to double + for (int j = 0; j < n_x_ + n_lambda_ + n_u_; j++) { + coeff[j] = Mcons2(i, j); + } + + lambda_expr.addTerms(coeff, delta_k, n_x_ + n_lambda_ + n_u_); + model.addConstr(lambda_expr >= 0); + model.addConstr(lambda_expr <= M_ * (1 - binary[i])); + + GRBLinExpr activation_expr = 0; + + /// convert VectorXd to double + for (int j = 0; j < n_x_ + n_lambda_ + n_u_; j++) { + coeff2[j] = Mcons1(i, j); + } + + activation_expr.addTerms(coeff2, delta_k, n_x_ + n_lambda_ + n_u_); + model.addConstr(activation_expr + c(i) >= 0); + model.addConstr(activation_expr + c(i) <= M_ * binary[i]); + } + + model.optimize(); + + VectorXd delta_kc(n_x_ + n_lambda_ + n_u_); + VectorXd binaryc(n_lambda_); + for (int i = 0; i < n_x_ + n_lambda_ + n_u_; i++) { + delta_kc(i) = delta_k[i].get(GRB_DoubleAttr_X); + } + for (int i = 0; i < n_lambda_; i++) { + binaryc(i) = binary[i].get(GRB_DoubleAttr_X); + } + + if (warm_start_index != -1) { + warm_start_delta_[admm_iteration][warm_start_index] = delta_kc; + warm_start_binary_[admm_iteration][warm_start_index] = binaryc; + } + return delta_kc; +} + +std::vector C3MIQP::GetWarmStartDelta() const { + return warm_start_delta_[0]; +} + +std::vector C3MIQP::GetWarmStartBinary() const { + return warm_start_binary_[0]; +} + +} // namespace solvers +} // namespace dairlib diff --git a/solvers/c3_miqp.h b/solvers/c3_miqp.h new file mode 100644 index 0000000000..eb61cc1479 --- /dev/null +++ b/solvers/c3_miqp.h @@ -0,0 +1,38 @@ +#pragma once + +#include + +#include + +#include "solvers/c3.h" +#include "solvers/lcs.h" + +namespace dairlib { +namespace solvers { + +static const double kVariableBounds = 100; + +class C3MIQP final : public C3 { + public: + /// Default constructor for time-varying LCS + C3MIQP(const LCS& LCS, const CostMatrices& costs, + const std::vector& xdesired, + const C3Options& options); + + ~C3MIQP() override = default; + + /// Virtual projection method + Eigen::VectorXd SolveSingleProjection( + const Eigen::MatrixXd& U, const Eigen::VectorXd& delta_c, + const Eigen::MatrixXd& E, const Eigen::MatrixXd& F, + const Eigen::MatrixXd& H, const Eigen::VectorXd& c, + const int admm_iteration, const int& warm_start_index = -1) override; + + std::vector GetWarmStartDelta() const; + std::vector GetWarmStartBinary() const; + + const int M_; +}; + +} // namespace solvers +} // namespace dairlib diff --git a/solvers/c3_miqp_no_gurobi.cc b/solvers/c3_miqp_no_gurobi.cc new file mode 100644 index 0000000000..be3dc728d4 --- /dev/null +++ b/solvers/c3_miqp_no_gurobi.cc @@ -0,0 +1,43 @@ +#include "solvers/c3_miqp.h" + +namespace dairlib { +namespace solvers { + +using Eigen::MatrixXd; +using Eigen::VectorXd; +using std::vector; + +C3MIQP::C3MIQP(const LCS& LCS, const CostMatrices& costs, + const vector& xdesired, const C3Options& options) + : C3(LCS, costs, xdesired, options), M_(options.M) { + throw std::runtime_error( + "The Gurobi bindings were not compiled. You'll need to use a different " + "projection method. To compile with Gurobi, follow the instruction " + "listed in install/README"); +} + +VectorXd C3MIQP::SolveSingleProjection(const MatrixXd& U, + const VectorXd& delta_c, + const MatrixXd& E, const MatrixXd& F, + const MatrixXd& H, const VectorXd& c, + const int admm_iteration, + const int& warm_start_index) { + throw std::runtime_error( + "The Gurobi bindings were not compiled. You'll need to use a different " + "projection method."); +} + +std::vector C3MIQP::GetWarmStartDelta() const { + throw std::runtime_error( + "The Gurobi bindings were not compiled. You'll need to use a different " + "projection method."); +} + +std::vector C3MIQP::GetWarmStartBinary() const { + throw std::runtime_error( + "The Gurobi bindings were not compiled. You'll need to use a different " + "projection method."); +} + +} // namespace solvers +} // namespace dairlib diff --git a/solvers/c3_options.h b/solvers/c3_options.h new file mode 100644 index 0000000000..159883b11a --- /dev/null +++ b/solvers/c3_options.h @@ -0,0 +1,170 @@ +#pragma once +#include + +#include "drake/common/yaml/yaml_read_archive.h" + +struct C3Options { + // Hyperparameters + int admm_iter; // total number of ADMM iterations + float rho; // initial value of the rho parameter + float rho_scale; // scaling of rho parameter (/rho = rho_scale * /rho) + int num_threads; // 0 is dynamic, greater than 0 for a fixed count + int delta_option; // different options for delta update + std::string projection_type; + std::string contact_model; + double M = 1000; // big M value for MIQP + bool warm_start; + bool use_predicted_x0; + bool end_on_qp_step; + bool use_robust_formulation; + double solve_time_filter_alpha; + double publish_frequency; + + std::vector u_horizontal_limits; + std::vector u_vertical_limits; + std::vector workspace_limits; + double workspace_margins; + + int N; + double gamma; + std::vector mu; + double dt; + double solve_dt; + int num_friction_directions; + int num_contacts; + + // See comments below for how we parse the .yaml into the cost matrices + Eigen::MatrixXd Q; + Eigen::MatrixXd R; + Eigen::MatrixXd G; + Eigen::MatrixXd U; + + // Quick scaling of the cost matrices. + // Q = w_Q * diag(q_vector) + // R = w_R * diag(r_vector) + // G = w_G * diag(g_vector) + // U = w_U * diag(u_vector) + double w_Q; + double w_R; + double w_G; + double w_U; + + // Unused except when parsing the costs from a yaml + // We assume a diagonal Q, R, G, U matrix, so we can just specify the diagonal + // terms as *_vector. To make indexing even easier, we split the parsing of + // the g_vector and u_vector into the x, lambda, and u terms. The Stewart and + // Trinkle contact model uses *_gamma, *_lambda_n, *_lambda_t while the + // Anitescu model uses *_lambda. + std::vector q_vector; + std::vector r_vector; + + std::vector g_vector; + std::vector g_x; + std::vector g_gamma; + std::vector g_lambda_n; + std::vector g_lambda_t; + std::vector g_lambda; + std::vector g_u; + + std::vector u_vector; + std::vector u_x; + std::vector u_gamma; + std::vector u_lambda_n; + std::vector u_lambda_t; + std::vector u_lambda; + std::vector u_u; + + + template + void Serialize(Archive* a) { + a->Visit(DRAKE_NVP(admm_iter)); + a->Visit(DRAKE_NVP(rho)); + a->Visit(DRAKE_NVP(rho_scale)); + a->Visit(DRAKE_NVP(num_threads)); + a->Visit(DRAKE_NVP(delta_option)); + a->Visit(DRAKE_NVP(contact_model)); + a->Visit(DRAKE_NVP(projection_type)); + if (projection_type == "QP") { + DRAKE_DEMAND(contact_model == "anitescu"); + } + a->Visit(DRAKE_NVP(warm_start)); + a->Visit(DRAKE_NVP(use_predicted_x0)); + a->Visit(DRAKE_NVP(end_on_qp_step)); + a->Visit(DRAKE_NVP(use_robust_formulation)); + a->Visit(DRAKE_NVP(solve_time_filter_alpha)); + a->Visit(DRAKE_NVP(publish_frequency)); + + a->Visit(DRAKE_NVP(workspace_limits)); + a->Visit(DRAKE_NVP(u_horizontal_limits)); + a->Visit(DRAKE_NVP(u_vertical_limits)); + a->Visit(DRAKE_NVP(workspace_margins)); + + a->Visit(DRAKE_NVP(mu)); + a->Visit(DRAKE_NVP(dt)); + a->Visit(DRAKE_NVP(solve_dt)); + a->Visit(DRAKE_NVP(num_friction_directions)); + a->Visit(DRAKE_NVP(num_contacts)); + + a->Visit(DRAKE_NVP(N)); + a->Visit(DRAKE_NVP(gamma)); + a->Visit(DRAKE_NVP(w_Q)); + a->Visit(DRAKE_NVP(w_R)); + a->Visit(DRAKE_NVP(w_G)); + a->Visit(DRAKE_NVP(w_U)); + a->Visit(DRAKE_NVP(q_vector)); + a->Visit(DRAKE_NVP(r_vector)); + a->Visit(DRAKE_NVP(g_x)); + a->Visit(DRAKE_NVP(g_gamma)); + a->Visit(DRAKE_NVP(g_lambda_n)); + a->Visit(DRAKE_NVP(g_lambda_t)); + a->Visit(DRAKE_NVP(g_lambda)); + a->Visit(DRAKE_NVP(g_u)); + a->Visit(DRAKE_NVP(u_x)); + a->Visit(DRAKE_NVP(u_gamma)); + a->Visit(DRAKE_NVP(u_lambda_n)); + a->Visit(DRAKE_NVP(u_lambda_t)); + a->Visit(DRAKE_NVP(u_lambda)); + a->Visit(DRAKE_NVP(u_u)); + + g_vector = std::vector(); + g_vector.insert(g_vector.end(), g_x.begin(), g_x.end()); + if (contact_model == "stewart_and_trinkle") { + g_vector.insert(g_vector.end(), g_gamma.begin(), g_gamma.end()); + g_vector.insert(g_vector.end(), g_lambda_n.begin(), g_lambda_n.end()); + g_vector.insert(g_vector.end(), g_lambda_t.begin(), g_lambda_t.end()); + } else { + g_vector.insert(g_vector.end(), g_lambda.begin(), g_lambda.end()); + } + + g_vector.insert(g_vector.end(), g_u.begin(), g_u.end()); + u_vector = std::vector(); + u_vector.insert(u_vector.end(), u_x.begin(), u_x.end()); + if (contact_model == "stewart_and_trinkle") { + u_vector.insert(u_vector.end(), u_gamma.begin(), u_gamma.end()); + u_vector.insert(u_vector.end(), u_lambda_n.begin(), u_lambda_n.end()); + u_vector.insert(u_vector.end(), u_lambda_t.begin(), u_lambda_t.end()); + } else { + u_vector.insert(u_vector.end(), u_lambda.begin(), u_lambda.end()); + } + u_vector.insert(u_vector.end(), u_u.begin(), u_u.end()); + + Eigen::VectorXd q = Eigen::Map( + this->q_vector.data(), this->q_vector.size()); + Eigen::VectorXd r = Eigen::Map( + this->r_vector.data(), this->r_vector.size()); + Eigen::VectorXd g = Eigen::Map( + this->g_vector.data(), this->g_vector.size()); + Eigen::VectorXd u = Eigen::Map( + this->u_vector.data(), this->u_vector.size()); + + DRAKE_DEMAND(g_lambda.size() == num_contacts * num_friction_directions * 2); + DRAKE_DEMAND(u_lambda.size() == num_contacts * num_friction_directions * 2); + DRAKE_DEMAND(mu.size() == num_contacts); + DRAKE_DEMAND(g.size() == u.size()); + + Q = w_Q * q.asDiagonal(); + R = w_R * r.asDiagonal(); + G = w_G * g.asDiagonal(); + U = w_U * u.asDiagonal(); + } +}; \ No newline at end of file diff --git a/solvers/c3_output.cc b/solvers/c3_output.cc new file mode 100644 index 0000000000..2f68be4b3d --- /dev/null +++ b/solvers/c3_output.cc @@ -0,0 +1,81 @@ +#include "c3_output.h" + +using Eigen::VectorXd; +using Eigen::VectorXf; +using std::vector; + +namespace dairlib { + +C3Output::C3Output(C3Solution c3_sol, C3Intermediates c3_intermediates) + : c3_solution_(c3_sol), c3_intermediates_(c3_intermediates) {} + +lcmt_c3_output C3Output::GenerateLcmObject(double time) const { + lcmt_c3_output c3_output; + lcmt_c3_solution c3_solution; + lcmt_c3_intermediates c3_intermediates; + c3_output.utime = static_cast(1e6 * time); + + c3_solution.num_points = c3_solution_.time_vector_.size(); + int knot_points = c3_solution.num_points; + c3_solution.num_state_variables = c3_solution_.x_sol_.rows(); + c3_solution.num_contact_variables = c3_solution_.lambda_sol_.rows(); + c3_solution.num_input_variables = c3_solution_.u_sol_.rows(); + c3_solution.time_vec.reserve(knot_points); + c3_solution.x_sol = vector>(c3_solution.num_state_variables, + vector(knot_points)); + c3_solution.lambda_sol = vector>( + c3_solution.num_contact_variables, vector(knot_points)); + c3_solution.u_sol = vector>(c3_solution.num_input_variables, + vector(knot_points)); + + c3_solution.time_vec = vector( + c3_solution_.time_vector_.data(), + c3_solution_.time_vector_.data() + c3_solution_.time_vector_.size()); + + c3_intermediates.num_total_variables = c3_intermediates_.delta_.rows(); + c3_intermediates.num_points = c3_solution.num_points; + c3_intermediates.time_vec.reserve(c3_intermediates.num_points); + c3_intermediates.z_sol = vector>( + c3_intermediates.num_total_variables, vector(knot_points)); + c3_intermediates.delta_sol = vector>( + c3_intermediates.num_total_variables, vector(knot_points)); + c3_intermediates.w_sol = vector>( + c3_intermediates.num_total_variables, vector(knot_points)); + c3_intermediates.time_vec = c3_solution.time_vec; + + for (int i = 0; i < c3_solution.num_state_variables; ++i) { + VectorXf temp_row = c3_solution_.x_sol_.row(i); + memcpy(c3_solution.x_sol[i].data(), temp_row.data(), + sizeof(float) * knot_points); + } + for (int i = 0; i < c3_solution.num_contact_variables; ++i) { + VectorXf temp_row = c3_solution_.lambda_sol_.row(i); + memcpy(c3_solution.lambda_sol[i].data(), temp_row.data(), + sizeof(float) * knot_points); + } + for (int i = 0; i < c3_solution.num_input_variables; ++i) { + VectorXf temp_row = c3_solution_.u_sol_.row(i); + memcpy(c3_solution.u_sol[i].data(), temp_row.data(), + sizeof(float) * knot_points); + } + for (int i = 0; i < c3_intermediates.num_total_variables; ++i) { + VectorXf temp_z_row = c3_intermediates_.z_.row(i); + VectorXf temp_delta_row = c3_intermediates_.delta_.row(i); + VectorXf temp_w_row = c3_intermediates_.w_.row(i); + memcpy(c3_intermediates.z_sol[i].data(), temp_z_row.data(), + sizeof(float) * knot_points); + memcpy(c3_intermediates.delta_sol[i].data(), temp_delta_row.data(), + sizeof(float) * knot_points); + memcpy(c3_intermediates.w_sol[i].data(), temp_w_row.data(), + sizeof(float) * knot_points); + } + + c3_output.c3_solution = c3_solution; + // Not assigning to reduce space +// c3_output.c3_intermediates = lcmt_c3_intermediates(); + c3_output.c3_intermediates = c3_intermediates; + + return c3_output; +} + +} // namespace dairlib diff --git a/solvers/c3_output.h b/solvers/c3_output.h new file mode 100644 index 0000000000..6e4f574211 --- /dev/null +++ b/solvers/c3_output.h @@ -0,0 +1,52 @@ +#pragma once + +#include +#include +#include +#include + +#include + +#include "dairlib/lcmt_c3_output.hpp" + +namespace dairlib { + +/// Used for outputting c3 solutions and intermediate variables for debugging purposes + +class C3Output { + public: + struct C3Solution { + C3Solution() = default; + + // Shape is (variable_vector_size, knot_points) + Eigen::VectorXf time_vector_; + Eigen::MatrixXf x_sol_; + Eigen::MatrixXf lambda_sol_; + Eigen::MatrixXf u_sol_; + }; + + struct C3Intermediates { + C3Intermediates() = default; + + // Shape is (variable_vector_size, knot_points) + Eigen::VectorXf time_vector_; + Eigen::MatrixXf z_; + Eigen::MatrixXf delta_; + Eigen::MatrixXf w_; + }; + + C3Output() = default; + C3Output(C3Solution c3_sol, C3Intermediates c3_intermediates); + + explicit C3Output(const lcmt_c3_output& traj); + + virtual ~C3Output() = default; + + lcmt_c3_output GenerateLcmObject(double time) const; + + private: + C3Solution c3_solution_; + C3Intermediates c3_intermediates_; +}; + +} // namespace dairlib diff --git a/solvers/c3_qp.cc b/solvers/c3_qp.cc new file mode 100644 index 0000000000..7c98511909 --- /dev/null +++ b/solvers/c3_qp.cc @@ -0,0 +1,108 @@ +#include "solvers/c3_qp.h" + +#include + +#include + +#include "solvers/c3_options.h" +#include "solvers/lcs.h" + +#include "drake/solvers/mathematical_program.h" +#include "drake/solvers/osqp_solver.h" +#include "drake/solvers/solve.h" + +namespace dairlib { +namespace solvers { + +using Eigen::MatrixXd; +using Eigen::VectorXd; +using std::vector; + +using drake::solvers::MathematicalProgram; +using drake::solvers::MathematicalProgramResult; +using drake::solvers::SolutionResult; +using drake::solvers::SolverOptions; + +using drake::solvers::OsqpSolver; +using drake::solvers::OsqpSolverDetails; +using drake::solvers::Solve; + +C3QP::C3QP(const LCS& LCS, const CostMatrices& costs, + const vector& xdesired, const C3Options& options) + : C3(LCS, costs, xdesired, options) {} + +VectorXd C3QP::SolveSingleProjection(const MatrixXd& U, const VectorXd& delta_c, + const MatrixXd& E, const MatrixXd& F, + const MatrixXd& H, const VectorXd& c, + const int admm_iteration, + const int& warm_start_index) { + drake::solvers::MathematicalProgram prog; + drake::solvers::SolverOptions solver_options; + drake::solvers::OsqpSolver osqp_; + + auto xn_ = prog.NewContinuousVariables(n_x_, "x"); + auto ln_ = prog.NewContinuousVariables(n_lambda_, "lambda"); + auto un_ = prog.NewContinuousVariables(n_u_, "u"); + + double alpha = 0.01; + double scaling = 1000; + + MatrixXd EFH(n_lambda_, n_x_ + n_lambda_ + n_u_); + EFH.block(0, 0, n_lambda_, n_x_) = E / scaling; + EFH.block(0, n_x_, n_lambda_, n_lambda_) = F / scaling; + EFH.block(0, n_x_ + n_lambda_, n_lambda_, n_u_) = H / scaling; + + prog.AddLinearConstraint( + EFH, -c / scaling, + Eigen::VectorXd::Constant(n_lambda_, + std::numeric_limits::infinity()), + {xn_, ln_, un_}); + + prog.AddLinearConstraint( + MatrixXd::Identity(n_lambda_, n_lambda_), VectorXd::Zero(n_lambda_), + Eigen::VectorXd::Constant(n_lambda_, + std::numeric_limits::infinity()), + ln_); + + MatrixXd New_U = U; + New_U.block(n_x_, n_x_, n_lambda_, n_lambda_) = alpha * F; + + VectorXd cost_linear = -delta_c.transpose() * New_U; + + prog.AddQuadraticCost(New_U, cost_linear, {xn_, ln_, un_}, 1); + + prog.AddQuadraticCost((1 - alpha) * F, VectorXd::Zero(n_lambda_), ln_, 1); + + solver_options.SetOption(OsqpSolver::id(), "max_iter", 500); + solver_options.SetOption(OsqpSolver::id(), "verbose", 0); + solver_options.SetOption(OsqpSolver::id(), "polish", 1); + solver_options.SetOption(OsqpSolver::id(), "polish_refine_iter", 1); + solver_options.SetOption(OsqpSolver::id(), "rho", 1e-4); + solver_options.SetOption(OsqpSolver::id(), "scaled_termination", 1); + solver_options.SetOption(OsqpSolver::id(), "linsys_solver", 0); + prog.SetSolverOptions(solver_options); + + MathematicalProgramResult result = osqp_.Solve(prog); + + VectorXd xSol = result.GetSolution(xn_); + VectorXd lamSol = result.GetSolution(ln_); + VectorXd uSol = result.GetSolution(un_); + + VectorXd delta_kc = VectorXd::Zero(n_x_ + n_lambda_ + n_u_); + delta_kc.segment(0, n_x_) = xSol; + delta_kc.segment(n_x_, n_lambda_) = lamSol; + delta_kc.segment(n_x_ + n_lambda_, n_u_) = uSol; + + return delta_kc; +} + +std::vector C3QP::GetWarmStartDelta() const { + return warm_start_delta_[0]; +} + +std::vector C3QP::GetWarmStartBinary() const { + return warm_start_binary_[0]; +} + +} // namespace solvers +} // namespace dairlib diff --git a/solvers/c3_qp.h b/solvers/c3_qp.h new file mode 100644 index 0000000000..d0e1edbc6d --- /dev/null +++ b/solvers/c3_qp.h @@ -0,0 +1,45 @@ +#pragma once + +#include + +#include + +#include "solvers/c3.h" +#include "solvers/lcs.h" + +#include "drake/solvers/mathematical_program.h" +#include "drake/solvers/moby_lcp_solver.h" +#include "drake/solvers/osqp_solver.h" +#include "drake/solvers/solve.h" + +#include "solvers/c3_options.h" + +namespace dairlib { +namespace solvers { + +class C3QP final : public C3 { + public: + /// Default constructor for time-varying LCS + C3QP(const LCS& LCS, const CostMatrices& costs, + const std::vector& xdesired, + const C3Options& options); + + ~C3QP() override = default; + + /// Virtual projection method + Eigen::VectorXd SolveSingleProjection(const Eigen::MatrixXd& U, + const Eigen::VectorXd& delta_c, + const Eigen::MatrixXd& E, + const Eigen::MatrixXd& F, + const Eigen::MatrixXd& H, + const Eigen::VectorXd& c, + const int admm_iteration, + const int& warm_start_index = -1) override; + + std::vector GetWarmStartDelta() const; + std::vector GetWarmStartBinary() const; + +}; + +} // namespace solvers +} // namespace dairlib diff --git a/solvers/lcs.cc b/solvers/lcs.cc new file mode 100644 index 0000000000..7433279a38 --- /dev/null +++ b/solvers/lcs.cc @@ -0,0 +1,113 @@ +#include "solvers/lcs.h" + +#include + +#include "drake/solvers/mathematical_program.h" +#include "drake/solvers/moby_lcp_solver.h" +using Eigen::MatrixXd; +using Eigen::VectorXd; +using std::vector; + +namespace dairlib { +namespace solvers { + +LCS::LCS(const vector& A, const vector& B, + const vector& D, const vector& d, + const vector& E, const vector& F, + const vector& H, const vector& c, double dt) + : A_(A), + B_(B), + D_(D), + d_(d), + E_(E), + F_(F), + H_(H), + c_(c), + N_(A_.size()), + dt_(dt), + n_(A_[0].rows()), + m_(D_[0].cols()), + k_(H_[0].cols()) {} + +LCS::LCS(const MatrixXd& A, const MatrixXd& B, const MatrixXd& D, + const VectorXd& d, const MatrixXd& E, const MatrixXd& F, + const MatrixXd& H, const VectorXd& c, const int& N, double dt) + : LCS(vector(N, A), vector(N, B), + vector(N, D), vector(N, d), + vector(N, E), vector(N, F), + vector(N, H), vector(N, c), dt) {} + +LCS::LCS(const LCS& lcs) + : N_(lcs.N_), dt_(lcs.dt_), n_(lcs.n_), m_(lcs.m_), k_(lcs.k_) { + A_.resize(N_); + B_.resize(N_); + D_.resize(N_); + d_.resize(N_); + E_.resize(N_); + F_.resize(N_); + H_.resize(N_); + c_.resize(N_); + for (int i = 0; i < lcs.N_; ++i) { + A_.at(i) = lcs.A_.at(i); + B_.at(i) = lcs.B_.at(i); + D_.at(i) = lcs.D_.at(i); + d_.at(i) = lcs.d_.at(i); + E_.at(i) = lcs.E_.at(i); + F_.at(i) = lcs.F_.at(i); + H_.at(i) = lcs.H_.at(i); + c_.at(i) = lcs.c_.at(i); + } + W_x_ = lcs.W_x_; + W_l_ = lcs.W_l_; + W_u_ = lcs.W_u_; + w_ = lcs.w_; + has_tangent_linearization_ = lcs.has_tangent_linearization_; +} + +LCS& LCS::operator=(const LCS& lcs) { + N_ = lcs.N_; + dt_ = lcs.dt_; + n_ = lcs.n_; + m_ = lcs.m_; + k_ = lcs.k_; + A_.resize(N_); + B_.resize(N_); + D_.resize(N_); + d_.resize(N_); + E_.resize(N_); + F_.resize(N_); + H_.resize(N_); + c_.resize(N_); + for (int i = 0; i < lcs.N_; ++i) { + A_.at(i) = lcs.A_.at(i); + B_.at(i) = lcs.B_.at(i); + D_.at(i) = lcs.D_.at(i); + d_.at(i) = lcs.d_.at(i); + E_.at(i) = lcs.E_.at(i); + F_.at(i) = lcs.F_.at(i); + H_.at(i) = lcs.H_.at(i); + c_.at(i) = lcs.c_.at(i); + } + W_x_ = lcs.W_x_; + W_l_ = lcs.W_l_; + W_u_ = lcs.W_u_; + w_ = lcs.w_; + has_tangent_linearization_ = lcs.has_tangent_linearization_; + return *this; +} + +const VectorXd LCS::Simulate(VectorXd& x_init, VectorXd& input) { + VectorXd x_final; + // calculate force + drake::solvers::MobyLCPSolver LCPSolver; + VectorXd force; + + auto flag = LCPSolver.SolveLcpLemke( + F_[0], E_[0] * x_init + c_[0] + H_[0] * input, &force); + // update + x_final = A_[0] * x_init + B_[0] * input + D_[0] * force + d_[0]; + return x_final; +} + +} // namespace solvers +} // namespace dairlib diff --git a/solvers/lcs.h b/solvers/lcs.h new file mode 100644 index 0000000000..8d1401497f --- /dev/null +++ b/solvers/lcs.h @@ -0,0 +1,70 @@ +#pragma once +#include +#include + +#include + +#include "drake/common/sorted_pair.h" +#include "drake/multibody/plant/multibody_plant.h" + +namespace dairlib { +namespace solvers { +class LCS { + public: + /// Constructor for time-varying LCS + /// @param A, B, D, d Dynamics constraints x_{k+1} = A_k x_k + B_k u_k + D_k + /// \lambda_k + d_k + /// @param E, F, H, c Complementarity constraints 0 <= \lambda_k \perp E_k + /// x_k + F_k \lambda_k + H_k u_k + c_k + LCS(const std::vector& A, + const std::vector& B, + const std::vector& D, + const std::vector& d, + const std::vector& E, + const std::vector& F, + const std::vector& H, + const std::vector& c, double dt); + + /// Constructor for time-invariant LCS + LCS(const Eigen::MatrixXd& A, const Eigen::MatrixXd& B, + const Eigen::MatrixXd& D, const Eigen::VectorXd& d, + const Eigen::MatrixXd& E, const Eigen::MatrixXd& F, + const Eigen::MatrixXd& H, const Eigen::VectorXd& c, const int& N, + double dt); + + LCS(const LCS& other); + LCS& operator=(const LCS&); + LCS(LCS&&) = default; + LCS& operator=(LCS&&) = default; + + /// Simulate the system for one-step + /// @param x_init Initial x value + /// @param input Input value + const Eigen::VectorXd Simulate(Eigen::VectorXd& x_init, + Eigen::VectorXd& input); + + public: + std::vector A_; + std::vector B_; + std::vector D_; + std::vector d_; + std::vector E_; + std::vector F_; + std::vector H_; + std::vector c_; + Eigen::MatrixXd W_x_; + Eigen::MatrixXd W_l_; + Eigen::MatrixXd W_u_; + Eigen::VectorXd w_; + bool has_tangent_linearization_ = false; + Eigen::MatrixXd J_c_; + int N_; + double dt_; + + int n_; + int m_; + int k_; +}; + +} // namespace solvers +} // namespace dairlib diff --git a/solvers/lcs_factory.cc b/solvers/lcs_factory.cc new file mode 100644 index 0000000000..f9cc620de7 --- /dev/null +++ b/solvers/lcs_factory.cc @@ -0,0 +1,434 @@ +#include "solvers/lcs_factory.h" + +#include + +#include "multibody/geom_geom_collider.h" +#include "multibody/kinematic/kinematic_evaluator_set.h" + +#include "drake/math/autodiff_gradient.h" +#include "drake/solvers/moby_lcp_solver.h" + +namespace dairlib { +namespace solvers { + +using std::set; +using std::vector; + +using drake::AutoDiffVecXd; +using drake::AutoDiffXd; +using drake::MatrixX; +using drake::SortedPair; +using drake::VectorX; +using drake::geometry::GeometryId; +using drake::math::ExtractGradient; +using drake::math::ExtractValue; +using drake::multibody::MultibodyPlant; +using drake::systems::Context; + +using Eigen::MatrixXd; +using Eigen::VectorXd; + +LCS LCSFactory::LinearizePlantToLCS( + const MultibodyPlant& plant, const Context& context, + const MultibodyPlant& plant_ad, + const Context& context_ad, + const vector>& contact_geoms, + int num_friction_directions, const std::vector& mu, double dt, + int N, ContactModel contact_model) { + int n_x = plant_ad.num_positions() + plant_ad.num_velocities(); + int n_u = plant_ad.num_actuators(); + + int n_contacts = contact_geoms.size(); + + DRAKE_DEMAND(plant_ad.num_velocities() == plant.num_velocities()); + DRAKE_DEMAND(plant_ad.num_positions() == plant.num_positions()); + int n_v = plant.num_velocities(); + int n_q = plant.num_positions(); + + AutoDiffVecXd C(n_v); + plant_ad.CalcBiasTerm(context_ad, &C); + VectorXd u_dyn = plant.get_actuation_input_port().Eval(context); + + auto B_dyn_ad = plant_ad.MakeActuationMatrix(); + AutoDiffVecXd Bu = + B_dyn_ad * plant_ad.get_actuation_input_port().Eval(context_ad); + + AutoDiffVecXd tau_g = plant_ad.CalcGravityGeneralizedForces(context_ad); + + drake::multibody::MultibodyForces f_app(plant_ad); + plant_ad.CalcForceElementsContribution(context_ad, &f_app); + + MatrixX M(n_v, n_v); + plant_ad.CalcMassMatrix(context_ad, &M); + + // If this ldlt is slow, there are alternate formulations which avoid it + AutoDiffVecXd vdot_no_contact = + M.ldlt().solve(tau_g + Bu + f_app.generalized_forces() - C); + // Constant term in dynamics, d_vv = d + A x_0 + B u_0 + VectorXd d_vv = ExtractValue(vdot_no_contact); + // Derivatives w.r.t. x and u, AB + MatrixXd AB_v = ExtractGradient(vdot_no_contact); + VectorXd x_dvv(n_q + n_v + n_u); + x_dvv << plant.GetPositions(context), plant.GetVelocities(context), u_dyn; + VectorXd x_dvvcomp = AB_v * x_dvv; + VectorXd d_v = d_vv - x_dvvcomp; + + /////////// + AutoDiffVecXd x_ad = plant_ad.GetPositionsAndVelocities(context_ad); + AutoDiffVecXd qdot_no_contact(n_q); + AutoDiffVecXd vel_ad = x_ad.tail(n_v); + + plant_ad.MapVelocityToQDot(context_ad, vel_ad, &qdot_no_contact); + MatrixXd AB_q = ExtractGradient(qdot_no_contact); + MatrixXd d_q = ExtractValue(qdot_no_contact); + Eigen::SparseMatrix Nqt; + Nqt = plant.MakeVelocityToQDotMap(context); + MatrixXd qdotNv = MatrixXd(Nqt); + + Eigen::SparseMatrix NqI; + NqI = plant.MakeQDotToVelocityMap(context); + MatrixXd vNqdot = MatrixXd(NqI); + + VectorXd phi(n_contacts); + MatrixXd J_n(n_contacts, n_v); + MatrixXd J_t(2 * n_contacts * num_friction_directions, n_v); + + for (int i = 0; i < n_contacts; i++) { + multibody::GeomGeomCollider collider( + plant, + contact_geoms[i]); // deleted num_friction_directions (check with + // Michael about changes in geomgeom) + auto [phi_i, J_i] = collider.EvalPolytope(context, num_friction_directions); + // J_i is 3 x n_v + // row (0) is contact normal + // rows (1-num_friction directions) are the contact tangents + + phi(i) = phi_i; + J_n.row(i) = J_i.row(0); + J_t.block(2 * i * num_friction_directions, 0, 2 * num_friction_directions, + n_v) = J_i.block(1, 0, 2 * num_friction_directions, n_v); + } + + auto M_ldlt = ExtractValue(M).ldlt(); + MatrixXd MinvJ_n_T = M_ldlt.solve(J_n.transpose()); + MatrixXd MinvJ_t_T = M_ldlt.solve(J_t.transpose()); + + MatrixXd A(n_x, n_x); + MatrixXd B(n_x, n_u); + VectorXd d(n_x); + + MatrixXd AB_v_q = AB_v.block(0, 0, n_v, n_q); + MatrixXd AB_v_v = AB_v.block(0, n_q, n_v, n_v); + MatrixXd AB_v_u = AB_v.block(0, n_x, n_v, n_u); + MatrixXd M_double = MatrixXd::Zero(n_v, n_v); + plant.CalcMassMatrix(context, &M_double); + + A.block(0, 0, n_q, n_q) = + MatrixXd::Identity(n_q, n_q) + dt * dt * qdotNv * AB_v_q; + A.block(0, n_q, n_q, n_v) = dt * qdotNv + dt * dt * qdotNv * AB_v_v; + A.block(n_q, 0, n_v, n_q) = dt * AB_v_q; + A.block(n_q, n_q, n_v, n_v) = dt * AB_v_v + MatrixXd::Identity(n_v, n_v); + + B.block(0, 0, n_q, n_u) = dt * dt * qdotNv * AB_v_u; + B.block(n_q, 0, n_v, n_u) = dt * AB_v_u; + + d.head(n_q) = dt * dt * qdotNv * d_v; + d.tail(n_v) = dt * d_v; + + MatrixXd E_t = + MatrixXd::Zero(n_contacts, 2 * n_contacts * num_friction_directions); + for (int i = 0; i < n_contacts; i++) { + E_t.block(i, i * (2 * num_friction_directions), 1, + 2 * num_friction_directions) = + MatrixXd::Ones(1, 2 * num_friction_directions); + } + + int n_lambda = 0; + if (contact_model == ContactModel::kStewartAndTrinkle) { + n_lambda = 2 * n_contacts + 2 * n_contacts * num_friction_directions; + } else { + n_lambda = 2 * n_contacts * num_friction_directions; + } + + // Matrices with contact variables + MatrixXd D = MatrixXd::Zero(n_x, n_lambda); + MatrixXd E = MatrixXd::Zero(n_lambda, n_x); + MatrixXd F = MatrixXd::Zero(n_lambda, n_lambda); + MatrixXd H = MatrixXd::Zero(n_lambda, n_u); + VectorXd c = VectorXd::Zero(n_lambda); + + if (contact_model == ContactModel::kStewartAndTrinkle) { + D.block(0, 2 * n_contacts, n_q, 2 * n_contacts * num_friction_directions) = + dt * dt * qdotNv * MinvJ_t_T; + D.block(n_q, 2 * n_contacts, n_v, + 2 * n_contacts * num_friction_directions) = dt * MinvJ_t_T; + + D.block(0, n_contacts, n_q, n_contacts) = dt * dt * qdotNv * MinvJ_n_T; + + D.block(n_q, n_contacts, n_v, n_contacts) = dt * MinvJ_n_T; + // Complementarity condition for gamma: mu lambda^n + E.block(n_contacts, 0, n_contacts, n_q) = + dt * dt * J_n * AB_v_q + J_n * vNqdot; + E.block(2 * n_contacts, 0, 2 * n_contacts * num_friction_directions, n_q) = + dt * J_t * AB_v_q; + E.block(n_contacts, n_q, n_contacts, n_v) = + dt * J_n + dt * dt * J_n * AB_v_v; + E.block(2 * n_contacts, n_q, 2 * n_contacts * num_friction_directions, + n_v) = J_t + dt * J_t * AB_v_v; + + VectorXd mu_vec = Eigen::Map( + mu.data(), mu.size()); + // Complementarity condition for gamma: mu lambda^n + F.block(0, n_contacts, n_contacts, n_contacts) = mu_vec.asDiagonal(); + + // Complementarity condition for gamma: lambda^t + F.block(0, 2 * n_contacts, n_contacts, + 2 * n_contacts * num_friction_directions) = -E_t; + + // Complementarity condition for lambda_n: dt J_n (lambda^n component of + // v_{k+1}) + F.block(n_contacts, n_contacts, n_contacts, n_contacts) = + dt * dt * J_n * MinvJ_n_T; + // Complementarity condition for lambda_n: dt J_n (lambda^t component of + // v_{k+1}) + F.block(n_contacts, 2 * n_contacts, n_contacts, + 2 * n_contacts * num_friction_directions) = + dt * dt * J_n * MinvJ_t_T; + // Complementarity condition for lambda_t: dt J_t (gamma component of + // v_{k+1}) + F.block(2 * n_contacts, 0, 2 * n_contacts * num_friction_directions, + n_contacts) = E_t.transpose(); + // Complementarity condition for lambda_t: dt J_t (lambda^n component of + // v_{k+1}) + F.block(2 * n_contacts, n_contacts, + 2 * n_contacts * num_friction_directions, n_contacts) = + dt * J_t * MinvJ_n_T; + // Complementarity condition for lambda_t: dt J_t (lambda^t component of + // v_{k+1}) + F.block(2 * n_contacts, 2 * n_contacts, + 2 * n_contacts * num_friction_directions, + 2 * n_contacts * num_friction_directions) = dt * J_t * MinvJ_t_T; + + H.block(n_contacts, 0, n_contacts, n_u) = dt * dt * J_n * AB_v_u; + H.block(2 * n_contacts, 0, 2 * n_contacts * num_friction_directions, n_u) = + dt * J_t * AB_v_u; + + c.segment(n_contacts, n_contacts) = + phi + dt * dt * J_n * d_v - J_n * vNqdot * plant.GetPositions(context); + c.segment(2 * n_contacts, 2 * n_contacts * num_friction_directions) = + J_t * dt * d_v; + } else if (contact_model == ContactModel::kAnitescu) { + VectorXd mu_vec = Eigen::Map( + mu.data(), mu.size()); + VectorXd anitescu_mu_vec = VectorXd::Zero(n_lambda); + for (int i = 0; i < mu_vec.rows(); i++) { + anitescu_mu_vec.segment((2 * num_friction_directions) * i, + 2 * num_friction_directions) = + mu_vec(i) * VectorXd::Ones(2 * num_friction_directions); + } + MatrixXd anitescu_mu_matrix = anitescu_mu_vec.asDiagonal(); + // Constructing friction bases + MatrixXd J_c = E_t.transpose() * J_n + anitescu_mu_matrix * J_t; + + MatrixXd MinvJ_c_T = M_ldlt.solve(J_c.transpose()); + + D.block(0, 0, n_q, n_lambda) = dt * dt * qdotNv * MinvJ_c_T; + D.block(n_q, 0, n_v, n_lambda) = dt * MinvJ_c_T; + + // q component of complementarity constraint + E.block(0, 0, n_lambda, n_q) = + dt * J_c * AB_v_q + E_t.transpose() * J_n * vNqdot / dt; + E.block(0, n_q, n_lambda, n_v) = J_c + dt * J_c * AB_v_v; + + // lambda component of complementarity constraint + F = dt * J_c * MinvJ_c_T; + + // u component of complementarity constraint + H = dt * J_c * AB_v_u; + // constant component of complementarity constraint + c = E_t.transpose() * phi / dt + dt * J_c * d_v - + E_t.transpose() * J_n * vNqdot * plant.GetPositions(context) / dt; + } + + LCS system(A, B, D, d, E, F, H, c, N, dt); + return system; +} + +std::pair> +LCSFactory::ComputeContactJacobian( + const drake::multibody::MultibodyPlant& plant, + const drake::systems::Context& context, + const std::vector>& + contact_geoms, + int num_friction_directions, const std::vector& mu, + dairlib::solvers::ContactModel contact_model) { + int n_contacts = contact_geoms.size(); + + int n_v = plant.num_velocities(); + + VectorXd phi(n_contacts); + MatrixXd J_n(n_contacts, n_v); + MatrixXd J_t(2 * n_contacts * num_friction_directions, n_v); + std::vector contact_points; + for (int i = 0; i < n_contacts; i++) { + multibody::GeomGeomCollider collider( + plant, + contact_geoms[i]); // deleted num_friction_directions (check with + // Michael about changes in geomgeom) + auto [phi_i, J_i] = collider.EvalPolytope(context, num_friction_directions); + auto [p_WCa, p_WCb] = collider.CalcWitnessPoints(context); + // TODO(yangwill): think about if we want to push back both witness points + contact_points.push_back(p_WCa); + phi(i) = phi_i; + J_n.row(i) = J_i.row(0); + J_t.block(2 * i * num_friction_directions, 0, 2 * num_friction_directions, + n_v) = J_i.block(1, 0, 2 * num_friction_directions, n_v); + } + + if (contact_model == ContactModel::kStewartAndTrinkle) { + MatrixXd J_c = MatrixXd::Zero( + n_contacts + 2 * n_contacts * num_friction_directions, n_v); + J_c << J_n, J_t; + return std::make_pair(J_c, contact_points); + } else if (contact_model == ContactModel::kAnitescu) { + MatrixXd E_t = + MatrixXd::Zero(n_contacts, 2 * n_contacts * num_friction_directions); + for (int i = 0; i < n_contacts; i++) { + E_t.block(i, i * (2 * num_friction_directions), 1, + 2 * num_friction_directions) = + MatrixXd::Ones(1, 2 * num_friction_directions); + } + int n_contact_vars = 2 * n_contacts * num_friction_directions; + + VectorXd mu_vec = Eigen::Map( + mu.data(), mu.size()); + VectorXd anitescu_mu_vec = VectorXd::Zero(n_contact_vars); + for (int i = 0; i < mu_vec.rows(); i++) { + double cur = mu_vec(i); + anitescu_mu_vec(4 * i) = cur; + anitescu_mu_vec(4 * i + 1) = cur; + anitescu_mu_vec(4 * i + 2) = cur; + anitescu_mu_vec(4 * i + 3) = cur; + } + MatrixXd anitescu_mu_matrix = anitescu_mu_vec.asDiagonal(); + MatrixXd J_c = E_t.transpose() * J_n + anitescu_mu_matrix * J_t; + return std::make_pair(J_c, contact_points); + } else { + std::cerr << ("Unknown projection type") << std::endl; + DRAKE_THROW_UNLESS(false); + } +} + +LCS LCSFactory::FixSomeModes(const LCS& other, set active_lambda_inds, + set inactive_lambda_inds) { + vector remaining_inds; + + // Assumes constant number of contacts per index + int n_lambda = other.F_[0].rows(); + + // Need to solve for lambda_active in terms of remaining elements + // Build temporary [F1, F2] by eliminating rows for inactive + for (int i = 0; i < n_lambda; i++) { + // active/inactive must be exclusive + DRAKE_ASSERT(!active_lambda_inds.count(i) || + !inactive_lambda_inds.count(i)); + + // In C++20, could use contains instead of count + if (!active_lambda_inds.count(i) && !inactive_lambda_inds.count(i)) { + remaining_inds.push_back(i); + } + } + + int n_remaining = remaining_inds.size(); + int n_active = active_lambda_inds.size(); + + vector A, B, D, E, F, H; + vector d, c; + + // Build selection matrices: + // S_a selects active indices + // S_r selects remaining indices + + MatrixXd S_a = MatrixXd::Zero(n_active, n_lambda); + MatrixXd S_r = MatrixXd::Zero(n_remaining, n_lambda); + + for (int i = 0; i < n_remaining; i++) { + S_r(i, remaining_inds[i]) = 1; + } + { + int i = 0; + for (auto ind_j : active_lambda_inds) { + S_a(i, ind_j) = 1; + i++; + } + } + + for (int k = 0; k < other.N_; k++) { + Eigen::BDCSVD svd; + svd.setThreshold(1e-5); + svd.compute(S_a * other.F_[k] * S_a.transpose(), + Eigen::ComputeFullU | Eigen::ComputeFullV); + + // F_active likely to be low-rank due to friction, but that should be OK + // MatrixXd res = svd.solve(F_ar); + + // Build new complementarity constraints + // F_a_inv = pinv(S_a * F * S_a^T) + // 0 <= \lambda_k \perp E_k x_k + F_k \lambda_k + H_k u_k + c_k + // 0 = S_a *(E x + F S_a^T \lambda_a + F S_r^T \lambda_r + H_k u_k + c_k) + // \lambda_a = -F_a_inv * (S_a F S_r^T * lambda_r + S_a E x + S_a H u + S_a + // c) + // + // 0 <= \lambda_r \perp S_r (I - F S_a^T F_a_inv S_a) E x + ... + // S_r (I - F S_a^T F_a_inv S_a) F S_r^T \lambda_r + + // ... S_r (I - F S_a^T F_a_inv S_a) H u + ... S_r (I - + // F S_a^T F_a_inv S_a) c + // + // Calling L = S_r (I - F S_a^T F_a_inv S_a)S_r * other.D_[k] + // E_k = L E + // F_k = L F S_r^t + // H_k = L H + // c_k = L c + // std::cout << S_r << std::endl << std::endl; + // std::cout << other.F_[k] << std::endl << std::endl; + // std::cout << other.F_[k] << std::endl << std::endl; + // auto tmp = S_r * (MatrixXd::Identity(n_lambda, n_lambda) - + // other.F_[k] * S_a.transpose()); + MatrixXd L = S_r * (MatrixXd::Identity(n_lambda, n_lambda) - + other.F_[k] * S_a.transpose() * svd.solve(S_a)); + MatrixXd E_k = L * other.E_[k]; + MatrixXd F_k = L * other.F_[k] * S_r.transpose(); + MatrixXd H_k = L * other.H_[k]; + MatrixXd c_k = L * other.c_[k]; + + // Similarly, + // A_k = A - D * S_a^T * F_a_inv * S_a * E + // B_k = B - D * S_a^T * F_a_inv * S_a * H + // D_k = D * S_r^T - D * S_a^T * F_a_inv * S_a F S_r^T + // d_k = d - D * S_a^T F_a_inv * S_a * c + // + // Calling P = D * S_a^T * F_a_inv * S_a + // + // A_k = A - P E + // B_k = B - P H + // D_k = S_r D - P S_r^T + // d_k = d - P c + MatrixXd P = other.D_[k] * S_a.transpose() * svd.solve(S_a); + MatrixXd A_k = other.A_[k] - P * other.E_[k]; + MatrixXd B_k = other.B_[k] - P * other.H_[k]; + MatrixXd D_k = other.D_[k] * S_r.transpose() - P * S_r.transpose(); + MatrixXd d_k = other.d_[k] - P * other.c_[k]; + E.push_back(E_k); + F.push_back(F_k); + H.push_back(H_k); + c.push_back(c_k); + A.push_back(A_k); + B.push_back(B_k); + D.push_back(D_k); + d.push_back(d_k); + } + return LCS(A, B, D, d, E, F, H, c, other.dt_); +} + +} // namespace solvers +} // namespace dairlib diff --git a/solvers/lcs_factory.h b/solvers/lcs_factory.h new file mode 100644 index 0000000000..e86d94d439 --- /dev/null +++ b/solvers/lcs_factory.h @@ -0,0 +1,65 @@ +#pragma once + +#include + +#include "solvers/lcs.h" + +namespace dairlib { +namespace solvers { + +enum class ContactModel { + kStewartAndTrinkle, /// Stewart and Trinkle timestepping contact + kAnitescu /// Anitescu convex contact +}; + +class LCSFactory { + public: + /// Build a time-invariant LCS, linearizing a MultibodyPlant + /// Contacts are specified by the pairs in contact_geoms. Each element + /// in the contact_geoms vector defines a collision. + /// This method also uses two copies of the Context, one for double and one + /// for AutoDiff. Given that Contexts can be expensive to create, this is + /// preferred to extracting the double-version from the AutoDiff. + /// + /// TODO: add variant allowing for different frictional properties per + /// contact + /// + /// @param plant The standard MultibodyPlant + /// @param context The context about which to linearize (double) + /// @param plant_ad An AutoDiffXd templated plant for gradient calculation + /// @param context The context about which to linearize (AutoDiffXd) + /// @param contact_geoms + /// @param num_friction faces + /// @param mu + /// @oaram dt The timestep for discretization + /// @param N + static LCS LinearizePlantToLCS( + const drake::multibody::MultibodyPlant& plant, + const drake::systems::Context& context, + const drake::multibody::MultibodyPlant& plant_ad, + const drake::systems::Context& context_ad, + const std::vector>& + contact_geoms, + int num_friction_directions, const std::vector& mu, double dt, + int N, ContactModel = ContactModel::kStewartAndTrinkle); + + static std::pair> ComputeContactJacobian( + const drake::multibody::MultibodyPlant& plant, + const drake::systems::Context& context, + const std::vector>& + contact_geoms, + int num_friction_directions, const std::vector& mu, + ContactModel = ContactModel::kStewartAndTrinkle); + + /// Create an LCS by fixing some modes from another LCS + /// Ignores generated inequalities that correspond to these modes, but + /// these could be returned via pointer if useful + /// + /// @param active_lambda_inds The indices for lambda thta must be non-zero + /// @param inactive_lambda_inds The indices for lambda that must be 0 + static LCS FixSomeModes(const LCS& other, std::set active_lambda_inds, + std::set inactive_lambda_inds); +}; + +} // namespace solvers +} // namespace dairlib diff --git a/tools/BUILD.bazel b/tools/BUILD.bazel new file mode 100644 index 0000000000..8d336c7370 --- /dev/null +++ b/tools/BUILD.bazel @@ -0,0 +1,6 @@ +package(default_visibility = ["//visibility:public"]) + +config_setting( + name = "with_gurobi", + values = {"define": "WITH_GUROBI=ON"}, +)