diff --git a/bindings/pydairlib/lcm/visualization/visualize_configs/long_jump.yaml b/bindings/pydairlib/lcm/visualization/visualize_configs/long_jump.yaml index 3970e01776..5f67cb36a8 100644 --- a/bindings/pydairlib/lcm/visualization/visualize_configs/long_jump.yaml +++ b/bindings/pydairlib/lcm/visualization/visualize_configs/long_jump.yaml @@ -5,4 +5,4 @@ visualize_mode: 2 realtime_rate: 1.0 num_poses: 10 use_transparency: 1 -use_springs: 1 \ No newline at end of file +use_springs: 1 diff --git a/examples/Cassie/kinematic_centroidal_planner/BUILD.bazel b/examples/Cassie/kinematic_centroidal_planner/BUILD.bazel index 4c32021195..c5aa1d74c0 100644 --- a/examples/Cassie/kinematic_centroidal_planner/BUILD.bazel +++ b/examples/Cassie/kinematic_centroidal_planner/BUILD.bazel @@ -41,7 +41,7 @@ cc_binary( "//solvers:optimization_utils", "//systems/primitives", "//examples/Cassie/kinematic_centroidal_planner:cassie_kinematic_centroidal_mpc", - "//examples/Cassie/kinematic_centroidal_planner:cassie_reference_utils", + "//examples/Cassie/kinematic_centroidal_planner:cassie_kc_utils", "@drake//:drake_shared_library", "@gflags", ], @@ -76,14 +76,15 @@ cc_library( ) cc_library( - name = "cassie_reference_utils", - srcs = ["cassie_reference_utils.cc"], - hdrs = ["cassie_reference_utils.h"], + name = "cassie_kc_utils", + srcs = ["cassie_kc_utils.cc"], + hdrs = ["cassie_kc_utils.h"], deps = [ "//examples/Cassie:cassie_utils", "//common", "//systems/trajectory_optimization/kinematic_centroidal_planner:kinematic_centroidal_planner", "//multibody:visualization_utils", + ":cassie_kinematic_centroidal_mpc", "@drake//:drake_shared_library", ], ) @@ -97,6 +98,7 @@ cc_library( "//common", "//systems/trajectory_optimization/kinematic_centroidal_planner", "//multibody:visualization_utils", + "//examples/Cassie/kinematic_centroidal_planner/simple_models:slip", "@drake//:drake_shared_library", ], ) diff --git a/examples/Cassie/kinematic_centroidal_planner/cassie_reference_utils.cc b/examples/Cassie/kinematic_centroidal_planner/cassie_kc_utils.cc similarity index 65% rename from examples/Cassie/kinematic_centroidal_planner/cassie_reference_utils.cc rename to examples/Cassie/kinematic_centroidal_planner/cassie_kc_utils.cc index 2b5215dbee..0860148d07 100644 --- a/examples/Cassie/kinematic_centroidal_planner/cassie_reference_utils.cc +++ b/examples/Cassie/kinematic_centroidal_planner/cassie_kc_utils.cc @@ -1,30 +1,34 @@ -#include "cassie_reference_utils.h" -#include +#include "cassie_kc_utils.h" + +#include +#include + #include +#include +#include #include #include -#include -#include -#include #include +#include +#include + #include "examples/Cassie/cassie_utils.h" #include "multibody/visualization_utils.h" -#include using drake::geometry::SceneGraph; using drake::multibody::MultibodyPlant; using drake::multibody::Parser; -Eigen::VectorXd GenerateNominalStand(const drake::multibody::MultibodyPlant &plant, - double com_height, - double stance_width, - bool visualize) { - using Eigen::VectorXd; +Eigen::VectorXd GenerateNominalStand( + const drake::multibody::MultibodyPlant& plant, double com_height, + double stance_width, bool visualize) { using Eigen::Vector3d; + using Eigen::VectorXd; int n_q = plant.num_positions(); int n_v = plant.num_velocities(); int n_x = n_q + n_v; - std::map positions_map = dairlib::multibody::MakeNameToPositionsMap(plant); + std::map positions_map = + dairlib::multibody::MakeNameToPositionsMap(plant); Eigen::VectorXd q_ik_guess = Eigen::VectorXd::Zero(n_q); @@ -58,12 +62,11 @@ Eigen::VectorXd GenerateNominalStand(const drake::multibody::MultibodyPlant(), - world_frame, drake::math::RotationMatrix(), eps); - ik.AddPositionConstraint(toe_left_frame, dairlib::LeftToeFront(plant).first, world_frame, - l_toe_pos - eps_vec, + ik.AddOrientationConstraint( + pelvis_frame, drake::math::RotationMatrix(), world_frame, + drake::math::RotationMatrix(), eps); + ik.AddPositionConstraint(toe_left_frame, dairlib::LeftToeFront(plant).first, + world_frame, l_toe_pos - eps_vec, l_toe_pos + eps_vec); - ik.AddPositionConstraint(toe_left_frame, dairlib::LeftToeRear(plant).first, world_frame, - l_heel_pos - eps_vec, + ik.AddPositionConstraint(toe_left_frame, dairlib::LeftToeRear(plant).first, + world_frame, l_heel_pos - eps_vec, l_heel_pos + eps_vec); - ik.AddPositionConstraint(toe_right_frame, dairlib::RightToeFront(plant).first, world_frame, - r_toe_pos - eps_vec, r_toe_pos + eps_vec); - ik.AddPositionConstraint(toe_right_frame, dairlib::RightToeRear(plant).first, world_frame, - r_heel_pos - eps_vec, r_heel_pos + eps_vec); + ik.AddPositionConstraint(toe_right_frame, dairlib::RightToeFront(plant).first, + world_frame, r_toe_pos - eps_vec, + r_toe_pos + eps_vec); + ik.AddPositionConstraint(toe_right_frame, dairlib::RightToeRear(plant).first, + world_frame, r_heel_pos - eps_vec, + r_heel_pos + eps_vec); ik.get_mutable_prog()->AddLinearConstraint( (ik.q())(positions_map.at("hip_yaw_left")) == 0); @@ -96,15 +102,16 @@ Eigen::VectorXd GenerateNominalStand(const drake::multibody::MultibodyPlantAddLinearConstraint( (ik.q())(positions_map.at("knee_left")) + (ik.q())(positions_map.at("ankle_joint_left")) == - M_PI * 13 / 180.0); + M_PI * 13 / 180.0); ik.get_mutable_prog()->AddLinearConstraint( (ik.q())(positions_map.at("knee_right")) + (ik.q())(positions_map.at("ankle_joint_right")) == - M_PI * 13 / 180.0); + M_PI * 13 / 180.0); - auto constraint = std::make_shared(&plant, std::nullopt, plant.world_frame(), context.get()); + auto constraint = std::make_shared( + &plant, std::nullopt, plant.world_frame(), context.get()); auto r = ik.get_mutable_prog()->NewContinuousVariables(3); - ik.get_mutable_prog()->AddConstraint(constraint, {ik.q(),r}); + ik.get_mutable_prog()->AddConstraint(constraint, {ik.q(), r}); Eigen::Vector3d rdes = {0, 0, com_height}; ik.get_mutable_prog()->AddBoundingBoxConstraint(rdes, rdes, r); @@ -117,15 +124,16 @@ Eigen::VectorXd GenerateNominalStand(const drake::multibody::MultibodyPlant builder_ik; - drake::geometry::SceneGraph& scene_graph_ik = *builder_ik.AddSystem(); + drake::geometry::SceneGraph& scene_graph_ik = + *builder_ik.AddSystem(); scene_graph_ik.set_name("scene_graph_ik"); MultibodyPlant plant_ik(0.0); Parser parser(&plant_ik, &scene_graph_ik); - std::string full_name = - dairlib::FindResourceOrThrow("examples/Cassie/urdf/cassie_fixed_springs.urdf"); + std::string full_name = dairlib::FindResourceOrThrow( + "examples/Cassie/urdf/cassie_fixed_springs.urdf"); parser.AddModelFromFile(full_name); plant_ik.Finalize(); @@ -148,4 +156,28 @@ Eigen::VectorXd GenerateNominalStand(const drake::multibody::MultibodyPlant GenerateComplexitySchedule( + int n_knot_points, const std::vector& complexity_string_list) { + std::vector complexity_schedule; + std::regex number_regex("(^|\\s)([0-9]+)($|\\s)"); + std::smatch match; + for (const auto& complexity_string : complexity_string_list) { + DRAKE_DEMAND(complexity_string.at(0) == 'c' or + complexity_string.at(0) == 's'); + if (complexity_string.at(0) == 'c') { + std::regex_search(complexity_string, match, number_regex); + std::cout << std::stoi(match[0]) << std::endl; + for (int i = 0; i < std::stoi(match[0]); i++) { + complexity_schedule.push_back(Complexity::KINEMATIC_CENTROIDAL); + } + } else if (complexity_string.at(0) == 's') { + std::regex_search(complexity_string, match, number_regex); + std::cout << std::stoi(match[0]) << std::endl; + for (int i = 0; i < std::stoi(match[0]); i++) { + complexity_schedule.push_back(Complexity::SLIP); + } + } + } + DRAKE_DEMAND(n_knot_points == complexity_schedule.size()); + return complexity_schedule; +} \ No newline at end of file diff --git a/examples/Cassie/kinematic_centroidal_planner/cassie_reference_utils.h b/examples/Cassie/kinematic_centroidal_planner/cassie_kc_utils.h similarity index 52% rename from examples/Cassie/kinematic_centroidal_planner/cassie_reference_utils.h rename to examples/Cassie/kinematic_centroidal_planner/cassie_kc_utils.h index d5fe476d4b..5dedaa2d73 100644 --- a/examples/Cassie/kinematic_centroidal_planner/cassie_reference_utils.h +++ b/examples/Cassie/kinematic_centroidal_planner/cassie_kc_utils.h @@ -1,9 +1,11 @@ #pragma once -#include #include #include +#include + +#include "examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.h" #include "multibody/kinematic/world_point_evaluator.h" /*! @@ -14,7 +16,9 @@ * @param visualize true if the stand should be visualized * @return vector of state [nq + nv] */ -Eigen::VectorXd GenerateNominalStand(const drake::multibody::MultibodyPlant &plant, - double com_height, - double stance_width, - bool visualize = false); \ No newline at end of file +Eigen::VectorXd GenerateNominalStand( + const drake::multibody::MultibodyPlant& plant, double com_height, + double stance_width, bool visualize = false); + +std::vector GenerateComplexitySchedule( + int n_knot_points, const std::vector& complexity_string_list); \ No newline at end of file diff --git a/examples/Cassie/kinematic_centroidal_planner/cassie_kcmpc_trajopt.cc b/examples/Cassie/kinematic_centroidal_planner/cassie_kcmpc_trajopt.cc index 911eb3a0eb..35eefd29ca 100644 --- a/examples/Cassie/kinematic_centroidal_planner/cassie_kcmpc_trajopt.cc +++ b/examples/Cassie/kinematic_centroidal_planner/cassie_kcmpc_trajopt.cc @@ -1,3 +1,5 @@ +#include + #include #include @@ -16,12 +18,13 @@ #include "common/find_resource.h" #include "dairlib/lcmt_timestamped_saved_traj.hpp" +#include "examples/Cassie/kinematic_centroidal_planner/cassie_kc_utils.h" #include "examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.h" -#include "examples/Cassie/kinematic_centroidal_planner/cassie_reference_utils.h" #include "examples/Cassie/kinematic_centroidal_planner/trajectory_parameters.h" #include "systems/primitives/subvector_pass_through.h" #include "systems/trajectory_optimization/kinematic_centroidal_planner/kcmpc_reference_generator.h" #include "systems/trajectory_optimization/kinematic_centroidal_planner/kinematic_centroidal_gains.h" +#include "systems/trajectory_optimization/kinematic_centroidal_planner/reference_generation_utils.h" using drake::geometry::DrakeVisualizer; using drake::geometry::SceneGraph; @@ -33,7 +36,7 @@ DEFINE_string(channel_reference, "KCMPC_REF", "MPC are published"); DEFINE_string( trajectory_parameters, - "examples/Cassie/kinematic_centroidal_planner/motions/motion_test.yaml", + "examples/Cassie/kinematic_centroidal_planner/motions/motion_walk.yaml", "YAML file that contains trajectory parameters such as speed, tolerance, " "target_com_height"); DEFINE_string(planner_parameters, @@ -90,25 +93,35 @@ int DoMain(int argc, char* argv[]) { gait_library["jump"] = jump; // Create reference std::vector gait_samples; - for (auto gait : traj_params.gait_sequence){ + for (auto gait : traj_params.gait_sequence) { gait_samples.push_back(gait_library.at(gait)); } DRAKE_DEMAND(gait_samples.size() == traj_params.duration_scaling.size()); std::vector time_points = KcmpcReferenceGenerator::GenerateTimePoints( traj_params.duration_scaling, gait_samples); + Eigen::VectorXd reference_state = GenerateNominalStand( + plant, traj_params.target_com_height, traj_params.stance_width, false); + // Create MPC and set gains CassieKinematicCentroidalSolver mpc( plant, traj_params.n_knot_points, - time_points.back() / (traj_params.n_knot_points - 1), 0.4); + time_points.back() / (traj_params.n_knot_points - 1), 0.4, + reference_state.head(plant.num_positions()), traj_params.spring_constant, + traj_params.damping_coefficient, + sqrt(pow(traj_params.target_com_height, 2) + + pow(traj_params.stance_width, 2)), + traj_params.stance_width); mpc.SetGains(gains); mpc.SetMinimumFootClearance(traj_params.swing_foot_minimum_height); + mpc.SetMaximumSlipLegLength(traj_params.max_slip_leg_length); + + mpc.SetComplexitySchedule(GenerateComplexitySchedule( + traj_params.n_knot_points, traj_params.complexity_schedule)); KcmpcReferenceGenerator generator(plant, plant_context.get(), CreateContactPoints(plant, 0)); - Eigen::VectorXd reference_state = GenerateNominalStand( - plant, traj_params.target_com_height, traj_params.stance_width, false); generator.SetNominalReferenceConfiguration( reference_state.head(plant.num_positions())); generator.SetComKnotPoints({time_points, traj_params.com_vel_vector}); @@ -131,7 +144,9 @@ int DoMain(int argc, char* argv[]) { mpc.SetContactTrackingReference( std::make_unique>( generator.contact_traj_)); - mpc.SetConstantMomentumReference(Eigen::VectorXd::Zero(6)); + mpc.SetMomentumReference( + std::make_unique>( + generator.momentum_trajectory_)); mpc.SetModeSequence(generator.contact_sequence_); // // // Set initial guess @@ -140,6 +155,7 @@ int DoMain(int argc, char* argv[]) { mpc.SetComPositionGuess(generator.com_trajectory_); mpc.SetContactGuess(generator.contact_traj_); mpc.SetForceGuess(generator.grf_traj_); + mpc.SetMomentumGuess(generator.momentum_trajectory_); { drake::solvers::SolverOptions options; @@ -148,7 +164,7 @@ int DoMain(int argc, char* argv[]) { options.SetOption(id, "dual_inf_tol", gains.tol); options.SetOption(id, "constr_viol_tol", gains.tol); options.SetOption(id, "compl_inf_tol", gains.tol); - options.SetOption(id, "max_iter", 200); + options.SetOption(id, "max_iter", 800); options.SetOption(id, "nlp_lower_bound_inf", -1e6); options.SetOption(id, "nlp_upper_bound_inf", 1e6); options.SetOption(id, "print_timing_statistics", "yes"); @@ -204,12 +220,11 @@ int DoMain(int argc, char* argv[]) { while (true) { drake::systems::Simulator simulator(*diagram); - simulator.set_target_realtime_rate(1.0); + simulator.set_target_realtime_rate(0.25); simulator.Initialize(); simulator.AdvanceTo(pp_xtraj.end_time()); + sleep(2); } } -int main(int argc, char* argv[]) { - DoMain(argc, argv); -} +int main(int argc, char* argv[]) { DoMain(argc, argv); } diff --git a/examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.cc b/examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.cc index b21b9bd6bf..879832168a 100644 --- a/examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.cc +++ b/examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.cc @@ -1,18 +1,399 @@ #include "cassie_kinematic_centroidal_solver.h" + #include "common/find_resource.h" #include "examples/Cassie/cassie_utils.h" +#include "examples/Cassie/kinematic_centroidal_planner/simple_models/slip_constraints.h" + +#include "drake/multibody/parsing/parser.h" + +CassieKinematicCentroidalSolver::CassieKinematicCentroidalSolver( + const drake::multibody::MultibodyPlant& plant, int n_knot_points, + double dt, double mu, const drake::VectorX& nominal_stand, double k, + double b, double r0, double stance_width) + : KinematicCentroidalSolver(plant, n_knot_points, dt, + CreateContactPoints(plant, mu)), + l_loop_evaluator_(dairlib::LeftLoopClosureEvaluator(Plant())), + r_loop_evaluator_(dairlib::RightLoopClosureEvaluator(Plant())), + loop_closure_evaluators(Plant()), + slip_contact_sequence_(n_knot_points), + k_(k), + r0_(r0), + b_(b), + nominal_stand_(nominal_stand), + positions_map_(dairlib::multibody::MakeNameToPositionsMap(plant_)), + velocity_map_(dairlib::multibody::MakeNameToVelocitiesMap(plant_)) { + AddPlantJointLimits(dairlib::JointNames()); + AddLoopClosure(); + + slip_contact_points_ = CreateSlipContactPoints(plant, mu); + for (int knot = 0; knot < n_knot_points; knot++) { + slip_com_vars_.push_back( + prog_->NewContinuousVariables(3, "slip_com_" + std::to_string(knot))); + slip_vel_vars_.push_back( + prog_->NewContinuousVariables(3, "slip_vel_" + std::to_string(knot))); + slip_contact_pos_vars_.push_back(prog_->NewContinuousVariables( + 2 * 3, "slip_contact_pos_" + std::to_string(knot))); + slip_contact_vel_vars_.push_back(prog_->NewContinuousVariables( + 2 * 3, "slip_contact_vel_" + std::to_string(knot))); + slip_force_vars_.push_back( + prog_->NewContinuousVariables(2, "slip_force_" + std::to_string(knot))); + } + m_ = plant_.CalcTotalMass(*contexts_[0]); +} + +std::vector> +CassieKinematicCentroidalSolver::CreateSlipContactPoints( + const drake::multibody::MultibodyPlant& plant, double mu) { + auto left_toe_pair = dairlib::LeftToeFront(plant); + auto right_toe_pair = dairlib::RightToeFront(plant); + std::vector active_inds{0, 1, 2}; + + auto left_toe_eval = dairlib::multibody::WorldPointEvaluator( + plant, left_toe_pair.first, left_toe_pair.second, + Eigen::Matrix3d::Identity(), Eigen::Vector3d::Zero(), active_inds); + left_toe_eval.set_frictional(); + left_toe_eval.set_mu(mu); + + auto right_toe_eval = dairlib::multibody::WorldPointEvaluator( + plant, right_toe_pair.first, right_toe_pair.second, + Eigen::Matrix3d::Identity(), Eigen::Vector3d::Zero(), active_inds); + right_toe_eval.set_frictional(); + right_toe_eval.set_mu(mu); + + return {left_toe_eval, right_toe_eval}; +} void CassieKinematicCentroidalSolver::AddLoopClosure() { loop_closure_evaluators.add_evaluator(&l_loop_evaluator_); loop_closure_evaluators.add_evaluator(&r_loop_evaluator_); auto loop_closure = std::make_shared>( - Plant(), - loop_closure_evaluators, - Eigen::VectorXd::Zero(2), + Plant(), loop_closure_evaluators, Eigen::VectorXd::Zero(2), Eigen::VectorXd::Zero(2)); for (int knot_point = 1; knot_point < num_knot_points(); knot_point++) { - AddKinematicConstraint(loop_closure, state_vars(knot_point).head(Plant().num_positions())); + AddKinematicConstraint( + loop_closure, state_vars(knot_point).head(Plant().num_positions())); + } +} + +void CassieKinematicCentroidalSolver::AddSlipConstraints(int knot_point) { + for (int contact_index = 0; contact_index < slip_contact_points_.size(); + contact_index++) { + prog_->AddConstraint((slip_contact_pos_vars(knot_point, contact_index) - + slip_com_vars_[knot_point]) + .squaredNorm() <= pow(max_slip_leg_length_, 2)); + + if (slip_contact_sequence_[knot_point][contact_index]) { + // Foot isn't moving + prog_->AddBoundingBoxConstraint( + Eigen::VectorXd::Zero(3), Eigen::VectorXd::Zero(3), + slip_contact_vel_vars(knot_point, contact_index)); + // Foot is on the ground + prog_->AddBoundingBoxConstraint( + slip_ground_offset_, slip_ground_offset_, + slip_contact_pos_vars(knot_point, contact_index)[2]); + } else { + prog_->AddBoundingBoxConstraint( + 0, 0, slip_force_vars_[knot_point][contact_index]); + // Feet are above the ground + double lb = 0; + // Check if at least one of the time points before or after is also in + // flight before restricting the foot to be in the air to limit over + // constraining the optimization problem + if (!is_first_knot(knot_point) and !is_last_knot(knot_point) and + (!slip_contact_sequence_[knot_point - 1][contact_index] or + !slip_contact_sequence_[knot_point + 1][contact_index])) { + lb = swing_foot_minimum_height_; + } + lb += slip_ground_offset_; + prog_->AddBoundingBoxConstraint( + lb, 0.5, slip_contact_pos_vars(knot_point, contact_index)[2]); + } + } +} + +void CassieKinematicCentroidalSolver::AddSlipCost(int knot_point, + double terminal_gain) { + const double t = dt_ * knot_point; + if (com_ref_traj_) { + prog_->AddQuadraticErrorCost(terminal_gain * Q_com_, + com_ref_traj_->value(t), + slip_com_vars_[knot_point]); + } + + // Project linear momentum cost into center of mass velocity by multiplying + // gain by mass + if (mom_ref_traj_) { + const Eigen::MatrixXd Q_vel = m_ * gains_.lin_momentum.asDiagonal(); + prog_->AddQuadraticErrorCost( + terminal_gain * Q_vel, Eigen::VectorXd(mom_ref_traj_->value(t)).tail(3), + slip_vel_vars_[knot_point]); + } + // Project com velocity + if (v_ref_traj_) { + const Eigen::MatrixXd Q_vel = Q_v_.block(3, 3, 3, 3); + prog_->AddQuadraticErrorCost( + terminal_gain * Q_vel, + Eigen::VectorXd(v_ref_traj_->value(t)).segment(4, 3), + slip_vel_vars_[knot_point]); + } + + if (contact_ref_traj_) { + const Eigen::MatrixXd Q_contact_pos = 2 * gains_.contact_pos.asDiagonal(); + prog_->AddQuadraticErrorCost( + terminal_gain * Q_contact_pos, + Eigen::VectorXd(contact_ref_traj_->value(t)).segment(0, 3), + slip_contact_pos_vars(knot_point, 0)); + prog_->AddQuadraticErrorCost( + terminal_gain * Q_contact_pos, + Eigen::VectorXd(contact_ref_traj_->value(t)).segment(6, 3), + slip_contact_pos_vars(knot_point, 1)); + + const Eigen::MatrixXd Q_contact_vel = 2 * gains_.contact_vel.asDiagonal(); + prog_->AddQuadraticErrorCost( + terminal_gain * Q_contact_vel, + Eigen::VectorXd(contact_ref_traj_->value(t)).segment(12, 3), + slip_contact_vel_vars(knot_point, 0)); + prog_->AddQuadraticErrorCost( + terminal_gain * Q_contact_vel, + Eigen::VectorXd(contact_ref_traj_->value(t)).segment(18, 3), + slip_contact_vel_vars(knot_point, 1)); + } + if (force_ref_traj_) { + const Eigen::MatrixXd Q_contact_pos = + Eigen::Vector2d(gains_.contact_force[2], gains_.contact_force[2]) + .asDiagonal(); + prog_->AddQuadraticErrorCost(terminal_gain * Q_contact_pos, + Eigen::Vector2d::Zero(2), + slip_force_vars_[knot_point]); + } +} + +void CassieKinematicCentroidalSolver::SetModeSequence( + const std::vector>& contact_sequence) { + KinematicCentroidalSolver::SetModeSequence(contact_sequence); + MapModeSequence(); +} +void CassieKinematicCentroidalSolver::SetModeSequence( + const drake::trajectories::PiecewisePolynomial& contact_sequence) { + KinematicCentroidalSolver::SetModeSequence(contact_sequence); + MapModeSequence(); +} + +void CassieKinematicCentroidalSolver::MapModeSequence() { + for (int knot_point = 0; knot_point < num_knot_points(); knot_point++) { + slip_contact_sequence_[knot_point] = + complex_mode_to_slip_mode_.at(contact_sequence_[knot_point]); + } +} +void CassieKinematicCentroidalSolver::AddSlipEqualityConstraint( + int knot_point) { + prog_->AddConstraint(slip_com_vars_[knot_point] == com_pos_vars(knot_point)); + prog_->AddConstraint(m_ * slip_vel_vars_[knot_point] == + momentum_vars(knot_point).tail(3)); + prog_->AddConstraint(slip_contact_pos_vars(knot_point, 0) == + contact_pos_vars(knot_point, 0)); + prog_->AddConstraint(slip_contact_pos_vars(knot_point, 1) == + contact_pos_vars(knot_point, 2)); + prog_->AddConstraint(slip_contact_vel_vars(knot_point, 0) == + contact_vel_vars(knot_point, 0)); + prog_->AddConstraint(slip_contact_vel_vars(knot_point, 1) == + contact_vel_vars(knot_point, 2)); + auto grf_constraint = std::make_shared( + plant_, reducers[knot_point], 2, 4, knot_point); + prog_->AddConstraint( + grf_constraint, + {com_pos_vars(knot_point), slip_vel_vars_[knot_point], + slip_contact_pos_vars_[knot_point], contact_force_[knot_point], + slip_force_vars_[knot_point]}); + + AddSlipPosturePrincipleConstraint(knot_point); +} + +void CassieKinematicCentroidalSolver::AddSlipDynamics(int knot_point) { + if (!is_last_knot(knot_point)) { + auto slip_com_dynamics = std::make_shared>( + r0_, k_, b_, m_, 2, slip_contact_sequence_[knot_point], + slip_contact_sequence_[knot_point + 1], dt_, knot_point); + + slip_dynamics_binding_.push_back(prog_->AddConstraint( + slip_com_dynamics, + {slip_com_vars_[knot_point], slip_vel_vars_[knot_point], + slip_contact_pos_vars_[knot_point], slip_force_vars_[knot_point], + slip_com_vars_[knot_point + 1], slip_vel_vars_[knot_point + 1], + slip_contact_pos_vars_[knot_point + 1], + slip_force_vars_[knot_point + 1]})); + + // Trapazoidal integration of the contact positions + prog_->AddConstraint(slip_contact_pos_vars_[knot_point + 1] == + slip_contact_pos_vars_[knot_point] + + 0.5 * dt_ * + (slip_contact_vel_vars_[knot_point] + + slip_contact_vel_vars_[knot_point + 1])); + } +} + +drake::solvers::VectorXDecisionVariable +CassieKinematicCentroidalSolver::slip_contact_pos_vars(int knot_point_index, + int slip_foot_index) { + return slip_contact_pos_vars_[knot_point_index].segment(3 * slip_foot_index, + 3); +} +drake::solvers::VectorXDecisionVariable +CassieKinematicCentroidalSolver::slip_contact_vel_vars(int knot_point_index, + int slip_foot_index) { + return slip_contact_vel_vars_[knot_point_index].segment(3 * slip_foot_index, + 3); +} + +void CassieKinematicCentroidalSolver::SetComPositionGuess( + const drake::trajectories::PiecewisePolynomial& com_trajectory) { + for (int knot_point = 0; knot_point < n_knot_points_; knot_point++) { + prog_->SetInitialGuess(slip_com_vars_[knot_point], + com_trajectory.value(dt_ * knot_point)); + } + KinematicCentroidalSolver::SetComPositionGuess(com_trajectory); +} + +void CassieKinematicCentroidalSolver::SetRobotStateGuess( + const drake::trajectories::PiecewisePolynomial& q_traj, + const drake::trajectories::PiecewisePolynomial& v_traj) { + for (int knot_point = 0; knot_point < n_knot_points_; knot_point++) { + dairlib::multibody::SetPositionsIfNew( + plant_, q_traj.value(dt_ * knot_point), contexts_[knot_point].get()); + dairlib::multibody::SetVelocitiesIfNew( + plant_, v_traj.value(dt_ * knot_point), contexts_[knot_point].get()); + for (int contact = 0; contact < slip_contact_points_.size(); contact++) { + prog_->SetInitialGuess( + slip_contact_pos_vars(knot_point, contact), + slip_contact_points_[contact].EvalFull(*contexts_[knot_point])); + prog_->SetInitialGuess( + slip_contact_vel_vars(knot_point, contact), + slip_contact_points_[contact].EvalFullTimeDerivative( + *contexts_[knot_point])); + } + } + KinematicCentroidalSolver::SetRobotStateGuess(q_traj, v_traj); +} + +void CassieKinematicCentroidalSolver::SetMomentumGuess( + const drake::trajectories::PiecewisePolynomial& + momentum_trajectory) { + for (int knot_point = 0; knot_point < n_knot_points_; knot_point++) { + prog_->SetInitialGuess( + slip_vel_vars_[knot_point], + drake::VectorX(momentum_trajectory.value(dt_ * knot_point)) + .tail(3) / + m_); + } + KinematicCentroidalSolver::SetMomentumGuess(momentum_trajectory); +} + +drake::VectorX CassieKinematicCentroidalSolver::LiftSlipSolution( + int knot_point) { + drake::VectorX slip_state(3 + 3 + 6 * slip_contact_points_.size() + + slip_contact_points_.size()); + slip_state << result_->GetSolution(slip_com_vars_[knot_point]), + result_->GetSolution(slip_vel_vars_[knot_point]), + result_->GetSolution(slip_contact_pos_vars_[knot_point]), + result_->GetSolution(slip_contact_vel_vars_[knot_point]), + result_->GetSolution(slip_force_vars_[knot_point]); + return lifters_[knot_point]->Lift(slip_state).tail(n_q_ + n_v_); +} + +void CassieKinematicCentroidalSolver::SetForceGuess( + const drake::trajectories::PiecewisePolynomial& force_trajectory) { + for (const auto& force_vars : slip_force_vars_) { + // TODO find better initial guess + prog_->SetInitialGuess(force_vars, + 0 * drake::VectorX::Ones(force_vars.size())); + } + KinematicCentroidalSolver::SetForceGuess(force_trajectory); +} +void CassieKinematicCentroidalSolver::Build( + const drake::solvers::SolverOptions& solver_options) { + for (int knot = 0; knot < n_knot_points_; knot++) { + lifters_.emplace_back(new SlipLifter( + plant_, contexts_[knot].get(), slip_contact_points_, + CreateContactPoints(plant_, 0), {{0, {0, 1}}, {1, {2, 3}}}, + nominal_stand_, k_, b_, r0_, slip_contact_sequence_[knot])); + reducers.emplace_back(new SlipReducer( + plant_, contexts_[knot].get(), slip_contact_points_, + CreateContactPoints(plant_, 0), {{0, {0, 1}}, {1, {2, 3}}}, k_, b_, r0_, + slip_contact_sequence_[knot])); } + KinematicCentroidalSolver::Build(solver_options); +} +void CassieKinematicCentroidalSolver::AddSlipPosturePrincipleConstraint( + int knot_point) { + const double eps = 1e-2; + // Zero hip yaw + prog_->AddBoundingBoxConstraint( + -eps, eps, x_vars_[knot_point][positions_map_.at("hip_yaw_left")]); + prog_->AddBoundingBoxConstraint( + -eps, eps, x_vars_[knot_point][positions_map_.at("hip_yaw_right")]); + // // Zero hip yaw dot + prog_->AddBoundingBoxConstraint( + -eps, eps, + state_vars(knot_point)[n_q_ + velocity_map_.at("hip_yaw_leftdot")]); + prog_->AddBoundingBoxConstraint( + -eps, eps, + state_vars(knot_point)[n_q_ + velocity_map_.at("hip_yaw_rightdot")]); + + // Identity orientation + // TODO figure out why this identity quaternion constraint is hard for the mpc + prog_->AddBoundingBoxConstraint( + 1 - eps, 1 + eps, state_vars(knot_point)[positions_map_.at("base_qw")]); + // prog_->AddBoundingBoxConstraint( + // -eps, eps, state_vars(knot_point)[positions_map_.at("base_qx")]); + // prog_->AddBoundingBoxConstraint( + // -eps, eps, state_vars(knot_point)[positions_map_.at("base_qy")]); + prog_->AddBoundingBoxConstraint( + -eps, eps, state_vars(knot_point)[positions_map_.at("base_qz")]); + + // Zero angular velocity + prog_->AddBoundingBoxConstraint( + -eps, eps, state_vars(knot_point)[n_q_ + velocity_map_.at("base_wx")]); + prog_->AddBoundingBoxConstraint( + -eps, eps, state_vars(knot_point)[n_q_ + velocity_map_.at("base_wy")]); + prog_->AddBoundingBoxConstraint( + -eps, eps, state_vars(knot_point)[n_q_ + velocity_map_.at("base_wz")]); + // Toe and heel same velocity + prog_->AddConstraint(contact_vel_vars(knot_point, 0) == + contact_vel_vars(knot_point, 1)); + prog_->AddConstraint(contact_vel_vars(knot_point, 2) == + contact_vel_vars(knot_point, 3)); + + // Toe and heel same height + prog_->AddConstraint(contact_pos_vars(knot_point, 0)[2] == + contact_pos_vars(knot_point, 1)[2]); + prog_->AddConstraint(contact_pos_vars(knot_point, 2)[2] == + contact_pos_vars(knot_point, 3)[2]); + + // // GRF per foot equal + prog_->AddConstraint(contact_force_vars(knot_point, 0) == + contact_force_vars(knot_point, 1)); + prog_->AddConstraint(contact_force_vars(knot_point, 2) == + contact_force_vars(knot_point, 3)); + + // TODO figure out this constraint (nonlinear constraint) + // GRF per foot parallel to vector from foot center to com + // prog_->AddConstraint((contact_force_vars(knot_point, 0) + // / 1.0).normalized() == + // (com_pos_vars(knot_point) - + // contact_pos_vars(knot_point, 0) / 2.0 - + // contact_pos_vars(knot_point, 1) / 2.0) + // .normalized()); + // + // prog_->AddConstraint((contact_force_vars(knot_point, 2) + // / 1.0).normalized() == + // (com_pos_vars(knot_point) - + // contact_pos_vars(knot_point, 2) / 2.0 - + // contact_pos_vars(knot_point, 3) / 2.0) + // .normalized()); +} +void CassieKinematicCentroidalSolver::SetMaximumSlipLegLength( + double max_leg_length) { + max_slip_leg_length_ = max_leg_length; } diff --git a/examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.h b/examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.h index de2c3a6d2d..071afe60e9 100644 --- a/examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.h +++ b/examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.h @@ -1,25 +1,111 @@ #pragma once #include + +#include "examples/Cassie/cassie_utils.h" +#include "examples/Cassie/kinematic_centroidal_planner/simple_models/slip_lifter.h" +#include "examples/Cassie/kinematic_centroidal_planner/simple_models/slip_reducer.h" #include "systems/trajectory_optimization/kinematic_centroidal_planner/kinematic_centroidal_solver.h" #include "systems/trajectory_optimization/kinematic_centroidal_planner/reference_generation_utils.h" + #include "drake/multibody/plant/multibody_plant.h" -#include "examples/Cassie/cassie_utils.h" /*! - * @brief Cassie specific child class for kinematic centroidal mpc. Adds loop closure, joint limits, and cassie contact points + * @brief Cassie specific child class for kinematic centroidal mpc. Adds loop + * closure, joint limits, and cassie contact points */ class CassieKinematicCentroidalSolver : public KinematicCentroidalSolver { public: - CassieKinematicCentroidalSolver(const drake::multibody::MultibodyPlant& plant, int n_knot_points, double dt, double mu) : - KinematicCentroidalSolver(plant, n_knot_points, dt, CreateContactPoints(plant, mu)), - l_loop_evaluator_(dairlib::LeftLoopClosureEvaluator(Plant())), - r_loop_evaluator_(dairlib::RightLoopClosureEvaluator(Plant())), - loop_closure_evaluators(Plant()){ - AddPlantJointLimits(dairlib::JointNames()); - AddLoopClosure(); - } + CassieKinematicCentroidalSolver( + const drake::multibody::MultibodyPlant& plant, int n_knot_points, + double dt, double mu, const drake::VectorX& nominal_stand, + double k = 1000, double b = 20, double r0 = 0.5, + double stance_width = 0.2); + + std::vector> + CreateSlipContactPoints(const drake::multibody::MultibodyPlant& plant, + double mu); + + /*! + * @brief Set the mode sequence + * @param contact_sequence vector where + * `contact_sequence[knot_point][contact_index]` tells you if at `knot_point` + * is `contact_index` active + */ + void SetModeSequence( + const std::vector>& contact_sequence) override; + + /*! + * @brief Set the mode sequence via a trajectory. The value of the trajectory + * at each time, cast to a bool is if a contact point is active or not + * @param contact_sequence + */ + void SetModeSequence(const drake::trajectories::PiecewisePolynomial& + contact_sequence) override; -private: + drake::solvers::VectorXDecisionVariable slip_contact_pos_vars( + int knot_point_index, int slip_foot_index); + + drake::solvers::VectorXDecisionVariable slip_contact_vel_vars( + int knot_point_index, int slip_foot_index); + + /*! + * @brief Overload of setting kc com position to also set slip com position + * @param com_trajectory + */ + void SetComPositionGuess( + const drake::trajectories::PiecewisePolynomial& com_trajectory) + override; + + /*! + * @brief overload of setting kc generalized state guess to also set slip + * contact postion and velocity + * @param q_traj + * @param v_traj + */ + void SetRobotStateGuess( + const drake::trajectories::PiecewisePolynomial& q_traj, + const drake::trajectories::PiecewisePolynomial& v_traj) override; + + /*! + * @brief overload of setting kc momentum guess to set slip com velocity + * @param momentum_trajectory + */ + void SetMomentumGuess(const drake::trajectories::PiecewisePolynomial& + momentum_trajectory) override; + + /*! + * @brief overload of setting kc force to set initial guess for slip force + * @param force_trajectory + */ + void SetForceGuess(const drake::trajectories::PiecewisePolynomial& + force_trajectory) override; + + void Build(const drake::solvers::SolverOptions& solver_options) override; + + void SetMaximumSlipLegLength(double max_leg_length); + + private: + /*! + * @brief Add constraint at knot point for complex state to be on the slip + * submanifold + * @param knot_point + */ + void AddSlipPosturePrincipleConstraint(int knot_point); + + /*! + * @brief map complex mode sequence to the simple mode sequence + */ + void MapModeSequence(); + + void AddSlipConstraints(int knot_point) override; + + void AddSlipCost(int knot_point, double terminal_gain) override; + + void AddSlipEqualityConstraint(int knot_point) override; + + void AddSlipDynamics(int knot_point) override; + + drake::VectorX LiftSlipSolution(int knot_point) override; /*! * @brief Adds loop closure constraints to the mpc @@ -30,5 +116,36 @@ class CassieKinematicCentroidalSolver : public KinematicCentroidalSolver { dairlib::multibody::DistanceEvaluator r_loop_evaluator_; dairlib::multibody::KinematicEvaluatorSet loop_closure_evaluators; -}; + std::vector> slip_contact_sequence_; + double k_; + double r0_; + double b_; + double m_; + const drake::VectorX nominal_stand_; + const double slip_ground_offset_ = 0; + double max_slip_leg_length_ = 10; + std::vector slip_com_vars_; + std::vector slip_vel_vars_; + std::vector slip_contact_pos_vars_; + std::vector slip_contact_vel_vars_; + std::vector slip_force_vars_; + + std::vector> lifters_; + std::vector> reducers; + + std::vector> + slip_contact_points_; + + std::unordered_map, std::vector> + complex_mode_to_slip_mode_{ + {{true, true, true, true}, {true, true}}, + {{true, true, false, false}, {true, false}}, + {{false, false, true, true}, {false, true}}, + {{false, false, false, false}, {false, false}}}; + std::vector> + slip_dynamics_binding_; + + std::map positions_map_; + std::map velocity_map_; +}; diff --git a/examples/Cassie/kinematic_centroidal_planner/gaits/run.yaml b/examples/Cassie/kinematic_centroidal_planner/gaits/run.yaml new file mode 100644 index 0000000000..f676a9c7e7 --- /dev/null +++ b/examples/Cassie/kinematic_centroidal_planner/gaits/run.yaml @@ -0,0 +1,14 @@ +gait_pattern: + - start_phase: 0.0 + end_phase: 0.45 + contact_status: [true, true, false, false] + - start_phase: 0.45 + end_phase: 0.5 + contact_status: [false, false, false, false] + - start_phase: 0.5 + end_phase: 0.95 + contact_status: [false, false, true, true] + - start_phase: 0.95 + end_phase: 1.0 + contact_status: [false, false, false, false] +period: 1.0 \ No newline at end of file diff --git a/examples/Cassie/kinematic_centroidal_planner/gaits/stand.yaml b/examples/Cassie/kinematic_centroidal_planner/gaits/stand.yaml index cc525ff774..bbe933b88f 100644 --- a/examples/Cassie/kinematic_centroidal_planner/gaits/stand.yaml +++ b/examples/Cassie/kinematic_centroidal_planner/gaits/stand.yaml @@ -2,4 +2,4 @@ gait_pattern: - start_phase: 0.0 end_phase: 1.0 contact_status: [true, true, true, true] -period: 1.0 +period: 0.55 diff --git a/examples/Cassie/kinematic_centroidal_planner/gaits/walk.yaml b/examples/Cassie/kinematic_centroidal_planner/gaits/walk.yaml index b1f566edd9..10cc5868a2 100644 --- a/examples/Cassie/kinematic_centroidal_planner/gaits/walk.yaml +++ b/examples/Cassie/kinematic_centroidal_planner/gaits/walk.yaml @@ -11,4 +11,4 @@ gait_pattern: - start_phase: 0.9 end_phase: 1.0 contact_status: [true, true, true, true] -period: 1.25 \ No newline at end of file +period: 1.1 diff --git a/examples/Cassie/kinematic_centroidal_planner/kinematic_centroidal_mpc_gains.yaml b/examples/Cassie/kinematic_centroidal_planner/kinematic_centroidal_mpc_gains.yaml index 20b3554563..d6f8f2eddf 100644 --- a/examples/Cassie/kinematic_centroidal_planner/kinematic_centroidal_mpc_gains.yaml +++ b/examples/Cassie/kinematic_centroidal_planner/kinematic_centroidal_mpc_gains.yaml @@ -1,4 +1,4 @@ -com_position: [1, 1, 10] +com_position: [2, 2, 5] generalized_positions: base_qw: 1.0 base_qx: 1.0 @@ -7,30 +7,30 @@ generalized_positions: base_x: 0 base_y: 0 base_z: 0 - hip_roll_: 0.01 - hip_yaw_: 0.01 - hip_pitch_: 0.001 - knee_: 0.001 - ankle_joint_: 0.001 - toe_: 0.0000 + hip_roll_: 0.001 + hip_yaw_: 0.001 + hip_pitch_: 0.00001 + knee_: 0.00001 + ankle_joint_: 0.00001 + toe_: 0.000001 generalized_velocities: base_wx: 0.5 base_wy: 0.5 base_wz: 0.5 - base_vx: 0.1 - base_vy: 0.1 + base_vx: 0.2 + base_vy: 0.2 base_vz: 0.1 - hip_roll_: 0.02 - hip_yaw_: 0.02 - hip_pitch_: 0.002 - knee_: 0.002 - ankle_joint_: 0.002 - toe_: 0.004 + hip_roll_: 0.002 + hip_yaw_: 0.002 + hip_pitch_: 0.00002 + knee_: 0.00002 + ankle_joint_: 0.00002 + toe_: 0.00004 -contact_pos: [0.1, 16, 0.001] -contact_vel: [0.02, 0.02, 0.02] -contact_force: [0.0001, 0.0001, 0.0001] +contact_pos: [2, 5, 0.001] +contact_vel: [0.4, 0.4, 0.1] +contact_force: [0.00002, 0.00002, 0.0004] lin_momentum: [0.00001, 0.00001, 0.00001] -ang_momentum: [0.0000001, 0.0000001, 0.0000001] +ang_momentum: [0.0000000, 0.0000000, 0.0000000] tol: 1e-2 diff --git a/examples/Cassie/kinematic_centroidal_planner/motions/BUILD.bazel b/examples/Cassie/kinematic_centroidal_planner/motions/BUILD.bazel index 1e7674b735..a68c695a36 100644 --- a/examples/Cassie/kinematic_centroidal_planner/motions/BUILD.bazel +++ b/examples/Cassie/kinematic_centroidal_planner/motions/BUILD.bazel @@ -5,7 +5,8 @@ filegroup( data = ["motion_test.yaml", "motion_left_step.yaml", "motion_right_step.yaml", - "motion_jump.yaml"], + "motion_jump.yaml", + "motion_walk.yaml"], visibility = ["//visibility:public"], ) diff --git a/examples/Cassie/kinematic_centroidal_planner/motions/motion_jump.yaml b/examples/Cassie/kinematic_centroidal_planner/motions/motion_jump.yaml index ffc7579313..3f8f8e2111 100644 --- a/examples/Cassie/kinematic_centroidal_planner/motions/motion_jump.yaml +++ b/examples/Cassie/kinematic_centroidal_planner/motions/motion_jump.yaml @@ -2,6 +2,9 @@ n_knot_points: 25 target_com_height: 0.8 stance_width: 0.25 swing_foot_minimum_height: 0.015 +max_slip_leg_length: 1 +spring_constant: 3000 +damping_coefficient: 115 duration_scaling: [ 1.0, 1.0, 1.0 ] gait_sequence: [ 'stand', 'jump', 'stand' ] @@ -9,3 +12,4 @@ com_vel_values: [ 0, 0, 0, 0.5, 0, 0, 0, 0, 0 ] +complexity_schedule: ['c 25'] diff --git a/examples/Cassie/kinematic_centroidal_planner/motions/motion_left_step.yaml b/examples/Cassie/kinematic_centroidal_planner/motions/motion_left_step.yaml index aee4daa77a..3b1f73c7ce 100644 --- a/examples/Cassie/kinematic_centroidal_planner/motions/motion_left_step.yaml +++ b/examples/Cassie/kinematic_centroidal_planner/motions/motion_left_step.yaml @@ -2,6 +2,9 @@ n_knot_points: 20 target_com_height: 0.8 stance_width: 0.25 swing_foot_minimum_height: 0.1 +max_slip_leg_length: 1 +spring_constant: 3000 +damping_coefficient: 115 duration_scaling: [ 1.0, 1.0, 1.0 ] gait_sequence: [ 'stand', 'left_step', 'stand' ] @@ -9,3 +12,4 @@ com_vel_values: [ 0, 0, 0, 1.0, 0, 0, 0, 0, 0 ] +complexity_schedule: ['c 20'] diff --git a/examples/Cassie/kinematic_centroidal_planner/motions/motion_right_step.yaml b/examples/Cassie/kinematic_centroidal_planner/motions/motion_right_step.yaml index feb7ef4c5b..1ba16e6a06 100644 --- a/examples/Cassie/kinematic_centroidal_planner/motions/motion_right_step.yaml +++ b/examples/Cassie/kinematic_centroidal_planner/motions/motion_right_step.yaml @@ -2,6 +2,9 @@ n_knot_points: 20 target_com_height: 0.8 stance_width: 0.25 swing_foot_minimum_height: 0.1 +max_slip_leg_length: 1 +spring_constant: 3000 +damping_coefficient: 115 duration_scaling: [ 1.0, 1.0, 1.0 ] gait_sequence: [ 'stand', 'right_step', 'stand' ] @@ -9,3 +12,4 @@ com_vel_values: [ 0, 0, 0, 1.0, 0, 0, 0, 0, 0 ] +complexity_schedule: ['c 20'] diff --git a/examples/Cassie/kinematic_centroidal_planner/motions/motion_test.yaml b/examples/Cassie/kinematic_centroidal_planner/motions/motion_test.yaml index ee2d58ea91..01b8b32e90 100644 --- a/examples/Cassie/kinematic_centroidal_planner/motions/motion_test.yaml +++ b/examples/Cassie/kinematic_centroidal_planner/motions/motion_test.yaml @@ -2,6 +2,9 @@ n_knot_points: 25 target_com_height: 0.8 stance_width: 0.25 swing_foot_minimum_height: 0.03 +max_slip_leg_length: 1 +spring_constant: 3000 +damping_coefficient: 115 duration_scaling: [ 1.0, 1.0, 1.0 ] gait_sequence: [ 'stand', 'left_step', 'stand' ] @@ -9,3 +12,5 @@ com_vel_values: [ 0, 0, 0, 0, 0, 0, 0, 0, 0 ] +complexity_schedule: ['c 25'] + diff --git a/examples/Cassie/kinematic_centroidal_planner/motions/motion_walk.yaml b/examples/Cassie/kinematic_centroidal_planner/motions/motion_walk.yaml new file mode 100644 index 0000000000..e55d663b9e --- /dev/null +++ b/examples/Cassie/kinematic_centroidal_planner/motions/motion_walk.yaml @@ -0,0 +1,15 @@ +n_knot_points: 45 +target_com_height: 0.8 +stance_width: 0.25 +swing_foot_minimum_height: 0.02 +max_slip_leg_length: 1 +spring_constant: 3000 +damping_coefficient: 115 +duration_scaling: + [ 1.0, 3.0, 1.0 ] +gait_sequence: [ 'stand', 'walk', 'walk' ] +com_vel_values: + [ 0, 0, 0, + 0.5, 0.0, 0, + 0, 0, 0 ] +complexity_schedule: ['c 10', 's 20', 'c 15'] diff --git a/examples/Cassie/kinematic_centroidal_planner/simple_models/BUILD.bazel b/examples/Cassie/kinematic_centroidal_planner/simple_models/BUILD.bazel new file mode 100644 index 0000000000..b18e0ac35a --- /dev/null +++ b/examples/Cassie/kinematic_centroidal_planner/simple_models/BUILD.bazel @@ -0,0 +1,37 @@ +package(default_visibility = ["//visibility:public"]) + + +cc_library( + name = "slip", + srcs = ["slip_constraints.cc", + "slip_lifter.cc", + "slip_reducer.cc", + "slip_utils.cc"], + hdrs = ["slip_constraints.h", + "slip_lifter.h", + "slip_reducer.h", + "slip_utils.h"], + deps = [ + "//solvers:constraints", + "//multibody/kinematic:kinematic", + "//multibody/kinematic:constraints", + "//solvers:nonlinear_cost", + "@drake//:drake_shared_library", + ], +) + +cc_binary( + name = "cassie_slip_lifting_test", + srcs = ["slip_lifting_test.cc"], + deps = [ + "//examples/Cassie:cassie_utils", + "slip", + "//common", + "//multibody:visualization_utils", + "//multibody/kinematic", + "//systems/primitives", + "//examples/Cassie/kinematic_centroidal_planner:cassie_reference_utils", + "@drake//:drake_shared_library", + "@gflags", + ], +) diff --git a/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_constraints.cc b/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_constraints.cc new file mode 100644 index 0000000000..675d0e371c --- /dev/null +++ b/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_constraints.cc @@ -0,0 +1,162 @@ +#include "slip_constraints.h" + +#include +#include + +#include "examples/Cassie/kinematic_centroidal_planner/simple_models/slip_utils.h" +#include "multibody/multibody_utils.h" + +double SlipGrf(double k, double r0, double b, double r, double dr, + double force) {} + +SlipReductionConstraint::SlipReductionConstraint( + const drake::multibody::MultibodyPlant& plant, + std::shared_ptr reducing_function, int n_slip_feet, + int n_complex_feet, int knot_index) + : dairlib::solvers::NonlinearConstraint( + 3 + 3 + 3 * n_slip_feet + 3 * n_slip_feet + n_slip_feet, + 3 + 3 + 3 * n_slip_feet + 3 * n_slip_feet + n_slip_feet + 6 + 3 + + 3 * 3 * n_complex_feet + plant.num_positions() + + plant.num_velocities(), + Eigen::VectorXd::Zero(3 + 3 + 3 * 2 * n_slip_feet + n_slip_feet), + Eigen::VectorXd::Zero(3 + 3 + 3 * 2 * n_slip_feet + n_slip_feet), + "SlipReductionConstraint[" + std::to_string(knot_index) + "]"), + reducing_function_(reducing_function), + slip_dim_(3 + 3 + 2 * 3 * n_slip_feet + n_slip_feet), + complex_dim_(6 + 3 + 3 * 3 * n_complex_feet + plant.num_positions() + + plant.num_velocities()) {} + +/// Input is of the form: +/// slip_com +/// slip_velocity +/// slip_contact_pos +/// slip_contact_vel +/// slip_force +/// complex_com +/// complex_ang_momentum +/// complex_lin_momentum +/// complex_contact_pos +/// complex_contact_vel +/// complex_contact_force +/// complex_gen_pos +/// complex_gen_vel +void SlipReductionConstraint::EvaluateConstraint( + const Eigen::Ref>& x, + drake::VectorX* y) const { + const auto& slip_state = x.head(slip_dim_); + const auto& complex_state = x.tail(complex_dim_); + *y = reducing_function_->Reduce(complex_state) - slip_state; +} + +SlipGrfReductionConstrain::SlipGrfReductionConstrain( + const drake::multibody::MultibodyPlant& plant, + std::shared_ptr reducing_function, int n_slip_feet, + int n_complex_feet, int knot_index) + : dairlib::solvers::NonlinearConstraint( + n_slip_feet, + 3 + 3 * n_slip_feet + 3 * n_complex_feet + n_slip_feet + 3, + Eigen::VectorXd::Zero(n_slip_feet), + Eigen::VectorXd::Zero(n_slip_feet), + "SlipGrfReductionConstraint[" + std::to_string(knot_index) + "]"), + reducing_function_(reducing_function), + n_slip_feet_(n_slip_feet), + n_complex_feet_(n_complex_feet) {} +/// Input is of the form: +/// complex_com +/// slip com velocity +/// slip_contact_pos +/// complex_grf +/// slip_contact_force +void SlipGrfReductionConstrain::EvaluateConstraint( + const Eigen::Ref>& x, + drake::VectorX* y) const { + const auto& complex_com = x.head(3); + const auto& slip_vel = x.segment(3, 3); + const auto& slip_contact_pos = x.segment(3 + 3, 3 * n_slip_feet_); + const auto& complex_grf = + x.segment(3 + 3 + 3 * n_slip_feet_, 3 * n_complex_feet_); + const auto& slip_contact_force = + x.segment(3 + 3 + 3 * n_slip_feet_ + 3 * n_complex_feet_, n_slip_feet_); + *y = reducing_function_->ReduceGrf(complex_com, slip_vel, slip_contact_pos, + complex_grf) - + slip_contact_force; +} + +template +SlipDynamicsConstraint::SlipDynamicsConstraint( + double r0, double k, double b, T m, int n_feet, + std::vector contact_mask0, std::vector contact_mask1, double dt, + int knot_index) + : dairlib::solvers::NonlinearConstraint( + 3 + 3, 2 * (3 + 3 + 3 * n_feet + n_feet), Eigen::VectorXd::Zero(6), + Eigen::VectorXd::Zero(6), + "SlipDynamicsConstraint[" + std::to_string(knot_index) + "]"), + r0_(r0), + k_(k), + b_(b), + m_(m), + n_feet_(n_feet), + contact_mask0_(std::move(contact_mask0)), + contact_mask1_(std::move(contact_mask1)), + dt_(dt) {} + +/// The format of the input to the eval() function is in the order +/// - com0, center of mass at time k +/// - vel0, center of mass velocity at time k +/// - contact_pos0, active contact positions at time k +/// - force0, slip force in parallel with spring at time k +/// - com1, center of mass at time k+1 +/// - vel1, center of mass velocity at time k+1 +/// - contact_pos1, active contact positions at time k+1 +/// - force1, slip force in parallel with spring at time k+1 +template +void SlipDynamicsConstraint::EvaluateConstraint( + const Eigen::Ref>& x, drake::VectorX* y) const { + const auto& com0 = x.head(3); + const auto& vel0 = x.segment(3, 3); + const auto& contact_pos0 = x.segment(3 + 3, n_feet_ * 3); + const auto& force0 = x.segment(3 + 3 + n_feet_ * 3, n_feet_); + const auto& com1 = x.segment(3 + 3 + n_feet_ * 3 + n_feet_, 3); + const auto& vel1 = x.segment(3 + 3 + n_feet_ * 3 + 3 + n_feet_, 3); + const auto& contact_pos1 = + x.segment(3 + 3 + n_feet_ * 3 + 3 + 3 + n_feet_, n_feet_ * 3); + const auto& force1 = + x.segment(3 + 3 + n_feet_ * 3 + 3 + 3 + n_feet_ + n_feet_ * 3, n_feet_); + + const auto& x0 = x.head(6); + const auto& x1 = x.segment(3 + 3 + n_feet_ * 3 + n_feet_, 6); + + drake::Vector xdot0 = CalcTimeDerivativesWithForce( + com0, vel0, contact_pos0, force0, contact_mask0_); + drake::Vector xdot1 = CalcTimeDerivativesWithForce( + com1, vel1, contact_pos1, force1, contact_mask1_); + + // Predict state and return error + const auto x1Predict = x0 + 0.5 * dt_ * (xdot0 + xdot1); + *y = x1 - x1Predict; +} + +template +drake::VectorX SlipDynamicsConstraint::CalcTimeDerivativesWithForce( + const drake::VectorX& com_position, const drake::VectorX& com_vel, + const drake::VectorX& contact_loc, const drake::VectorX& slip_force, + const std::vector& contact_mask) const { + drake::Vector3 ddcom = {0, 0, -9.81}; + for (int foot = 0; foot < n_feet_; foot++) { + if (contact_mask[foot]) { + drake::Vector3 com_rt_foot = + com_position - contact_loc.segment(3 * foot, 3); + const auto r = com_rt_foot.norm(); + const auto unit_vec = com_rt_foot / r; + const auto dr = com_vel.dot(unit_vec); + auto F = CalcSlipGrf(k_, r0_, b_, r, dr, slip_force[foot]); + ddcom = ddcom + F * unit_vec / m_; + } + } + drake::Vector6 derivative; + derivative << com_vel, ddcom; + return derivative; +} + +DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS( + class SlipDynamicsConstraint); diff --git a/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_constraints.h b/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_constraints.h new file mode 100644 index 0000000000..558a3ab1f7 --- /dev/null +++ b/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_constraints.h @@ -0,0 +1,76 @@ +#pragma once + +#include + +#include "examples/Cassie/kinematic_centroidal_planner/simple_models/slip_lifter.h" +#include "examples/Cassie/kinematic_centroidal_planner/simple_models/slip_reducer.h" +#include "multibody/kinematic/kinematic_evaluator_set.h" +#include "multibody/kinematic/world_point_evaluator.h" +#include "solvers/nonlinear_constraint.h" +#include "solvers/nonlinear_cost.h" + +#include "drake/common/drake_copyable.h" +#include "drake/common/symbolic.h" +#include "drake/solvers/constraint.h" + +class SlipReductionConstraint + : public dairlib::solvers::NonlinearConstraint { + public: + SlipReductionConstraint(const drake::multibody::MultibodyPlant& plant, + std::shared_ptr reducing_function, + int n_slip_feet, int n_complex_feet, int knot_index); + + private: + void EvaluateConstraint(const Eigen::Ref>& x, + drake::VectorX* y) const override; + + std::shared_ptr reducing_function_; + const int slip_dim_; + const int complex_dim_; +}; + +class SlipGrfReductionConstrain + : public dairlib::solvers::NonlinearConstraint { + public: + SlipGrfReductionConstrain( + const drake::multibody::MultibodyPlant& plant, + std::shared_ptr reducing_function, int n_slip_feet, + int n_complex_feet, int knot_index); + + private: + void EvaluateConstraint(const Eigen::Ref>& x, + drake::VectorX* y) const override; + + std::shared_ptr reducing_function_; + const int n_slip_feet_; + const int n_complex_feet_; +}; + +template +class SlipDynamicsConstraint : public dairlib::solvers::NonlinearConstraint { + public: + SlipDynamicsConstraint(double r0, double k, double b, T m, int n_feet, + std::vector contact_mask0, + std::vector contact_mask1, double dt, + int knot_index); + + private: + void EvaluateConstraint(const Eigen::Ref>& x, + drake::VectorX* y) const override; + + drake::VectorX CalcTimeDerivativesWithForce( + const drake::VectorX& com_position, const drake::VectorX& com_vel, + const drake::VectorX& contact_loc, const drake::VectorX& slip_force, + const std::vector& contact_mask) const; + + const double r0_; + const double k_; + const double b_; + const T m_; + const int n_feet_; + + // Boolean vectors describing which feet are active (in stance). + const std::vector contact_mask0_; + const std::vector contact_mask1_; + const double dt_; +}; \ No newline at end of file diff --git a/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_lifter.cc b/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_lifter.cc new file mode 100644 index 0000000000..8b1ebe5b72 --- /dev/null +++ b/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_lifter.cc @@ -0,0 +1,304 @@ +#include "slip_lifter.h" + +#include + +#include + +#include "examples/Cassie/kinematic_centroidal_planner/simple_models/slip_utils.h" +#include "multibody/multibody_utils.h" + +SlipLifter::SlipLifter( + const drake::multibody::MultibodyPlant& plant, + drake::systems::Context* context, + const std::vector>& + slip_contact_points, + const std::vector>& + complex_contact_points, + const std::map>& + simple_foot_index_to_complex_foot_index, + const drake::VectorX& nominal_stand, double k, double b, double r0, + const std::vector& contact_mask) + : plant_(plant), + context_(context), + ik_(plant, context), + m_(plant.CalcTotalMass(*context)), + k_(k), + b_(b), + r0_(r0), + slip_contact_points_(slip_contact_points), + complex_contact_points_(complex_contact_points), + simple_foot_index_to_complex_foot_index_( + simple_foot_index_to_complex_foot_index), + n_q_(plant.num_positions()), + n_v_(plant.num_velocities()), + slip_contact_mask_(contact_mask) { + ik_.get_mutable_prog()->SetInitialGuess(ik_.q(), nominal_stand); + com_vars_ = ik_.get_mutable_prog()->NewContinuousVariables(3); + auto constraint = std::make_shared( + &plant, std::nullopt, plant.world_frame(), context); + ik_.get_mutable_prog()->AddConstraint(constraint, {ik_.q(), com_vars_}); + + const auto& world_frame = plant.world_frame(); + const auto& pelvis_frame = plant.GetFrameByName("pelvis"); + + ik_.AddOrientationConstraint( + pelvis_frame, drake::math::RotationMatrix(), world_frame, + drake::math::RotationMatrix(), 1e-4); + + std::map positions_map = + dairlib::multibody::MakeNameToPositionsMap(plant); + ik_.get_mutable_prog()->AddLinearConstraint( + (ik_.q())(positions_map.at("hip_yaw_left")) == 0); + ik_.get_mutable_prog()->AddLinearConstraint( + (ik_.q())(positions_map.at("hip_yaw_right")) == 0); + + // Four bar linkage constraint (without spring) + ik_.get_mutable_prog()->AddLinearConstraint( + (ik_.q())(positions_map.at("knee_left")) + + (ik_.q())(positions_map.at("ankle_joint_left")) == + M_PI * 13 / 180.0); + ik_.get_mutable_prog()->AddLinearConstraint( + (ik_.q())(positions_map.at("knee_right")) + + (ik_.q())(positions_map.at("ankle_joint_right")) == + M_PI * 13 / 180.0); +} + +drake::VectorX SlipLifter::LiftGeneralizedPosition( + const drake::Vector3& com_position, + const drake::VectorX& slip_feet_positions) const { + DRAKE_DEMAND(slip_feet_positions.size() == 3 * slip_contact_points_.size()); + // Add com position constraint + const auto com_constraint = ik_.get_mutable_prog()->AddBoundingBoxConstraint( + Eigen::VectorXd::Zero(3), Eigen::VectorXd::Zero(3), com_vars_); + // Add feet position constraint + std::vector> + foot_constraints; + for (int i = 0; i < slip_contact_points_.size(); i++) { + const auto& slip_spatial_foot_pos = slip_feet_positions.segment(3 * i, 3); + const drake::Vector3 slip_foot_rt_com = + slip_spatial_foot_pos - com_position; + foot_constraints.push_back(ik_.AddPositionConstraint( + slip_contact_points_[i].get_frame(), slip_contact_points_[i].get_pt_A(), + plant_.world_frame(), std::nullopt, slip_foot_rt_com, + slip_foot_rt_com)); + for (const auto complex_index : + simple_foot_index_to_complex_foot_index_.at(i)) { + const drake::Vector3 lb{-100, -100, slip_foot_rt_com[2]}; + const drake::Vector3 ub{100, 100, slip_foot_rt_com[2]}; + foot_constraints.push_back(ik_.AddPositionConstraint( + complex_contact_points_[complex_index].get_frame(), + complex_contact_points_[complex_index].get_pt_A(), + plant_.world_frame(), std::nullopt, lb, ub)); + } + } + // Set initial guess for com + ik_.get_mutable_prog()->SetInitialGuess(com_vars_, Eigen::VectorXd::Zero(3)); + // Solve + const auto result = drake::solvers::Solve(ik_.prog()); + const auto q_sol = result.GetSolution(ik_.q()); + // Normalize quaternion + drake::VectorX q_sol_normd(n_q_); + q_sol_normd << q_sol.head(4).normalized(), q_sol.tail(n_q_ - 4); + + q_sol_normd.segment(4, 3) = q_sol_normd.segment(4, 3) + com_position; + // Set initial guess for next time + ik_.get_mutable_prog()->SetInitialGuess(ik_.q(), q_sol_normd); + // Remove added constraints + ik_.get_mutable_prog()->RemoveConstraint(com_constraint); + for (const auto& constraint : foot_constraints) { + ik_.get_mutable_prog()->RemoveConstraint(constraint); + } + return q_sol_normd; +} +drake::VectorX SlipLifter::LiftGeneralizedVelocity( + const drake::VectorX& generalized_pos, + const drake::Vector3& linear_momentum, + const drake::Vector3& com_pos, + const drake::VectorX& slip_feet_velocities) const { + DRAKE_DEMAND(slip_feet_velocities.size() == 3 * slip_contact_points_.size()); + // Preallocate linear constraint + drake::MatrixX A(3 + 2 * 3 * slip_contact_points_.size(), + n_v_); // 3 rows for linear momentum, 3 rows for + // each slip foot, 3 rows for each slip foot + drake::VectorX b(3 + 2 * 3 * slip_contact_points_.size()); + + // order of constraints are: slip foot velocity, zero toe rotation, linear + // momentum, Zero toe rotation accomplished by constraining per foot complex + // contact velocities to be equal + + // set b for linear momentum + b.tail(3) = linear_momentum; + + dairlib::multibody::SetPositionsIfNew(plant_, generalized_pos, + context_); + for (int i = 0; i < slip_contact_points_.size(); i++) { + // Set A and b for slip foot velocity constraint + b.segment(3 * i, 3) = slip_feet_velocities.segment(3 * i, 3); + A.middleRows(3 * i, 3) = + slip_contact_points_[i].EvalFullJacobian(*context_); + // Set A for zero toe rotation + const auto contact_it = simple_foot_index_to_complex_foot_index_.find(i); + A.middleRows(3 * slip_contact_points_.size() + 3 * i, 3) = + complex_contact_points_[contact_it->second[0]].EvalFullJacobian( + *context_) - + complex_contact_points_[contact_it->second[1]].EvalFullJacobian( + *context_); + } + + // Finite difference to calculate momentum jacobian + // TODO replace this with analytical gradient + drake::VectorX x_val = drake::VectorX::Zero(n_v_); + drake::VectorX yi(6); + dairlib::multibody::SetVelocitiesIfNew(plant_, x_val, context_); + auto y0 = plant_.CalcSpatialMomentumInWorldAboutPoint(*context_, com_pos) + .translational(); + const double eps = 1e-7; + for (int i = 0; i < n_v_; i++) { + x_val(i) += eps; + dairlib::multibody::SetVelocitiesIfNew(plant_, x_val, context_); + x_val(i) -= eps; + A.col(i).tail(3) = + (plant_.CalcSpatialMomentumInWorldAboutPoint(*context_, com_pos) + .translational() - + y0) / + eps; + } + + // solve + drake::VectorX rv(n_v_); + // Set base angular velocity to zero + rv.head(3) = drake::VectorX::Zero(3); + // Solve the linear least squares for other velocities + rv.tail(n_v_ - 3) = A.rightCols(n_v_ - 3) + .bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV) + .solve(b); + return rv; +} + +drake::VectorX SlipLifter::LiftContactPos( + const drake::VectorX& generalized_position) const { + dairlib::multibody::SetPositionsIfNew(plant_, generalized_position, + context_); + drake::VectorX rv(complex_contact_points_.size() * 3); + for (int i = 0; i < complex_contact_points_.size(); i++) { + rv.segment(3 * i, 3) = complex_contact_points_[i].EvalFull(*context_); + } + return rv; +} + +drake::VectorX SlipLifter::LiftContactVel( + const drake::VectorX& generalized_pos, + const drake::VectorX& generalized_vel) const { + dairlib::multibody::SetPositionsIfNew(plant_, generalized_pos, + context_); + dairlib::multibody::SetVelocitiesIfNew(plant_, generalized_vel, + context_); + drake::VectorX rv(complex_contact_points_.size() * 3); + for (int i = 0; i < complex_contact_points_.size(); i++) { + rv.segment(3 * i, 3) = + complex_contact_points_[i].EvalFullTimeDerivative(*context_); + } + return rv; +} + +drake::VectorX SlipLifter::LiftGrf( + const drake::VectorX& com_pos, + const drake::VectorX& com_vel, + const drake::VectorX& slip_feet_pos, + const drake::VectorX& slip_force, + const drake::VectorX& complex_contact_point_pos) const { + drake::VectorX rv(complex_contact_points_.size() * 3); + // Loop through the slip feet + for (int simple_index = 0; simple_index < slip_contact_points_.size(); + simple_index++) { + // Calculate the slip grf + double r = (com_pos - slip_feet_pos.segment(simple_index * 3, 3)).norm(); + double dr = (com_pos - slip_feet_pos.segment(simple_index * 3, 3)) + .normalized() + .dot(com_vel); + double slip_grf_mag = + slip_contact_mask_[simple_index] + ? CalcSlipGrf(k_, r0_, b_, r, dr, slip_force[simple_index]) + : 0; + // Find the average location for all of the complex contact points that make + // up the SLIP foot + drake::Vector3 average_pos = drake::VectorX::Zero(3); + + const auto& complex_feet_list = + simple_foot_index_to_complex_foot_index_.at(simple_index); + for (const auto& complex_index : complex_feet_list) { + average_pos = + average_pos + complex_contact_point_pos.segment(3 * complex_index, 3); + } + average_pos = average_pos / complex_feet_list.size(); + + // Direction of all the grf for this slip foot must be parallel to not + // create internal forces direction is the vector from average contact point + // to com, so no moment from sum of grf + const auto dir = (com_pos - average_pos).normalized(); + // Distribute grf magnitude across all of the complex contact points + for (const auto& complex_index : complex_feet_list) { + rv.segment(3 * complex_index, 3) = + dir * slip_grf_mag / complex_feet_list.size(); + } + } + return rv; +} + +/// Input is of the form: +/// slip_com +/// slip_velocity +/// slip_contact_pos +/// slip_contact_vel +/// slip_force +/// Output is of the form: +/// complex_com +/// complex_ang_momentum +/// complex_lin_momentum +/// complex_contact_pos +/// complex_contact_vel +/// complex_contact_force +/// complex_gen_pos +/// complex_gen_vel +void SlipLifter::Lift( + const Eigen::Ref>& slip_state, + drake::VectorX* complex_state) const { + const auto& slip_com = slip_state.head(kSLIP_DIM); + const auto& slip_vel = slip_state.segment(kSLIP_DIM, kSLIP_DIM); + const auto& slip_contact_pos = slip_state.segment( + kSLIP_DIM + kSLIP_DIM, kSLIP_DIM * slip_contact_points_.size()); + const auto& slip_contact_vel = slip_state.segment( + kSLIP_DIM + kSLIP_DIM + kSLIP_DIM * slip_contact_points_.size(), + kSLIP_DIM * slip_contact_points_.size()); + const auto& slip_force = slip_state.segment( + kSLIP_DIM + kSLIP_DIM + kSLIP_DIM * slip_contact_points_.size() + + kSLIP_DIM * slip_contact_points_.size(), + slip_contact_points_.size()); + + const drake::Vector3 lin_mom = slip_vel * m_; + const auto& generalized_pos = + LiftGeneralizedPosition(slip_com, slip_contact_pos); + const auto& generalized_vel = LiftGeneralizedVelocity( + generalized_pos, lin_mom, slip_com, slip_contact_vel); + + dairlib::multibody::SetPositionsIfNew(plant_, generalized_pos, + context_); + dairlib::multibody::SetVelocitiesIfNew(plant_, generalized_vel, + context_); + const auto& complex_contact_pos = LiftContactPos(generalized_pos); + + (*complex_state) << slip_com, + plant_.CalcSpatialMomentumInWorldAboutPoint(*context_, slip_com) + .get_coeffs(), + complex_contact_pos, LiftContactVel(generalized_pos, generalized_vel), + LiftGrf(slip_com, slip_vel, slip_contact_pos, slip_force, + complex_contact_pos), + generalized_pos, generalized_vel; +} +drake::VectorX SlipLifter::Lift( + const Eigen::Ref>& slip_state) const { + drake::VectorX complex_state( + 6 + 3 + 3 * 3 * complex_contact_points_.size() + n_q_ + n_v_); + Lift(slip_state, &complex_state); + return complex_state; +} diff --git a/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_lifter.h b/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_lifter.h new file mode 100644 index 0000000000..e5daaddbb8 --- /dev/null +++ b/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_lifter.h @@ -0,0 +1,150 @@ +#pragma once +#include +#include +#include + +#include "multibody/kinematic/kinematic_evaluator_set.h" +#include "multibody/kinematic/world_point_evaluator.h" +#include "solvers/nonlinear_constraint.h" + +#include "drake/common/drake_copyable.h" +#include "drake/common/symbolic.h" +#include "drake/solvers/constraint.h" +#include "drake/systems/trajectory_optimization/multiple_shooting.h" + +/*! + * @brief This class lifts the slip state and control to an equivalent kinematic + * centroidal state and control. It assumes that there is zero orientation and + * angular momentum + * + * @note Currently this class assumes the slip contact points or [ltoe, rtoe] + * and the complex contact points are [ltoe, lheel, rtoe, rheel] + */ +class SlipLifter { + public: + /*! + * @brief Constructor + * @param plant + * @param context + * @param slip_contact_points vector of world point evaluators for slip + * contact points + * @param complex_contact_points vector of world point evaluators for complex + * contact points + * @param simple_foot_index_to_complex_foot_index map from slip contact point + * index to vector of complex contact points on the same feet + * @param nominal_stand + * @param k + * @param b + * @param r0 + * @param contact_mask vector of booleans describing which slip contact points + * are in stance + */ + SlipLifter(const drake::multibody::MultibodyPlant& plant, + drake::systems::Context* context, + const std::vector>& + slip_contact_points, + const std::vector>& + complex_contact_points, + const std::map>& + simple_foot_index_to_complex_foot_index, + const drake::VectorX& nominal_stand, double k, double b, + double r0, const std::vector& contact_mask); + + /// Input is of the form: + /// slip_com + /// slip_velocity + /// slip_contact_pos + /// slip_contact_vel + /// slip_force + /// Output is of the form: + /// complex_com + /// complex_ang_momentum + /// complex_lin_momentum + /// complex_contact_pos + /// complex_contact_vel + /// complex_contact_force + /// complex_gen_pos + /// complex_gen_vel + void Lift(const Eigen::Ref>& slip_state, + drake::VectorX* complex_state) const; + + drake::VectorX Lift( + const Eigen::Ref>& slip_state) const; + + private: + /*! + * @brief given the center of mass position and slip feet position solve for + * the generalized position of the full robot + * @note assumes identity orientation + * @note implements numerical ik (can be slow) + * @param com_position center of mass position in the world + * @param slip_feet_positions [3*n_slip_feet] locations of the slip + * feet + * @return the generalized positions + */ + drake::VectorX LiftGeneralizedPosition( + const drake::Vector3& com_position, + const drake::VectorX& slip_feet_positions) const; + + /*! + * @brief Given a generalized position calculate the generalized velocity that + * is the least squares solution to tracking the linear momentum and foot + * velocity with 0 floating base angular velocity. + * @note Jacobian for momentum is calculated numerically + * @param generalized_pos + * @param linear_momentum + * @param com_pos + * @param slip_feet_velocities [2*n_slip_feet] the foot velocity + * @return generalized velocities + */ + drake::VectorX LiftGeneralizedVelocity( + const drake::VectorX& generalized_pos, + const drake::Vector3& linear_momentum, + const drake::Vector3& com_pos, + const drake::VectorX& slip_feet_velocities) const; + + drake::VectorX LiftContactPos( + const drake::VectorX& generalized_position) const; + + drake::VectorX LiftContactVel( + const drake::VectorX& generalized_pos, + const drake::VectorX& generalized_vel) const; + + /*! + * @brief Lifts the grf by evenly distributing the slip grf into the + * equivalent toe and heel such that the toe and heel grf are equal and + * parallel + * @param com_pos + * @param com_vel + * @param slip_feet_pos + * @param slip_force + * @param complex_contact_point_pos + * @return + */ + drake::VectorX LiftGrf( + const drake::VectorX& com_pos, + const drake::VectorX& com_vel, + const drake::VectorX& slip_feet_pos, + const drake::VectorX& slip_force, + const drake::VectorX& complex_contact_point_pos) const; + + const drake::multibody::MultibodyPlant& plant_; + mutable drake::systems::Context* context_; + mutable drake::multibody::InverseKinematics ik_; + const double m_; + const double k_; + const double b_; + const double r0_; + const std::vector> + slip_contact_points_; + const std::vector> + complex_contact_points_; + const std::map> + simple_foot_index_to_complex_foot_index_; + const int n_q_; + const int n_v_; + const std::vector slip_contact_mask_; + drake::solvers::VectorXDecisionVariable com_vars_; + + const int kSLIP_DIM = 3; +}; diff --git a/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_lifting_test.cc b/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_lifting_test.cc new file mode 100644 index 0000000000..2331787c68 --- /dev/null +++ b/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_lifting_test.cc @@ -0,0 +1,160 @@ +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "common/find_resource.h" +#include "examples/Cassie/cassie_utils.h" +#include "examples/Cassie/kinematic_centroidal_planner/cassie_kc_utils.h" +#include "examples/Cassie/kinematic_centroidal_planner/simple_models/slip_constraints.h" +#include "examples/Cassie/kinematic_centroidal_planner/simple_models/slip_lifter.h" +#include "examples/Cassie/kinematic_centroidal_planner/simple_models/slip_reducer.h" +#include "multibody/visualization_utils.h" +#include "systems/primitives/subvector_pass_through.h" + +using drake::geometry::DrakeVisualizer; +using drake::geometry::SceneGraph; +using drake::multibody::MultibodyPlant; +using drake::multibody::Parser; + +int main(int argc, char* argv[]) { + // Create fix-spring Cassie MBP + drake::systems::DiagramBuilder builder; + SceneGraph& scene_graph = *builder.AddSystem(); + scene_graph.set_name("scene_graph"); + + MultibodyPlant plant(0.0); + MultibodyPlant plant_vis(0.0); + + Parser parser(&plant); + Parser parser_vis(&plant_vis, &scene_graph); + + std::string full_name = dairlib::FindResourceOrThrow( + "examples/Cassie/urdf/cassie_fixed_springs.urdf"); + parser.AddModelFromFile(full_name); + parser_vis.AddModelFromFile(full_name); + plant.Finalize(); + plant_vis.Finalize(); + Eigen::VectorXd reference_state = + GenerateNominalStand(plant, 0.8, 0.2, false); + + auto context = plant.CreateDefaultContext(); + + auto left_toe_pair = dairlib::LeftToeFront(plant); + auto left_heel_pair = dairlib::LeftToeRear(plant); + auto right_toe_pair = dairlib::RightToeFront(plant); + auto right_heel_pair = dairlib::RightToeRear(plant); + + std::vector active_inds{0, 1, 2}; + + auto left_toe_eval = dairlib::multibody::WorldPointEvaluator( + plant, left_toe_pair.first, left_toe_pair.second, + Eigen::Matrix3d::Identity(), Eigen::Vector3d::Zero(), active_inds); + + auto left_heel_eval = dairlib::multibody::WorldPointEvaluator( + plant, left_heel_pair.first, left_heel_pair.second, + Eigen::Matrix3d::Identity(), Eigen::Vector3d::Zero(), active_inds); + + auto right_toe_eval = dairlib::multibody::WorldPointEvaluator( + plant, right_toe_pair.first, right_toe_pair.second, + Eigen::Matrix3d::Identity(), Eigen::Vector3d::Zero(), active_inds); + + auto right_heel_eval = dairlib::multibody::WorldPointEvaluator( + plant, right_heel_pair.first, right_heel_pair.second, + Eigen::Matrix3d::Identity(), Eigen::Vector3d::Zero(), active_inds); + + dairlib::multibody::SetPositionsAndVelocitiesIfNew( + plant, reference_state, context.get()); + + std::vector contact_mask = {true, true}; + double r0 = 0.8; + double k = 2000; + double b = 20; + SlipLifter lifter( + plant, context.get(), {left_toe_eval, right_toe_eval}, + {left_toe_eval, left_heel_eval, right_toe_eval, right_heel_eval}, + {{0, {0, 1}}, {1, {2, 3}}}, reference_state.head(plant.num_positions()), + k, b, r0, contact_mask); + SlipReducer reducer( + plant, context.get(), {left_toe_eval, right_toe_eval}, + {left_toe_eval, left_heel_eval, right_toe_eval, right_heel_eval}, + {{0, {0, 1}}, {1, {2, 3}}}, k, b, r0, contact_mask); + + Eigen::Vector3d slip_com = {0.2, 0, 0.7}; + Eigen::Vector3d slip_vel = {0.1, 0, 0.1}; + Eigen::VectorXd slip_feet(6); + slip_feet << 0.0, 0.2, 0.0, 0.0, -0.2, 0.0; + Eigen::VectorXd slip_foot_vel(6); + slip_foot_vel << 0.11, 0.12, 0.0, 0.15, 0.18, 0.0; + Eigen::Vector2d slip_force = {0.5, 0.3}; + + Eigen::VectorXd slip_state(3 + 3 + 6 + 6 + 2); + slip_state << slip_com, slip_vel, slip_feet, slip_foot_vel, slip_force; + Eigen::VectorXd complex_state(6 + 3 + 3 * 3 * 4 + plant.num_positions() + + plant.num_velocities()); + + auto start = std::chrono::high_resolution_clock::now(); + lifter.Lift(slip_state, &complex_state); + auto finish = std::chrono::high_resolution_clock::now(); + std::chrono::duration elapsed = finish - start; + std::cout << "Solve time:" << elapsed.count() << std::endl; + + start = std::chrono::high_resolution_clock::now(); + lifter.Lift(slip_state, &complex_state); + finish = std::chrono::high_resolution_clock::now(); + elapsed = finish - start; + std::cout << "Solve time 2:" << elapsed.count() << std::endl; + auto constraint = SlipReductionConstraint( + plant, std::make_shared(reducer), 2, 4, 0); + + drake::VectorX error(slip_state.size()); + drake::VectorX input(slip_state.size() + complex_state.size()); + input << slip_state, complex_state; + constraint.DoEval(input, &error); + std::cout << "Max Error in inverse test: " << error.cwiseAbs().maxCoeff() + << std::endl; + + auto grf_constraint = SlipGrfReductionConstrain( + plant, std::make_shared(reducer), 2, 4, 0); + drake::VectorX grf_error(2); + drake::VectorX grf_input(3 + 3 * 2 + 3 * 4 + 2 + 3); + grf_input << slip_com, slip_vel, slip_feet, + complex_state.segment(3 + 6 + 12 + 12, 12), slip_force; + grf_constraint.DoEval(grf_input, &grf_error); + std::cout << "Max Error in grf inverse test: " + << grf_error.cwiseAbs().maxCoeff() << std::endl; + if (true) { + // Build temporary diagram for visualization + drake::systems::DiagramBuilder builder_ik; + drake::geometry::SceneGraph& scene_graph_ik = + *builder_ik.AddSystem(); + scene_graph_ik.set_name("scene_graph_ik"); + MultibodyPlant plant_ik(0.0); + Parser parser(&plant_ik, &scene_graph_ik); + std::string full_name = dairlib::FindResourceOrThrow( + "examples/Cassie/urdf/cassie_fixed_springs.urdf"); + parser.AddModelFromFile(full_name); + plant_ik.Finalize(); + + // Visualize + const auto& x_const = + complex_state.tail(plant.num_positions() + plant.num_velocities()); + drake::trajectories::PiecewisePolynomial pp_xtraj(x_const); + + dairlib::multibody::ConnectTrajectoryVisualizer(&plant_ik, &builder_ik, + &scene_graph_ik, pp_xtraj); + auto diagram = builder_ik.Build(); + drake::systems::Simulator simulator(*diagram); + simulator.set_target_realtime_rate(.1); + simulator.Initialize(); + simulator.AdvanceTo(0.1); + } +} diff --git a/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_reducer.cc b/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_reducer.cc new file mode 100644 index 0000000000..284cbaf002 --- /dev/null +++ b/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_reducer.cc @@ -0,0 +1,109 @@ +#include "slip_reducer.h" + +#include + +#include "examples/Cassie/kinematic_centroidal_planner/simple_models/slip_utils.h" +#include "multibody/multibody_utils.h" + +SlipReducer::SlipReducer( + const drake::multibody::MultibodyPlant& plant, + drake::systems::Context* context, + const std::vector>& + slip_contact_points, + const std::vector>& + complex_contact_points, + const std::map>& + simple_foot_index_to_complex_foot_index, + double k, double b, double r0, const std::vector& contact_mask) + : plant_(plant), + context_(context), + k_(k), + b_(b), + r0_(r0), + m_(plant.CalcTotalMass(*context)), + slip_contact_points_(slip_contact_points), + complex_contact_points_(complex_contact_points), + simple_foot_index_to_complex_foot_index_( + simple_foot_index_to_complex_foot_index), + n_q_(plant.num_positions()), + n_v_(plant.num_velocities()), + slip_contact_mask_(contact_mask) {} +/// Input is of the form: +/// complex_com +/// complex_ang_momentum +/// complex_lin_momentum +/// complex_contact_pos +/// complex_contact_vel +/// complex_contact_force +/// complex_gen_pos +/// complex_gen_vel +/// Output is of the form: +/// slip_com +/// slip_velocity +/// slip_contact_pos +/// slip_contact_vel +/// slip_force +void SlipReducer::Reduce( + const Eigen::Ref>& complex_state, + drake::VectorX* slip_state) const { + const auto& complex_com = complex_state.segment(0, 3); + const auto& complex_ang_momentum = complex_state.segment(3, 3); + const auto& complex_lin_momentum = complex_state.segment(3 + 3, 3); + const auto& complex_grf = + complex_state.segment(3 + 3 + 3 + 2 * 3 * complex_contact_points_.size(), + 3 * complex_contact_points_.size()); + const auto& complex_gen_state = complex_state.tail(n_q_ + n_v_); + + dairlib::multibody::SetPositionsAndVelocitiesIfNew( + plant_, complex_gen_state, context_); + slip_state->head(kSLIP_DIM) = complex_com; + slip_state->segment(kSLIP_DIM, kSLIP_DIM) = complex_lin_momentum / m_; + for (int i = 0; i < slip_contact_points_.size(); i++) { + slip_state->segment(kSLIP_DIM + kSLIP_DIM + kSLIP_DIM * i, kSLIP_DIM) = + slip_contact_points_[i].EvalFull(*context_); + slip_state->segment( + kSLIP_DIM + kSLIP_DIM + kSLIP_DIM * slip_contact_points_.size() + + kSLIP_DIM * i, + kSLIP_DIM) = slip_contact_points_[i].EvalFullTimeDerivative(*context_); + } + slip_state->tail(slip_contact_points_.size()) = + ReduceGrf(complex_com, complex_lin_momentum / m_, + slip_state->segment(kSLIP_DIM + kSLIP_DIM, + slip_contact_points_.size() * kSLIP_DIM), + complex_grf); +} + +drake::VectorX SlipReducer::Reduce( + const Eigen::Ref>& complex_state) const { + drake::VectorX slip_state( + kSLIP_DIM + kSLIP_DIM + 2 * kSLIP_DIM * slip_contact_points_.size() + + slip_contact_points_.size()); + Reduce(complex_state, &slip_state); + return slip_state; +} + +drake::VectorX SlipReducer::ReduceGrf( + const Eigen::Ref>& complex_com, + const Eigen::Ref>& com_vel, + const Eigen::Ref>& slip_contact_pos, + const Eigen::Ref>& complex_grf) const { + DRAKE_DEMAND(complex_com.size() == 3); + DRAKE_DEMAND(slip_contact_pos.size() == 3 * slip_contact_points_.size()); + Eigen::VectorXd slip_force(slip_contact_points_.size()); + for (int i = 0; i < slip_contact_points_.size(); i++) { + const double r = (complex_com - slip_contact_pos.segment(3 * i, 3)).norm(); + const Eigen::VectorXd unit_vec = + (complex_com - slip_contact_pos.segment(3 * i, 3)).normalized(); + const double dr = unit_vec.dot(com_vel); + const auto spring_force = CalcSlipGrf(k_, r0_, b_, r, dr, 0); + auto complex_feet_it = simple_foot_index_to_complex_foot_index_.find(i); + Eigen::Vector3d complex_force = Eigen::Vector3d::Zero(3); + for (const auto complex_index : complex_feet_it->second) { + complex_force = complex_force + complex_grf.segment(3 * complex_index, 3); + } + const double mag = complex_force.dot(unit_vec) > 0 ? complex_force.norm() + : -complex_force.norm(); + slip_force.coeffRef(i) = slip_contact_mask_[i] ? mag - spring_force : 0; + } + return slip_force; +} diff --git a/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_reducer.h b/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_reducer.h new file mode 100644 index 0000000000..7bcec007ac --- /dev/null +++ b/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_reducer.h @@ -0,0 +1,57 @@ +#pragma once +#include +#include +#include + +#include "multibody/kinematic/kinematic_evaluator_set.h" +#include "multibody/kinematic/world_point_evaluator.h" +#include "solvers/nonlinear_constraint.h" + +#include "drake/common/drake_copyable.h" +#include "drake/common/symbolic.h" +#include "drake/solvers/constraint.h" +#include "drake/systems/trajectory_optimization/multiple_shooting.h" + +class SlipReducer { + public: + SlipReducer( + const drake::multibody::MultibodyPlant& plant, + drake::systems::Context* context, + const std::vector>& + slip_contact_points, + const std::vector>& + complex_contact_points, + const std::map>& + simple_foot_index_to_complex_foot_index, + double k, double b, double r0, const std::vector& contact_mask); + + void Reduce(const Eigen::Ref>& complex_state, + drake::VectorX* slip_state) const; + + drake::VectorX Reduce( + const Eigen::Ref>& complex_state) const; + + drake::VectorX ReduceGrf( + const Eigen::Ref>& complex_com, + const Eigen::Ref>& com_vel, + const Eigen::Ref>& slip_contact_pos, + const Eigen::Ref>& complex_grf) const; + + private: + const drake::multibody::MultibodyPlant& plant_; + mutable drake::systems::Context* context_; + const double k_; + const double b_; + const double r0_; + const double m_; + const std::vector> + slip_contact_points_; + const std::vector> + complex_contact_points_; + const std::map> + simple_foot_index_to_complex_foot_index_; + const int n_q_; + const int n_v_; + const std::vector slip_contact_mask_; + const int kSLIP_DIM = 3; +}; diff --git a/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_utils.cc b/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_utils.cc new file mode 100644 index 0000000000..4334c61dcb --- /dev/null +++ b/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_utils.cc @@ -0,0 +1,20 @@ +#include "examples/Cassie/kinematic_centroidal_planner/simple_models/slip_utils.h" + +#include + +#include "multibody/multibody_utils.h" + +template +T CalcSlipGrf(double k, double r0, double b, T r, T dr, T force) { + auto F = force + k * (r0 - r) + b * (0 - dr); + if (F < 0) { + F = 0; + } + return F; +} + +template double CalcSlipGrf(double k, double r0, double b, double r, + double dr, double force); +template drake::AutoDiffXd CalcSlipGrf( + double k, double r0, double b, drake::AutoDiffXd r, drake::AutoDiffXd dr, + drake::AutoDiffXd force); diff --git a/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_utils.h b/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_utils.h new file mode 100644 index 0000000000..8cbdbe1183 --- /dev/null +++ b/examples/Cassie/kinematic_centroidal_planner/simple_models/slip_utils.h @@ -0,0 +1,4 @@ +#pragma once + +template +T CalcSlipGrf(double k, double r0, double b, T r, T dr, T force); diff --git a/examples/Cassie/kinematic_centroidal_planner/trajectory_parameters.h b/examples/Cassie/kinematic_centroidal_planner/trajectory_parameters.h index 7e76be44fe..448b37bcdf 100644 --- a/examples/Cassie/kinematic_centroidal_planner/trajectory_parameters.h +++ b/examples/Cassie/kinematic_centroidal_planner/trajectory_parameters.h @@ -9,6 +9,9 @@ struct TrajectoryParameters { double target_com_height; double stance_width; double swing_foot_minimum_height; + double max_slip_leg_length; + double spring_constant; + double damping_coefficient; std::vector duration_scaling; std::vector gait_sequence; @@ -16,15 +19,21 @@ struct TrajectoryParameters { std::vector com_vel_vector; + std::vector complexity_schedule; + template void Serialize(Archive* a) { a->Visit(DRAKE_NVP(n_knot_points)); a->Visit(DRAKE_NVP(target_com_height)); a->Visit(DRAKE_NVP(stance_width)); a->Visit(DRAKE_NVP(swing_foot_minimum_height)); + a->Visit(DRAKE_NVP(max_slip_leg_length)); + a->Visit(DRAKE_NVP(spring_constant)); + a->Visit(DRAKE_NVP(damping_coefficient)); a->Visit(DRAKE_NVP(duration_scaling)); a->Visit(DRAKE_NVP(gait_sequence)); a->Visit(DRAKE_NVP(com_vel_values)); + a->Visit(DRAKE_NVP(complexity_schedule)); Eigen::Map input(com_vel_values.data(), 3, com_vel_values.size() / 3); for (int i = 0; i < input.cols(); ++i) { @@ -32,4 +41,4 @@ struct TrajectoryParameters { } DRAKE_DEMAND(duration_scaling.size() == gait_sequence.size()); } -}; \ No newline at end of file +}; diff --git a/examples/Cassie/systems/cassie_out_to_radio.cc b/examples/Cassie/systems/cassie_out_to_radio.cc index 94e8c6d026..58d8eb1d9d 100644 --- a/examples/Cassie/systems/cassie_out_to_radio.cc +++ b/examples/Cassie/systems/cassie_out_to_radio.cc @@ -26,4 +26,4 @@ void CassieOutToRadio::CalcRadioOut( } } // namespace systems -} // namespace dairlib \ No newline at end of file +} // namespace dairlib diff --git a/examples/Cassie/systems/cassie_out_to_radio.h b/examples/Cassie/systems/cassie_out_to_radio.h index ab38c307e7..77277b392a 100644 --- a/examples/Cassie/systems/cassie_out_to_radio.h +++ b/examples/Cassie/systems/cassie_out_to_radio.h @@ -26,4 +26,4 @@ class CassieOutToRadio : public drake::systems::LeafSystem { int radio_output_port_; }; } // namespace systems -} // namespace dairlib \ No newline at end of file +} // namespace dairlib diff --git a/multibody/kinematic/world_point_evaluator.h b/multibody/kinematic/world_point_evaluator.h index 420aaf22da..58f7cb3b59 100644 --- a/multibody/kinematic/world_point_evaluator.h +++ b/multibody/kinematic/world_point_evaluator.h @@ -93,6 +93,9 @@ class WorldPointEvaluator : public KinematicEvaluator { /// evaluator's output. void set_frictional() { is_frictional_ = true; }; + const drake::multibody::Frame& get_frame() const { return frame_A_; }; + const Eigen::Vector3d& get_pt_A() const { return pt_A_; }; + private: const Eigen::Vector3d pt_A_; const drake::multibody::Frame& frame_A_; diff --git a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc index 374ea4f982..68f5716393 100644 --- a/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc +++ b/systems/controllers/kinematic_centroidal_mpc/kinematic_centroidal_mpc.cc @@ -2,17 +2,15 @@ #include -//#include "multibody/multibody_utils.h" #include "common/eigen_utils.h" +#include "examples/Cassie/kinematic_centroidal_planner/cassie_kc_utils.h" #include "examples/Cassie/kinematic_centroidal_planner/cassie_kinematic_centroidal_solver.h" -#include "examples/Cassie/kinematic_centroidal_planner/cassie_reference_utils.h" #include "systems/framework/output_vector.h" #include "systems/trajectory_optimization/kinematic_centroidal_planner/gait.h" #include "systems/trajectory_optimization/kinematic_centroidal_planner/kcmpc_reference_generator.h" #include "drake/common/trajectories/piecewise_polynomial.h" #include "drake/systems/framework/leaf_system.h" -//#include using dairlib::systems::BasicVector; using dairlib::systems::OutputVector; diff --git a/systems/framework/lcm_driven_loop.h b/systems/framework/lcm_driven_loop.h index 2647ea3ee2..8d29b53030 100644 --- a/systems/framework/lcm_driven_loop.h +++ b/systems/framework/lcm_driven_loop.h @@ -327,4 +327,4 @@ class LcmDrivenLoop { }; } // namespace systems -} // namespace dairlib \ No newline at end of file +} // namespace dairlib diff --git a/systems/trajectory_optimization/kinematic_centroidal_planner/kcmpc_reference_generator.cc b/systems/trajectory_optimization/kinematic_centroidal_planner/kcmpc_reference_generator.cc index 68b81f8738..58c4bc71bc 100644 --- a/systems/trajectory_optimization/kinematic_centroidal_planner/kcmpc_reference_generator.cc +++ b/systems/trajectory_optimization/kinematic_centroidal_planner/kcmpc_reference_generator.cc @@ -19,6 +19,8 @@ void KcmpcReferenceGenerator::Generate() { Eigen::Vector3d com = q_ref_.segment(4, 3) - p_ScmBase_W_; com_trajectory_ = GenerateComTrajectory(com, com_vel_knot_points_.samples, com_vel_knot_points_.times); + momentum_trajectory_ = GenerateMomentumTrajectory( + com_vel_knot_points_.samples, com_vel_knot_points_.times, m_); q_trajectory_ = GenerateGeneralizedPosTrajectory(q_ref_, p_ScmBase_W_, com_trajectory_, 4); v_trajectory_ = GenerateGeneralizedVelTrajectory(com_trajectory_, @@ -61,4 +63,4 @@ std::vector KcmpcReferenceGenerator::GenerateTimePoints( time_points.push_back(time_points.back() + duration); } return time_points; -} \ No newline at end of file +} diff --git a/systems/trajectory_optimization/kinematic_centroidal_planner/kcmpc_reference_generator.h b/systems/trajectory_optimization/kinematic_centroidal_planner/kcmpc_reference_generator.h index 7a49a96f0d..421a53247a 100644 --- a/systems/trajectory_optimization/kinematic_centroidal_planner/kcmpc_reference_generator.h +++ b/systems/trajectory_optimization/kinematic_centroidal_planner/kcmpc_reference_generator.h @@ -71,6 +71,7 @@ class KcmpcReferenceGenerator { drake::trajectories::PiecewisePolynomial contact_sequence_; drake::trajectories::PiecewisePolynomial grf_traj_; drake::trajectories::PiecewisePolynomial contact_traj_; + drake::trajectories::PiecewisePolynomial momentum_trajectory_; private: const drake::multibody::MultibodyPlant& plant_; @@ -81,4 +82,4 @@ class KcmpcReferenceGenerator { KnotPoints gait_knot_points_; double m_; Eigen::Vector3d p_ScmBase_W_; -}; \ No newline at end of file +}; diff --git a/systems/trajectory_optimization/kinematic_centroidal_planner/kinematic_centroidal_gains.h b/systems/trajectory_optimization/kinematic_centroidal_planner/kinematic_centroidal_gains.h index 830867b697..1a3464209e 100644 --- a/systems/trajectory_optimization/kinematic_centroidal_planner/kinematic_centroidal_gains.h +++ b/systems/trajectory_optimization/kinematic_centroidal_planner/kinematic_centroidal_gains.h @@ -26,4 +26,4 @@ struct KinematicCentroidalGains { a->Visit(DRAKE_NVP(ang_momentum)); a->Visit(DRAKE_NVP(tol)); } -}; \ No newline at end of file +}; diff --git a/systems/trajectory_optimization/kinematic_centroidal_planner/kinematic_centroidal_solver.cc b/systems/trajectory_optimization/kinematic_centroidal_planner/kinematic_centroidal_solver.cc index b517bcd8f4..08d6a403a3 100644 --- a/systems/trajectory_optimization/kinematic_centroidal_planner/kinematic_centroidal_solver.cc +++ b/systems/trajectory_optimization/kinematic_centroidal_planner/kinematic_centroidal_solver.cc @@ -35,7 +35,8 @@ KinematicCentroidalSolver::KinematicCentroidalSolver( Q_force_( Eigen::MatrixXd::Zero(3 * n_contact_points_, 3 * n_contact_points_)), contact_sequence_(n_knot_points), - contexts_(n_knot_points) { + contexts_(n_knot_points), + complexity_schedule_(n_knot_points) { n_joint_q_ = n_q_ - kCentroidalPosDim; n_joint_v_ = n_v_ - kCentroidalVelDim; prog_ = std::make_unique(); @@ -97,8 +98,8 @@ void KinematicCentroidalSolver::SetMomentumReference( mom_ref_traj_ = std::move(ref_traj); } -void KinematicCentroidalSolver::AddCentroidalDynamics() { - for (int knot_point = 0; knot_point < n_knot_points_ - 1; knot_point++) { +void KinematicCentroidalSolver::AddCentroidalDynamics(int knot_point) { + if (!(is_last_knot(knot_point))) { auto constraint = std::make_shared>( plant_, contexts_[knot_point].get(), n_contact_points_, dt_, knot_point); @@ -111,8 +112,8 @@ void KinematicCentroidalSolver::AddCentroidalDynamics() { } } -void KinematicCentroidalSolver::AddKinematicsIntegrator() { - for (int knot_point = 0; knot_point < n_knot_points_ - 1; knot_point++) { +void KinematicCentroidalSolver::AddKinematicsIntegrator(int knot_point) { + if (!is_last_knot(knot_point)) { // Integrate generalized velocities to get generalized positions auto constraint = std::make_shared>( plant_, contexts_[knot_point].get(), contexts_[knot_point + 1].get(), @@ -135,90 +136,83 @@ void KinematicCentroidalSolver::AddKinematicsIntegrator() { } } -void KinematicCentroidalSolver::AddContactConstraints() { - for (int knot_point = 0; knot_point < n_knot_points_; knot_point++) { - for (int contact_index = 0; contact_index < n_contact_points_; - contact_index++) { - // Make sure feet in stance are not moving and on the ground - if (contact_sequence_[knot_point][contact_index]) { - prog_->AddBoundingBoxConstraint( - Eigen::VectorXd::Zero(3), Eigen::VectorXd::Zero(3), - contact_vel_vars(knot_point, contact_index)); - if (knot_point != 0) { - prog_->AddBoundingBoxConstraint( - 0, 0, contact_pos_vars(knot_point, contact_index)[2]); - } - } else { - // Feet are above the ground - double lb = 0; - // Check if at least one of the time points before or after is also in - // flight before restricting the foot to be in the air to limit over - // constraining the optimization problem - if (!is_first_knot(knot_point) and !is_last_knot(knot_point) and - (!contact_sequence_[knot_point - 1][contact_index] or - !contact_sequence_[knot_point + 1][contact_index])) { - lb = swing_foot_minimum_height_; - } +void KinematicCentroidalSolver::AddContactConstraints(int knot_point) { + for (int contact_index = 0; contact_index < n_contact_points_; + contact_index++) { + // Make sure feet in stance are not moving and on the ground + if (contact_sequence_[knot_point][contact_index]) { + prog_->AddBoundingBoxConstraint( + Eigen::VectorXd::Zero(3), Eigen::VectorXd::Zero(3), + contact_vel_vars(knot_point, contact_index)); + if (knot_point != 0) { prog_->AddBoundingBoxConstraint( - lb, 10, contact_pos_vars(knot_point, contact_index)[2]); + 0, 0, contact_pos_vars(knot_point, contact_index)[2]); } + } else { + // Feet are above the ground + double lb = 0; + // Check if at least one of the time points before or after is also in + // flight before restricting the foot to be in the air to limit over + // constraining the optimization problem + if (!is_first_knot(knot_point) and !is_last_knot(knot_point) and + (!contact_sequence_[knot_point - 1][contact_index] or + !contact_sequence_[knot_point + 1][contact_index])) { + lb = swing_foot_minimum_height_; + } + prog_->AddBoundingBoxConstraint( + lb, 10, contact_pos_vars(knot_point, contact_index)[2]); } } } -void KinematicCentroidalSolver::AddCentroidalKinematicConsistency() { - for (int knot_point = 0; knot_point < n_knot_points_; knot_point++) { - // Ensure linear and angular momentum line up - auto centroidal_momentum = - std::make_shared>( - plant_, contexts_[knot_point].get(), knot_point); - prog_->AddConstraint(centroidal_momentum, - {state_vars(knot_point), com_pos_vars(knot_point), - momentum_vars(knot_point)}); - for (int contact_index = 0; contact_index < n_contact_points_; - contact_index++) { - // Ensure foot position line up with kinematics - auto foot_position_constraint = std::make_shared< - dairlib::multibody::KinematicPositionConstraint>( - plant_, contact_sets_[contact_index], Eigen::Vector3d::Zero(), - Eigen::Vector3d::Zero(), full_constraint_relative_); - prog_->AddConstraint(foot_position_constraint, - {state_vars(knot_point).head(n_q_), - contact_pos_vars(knot_point, contact_index)}); - } - // Constrain com position - auto com_position = - std::make_shared>( - plant_, contexts_[knot_point].get(), knot_point); - prog_->AddConstraint(com_position, - {com_pos_vars(knot_point), state_vars(knot_point)}); +void KinematicCentroidalSolver::AddCentroidalKinematicConsistency( + int knot_point) { + // Ensure linear and angular momentum line up + auto centroidal_momentum = + std::make_shared>( + plant_, contexts_[knot_point].get(), knot_point); + prog_->AddConstraint(centroidal_momentum, + {state_vars(knot_point), com_pos_vars(knot_point), + momentum_vars(knot_point)}); + for (int contact_index = 0; contact_index < n_contact_points_; + contact_index++) { + // Ensure foot position line up with kinematics + auto foot_position_constraint = std::make_shared< + dairlib::multibody::KinematicPositionConstraint>( + plant_, contact_sets_[contact_index], Eigen::Vector3d::Zero(), + Eigen::Vector3d::Zero(), full_constraint_relative_); + prog_->AddConstraint(foot_position_constraint, + {state_vars(knot_point).head(n_q_), + contact_pos_vars(knot_point, contact_index)}); } + // Constrain com position + auto com_position = std::make_shared>( + plant_, contexts_[knot_point].get(), knot_point); + prog_->AddConstraint(com_position, + {com_pos_vars(knot_point), state_vars(knot_point)}); } -void KinematicCentroidalSolver::AddFrictionConeConstraints() { - for (int knot_point = 0; knot_point < n_knot_points_; knot_point++) { - for (int contact_index = 0; contact_index < n_contact_points_; - contact_index++) { - auto force_constraints_vec = - contact_points_[contact_index].CreateLinearFrictionConstraints(); - for (const auto& constraint : force_constraints_vec) { - prog_->AddConstraint(constraint, - contact_force_vars(knot_point, contact_index)); - } +void KinematicCentroidalSolver::AddFrictionConeConstraints(int knot_point) { + for (int contact_index = 0; contact_index < n_contact_points_; + contact_index++) { + auto force_constraints_vec = + contact_points_[contact_index].CreateLinearFrictionConstraints(); + for (const auto& constraint : force_constraints_vec) { + prog_->AddConstraint(constraint, + contact_force_vars(knot_point, contact_index)); } } } -void KinematicCentroidalSolver::AddFlightContactForceConstraints() { - for (int knot_point = 0; knot_point < n_knot_points_; knot_point++) { - for (int contact_index = 0; contact_index < n_contact_points_; - contact_index++) { - // Feet in flight produce no force - if (!contact_sequence_[knot_point][contact_index]) { - prog_->AddBoundingBoxConstraint( - Eigen::VectorXd::Zero(3), Eigen::VectorXd::Zero(3), - contact_force_vars(knot_point, contact_index)); - } +void KinematicCentroidalSolver::AddFlightContactForceConstraints( + int knot_point) { + for (int contact_index = 0; contact_index < n_contact_points_; + contact_index++) { + // Feet in flight produce no force + if (!contact_sequence_[knot_point][contact_index]) { + prog_->AddBoundingBoxConstraint( + Eigen::VectorXd::Zero(3), Eigen::VectorXd::Zero(3), + contact_force_vars(knot_point, contact_index)); } } } @@ -238,12 +232,41 @@ KinematicCentroidalSolver::joint_vel_vars(int knotpoint_index) const { void KinematicCentroidalSolver::Build( const drake::solvers::SolverOptions& solver_options) { - AddCentroidalDynamics(); - AddKinematicsIntegrator(); - AddContactConstraints(); - AddCentroidalKinematicConsistency(); - AddFrictionConeConstraints(); - AddFlightContactForceConstraints(); + for (int knot_point = 0; knot_point < n_knot_points_; knot_point++) { + switch (complexity_schedule_[knot_point]) { + case KINEMATIC_CENTROIDAL: + // Add complex constraints + AddContactConstraints(knot_point); + AddCentroidalKinematicConsistency(knot_point); + AddFrictionConeConstraints(knot_point); + AddFlightContactForceConstraints(knot_point); + // Add complex dynamics + AddCentroidalDynamics(knot_point); + AddKinematicsIntegrator(knot_point); + if (!is_last_knot(knot_point) and + complexity_schedule_[knot_point + 1] == SLIP) { + AddSlipEqualityConstraint(knot_point + 1); + AddCentroidalKinematicConsistency(knot_point + 1); + } + break; + case SLIP: + AddSlipConstraints(knot_point); + if (!is_last_knot(knot_point)) { + switch (complexity_schedule_[knot_point + 1]) { + case KINEMATIC_CENTROIDAL: + AddCentroidalDynamics(knot_point); + AddKinematicsIntegrator(knot_point); + AddSlipEqualityConstraint(knot_point); + AddCentroidalKinematicConsistency(knot_point); + break; + case SLIP: + AddSlipDynamics(knot_point); + break; + } + } + break; + } + } AddCosts(); prog_->SetSolverOptions(solver_options); } @@ -256,104 +279,93 @@ void KinematicCentroidalSolver::SetConstantMomentumReference( } double KinematicCentroidalSolver::GetKnotpointGain(int knot_point) const { - const double terminal_gain = is_last_knot(knot_point) ? 100 : 1; + const double terminal_gain = is_last_knot(knot_point) ? 1 : 1; const double collocation_gain = (is_first_knot(knot_point) or is_last_knot(knot_point)) ? 0.5 : 1; return terminal_gain * collocation_gain; } void KinematicCentroidalSolver::AddCosts() { + // Loop through knot points and add running cost for (int knot_point = 0; knot_point < n_knot_points_; knot_point++) { - double knot_point_gain = GetKnotpointGain(knot_point); double t = dt_ * knot_point; - if (q_ref_traj_) { - q_ref_cost_.push_back( - prog_ - ->AddQuadraticErrorCost(knot_point_gain * Q_q_, - q_ref_traj_->value(t), - state_vars(knot_point).head(n_q_)) - .evaluator()); - } - if (v_ref_traj_) { - v_ref_cost_.push_back( - prog_ - ->AddQuadraticErrorCost(knot_point_gain * Q_v_, - v_ref_traj_->value(t), - state_vars(knot_point).tail(n_v_)) - .evaluator()); - } - if (com_ref_traj_) { - com_ref_cost_.push_back( - prog_ - ->AddQuadraticErrorCost(knot_point_gain * Q_com_, - com_ref_traj_->value(t), - com_pos_vars(knot_point)) - .evaluator()); - } - if (mom_ref_traj_) { - mom_ref_cost_.push_back( - prog_ - ->AddQuadraticErrorCost(knot_point_gain * Q_mom_, - mom_ref_traj_->value(t), - momentum_vars(knot_point)) - .evaluator()); - } - if (contact_ref_traj_) { - contact_ref_cost_.push_back( - prog_ - ->AddQuadraticErrorCost( - knot_point_gain * Q_contact_, contact_ref_traj_->value(t), - {contact_pos_[knot_point], contact_vel_[knot_point]}) - .evaluator()); - } - if (force_ref_traj_) { - force_ref_cost_.push_back( - prog_ - ->AddQuadraticErrorCost(knot_point_gain * Q_force_, - force_ref_traj_->value(t), - contact_force_[knot_point]) - .evaluator()); + + switch (complexity_schedule_[knot_point]) { + case KINEMATIC_CENTROIDAL: + // If kinematic centroidal always going to integrate to kinematic + // centroidal + AddKinematicCentroidalCosts(knot_point, t, 0.5); + if (!is_last_knot(knot_point)) { + AddKinematicCentroidalCosts(knot_point + 1, t + dt_, 0.5); + } + break; + case SLIP: + // If slip, and integrating to kinematic centroidal use kinematic + // centroidal cost If slip and integrating to slip use slip costs + if (!is_last_knot(knot_point)) { + switch (complexity_schedule_[knot_point + 1]) { + case KINEMATIC_CENTROIDAL: + AddKinematicCentroidalCosts(knot_point, t, 0.5); + AddKinematicCentroidalCosts(knot_point + 1, t + dt_, 0.5); + break; + case SLIP: + AddSlipCost(knot_point, 0.5); + AddSlipCost(knot_point + 1, 0.5); + break; + } + } else { + AddSlipCost(knot_point, 0.5); + } + break; } } + int knot_point = n_knot_points_ - 1; + double knot_point_gain = GetKnotpointGain(knot_point); + double t = dt_ * knot_point; + switch (complexity_schedule_[knot_point]) { + case KINEMATIC_CENTROIDAL: + AddKinematicCentroidalCosts(knot_point, t, knot_point_gain); + break; + case SLIP: + AddSlipCost(knot_point, knot_point_gain); + break; + } } -void KinematicCentroidalSolver::UpdateCosts() { - for (int knot_point = 0; knot_point < n_knot_points_; knot_point++) { - const double knot_point_gain = GetKnotpointGain(knot_point); - double t = dt_ * knot_point; - if (q_ref_cost_[knot_point]) { - q_ref_cost_[knot_point]->UpdateCoefficients( - knot_point_gain * 2 * Q_q_, - -knot_point_gain * 2 * Q_q_ * q_ref_traj_->value(t)); - } - if (v_ref_cost_[knot_point]) { - v_ref_cost_[knot_point]->UpdateCoefficients( - knot_point_gain * 2 * Q_v_, - -knot_point_gain * 2 * Q_v_ * v_ref_traj_->value(t)); - } - if (com_ref_cost_[knot_point]) { - com_ref_cost_[knot_point]->UpdateCoefficients( - knot_point_gain * 2 * Q_com_, - -knot_point_gain * 2 * Q_com_ * com_ref_traj_->value(t)); - } - if (mom_ref_cost_[knot_point]) { - mom_ref_cost_[knot_point]->UpdateCoefficients( - knot_point_gain * 2 * Q_mom_, - -knot_point_gain * 2 * Q_mom_ * mom_ref_traj_->value(t)); - } - if (contact_ref_cost_[knot_point]) { - contact_ref_cost_[knot_point]->UpdateCoefficients( - knot_point_gain * 2 * Q_contact_, - -knot_point_gain * 2 * Q_contact_ * contact_ref_traj_->value(t)); - } - if (force_ref_cost_[knot_point]) { - force_ref_cost_[knot_point]->UpdateCoefficients( - knot_point_gain * 2 * Q_force_, - -knot_point_gain * 2 * Q_force_ * force_ref_traj_->value(t)); - } +void KinematicCentroidalSolver::AddKinematicCentroidalCosts( + int knot_point, double t, double knot_point_gain) { + if (q_ref_traj_) { + prog_->AddQuadraticErrorCost(knot_point_gain * Q_q_, q_ref_traj_->value(t), + state_vars(knot_point).head(n_q_)); + } + if (v_ref_traj_) { + prog_->AddQuadraticErrorCost(knot_point_gain * Q_v_, v_ref_traj_->value(t), + state_vars(knot_point).tail(n_v_)); + } + if (com_ref_traj_) { + prog_->AddQuadraticErrorCost(knot_point_gain * Q_com_, + com_ref_traj_->value(t), + com_pos_vars(knot_point)); + } + if (mom_ref_traj_) { + prog_->AddQuadraticErrorCost(knot_point_gain * Q_mom_, + mom_ref_traj_->value(t), + momentum_vars(knot_point)); + } + if (contact_ref_traj_) { + prog_->AddQuadraticErrorCost( + knot_point_gain * Q_contact_, contact_ref_traj_->value(t), + {contact_pos_[knot_point], contact_vel_[knot_point]}); + } + if (force_ref_traj_) { + prog_->AddQuadraticErrorCost(knot_point_gain * Q_force_, + force_ref_traj_->value(t), + contact_force_[knot_point]); } } +void KinematicCentroidalSolver::UpdateCosts() { DRAKE_DEMAND(false); } + void KinematicCentroidalSolver::SetZeroInitialGuess() { Eigen::VectorXd initialGuess = Eigen::VectorXd::Zero(n_q_ + n_v_ + n_contact_points_ * 9 + 6 + 3); @@ -387,7 +399,14 @@ KinematicCentroidalSolver::Solve() { std::vector> states; for (int knot_point = 0; knot_point < n_knot_points_; knot_point++) { time_points.emplace_back(dt_ * knot_point); - states.emplace_back(result_->GetSolution(state_vars(knot_point))); + switch (complexity_schedule_[knot_point]) { + case KINEMATIC_CENTROIDAL: + states.emplace_back(result_->GetSolution(state_vars(knot_point))); + break; + case SLIP: + states.emplace_back(LiftSlipSolution(knot_point)); + break; + } } return drake::trajectories::PiecewisePolynomial::FirstOrderHold( time_points, states); @@ -533,7 +552,6 @@ void KinematicCentroidalSolver::AddPlantJointLimits( } } } - drake::solvers::VectorXDecisionVariable KinematicCentroidalSolver::com_pos_vars( int knotpoint_index) const { return com_vars_[knotpoint_index]; @@ -640,6 +658,16 @@ void KinematicCentroidalSolver::SetForceGuess( } } +void KinematicCentroidalSolver::SetMomentumGuess( + const drake::trajectories::PiecewisePolynomial& + momentum_trajectory) { + DRAKE_DEMAND(momentum_trajectory.rows() == 6); + for (int knot_point = 0; knot_point < n_knot_points_; knot_point++) { + prog_->SetInitialGuess(mom_vars_[knot_point], + momentum_trajectory.value(dt_ * knot_point)); + } +} + void KinematicCentroidalSolver::SetModeSequence( const std::vector>& contact_sequence) { contact_sequence_ = contact_sequence; @@ -647,7 +675,8 @@ void KinematicCentroidalSolver::SetModeSequence( void KinematicCentroidalSolver::SetModeSequence( const drake::trajectories::PiecewisePolynomial& contact_sequence) { for (int knot_point = 0; knot_point < n_knot_points_; knot_point++) { - for (int contact_index = 0; contact_index < 4; contact_index++) { + for (int contact_index = 0; contact_index < n_contact_points_; + contact_index++) { contact_sequence_[knot_point][contact_index] = contact_sequence.value(dt_ * knot_point).coeff(contact_index); } @@ -671,6 +700,7 @@ void KinematicCentroidalSolver::UpdateInitialStateConstraint( void KinematicCentroidalSolver::SetGains( const KinematicCentroidalGains& gains) { + gains_ = gains; std::map positions_map = dairlib::multibody::MakeNameToPositionsMap(plant_); std::map velocities_map = diff --git a/systems/trajectory_optimization/kinematic_centroidal_planner/kinematic_centroidal_solver.h b/systems/trajectory_optimization/kinematic_centroidal_planner/kinematic_centroidal_solver.h index f460a3836d..daafb337fd 100644 --- a/systems/trajectory_optimization/kinematic_centroidal_planner/kinematic_centroidal_solver.h +++ b/systems/trajectory_optimization/kinematic_centroidal_planner/kinematic_centroidal_solver.h @@ -15,11 +15,13 @@ #include "drake/multibody/plant/multibody_plant.h" #include "drake/systems/framework/context.h" +enum Complexity { KINEMATIC_CENTROIDAL, SLIP }; + /*! * @brief Class for solving nonlinear kinematic centroidal mpc. Implementation * is based on Dai, Hongkai, Andres Valenzuela, and Russ Tedrake. “Whole-Body - * TrajectoryParameters Planning with Centroidal Dynamics and Full Kinematics.” - * 2014 IEEE-RAS International Conference on Humanoid Robots (November 2014). + * Motion Planning with Centroidal Dynamics and Full Kinematics.” 2014 IEEE-RAS + * International Conference on Humanoid Robots (November 2014). * * The optimization contains two coupled problems. The centroidal problem * optimizes over: Angular momentum Linear momentum Center of mass position @@ -193,7 +195,7 @@ class KinematicCentroidalSolver { * @brief Adds standard constraints to optimization problem and sets options * @param solver_options */ - void Build(const drake::solvers::SolverOptions& solver_options); + virtual void Build(const drake::solvers::SolverOptions& solver_options); /*! * @brief Solve the optimization problem and return a piecewise linear @@ -257,21 +259,26 @@ class KinematicCentroidalSolver { const drake::trajectories::PiecewisePolynomial& state_trajectory); // TODO remove once drake has trajectory stacking - void SetRobotStateGuess( + virtual void SetRobotStateGuess( const drake::trajectories::PiecewisePolynomial& q_traj, const drake::trajectories::PiecewisePolynomial& v_traj); void SetComPositionGuess(const drake::Vector3& state); - void SetComPositionGuess( + virtual void SetComPositionGuess( const drake::trajectories::PiecewisePolynomial& com_trajectory); - void SetContactGuess(const drake::trajectories::PiecewisePolynomial& - contact_trajectory); + virtual void SetContactGuess( + const drake::trajectories::PiecewisePolynomial& + contact_trajectory); - void SetForceGuess( + virtual void SetForceGuess( const drake::trajectories::PiecewisePolynomial& force_trajectory); + virtual void SetMomentumGuess( + const drake::trajectories::PiecewisePolynomial& + momentum_trajectory); + void CreateVisualizationCallback(const std::string& model_file, double alpha, const std::string& weld_frame_to_world = ""); @@ -319,14 +326,15 @@ class KinematicCentroidalSolver { * `contact_sequence[knot_point][contact_index]` tells you if at `knot_point` * is `contact_index` active */ - void SetModeSequence(const std::vector>& contact_sequence); + virtual void SetModeSequence( + const std::vector>& contact_sequence); /*! * @brief Set the mode sequence via a trajectory. The value of the trajectory * at each time, cast to a bool is if a contact point is active or not * @param contact_sequence */ - void SetModeSequence( + virtual void SetModeSequence( const drake::trajectories::PiecewisePolynomial& contact_sequence); void AddInitialStateConstraint(const Eigen::VectorXd& state); @@ -341,38 +349,43 @@ class KinematicCentroidalSolver { */ void UpdateCosts(); - private: + void SetComplexitySchedule( + const std::vector& complexity_schedule) { + complexity_schedule_ = complexity_schedule; + }; + + protected: /*! * @brief Adds dynamics for centroidal state */ - void AddCentroidalDynamics(); + void AddCentroidalDynamics(int knot_point); /*! * @brief Enforces zero force for feet in flight */ - void AddFlightContactForceConstraints(); + void AddFlightContactForceConstraints(int knot_point); /*! * @brief Enforce dynamics for kinematics and location of the contacts */ - void AddKinematicsIntegrator(); + void AddKinematicsIntegrator(int knot_point); /*! * @brief Feet that in stance are not moving and on the ground, feet in the * air are above the ground */ - void AddContactConstraints(); + void AddContactConstraints(int knot_point); /*! * @brief Ensures that contact point for feet line up with kinematics, and * centroidal state lines up with kinematic state */ - void AddCentroidalKinematicConsistency(); + void AddCentroidalKinematicConsistency(int knot_point); /*! * @brief Ensures feet are not pulling on the ground */ - void AddFrictionConeConstraints(); + void AddFrictionConeConstraints(int knot_point); // void AddTorqueLimits(); /*! @@ -380,11 +393,58 @@ class KinematicCentroidalSolver { */ double GetKnotpointGain(int knot_point) const; + /*! + * @brief Add relevant slip constraints to knot point + * @param knot_point + */ + virtual void AddSlipConstraints(int knot_point) { DRAKE_DEMAND(false); }; + + /*! + * @brief add slip cost to knot point + * @param knot_point + * @param gain + */ + virtual void AddSlipCost(int knot_point, double gain) { + DRAKE_DEMAND(false); + }; + + /*! + * @brief Add constraint to knot point that slip state is equivalent to complex state + * @param knot_point + */ + virtual void AddSlipEqualityConstraint(int knot_point) { + DRAKE_DEMAND(false); + }; + + /*! + * @brief Add constraint to knot point + * @param knot_point + */ + virtual void AddSlipDynamics(int knot_point) { DRAKE_DEMAND(false); }; + + /*! + * @brief Lift the slip solution to generalized state at knot point + * @param knot_point + * @return full robot generalized state + */ + virtual drake::VectorX LiftSlipSolution(int knot_point) { + DRAKE_DEMAND(false); + }; + /*! * @brief Add costs from internally stored variables */ void AddCosts(); + /*! + * @brief Add cost for kinematic centroidal at knot point + * @param knot_point + * @param t + * @param knot_point_gain + */ + void AddKinematicCentroidalCosts(int knot_point, double t, + double knot_point_gain); + bool is_first_knot(int knot_point_index) const { return knot_point_index == 0; }; @@ -467,4 +527,7 @@ class KinematicCentroidalSolver { // saving and publishing solutions std::unique_ptr lcm_; dairlib::LcmTrajectory lcm_trajectory_; + + std::vector complexity_schedule_; + KinematicCentroidalGains gains_; }; diff --git a/systems/trajectory_optimization/kinematic_centroidal_planner/reference_generation_utils.cc b/systems/trajectory_optimization/kinematic_centroidal_planner/reference_generation_utils.cc index fb0a90af73..83ec321e75 100644 --- a/systems/trajectory_optimization/kinematic_centroidal_planner/reference_generation_utils.cc +++ b/systems/trajectory_optimization/kinematic_centroidal_planner/reference_generation_utils.cc @@ -1,12 +1,12 @@ #include "systems/trajectory_optimization/kinematic_centroidal_planner/reference_generation_utils.h" +#include "examples/Cassie/cassie_utils.h" #include "multibody/multibody_utils.h" #include "examples/Cassie/cassie_utils.h" - -std::vector> CreateContactPoints(const drake::multibody::MultibodyPlant< - double> &plant, - double mu) { +std::vector> +CreateContactPoints(const drake::multibody::MultibodyPlant& plant, + double mu) { auto left_toe_pair = dairlib::LeftToeFront(plant); auto left_heel_pair = dairlib::LeftToeRear(plant); auto right_toe_pair = dairlib::RightToeFront(plant); @@ -58,6 +58,20 @@ drake::trajectories::PiecewisePolynomial GenerateComTrajectory( time_points, samples); } +drake::trajectories::PiecewisePolynomial GenerateMomentumTrajectory( + const std::vector& vel_ewrt_w, + const std::vector& time_points, double m) { + DRAKE_DEMAND(vel_ewrt_w.size() == (time_points.size() - 1)); + auto n_points = time_points.size(); + std::vector> samples(n_points); + for (int i = 0; i < n_points; i++) { + samples[i] = drake::Vector6d(); + samples[i] << drake::Vector3::Zero(3), vel_ewrt_w[i] * m; + } + return drake::trajectories::PiecewisePolynomial::ZeroOrderHold( + time_points, samples); +} + drake::trajectories::PiecewisePolynomial GenerateGeneralizedPosTrajectory( const Eigen::VectorXd& nominal_stand, const Eigen::Vector3d& p_ScmBase_W, @@ -92,7 +106,7 @@ GenerateGeneralizedVelTrajectory( drake::trajectories::PiecewisePolynomial GenerateModeSequence( const std::vector& gait_sequence, const std::vector& time_points) { - DRAKE_DEMAND(gait_sequence.size() == time_points.size()-1); + DRAKE_DEMAND(gait_sequence.size() == time_points.size() - 1); auto traj = gait_sequence[0].ToTrajectory(time_points[0], time_points[1]); for (int i = 1; i < gait_sequence.size(); i++) { diff --git a/systems/trajectory_optimization/kinematic_centroidal_planner/reference_generation_utils.h b/systems/trajectory_optimization/kinematic_centroidal_planner/reference_generation_utils.h index 0c492b3008..251d815fb1 100644 --- a/systems/trajectory_optimization/kinematic_centroidal_planner/reference_generation_utils.h +++ b/systems/trajectory_optimization/kinematic_centroidal_planner/reference_generation_utils.h @@ -1,7 +1,8 @@ #pragma once -#include #include +#include + #include "multibody/kinematic/world_point_evaluator.h" #include "systems/trajectory_optimization/kinematic_centroidal_planner/gait.h" @@ -11,79 +12,101 @@ * @param mu coefficient of friction * @return */ -std::vector> CreateContactPoints(const drake::multibody::MultibodyPlant& plant, double mu); - +std::vector> +CreateContactPoints(const drake::multibody::MultibodyPlant& plant, + double mu); /*! - * @brief given an initial com and velocity of com emrt w, calculate a com trajectory + * @brief given an initial com and velocity of com emrt w, calculate a com + * trajectory * @param current_com - * @param vel_ewrt_w velocity of the com ewrt. Given a zoh between time points, must be the same length as time_points - * @param time_points time values correspeding to velocities, trajectory start at the first, and end at the last, must be the same length as vel_ewrt_w + * @param vel_ewrt_w velocity of the com ewrt. Given a zoh between time points, + * must be the same length as time_points + * @param time_points time values correspeding to velocities, trajectory start + * at the first, and end at the last, must be the same length as vel_ewrt_w * @return trajectory of com velocity */ -drake::trajectories::PiecewisePolynomial GenerateComTrajectory(const Eigen::Vector3d& current_com, - const std::vector& vel_ewrt_w, - const std::vector& time_points); +drake::trajectories::PiecewisePolynomial GenerateComTrajectory( + const Eigen::Vector3d& current_com, + const std::vector& vel_ewrt_w, + const std::vector& time_points); + +drake::trajectories::PiecewisePolynomial GenerateMomentumTrajectory( + const std::vector& vel_ewrt_w, + const std::vector& time_points, double m); /*! - * @brief Constructs a trajectory for the generalized positions of a constant joint state and floating base position from com trajectory + * @brief Constructs a trajectory for the generalized positions of a constant + * joint state and floating base position from com trajectory * @param nominal_stand [nq] generalized state * @param p_ScmBase_W vector from com to floating base in world frame * @param com_traj trajectory for the center of mass - * @param base_pos_start index where the floating base position starts in generalized state + * @param base_pos_start index where the floating base position starts in + * generalized state * @return trajectory of generalized position */ -drake::trajectories::PiecewisePolynomial GenerateGeneralizedPosTrajectory(const Eigen::VectorXd& nominal_stand, - const Eigen::Vector3d& p_ScmBase_W, - const drake::trajectories::PiecewisePolynomial& com_traj, - int base_pos_start); +drake::trajectories::PiecewisePolynomial +GenerateGeneralizedPosTrajectory( + const Eigen::VectorXd& nominal_stand, const Eigen::Vector3d& p_ScmBase_W, + const drake::trajectories::PiecewisePolynomial& com_traj, + int base_pos_start); /*! - * @brief constructs a trajectory for the generalized velocity where the joint velocity is 0 and floating base val from com trajectory + * @brief constructs a trajectory for the generalized velocity where the joint + * velocity is 0 and floating base val from com trajectory * @param com_traj * @param n_v number of velocity states * @param base_vel_start index where base vel starts * @return trajectory of generalized velocity */ -drake::trajectories::PiecewisePolynomial GenerateGeneralizedVelTrajectory(const drake::trajectories::PiecewisePolynomial& com_traj, - int n_v, - int base_vel_start); +drake::trajectories::PiecewisePolynomial +GenerateGeneralizedVelTrajectory( + const drake::trajectories::PiecewisePolynomial& com_traj, int n_v, + int base_vel_start); /*! - * @brief given a vector of gaits and time points corresponding to when the gaits are active generate a mode sequence. - * The transition between gaits can be awkward depending on where the gaits are in phase space during the transition - * {TODO SRL} make transition clean by attempting to shift phase so mode sequence lines up + * @brief given a vector of gaits and time points corresponding to when the + * gaits are active generate a mode sequence. The transition between gaits can + * be awkward depending on where the gaits are in phase space during the + * transition {TODO SRL} make transition clean by attempting to shift phase so + * mode sequence lines up * @param gait_sequence vector of gaits * @param time_points vector of time points when each gait is active - * @return trajectory of mode status start at time_point(0) and ending at time_point.end()-1 + * @return trajectory of mode status start at time_point(0) and ending at + * time_point.end()-1 */ -drake::trajectories::PiecewisePolynomial GenerateModeSequence(const std::vector& gait_sequence, - const std::vector& time_points); +drake::trajectories::PiecewisePolynomial GenerateModeSequence( + const std::vector& gait_sequence, + const std::vector& time_points); /*! - * @brief given a trajectory which describes the mode, and the mass of the robot calculate a nominal grf trajectory where the weight of the robot - * is distributed over the active contact points + * @brief given a trajectory which describes the mode, and the mass of the robot + * calculate a nominal grf trajectory where the weight of the robot is + * distributed over the active contact points * @param mode_trajectory * @param m * @return trajectory of grf for reference */ -drake::trajectories::PiecewisePolynomial GenerateGrfReference(const drake::trajectories::PiecewisePolynomial& mode_trajectory, - double m); +drake::trajectories::PiecewisePolynomial GenerateGrfReference( + const drake::trajectories::PiecewisePolynomial& mode_trajectory, + double m); /*! - * @brief Calculate trajectory of world point evaluators from generalized state trajectory. This assumes first order hold - * between knot points in state trajectories. - * TODO If we start using more complicated state references with this function sample time more coarsely + * @brief Calculate trajectory of world point evaluators from generalized state + * trajectory. This assumes first order hold between knot points in state + * trajectories. + * TODO If we start using more complicated state references with this function + * sample time more coarsely * @param plant * @param contacts vector of world point evaluators * @param q_traj generalized position trajectory * @param v_traj generalized velocity trajectory - * @return trajectory of contact points stacked [contact1_pos, contact1_vel, ... contact_n_pos, contact_n_vel] + * @return trajectory of contact points stacked [contact1_pos, contact1_vel, ... + * contact_n_pos, contact_n_vel] */ -drake::trajectories::PiecewisePolynomial GenerateContactPointReference(const drake::multibody::MultibodyPlant &plant, - const std::vector> &contacts, - const drake::trajectories::PiecewisePolynomial< - double> &q_traj, - const drake::trajectories::PiecewisePolynomial< - double> &v_traj); \ No newline at end of file +drake::trajectories::PiecewisePolynomial GenerateContactPointReference( + const drake::multibody::MultibodyPlant& plant, + const std::vector>& + contacts, + const drake::trajectories::PiecewisePolynomial& q_traj, + const drake::trajectories::PiecewisePolynomial& v_traj);