diff --git a/WORKSPACE b/WORKSPACE index e06189161f..8fce6c5d73 100644 --- a/WORKSPACE +++ b/WORKSPACE @@ -3,6 +3,23 @@ workspace(name = "dairlib") +# Support for apple toolchains on Bazel 7+ +# https://github.com/bazelbuild/apple_support/releases +load("@bazel_tools//tools/build_defs/repo:http.bzl", "http_archive") + +http_archive( + name = "build_bazel_apple_support", + sha256 = "c4bb2b7367c484382300aee75be598b92f847896fb31bbd22f3a2346adf66a80", + url = "https://github.com/bazelbuild/apple_support/releases/download/1.15.1/apple_support.1.15.1.tar.gz", +) + +load( + "@build_bazel_apple_support//lib:repositories.bzl", + "apple_support_dependencies", +) + +apple_support_dependencies() + # dairlib can use either a local version of drake or a pegged revision # If the environment variable DAIRLIB_LOCAL_DRAKE_PATH is set, it will use # a local version, ad the specified path. Otherwise, it will get a pegged @@ -22,8 +39,10 @@ DRAKE_CHECKSUM = "6ff298d7fbc33cb17963509f86fcd9cb6816d455b97b3fd589e1085e0548c2 load("//:environ.bzl", "drake_repository") load("//:environ.bzl", "inekf_repository") -drake_repository(name="drake_path") -inekf_repository(name="inekf_path") +drake_repository(name = "drake_path") + +inekf_repository(name = "inekf_path") + load("@drake_path//:environ.bzl", "DAIRLIB_LOCAL_DRAKE_PATH") load("@inekf_path//:environ.bzl", "DAIRLIB_LOCAL_INEKF_PATH") diff --git a/examples/Cassie/BUILD.bazel b/examples/Cassie/BUILD.bazel index 0132877122..b2bc1a0ec1 100644 --- a/examples/Cassie/BUILD.bazel +++ b/examples/Cassie/BUILD.bazel @@ -66,7 +66,7 @@ cc_library( hdrs = ["cassie_state_estimator_settings.h"], deps = [ "@drake//:drake_shared_library", - ] + ], ) cc_library( @@ -75,9 +75,9 @@ cc_library( hdrs = ["cassie_state_estimator.h"], deps = [ ":cassie_utils", + "//examples/Cassie:cassie_state_estimator_settings", "//examples/Cassie/datatypes:cassie_names", "//examples/Cassie/datatypes:cassie_out_t", - "//examples/Cassie:cassie_state_estimator_settings", "//multibody:utils", "//multibody/kinematic", "//systems/framework:vector", @@ -185,9 +185,9 @@ cc_binary( "//examples/Cassie/systems:cassie_encoder", "//solvers:optimization_utils", "//systems:robot_lcm_systems", + "//systems:system_utils", "//systems/framework:geared_motor", "//systems/primitives", - "//systems:system_utils", "@drake//:drake_shared_library", "@gflags", ], @@ -249,9 +249,9 @@ cc_binary( "//lcmtypes:lcmt_robot", "//multibody:multibody_solvers", "//systems:robot_lcm_systems", + "//systems:system_utils", "//systems/framework:lcm_driven_loop", "//systems/primitives", - "//systems:system_utils", "@drake//:drake_shared_library", "@gflags", ], @@ -269,9 +269,9 @@ cc_binary( "//examples/Cassie/systems:input_supervisor", "//lcmtypes:lcmt_robot", "//systems:robot_lcm_systems", + "//systems:system_utils", "//systems/controllers:linear_controller", "//systems/controllers:pd_config_lcm", - "//systems:system_utils", "@drake//:drake_shared_library", "@gflags", ], @@ -296,8 +296,8 @@ cc_binary( deps = [ "//lcmtypes:lcmt_robot", "@drake//lcm", - "@lcm", "@gflags", + "@lcm", ], ) @@ -307,8 +307,8 @@ cc_binary( deps = [ "//lcmtypes:lcmt_robot", "@drake//lcm", - "@lcm", "@gflags", + "@lcm", ], ) @@ -350,18 +350,18 @@ cc_binary( deps = [ ":cassie_urdf", ":cassie_utils", - "//examples/Cassie/osc", "//examples/Cassie/contact_scheduler:all", - "//examples/Cassie/systems:cassie_out_to_radio", + "//examples/Cassie/osc", "//examples/Cassie/osc_jump", "//examples/Cassie/osc_run", + "//examples/Cassie/systems:cassie_out_to_radio", "//lcm:trajectory_saver", "//multibody:utils", "//systems:robot_lcm_systems", "//systems:system_utils", - "//systems/filters:floating_base_velocity_filter", - "//systems/controllers/osc:osc_tracking_datas", "//systems/controllers:controllers_all", + "//systems/controllers/osc:osc_tracking_datas", + "//systems/filters:floating_base_velocity_filter", "//systems/framework:lcm_driven_loop", "//systems/primitives", "@drake//:drake_shared_library", @@ -372,6 +372,7 @@ cc_binary( cc_binary( name = "run_osc_walking_controller", srcs = ["run_osc_walking_controller.cc"], + tags = ["manual"], deps = [ ":cassie_urdf", ":cassie_utils", @@ -382,11 +383,11 @@ cc_binary( "//multibody:view_frame", "//multibody/kinematic", "//systems:robot_lcm_systems", + "//systems:system_utils", + "//systems/controllers:controllers_all", "//systems/controllers:fsm_event_time", "//systems/framework:lcm_driven_loop", "//systems/primitives", - "//systems/controllers:controllers_all", - "//systems:system_utils", "@drake//:drake_shared_library", "@gflags", ], @@ -399,17 +400,17 @@ cc_binary( ":cassie_urdf", ":cassie_utils", "//examples/Cassie/osc", - "//examples/impact_invariant_control:impact_aware_time_based_fsm", "//examples/Cassie/systems:cassie_out_to_radio", "//examples/Cassie/systems:simulator_drift", + "//examples/impact_invariant_control:impact_aware_time_based_fsm", "//multibody:utils", "//multibody:view_frame", "//multibody/kinematic", "//solvers:solver_options_io", "//systems:robot_lcm_systems", "//systems:system_utils", - "//systems/controllers:fsm_event_time", "//systems/controllers:controllers_all", + "//systems/controllers:fsm_event_time", "//systems/filters:floating_base_velocity_filter", "//systems/framework:lcm_driven_loop", "//systems/primitives", @@ -429,11 +430,11 @@ cc_binary( "//multibody:utils", "//multibody/kinematic", "//systems:robot_lcm_systems", + "//systems:system_utils", "//systems/controllers/osc:operational_space_control", "//systems/controllers/osc:osc_gains", "//systems/framework:lcm_driven_loop", "//systems/primitives", - "//systems:system_utils", "@drake//:drake_shared_library", "@gflags", ], diff --git a/examples/Cassie/cassie_fixed_point_solver.cc b/examples/Cassie/cassie_fixed_point_solver.cc index e4b6d394f2..fdd5f9f00e 100644 --- a/examples/Cassie/cassie_fixed_point_solver.cc +++ b/examples/Cassie/cassie_fixed_point_solver.cc @@ -88,14 +88,14 @@ void CassieFixedPointSolver( program.AddJointLimitConstraints(q); // Fix floating base - program.AddConstraint(q(positions_map.at("base_qw")) == 1); - program.AddConstraint(q(positions_map.at("base_qx")) == 0); - program.AddConstraint(q(positions_map.at("base_qy")) == 0); - program.AddConstraint(q(positions_map.at("base_qz")) == 0); + program.AddConstraint(q(positions_map.at("pelvis_qw")) == 1); + program.AddConstraint(q(positions_map.at("pelvis_qx")) == 0); + program.AddConstraint(q(positions_map.at("pelvis_qy")) == 0); + program.AddConstraint(q(positions_map.at("pelvis_qz")) == 0); - program.AddConstraint(q(positions_map.at("base_x")) == 0); - program.AddConstraint(q(positions_map.at("base_y")) == 0); - program.AddConstraint(q(positions_map.at("base_z")) == height); + program.AddConstraint(q(positions_map.at("pelvis_x")) == 0); + program.AddConstraint(q(positions_map.at("pelvis_y")) == 0); + program.AddConstraint(q(positions_map.at("pelvis_z")) == height); // Add symmetry constraints, and zero roll/pitch on the hip program.AddConstraint(q(positions_map.at("knee_left")) == @@ -143,7 +143,7 @@ void CassieFixedPointSolver( // Set initial guess/cost for q using a vaguely neutral position Eigen::VectorXd q_guess = Eigen::VectorXd::Zero(plant.num_positions()); q_guess(0) = 1; // quaternion - q_guess(positions_map.at("base_z")) = height; + q_guess(positions_map.at("pelvis_z")) = height; q_guess(positions_map.at("hip_pitch_left")) = 1; q_guess(positions_map.at("knee_left")) = -2; q_guess(positions_map.at("ankle_joint_left")) = 2; @@ -334,27 +334,27 @@ void CassieInitStateSolver( int n_v = plant.num_velocities(); // Fix floating base - program.AddBoundingBoxConstraint(1, 1, q(positions_map.at("base_qw"))); - program.AddBoundingBoxConstraint(0, 0, q(positions_map.at("base_qx"))); - program.AddBoundingBoxConstraint(0, 0, q(positions_map.at("base_qy"))); - program.AddBoundingBoxConstraint(0, 0, q(positions_map.at("base_qz"))); + program.AddBoundingBoxConstraint(1, 1, q(positions_map.at("pelvis_qw"))); + program.AddBoundingBoxConstraint(0, 0, q(positions_map.at("pelvis_qx"))); + program.AddBoundingBoxConstraint(0, 0, q(positions_map.at("pelvis_qy"))); + program.AddBoundingBoxConstraint(0, 0, q(positions_map.at("pelvis_qz"))); - program.AddBoundingBoxConstraint(0, 0, q(positions_map.at("base_x"))); - program.AddBoundingBoxConstraint(0, 0, q(positions_map.at("base_y"))); + program.AddBoundingBoxConstraint(0, 0, q(positions_map.at("pelvis_x"))); + program.AddBoundingBoxConstraint(0, 0, q(positions_map.at("pelvis_y"))); program.AddBoundingBoxConstraint(height, height, - q(positions_map.at("base_z"))); + q(positions_map.at("pelvis_z"))); program.AddBoundingBoxConstraint(-10, 10, v); - program.AddBoundingBoxConstraint(0, 0, v(vel_map.at("base_wx"))); - program.AddBoundingBoxConstraint(0, 0, v(vel_map.at("base_wy"))); - program.AddBoundingBoxConstraint(0, 0, v(vel_map.at("base_wz"))); + program.AddBoundingBoxConstraint(0, 0, v(vel_map.at("pelvis_wx"))); + program.AddBoundingBoxConstraint(0, 0, v(vel_map.at("pelvis_wy"))); + program.AddBoundingBoxConstraint(0, 0, v(vel_map.at("pelvis_wz"))); program.AddBoundingBoxConstraint(pelvis_xy_vel(0), pelvis_xy_vel(0), - v(vel_map.at("base_vx"))); + v(vel_map.at("pelvis_vx"))); program.AddBoundingBoxConstraint(pelvis_xy_vel(1), pelvis_xy_vel(1), - v(vel_map.at("base_vy"))); - program.AddBoundingBoxConstraint(0, 0, v(vel_map.at("base_vz"))); + v(vel_map.at("pelvis_vy"))); + program.AddBoundingBoxConstraint(0, 0, v(vel_map.at("pelvis_vz"))); // Add symmetry constraints, and zero roll/pitch on the hip program.AddConstraint(q(positions_map.at("knee_left")) == diff --git a/examples/Cassie/cassie_state_estimator.cc b/examples/Cassie/cassie_state_estimator.cc index b4b84d9755..4e358665be 100644 --- a/examples/Cassie/cassie_state_estimator.cc +++ b/examples/Cassie/cassie_state_estimator.cc @@ -1102,6 +1102,9 @@ void CassieStateEstimator::DoCalcNextUpdateTime( const auto& self = dynamic_cast(system); return self.Update(c, s); }; + auto& uu_events = events->get_mutable_unrestricted_update_events(); + uu_events.AddEvent(UnrestrictedUpdateEvent( + drake::systems::TriggerType::kTimed, callback)); } else { *time = INFINITY; } diff --git a/examples/Cassie/networking/cassie_udp_publisher.cc b/examples/Cassie/networking/cassie_udp_publisher.cc index a9f789378a..c5db6a378f 100644 --- a/examples/Cassie/networking/cassie_udp_publisher.cc +++ b/examples/Cassie/networking/cassie_udp_publisher.cc @@ -42,8 +42,8 @@ CassieUDPPublisher::CassieUDPPublisher(const std::string& address, server_address_.sin_family = AF_INET; // IPv4 server_address_.sin_port = htons(port); - // Declare a forced publish so that any time Publish(.) is called on this - // system (or a Diagram containing it), a message is emitted. + // Declare a forced publish so that any time ForcedPublish(.) is called on + // this system (or a Diagram containing it), a message is emitted. if (publish_triggers.find(TriggerType::kForced) != publish_triggers.end()) { this->DeclareForcedPublishEvent( &CassieUDPPublisher::PublishInputAsUDPMessage); diff --git a/examples/Cassie/osc/heading_traj_generator.cc b/examples/Cassie/osc/heading_traj_generator.cc index 30bd5af970..7323360f1f 100644 --- a/examples/Cassie/osc/heading_traj_generator.cc +++ b/examples/Cassie/osc/heading_traj_generator.cc @@ -1,9 +1,7 @@ #include "examples/Cassie/osc/heading_traj_generator.h" #include - -#include - +#include "drake/common/trajectories/piecewise_quaternion.h" #include "multibody/multibody_utils.h" using std::string; @@ -21,6 +19,7 @@ using drake::systems::Context; using drake::systems::LeafSystem; using drake::trajectories::PiecewisePolynomial; +using drake::trajectories::PiecewiseQuaternionSlerp; namespace dairlib { namespace cassie { @@ -43,7 +42,7 @@ HeadingTrajGenerator::HeadingTrajGenerator( this->DeclareVectorInputPort("pelvis_yaw", BasicVector(1)) .get_index(); // Provide an instance to allocate the memory first (for the output) - PiecewisePolynomial pp(VectorXd(0)); + PiecewiseQuaternionSlerp pp; drake::trajectories::Trajectory& traj_inst = pp; this->DeclareAbstractOutputPort("pelvis_quat", traj_inst, &HeadingTrajGenerator::CalcHeadingTraj); @@ -100,19 +99,17 @@ void HeadingTrajGenerator::CalcHeadingTraj( Quaterniond relative_quat(cos(des_delta_yaw / 2), 0, 0, sin(des_delta_yaw / 2)); Quaterniond final_quat = relative_quat * init_quat; - Eigen::Vector4d pelvis_rotation_f; - pelvis_rotation_f << final_quat.w(), final_quat.vec(); const std::vector breaks = {context.get_time(), context.get_time() + dt}; - std::vector knots(breaks.size(), MatrixXd::Zero(4, 1)); - knots[0] = pelvis_rotation_i; - knots[1] = pelvis_rotation_f; - const auto pp = PiecewisePolynomial::FirstOrderHold(breaks, knots); + std::vector knots(breaks.size(), Quaterniond()); + knots[0] = init_quat; + knots[1] = final_quat; + const auto pp = PiecewiseQuaternionSlerp(breaks, knots); // Assign traj auto* pp_traj = - (PiecewisePolynomial*)dynamic_cast*>( + (PiecewiseQuaternionSlerp*)dynamic_cast*>( traj); *pp_traj = pp; } diff --git a/examples/Cassie/osc/standing_pelvis_orientation_traj.cc b/examples/Cassie/osc/standing_pelvis_orientation_traj.cc index 0717cb49fc..7e154d9e77 100644 --- a/examples/Cassie/osc/standing_pelvis_orientation_traj.cc +++ b/examples/Cassie/osc/standing_pelvis_orientation_traj.cc @@ -3,12 +3,14 @@ #include #include "drake/common/trajectories/piecewise_polynomial.h" +#include "drake/common/trajectories/piecewise_quaternion.h" #include "drake/common/trajectories/trajectory.h" #include "drake/math/wrap_to.h" using dairlib::systems::OutputVector; using drake::systems::BasicVector; using drake::trajectories::PiecewisePolynomial; +using drake::trajectories::PiecewiseQuaternionSlerp; using drake::trajectories::Trajectory; using Eigen::MatrixXd; using Eigen::Vector3d; @@ -39,7 +41,7 @@ StandingPelvisOrientationTraj::StandingPelvisOrientationTraj( this->DeclareAbstractInputPort("radio_out", drake::Value{}) .get_index(); - PiecewisePolynomial empty_pp_traj(Eigen::VectorXd(0)); + PiecewiseQuaternionSlerp empty_pp_traj; Trajectory& traj_inst = empty_pp_traj; this->set_name(traj_name); this->DeclareAbstractOutputPort(traj_name, traj_inst, @@ -57,7 +59,7 @@ void StandingPelvisOrientationTraj::CalcTraj( VectorXd q = robot_output->GetPositions(); plant_.SetPositions(context_, q); auto* casted_traj = - (PiecewisePolynomial*)dynamic_cast*>( + (PiecewiseQuaternionSlerp*)dynamic_cast*>( traj); Vector3d pt_0; Vector3d pt_1; @@ -83,11 +85,12 @@ void StandingPelvisOrientationTraj::CalcTraj( 0.5 * (atan2(l_foot(1), l_foot(0)) + atan2(r_foot(1), r_foot(0))), -M_PI, M_PI) + radio_out->channel[3]; - + const std::vector breaks = {context.get_time(), + context.get_time() + 1.0}; auto rot_mat = drake::math::RotationMatrix(drake::math::RollPitchYaw(rpy)); - *casted_traj = PiecewisePolynomial(rot_mat.ToQuaternionAsVector4()); + *casted_traj = PiecewiseQuaternionSlerp(breaks, {rot_mat.ToQuaternion(), rot_mat.ToQuaternion()}); } } // namespace dairlib::cassie::osc diff --git a/examples/Cassie/run_osc_jumping_controller.cc b/examples/Cassie/run_osc_jumping_controller.cc index 9de6d464ec..a7313d6325 100644 --- a/examples/Cassie/run_osc_jumping_controller.cc +++ b/examples/Cassie/run_osc_jumping_controller.cc @@ -99,24 +99,24 @@ int DoMain(int argc, char* argv[]) { DiagramBuilder builder; // Built the Cassie MBPs - drake::multibody::MultibodyPlant plant_w_spr(0.0); - AddCassieMultibody(&plant_w_spr, nullptr, true, + drake::multibody::MultibodyPlant plant(0.0); + AddCassieMultibody(&plant, nullptr, true, "examples/Cassie/urdf/cassie_v2_conservative.urdf", true /*spring model*/, false /*loop closure*/); - plant_w_spr.Finalize(); + plant.Finalize(); - auto context_w_spr = plant_w_spr.CreateDefaultContext(); + auto plant_context = plant.CreateDefaultContext(); // Get contact frames and position - auto left_toe = LeftToeFront(plant_w_spr); - auto left_heel = LeftToeRear(plant_w_spr); - auto right_toe = RightToeFront(plant_w_spr); - auto right_heel = RightToeRear(plant_w_spr); + auto left_toe = LeftToeFront(plant); + auto left_heel = LeftToeRear(plant); + auto right_toe = RightToeFront(plant); + auto right_heel = RightToeRear(plant); // Create maps for joints - map pos_map = multibody::MakeNameToPositionsMap(plant_w_spr); - map vel_map = multibody::MakeNameToVelocitiesMap(plant_w_spr); - map act_map = multibody::MakeNameToActuatorsMap(plant_w_spr); + map pos_map = multibody::MakeNameToPositionsMap(plant); + map vel_map = multibody::MakeNameToVelocitiesMap(plant); + map act_map = multibody::MakeNameToActuatorsMap(plant); std::vector&>> feet_contact_points = {left_toe, right_toe}; @@ -132,7 +132,7 @@ int DoMain(int argc, char* argv[]) { /**** Get trajectory from optimization ****/ const DirconTrajectory& dircon_trajectory = DirconTrajectory( - plant_w_spr, FindResourceOrThrow(FLAGS_folder_path + FLAGS_traj_name)); + plant, FindResourceOrThrow(FLAGS_folder_path + FLAGS_traj_name)); string output_traj_path = FLAGS_folder_path + FLAGS_traj_name + "_processed"; if (osc_gains.relative_feet) { output_traj_path += "_rel"; @@ -241,33 +241,33 @@ int DoMain(int argc, char* argv[]) { drake::lcm::DrakeLcm lcm("udpm://239.255.76.67:7667?ttl=0"); auto state_receiver = - builder.AddSystem(plant_w_spr); + builder.AddSystem(plant); // This actually outputs the target position of the pelvis not the true // center of mass auto pelvis_trans_traj_generator = builder.AddSystem( - plant_w_spr, context_w_spr.get(), pelvis_trans_traj, + plant, plant_context.get(), pelvis_trans_traj, feet_contact_points, FLAGS_delay_time); auto l_foot_traj_generator = builder.AddSystem( - plant_w_spr, context_w_spr.get(), "hip_left", true, l_foot_trajectory, + plant, plant_context.get(), "hip_left", true, l_foot_trajectory, l_hip_trajectory, FLAGS_delay_time); auto r_foot_traj_generator = builder.AddSystem( - plant_w_spr, context_w_spr.get(), "hip_right", false, r_foot_trajectory, + plant, plant_context.get(), "hip_right", false, r_foot_trajectory, r_hip_trajectory, FLAGS_delay_time); auto pelvis_rot_traj_generator = builder.AddSystem( pelvis_rot_trajectory, "pelvis_rot_traj", FLAGS_delay_time); auto fsm = builder.AddSystem( - plant_w_spr, transition_times, FLAGS_contact_based_fsm, + plant, transition_times, FLAGS_contact_based_fsm, gains.impact_threshold, (osc_jump::JUMPING_FSM_STATE)FLAGS_init_fsm_state); auto command_pub = builder.AddSystem(LcmPublisherSystem::Make( FLAGS_channel_u, &lcm, TriggerTypeSet({TriggerType::kForced}))); auto command_sender = - builder.AddSystem(plant_w_spr); + builder.AddSystem(plant); auto osc = builder.AddSystem( - plant_w_spr, context_w_spr.get(), true); + plant, plant_context.get(), true); auto osc_debug_pub = builder.AddSystem(LcmPublisherSystem::Make( "OSC_DEBUG_JUMPING", &lcm, TriggerTypeSet({TriggerType::kForced}))); @@ -315,16 +315,16 @@ int DoMain(int argc, char* argv[]) { osc->SetContactFriction(gains.mu); auto left_toe_evaluator = multibody::WorldPointEvaluator( - plant_w_spr, left_toe.first, left_toe.second, Matrix3d::Identity(), + plant, left_toe.first, left_toe.second, Matrix3d::Identity(), Vector3d::Zero(), {1, 2}); auto left_heel_evaluator = multibody::WorldPointEvaluator( - plant_w_spr, left_heel.first, left_heel.second, Matrix3d::Identity(), + plant, left_heel.first, left_heel.second, Matrix3d::Identity(), Vector3d::Zero(), {0, 1, 2}); auto right_toe_evaluator = multibody::WorldPointEvaluator( - plant_w_spr, right_toe.first, right_toe.second, Matrix3d::Identity(), + plant, right_toe.first, right_toe.second, Matrix3d::Identity(), Vector3d::Zero(), {1, 2}); auto right_heel_evaluator = multibody::WorldPointEvaluator( - plant_w_spr, right_heel.first, right_heel.second, Matrix3d::Identity(), + plant, right_heel.first, right_heel.second, Matrix3d::Identity(), Vector3d::Zero(), {0, 1, 2}); vector stance_modes = { osc_jump::BALANCE, osc_jump::CROUCH, osc_jump::LAND}; @@ -350,30 +350,30 @@ int DoMain(int argc, char* argv[]) { stance_modes ); - multibody::KinematicEvaluatorSet evaluators(plant_w_spr); + multibody::KinematicEvaluatorSet evaluators(plant); // Fix the springs in the dynamics - auto pos_idx_map = multibody::MakeNameToPositionsMap(plant_w_spr); - auto vel_idx_map = multibody::MakeNameToVelocitiesMap(plant_w_spr); + auto pos_idx_map = multibody::MakeNameToPositionsMap(plant); + auto vel_idx_map = multibody::MakeNameToVelocitiesMap(plant); auto left_fixed_knee_spring = - FixedJointEvaluator(plant_w_spr, pos_idx_map.at("knee_joint_left"), + FixedJointEvaluator(plant, pos_idx_map.at("knee_joint_left"), vel_idx_map.at("knee_joint_leftdot"), 0); auto right_fixed_knee_spring = - FixedJointEvaluator(plant_w_spr, pos_idx_map.at("knee_joint_right"), + FixedJointEvaluator(plant, pos_idx_map.at("knee_joint_right"), vel_idx_map.at("knee_joint_rightdot"), 0); auto left_fixed_ankle_spring = FixedJointEvaluator( - plant_w_spr, pos_idx_map.at("ankle_spring_joint_left"), + plant, pos_idx_map.at("ankle_spring_joint_left"), vel_idx_map.at("ankle_spring_joint_leftdot"), 0); auto right_fixed_ankle_spring = FixedJointEvaluator( - plant_w_spr, pos_idx_map.at("ankle_spring_joint_right"), + plant, pos_idx_map.at("ankle_spring_joint_right"), vel_idx_map.at("ankle_spring_joint_rightdot"), 0); evaluators.add_evaluator(&left_fixed_knee_spring); evaluators.add_evaluator(&right_fixed_knee_spring); evaluators.add_evaluator(&left_fixed_ankle_spring); evaluators.add_evaluator(&right_fixed_ankle_spring); - auto left_loop = LeftLoopClosureEvaluator(plant_w_spr); - auto right_loop = RightLoopClosureEvaluator(plant_w_spr); + auto left_loop = LeftLoopClosureEvaluator(plant); + auto right_loop = RightLoopClosureEvaluator(plant); evaluators.add_evaluator(&left_loop); evaluators.add_evaluator(&right_loop); @@ -383,10 +383,10 @@ int DoMain(int argc, char* argv[]) { /**** Tracking Data for OSC *****/ auto pelvis_tracking_data = std::make_unique( "pelvis_trans_traj", osc_gains.K_p_com, osc_gains.K_d_com, - osc_gains.W_com, plant_w_spr, plant_w_spr); + osc_gains.W_com, plant); auto pelvis_rot_tracking_data = std::make_unique( "pelvis_rot_traj", osc_gains.K_p_pelvis, osc_gains.K_d_pelvis, - osc_gains.W_pelvis, plant_w_spr, plant_w_spr); + osc_gains.W_pelvis, plant); for (auto mode : stance_modes) { pelvis_tracking_data->AddStateAndPointToTrack(mode, "pelvis"); pelvis_rot_tracking_data->AddStateAndFrameToTrack(mode, "pelvis"); @@ -400,10 +400,10 @@ int DoMain(int argc, char* argv[]) { auto left_foot_tracking_data = std::make_unique( "left_ft_traj", osc_gains.K_p_flight_foot, osc_gains.K_d_flight_foot, - osc_gains.W_flight_foot, plant_w_spr, plant_w_spr); + osc_gains.W_flight_foot, plant); auto right_foot_tracking_data = std::make_unique( "right_ft_traj", osc_gains.K_p_flight_foot, osc_gains.K_d_flight_foot, - osc_gains.W_flight_foot, plant_w_spr, plant_w_spr); + osc_gains.W_flight_foot, plant); left_foot_tracking_data->AddStateAndPointToTrack(osc_jump::FLIGHT, "toe_left"); right_foot_tracking_data->AddStateAndPointToTrack(osc_jump::FLIGHT, @@ -411,10 +411,10 @@ int DoMain(int argc, char* argv[]) { auto left_hip_tracking_data = std::make_unique( "left_hip_traj", osc_gains.K_p_flight_foot, osc_gains.K_d_flight_foot, - osc_gains.W_flight_foot, plant_w_spr, plant_w_spr); + osc_gains.W_flight_foot, plant); auto right_hip_tracking_data = std::make_unique( "right_hip_traj", osc_gains.K_p_flight_foot, osc_gains.K_d_flight_foot, - osc_gains.W_flight_foot, plant_w_spr, plant_w_spr); + osc_gains.W_flight_foot, plant); left_hip_tracking_data->AddStateAndPointToTrack(osc_jump::FLIGHT, "hip_left"); right_hip_tracking_data->AddStateAndPointToTrack(osc_jump::FLIGHT, "hip_right"); @@ -422,21 +422,21 @@ int DoMain(int argc, char* argv[]) { auto left_foot_rel_tracking_data = std::make_unique( "left_ft_traj", osc_gains.K_p_flight_foot, osc_gains.K_d_flight_foot, - osc_gains.W_flight_foot, plant_w_spr, plant_w_spr, + osc_gains.W_flight_foot, plant, left_foot_tracking_data.get(), left_hip_tracking_data.get()); auto right_foot_rel_tracking_data = std::make_unique( "right_ft_traj", osc_gains.K_p_flight_foot, osc_gains.K_d_flight_foot, - osc_gains.W_flight_foot, plant_w_spr, plant_w_spr, + osc_gains.W_flight_foot, plant, right_foot_tracking_data.get(), right_hip_tracking_data.get()); // Flight phase hip yaw tracking auto left_hip_yaw_tracking_data = std::make_unique( "hip_yaw_left_traj", osc_gains.K_p_hip_yaw, osc_gains.K_d_hip_yaw, - osc_gains.W_hip_yaw, plant_w_spr, plant_w_spr); + osc_gains.W_hip_yaw, plant); auto right_hip_yaw_tracking_data = std::make_unique( "hip_yaw_right_traj", osc_gains.K_p_hip_yaw, osc_gains.K_d_hip_yaw, - osc_gains.W_hip_yaw, plant_w_spr, plant_w_spr); + osc_gains.W_hip_yaw, plant); left_hip_yaw_tracking_data->AddStateAndJointToTrack( osc_jump::FLIGHT, "hip_yaw_left", "hip_yaw_leftdot"); right_hip_yaw_tracking_data->AddStateAndJointToTrack( @@ -454,10 +454,10 @@ int DoMain(int argc, char* argv[]) { MatrixXd K_d_swing_toe = osc_gains.swing_toe_kd * MatrixXd::Identity(1, 1); auto left_toe_angle_tracking_data = std::make_unique( "left_toe_angle_traj", K_p_swing_toe, K_d_swing_toe, W_swing_toe, - plant_w_spr, plant_w_spr); + plant); auto right_toe_angle_tracking_data = std::make_unique( "right_toe_angle_traj", K_p_swing_toe, K_d_swing_toe, W_swing_toe, - plant_w_spr, plant_w_spr); + plant); vector&>> left_foot_points = { left_heel, left_toe}; @@ -466,11 +466,11 @@ int DoMain(int argc, char* argv[]) { auto left_toe_angle_traj_gen = builder.AddSystem( - plant_w_spr, context_w_spr.get(), l_toe_trajectory, + plant, plant_context.get(), l_toe_trajectory, pos_map["toe_left"], left_foot_points, "left_toe_angle_traj"); auto right_toe_angle_traj_gen = builder.AddSystem( - plant_w_spr, context_w_spr.get(), r_toe_trajectory, + plant, plant_context.get(), r_toe_trajectory, pos_map["toe_right"], right_foot_points, "right_toe_angle_traj"); left_toe_angle_tracking_data->AddStateAndJointToTrack( diff --git a/examples/Cassie/run_osc_running_controller.cc b/examples/Cassie/run_osc_running_controller.cc index 10e7d2dbae..ef527340aa 100644 --- a/examples/Cassie/run_osc_running_controller.cc +++ b/examples/Cassie/run_osc_running_controller.cc @@ -314,16 +314,16 @@ int DoMain(int argc, char* argv[]) { auto pelvis_tracking_data = std::make_unique( "pelvis_trans_traj", osc_gains.K_p_pelvis, osc_gains.K_d_pelvis, - osc_gains.W_pelvis, plant, plant); + osc_gains.W_pelvis, plant); auto stance_foot_tracking_data = std::make_unique( "stance_ft_traj", osc_gains.K_p_swing_foot, osc_gains.K_d_swing_foot, - osc_gains.W_swing_foot, plant, plant); + osc_gains.W_swing_foot, plant); auto left_foot_tracking_data = std::make_unique( "left_ft_traj", osc_gains.K_p_swing_foot, osc_gains.K_d_swing_foot, - osc_gains.W_swing_foot, plant, plant); + osc_gains.W_swing_foot, plant); auto right_foot_tracking_data = std::make_unique( "right_ft_traj", osc_gains.K_p_swing_foot, osc_gains.K_d_swing_foot, - osc_gains.W_swing_foot, plant, plant); + osc_gains.W_swing_foot, plant); pelvis_tracking_data->AddStateAndPointToTrack(RunningFsmState::kLeftStance, "pelvis"); pelvis_tracking_data->AddStateAndPointToTrack(RunningFsmState::kRightStance, @@ -349,11 +349,11 @@ int DoMain(int argc, char* argv[]) { auto left_foot_yz_tracking_data = std::make_unique( "left_ft_traj", osc_gains.K_p_swing_foot, osc_gains.K_d_swing_foot, - osc_gains.W_swing_foot, plant, plant); + osc_gains.W_swing_foot, plant); auto right_foot_yz_tracking_data = std::make_unique( "right_ft_traj", osc_gains.K_p_swing_foot, osc_gains.K_d_swing_foot, - osc_gains.W_swing_foot, plant, plant); + osc_gains.W_swing_foot, plant); left_foot_yz_tracking_data->AddStateAndPointToTrack( RunningFsmState::kLeftFlight, "toe_left"); right_foot_yz_tracking_data->AddStateAndPointToTrack( @@ -361,10 +361,10 @@ int DoMain(int argc, char* argv[]) { auto left_hip_tracking_data = std::make_unique( "left_hip_traj", osc_gains.K_p_swing_foot, osc_gains.K_d_swing_foot, - osc_gains.W_swing_foot, plant, plant); + osc_gains.W_swing_foot, plant); auto right_hip_tracking_data = std::make_unique( "right_hip_traj", osc_gains.K_p_swing_foot, osc_gains.K_d_swing_foot, - osc_gains.W_swing_foot, plant, plant); + osc_gains.W_swing_foot, plant); left_hip_tracking_data->AddStateAndPointToTrack( RunningFsmState::kRightStance, "pelvis"); right_hip_tracking_data->AddStateAndPointToTrack( @@ -381,11 +381,11 @@ int DoMain(int argc, char* argv[]) { auto left_hip_yz_tracking_data = std::make_unique( "left_hip_traj", osc_gains.K_p_swing_foot, osc_gains.K_d_swing_foot, - osc_gains.W_swing_foot, plant, plant); + osc_gains.W_swing_foot, plant); auto right_hip_yz_tracking_data = std::make_unique( "right_hip_traj", osc_gains.K_p_swing_foot, osc_gains.K_d_swing_foot, - osc_gains.W_swing_foot, plant, plant); + osc_gains.W_swing_foot, plant); left_hip_yz_tracking_data->AddStateAndPointToTrack( RunningFsmState::kLeftFlight, "pelvis"); right_hip_yz_tracking_data->AddStateAndPointToTrack( @@ -394,17 +394,17 @@ int DoMain(int argc, char* argv[]) { auto left_foot_rel_tracking_data = std::make_unique( "left_ft_traj", osc_gains.K_p_swing_foot, osc_gains.K_d_swing_foot, - osc_gains.W_swing_foot, plant, plant, left_foot_tracking_data.get(), + osc_gains.W_swing_foot, plant, left_foot_tracking_data.get(), left_hip_tracking_data.get()); auto right_foot_rel_tracking_data = std::make_unique( "right_ft_traj", osc_gains.K_p_swing_foot, osc_gains.K_d_swing_foot, - osc_gains.W_swing_foot, plant, plant, right_foot_tracking_data.get(), + osc_gains.W_swing_foot, plant, right_foot_tracking_data.get(), right_hip_tracking_data.get()); auto pelvis_trans_rel_tracking_data = std::make_unique( "pelvis_trans_traj", osc_gains.K_p_pelvis, osc_gains.K_d_pelvis, - osc_gains.W_pelvis, plant, plant, pelvis_tracking_data.get(), + osc_gains.W_pelvis, plant, pelvis_tracking_data.get(), stance_foot_tracking_data.get()); left_foot_rel_tracking_data->SetViewFrame(pelvis_view_frame); right_foot_rel_tracking_data->SetViewFrame(pelvis_view_frame); @@ -436,7 +436,7 @@ int DoMain(int argc, char* argv[]) { auto pelvis_rot_tracking_data = std::make_unique( "pelvis_rot_traj", osc_gains.K_p_pelvis_rot, osc_gains.K_d_pelvis_rot, - osc_gains.W_pelvis_rot, plant, plant); + osc_gains.W_pelvis_rot, plant); pelvis_rot_tracking_data->AddStateAndFrameToTrack( RunningFsmState::kLeftStance, "pelvis"); pelvis_rot_tracking_data->AddStateAndFrameToTrack( @@ -466,10 +466,10 @@ int DoMain(int argc, char* argv[]) { // Swing toe joint tracking auto left_toe_angle_tracking_data = std::make_unique( "left_toe_angle_traj", osc_gains.K_p_swing_toe, osc_gains.K_d_swing_toe, - osc_gains.W_swing_toe, plant, plant); + osc_gains.W_swing_toe, plant); auto right_toe_angle_tracking_data = std::make_unique( "right_toe_angle_traj", osc_gains.K_p_swing_toe, osc_gains.K_d_swing_toe, - osc_gains.W_swing_toe, plant, plant); + osc_gains.W_swing_toe, plant); left_toe_angle_tracking_data->AddStateAndJointToTrack( RunningFsmState::kRightStance, "toe_left", "toe_leftdot"); left_toe_angle_tracking_data->AddStateAndJointToTrack( @@ -486,10 +486,10 @@ int DoMain(int argc, char* argv[]) { // Swing hip yaw joint tracking auto left_hip_yaw_tracking_data = std::make_unique( "hip_yaw_left_traj", osc_gains.K_p_hip_yaw, osc_gains.K_d_hip_yaw, - osc_gains.W_hip_yaw, plant, plant); + osc_gains.W_hip_yaw, plant); auto right_hip_yaw_tracking_data = std::make_unique( "hip_yaw_right_traj", osc_gains.K_p_hip_yaw, osc_gains.K_d_hip_yaw, - osc_gains.W_hip_yaw, plant, plant); + osc_gains.W_hip_yaw, plant); left_hip_yaw_tracking_data->AddJointToTrack("hip_yaw_left", "hip_yaw_leftdot"); right_hip_yaw_tracking_data->AddJointToTrack("hip_yaw_right", diff --git a/examples/Cassie/run_osc_standing_controller.cc b/examples/Cassie/run_osc_standing_controller.cc index f63b104c36..5bd3e92128 100644 --- a/examples/Cassie/run_osc_standing_controller.cc +++ b/examples/Cassie/run_osc_standing_controller.cc @@ -71,21 +71,21 @@ int DoMain(int argc, char* argv[]) { gflags::ParseCommandLineFlags(&argc, &argv, true); // Build Cassie MBP - drake::multibody::MultibodyPlant plant_w_springs(0.0); - AddCassieMultibody(&plant_w_springs, nullptr, true /*floating base*/, + drake::multibody::MultibodyPlant plant(0.0); + AddCassieMultibody(&plant, nullptr, true /*floating base*/, "examples/Cassie/urdf/cassie_v2.urdf", true /*spring model*/, false /*loop closure*/); - plant_w_springs.Finalize(); + plant.Finalize(); - auto context_w_spr = plant_w_springs.CreateDefaultContext(); + auto plant_context = plant.CreateDefaultContext(); // Get contact frames and position (doesn't matter whether we use - // plant_w_springs or plant_w_springs because the contact frames exit in both + // plant or plant because the contact frames exit in both // plants) - auto left_toe = LeftToeFront(plant_w_springs); - auto left_heel = LeftToeRear(plant_w_springs); - auto right_toe = RightToeFront(plant_w_springs); - auto right_heel = RightToeRear(plant_w_springs); + auto left_toe = LeftToeFront(plant); + auto left_heel = LeftToeRear(plant); + auto right_toe = RightToeFront(plant); + auto right_heel = RightToeRear(plant); // Build the controller diagram DiagramBuilder builder; @@ -105,7 +105,7 @@ int DoMain(int argc, char* argv[]) { // Create state receiver. auto state_receiver = - builder.AddSystem(plant_w_springs); + builder.AddSystem(plant); auto cassie_out_receiver = builder.AddSystem(LcmSubscriberSystem::Make( @@ -118,7 +118,7 @@ int DoMain(int argc, char* argv[]) { builder.AddSystem(LcmPublisherSystem::Make( FLAGS_channel_u, &lcm_local, TriggerTypeSet({TriggerType::kForced}))); auto command_sender = - builder.AddSystem(plant_w_springs); + builder.AddSystem(plant); builder.Connect(command_sender->get_output_port(0), command_pub->get_input_port()); @@ -133,11 +133,11 @@ int DoMain(int argc, char* argv[]) { std::vector&>> feet_contact_points = {left_toe, left_heel, right_toe, right_heel}; auto com_traj_generator = builder.AddSystem( - plant_w_springs, context_w_spr.get(), feet_contact_points, FLAGS_height, + plant, plant_context.get(), feet_contact_points, FLAGS_height, FLAGS_use_radio); auto pelvis_rot_traj_generator = builder.AddSystem( - plant_w_springs, context_w_spr.get(), feet_contact_points, + plant, plant_context.get(), feet_contact_points, "pelvis_rot_traj"); builder.Connect(state_receiver->get_output_port(0), com_traj_generator->get_input_port_state()); @@ -152,31 +152,31 @@ int DoMain(int argc, char* argv[]) { // Create Operational space control auto osc = builder.AddSystem( - plant_w_springs, context_w_spr.get(), false); + plant, plant_context.get(), false); // Distance constraint - multibody::KinematicEvaluatorSet evaluators(plant_w_springs); - auto left_loop = LeftLoopClosureEvaluator(plant_w_springs); - auto right_loop = RightLoopClosureEvaluator(plant_w_springs); + multibody::KinematicEvaluatorSet evaluators(plant); + auto left_loop = LeftLoopClosureEvaluator(plant); + auto right_loop = RightLoopClosureEvaluator(plant); evaluators.add_evaluator(&left_loop); evaluators.add_evaluator(&right_loop); - auto pos_idx_map = multibody::MakeNameToPositionsMap(plant_w_springs); - auto vel_idx_map = multibody::MakeNameToVelocitiesMap(plant_w_springs); + auto pos_idx_map = multibody::MakeNameToPositionsMap(plant); + auto vel_idx_map = multibody::MakeNameToVelocitiesMap(plant); auto left_fixed_knee_spring = - multibody::FixedJointEvaluator(plant_w_springs, pos_idx_map.at + multibody::FixedJointEvaluator(plant, pos_idx_map.at ("knee_joint_left"), vel_idx_map.at("knee_joint_leftdot"), 0); auto right_fixed_knee_spring = - multibody::FixedJointEvaluator(plant_w_springs, pos_idx_map.at + multibody::FixedJointEvaluator(plant, pos_idx_map.at ("knee_joint_right"), vel_idx_map.at("knee_joint_rightdot"), 0); auto left_fixed_ankle_spring = - multibody::FixedJointEvaluator(plant_w_springs, pos_idx_map.at + multibody::FixedJointEvaluator(plant, pos_idx_map.at ("ankle_spring_joint_left"), vel_idx_map.at("ankle_spring_joint_leftdot"), 0); auto right_fixed_ankle_spring = - multibody::FixedJointEvaluator(plant_w_springs, pos_idx_map.at + multibody::FixedJointEvaluator(plant, pos_idx_map.at ("ankle_spring_joint_right"), vel_idx_map.at("ankle_spring_joint_rightdot"), 0); evaluators.add_evaluator(&left_fixed_knee_spring); @@ -196,16 +196,16 @@ int DoMain(int argc, char* argv[]) { osc->SetContactFriction(mu); // Add contact points auto left_toe_evaluator = multibody::WorldPointEvaluator( - plant_w_springs, left_toe.first, left_toe.second, Matrix3d::Identity(), + plant, left_toe.first, left_toe.second, Matrix3d::Identity(), Vector3d::Zero(), {1, 2}); auto left_heel_evaluator = multibody::WorldPointEvaluator( - plant_w_springs, left_heel.first, left_heel.second, Matrix3d::Identity(), + plant, left_heel.first, left_heel.second, Matrix3d::Identity(), Vector3d::Zero(), {0, 1, 2}); auto right_toe_evaluator = multibody::WorldPointEvaluator( - plant_w_springs, right_toe.first, right_toe.second, Matrix3d::Identity(), + plant, right_toe.first, right_toe.second, Matrix3d::Identity(), Vector3d::Zero(), {1, 2}); auto right_heel_evaluator = multibody::WorldPointEvaluator( - plant_w_springs, right_heel.first, right_heel.second, + plant, right_heel.first, right_heel.second, Matrix3d::Identity(), Vector3d::Zero(), {0, 1, 2}); osc->AddContactPoint( @@ -225,27 +225,21 @@ int DoMain(int argc, char* argv[]) { unique_ptr>(&right_heel_evaluator) ); // Cost - int n_v = plant_w_springs.num_velocities(); + int n_v = plant.num_velocities(); MatrixXd Q_accel = gains.w_accel * MatrixXd::Identity(n_v, n_v); osc->SetAccelerationCostWeights(Q_accel); - // Center of mass tracking - // Weighting x-y higher than z, as they are more important to balancing - // ComTrackingData center_of_mass_traj("com_traj", K_p_com, K_d_com, - // W_com * FLAGS_cost_weight_multiplier, - // plant_w_springs, plant_w_springs); - auto center_of_mass_traj = std::make_unique( + + auto pelvis_tracking_data = std::make_unique( "com_traj", osc_gains.K_p_pelvis, osc_gains.K_d_pelvis, - osc_gains.W_pelvis * FLAGS_cost_weight_multiplier, plant_w_springs, - plant_w_springs); - center_of_mass_traj->AddPointToTrack("pelvis"); + osc_gains.W_pelvis, plant); + pelvis_tracking_data->AddPointToTrack("pelvis"); double cutoff_freq = 5; // in Hz double tau = 1 / (2 * M_PI * cutoff_freq); - center_of_mass_traj->SetLowPassFilter(tau, {1}); - osc->AddTrackingData(std::move(center_of_mass_traj)); + pelvis_tracking_data->SetLowPassFilter(tau, {1}); + osc->AddTrackingData(std::move(pelvis_tracking_data)); auto pelvis_rot_tracking_data = std::make_unique( "pelvis_rot_traj", osc_gains.K_p_pelvis_rot, osc_gains.K_d_pelvis_rot, - osc_gains.W_pelvis_rot * FLAGS_cost_weight_multiplier, plant_w_springs, - plant_w_springs); + osc_gains.W_pelvis_rot, plant); pelvis_rot_tracking_data->AddFrameToTrack("pelvis"); osc->AddTrackingData(std::move(pelvis_rot_tracking_data)); @@ -259,11 +253,11 @@ int DoMain(int argc, char* argv[]) { auto left_hip_yaw_traj = std::make_unique( "left_hip_yaw_traj", hip_yaw_kp * MatrixXd::Ones(1, 1), hip_yaw_kd * MatrixXd::Ones(1, 1), w_hip_yaw * MatrixXd::Ones(1, 1), - plant_w_springs, plant_w_springs); + plant); auto right_hip_yaw_traj = std::make_unique( "right_hip_yaw_traj", hip_yaw_kp * MatrixXd::Ones(1, 1), hip_yaw_kd * MatrixXd::Ones(1, 1), w_hip_yaw * MatrixXd::Ones(1, 1), - plant_w_springs, plant_w_springs); + plant); left_hip_yaw_traj->AddJointToTrack("hip_yaw_left", "hip_yaw_leftdot"); osc->AddConstTrackingData(std::move(left_hip_yaw_traj), VectorXd::Zero(1)); right_hip_yaw_traj->AddJointToTrack("hip_yaw_right", "hip_yaw_rightdot"); diff --git a/examples/Cassie/run_osc_walking_controller_alip.cc b/examples/Cassie/run_osc_walking_controller_alip.cc index db5d8a8392..a18ff3a4f4 100644 --- a/examples/Cassie/run_osc_walking_controller_alip.cc +++ b/examples/Cassie/run_osc_walking_controller_alip.cc @@ -95,45 +95,43 @@ int DoMain(int argc, char* argv[]) { auto gains = drake::yaml::LoadYamlFile(FLAGS_gains_filename); // Build Cassie MBP - drake::multibody::MultibodyPlant plant_w_spr(0.0); - AddCassieMultibody(&plant_w_spr, nullptr, true /*floating base*/, + drake::multibody::MultibodyPlant plant(0.0); + AddCassieMultibody(&plant, nullptr, true /*floating base*/, "examples/Cassie/urdf/cassie_v2.urdf", true /*spring model*/, false /*loop closure*/); - plant_w_spr.Finalize(); + plant.Finalize(); - auto context_w_spr = plant_w_spr.CreateDefaultContext(); + auto plant_context = plant.CreateDefaultContext(); // Build the controller diagram DiagramBuilder builder; drake::lcm::DrakeLcm lcm_local("udpm://239.255.76.67:7667?ttl=0"); - - // Get contact frames and position (doesn't matter whether we use - // plant_w_spr or plant_wospr because the contact frames exit in both - // plants) - auto left_toe = LeftToeFront(plant_w_spr); - auto left_heel = LeftToeRear(plant_w_spr); - auto right_toe = RightToeFront(plant_w_spr); - auto right_heel = RightToeRear(plant_w_spr); + + // Contact points + auto left_toe = LeftToeFront(plant); + auto left_heel = LeftToeRear(plant); + auto right_toe = RightToeFront(plant); + auto right_heel = RightToeRear(plant); // Get body frames and points Vector3d center_of_pressure = left_heel.first + gains.contact_point_pos * (left_toe.first - left_heel.first); auto left_toe_mid = std::pair&>( - center_of_pressure, plant_w_spr.GetFrameByName("toe_left")); + center_of_pressure, plant.GetFrameByName("toe_left")); auto right_toe_mid = std::pair&>( - center_of_pressure, plant_w_spr.GetFrameByName("toe_right")); + center_of_pressure, plant.GetFrameByName("toe_right")); auto left_toe_origin = std::pair&>( - Vector3d::Zero(), plant_w_spr.GetFrameByName("toe_left")); + Vector3d::Zero(), plant.GetFrameByName("toe_left")); auto right_toe_origin = std::pair&>( - Vector3d::Zero(), plant_w_spr.GetFrameByName("toe_right")); + Vector3d::Zero(), plant.GetFrameByName("toe_right")); // Create state receiver. auto state_receiver = - builder.AddSystem(plant_w_spr); + builder.AddSystem(plant); auto pelvis_filt = builder.AddSystem( - plant_w_spr, gains.pelvis_xyz_vel_filter_tau); + plant, gains.pelvis_xyz_vel_filter_tau); builder.Connect(*state_receiver, *pelvis_filt); if (FLAGS_publish_filtered_state) { @@ -149,16 +147,16 @@ int DoMain(int argc, char* argv[]) { builder.AddSystem(LcmPublisherSystem::Make( FLAGS_channel_u, &lcm_local, TriggerTypeSet({TriggerType::kForced}))); auto command_sender = - builder.AddSystem(plant_w_spr); + builder.AddSystem(plant); builder.Connect(command_sender->get_output_port(0), command_pub->get_input_port()); // Add emulator for floating base drift Eigen::VectorXd drift_mean = - Eigen::VectorXd::Zero(plant_w_spr.num_positions()); + Eigen::VectorXd::Zero(plant.num_positions()); Eigen::MatrixXd drift_cov = Eigen::MatrixXd::Zero( - plant_w_spr.num_positions(), plant_w_spr.num_positions()); + plant.num_positions(), plant.num_positions()); drift_cov(4, 4) = FLAGS_drift_rate; // x drift_cov(5, 5) = FLAGS_drift_rate; // y drift_cov(6, 6) = FLAGS_drift_rate; // z @@ -166,7 +164,7 @@ int DoMain(int argc, char* argv[]) { // changing SimulatorDrift. auto simulator_drift = - builder.AddSystem(plant_w_spr, drift_mean, drift_cov); + builder.AddSystem(plant, drift_mean, drift_cov); builder.Connect(pelvis_filt->get_output_port(0), simulator_drift->get_input_port_state()); @@ -181,7 +179,7 @@ int DoMain(int argc, char* argv[]) { cassie::osc::HighLevelCommand* high_level_command; if (FLAGS_use_radio) { high_level_command = builder.AddSystem( - plant_w_spr, context_w_spr.get(), gains.vel_scale_rot, + plant, plant_context.get(), gains.vel_scale_rot, gains.vel_scale_trans_sagital, gains.vel_scale_trans_lateral, 0.4); auto cassie_out_receiver = builder.AddSystem(LcmSubscriberSystem::Make( @@ -191,7 +189,7 @@ int DoMain(int argc, char* argv[]) { high_level_command->get_input_port_radio()); } else { high_level_command = builder.AddSystem( - plant_w_spr, context_w_spr.get(), gains.kp_yaw, gains.kd_yaw, + plant, plant_context.get(), gains.kp_yaw, gains.kd_yaw, gains.vel_max_yaw, gains.kp_pos_sagital, gains.kd_pos_sagital, gains.vel_max_sagital, gains.kp_pos_lateral, gains.kd_pos_lateral, gains.vel_max_lateral, gains.target_pos_offset, global_target_position, @@ -202,7 +200,7 @@ int DoMain(int argc, char* argv[]) { // Create heading traj generator auto head_traj_gen = builder.AddSystem( - plant_w_spr, context_w_spr.get()); + plant, plant_context.get()); builder.Connect(simulator_drift->get_output_port(0), head_traj_gen->get_state_input_port()); builder.Connect(high_level_command->get_output_port_yaw(), @@ -225,7 +223,7 @@ int DoMain(int argc, char* argv[]) { right_support_duration, double_support_duration}; auto fsm = builder.AddSystem( - plant_w_spr, fsm_states, state_durations); + plant, fsm_states, state_durations); builder.Connect(simulator_drift->get_output_port(0), fsm->get_state_input_port()); @@ -234,7 +232,7 @@ int DoMain(int argc, char* argv[]) { right_stance_state}; auto liftoff_event_time = builder.AddSystem( - plant_w_spr, single_support_states); + plant, single_support_states); liftoff_event_time->set_name("liftoff_time"); builder.Connect(fsm->get_output_port(0), liftoff_event_time->get_input_port_fsm()); @@ -244,7 +242,7 @@ int DoMain(int argc, char* argv[]) { post_right_double_support_state}; auto touchdown_event_time = builder.AddSystem( - plant_w_spr, double_support_states); + plant, double_support_states); touchdown_event_time->set_name("touchdown_time"); builder.Connect(fsm->get_output_port(0), touchdown_event_time->get_input_port_fsm()); @@ -272,7 +270,7 @@ int DoMain(int argc, char* argv[]) { contact_points_in_each_state.push_back({right_toe_mid}); auto alip_traj_generator = builder.AddSystem( - plant_w_spr, context_w_spr.get(), desired_com_height, + plant, plant_context.get(), desired_com_height, unordered_fsm_states, unordered_state_durations, contact_points_in_each_state, gains.Q_alip_kalman_filter.asDiagonal(), gains.R_alip_kalman_filter.asDiagonal()); @@ -299,7 +297,7 @@ int DoMain(int argc, char* argv[]) { left_toe_origin, right_toe_origin}; auto swing_ft_traj_generator = builder.AddSystem( - plant_w_spr, context_w_spr.get(), left_right_support_fsm_states, + plant, plant_context.get(), left_right_support_fsm_states, left_right_support_state_durations, left_right_foot, "pelvis", double_support_duration, gains.mid_foot_height, gains.final_foot_height, gains.final_foot_velocity_z, @@ -317,18 +315,18 @@ int DoMain(int argc, char* argv[]) { swing_ft_traj_generator->get_input_port_vdes()); // Swing toe joint trajectory - map pos_map = multibody::MakeNameToPositionsMap(plant_w_spr); + map pos_map = multibody::MakeNameToPositionsMap(plant); vector&>> left_foot_points = { left_heel, left_toe}; vector&>> right_foot_points = { right_heel, right_toe}; auto left_toe_angle_traj_gen = builder.AddSystem( - plant_w_spr, context_w_spr.get(), pos_map["toe_left"], + plant, plant_context.get(), pos_map["toe_left"], left_foot_points, "left_toe_angle_traj"); auto right_toe_angle_traj_gen = builder.AddSystem( - plant_w_spr, context_w_spr.get(), pos_map["toe_right"], + plant, plant_context.get(), pos_map["toe_right"], right_foot_points, "right_toe_angle_traj"); builder.Connect(pelvis_filt->get_output_port(0), left_toe_angle_traj_gen->get_input_port_state()); @@ -337,23 +335,24 @@ int DoMain(int argc, char* argv[]) { // Create Operational space control auto osc = builder.AddSystem( - plant_w_spr, context_w_spr.get(), true); + plant, plant_context.get(), true); // Cost - int n_v = plant_w_spr.num_velocities(); - int n_u = plant_w_spr.num_actuators(); + int n_v = plant.num_velocities(); + int n_u = plant.num_actuators(); MatrixXd Q_accel = gains.w_accel * MatrixXd::Identity(n_v, n_v); osc->SetAccelerationCostWeights(Q_accel); osc->SetInputSmoothingCostWeights(gains.w_input_reg * MatrixXd::Identity(n_u, n_u)); // Constraints in OSC - multibody::KinematicEvaluatorSet evaluators(plant_w_spr); + multibody::KinematicEvaluatorSet evaluators(plant); // 1. fourbar constraint - auto left_loop = LeftLoopClosureEvaluator(plant_w_spr); - auto right_loop = RightLoopClosureEvaluator(plant_w_spr); + auto left_loop = LeftLoopClosureEvaluator(plant); + auto right_loop = RightLoopClosureEvaluator(plant); evaluators.add_evaluator(&left_loop); evaluators.add_evaluator(&right_loop); + // 2. fixed spring constraint // Note that we set the position value to 0, but this is not used in OSC, // because OSC constraint only use JdotV and J. @@ -361,19 +360,19 @@ int DoMain(int argc, char* argv[]) { std::unique_ptr> right_fixed_knee_spring; std::unique_ptr> left_fixed_ankle_spring; std::unique_ptr> right_fixed_ankle_spring; - auto pos_idx_map = multibody::MakeNameToPositionsMap(plant_w_spr); - auto vel_idx_map = multibody::MakeNameToVelocitiesMap(plant_w_spr); + auto pos_idx_map = multibody::MakeNameToPositionsMap(plant); + auto vel_idx_map = multibody::MakeNameToVelocitiesMap(plant); left_fixed_knee_spring = std::make_unique>( - plant_w_spr, pos_idx_map.at("knee_joint_left"), + plant, pos_idx_map.at("knee_joint_left"), vel_idx_map.at("knee_joint_leftdot"), 0); right_fixed_knee_spring = std::make_unique>( - plant_w_spr, pos_idx_map.at("knee_joint_right"), + plant, pos_idx_map.at("knee_joint_right"), vel_idx_map.at("knee_joint_rightdot"), 0); left_fixed_ankle_spring = std::make_unique>( - plant_w_spr, pos_idx_map.at("ankle_spring_joint_left"), + plant, pos_idx_map.at("ankle_spring_joint_left"), vel_idx_map.at("ankle_spring_joint_leftdot"), 0); right_fixed_ankle_spring = std::make_unique>( - plant_w_spr, pos_idx_map.at("ankle_spring_joint_right"), + plant, pos_idx_map.at("ankle_spring_joint_right"), vel_idx_map.at("ankle_spring_joint_rightdot"), 0); evaluators.add_evaluator(left_fixed_knee_spring.get()); evaluators.add_evaluator(right_fixed_knee_spring.get()); @@ -383,25 +382,23 @@ int DoMain(int argc, char* argv[]) { std::unique_ptr>(&evaluators)); // Soft constraint - // w_contact_relax shouldn't be too big, cause we want tracking error to be - // important osc->SetContactSoftConstraintWeight(gains.w_soft_constraint); - // Friction coefficient osc->SetContactFriction(gains.mu); + // Add contact points (The position doesn't matter. It's not used in OSC) - const auto& pelvis = plant_w_spr.GetBodyByName("pelvis"); + const auto& pelvis = plant.GetBodyByName("pelvis"); multibody::WorldYawViewFrame view_frame(pelvis); auto left_toe_evaluator = multibody::WorldPointEvaluator( - plant_w_spr, left_toe.first, left_toe.second, view_frame, + plant, left_toe.first, left_toe.second, view_frame, Matrix3d::Identity(), Vector3d::Zero(), {1, 2}); auto left_heel_evaluator = multibody::WorldPointEvaluator( - plant_w_spr, left_heel.first, left_heel.second, view_frame, + plant, left_heel.first, left_heel.second, view_frame, Matrix3d::Identity(), Vector3d::Zero(), {0, 1, 2}); auto right_toe_evaluator = multibody::WorldPointEvaluator( - plant_w_spr, right_toe.first, right_toe.second, view_frame, + plant, right_toe.first, right_toe.second, view_frame, Matrix3d::Identity(), Vector3d::Zero(), {1, 2}); auto right_heel_evaluator = multibody::WorldPointEvaluator( - plant_w_spr, right_heel.first, right_heel.second, view_frame, + plant, right_heel.first, right_heel.second, view_frame, Matrix3d::Identity(), Vector3d::Zero(), {0, 1, 2}); osc->AddContactPoint( @@ -450,23 +447,23 @@ int DoMain(int argc, char* argv[]) { auto swing_foot_data = std::make_unique ( "swing_ft_data", gains.K_p_swing_foot, gains.K_d_swing_foot, - gains.W_swing_foot, plant_w_spr, plant_w_spr); + gains.W_swing_foot, plant); swing_foot_data->AddStateAndPointToTrack(left_stance_state, "toe_right"); swing_foot_data->AddStateAndPointToTrack(right_stance_state, "toe_left"); - auto vel_map = MakeNameToVelocitiesMap(plant_w_spr); + auto vel_map = MakeNameToVelocitiesMap(plant); - auto com_data = std::make_unique ("com_data", gains.K_p_swing_foot, - gains.K_d_swing_foot, gains.W_swing_foot, - plant_w_spr, plant_w_spr); + auto com_data = std::make_unique ( + "com_data", gains.K_p_swing_foot, gains.K_d_swing_foot, + gains.W_swing_foot, plant); com_data->AddFiniteStateToTrack(left_stance_state); com_data->AddFiniteStateToTrack(right_stance_state); auto swing_ft_traj_local = std::make_unique ( "swing_ft_traj", gains.K_p_swing_foot, gains.K_d_swing_foot, - gains.W_swing_foot, plant_w_spr, plant_w_spr, swing_foot_data.get(), + gains.W_swing_foot, plant, swing_foot_data.get(), com_data.get()); auto pelvis_view_frame = std::make_shared>( - plant_w_spr.GetBodyByName("pelvis")); + plant.GetBodyByName("pelvis")); swing_ft_traj_local->SetViewFrame(pelvis_view_frame); swing_ft_traj_local->SetTimeVaryingPDGainMultiplier( @@ -475,9 +472,10 @@ int DoMain(int argc, char* argv[]) { swing_ft_accel_gain_multiplier_gain_multiplier); osc->AddTrackingData(std::move(swing_ft_traj_local)); - auto center_of_mass_traj = std::make_unique ("alip_com_traj", gains.K_p_com, - gains.K_d_com, gains.W_com, plant_w_spr, - plant_w_spr); + auto center_of_mass_traj = std::make_unique( + "alip_com_traj", gains.K_p_com, gains.K_d_com, gains.W_com, plant); + center_of_mass_traj->SetViewFrame(pelvis_view_frame); + // FiniteStatesToTrack cannot be empty center_of_mass_traj->AddFiniteStateToTrack(-1); osc->AddTrackingData(std::move(center_of_mass_traj)); @@ -485,13 +483,13 @@ int DoMain(int argc, char* argv[]) { // Pelvis rotation tracking (pitch and roll) auto pelvis_balance_traj = std::make_unique ( "pelvis_balance_traj", gains.K_p_pelvis_balance, gains.K_d_pelvis_balance, - gains.W_pelvis_balance, plant_w_spr, plant_w_spr); + gains.W_pelvis_balance, plant); pelvis_balance_traj->AddFrameToTrack("pelvis"); osc->AddTrackingData(std::move(pelvis_balance_traj)); // Pelvis rotation tracking (yaw) auto pelvis_heading_traj = std::make_unique ( "pelvis_heading_traj", gains.K_p_pelvis_heading, gains.K_d_pelvis_heading, - gains.W_pelvis_heading, plant_w_spr, plant_w_spr); + gains.W_pelvis_heading, plant); pelvis_heading_traj->AddFrameToTrack("pelvis"); osc->AddTrackingData(std::move(pelvis_heading_traj), gains.period_of_no_heading_control); @@ -499,10 +497,10 @@ int DoMain(int argc, char* argv[]) { // Swing toe joint tracking auto swing_toe_traj_left = std::make_unique ( "left_toe_angle_traj", gains.K_p_swing_toe, gains.K_d_swing_toe, - gains.W_swing_toe, plant_w_spr, plant_w_spr); + gains.W_swing_toe, plant); auto swing_toe_traj_right = std::make_unique ( "right_toe_angle_traj", gains.K_p_swing_toe, gains.K_d_swing_toe, - gains.W_swing_toe, plant_w_spr, plant_w_spr); + gains.W_swing_toe, plant); swing_toe_traj_right->AddStateAndJointToTrack(left_stance_state, "toe_right", "toe_rightdot"); swing_toe_traj_left->AddStateAndJointToTrack(right_stance_state, "toe_left", @@ -517,7 +515,7 @@ int DoMain(int argc, char* argv[]) { // Swing hip yaw joint tracking auto swing_hip_yaw_traj = std::make_unique ( "swing_hip_yaw_traj", gains.K_p_hip_yaw, gains.K_d_hip_yaw, - gains.W_hip_yaw, plant_w_spr, plant_w_spr); + gains.W_hip_yaw, plant); swing_hip_yaw_traj->AddStateAndJointToTrack(left_stance_state, "hip_yaw_right", "hip_yaw_rightdot"); swing_hip_yaw_traj->AddStateAndJointToTrack(right_stance_state, "hip_yaw_left", diff --git a/examples/impact_invariant_control/run_joint_space_walking_controller.cc b/examples/impact_invariant_control/run_joint_space_walking_controller.cc index 85ab093d5c..6293267869 100644 --- a/examples/impact_invariant_control/run_joint_space_walking_controller.cc +++ b/examples/impact_invariant_control/run_joint_space_walking_controller.cc @@ -172,7 +172,7 @@ int DoMain(int argc, char* argv[]) { MatrixXd K_p = gains.JointKp[joint_idx] * MatrixXd::Identity(1, 1); MatrixXd K_d = gains.JointKd[joint_idx] * MatrixXd::Identity(1, 1); joint_tracking_data_vec.push_back(std::make_unique( - joint_name + "_traj", K_p, K_d, W, plant, plant)); + joint_name + "_traj", K_p, K_d, W, plant)); joint_tracking_data_vec[joint_idx]->AddJointToTrack(joint_name, joint_name + "dot"); joint_tracking_data_vec[joint_idx]->SetImpactInvariantProjection(true); @@ -187,6 +187,7 @@ int DoMain(int argc, char* argv[]) { } osc->SetOsqpSolverOptionsFromYaml( FLAGS_osqp_settings); + // Build OSC problem osc->Build(); std::cout << "Built OSC" << std::endl; diff --git a/systems/controllers/osc/com_tracking_data.cc b/systems/controllers/osc/com_tracking_data.cc index 6170a7b38b..1713661d98 100644 --- a/systems/controllers/osc/com_tracking_data.cc +++ b/systems/controllers/osc/com_tracking_data.cc @@ -15,34 +15,40 @@ namespace dairlib::systems::controllers { /**** ComTrackingData ****/ ComTrackingData::ComTrackingData(const string& name, const MatrixXd& K_p, const MatrixXd& K_d, const MatrixXd& W, - const MultibodyPlant& plant_w_spr, - const MultibodyPlant& plant_wo_spr) - : OptionsTrackingData(name, kSpaceDim, kSpaceDim, K_p, K_d, W, plant_w_spr, - plant_wo_spr) { + const MultibodyPlant& plant) + : OptionsTrackingData(name, kSpaceDim, kSpaceDim, K_p, K_d, W, plant) {} + +void ComTrackingData::UpdateY( + const VectorXd& x, const Context& context, + OscTrackingDataState& tracking_data_state) const { + tracking_data_state.y_ = + plant_.CalcCenterOfMassPositionInWorld(context); } -void ComTrackingData::UpdateY(const VectorXd& x_w_spr, - const Context& context_w_spr) { - y_ = plant_w_spr_.CalcCenterOfMassPositionInWorld(context_w_spr); +void ComTrackingData::UpdateYdot( + const VectorXd& x, const Context& context, + OscTrackingDataState& tracking_data_state) const { + tracking_data_state.ydot_ = + plant_.CalcCenterOfMassTranslationalVelocityInWorld( + context); } -void ComTrackingData::UpdateYdot(const VectorXd& x_w_spr, - const Context& context_w_spr) { - ydot_ = plant_w_spr_.CalcCenterOfMassTranslationalVelocityInWorld( - context_w_spr); +void ComTrackingData::UpdateJ( + const VectorXd& x_wo_spr, const Context& context_wo_spr, + OscTrackingDataState& tracking_data_state) const { + tracking_data_state.J_ = + MatrixXd::Zero(kSpaceDim, plant_.num_velocities()); + plant_.CalcJacobianCenterOfMassTranslationalVelocity( + context_wo_spr, JacobianWrtVariable::kV, world_, world_, + &tracking_data_state.J_); } -void ComTrackingData::UpdateJ(const VectorXd& x_wo_spr, - const Context& context_wo_spr) { - J_ = MatrixXd::Zero(kSpaceDim, plant_wo_spr_.num_velocities()); - plant_wo_spr_.CalcJacobianCenterOfMassTranslationalVelocity( - context_wo_spr, JacobianWrtVariable::kV, world_w_spr_, world_w_spr_, &J_); +void ComTrackingData::UpdateJdotV( + const VectorXd& x_wo_spr, const Context& context_wo_spr, + OscTrackingDataState& tracking_data_state) const { + tracking_data_state.JdotV_ = + plant_.CalcBiasCenterOfMassTranslationalAcceleration( + context_wo_spr, JacobianWrtVariable::kV, world_, world_); } -void ComTrackingData::UpdateJdotV(const VectorXd& x_wo_spr, - const Context& context_wo_spr) { - JdotV_ = plant_wo_spr_.CalcBiasCenterOfMassTranslationalAcceleration( - context_wo_spr, JacobianWrtVariable::kV, world_wo_spr_, world_wo_spr_); } - -} \ No newline at end of file diff --git a/systems/controllers/osc/com_tracking_data.h b/systems/controllers/osc/com_tracking_data.h index 56c82c29b3..8d7eb7b770 100644 --- a/systems/controllers/osc/com_tracking_data.h +++ b/systems/controllers/osc/com_tracking_data.h @@ -9,18 +9,21 @@ class ComTrackingData final : public OptionsTrackingData { public: ComTrackingData(const std::string& name, const Eigen::MatrixXd& K_p, const Eigen::MatrixXd& K_d, const Eigen::MatrixXd& W, - const drake::multibody::MultibodyPlant& plant_w_spr, - const drake::multibody::MultibodyPlant& plant_wo_spr); + const drake::multibody::MultibodyPlant& plant); private: - void UpdateY(const Eigen::VectorXd& x_w_spr, - const drake::systems::Context& context_w_spr) final; - void UpdateYdot(const Eigen::VectorXd& x_w_spr, - const drake::systems::Context& context_w_spr) final; - void UpdateJ(const Eigen::VectorXd& x_wo_spr, - const drake::systems::Context& context_wo_spr) final; - void UpdateJdotV(const Eigen::VectorXd& x_wo_spr, - const drake::systems::Context& context_wo_spr) final; + void UpdateY(const Eigen::VectorXd& x, + const drake::systems::Context& context, + OscTrackingDataState& tracking_data_state) const final; + void UpdateYdot(const Eigen::VectorXd& x, + const drake::systems::Context& context, + OscTrackingDataState& tracking_data_state) const final; + void UpdateJ(const Eigen::VectorXd& x, + const drake::systems::Context& context, + OscTrackingDataState& tracking_data_state) const final; + void UpdateJdotV(const Eigen::VectorXd& x, + const drake::systems::Context& context, + OscTrackingDataState& tracking_data_state) const final; void CheckDerivedOscTrackingData() final {} }; diff --git a/systems/controllers/osc/inverse_dynamics_qp.cc b/systems/controllers/osc/inverse_dynamics_qp.cc index abcb0e54f0..87724b61e5 100644 --- a/systems/controllers/osc/inverse_dynamics_qp.cc +++ b/systems/controllers/osc/inverse_dynamics_qp.cc @@ -158,9 +158,14 @@ void InverseDynamicsQp::UpdateDynamics( // TODO (@Brian-Acosta) add option to turn off gravity comp bias = bias - grav; - MatrixXd Jh = holonomic_constraints_->EvalFullJacobian(*context_); - VectorXd - Jh_dot_v = holonomic_constraints_->EvalFullJacobianDotTimesV(*context_); + MatrixXd Jh = MatrixXd::Zero(nh_, nv_); + VectorXd Jh_dot_v = VectorXd::Zero(nh_); + + if (holonomic_constraints_ != nullptr) { + Jh = holonomic_constraints_->EvalFullJacobian(*context_); + Jh_dot_v = holonomic_constraints_->EvalFullJacobianDotTimesV(*context_); + } + MatrixXd Jc_active = MatrixXd::Zero(nc_active_, nv_); VectorXd Jc_active_dot_v = VectorXd::Zero(nc_active_); MatrixXd Jc = MatrixXd::Zero(nc_, nv_); diff --git a/systems/controllers/osc/inverse_dynamics_qp.h b/systems/controllers/osc/inverse_dynamics_qp.h index ce1b19ed0e..b8eee1c7e8 100644 --- a/systems/controllers/osc/inverse_dynamics_qp.h +++ b/systems/controllers/osc/inverse_dynamics_qp.h @@ -235,6 +235,8 @@ class InverseDynamicsQp { return all_costs_.count(name) > 0; } + bool built() const { return built_; } + private: // Multibody Dynamics diff --git a/systems/controllers/osc/joint_space_tracking_data.cc b/systems/controllers/osc/joint_space_tracking_data.cc index 37d0cf3488..d4f838e683 100644 --- a/systems/controllers/osc/joint_space_tracking_data.cc +++ b/systems/controllers/osc/joint_space_tracking_data.cc @@ -20,14 +20,12 @@ using multibody::MakeNameToVelocitiesMap; /**** JointSpaceTrackingData ****/ JointSpaceTrackingData::JointSpaceTrackingData( const string& name, const MatrixXd& K_p, const MatrixXd& K_d, - const MatrixXd& W, const MultibodyPlant& plant_w_spr, - const MultibodyPlant& plant_wo_spr) - : OptionsTrackingData(name, K_p.rows(), K_d.rows(), K_p, K_d, W, - plant_w_spr, plant_wo_spr) { -} + const MatrixXd& W, const MultibodyPlant& plant) + : OptionsTrackingData(name, K_p.rows(), K_d.rows(), K_p, K_d, W, plant) {} -void JointSpaceTrackingData::AddJointToTrack(const std::string& joint_pos_name, - const std::string& joint_vel_name) { +void JointSpaceTrackingData::AddJointToTrack( + const std::string& joint_pos_name, + const std::string& joint_vel_name) { AddStateAndJointToTrack(-1, joint_pos_name, joint_vel_name); } @@ -35,18 +33,13 @@ void JointSpaceTrackingData::AddStateAndJointToTrack( int fsm_state, const std::string& joint_pos_name, const std::string& joint_vel_name) { AddFiniteStateToTrack(fsm_state); - joint_pos_idx_w_spr_[fsm_state] = - { - MakeNameToPositionsMap(plant_w_spr_).at(joint_pos_name)}; - joint_vel_idx_w_spr_[fsm_state] = + joint_pos_idx_[fsm_state] = { - MakeNameToVelocitiesMap(plant_w_spr_).at(joint_vel_name)}; - joint_pos_idx_wo_spr_[fsm_state] = + MakeNameToPositionsMap(plant_).at(joint_pos_name)}; + joint_vel_idx_[fsm_state] = { - MakeNameToPositionsMap(plant_wo_spr_).at(joint_pos_name)}; - joint_vel_idx_wo_spr_[fsm_state] = - { - MakeNameToVelocitiesMap(plant_wo_spr_).at(joint_vel_name)}; + MakeNameToVelocitiesMap(plant_).at(joint_vel_name)}; + } void JointSpaceTrackingData::AddJointsToTrack( @@ -61,76 +54,64 @@ void JointSpaceTrackingData::AddStateAndJointsToTrack( AddFiniteStateToTrack(fsm_state); std::vector ordered_index_set; for (const auto& mem : joint_pos_names) { - ordered_index_set.push_back(MakeNameToPositionsMap(plant_w_spr_).at(mem)); - } - joint_pos_idx_w_spr_[fsm_state] = ordered_index_set; - ordered_index_set.clear(); - for (const auto& mem : joint_vel_names) { - ordered_index_set.push_back(MakeNameToVelocitiesMap(plant_w_spr_).at(mem)); - } - joint_vel_idx_w_spr_[fsm_state] = ordered_index_set; - ordered_index_set.clear(); - for (const auto& mem : joint_pos_names) { - ordered_index_set.push_back(MakeNameToPositionsMap(plant_wo_spr_).at(mem)); + ordered_index_set.push_back(MakeNameToPositionsMap(plant_).at(mem)); } - joint_pos_idx_wo_spr_[fsm_state] = ordered_index_set; + joint_pos_idx_[fsm_state] = ordered_index_set; ordered_index_set.clear(); for (const auto& mem : joint_vel_names) { - ordered_index_set.push_back(MakeNameToVelocitiesMap(plant_wo_spr_).at(mem)); + ordered_index_set.push_back(MakeNameToVelocitiesMap(plant_).at(mem)); } - joint_vel_idx_wo_spr_[fsm_state] = ordered_index_set; + joint_vel_idx_[fsm_state] = ordered_index_set; } -void JointSpaceTrackingData::UpdateY(const VectorXd& x_w_spr, - const Context& context_w_spr) { +void JointSpaceTrackingData::UpdateY( + const VectorXd& x, const Context& context, + OscTrackingDataState& td_state) const { VectorXd y(GetYDim()); for (int i = 0; i < GetYDim(); i++) { - y(i) = x_w_spr(joint_pos_idx_w_spr_.at(fsm_state_).at(i)); + y(i) = x(joint_pos_idx_.at(td_state.fsm_state_).at(i)); } - y_ = y; + td_state.y_ = y; } -void JointSpaceTrackingData::UpdateYdot(const VectorXd& x_w_spr, - const Context& context_w_spr) { +void JointSpaceTrackingData::UpdateYdot( + const VectorXd& x, const Context& context, + OscTrackingDataState& td_state) const { VectorXd ydot(GetYdotDim()); for (int i = 0; i < GetYdotDim(); i++) { - ydot(i) = x_w_spr(plant_w_spr_.num_positions() + - joint_vel_idx_w_spr_.at(fsm_state_).at(i)); + ydot(i) = x(plant_.num_positions() + + joint_vel_idx_.at(td_state.fsm_state_).at(i)); } - ydot_ = ydot; + td_state.ydot_ = ydot; } -void JointSpaceTrackingData::UpdateJ(const VectorXd& x_wo_spr, - const Context& context_wo_spr) { - MatrixXd J = MatrixXd::Zero(GetYdotDim(), plant_wo_spr_.num_velocities()); +void JointSpaceTrackingData::UpdateJ( + const VectorXd& x, const Context& context, + OscTrackingDataState& td_state) const { + MatrixXd J = MatrixXd::Zero(GetYdotDim(), plant_.num_velocities()); for (int i = 0; i < GetYdotDim(); i++) { - J(i, joint_vel_idx_wo_spr_.at(fsm_state_).at(i)) = 1; + J(i, joint_vel_idx_.at(td_state.fsm_state_).at(i)) = 1; } - J_ = J; + td_state.J_ = J; } void JointSpaceTrackingData::UpdateJdotV( - const VectorXd& x_wo_spr, const Context& context_wo_spr) { - JdotV_ = VectorXd::Zero(GetYdotDim()); + const VectorXd& x, const Context& context, + OscTrackingDataState& td_state) const { + td_state.JdotV_ = VectorXd::Zero(GetYdotDim()); } void JointSpaceTrackingData::CheckDerivedOscTrackingData() { - for (auto fsm_joint_pair : joint_pos_idx_w_spr_) { - DRAKE_DEMAND(joint_pos_idx_w_spr_.at(fsm_joint_pair.first).size() == GetYDim()); - DRAKE_DEMAND(joint_pos_idx_wo_spr_.at(fsm_joint_pair.first).size() == GetYDim()); - DRAKE_DEMAND(joint_vel_idx_w_spr_.at(fsm_joint_pair.first).size() == GetYdotDim()); - DRAKE_DEMAND(joint_vel_idx_wo_spr_.at(fsm_joint_pair.first).size() == GetYdotDim()); + for (auto fsm_joint_pair : joint_pos_idx_) { + DRAKE_DEMAND(joint_pos_idx_.at(fsm_joint_pair.first).size() == GetYDim()); + DRAKE_DEMAND(joint_vel_idx_.at(fsm_joint_pair.first).size() == GetYdotDim()); } if (active_fsm_states_.empty()) { - DRAKE_DEMAND(joint_pos_idx_w_spr_.size() == 1); - DRAKE_DEMAND(joint_vel_idx_w_spr_.size() == 1); - DRAKE_DEMAND(joint_pos_idx_wo_spr_.size() == 1); - DRAKE_DEMAND(joint_vel_idx_wo_spr_.size() == 1); + DRAKE_DEMAND(joint_pos_idx_.size() == 1); + DRAKE_DEMAND(joint_vel_idx_.size() == 1); } else { - DRAKE_DEMAND(joint_pos_idx_w_spr_.size() == active_fsm_states_.size()); - DRAKE_DEMAND(joint_vel_idx_w_spr_.size() == active_fsm_states_.size()); - DRAKE_DEMAND(joint_pos_idx_wo_spr_.size() == active_fsm_states_.size()); - DRAKE_DEMAND(joint_vel_idx_wo_spr_.size() == active_fsm_states_.size()); + DRAKE_DEMAND(joint_pos_idx_.size() == active_fsm_states_.size()); + DRAKE_DEMAND(joint_vel_idx_.size() == active_fsm_states_.size()); } } diff --git a/systems/controllers/osc/joint_space_tracking_data.h b/systems/controllers/osc/joint_space_tracking_data.h index 7c407b2e7b..d5a349d84d 100644 --- a/systems/controllers/osc/joint_space_tracking_data.h +++ b/systems/controllers/osc/joint_space_tracking_data.h @@ -23,8 +23,7 @@ class JointSpaceTrackingData final : public OptionsTrackingData { JointSpaceTrackingData( const std::string& name, const Eigen::MatrixXd& K_p, const Eigen::MatrixXd& K_d, const Eigen::MatrixXd& W, - const drake::multibody::MultibodyPlant& plant_w_spr, - const drake::multibody::MultibodyPlant& plant_wo_spr); + const drake::multibody::MultibodyPlant& plant); // For single joint void AddJointToTrack(const std::string& joint_pos_name, @@ -39,23 +38,25 @@ class JointSpaceTrackingData final : public OptionsTrackingData { const std::vector& joint_vel_names); private: - void UpdateY(const Eigen::VectorXd& x_w_spr, - const drake::systems::Context& context_w_spr) final; - void UpdateYdot(const Eigen::VectorXd& x_w_spr, - const drake::systems::Context& context_w_spr) final; - void UpdateJ(const Eigen::VectorXd& x_wo_spr, - const drake::systems::Context& context_wo_spr) final; - void UpdateJdotV(const Eigen::VectorXd& x_wo_spr, - const drake::systems::Context& context_wo_spr) final; + void UpdateY(const Eigen::VectorXd& x, + const drake::systems::Context& context, + OscTrackingDataState& tracking_data_state) const final; + void UpdateYdot(const Eigen::VectorXd& x, + const drake::systems::Context& context, + OscTrackingDataState& tracking_data_state) const final; + void UpdateJ(const Eigen::VectorXd& x, + const drake::systems::Context& context, + OscTrackingDataState& tracking_data_state) const final; + void UpdateJdotV(const Eigen::VectorXd& x, + const drake::systems::Context& context, + OscTrackingDataState& tracking_data_state) const final; void CheckDerivedOscTrackingData() final; - // `joint_pos_idx_wo_spr` is the index of the joint position - // `joint_vel_idx_wo_spr` is the index of the joint velocity - std::unordered_map> joint_pos_idx_w_spr_; - std::unordered_map> joint_vel_idx_w_spr_; - std::unordered_map> joint_pos_idx_wo_spr_; - std::unordered_map> joint_vel_idx_wo_spr_; + // `joint_pos_idx` is the index of the joint position + // `joint_vel_idx` is the index of the joint velocity + std::unordered_map> joint_pos_idx_; + std::unordered_map> joint_vel_idx_; }; } // namespace controllers diff --git a/systems/controllers/osc/operational_space_control.cc b/systems/controllers/osc/operational_space_control.cc index 9b0db4f84e..6651478d57 100644 --- a/systems/controllers/osc/operational_space_control.cc +++ b/systems/controllers/osc/operational_space_control.cc @@ -20,7 +20,6 @@ using Eigen::MatrixXd; using Eigen::Vector2d; using Eigen::VectorXd; -using dairlib::multibody::CreateContext; using drake::multibody::JacobianWrtVariable; using drake::multibody::JointActuatorIndex; using drake::multibody::JointIndex; @@ -39,8 +38,6 @@ using drake::solvers::Solve; namespace dairlib::systems::controllers { -using multibody::CreateWithSpringsToWithoutSpringsMapPos; -using multibody::CreateWithSpringsToWithoutSpringsMapVel; using multibody::MakeNameToActuatorsMap; using multibody::MakeNameToVelocitiesMap; using multibody::SetPositionsIfNew; @@ -80,6 +77,7 @@ OperationalSpaceControl::OperationalSpaceControl( DeclarePerStepDiscreteUpdateEvent( &OperationalSpaceControl::DiscreteVariableUpdate); prev_fsm_state_idx_ = this->DeclareDiscreteState(-0.1 * VectorXd::Ones(1)); + prev_mode_fsm_state_idx_ = this->DeclareDiscreteState(-0.1 * VectorXd::Ones(1)); prev_event_time_idx_ = this->DeclareDiscreteState(VectorXd::Zero(1)); } @@ -97,6 +95,18 @@ OperationalSpaceControl::OperationalSpaceControl( &OperationalSpaceControl::CheckTracking) .get_index(); + osc_solution_cache_ = DeclareCacheEntry( + "osc_solution", + drake::systems::ValueProducer( + this, &OperationalSpaceControl::AllocateSolution, + &OperationalSpaceControl::SolveIDQP)).cache_index(); + + tracking_data_cache_ = DeclareCacheEntry( + "tracking_data_states", + drake::systems::ValueProducer( + this, &OperationalSpaceControl::AllocateTrackingDataStates, + &OperationalSpaceControl::UpdateTrackingData)).cache_index(); + const std::map& vel_map = multibody::MakeNameToVelocitiesMap(plant); @@ -181,30 +191,51 @@ void OperationalSpaceControl::AddKinematicConstraint( // Tracking data methods void OperationalSpaceControl::AddTrackingData( std::unique_ptr tracking_data, double t_lb, double t_ub) { - tracking_data_vec_->push_back(std::move(tracking_data)); + tracking_data_vec_.push_back(std::move(tracking_data)); fixed_position_vec_.emplace_back(VectorXd::Zero(0)); // Construct input ports and add element to traj_name_to_port_index_map_ if // the port for the traj is not created yet - string traj_name = tracking_data_vec_->back()->GetName(); + string traj_name = tracking_data_vec_.back()->GetName(); if (traj_name_to_port_index_map_.find(traj_name) == traj_name_to_port_index_map_.end()) { PiecewisePolynomial pp = PiecewisePolynomial(); - int port_index = - this->DeclareAbstractInputPort( - traj_name, - drake::Value>(pp)) + traj_name_to_port_index_map_[traj_name] = this->DeclareAbstractInputPort( + traj_name, drake::Value>(pp)) .get_index(); - traj_name_to_port_index_map_[traj_name] = port_index; } } void OperationalSpaceControl::AddConstTrackingData( std::unique_ptr tracking_data, const VectorXd& v, double t_lb, double t_ub) { - tracking_data_vec_->push_back(std::move(tracking_data)); + tracking_data_vec_.push_back(std::move(tracking_data)); fixed_position_vec_.push_back(v); } +std::unique_ptr> +OperationalSpaceControl::AllocateTrackingDataStates() const { + auto states = std::make_unique>(); + for (const auto& data: tracking_data_vec_) { + states->push_back(data->AllocateState()); + } + return states; +} + +std::unique_ptr +OperationalSpaceControl::AllocateSolution() const { + DRAKE_DEMAND(this->id_qp_.built()); + std::unique_ptr sol = + std::make_unique(); + sol->u_sol_ = VectorXd::Zero(this->n_u_); + sol->u_prev_ = VectorXd::Zero(this->n_u_); + sol->dv_sol_ = VectorXd::Zero(this->n_v_); + sol->lambda_h_sol_ = VectorXd::Zero(this->id_qp_.nh()); + sol->lambda_c_sol_ = VectorXd::Zero(this->id_qp_.nc()); + sol->epsilon_sol_ = VectorXd::Zero(this->id_qp_.nc_active()); + sol->solve_time_ = 0; + return sol; +} + // Osc checkers and constructor void OperationalSpaceControl::CheckCostSettings() { if (W_input_.size() != 0) { @@ -229,27 +260,14 @@ void OperationalSpaceControl::Build() { // Checker CheckCostSettings(); CheckConstraintSettings(); - for (auto& tracking_data : *tracking_data_vec_) { + for (auto& tracking_data : tracking_data_vec_) { tracking_data->CheckOscTrackingData(); - DRAKE_DEMAND(&tracking_data->plant_w_spr() == &plant_); + DRAKE_DEMAND(&tracking_data->plant() == &plant_); } // Construct QP id_qp_.Build(); - // Initialize solution - dv_sol_ = std::make_unique(n_v_); - u_sol_ = std::make_unique(n_u_); - lambda_c_sol_ = std::make_unique(id_qp_.nc()); - lambda_h_sol_ = std::make_unique(id_qp_.nh()); - epsilon_sol_ = std::make_unique(id_qp_.nc_active()); - u_prev_ = std::make_unique(n_u_); - dv_sol_->setZero(); - u_sol_->setZero(); - lambda_c_sol_->setZero(); - lambda_h_sol_->setZero(); - u_prev_->setZero(); - // Add costs // 1. input cost if (W_input_.size() > 0) { @@ -290,7 +308,7 @@ void OperationalSpaceControl::Build() { } // 4. Tracking cost - for (const auto& data : *tracking_data_vec_) { + for (const auto& data : tracking_data_vec_) { id_qp_.AddQuadraticCost(data->GetName(), MatrixXd::Zero(n_v_, n_v_), VectorXd::Zero(n_v_), id_qp_.dv()); } @@ -309,16 +327,13 @@ void OperationalSpaceControl::Build() { // (Testing) 6. contact force blending if (ds_duration_ > 0) { - int nc = id_qp_.nc(); const auto& lambda = id_qp_.lambda_c(); - blend_constraint_ = id_qp_.get_mutable_prog().AddLinearEqualityConstraint( - MatrixXd::Zero(1, nc / kSpaceDim), VectorXd::Zero(1), - {lambda.segment(kSpaceDim * 0 + 2, 1), - lambda.segment(kSpaceDim * 1 + 2, 1), - lambda.segment(kSpaceDim * 2 + 2, 1), - lambda.segment(kSpaceDim * 3 + 2, 1)}) - .evaluator() - .get(); + id_qp_.AddQuadraticCost( + "ds_blending", MatrixXd::Zero(4, 4), VectorXd::Zero(4), + {lambda.segment(kSpaceDim * 0 + 2, 1), + lambda.segment(kSpaceDim * 1 + 2, 1), + lambda.segment(kSpaceDim * 2 + 2, 1), + lambda.segment(kSpaceDim * 3 + 2, 1)}); } solver_ = std::make_unique(); @@ -335,10 +350,15 @@ drake::systems::EventStatus OperationalSpaceControl::DiscreteVariableUpdate( (OutputVector*)this->EvalVectorInput(context, state_port_); double timestamp = robot_output->get_timestamp(); - auto prev_fsm_state = discrete_state->get_mutable_vector(prev_fsm_state_idx_) - .get_mutable_value(); + auto prev_fsm_state = + discrete_state->get_mutable_vector( + prev_fsm_state_idx_).get_mutable_value(); + auto prev_mode_fsm_state = + discrete_state->get_mutable_vector( + prev_mode_fsm_state_idx_).get_mutable_value(); + if (fsm_state(0) != prev_fsm_state(0)) { - prev_distinct_fsm_state_ = prev_fsm_state(0); + prev_mode_fsm_state(0) = prev_fsm_state(0); prev_fsm_state(0) = fsm_state(0); discrete_state->get_mutable_vector(prev_event_time_idx_).get_mutable_value() @@ -351,71 +371,133 @@ drake::systems::EventStatus OperationalSpaceControl::DiscreteVariableUpdate( return drake::systems::EventStatus::Succeeded(); } -VectorXd OperationalSpaceControl::SolveQp( - const VectorXd& x_w_spr, const VectorXd& x_wo_spr, - const drake::systems::Context& context, double t, int fsm_state, - double t_since_last_state_switch, double alpha, int next_fsm_state) const { +void OperationalSpaceControl::UpdateTrackingData( + const drake::systems::Context &context, + std::vector *states) const { - // Update context + auto robot_output = dynamic_cast*>( + EvalVectorInput(context, state_port_)); + + VectorXd x = robot_output->GetState(); + + // Update plant context SetPositionsIfNew( - plant_, x_w_spr.head(plant_.num_positions()), context_); + plant_, x.head(plant_.num_positions()), context_); SetVelocitiesIfNew( - plant_, x_w_spr.tail(plant_.num_velocities()), context_); - - const auto active_contact_names = contact_names_map_.count(fsm_state) > 0 ? - contact_names_map_.at(fsm_state) : std::vector(); - id_qp_.UpdateDynamics(x_w_spr, active_contact_names, {}); + plant_, x.tail(plant_.num_velocities()), context_); + double timestamp = robot_output->get_timestamp(); + int fsm_state = -1; + int next_fsm_state = -1; + double alpha = 0; + double clock_time = timestamp; + double prev_event_time = 0; - // Invariant Impacts - // Only update when near an impact - bool near_impact = alpha != 0; - VectorXd v_proj = VectorXd::Zero(n_v_); + VectorXd u_sol(n_u_); + if (used_with_finite_state_machine_) { + // Read in finite state machine + auto fsm_output = this->EvalVectorInput(context, fsm_port_); + fsm_state = fsm_output->get_value()(0); - if (near_impact) { - MatrixXd M(n_v_, n_v_); - plant_.CalcMassMatrix(*context_, &M); + if (this->get_input_port(clock_port_).HasValue(context)) { + auto clock = this->EvalVectorInput(context, clock_port_); + clock_time = clock->get_value()(0); + } - UpdateImpactInvariantProjection( - x_w_spr, x_wo_spr, context, t, t_since_last_state_switch, - fsm_state, next_fsm_state, M); - // Need to call Update before this to get the updated jacobian - v_proj = alpha * M_Jt_ * ii_lambda_sol_ + 1e-13 * VectorXd::Ones(n_v_); + if (this->get_input_port_impact_info().HasValue(context)) { + auto impact_info = dynamic_cast*>( + this->EvalVectorInput(context, impact_info_port_)); + alpha = impact_info->GetAlpha(); + next_fsm_state = impact_info->GetCurrentContactMode(); + } + // Get discrete states + prev_event_time = + context.get_discrete_state(prev_event_time_idx_).get_value()(0); } - // Update costs - // 4. Tracking cost - for (unsigned int i = 0; i < tracking_data_vec_->size(); i++) { - auto tracking_data = tracking_data_vec_->at(i).get(); + // Update tracking data jacobians, etc. + VectorXd v_proj = VectorXd::Zero(n_v_); + for (unsigned int i = 0; i < tracking_data_vec_.size(); i++) { + const auto tracking_data = tracking_data_vec_.at(i).get(); + auto &tracking_data_state = states->at(i); if (tracking_data->IsActive(fsm_state)) { - // Check whether or not it is a constant trajectory, and update - // TrackingData if (fixed_position_vec_.at(i).size() != 0) { // Create constant trajectory and update tracking_data->Update( - x_w_spr, *context_, x_wo_spr, *context_, - PiecewisePolynomial(fixed_position_vec_.at(i)), t, - t_since_last_state_switch, fsm_state, v_proj); + x, *context_, + PiecewisePolynomial(fixed_position_vec_.at(i)), clock_time, + timestamp - prev_event_time, fsm_state, v_proj, tracking_data_state); } else { - // Read in traj from input port - const string& traj_name = tracking_data->GetName(); + const string &traj_name = tracking_data->GetName(); int port_index = traj_name_to_port_index_map_.at(traj_name); - const drake::AbstractValue* input_traj = + const drake::AbstractValue *input_traj = this->EvalAbstractInput(context, port_index); DRAKE_DEMAND(input_traj != nullptr); - const auto& traj = + const auto &traj = input_traj->get_value>(); // Update - tracking_data->Update(x_w_spr, *context_, x_wo_spr, - *context_, traj, t, - t_since_last_state_switch, fsm_state, v_proj); + tracking_data->Update( + x, *context_, traj, clock_time, timestamp - prev_event_time, + fsm_state, v_proj, tracking_data_state); } + } + } + + // Do impact invariant projection + if (alpha != 0) { + MatrixXd M(n_v_, n_v_); + plant_.CalcMassMatrix(*context_, &M); + + VectorXd v_perp = CalcImpactInvariantProjection( + fsm_state, next_fsm_state, M, x.tail(n_v_), *states); + // Need to call Update before this to get the updated jacobian + v_proj = alpha * v_perp; + } + + for (unsigned int i = 0; i < tracking_data_vec_.size(); i++) { + const auto tracking_data = tracking_data_vec_.at(i).get(); + auto &tracking_data_state = states->at(i); + if (tracking_data->IsActive(fsm_state) && tracking_data->GetImpactInvariantProjection()) { + tracking_data->MakeImpactInvariantCorrection( + clock_time, timestamp - prev_event_time, fsm_state, v_proj, + tracking_data_state); + } + } + +} + +VectorXd OperationalSpaceControl::SolveQp( + const VectorXd& x, + const drake::systems::Context& context, double t, int fsm_state, + double t_since_last_state_switch, double alpha, int next_fsm_state, + OperationalSpaceControl::id_qp_solution* sol) const { + + // Update context + SetPositionsIfNew( + plant_, x.head(plant_.num_positions()), context_); + SetVelocitiesIfNew( + plant_, x.tail(plant_.num_velocities()), context_); - const VectorXd& ddy_t = tracking_data->GetYddotCommand(); - const MatrixXd& W = tracking_data->GetWeight(); - const MatrixXd& J_t = tracking_data->GetJ(); - const VectorXd& JdotV_t = tracking_data->GetJdotTimesV(); + const auto active_contact_names = contact_names_map_.count(fsm_state) > 0 ? + contact_names_map_.at(fsm_state) : std::vector(); + id_qp_.UpdateDynamics(x, active_contact_names, {}); + + const auto& tracking_data_states = get_cache_entry( + tracking_data_cache_).Eval>(context); + + // Update costs + // 4. Tracking cost + for (unsigned int i = 0; i < tracking_data_vec_.size(); i++) { + const auto tracking_data = tracking_data_vec_.at(i).get(); + const auto& tracking_data_state = tracking_data_states.at(i); + + if (tracking_data->IsActive(fsm_state)) { + + const VectorXd& ddy_t = tracking_data_state.yddot_command_; + const MatrixXd& W = tracking_data_state.time_varying_weight_; + const MatrixXd& J_t = tracking_data_state.J_; + const VectorXd& JdotV_t = tracking_data_state.JdotV_; const VectorXd constant_term = (JdotV_t - ddy_t); id_qp_.UpdateCost( @@ -433,9 +515,9 @@ VectorXd OperationalSpaceControl::SolveQp( // Add joint limit constraints if (w_joint_limit_ > 0) { VectorXd w_joint_limit = - K_joint_pos_ * (x_wo_spr.head(plant_.num_positions()) + K_joint_pos_ * (x.head(plant_.num_positions()) .tail(n_revolute_joints_) -q_max_).cwiseMax(0) + - K_joint_pos_ * (x_wo_spr.head(plant_.num_positions()) + K_joint_pos_ * (x.head(plant_.num_positions()) .tail(n_revolute_joints_) - q_min_).cwiseMin(0); id_qp_.UpdateCost( "joint_limit_cost", @@ -449,23 +531,29 @@ VectorXd OperationalSpaceControl::SolveQp( MatrixXd A = MatrixXd::Zero(1, nc / kSpaceDim); if (std::find(ds_states_.begin(), ds_states_.end(), fsm_state) != ds_states_.end()) { - double s = std::clamp(t_since_last_state_switch / ds_duration_, 0.0, 1.0); + double s = std::clamp(t_since_last_state_switch / ds_duration_, 0.05, 0.95); + + int prev_mode_fsm_state = context.get_discrete_state( + prev_mode_fsm_state_idx_).get_value()(0); + double alpha_left = 0; double alpha_right = 0; - if (prev_distinct_fsm_state_ == right_support_state_) { + if (prev_mode_fsm_state == right_support_state_) { // We want left foot force to gradually increase - alpha_left = std::clamp(1.0 - s, 0.0, 1.0); + alpha_left = 1.0 - s; alpha_right = -s; - } else if (prev_distinct_fsm_state_ == left_support_state_) { + } else if (prev_mode_fsm_state == left_support_state_) { alpha_left = -s; - alpha_right = std::clamp(1.0 - s, 0.0, 1.0); + alpha_right = 1.0 - s; } - A(0, 0) = alpha_left / 2; - A(0, 1) = alpha_left / 2; - A(0, 2) = alpha_right / 2; - A(0, 3) = alpha_right / 2; + A(0, 0) = alpha_left; + A(0, 1) = alpha_left; + A(0, 2) = alpha_right; + A(0, 3) = alpha_right; } - blend_constraint_->UpdateCoefficients(A, VectorXd::Zero(1)); + id_qp_.UpdateCost( + "ds_blending", + w_blend_constraint_ * A.transpose() * A, VectorXd::Zero(4), 0); } // test joint-level input cost by fsm state @@ -480,10 +568,10 @@ VectorXd OperationalSpaceControl::SolveQp( } // (Testing) 7. Cost for staying close to the previous input - if (W_input_smoothing_.size() > 0 && u_prev_) { + if (W_input_smoothing_.size() > 0 && sol->u_prev_.rows() == plant_.num_actuators()) { id_qp_.UpdateCost( - "input_smoothing_cost", W_input_smoothing_, -W_input_smoothing_ * *u_prev_, - 0.5 * u_prev_->transpose() * W_input_smoothing_ * *u_prev_); + "input_smoothing_cost", W_input_smoothing_, -W_input_smoothing_ * sol->u_prev_, + 0.5 * sol->u_prev_.transpose() * W_input_smoothing_ * sol->u_prev_); } if (W_lambda_c_reg_.size() > 0) { @@ -494,7 +582,7 @@ VectorXd OperationalSpaceControl::SolveQp( if (W_lambda_h_reg_.size() > 0) { id_qp_.UpdateCost( - "lambda_h_reg", + "lambda_h_cost", (1 + alpha) * W_lambda_h_reg_,VectorXd::Zero(id_qp_.nh())); } if (!solver_->IsInitialized()) { @@ -504,42 +592,88 @@ VectorXd OperationalSpaceControl::SolveQp( // Solve the QP MathematicalProgramResult result; result = solver_->Solve(id_qp_.get_prog()); - solve_time_ = result.get_solver_details().run_time; + sol->solve_time_ = result.get_solver_details().run_time; if (result.is_success()) { // Extract solutions - *dv_sol_ = result.GetSolution(id_qp_.dv()); - *u_sol_ = result.GetSolution(id_qp_.u()); - *lambda_c_sol_ = result.GetSolution(id_qp_.lambda_c()); - *lambda_h_sol_ = result.GetSolution(id_qp_.lambda_h()); - *epsilon_sol_ = result.GetSolution(id_qp_.epsilon()); + sol->dv_sol_ = result.GetSolution(id_qp_.dv()); + sol->u_sol_ = result.GetSolution(id_qp_.u()); + sol->u_prev_ = sol->u_sol_; + sol->lambda_c_sol_ = result.GetSolution(id_qp_.lambda_c()); + sol->lambda_h_sol_ = result.GetSolution(id_qp_.lambda_h()); + sol->epsilon_sol_ = result.GetSolution(id_qp_.epsilon()); + solver_->EnableWarmStart(); } else { - *u_prev_ = 0.99 * *u_sol_ + VectorXd::Random(n_u_); + sol->u_prev_ = 0.99 * sol->u_sol_ + VectorXd::Random(n_u_); solver_->DisableWarmStart(); } - for (auto& tracking_data : *tracking_data_vec_) { - if (tracking_data->IsActive(fsm_state)) { - tracking_data->StoreYddotCommandSol(*dv_sol_); + return sol->u_sol_; +} + + +void OperationalSpaceControl::SolveIDQP( + const drake::systems::Context &context, + OperationalSpaceControl::id_qp_solution *solution) const { + + // Read in current state and time + auto robot_output = dynamic_cast*>( + EvalVectorInput(context, state_port_)); + + VectorXd x = robot_output->GetState(); + + double timestamp = robot_output->get_timestamp(); + + double current_time = timestamp; + + VectorXd u_sol(n_u_); + if (used_with_finite_state_machine_) { + // Read in finite state machine + auto fsm_output = this->EvalVectorInput(context, fsm_port_); + double clock_time = current_time; + if (this->get_input_port(clock_port_).HasValue(context)) { + auto clock = this->EvalVectorInput(context, clock_port_); + clock_time = clock->get_value()(0); } + VectorXd fsm_state = fsm_output->get_value(); + + double alpha = 0; + int next_fsm_state = -1; + if (this->get_input_port_impact_info().HasValue(context)) { + auto impact_info = (ImpactInfoVector*)this->EvalVectorInput( + context, impact_info_port_); + alpha = impact_info->GetAlpha(); + next_fsm_state = impact_info->GetCurrentContactMode(); + } + // Get discrete states + const auto prev_event_time = + context.get_discrete_state(prev_event_time_idx_).get_value(); + u_sol = SolveQp(x, context, clock_time, fsm_state(0), + current_time - prev_event_time(0), alpha, next_fsm_state, solution); + } else { + u_sol = SolveQp(x, context, current_time, -1, current_time, + 0, -1, solution); } +} - return *u_sol_; +namespace { +static inline bool has_active_ii_proj( + const OscTrackingData* data, int fsm_state) { + return data->IsActive(fsm_state) and data->GetImpactInvariantProjection(); +} } // TODO (@Yangwill) test that this is equivalent to the previous impact // invariant implementation -void OperationalSpaceControl::UpdateImpactInvariantProjection( - const VectorXd& x_w_spr, const VectorXd& x_wo_spr, - const Context& context, double t, double t_since_last_state_switch, - int fsm_state, int next_fsm_state, const MatrixXd& M) const { +Eigen::VectorXd OperationalSpaceControl::CalcImpactInvariantProjection( + int fsm_state, int next_fsm_state, const Eigen::MatrixXd &M, + const VectorXd& v, + const std::vector &up_to_date_td_state) const { auto map_iterator = contact_names_map_.find(next_fsm_state); if (map_iterator == contact_names_map_.end()) { - ii_lambda_sol_ = VectorXd::Zero(id_qp_.nc() + id_qp_.nh()); - M_Jt_ = MatrixXd::Zero(n_v_, id_qp_.nc() + id_qp_.nh()); - return; + return VectorXd::Zero(n_v_); } std::vector next_contact_set = map_iterator->second; @@ -558,48 +692,26 @@ void OperationalSpaceControl::UpdateImpactInvariantProjection( J_next.block(row_start, 0, id_qp_.nh(), n_v_) = id_qp_.get_holonomic_evaluators().EvalFullJacobian(*context_); } - M_Jt_ = M.llt().solve(J_next.transpose()); - int active_tracking_data_dim = 0; - for (unsigned int i = 0; i < tracking_data_vec_->size(); i++) { - auto tracking_data = tracking_data_vec_->at(i).get(); + Eigen::MatrixXd M_Jt = M.llt().solve(J_next.transpose()); - if (tracking_data->IsActive(fsm_state) && - tracking_data->GetImpactInvariantProjection()) { - VectorXd v_proj = VectorXd::Zero(n_v_); + int active_tracking_data_dim = 0; + for (const auto& tracking_data : tracking_data_vec_) { + if (has_active_ii_proj(tracking_data.get(), fsm_state)) { active_tracking_data_dim += tracking_data->GetYdotDim(); - if (fixed_position_vec_.at(i).size() != 0) { - // Create constant trajectory and update - tracking_data->Update( - x_w_spr, *context_, x_wo_spr, *context_, - PiecewisePolynomial(fixed_position_vec_.at(i)), t, - t_since_last_state_switch, fsm_state, v_proj); - } else { - // Read in traj from input port - const string& traj_name = tracking_data->GetName(); - int port_index = traj_name_to_port_index_map_.at(traj_name); - const drake::AbstractValue* input_traj = - EvalAbstractInput(context, port_index); - const auto& traj = - input_traj->get_value>(); - tracking_data->Update(x_w_spr, *context_, x_wo_spr, - *context_, traj, t, - t_since_last_state_switch, fsm_state, v_proj); - } } } MatrixXd A = MatrixXd::Zero(active_tracking_data_dim, active_constraint_dim); VectorXd ydot_err_vec = VectorXd::Zero(active_tracking_data_dim); int start_row = 0; - for (auto& tracking_data : *tracking_data_vec_) { - if (tracking_data->IsActive(fsm_state) && - tracking_data->GetImpactInvariantProjection()) { - A.block(start_row, 0, tracking_data->GetYdotDim(), - active_constraint_dim) = tracking_data->GetJ() * M_Jt_; - ydot_err_vec.segment(start_row, tracking_data->GetYdotDim()) = - tracking_data->GetErrorYdot(); - start_row += tracking_data->GetYdotDim(); + for (int i = 0; i < tracking_data_vec_.size(); ++i) { + if (has_active_ii_proj(tracking_data_vec_.at(i).get(), fsm_state)) { + A.block(start_row, 0, tracking_data_vec_.at(i)->GetYdotDim(), + active_constraint_dim) = up_to_date_td_state.at(i).J_ * M_Jt; + ydot_err_vec.segment(start_row, tracking_data_vec_.at(i)->GetYdotDim()) = + up_to_date_td_state.at(i).error_ydot_; + start_row += tracking_data_vec_.at(i)->GetYdotDim(); } } @@ -610,11 +722,12 @@ void OperationalSpaceControl::UpdateImpactInvariantProjection( A.transpose() * A; VectorXd b_constrained = VectorXd::Zero(active_constraint_dim + id_qp_.nh()); VectorXd Ab = A.transpose() * ydot_err_vec; + if (id_qp_.nh() > 0) { MatrixXd J_h = id_qp_.get_holonomic_evaluators().EvalFullJacobian( *context_); - MatrixXd C = J_h * M_Jt_; - VectorXd d = J_h * x_w_spr.tail(n_v_); + MatrixXd C = J_h * M_Jt; + VectorXd d = J_h * v; A_constrained.block(active_constraint_dim, 0, id_qp_.nh(), active_constraint_dim) = C; A_constrained.block(0, active_constraint_dim, active_constraint_dim, id_qp_.nh()) = @@ -624,9 +737,11 @@ void OperationalSpaceControl::UpdateImpactInvariantProjection( b_constrained << Ab; } - ii_lambda_sol_ = A_constrained.completeOrthogonalDecomposition() + MatrixXd ii_lambda_sol = A_constrained.completeOrthogonalDecomposition() .solve(b_constrained) .head(active_constraint_dim); + + return M_Jt * ii_lambda_sol + 1e-13 * VectorXd::Ones(n_v_); } void OperationalSpaceControl::AssignOscLcmOutput( @@ -644,14 +759,17 @@ void OperationalSpaceControl::AssignOscLcmOutput( output->utime = state->get_timestamp() * 1e6; output->fsm_state = fsm_state; + auto osc_solution = get_cache_entry( + osc_solution_cache_).Eval(context); + const std::vector> potential_regularization_cost_names_and_vars { - {"input_cost", *u_sol_}, - {"acceleration_cost", *dv_sol_}, - {"soft_constraint_cost", *epsilon_sol_}, - {"input_smoothing_cost", *u_sol_}, - {"lambda_c_cost", *lambda_c_sol_}, - {"lambda_h_cost", *lambda_h_sol_} + {"input_cost", osc_solution.u_sol_}, + {"acceleration_cost", osc_solution.dv_sol_}, + {"soft_constraint_cost", osc_solution.epsilon_sol_}, + {"input_smoothing_cost", osc_solution.u_sol_}, + {"lambda_c_cost", osc_solution.lambda_c_sol_}, + {"lambda_h_cost", osc_solution.lambda_h_sol_} }; output->regularization_cost_names.clear(); @@ -667,30 +785,34 @@ void OperationalSpaceControl::AssignOscLcmOutput( } output->num_regularization_costs = output->regularization_costs.size(); - output->tracking_data_names.clear(); - output->tracking_data.clear(); - output->tracking_costs.clear(); - lcmt_osc_qp_output qp_output; - qp_output.solve_time = solve_time_; + qp_output.solve_time = osc_solution.solve_time_; qp_output.u_dim = n_u_; qp_output.lambda_c_dim = id_qp_.nc(); qp_output.lambda_h_dim = id_qp_.nh(); qp_output.v_dim = n_v_; qp_output.epsilon_dim = id_qp_.nc_active(); - qp_output.u_sol = CopyVectorXdToStdVector(*u_sol_); - qp_output.lambda_c_sol = CopyVectorXdToStdVector(*lambda_c_sol_); - qp_output.lambda_h_sol = CopyVectorXdToStdVector(*lambda_h_sol_); - qp_output.dv_sol = CopyVectorXdToStdVector(*dv_sol_); - qp_output.epsilon_sol = CopyVectorXdToStdVector(*epsilon_sol_); + qp_output.u_sol = CopyVectorXdToStdVector(osc_solution.u_sol_); + qp_output.lambda_c_sol = CopyVectorXdToStdVector(osc_solution.lambda_c_sol_); + qp_output.lambda_h_sol = CopyVectorXdToStdVector(osc_solution.lambda_h_sol_); + qp_output.dv_sol = CopyVectorXdToStdVector(osc_solution.dv_sol_); + qp_output.epsilon_sol = CopyVectorXdToStdVector(osc_solution.epsilon_sol_); output->qp_output = qp_output; output->tracking_data = std::vector(); output->tracking_costs = std::vector(); output->tracking_data_names = std::vector(); - for (unsigned int i = 0; i < tracking_data_vec_->size(); i++) { - auto tracking_data = tracking_data_vec_->at(i).get(); + output->tracking_data.clear(); + output->tracking_costs.clear(); + output->tracking_data_names.clear(); + + const auto& tracking_data_states = get_cache_entry( + tracking_data_cache_).Eval>(context); + + for (unsigned int i = 0; i < tracking_data_vec_.size(); i++) { + const auto& tracking_data = tracking_data_vec_.at(i).get(); + const auto& tracking_data_state = tracking_data_states.at(i); lcmt_osc_tracking_data osc_output; osc_output.y_dim = tracking_data->GetYDim(); @@ -708,25 +830,29 @@ void OperationalSpaceControl::AssignOscLcmOutput( osc_output.yddot_command_sol = std::vector(osc_output.ydot_dim); if (tracking_data->IsActive(fsm_state)) { - osc_output.y = CopyVectorXdToStdVector(tracking_data->GetY()); - osc_output.y_des = CopyVectorXdToStdVector(tracking_data->GetYDes()); - osc_output.error_y = CopyVectorXdToStdVector(tracking_data->GetErrorY()); - osc_output.ydot = CopyVectorXdToStdVector(tracking_data->GetYdot()); - osc_output.ydot_des = - CopyVectorXdToStdVector(tracking_data->GetYdotDes()); - osc_output.error_ydot = - CopyVectorXdToStdVector(tracking_data->GetErrorYdot()); - osc_output.yddot_des = - CopyVectorXdToStdVector(tracking_data->GetYddotDes()); - osc_output.yddot_command = - CopyVectorXdToStdVector(tracking_data->GetYddotCommand()); - osc_output.yddot_command_sol = - CopyVectorXdToStdVector(tracking_data->GetYddotCommandSol()); + osc_output.y = CopyVectorXdToStdVector( + tracking_data_state.y_); + osc_output.y_des = CopyVectorXdToStdVector( + tracking_data_state.y_des_); + osc_output.error_y = CopyVectorXdToStdVector( + tracking_data_state.error_y_); + osc_output.ydot = CopyVectorXdToStdVector( + tracking_data_state.ydot_); + osc_output.ydot_des = CopyVectorXdToStdVector( + tracking_data_state.ydot_des_); + osc_output.error_ydot = CopyVectorXdToStdVector( + tracking_data_state.error_ydot_); + osc_output.yddot_des = CopyVectorXdToStdVector( + tracking_data_state.yddot_des_converted_); + osc_output.yddot_command = CopyVectorXdToStdVector( + tracking_data_state.yddot_command_); + osc_output.yddot_command_sol = CopyVectorXdToStdVector( + tracking_data_state.CalcYddotCommandSol(osc_solution.dv_sol_)); VectorXd y_tracking_cost = VectorXd::Zero(1); id_qp_.get_cost_evaluator( tracking_data->GetName() - ).Eval(*dv_sol_, &y_tracking_cost); + ).Eval(osc_solution.dv_sol_, &y_tracking_cost); total_cost += y_tracking_cost[0]; output->tracking_costs.push_back(y_tracking_cost[0]); output->tracking_data.push_back(osc_output); @@ -734,7 +860,6 @@ void OperationalSpaceControl::AssignOscLcmOutput( } } - *u_prev_ = *u_sol_; output->num_tracking_data = output->tracking_data_names.size(); output->num_regularization_costs = output->regularization_cost_names.size(); } @@ -742,62 +867,33 @@ void OperationalSpaceControl::AssignOscLcmOutput( void OperationalSpaceControl::CalcOptimalInput( const drake::systems::Context& context, systems::TimestampedVector* control) const { - // Read in current state and time + auto robot_output = dynamic_cast*>( EvalVectorInput(context, state_port_)); - VectorXd x_w_spr = robot_output->GetState(); - VectorXd x_wo_spr = x_w_spr; - - double timestamp = robot_output->get_timestamp(); - - double current_time = timestamp; - - VectorXd u_sol(n_u_); - if (used_with_finite_state_machine_) { - // Read in finite state machine - auto fsm_output = this->EvalVectorInput(context, fsm_port_); - double clock_time = current_time; - if (this->get_input_port(clock_port_).HasValue(context)) { - auto clock = this->EvalVectorInput(context, clock_port_); - clock_time = clock->get_value()(0); - } - VectorXd fsm_state = fsm_output->get_value(); - - double alpha = 0; - int next_fsm_state = -1; - if (this->get_input_port_impact_info().HasValue(context)) { - auto impact_info = (ImpactInfoVector*)this->EvalVectorInput( - context, impact_info_port_); - alpha = impact_info->GetAlpha(); - next_fsm_state = impact_info->GetCurrentContactMode(); - } - // Get discrete states - const auto prev_event_time = - context.get_discrete_state(prev_event_time_idx_).get_value(); - u_sol = SolveQp(x_w_spr, x_wo_spr, context, clock_time, fsm_state(0), - current_time - prev_event_time(0), alpha, next_fsm_state); - } else { - u_sol = SolveQp(x_w_spr, x_wo_spr, context, current_time, -1, current_time, - 0, -1); - } + auto osc_solution = get_cache_entry( + osc_solution_cache_).Eval(context); // Assign the control input - control->SetDataVector(u_sol); + control->SetDataVector(osc_solution.u_sol_); control->set_timestamp(robot_output->get_timestamp()); } void OperationalSpaceControl::CheckTracking( const drake::systems::Context& context, TimestampedVector* output) const { - auto robot_output = - (OutputVector*)this->EvalVectorInput(context, state_port_); + + auto robot_output = dynamic_cast*>( + EvalVectorInput(context, state_port_)); + auto osc_solution = get_cache_entry( + osc_solution_cache_).Eval(context); + output->set_timestamp(robot_output->get_timestamp()); output->get_mutable_value()(0) = 0.0; VectorXd y_soft_constraint_cost = VectorXd::Zero(1); if (id_qp_.has_cost_named("soft_constraint_cost")) { id_qp_.get_cost_evaluator("soft_constraint_cost").Eval( - *epsilon_sol_, &y_soft_constraint_cost); + osc_solution.epsilon_sol_, &y_soft_constraint_cost); } if (y_soft_constraint_cost[0] > 1e5 || isnan(y_soft_constraint_cost[0])) { output->get_mutable_value()(0) = 1.0; diff --git a/systems/controllers/osc/operational_space_control.h b/systems/controllers/osc/operational_space_control.h index ca787afb65..c78a8638fb 100644 --- a/systems/controllers/osc/operational_space_control.h +++ b/systems/controllers/osc/operational_space_control.h @@ -243,12 +243,6 @@ class OperationalSpaceControl : public drake::systems::LeafSystem { void AddConstTrackingData( std::unique_ptr tracking_data, const Eigen::VectorXd& v, double t_lb = 0, double t_ub = std::numeric_limits::infinity()); - std::vector>* GetAllTrackingData() { - return tracking_data_vec_.get(); - } - OscTrackingData* GetTrackingDataByIndex(int index) { - return tracking_data_vec_->at(index).get(); - } // Optional features void SetUpDoubleSupportPhaseBlending(double ds_duration, @@ -265,45 +259,60 @@ class OperationalSpaceControl : public drake::systems::LeafSystem { .GetAsSolverOptions(drake::solvers::OsqpSolver::id()) ); }; + // OSC LeafSystem builder void Build(); private: + + struct id_qp_solution { + Eigen::VectorXd dv_sol_; + Eigen::VectorXd u_sol_; + Eigen::VectorXd lambda_c_sol_; + Eigen::VectorXd lambda_h_sol_; + Eigen::VectorXd epsilon_sol_; + Eigen::VectorXd u_prev_; + double solve_time_; + }; + + std::unique_ptr AllocateSolution() const; + // Osc checkers and constructor-related methods void CheckCostSettings(); void CheckConstraintSettings(); // Get solution of OSC - Eigen::VectorXd SolveQp(const Eigen::VectorXd& x_w_spr, - const Eigen::VectorXd& x_wo_spr, + Eigen::VectorXd SolveQp(const Eigen::VectorXd& x, const drake::systems::Context& context, double t, int fsm_state, double t_since_last_state_switch, double alpha, - int next_fsm_state) const; + int next_fsm_state, id_qp_solution* sol) const; + + void SolveIDQP(const drake::systems::Context& context, + id_qp_solution* solution) const; + + std::unique_ptr> + AllocateTrackingDataStates() const; + + void UpdateTrackingData(const drake::systems::Context& context, + std::vector* states) const; + - // Solves the optimization problem: - // min_{\lambda} || ydot_{des} - J_{y}(qdot + M^{-1} J_{\lambda}^T \lambda||_2 - // s.t. constraints - // In the IROS 2021 paper, the problem was unconstrained and could be solved - // using the closed form least squares solution - void UpdateImpactInvariantProjection( - const Eigen::VectorXd& x_w_spr, const Eigen::VectorXd& x_wo_spr, - const drake::systems::Context& context, double t, - double t_since_last_state_switch, int fsm_state, int next_fsm_state, - const Eigen::MatrixXd& M) const; // Solves the optimization problem: // min_{\lambda} || ydot_{des} - J_{y}(qdot + M^{-1} J_{\lambda}^T \lambda||_2 - // s.t. constraints - // By adding constraints on lambda, we can impose scaling on the - // impact-invariant projection. - // The current constraints are lambda \in convex_hull \alpha * [-FC, FC] - // defined by the normal impulse from the nominal trajectory - void UpdateImpactInvariantProjectionQP( - const Eigen::VectorXd& x_w_spr, const Eigen::VectorXd& x_wo_spr, - const drake::systems::Context& context, double t, - double t_since_last_state_switch, int fsm_state, int next_fsm_state, - const Eigen::MatrixXd& M) const; + // s.t. holonomic constraints + // + // In the IROS 2021 paper, "Impact Invariant Control with Applications to + // Bipedal Locomotion" by Yang and Posa the problem was unconstrained and + // could be solved using the closed form least squares solution. + // + // Returns v_perp, where v - v_perp is the projection of v to the impact + // invariant subspace + Eigen::VectorXd CalcImpactInvariantProjection( + int fsm_state, int next_fsm_state, const Eigen::MatrixXd& M, + const Eigen::VectorXd& v, + const std::vector& up_to_date_td_state) const; // Discrete update that stores the previous state transition time drake::systems::EventStatus DiscreteVariableUpdate( @@ -316,25 +325,32 @@ class OperationalSpaceControl : public drake::systems::LeafSystem { // Output function void CalcOptimalInput(const drake::systems::Context& context, systems::TimestampedVector* control) const; + // Safety function that triggers when the tracking cost is too high void CheckTracking(const drake::systems::Context& context, TimestampedVector* output) const; // Input/Output ports - int osc_debug_port_; - int osc_output_port_; - int state_port_; - int clock_port_; - int fsm_port_; - int impact_info_port_; - int failure_port_; + drake::systems::OutputPortIndex osc_debug_port_; + drake::systems::OutputPortIndex osc_output_port_; + drake::systems::OutputPortIndex failure_port_; + drake::systems::InputPortIndex state_port_; + drake::systems::InputPortIndex clock_port_; + drake::systems::InputPortIndex fsm_port_; + drake::systems::InputPortIndex impact_info_port_; + + // Caches + drake::systems::CacheIndex osc_solution_cache_; + drake::systems::CacheIndex tracking_data_cache_; // Discrete update - int prev_fsm_state_idx_; - int prev_event_time_idx_; + drake::systems::DiscreteStateIndex prev_fsm_state_idx_; + drake::systems::DiscreteStateIndex prev_mode_fsm_state_idx_; + drake::systems::DiscreteStateIndex prev_event_time_idx_; // Map from (non-const) trajectory names to input port indices - std::map traj_name_to_port_index_map_; + std::map traj_name_to_port_index_map_; // MBP's. const drake::multibody::MultibodyPlant& plant_; @@ -369,18 +385,6 @@ class OperationalSpaceControl : public drake::systems::LeafSystem { // MathematicalProgram mutable InverseDynamicsQp id_qp_; - // OSC solution - std::unique_ptr dv_sol_; - std::unique_ptr u_sol_; - std::unique_ptr lambda_c_sol_; - std::unique_ptr lambda_h_sol_; - std::unique_ptr epsilon_sol_; - std::unique_ptr u_prev_; - mutable double solve_time_{}; - - mutable Eigen::VectorXd ii_lambda_sol_; - mutable Eigen::MatrixXd M_Jt_; - // OSC cost members /// Using u cost would push the robot away from the fixed point, so the user /// could consider using acceleration cost instead. @@ -403,9 +407,7 @@ class OperationalSpaceControl : public drake::systems::LeafSystem { std::map> contact_names_map_ = {}; // OSC tracking data (stored as a pointer because of caching) - std::unique_ptr>> - tracking_data_vec_ = - std::make_unique>>(); + std::vector> tracking_data_vec_{}; // Fixed position of constant trajectories std::vector fixed_position_vec_; @@ -415,10 +417,7 @@ class OperationalSpaceControl : public drake::systems::LeafSystem { int left_support_state_{}; int right_support_state_{}; std::vector ds_states_{}; - double w_blend_constraint_ = 0.1; // for soft constraint - mutable double prev_distinct_fsm_state_ = -1; - drake::solvers::LinearEqualityConstraint* blend_constraint_ = nullptr; - drake::solvers::VectorXDecisionVariable epsilon_blend_{}; + double w_blend_constraint_ = 10; // for soft constraint }; } // namespace dairlib::systems::controllers diff --git a/systems/controllers/osc/options_tracking_data.cc b/systems/controllers/osc/options_tracking_data.cc index 9f9023f5ab..6df1c29bac 100644 --- a/systems/controllers/osc/options_tracking_data.cc +++ b/systems/controllers/osc/options_tracking_data.cc @@ -19,98 +19,102 @@ namespace dairlib::systems::controllers { OptionsTrackingData::OptionsTrackingData( const string& name, int n_y, int n_ydot, const MatrixXd& K_p, const MatrixXd& K_d, const MatrixXd& W, - const MultibodyPlant& plant_w_spr, - const MultibodyPlant& plant_wo_spr) - : OscTrackingData(name, n_y, n_ydot, K_p, K_d, W, plant_w_spr, - plant_wo_spr) { + const MultibodyPlant& plant) + : OscTrackingData(name, n_y, n_ydot, K_p, K_d, W, plant) { yddot_cmd_lb_ = std::numeric_limits::lowest() * VectorXd::Ones(n_ydot_); yddot_cmd_ub_ = std::numeric_limits::max() * VectorXd::Ones(n_ydot_); } void OptionsTrackingData::UpdateActual( - const Eigen::VectorXd& x_w_spr, - const drake::systems::Context& context_w_spr, - const Eigen::VectorXd& x_wo_spr, - const drake::systems::Context& context_wo_spr, double t) { - OscTrackingData::UpdateActual(x_w_spr, context_w_spr, x_wo_spr, - context_wo_spr, t); + const Eigen::VectorXd& x, + const drake::systems::Context& context, double t, + OscTrackingDataState& td_state) const { + OscTrackingData::UpdateActual(x, context, t, td_state); if (with_view_frame_) { - view_frame_rot_T_ = - view_frame_->CalcWorldToFrameRotation(plant_w_spr_, context_w_spr); + td_state.view_frame_rot_T_ = + view_frame_->CalcWorldToFrameRotation(plant_, context); if (!is_rotational_tracking_data_) { - y_ = view_frame_rot_T_ * y_; - ydot_ = view_frame_rot_T_ * ydot_; + td_state.y_ =td_state.view_frame_rot_T_ * td_state.y_; + td_state.ydot_ = td_state.view_frame_rot_T_ * td_state.ydot_; } - J_ = view_frame_rot_T_ * J_; - JdotV_ = view_frame_rot_T_ * JdotV_; + td_state.J_ = td_state.view_frame_rot_T_ * td_state.J_; + td_state.JdotV_ = td_state.view_frame_rot_T_ * td_state.JdotV_; } - UpdateFilters(t); + UpdateFilters(t, td_state); } -void OptionsTrackingData::UpdateFilters(double t) { +void OptionsTrackingData::UpdateFilters( + double t, OscTrackingDataState& td_state) const { + if (tau_ > 0) { - if (last_timestamp_ < 0) { + if (td_state.last_timestamp_ < 0) { // Initialize - filtered_y_ = y_; - filtered_ydot_ = ydot_; - } else if (t != last_timestamp_) { - double dt = t - last_timestamp_; + td_state.filtered_y_ = td_state.y_; + td_state.filtered_ydot_ = td_state.ydot_; + } else if (t != td_state.last_timestamp_) { + double dt = t - td_state.last_timestamp_; double alpha = dt / (dt + tau_); if (this->is_rotational_tracking_data_) { // quaternion auto slerp = drake::trajectories::PiecewiseQuaternionSlerp( - {0, 1}, {Quaterniond(y_[0], y_[1], y_[2], y_[3]), - Quaterniond(filtered_y_[0], filtered_y_[1], filtered_y_[2], - filtered_y_[3])}); - filtered_y_ = slerp.value(1 - alpha); + {0, 1}, + {Quaterniond( + td_state.y_[0], td_state.y_[1], td_state.y_[2], td_state.y_[3]), + Quaterniond( + td_state.filtered_y_[0], td_state.filtered_y_[1], + td_state.filtered_y_[2],td_state.filtered_y_[3])}); + td_state.filtered_y_ = slerp.value(1 - alpha); } else { - filtered_y_ = alpha * y_ + (1 - alpha) * filtered_y_; + td_state.filtered_y_ = alpha * td_state.y_ + (1 - alpha) * td_state.filtered_y_; } - filtered_ydot_ = alpha * ydot_ + (1 - alpha) * filtered_ydot_; + td_state.filtered_ydot_ = alpha * td_state.ydot_ + (1 - alpha) * td_state.filtered_ydot_; } // Assign filtered values if (n_y_ == 4) { - y_ = filtered_y_; - ydot_ = filtered_ydot_; + td_state.y_ = td_state.filtered_y_; + td_state.ydot_ = td_state.filtered_ydot_; } else { for (auto idx : low_pass_filter_element_idx_) { - y_(idx) = filtered_y_(idx); - ydot_(idx) = filtered_ydot_(idx); + td_state.y_(idx) = td_state.filtered_y_(idx); + td_state.ydot_(idx) = td_state.filtered_ydot_(idx); } } // Update timestamp - last_timestamp_ = t; + td_state.last_timestamp_ = t; } } -void OptionsTrackingData::UpdateYError() { error_y_ = y_des_ - y_; } +void OptionsTrackingData::UpdateYError(OscTrackingDataState& td_state) const{ + td_state.error_y_ = td_state.y_des_ - td_state.y_; +} -void OptionsTrackingData::UpdateYdotError(const Eigen::VectorXd& v_proj) { - error_ydot_ = ydot_des_ - ydot_; +void OptionsTrackingData::UpdateYdotError( + const Eigen::VectorXd& v_proj, OscTrackingDataState& td_state) const { + td_state.error_ydot_ = td_state.ydot_des_ - td_state.ydot_; if (impact_invariant_projection_) { - error_ydot_ -= GetJ() * v_proj; + td_state.error_ydot_ -= td_state.J_ * v_proj; } else if (no_derivative_feedback_ && !v_proj.isZero()) { - error_ydot_ = VectorXd::Zero(n_ydot_); + td_state.error_ydot_ = VectorXd::Zero(n_ydot_); } } -void OptionsTrackingData::UpdateYddotDes(double t, - double t_since_state_switch) { - yddot_des_converted_ = yddot_des_; +void OptionsTrackingData::UpdateYddotDes( + double t, double t_since_state_switch, OscTrackingDataState& td_state) const { + td_state.yddot_des_converted_ = td_state.yddot_des_; for (auto idx : idx_zero_feedforward_accel_) { - yddot_des_converted_(idx) = 0; + td_state.yddot_des_converted_(idx) = 0; } if (ff_accel_multiplier_traj_ != nullptr) { - yddot_des_converted_ = + td_state.yddot_des_converted_ = ff_accel_multiplier_traj_->value(t_since_state_switch) * - yddot_des_converted_; + td_state.yddot_des_converted_; } } -void OptionsTrackingData::UpdateYddotCmd(double t, - double t_since_state_switch) { +void OptionsTrackingData::UpdateYddotCmd( + double t, double t_since_state_switch, OscTrackingDataState& td_state) const { // 4. Update command output (desired output with pd control) MatrixXd p_gain_multiplier = (p_gain_multiplier_traj_ != nullptr) @@ -122,18 +126,19 @@ void OptionsTrackingData::UpdateYddotCmd(double t, ? d_gain_multiplier_traj_->value(t_since_state_switch) : MatrixXd::Identity(n_ydot_, n_ydot_); - yddot_command_ = yddot_des_converted_ + p_gain_multiplier * K_p_ * error_y_ + - d_gain_multiplier * K_d_ * error_ydot_; - yddot_command_ = eigen_clamp(yddot_command_, yddot_cmd_lb_, yddot_cmd_ub_); - UpdateW(t, t_since_state_switch); + td_state.yddot_command_ = td_state.yddot_des_converted_ + p_gain_multiplier * K_p_ * td_state.error_y_ + + d_gain_multiplier * K_d_ * td_state.error_ydot_; + td_state.yddot_command_ = eigen_clamp(td_state.yddot_command_, yddot_cmd_lb_, yddot_cmd_ub_); + UpdateW(t, t_since_state_switch, td_state); } -void OptionsTrackingData::UpdateW(double t, double t_since_state_switch) { +void OptionsTrackingData::UpdateW( + double t, double t_since_state_switch, OscTrackingDataState& td_state) const { if (weight_trajectory_ != nullptr) { - time_varying_weight_ = - weight_trajectory_->value(time_through_trajectory_).row(0)[0] * W_; + td_state.time_varying_weight_ = + weight_trajectory_->value(td_state.time_through_trajectory_).row(0)[0] * W_; } else { - time_varying_weight_ = W_; + td_state.time_varying_weight_ = W_; } } diff --git a/systems/controllers/osc/options_tracking_data.h b/systems/controllers/osc/options_tracking_data.h index eff035991c..89d92a0a5c 100644 --- a/systems/controllers/osc/options_tracking_data.h +++ b/systems/controllers/osc/options_tracking_data.h @@ -13,8 +13,7 @@ class OptionsTrackingData : public OscTrackingData { OptionsTrackingData( const std::string& name, int n_y, int n_ydot, const Eigen::MatrixXd& K_p, const Eigen::MatrixXd& K_d, const Eigen::MatrixXd& W, - const drake::multibody::MultibodyPlant& plant_w_spr, - const drake::multibody::MultibodyPlant& plant_wo_spr); + const drake::multibody::MultibodyPlant& plant); // enable the low pass filter void SetLowPassFilter(double tau, const std::set& element_idx = {}); @@ -49,10 +48,6 @@ class OptionsTrackingData : public OscTrackingData { with_view_frame_ = true; } - const Eigen::MatrixXd& GetWeight() const override { - return time_varying_weight_; - } - // disable feedforward acceleration for the components of the task space given // by indices void DisableFeedforwardAccel(const std::set& indices) { @@ -77,18 +72,15 @@ class OptionsTrackingData : public OscTrackingData { std::set idx_zero_feedforward_accel_ = {}; std::shared_ptr> view_frame_; - Eigen::Matrix3d view_frame_rot_T_; bool with_view_frame_ = false; bool is_rotational_tracking_data_ = false; private: // This method is called from the parent class (OscTrackingData) due to C++ // polymorphism. - void UpdateActual(const Eigen::VectorXd& x_w_spr, - const drake::systems::Context& context_w_spr, - const Eigen::VectorXd& x_wo_spr, - const drake::systems::Context& context_wo_spr, - double t) override; + void UpdateActual(const Eigen::VectorXd& xr, + const drake::systems::Context& context, + double t, OscTrackingDataState& td_state) const override; // We don't override the following methods (leave them to children classes): // UpdateY @@ -97,23 +89,19 @@ class OptionsTrackingData : public OscTrackingData { // UpdateJdotV // Override the error method - void UpdateYError() override; - void UpdateYdotError(const Eigen::VectorXd& v_proj) override; - void UpdateYddotDes(double t, double t_since_state_switch) override; - void UpdateYddotCmd(double t, double t_since_state_switch) override; - void UpdateW(double t, double t_since_state_switch); - - void UpdateFilters(double t); - - // Members of low-pass filter - Eigen::VectorXd filtered_y_; - Eigen::VectorXd filtered_ydot_; - Eigen::MatrixXd time_varying_weight_; + void UpdateYError(OscTrackingDataState& tracking_data_state) const override; + void UpdateYdotError(const Eigen::VectorXd& v_proj, OscTrackingDataState& tracking_data_state) const override; + void UpdateYddotDes(double t, double t_since_state_switch, OscTrackingDataState& tracking_data_state) const override; + void UpdateYddotCmd(double t, double t_since_state_switch, OscTrackingDataState& tracking_data_state) const override; + void UpdateW(double t, double t_since_state_switch, OscTrackingDataState& tracking_data_state) const; + + void UpdateFilters(double t, OscTrackingDataState& tracking_data_state) const; + + // low pass filter implementation double tau_ = -1; std::set low_pass_filter_element_idx_; - double last_timestamp_ = -1; }; } // namespace controllers } // namespace systems -} // namespace dairlib \ No newline at end of file +} // namespace dairlib diff --git a/systems/controllers/osc/osc_tracking_data.cc b/systems/controllers/osc/osc_tracking_data.cc index 10417594b8..9a8ef21a9e 100644 --- a/systems/controllers/osc/osc_tracking_data.cc +++ b/systems/controllers/osc/osc_tracking_data.cc @@ -31,12 +31,9 @@ using multibody::MakeNameToVelocitiesMap; OscTrackingData::OscTrackingData(const string& name, int n_y, int n_ydot, const MatrixXd& K_p, const MatrixXd& K_d, const MatrixXd& W, - const MultibodyPlant& plant_w_spr, - const MultibodyPlant& plant_wo_spr) - : plant_w_spr_(plant_w_spr), - plant_wo_spr_(plant_wo_spr), - world_w_spr_(plant_w_spr_.world_frame()), - world_wo_spr_(plant_wo_spr_.world_frame()), + const MultibodyPlant& plant) + : plant_(plant), + world_(plant_.world_frame()), name_(name), n_y_(n_y), n_ydot_(n_ydot), @@ -44,80 +41,100 @@ OscTrackingData::OscTrackingData(const string& name, int n_y, int n_ydot, K_d_(K_d), W_(W) {} +OscTrackingDataState OscTrackingData::AllocateState() const { + OscTrackingDataState state; + state.y_ = VectorXd::Zero(n_y_); + state.y_des_ = VectorXd::Zero(n_y_); + state.error_y_ = VectorXd::Zero(n_ydot_); + state.ydot_ = VectorXd::Zero(n_ydot_); + state.ydot_des_ = VectorXd::Zero(n_ydot_); + state.error_ydot_ = VectorXd::Zero(n_ydot_); + state.yddot_des_ = VectorXd::Zero(n_ydot_); + state.yddot_des_converted_ = VectorXd::Zero(n_ydot_); + state.yddot_command_ = VectorXd::Zero(n_ydot_); + + state.J_ = MatrixXd::Zero(n_ydot_, plant_.num_velocities()); + state.JdotV_ = VectorXd::Zero(n_ydot_); + state.view_frame_rot_T_ = Eigen::Matrix3d::Identity(); + state.name_ = name_; + state.fsm_state_ = -1; + + // Members of low-pass filter + state.filtered_y_ = VectorXd::Zero(n_y_); + state.filtered_ydot_ = VectorXd::Zero(n_ydot_); + state.time_varying_weight_ = W_; + + return state; +} + // Update void OscTrackingData::Update( - const VectorXd& x_w_spr, const Context& context_w_spr, - const VectorXd& x_wo_spr, const Context& context_wo_spr, + const VectorXd& x, const Context& context, const drake::trajectories::Trajectory& traj, double t, - double t_since_state_switch, const int fsm_state, const VectorXd& v_proj) { - fsm_state_ = fsm_state; + double t_since_state_switch, const int fsm_state, const VectorXd& v_proj, + OscTrackingDataState& tracking_data_state) const { + + tracking_data_state.fsm_state_ = fsm_state; // If the set of active states contains -1, the tracking data is always active if (active_fsm_states_.count(-1)) { - fsm_state_ = -1; + tracking_data_state.fsm_state_ = -1; } DRAKE_ASSERT(IsActive(fsm_state)); - UpdateActual(x_w_spr, context_w_spr, x_wo_spr, context_wo_spr, t); - UpdateDesired(traj, t, t_since_state_switch); + UpdateActual(x, context, t, tracking_data_state); + UpdateDesired(traj, t, t_since_state_switch, tracking_data_state); // 3. Update error // Careful: must update y and y_des before calling UpdateYError() - UpdateYError(); - UpdateYdotError(v_proj); - UpdateYddotCmd(t, t_since_state_switch); + UpdateYError(tracking_data_state); + UpdateYdotError(v_proj, tracking_data_state); + UpdateYddotCmd(t, t_since_state_switch, tracking_data_state); } void OscTrackingData::UpdateActual( - const Eigen::VectorXd& x_w_spr, - const drake::systems::Context& context_w_spr, - const Eigen::VectorXd& x_wo_spr, - const drake::systems::Context& context_wo_spr, double t) { + const Eigen::VectorXd& x, + const drake::systems::Context& context, + double t, OscTrackingDataState& tracking_data_state) const { // 1. Update actual output - if (use_springs_in_eval_) { - UpdateY(x_w_spr, context_w_spr); - UpdateYdot(x_w_spr, context_w_spr); - } else { - UpdateY(x_wo_spr, context_wo_spr); - UpdateYdot(x_wo_spr, context_wo_spr); - } - - UpdateJ(x_wo_spr, context_wo_spr); - UpdateJdotV(x_wo_spr, context_wo_spr); + UpdateY(x, context, tracking_data_state); + UpdateYdot(x, context, tracking_data_state); + UpdateJ(x, context, tracking_data_state); + UpdateJdotV(x, context, tracking_data_state); } void OscTrackingData::UpdateDesired( const drake::trajectories::Trajectory& traj, double t, - double t_since_state_switch) { + double t_since_state_switch, OscTrackingDataState& tracking_data_state) const { // 2. Update desired output if (traj.has_derivative()) { if (traj.rows() == 2 * n_ydot_) { - y_des_ = traj.value(t).topRows(n_y_); - ydot_des_ = traj.EvalDerivative(t, 1).topRows(n_ydot_); - yddot_des_ = traj.EvalDerivative(t, 1).bottomRows(n_ydot_); + tracking_data_state.y_des_ = traj.value(t).topRows(n_y_); + tracking_data_state.ydot_des_ = traj.EvalDerivative(t, 1).topRows(n_ydot_); + tracking_data_state.yddot_des_ = traj.EvalDerivative(t, 1).bottomRows(n_ydot_); } else { - y_des_ = traj.value(t); - ydot_des_ = traj.EvalDerivative(t, 1); - yddot_des_ = traj.EvalDerivative(t, 2); + tracking_data_state.y_des_ = traj.value(t); + tracking_data_state.ydot_des_ = traj.EvalDerivative(t, 1); + tracking_data_state.yddot_des_ = traj.EvalDerivative(t, 2); } } // TODO (yangwill): Remove this edge case after EvalDerivative has been // implemented for ExponentialPlusPiecewisePolynomial else { - y_des_ = traj.value(t); - ydot_des_ = traj.MakeDerivative(1)->value(t); - yddot_des_ = traj.MakeDerivative(2)->value(t); + tracking_data_state.y_des_ = traj.value(t); + tracking_data_state.ydot_des_ = traj.MakeDerivative(1)->value(t); + tracking_data_state.yddot_des_ = traj.MakeDerivative(2)->value(t); } - UpdateYddotDes(t, t_since_state_switch); - time_through_trajectory_ = t - traj.start_time(); + UpdateYddotDes(t, t_since_state_switch, tracking_data_state); + tracking_data_state.time_through_trajectory_ = t - traj.start_time(); } -void OscTrackingData::UpdateYddotCmd(double t, double t_since_state_switch) { - yddot_command_ = - yddot_des_converted_ + (K_p_ * (error_y_) + K_d_ * (error_ydot_)); +void OscTrackingData::UpdateYddotCmd( + double t, double t_since_state_switch, + OscTrackingDataState& tracking_data_state) const { + tracking_data_state.yddot_command_ = + tracking_data_state.yddot_des_converted_ + + (K_p_ * (tracking_data_state.error_y_) + K_d_ * (tracking_data_state.error_ydot_)); } -void OscTrackingData::StoreYddotCommandSol(const VectorXd& dv) { - yddot_command_sol_ = J_ * dv + JdotV_; -} void OscTrackingData::AddFiniteStateToTrack(int state) { DRAKE_DEMAND(!active_fsm_states_.count(state)); diff --git a/systems/controllers/osc/osc_tracking_data.h b/systems/controllers/osc/osc_tracking_data.h index cf7e42d9f2..2aedbdf67b 100644 --- a/systems/controllers/osc/osc_tracking_data.h +++ b/systems/controllers/osc/osc_tracking_data.h @@ -16,13 +16,54 @@ namespace controllers { static constexpr int kSpaceDim = 3; static constexpr int kQuaternionDim = 4; +struct OscTrackingDataState { + std::string name_; + Eigen::Matrix3d view_frame_rot_T_; + + int fsm_state_; + double time_through_trajectory_ = 0; + + // Actual outputs, Jacobian and dJ/dt * v + Eigen::VectorXd y_; + Eigen::VectorXd error_y_; + Eigen::VectorXd ydot_; + Eigen::VectorXd error_ydot_; + Eigen::MatrixXd J_; + Eigen::VectorXd JdotV_; + + // Desired output + Eigen::VectorXd y_des_; + Eigen::VectorXd ydot_des_; + Eigen::VectorXd yddot_des_; + Eigen::VectorXd yddot_des_converted_; + + // Commanded acceleration after feedback terms + Eigen::VectorXd yddot_command_; + + // Members of low-pass filter + Eigen::VectorXd filtered_y_; + Eigen::VectorXd filtered_ydot_; + Eigen::MatrixXd time_varying_weight_; + double last_timestamp_ = -1; + + /*! + * Calculate the operational space acceleration of this tracking data + * at it's current state, given a generalized acceleration dv + */ + Eigen::VectorXd CalcYddotCommandSol(const Eigen::VectorXd& dv) const { + return J_ * dv + JdotV_; + } +}; + + class OscTrackingData { public: OscTrackingData(const std::string& name, int n_y, int n_ydot, const Eigen::MatrixXd& K_p, const Eigen::MatrixXd& K_d, const Eigen::MatrixXd& W, - const drake::multibody::MultibodyPlant& plant_w_spr, - const drake::multibody::MultibodyPlant& plant_wo_spr); + const drake::multibody::MultibodyPlant& plant); + + OscTrackingDataState AllocateState() const; virtual ~OscTrackingData() = default; @@ -33,21 +74,28 @@ class OscTrackingData { // - update the errors (Calling virtual methods) // - update commanded accelerations (desired output with pd control) // Inputs/Arguments: - // - `x_w_spr`, state of the robot (with spring) - // - `context_w_spr`, plant context of the robot (without spring) - // - `x_wo_spr`, state of the robot (with spring) - // - `context_wo_spr`, plant context of the robot (without spring) + // - `x`, state of the robot (with spring) + // - `context`, plant context of the robot (without spring) + // - `x`, state of the robot (with spring) + // - `context`, plant context of the robot (without spring) // - `traj`, desired trajectory // - `t`, current time // - `t`, time since the last state switch // - `v_proj`, impact invariant velocity projection - virtual void Update(const Eigen::VectorXd& x_w_spr, - const drake::systems::Context& context_w_spr, - const Eigen::VectorXd& x_wo_spr, - const drake::systems::Context& context_wo_spr, + virtual void Update(const Eigen::VectorXd& x, + const drake::systems::Context& context, const drake::trajectories::Trajectory& traj, double t, double t_since_state_switch, int fsm_state, - const Eigen::VectorXd& v_proj); + const Eigen::VectorXd& v_proj, + OscTrackingDataState& tracking_data_state) const; + + void MakeImpactInvariantCorrection( + double t, double t_since_state_switch, int fsm_state, + const Eigen::VectorXd& v_proj, + OscTrackingDataState& tracking_data_state) const { + UpdateYdotError(v_proj, tracking_data_state); + UpdateYddotCmd(t, t_since_state_switch, tracking_data_state); + } // Add this state to the list of fsm states where this tracking data is active void AddFiniteStateToTrack(int state); @@ -56,11 +104,6 @@ class OscTrackingData { } void CheckOscTrackingData(); - // Set whether to use springs in the calculation of the actual outputs - void SetSpringsInKinematicCalculation(bool use_springs_in_eval) { - use_springs_in_eval_ = use_springs_in_eval; - } - // Set whether to use the impact invariant projection void SetImpactInvariantProjection(bool use_impact_invariant_projection) { impact_invariant_projection_ = use_impact_invariant_projection; @@ -72,87 +115,42 @@ class OscTrackingData { } // Get whether to use the impact invariant projection - bool GetImpactInvariantProjection() { return impact_invariant_projection_; } + bool GetImpactInvariantProjection() const { return impact_invariant_projection_; } // Get whether to use no derivative feedback near impacts - bool GetNoDerivativeFeedback() { return no_derivative_feedback_; } - - // Getters for debugging - const Eigen::VectorXd& GetY() const { return y_; } - const Eigen::VectorXd& GetYDes() const { return y_des_; } - const Eigen::VectorXd& GetErrorY() const { return error_y_; } - const Eigen::VectorXd& GetYdot() const { return ydot_; } - const Eigen::VectorXd& GetYdotDes() const { return ydot_des_; } - const Eigen::VectorXd& GetErrorYdot() const { return error_ydot_; } - const Eigen::VectorXd& GetYddotDes() const { return yddot_des_; } - const Eigen::VectorXd& GetYddotCommandSol() const { - return yddot_command_sol_; - } + bool GetNoDerivativeFeedback() const { return no_derivative_feedback_; } // Getters used by osc block const Eigen::MatrixXd& GetKp() const { return K_p_; } const Eigen::MatrixXd& GetKd() const { return K_d_; } - const Eigen::MatrixXd& GetJ() const { return J_; } - const Eigen::VectorXd& GetJdotTimesV() const { return JdotV_; } - const Eigen::VectorXd& GetYddotCommand() const { return yddot_command_; } virtual const Eigen::MatrixXd& GetWeight() const { return W_; } // Getters const std::string& GetName() const { return name_; }; - const std::set& GetActiveStates() { return active_fsm_states_; }; + const std::set& GetActiveStates() const { return active_fsm_states_; }; int GetYDim() const { return n_y_; }; int GetYdotDim() const { return n_ydot_; }; - const drake::multibody::MultibodyPlant& plant_w_spr() const { - return plant_w_spr_; - }; - const drake::multibody::MultibodyPlant& plant_wo_spr() const { - return plant_wo_spr_; - }; - void StoreYddotCommandSol(const Eigen::VectorXd& dv); + const drake::multibody::MultibodyPlant& plant() const { + return plant_; + }; protected: virtual void UpdateActual( - const Eigen::VectorXd& x_w_spr, - const drake::systems::Context& context_w_spr, - const Eigen::VectorXd& x_wo_spr, - const drake::systems::Context& context_wo_spr, double t); + const Eigen::VectorXd& x, + const drake::systems::Context& context, double t, + OscTrackingDataState& tracking_data_state) const; // Output dimension - int n_y_; - int n_ydot_; - - // Current fsm state - int fsm_state_; - - // Flags - bool use_springs_in_eval_ = true; - bool impact_invariant_projection_ = false; - bool no_derivative_feedback_ = false; - - double time_through_trajectory_ = 0; - - // Actual outputs, Jacobian and dJ/dt * v - Eigen::VectorXd y_; - Eigen::VectorXd error_y_; - Eigen::VectorXd ydot_; - Eigen::VectorXd error_ydot_; - Eigen::MatrixXd J_; - Eigen::VectorXd JdotV_; + const int n_y_; + const int n_ydot_; // PD control gains Eigen::MatrixXd K_p_; Eigen::MatrixXd K_d_; - // Desired output - Eigen::VectorXd y_des_; - Eigen::VectorXd ydot_des_; - Eigen::VectorXd yddot_des_; - Eigen::VectorXd yddot_des_converted_; - - // Commanded acceleration after feedback terms - Eigen::VectorXd yddot_command_; - // OSC solution - Eigen::VectorXd yddot_command_sol_; + // Flags + bool impact_invariant_projection_ = false; + bool no_derivative_feedback_ = false; // `state_` is the finite state machine state when the tracking is enabled // If `state_` is empty, then the tracking is always on. @@ -161,45 +159,55 @@ class OscTrackingData { // Cost weights Eigen::MatrixXd W_; - /// OSC calculates feedback positions/velocities from `plant_w_spr_`, - /// but in the optimization it uses `plant_wo_spr_`. The reason of using + /// OSC calculates feedback positions/velocities from `plant_`, + /// but in the optimization it uses `plant_`. The reason of using /// MultibodyPlant without springs is that the OSC cannot track desired /// acceleration instantaneously when springs exist. (relative degrees of 4) - const drake::multibody::MultibodyPlant& plant_w_spr_; - const drake::multibody::MultibodyPlant& plant_wo_spr_; + const drake::multibody::MultibodyPlant& plant_; // World frames - const drake::multibody::RigidBodyFrame& world_w_spr_; - const drake::multibody::RigidBodyFrame& world_wo_spr_; + const drake::multibody::RigidBodyFrame& world_; + // Trajectory name + std::string name_; private: void UpdateDesired(const drake::trajectories::Trajectory& traj, - double t, double t_since_state_switch); + double t, double t_since_state_switch, + OscTrackingDataState& tracking_data_state) const; // Update actual output methods virtual void UpdateY( - const Eigen::VectorXd& x_w_spr, - const drake::systems::Context& context_w_spr) = 0; + const Eigen::VectorXd& x, + const drake::systems::Context& context, + OscTrackingDataState& tracking_data_state) const = 0; virtual void UpdateYdot( - const Eigen::VectorXd& x_w_spr, - const drake::systems::Context& context_w_spr) = 0; + const Eigen::VectorXd& x, + const drake::systems::Context& context, + OscTrackingDataState& tracking_data_state) const = 0; virtual void UpdateJ( - const Eigen::VectorXd& x_wo_spr, - const drake::systems::Context& context_wo_spr) = 0; + const Eigen::VectorXd& x, + const drake::systems::Context& context, + OscTrackingDataState& tracking_data_state) const = 0; virtual void UpdateJdotV( - const Eigen::VectorXd& x_wo_spr, - const drake::systems::Context& context_wo_spr) = 0; + const Eigen::VectorXd& x, + const drake::systems::Context& context, + OscTrackingDataState& tracking_data_state) const = 0; + // Update error methods - virtual void UpdateYError() = 0; - virtual void UpdateYdotError(const Eigen::VectorXd& v_proj) = 0; - virtual void UpdateYddotDes(double t, double t_since_state_switch) = 0; - virtual void UpdateYddotCmd(double t, double t_since_state_switch); + virtual void UpdateYError(OscTrackingDataState& tracking_data_state) const = 0; + virtual void UpdateYdotError( + const Eigen::VectorXd& v_proj, + OscTrackingDataState& tracking_data_state) const = 0; + virtual void UpdateYddotDes( + double t, double t_since_state_switch, + OscTrackingDataState& tracking_data_state) const = 0; + virtual void UpdateYddotCmd( + double t, double t_since_state_switch, + OscTrackingDataState& tracking_data_state) const; // Finalize and ensure that users construct OscTrackingData derived class // correctly. virtual void CheckDerivedOscTrackingData() = 0; - // Trajectory name - std::string name_; }; } // namespace controllers diff --git a/systems/controllers/osc/relative_translation_tracking_data.cc b/systems/controllers/osc/relative_translation_tracking_data.cc index eaf179611d..b3f9249151 100644 --- a/systems/controllers/osc/relative_translation_tracking_data.cc +++ b/systems/controllers/osc/relative_translation_tracking_data.cc @@ -19,54 +19,57 @@ namespace dairlib::systems::controllers { RelativeTranslationTrackingData::RelativeTranslationTrackingData( const std::string& name, const Eigen::MatrixXd& K_p, const Eigen::MatrixXd& K_d, const Eigen::MatrixXd& W, - const drake::multibody::MultibodyPlant& plant_w_spr, - const drake::multibody::MultibodyPlant& plant_wo_spr, + const drake::multibody::MultibodyPlant& plant, OptionsTrackingData* to_frame_data, OptionsTrackingData* from_frame_data) - : OptionsTrackingData(name, kSpaceDim, kSpaceDim, K_p, K_d, W, plant_w_spr, - plant_wo_spr), + : OptionsTrackingData(name, kSpaceDim, kSpaceDim, K_p, K_d, W, plant), to_frame_data_(to_frame_data), from_frame_data_(from_frame_data) { auto states1 = to_frame_data->GetActiveStates(); auto states2 = from_frame_data->GetActiveStates(); active_fsm_states_ = states1; DRAKE_DEMAND(states1 == states2); + + to_state_ = to_frame_data_->AllocateState(); + from_state_ = from_frame_data_->AllocateState(); } void RelativeTranslationTrackingData::Update( - const VectorXd& x_w_spr, const Context& context_w_spr, - const VectorXd& x_wo_spr, const Context& context_wo_spr, + const VectorXd& x, const Context& context, const drake::trajectories::Trajectory& traj, double t, - double t_gait_cycle, const int fsm_state, const VectorXd& v_proj) { + double t_gait_cycle, const int fsm_state, const VectorXd& v_proj, + OscTrackingDataState& td_state) const { + // Currently there are redundant calculation here. For both to_frame_data_, // and from_frame_data_, we don't nee to run UpdateDesired, UpdateYError, // UpdateYdotError and UpdateYddotCmd inside Update(). // TODO: improve this to make it slightly more efficient. - to_frame_data_->Update(x_w_spr, context_w_spr, x_wo_spr, context_wo_spr, traj, - t, t_gait_cycle, fsm_state, v_proj); - from_frame_data_->Update(x_w_spr, context_w_spr, x_wo_spr, context_wo_spr, - traj, t, t_gait_cycle, fsm_state, v_proj); - OptionsTrackingData::Update(x_w_spr, context_w_spr, x_wo_spr, context_wo_spr, - traj, t, t_gait_cycle, fsm_state, v_proj); + to_frame_data_->Update(x, context, traj,t, t_gait_cycle, fsm_state, v_proj, to_state_); + from_frame_data_->Update(x, context, traj, t, t_gait_cycle, fsm_state, v_proj, from_state_); + OptionsTrackingData::Update(x, context, traj, t, t_gait_cycle, fsm_state, v_proj, td_state); } void RelativeTranslationTrackingData::UpdateY( - const VectorXd& x_w_spr, const Context& context_w_spr) { - y_ = to_frame_data_->GetY() - from_frame_data_->GetY(); + const VectorXd& x, const Context& context, + OscTrackingDataState& td_state) const { + td_state.y_ = to_state_.y_ - from_state_.y_; } void RelativeTranslationTrackingData::UpdateYdot( - const VectorXd& x_w_spr, const Context& context_w_spr) { - ydot_ = to_frame_data_->GetYdot() - from_frame_data_->GetYdot(); + const VectorXd& x, const Context& context, + OscTrackingDataState& td_state) const { + td_state.ydot_ = to_state_.ydot_ - from_state_.ydot_; } void RelativeTranslationTrackingData::UpdateJ( - const VectorXd& x_wo_spr, const Context& context_wo_spr) { - J_ = to_frame_data_->GetJ() - from_frame_data_->GetJ(); + const VectorXd& x, const Context& context, + OscTrackingDataState& td_state) const { + td_state.J_ = to_state_.J_ - from_state_.J_; } void RelativeTranslationTrackingData::UpdateJdotV( - const VectorXd& x_wo_spr, const Context& context_wo_spr) { - JdotV_ = to_frame_data_->GetJdotTimesV() - from_frame_data_->GetJdotTimesV(); + const VectorXd& x, const Context& context, + OscTrackingDataState& td_state) const { + td_state.JdotV_ = to_state_.JdotV_ - from_state_.JdotV_; } void RelativeTranslationTrackingData::CheckDerivedOscTrackingData() { diff --git a/systems/controllers/osc/relative_translation_tracking_data.h b/systems/controllers/osc/relative_translation_tracking_data.h index 73dbda1d65..7927bc6ad1 100644 --- a/systems/controllers/osc/relative_translation_tracking_data.h +++ b/systems/controllers/osc/relative_translation_tracking_data.h @@ -23,32 +23,39 @@ class RelativeTranslationTrackingData final : public OptionsTrackingData { RelativeTranslationTrackingData( const std::string& name, const Eigen::MatrixXd& K_p, const Eigen::MatrixXd& K_d, const Eigen::MatrixXd& W, - const drake::multibody::MultibodyPlant& plant_w_spr, - const drake::multibody::MultibodyPlant& plant_wo_spr, + const drake::multibody::MultibodyPlant& plant, OptionsTrackingData* to_frame_data, OptionsTrackingData* from_frame_data); - void Update(const Eigen::VectorXd& x_w_spr, - const drake::systems::Context& context_w_spr, - const Eigen::VectorXd& x_wo_spr, - const drake::systems::Context& context_wo_spr, + void Update(const Eigen::VectorXd& x, + const drake::systems::Context& context, const drake::trajectories::Trajectory& traj, double t, double t_gait_cycle, int fsm_state, - const Eigen::VectorXd& v_proj) final; + const Eigen::VectorXd& v_proj, + OscTrackingDataState& tracking_data_state) const final; private: - void UpdateY(const Eigen::VectorXd& x_wo_spr, - const drake::systems::Context& context_wo_spr) final; - void UpdateYdot(const Eigen::VectorXd& x_wo_spr, - const drake::systems::Context& context_wo_spr) final; - void UpdateJ(const Eigen::VectorXd& x_wo_spr, - const drake::systems::Context& context_wo_spr) final; - void UpdateJdotV(const Eigen::VectorXd& x_wo_spr, - const drake::systems::Context& context_wo_spr) final; + void UpdateY(const Eigen::VectorXd& x, + const drake::systems::Context& context, + OscTrackingDataState& tracking_data_state) const final; + void UpdateYdot(const Eigen::VectorXd& x, + const drake::systems::Context& context, + OscTrackingDataState& tracking_data_state) const final; + void UpdateJ(const Eigen::VectorXd& x, + const drake::systems::Context& context, + OscTrackingDataState& tracking_data_state) const final; + void UpdateJdotV(const Eigen::VectorXd& x, + const drake::systems::Context& context, + OscTrackingDataState& tracking_data_state) const final; void CheckDerivedOscTrackingData() final; OptionsTrackingData* to_frame_data_; OptionsTrackingData* from_frame_data_; + + // Used to update to and from tracking data, + // but all persistent state is stored in external state + mutable OscTrackingDataState to_state_; + mutable OscTrackingDataState from_state_; }; } // namespace controllers diff --git a/systems/controllers/osc/rot_space_tracking_data.cc b/systems/controllers/osc/rot_space_tracking_data.cc index 60e0174287..addb6d2b61 100644 --- a/systems/controllers/osc/rot_space_tracking_data.cc +++ b/systems/controllers/osc/rot_space_tracking_data.cc @@ -20,10 +20,8 @@ namespace dairlib::systems::controllers { /**** RotTaskSpaceTrackingData ****/ RotTaskSpaceTrackingData::RotTaskSpaceTrackingData( const string& name, const MatrixXd& K_p, const MatrixXd& K_d, - const MatrixXd& W, const MultibodyPlant& plant_w_spr, - const MultibodyPlant& plant_wo_spr) - : OptionsTrackingData(name, kQuaternionDim, kSpaceDim, K_p, K_d, W, - plant_w_spr, plant_wo_spr) { + const MatrixXd& W, const MultibodyPlant& plant) + : OptionsTrackingData(name, kQuaternionDim, kSpaceDim, K_p, K_d, W, plant) { is_rotational_tracking_data_ = true; } @@ -35,97 +33,83 @@ void RotTaskSpaceTrackingData::AddStateAndFrameToTrack( int fsm_state, const std::string& body_name, const Eigen::Isometry3d& frame_pose) { AddFiniteStateToTrack(fsm_state); - DRAKE_DEMAND(plant_w_spr_.HasBodyNamed(body_name)); - DRAKE_DEMAND(plant_wo_spr_.HasBodyNamed(body_name)); - body_frames_w_spr_[fsm_state] = - &plant_w_spr_.GetBodyByName(body_name).body_frame(); - body_frames_wo_spr_[fsm_state] = - &plant_wo_spr_.GetBodyByName(body_name).body_frame(); + DRAKE_DEMAND(plant_.HasBodyNamed(body_name)); + DRAKE_DEMAND(plant_.HasBodyNamed(body_name)); + body_frames_[fsm_state] = + &plant_.GetBodyByName(body_name).body_frame(); frame_poses_[fsm_state] = frame_pose; } -void RotTaskSpaceTrackingData::UpdateY(const VectorXd& x_w_spr, - const Context& context_w_spr) { - auto transform_mat = plant_w_spr_.CalcRelativeTransform( - context_w_spr, plant_w_spr_.world_frame(), - *body_frames_w_spr_[fsm_state_]); +void RotTaskSpaceTrackingData::UpdateY( + const VectorXd& x, const Context& context, + OscTrackingDataState& td_state) const { + auto transform_mat = plant_.CalcRelativeTransform( + context, plant_.world_frame(), *body_frames_.at(td_state.fsm_state_)); Quaterniond y_quat(transform_mat.rotation() * - frame_poses_[fsm_state_].linear()); + frame_poses_.at(td_state.fsm_state_).linear()); Eigen::Vector4d y_4d; y_4d << y_quat.w(), y_quat.vec(); - y_ = y_4d; + td_state.y_ = y_4d; } -void RotTaskSpaceTrackingData::UpdateYError() { - DRAKE_DEMAND(y_des_.size() == 4); - Quaterniond y_quat_des(y_des_(0), y_des_(1), y_des_(2), y_des_(3)); - Quaterniond y_quat(y_(0), y_(1), y_(2), y_(3)); +void RotTaskSpaceTrackingData::UpdateYError(OscTrackingDataState& td_state) const { + DRAKE_DEMAND(td_state.y_des_.size() == 4); + Quaterniond y_quat_des(td_state.y_des_(0), td_state.y_des_(1), td_state.y_des_(2), td_state.y_des_(3)); + Quaterniond y_quat(td_state.y_(0), td_state.y_(1), td_state.y_(2), td_state.y_(3)); Eigen::AngleAxis angle_axis_diff(y_quat_des * y_quat.inverse()); - error_y_ = angle_axis_diff.angle() * angle_axis_diff.axis(); + td_state.error_y_ = angle_axis_diff.angle() * angle_axis_diff.axis(); if (with_view_frame_) { - error_y_ = view_frame_rot_T_ * error_y_; + td_state.error_y_ = td_state.view_frame_rot_T_ * td_state.error_y_; } } void RotTaskSpaceTrackingData::UpdateYdot( - const VectorXd& x_w_spr, const Context& context_w_spr) { - MatrixXd J_spatial(6, plant_w_spr_.num_velocities()); - plant_w_spr_.CalcJacobianSpatialVelocity( - context_w_spr, JacobianWrtVariable::kV, *body_frames_w_spr_[fsm_state_], - frame_poses_[fsm_state_].translation(), world_w_spr_, world_w_spr_, + const VectorXd& x, const Context& context, OscTrackingDataState& td_state) const { + MatrixXd J_spatial(6, plant_.num_velocities()); + plant_.CalcJacobianSpatialVelocity( + context, JacobianWrtVariable::kV, *body_frames_.at(td_state.fsm_state_), + frame_poses_.at(td_state.fsm_state_).translation(), world_, world_, &J_spatial); - ydot_ = J_spatial.block(0, 0, kSpaceDim, J_spatial.cols()) * - x_w_spr.tail(plant_w_spr_.num_velocities()); + td_state.ydot_ = J_spatial.block(0, 0, kSpaceDim, J_spatial.cols()) * + x.tail(plant_.num_velocities()); } -void RotTaskSpaceTrackingData::UpdateYdotError(const Eigen::VectorXd& v_proj) { - // Transform qdot to w - Quaterniond y_quat_des(y_des_(0), y_des_(1), y_des_(2), y_des_(3)); - Quaterniond dy_quat_des(ydot_des_(0), ydot_des_(1), ydot_des_(2), - ydot_des_(3)); - Vector3d w_des_ = 2 * (dy_quat_des * y_quat_des.conjugate()).vec(); +void RotTaskSpaceTrackingData::UpdateYdotError( + const Eigen::VectorXd& v_proj, OscTrackingDataState& td_state) const { + DRAKE_DEMAND(td_state.ydot_des_.size() == 3); // Because we transform the error here rather than in the parent // options_tracking_data, and because J_y is already transformed in the view // frame, we need to undo the transformation on J_y - error_ydot_ = - w_des_ - ydot_ - view_frame_rot_T_.transpose() * GetJ() * v_proj; + td_state.error_ydot_ = + td_state.ydot_des_ - td_state.ydot_ - td_state.view_frame_rot_T_.transpose() * td_state.J_ * v_proj; if (with_view_frame_) { - error_ydot_ = view_frame_rot_T_ * error_ydot_; + td_state.error_ydot_ = td_state.view_frame_rot_T_ * td_state.error_ydot_; } - - ydot_des_ = - w_des_; // Overwrite 4d quat_dot with 3d omega. Need this for osc logging } -void RotTaskSpaceTrackingData::UpdateJ(const VectorXd& x_wo_spr, - const Context& context_wo_spr) { - MatrixXd J_spatial(6, plant_wo_spr_.num_velocities()); - plant_wo_spr_.CalcJacobianSpatialVelocity( - context_wo_spr, JacobianWrtVariable::kV, *body_frames_wo_spr_[fsm_state_], - frame_poses_[fsm_state_].translation(), world_wo_spr_, world_wo_spr_, +void RotTaskSpaceTrackingData::UpdateJ(const VectorXd& x, + const Context& context, OscTrackingDataState& td_state) const { + MatrixXd J_spatial(6, plant_.num_velocities()); + plant_.CalcJacobianSpatialVelocity( + context, JacobianWrtVariable::kV, *body_frames_.at(td_state.fsm_state_), + frame_poses_.at(td_state.fsm_state_).translation(), world_, world_, &J_spatial); - J_ = J_spatial.block(0, 0, kSpaceDim, J_spatial.cols()); + td_state.J_ = J_spatial.block(0, 0, kSpaceDim, J_spatial.cols()); } void RotTaskSpaceTrackingData::UpdateJdotV( - const VectorXd& x_wo_spr, const Context& context_wo_spr) { - JdotV_ = - plant_wo_spr_ - .CalcBiasSpatialAcceleration(context_wo_spr, JacobianWrtVariable::kV, - *body_frames_wo_spr_[fsm_state_], - frame_poses_[fsm_state_].translation(), - world_wo_spr_, world_wo_spr_) - .rotational(); + const VectorXd& x, const Context& context, OscTrackingDataState& td_state) const { + td_state.JdotV_ = plant_.CalcBiasSpatialAcceleration( + context, JacobianWrtVariable::kV, *body_frames_.at(td_state.fsm_state_), + frame_poses_.at(td_state.fsm_state_).translation(), world_, world_ + ).rotational(); } -void RotTaskSpaceTrackingData::UpdateYddotDes(double, double) { - // Convert ddq into angular acceleration - // See https://physics.stackexchange.com/q/460311 - Quaterniond y_quat_des(y_des_(0), y_des_(1), y_des_(2), y_des_(3)); - Quaterniond yddot_quat_des(yddot_des_(0), yddot_des_(1), yddot_des_(2), - yddot_des_(3)); - yddot_des_converted_ = 2 * (yddot_quat_des * y_quat_des.conjugate()).vec(); +void RotTaskSpaceTrackingData::UpdateYddotDes( + double, double, OscTrackingDataState& td_state) const { + DRAKE_DEMAND(td_state.yddot_des_.size() == 3); + td_state.yddot_des_converted_ = td_state.yddot_des_; if (!idx_zero_feedforward_accel_.empty()) { std::cerr << "RotTaskSpaceTrackingData does not support zero feedforward " "acceleration"; @@ -136,9 +120,5 @@ void RotTaskSpaceTrackingData::UpdateYddotDes(double, double) { } } -void RotTaskSpaceTrackingData::CheckDerivedOscTrackingData() { - if (!body_frames_w_spr_.empty()) { - body_frames_w_spr_ = body_frames_wo_spr_; - } -} +void RotTaskSpaceTrackingData::CheckDerivedOscTrackingData() {} } // namespace dairlib::systems::controllers diff --git a/systems/controllers/osc/rot_space_tracking_data.h b/systems/controllers/osc/rot_space_tracking_data.h index 6b8b9aebb4..2b28ad86a9 100644 --- a/systems/controllers/osc/rot_space_tracking_data.h +++ b/systems/controllers/osc/rot_space_tracking_data.h @@ -25,8 +25,7 @@ class RotTaskSpaceTrackingData final : public OptionsTrackingData { RotTaskSpaceTrackingData( const std::string& name, const Eigen::MatrixXd& K_p, const Eigen::MatrixXd& K_d, const Eigen::MatrixXd& W, - const drake::multibody::MultibodyPlant& plant_w_spr, - const drake::multibody::MultibodyPlant& plant_wo_spr); + const drake::multibody::MultibodyPlant& plant); void AddFrameToTrack( const std::string& body_name, @@ -37,22 +36,26 @@ class RotTaskSpaceTrackingData final : public OptionsTrackingData { protected: std::unordered_map*> - body_frames_w_spr_; - std::unordered_map*> - body_frames_wo_spr_; + body_frames_; private: - void UpdateY(const Eigen::VectorXd& x_w_spr, - const drake::systems::Context& context_w_spr) final; - void UpdateYError() final; - void UpdateYdot(const Eigen::VectorXd& x_w_spr, - const drake::systems::Context& context_w_spr) final; - void UpdateYdotError(const Eigen::VectorXd& v_proj) final; - void UpdateJ(const Eigen::VectorXd& x_wo_spr, - const drake::systems::Context& context_wo_spr) final; - void UpdateJdotV(const Eigen::VectorXd& x_wo_spr, - const drake::systems::Context& context_wo_spr) final; - void UpdateYddotDes(double t, double t_since_state_switch) override; + void UpdateY(const Eigen::VectorXd& x, + const drake::systems::Context& context, + OscTrackingDataState& tracking_data_state) const final; + void UpdateYError(OscTrackingDataState& tracking_data_state) const final; + void UpdateYdot(const Eigen::VectorXd& x, + const drake::systems::Context& context, + OscTrackingDataState& tracking_data_state) const final; + void UpdateYdotError(const Eigen::VectorXd& v_proj, + OscTrackingDataState& tracking_data_state) const final; + void UpdateJ(const Eigen::VectorXd& x, + const drake::systems::Context& context, + OscTrackingDataState& tracking_data_state) const final; + void UpdateJdotV(const Eigen::VectorXd& x, + const drake::systems::Context& context, + OscTrackingDataState& tracking_data_state) const final; + void UpdateYddotDes(double t, double t_since_state_switch, + OscTrackingDataState& tracking_data_state) const override; void CheckDerivedOscTrackingData() final; // frame_pose_ represents the pose of the frame (w.r.t. the body's frame) diff --git a/systems/controllers/osc/trans_space_tracking_data.cc b/systems/controllers/osc/trans_space_tracking_data.cc index c6dd155001..00dcfd0971 100644 --- a/systems/controllers/osc/trans_space_tracking_data.cc +++ b/systems/controllers/osc/trans_space_tracking_data.cc @@ -17,10 +17,8 @@ namespace dairlib::systems::controllers { /**** TransTaskSpaceTrackingData ****/ TransTaskSpaceTrackingData::TransTaskSpaceTrackingData( const string& name, const MatrixXd& K_p, const MatrixXd& K_d, - const MatrixXd& W, const MultibodyPlant& plant_w_spr, - const MultibodyPlant& plant_wo_spr) - : OptionsTrackingData(name, kSpaceDim, kSpaceDim, K_p, K_d, W, plant_w_spr, - plant_wo_spr) {} + const MatrixXd& W, const MultibodyPlant& plant) + : OptionsTrackingData(name, kSpaceDim, kSpaceDim, K_p, K_d, W, plant) {} void TransTaskSpaceTrackingData::AddPointToTrack(const std::string& body_name, const Vector3d& pt_on_body) { @@ -29,53 +27,51 @@ void TransTaskSpaceTrackingData::AddPointToTrack(const std::string& body_name, void TransTaskSpaceTrackingData::AddStateAndPointToTrack( int fsm_state, const std::string& body_name, const Vector3d& pt_on_body) { AddFiniteStateToTrack(fsm_state); - DRAKE_DEMAND(plant_w_spr_.HasBodyNamed(body_name)); - DRAKE_DEMAND(plant_wo_spr_.HasBodyNamed(body_name)); - body_frames_w_spr_[fsm_state] = - &plant_w_spr_.GetBodyByName(body_name).body_frame(); - body_frames_wo_spr_[fsm_state] = - &plant_wo_spr_.GetBodyByName(body_name).body_frame(); + DRAKE_DEMAND(plant_.HasBodyNamed(body_name)); + DRAKE_DEMAND(plant_.HasBodyNamed(body_name)); + body_frames_[fsm_state] = + &plant_.GetBodyByName(body_name).body_frame(); pts_on_body_[fsm_state] = pt_on_body; } -void TransTaskSpaceTrackingData::UpdateY(const VectorXd& x_w_spr, - const Context& context_w_spr) { - y_ = Vector3d::Zero(); - plant_w_spr_.CalcPointsPositions(context_w_spr, - *body_frames_wo_spr_.at(fsm_state_), - pts_on_body_[fsm_state_], world_w_spr_, &y_); +void TransTaskSpaceTrackingData::UpdateY( + const VectorXd& x, const Context& context, + OscTrackingDataState& td_state) const { + td_state.y_ = Vector3d::Zero(); + plant_.CalcPointsPositions( + context, *body_frames_.at(td_state.fsm_state_), + pts_on_body_.at(td_state.fsm_state_), world_, &td_state.y_); } void TransTaskSpaceTrackingData::UpdateYdot( - const VectorXd& x_w_spr, const Context& context_w_spr) { - MatrixXd J(kSpaceDim, plant_w_spr_.num_velocities()); - plant_w_spr_.CalcJacobianTranslationalVelocity( - context_w_spr, JacobianWrtVariable::kV, - *body_frames_w_spr_.at(fsm_state_), pts_on_body_[fsm_state_], - world_w_spr_, world_w_spr_, &J); - ydot_ = J * x_w_spr.tail(plant_w_spr_.num_velocities()); + const VectorXd& x, const Context& context, + OscTrackingDataState& td_state) const { + MatrixXd J(kSpaceDim, plant_.num_velocities()); + plant_.CalcJacobianTranslationalVelocity( + context, JacobianWrtVariable::kV, + *body_frames_.at(td_state.fsm_state_), pts_on_body_.at(td_state.fsm_state_), + world_, world_, &J); + td_state.ydot_ = J * x.tail(plant_.num_velocities()); } void TransTaskSpaceTrackingData::UpdateJ( - const VectorXd& x_wo_spr, const Context& context_wo_spr) { - J_ = MatrixXd::Zero(kSpaceDim, plant_wo_spr_.num_velocities()); - plant_wo_spr_.CalcJacobianTranslationalVelocity( - context_wo_spr, JacobianWrtVariable::kV, - *body_frames_wo_spr_.at(fsm_state_), pts_on_body_[fsm_state_], - world_wo_spr_, world_wo_spr_, &J_); + const VectorXd& x, const Context& context, + OscTrackingDataState& td_state) const { + td_state.J_ = MatrixXd::Zero(kSpaceDim, plant_.num_velocities()); + plant_.CalcJacobianTranslationalVelocity( + context, JacobianWrtVariable::kV, + *body_frames_.at(td_state.fsm_state_), pts_on_body_.at(td_state.fsm_state_), + world_, world_, &td_state.J_); } void TransTaskSpaceTrackingData::UpdateJdotV( - const VectorXd& x_wo_spr, const Context& context_wo_spr) { - JdotV_ = plant_wo_spr_.CalcBiasTranslationalAcceleration( - context_wo_spr, drake::multibody::JacobianWrtVariable::kV, - *body_frames_wo_spr_.at(fsm_state_), pts_on_body_[fsm_state_], - world_wo_spr_, world_wo_spr_); + const VectorXd& x, const Context& context, + OscTrackingDataState& td_state) const { + td_state.JdotV_ = plant_.CalcBiasTranslationalAcceleration( + context, drake::multibody::JacobianWrtVariable::kV, + *body_frames_.at(td_state.fsm_state_), pts_on_body_.at(td_state.fsm_state_), + world_, world_); } -void TransTaskSpaceTrackingData::CheckDerivedOscTrackingData() { - if (!body_frames_w_spr_.empty()) { - body_frames_w_spr_ = body_frames_wo_spr_; - } -} +void TransTaskSpaceTrackingData::CheckDerivedOscTrackingData() {} } // namespace dairlib::systems::controllers diff --git a/systems/controllers/osc/trans_space_tracking_data.h b/systems/controllers/osc/trans_space_tracking_data.h index 909f150803..05f0a46e16 100644 --- a/systems/controllers/osc/trans_space_tracking_data.h +++ b/systems/controllers/osc/trans_space_tracking_data.h @@ -23,8 +23,7 @@ class TransTaskSpaceTrackingData final : public OptionsTrackingData { TransTaskSpaceTrackingData( const std::string& name, const Eigen::MatrixXd& K_p, const Eigen::MatrixXd& K_d, const Eigen::MatrixXd& W, - const drake::multibody::MultibodyPlant& plant_w_spr, - const drake::multibody::MultibodyPlant& plant_wo_sp); + const drake::multibody::MultibodyPlant& plant); void AddPointToTrack( const std::string& body_name, const Eigen::Vector3d& pt_on_body = Eigen::Vector3d::Zero()); @@ -34,19 +33,21 @@ class TransTaskSpaceTrackingData final : public OptionsTrackingData { protected: std::unordered_map*> - body_frames_w_spr_; - std::unordered_map*> - body_frames_wo_spr_; + body_frames_; private: - void UpdateY(const Eigen::VectorXd& x_w_spr, - const drake::systems::Context& context_w_spr) final; - void UpdateYdot(const Eigen::VectorXd& x_w_spr, - const drake::systems::Context& context_w_spr) final; - void UpdateJ(const Eigen::VectorXd& x_wo_spr, - const drake::systems::Context& context_wo_spr) final; - void UpdateJdotV(const Eigen::VectorXd& x_wo_spr, - const drake::systems::Context& context_wo_spr) final; + void UpdateY(const Eigen::VectorXd& x, + const drake::systems::Context& context, + OscTrackingDataState& tracking_data_state) const final; + void UpdateYdot(const Eigen::VectorXd& x, + const drake::systems::Context& context, + OscTrackingDataState& tracking_data_state) const final; + void UpdateJ(const Eigen::VectorXd& x, + const drake::systems::Context& context, + OscTrackingDataState& tracking_data_state) const final; + void UpdateJdotV(const Eigen::VectorXd& x, + const drake::systems::Context& context, + OscTrackingDataState& tracking_data_state) const final; void CheckDerivedOscTrackingData() final;