diff --git a/bindings/pydairlib/multibody/BUILD.bazel b/bindings/pydairlib/multibody/BUILD.bazel index 9de8f1de70..eb6c8266c5 100644 --- a/bindings/pydairlib/multibody/BUILD.bazel +++ b/bindings/pydairlib/multibody/BUILD.bazel @@ -16,6 +16,7 @@ pybind_py_library( cc_deps = [ "//multibody:multipose_visualizer", "//multibody:utils", + "//multibody:visualization_utils", "@drake//:drake_shared_library", ], cc_so_name = "multibody", diff --git a/bindings/pydairlib/multibody/multibody_py.cc b/bindings/pydairlib/multibody/multibody_py.cc index 896d7e4789..2a993d785e 100644 --- a/bindings/pydairlib/multibody/multibody_py.cc +++ b/bindings/pydairlib/multibody/multibody_py.cc @@ -5,6 +5,7 @@ #include "multibody/multibody_utils.h" #include "multibody/multipose_visualizer.h" +#include "multibody/visualization_utils.h" namespace py = pybind11; @@ -22,12 +23,18 @@ PYBIND11_MODULE(multibody, m) { .def(py::init()) .def(py::init()) .def(py::init()) - .def("DrawPoses", &MultiposeVisualizer::DrawPoses, py::arg("poses")); + .def("DrawPoses", &MultiposeVisualizer::DrawPoses, py::arg("poses")) + .def("GetMeshcat", &MultiposeVisualizer::GetMeshcat); + + m.def("ConnectTrajectoryVisualizer", + &dairlib::multibody::ConnectTrajectoryVisualizer, py::arg("plant"), + py::arg("builder"), py::arg("scene_graph"), py::arg("trajectory")); m.def("MakeNameToPositionsMap", - &dairlib::multibody::MakeNameToPositionsMap, py::arg("plant")) + py::overload_cast&>(&dairlib::multibody::MakeNameToPositionsMap), + py::arg("plant")) .def("MakeNameToVelocitiesMap", - &dairlib::multibody::MakeNameToVelocitiesMap, + py::overload_cast&>(&dairlib::multibody::MakeNameToVelocitiesMap), py::arg("plant")) .def("MakeNameToActuatorsMap", &dairlib::multibody::MakeNameToActuatorsMap, @@ -38,10 +45,17 @@ PYBIND11_MODULE(multibody, m) { .def("CreateActuatorNameVectorFromMap", &dairlib::multibody::CreateActuatorNameVectorFromMap, py::arg("plant")) + .def("CreateWithSpringsToWithoutSpringsMapPos", + &dairlib::multibody::CreateWithSpringsToWithoutSpringsMapPos, + py::arg("plant_w_spr"), py::arg("plant_wo_spr")) + .def("CreateWithSpringsToWithoutSpringsMapVel", + &dairlib::multibody::CreateWithSpringsToWithoutSpringsMapVel, + py::arg("plant_w_spr"), py::arg("plant_wo_spr")) .def("AddFlatTerrain", &dairlib::multibody::AddFlatTerrain, py::arg("plant"), py::arg("scene_graph"), py::arg("mu_static"), py::arg("mu_kinetic"), py::arg("normal_W") = Eigen::Vector3d(0, 0, 1), + py::arg("stiffness") = 0, py::arg("dissipation_rate") = 0, py::arg("show_ground") = 1); } diff --git a/bindings/pydairlib/systems/robot_lcm_systems_py.cc b/bindings/pydairlib/systems/robot_lcm_systems_py.cc index 8117020da2..606197e509 100644 --- a/bindings/pydairlib/systems/robot_lcm_systems_py.cc +++ b/bindings/pydairlib/systems/robot_lcm_systems_py.cc @@ -8,7 +8,6 @@ #include "drake/multibody/plant/multibody_plant.h" namespace py = pybind11; -using py_rvp = py::return_value_policy; namespace dairlib { namespace pydairlib { @@ -18,32 +17,42 @@ PYBIND11_MODULE(robot_lcm_systems, m) { using drake::multibody::MultibodyPlant; using systems::RobotOutputSender; + using py_rvp = py::return_value_policy; py::class_>( m, "RobotOutputReceiver") - .def(py::init&>()); + .def(py::init&>()) + .def(py::init&, + drake::multibody::ModelInstanceIndex>()); py::class_>( m, "RobotInputReceiver") .def(py::init&>()); py::class_>( m, "RobotOutputSender") - .def(py::init&, bool>()) + .def(py::init&, bool, bool>()) + .def(py::init&, + drake::multibody::ModelInstanceIndex, + bool, bool>()) .def("get_input_port_state", &RobotOutputSender::get_input_port_state, - py_rvp::reference_internal) + py_rvp::reference_internal) .def("get_input_port_effort", &RobotOutputSender::get_input_port_effort, - py_rvp::reference_internal) + py_rvp::reference_internal) .def("get_input_port_imu", &RobotOutputSender::get_input_port_imu, - py_rvp::reference_internal); + py_rvp::reference_internal); py::class_>( m, "RobotCommandSender") .def(py::init&>()); m.def("AddActuationRecieverAndStateSenderLcm", - &dairlib::systems::AddActuationRecieverAndStateSenderLcm, + py::overload_cast*, + const MultibodyPlant&, + drake::systems::lcm::LcmInterfaceSystem*, std::string, + std::string, double, + drake::multibody::ModelInstanceIndex, bool, double>( + &dairlib::systems::AddActuationRecieverAndStateSenderLcm), py::arg("builder"), py::arg("plant"), py::arg("lcm"), py::arg("actuator_channel"), py::arg("state_channel"), - py::arg("publish_rate"), py::arg("publish_efforts"), - py::arg("actuator_delay")); - + py::arg("publish_rate"), py::arg("model_instance"), + py::arg("publish_efforts"), py::arg("actuator_delay")); } } // namespace pydairlib 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 376d5838d1..6fe105077c 100644 --- a/examples/Cassie/cassie_state_estimator.cc +++ b/examples/Cassie/cassie_state_estimator.cc @@ -435,20 +435,20 @@ void CassieStateEstimator::AssignNonFloatingBaseStateToOutputVector( void CassieStateEstimator::AssignFloatingBaseStateToOutputVector( const VectorXd& est_fb_state, OutputVector* output) const { - output->SetPositionAtIndex(position_idx_map_.at("base_qw"), est_fb_state(0)); - output->SetPositionAtIndex(position_idx_map_.at("base_qx"), est_fb_state(1)); - output->SetPositionAtIndex(position_idx_map_.at("base_qy"), est_fb_state(2)); - output->SetPositionAtIndex(position_idx_map_.at("base_qz"), est_fb_state(3)); - output->SetPositionAtIndex(position_idx_map_.at("base_x"), est_fb_state(4)); - output->SetPositionAtIndex(position_idx_map_.at("base_y"), est_fb_state(5)); - output->SetPositionAtIndex(position_idx_map_.at("base_z"), est_fb_state(6)); - - output->SetVelocityAtIndex(velocity_idx_map_.at("base_wx"), est_fb_state(7)); - output->SetVelocityAtIndex(velocity_idx_map_.at("base_wy"), est_fb_state(8)); - output->SetVelocityAtIndex(velocity_idx_map_.at("base_wz"), est_fb_state(9)); - output->SetVelocityAtIndex(velocity_idx_map_.at("base_vx"), est_fb_state(10)); - output->SetVelocityAtIndex(velocity_idx_map_.at("base_vy"), est_fb_state(11)); - output->SetVelocityAtIndex(velocity_idx_map_.at("base_vz"), est_fb_state(12)); + output->SetPositionAtIndex(position_idx_map_.at("pelvis_qw"), est_fb_state(0)); + output->SetPositionAtIndex(position_idx_map_.at("pelvis_qx"), est_fb_state(1)); + output->SetPositionAtIndex(position_idx_map_.at("pelvis_qy"), est_fb_state(2)); + output->SetPositionAtIndex(position_idx_map_.at("pelvis_qz"), est_fb_state(3)); + output->SetPositionAtIndex(position_idx_map_.at("pelvis_x"), est_fb_state(4)); + output->SetPositionAtIndex(position_idx_map_.at("pelvis_y"), est_fb_state(5)); + output->SetPositionAtIndex(position_idx_map_.at("pelvis_z"), est_fb_state(6)); + + output->SetVelocityAtIndex(velocity_idx_map_.at("pelvis_wx"), est_fb_state(7)); + output->SetVelocityAtIndex(velocity_idx_map_.at("pelvis_wy"), est_fb_state(8)); + output->SetVelocityAtIndex(velocity_idx_map_.at("pelvis_wz"), est_fb_state(9)); + output->SetVelocityAtIndex(velocity_idx_map_.at("pelvis_vx"), est_fb_state(10)); + output->SetVelocityAtIndex(velocity_idx_map_.at("pelvis_vy"), est_fb_state(11)); + output->SetVelocityAtIndex(velocity_idx_map_.at("pelvis_vz"), est_fb_state(12)); } /// EstimateContactFromSprings(). Conservative estimation. @@ -628,7 +628,7 @@ EventStatus CassieStateEstimator::Update( // is not triggered by CASSIE_STATE_SIMULATION message. // This wouldn't be an issue when you don't use ground truth state. if (output_gt.GetPositions().head(7).norm() == 0) { - output_gt.SetPositionAtIndex(position_idx_map_.at("base_qw"), 1); + output_gt.SetPositionAtIndex(position_idx_map_.at("pelvis_qw"), 1); } // Get kinematics cache for ground truth diff --git a/examples/Cassie/run_dircon_jumping.cc b/examples/Cassie/run_dircon_jumping.cc index a6c75a7df2..8521caf236 100644 --- a/examples/Cassie/run_dircon_jumping.cc +++ b/examples/Cassie/run_dircon_jumping.cc @@ -442,9 +442,9 @@ void SetKinematicConstraints(Dircon* trajopt, auto& prog = trajopt->prog(); // position constraints prog.AddBoundingBoxConstraint(0 - midpoint, 0 - midpoint, - x_0(pos_map.at("base_x"))); - prog.AddBoundingBoxConstraint(0, 0, x_0(pos_map.at("base_y"))); - prog.AddBoundingBoxConstraint(0, 0, x_f(pos_map.at("base_y"))); + x_0(pos_map.at("pelvis_x"))); + prog.AddBoundingBoxConstraint(0, 0, x_0(pos_map.at("pelvis_y"))); + prog.AddBoundingBoxConstraint(0, 0, x_f(pos_map.at("pelvis_y"))); // initial fb orientation constraint VectorXd quat_identity(4); @@ -472,13 +472,13 @@ void SetKinematicConstraints(Dircon* trajopt, // Jumping height constraints prog.AddBoundingBoxConstraint(rest_height - eps, rest_height + eps, - x_0(pos_map.at("base_z"))); + x_0(pos_map.at("pelvis_z"))); prog.AddBoundingBoxConstraint(0.5 * FLAGS_height + rest_height - eps, FLAGS_height + rest_height + eps, - x_top(pos_map.at("base_z"))); + x_top(pos_map.at("pelvis_z"))); prog.AddBoundingBoxConstraint(0.8 * FLAGS_height + rest_height - eps, 0.8 * FLAGS_height + rest_height + eps, - x_f(pos_map.at("base_z"))); + x_f(pos_map.at("pelvis_z"))); // Zero starting and final velocities prog.AddLinearConstraint(VectorXd::Zero(n_v) == x_0.tail(n_v)); diff --git a/examples/Cassie/run_dircon_squatting.cc b/examples/Cassie/run_dircon_squatting.cc index f5d5e2a36b..8385e7e385 100644 --- a/examples/Cassie/run_dircon_squatting.cc +++ b/examples/Cassie/run_dircon_squatting.cc @@ -114,13 +114,13 @@ void DoMain(double duration, int max_iter, const string& data_directory, map velocities_map = multibody::MakeNameToVelocitiesMap(plant); map actuators_map = multibody::MakeNameToActuatorsMap(plant); - int base_qw_idx = positions_map.at("base_qw"); - int base_qx_idx = positions_map.at("base_qx"); - int base_qy_idx = positions_map.at("base_qy"); - int base_qz_idx = positions_map.at("base_qz"); - int base_x_idx = positions_map.at("base_x"); - int base_y_idx = positions_map.at("base_y"); - int base_z_idx = positions_map.at("base_z"); + int pelvis_qw_idx = positions_map.at("pelvis_qw"); + int pelvis_qx_idx = positions_map.at("pelvis_qx"); + int pelvis_qy_idx = positions_map.at("pelvis_qy"); + int pelvis_qz_idx = positions_map.at("pelvis_qz"); + int pelvis_x_idx = positions_map.at("pelvis_x"); + int pelvis_y_idx = positions_map.at("pelvis_y"); + int pelvis_z_idx = positions_map.at("pelvis_z"); int hip_roll_left_idx = positions_map.at("hip_roll_left"); int hip_roll_right_idx = positions_map.at("hip_roll_right"); int hip_yaw_left_idx = positions_map.at("hip_yaw_left"); @@ -134,12 +134,12 @@ void DoMain(double duration, int max_iter, const string& data_directory, int toe_left_idx = positions_map.at("toe_left"); int toe_right_idx = positions_map.at("toe_right"); - int base_wx_idx = velocities_map.at("base_wx"); - int base_wy_idx = velocities_map.at("base_wy"); - int base_wz_idx = velocities_map.at("base_wz"); - int base_vx_idx = velocities_map.at("base_vx"); - int base_vy_idx = velocities_map.at("base_vy"); - int base_vz_idx = velocities_map.at("base_vz"); + int pelvis_wx_idx = velocities_map.at("pelvis_wx"); + int pelvis_wy_idx = velocities_map.at("pelvis_wy"); + int pelvis_wz_idx = velocities_map.at("pelvis_wz"); + int pelvis_vx_idx = velocities_map.at("pelvis_vx"); + int pelvis_vy_idx = velocities_map.at("pelvis_vy"); + int pelvis_vz_idx = velocities_map.at("pelvis_vz"); int hip_roll_leftdot_idx = velocities_map.at("hip_roll_leftdot"); int hip_roll_rightdot_idx = velocities_map.at("hip_roll_rightdot"); int hip_yaw_leftdot_idx = velocities_map.at("hip_yaw_leftdot"); @@ -238,8 +238,8 @@ void DoMain(double duration, int max_iter, const string& data_directory, double s_dyn_2 = (FLAGS_scale_variable) ? 6.0 : 1.0; double s_dyn_3 = (FLAGS_scale_variable) ? 85.0 : 1.0; double_support.SetDynamicsScale( - {base_qw_idx, base_qx_idx, base_qy_idx, base_qz_idx, base_x_idx, - base_y_idx, base_z_idx, hip_roll_left_idx, hip_roll_right_idx, + {pelvis_qw_idx, pelvis_qx_idx, pelvis_qy_idx, pelvis_qz_idx, pelvis_x_idx, + pelvis_y_idx, pelvis_z_idx, hip_roll_left_idx, hip_roll_right_idx, hip_yaw_left_idx, hip_yaw_right_idx, hip_pitch_left_idx, hip_pitch_right_idx, knee_left_idx, knee_right_idx}, 1.0 / 150.0); @@ -248,8 +248,8 @@ void DoMain(double duration, int max_iter, const string& data_directory, 1.0 / 150.0 / 3.33 / s_dyn_1); double_support.SetDynamicsScale({toe_left_idx, toe_right_idx}, 1.0 / 150.0); double_support.SetDynamicsScale( - {base_wx_idx, base_wy_idx, base_wz_idx, base_vx_idx, base_vy_idx, - base_vz_idx, hip_roll_leftdot_idx, hip_roll_rightdot_idx}, + {pelvis_wx_idx, pelvis_wy_idx, pelvis_wz_idx, pelvis_vx_idx, pelvis_vy_idx, + pelvis_vz_idx, hip_roll_leftdot_idx, hip_roll_rightdot_idx}, 1.0 / 150.0 / s_dyn_1); double_support.SetDynamicsScale({hip_yaw_leftdot_idx, hip_yaw_rightdot_idx}, 1.0 / 150.0 / s_dyn_2); @@ -338,20 +338,20 @@ void DoMain(double duration, int max_iter, const string& data_directory, auto xmid = trajopt.state_vars(0, (num_knotpoints - 1) / 2); // height constraint - prog.AddBoundingBoxConstraint(1, 1, x0(positions_map.at("base_z"))); - prog.AddBoundingBoxConstraint(1.1, 1.1, xf(positions_map.at("base_z"))); + prog.AddBoundingBoxConstraint(1, 1, x0(positions_map.at("pelvis_z"))); + prog.AddBoundingBoxConstraint(1.1, 1.1, xf(positions_map.at("pelvis_z"))); // initial pelvis position - prog.AddBoundingBoxConstraint(0, 0, x0(positions_map.at("base_x"))); - prog.AddBoundingBoxConstraint(0, 0, x0(positions_map.at("base_y"))); + prog.AddBoundingBoxConstraint(0, 0, x0(positions_map.at("pelvis_x"))); + prog.AddBoundingBoxConstraint(0, 0, x0(positions_map.at("pelvis_y"))); // pelvis pose constraints for (int i = 0; i < num_knotpoints; i++) { auto xi = trajopt.state(i); - prog.AddBoundingBoxConstraint(1, 1, xi(positions_map.at("base_qw"))); - prog.AddBoundingBoxConstraint(0, 0, xi(positions_map.at("base_qx"))); - prog.AddBoundingBoxConstraint(0, 0, xi(positions_map.at("base_qy"))); - prog.AddBoundingBoxConstraint(0, 0, xi(positions_map.at("base_qz"))); + prog.AddBoundingBoxConstraint(1, 1, xi(positions_map.at("pelvis_qw"))); + prog.AddBoundingBoxConstraint(0, 0, xi(positions_map.at("pelvis_qx"))); + prog.AddBoundingBoxConstraint(0, 0, xi(positions_map.at("pelvis_qy"))); + prog.AddBoundingBoxConstraint(0, 0, xi(positions_map.at("pelvis_qz"))); } // start/end velocity constraints @@ -449,9 +449,9 @@ void DoMain(double duration, int max_iter, const string& data_directory, trajopt.ScaleTimeVariables(0.015); // state std::vector idx_list = { - n_q + base_wx_idx, n_q + base_wy_idx, - n_q + base_wz_idx, n_q + base_vx_idx, - n_q + base_vy_idx, n_q + base_vz_idx, + n_q + pelvis_wx_idx, n_q + pelvis_wy_idx, + n_q + pelvis_wz_idx, n_q + pelvis_vx_idx, + n_q + pelvis_vy_idx, n_q + pelvis_vz_idx, n_q + hip_roll_leftdot_idx, n_q + hip_roll_rightdot_idx, n_q + hip_yaw_leftdot_idx, n_q + hip_yaw_rightdot_idx}; trajopt.ScaleStateVariables(idx_list, 6); @@ -546,13 +546,13 @@ void DoMain(double duration, int max_iter, const string& data_directory, // produces NAN value in some calculation. for (int i = 0; i < num_knotpoints; i++) { auto xi = trajopt.state(i); - if ((prog.GetInitialGuess(xi.segment<4>(base_qw_idx)).norm() == 0) || + if ((prog.GetInitialGuess(xi.segment<4>(pelvis_qw_idx)).norm() == 0) || std::isnan( - prog.GetInitialGuess(xi.segment<4>(base_qw_idx)).norm())) { - prog.SetInitialGuess(xi(base_qw_idx), 1); - prog.SetInitialGuess(xi(base_qx_idx), 0); - prog.SetInitialGuess(xi(base_qy_idx), 0); - prog.SetInitialGuess(xi(base_qz_idx), 0); + prog.GetInitialGuess(xi.segment<4>(pelvis_qw_idx)).norm())) { + prog.SetInitialGuess(xi(pelvis_qw_idx), 1); + prog.SetInitialGuess(xi(pelvis_qx_idx), 0); + prog.SetInitialGuess(xi(pelvis_qy_idx), 0); + prog.SetInitialGuess(xi(pelvis_qz_idx), 0); } } diff --git a/examples/Cassie/run_dircon_walking.cc b/examples/Cassie/run_dircon_walking.cc index ba3b2b4450..357f1c0165 100644 --- a/examples/Cassie/run_dircon_walking.cc +++ b/examples/Cassie/run_dircon_walking.cc @@ -119,13 +119,13 @@ vector GetInitGuessForQ(int N, double stride_length, map pos_value_map; Eigen::Vector4d quat(2000.06, -0.339462, -0.609533, -0.760854); quat.normalize(); - pos_value_map["base_qw"] = quat(0); - pos_value_map["base_qx"] = quat(1); - pos_value_map["base_qy"] = quat(2); - pos_value_map["base_qz"] = quat(3); - pos_value_map["base_x"] = 0.000889849; - pos_value_map["base_y"] = 0.000626865; - pos_value_map["base_z"] = 1.0009; + pos_value_map["pelvis_qw"] = quat(0); + pos_value_map["pelvis_qx"] = quat(1); + pos_value_map["pelvis_qy"] = quat(2); + pos_value_map["pelvis_qz"] = quat(3); + pos_value_map["pelvis_x"] = 0.000889849; + pos_value_map["pelvis_y"] = 0.000626865; + pos_value_map["pelvis_z"] = 1.0009; pos_value_map["hip_roll_left"] = -0.0112109; pos_value_map["hip_roll_right"] = 0.00927845; pos_value_map["hip_yaw_left"] = -0.000600725; @@ -411,13 +411,13 @@ void DoMain(double duration, double stride_length, double ground_incline, options_list[i].setConstraintRelative(3, true); } - int base_qw_idx = pos_map.at("base_qw"); - int base_qx_idx = pos_map.at("base_qx"); - int base_qy_idx = pos_map.at("base_qy"); - int base_qz_idx = pos_map.at("base_qz"); - int base_x_idx = pos_map.at("base_x"); - int base_y_idx = pos_map.at("base_y"); - int base_z_idx = pos_map.at("base_z"); + int pelvis_qw_idx = pos_map.at("pelvis_qw"); + int pelvis_qx_idx = pos_map.at("pelvis_qx"); + int pelvis_qy_idx = pos_map.at("pelvis_qy"); + int pelvis_qz_idx = pos_map.at("pelvis_qz"); + int pelvis_x_idx = pos_map.at("pelvis_x"); + int pelvis_y_idx = pos_map.at("pelvis_y"); + int pelvis_z_idx = pos_map.at("pelvis_z"); int hip_roll_left_idx = pos_map.at("hip_roll_left"); int hip_roll_right_idx = pos_map.at("hip_roll_right"); int hip_yaw_left_idx = pos_map.at("hip_yaw_left"); @@ -431,12 +431,12 @@ void DoMain(double duration, double stride_length, double ground_incline, int toe_left_idx = pos_map.at("toe_left"); int toe_right_idx = pos_map.at("toe_right"); - int base_wx_idx = vel_map.at("base_wx"); - int base_wy_idx = vel_map.at("base_wy"); - int base_wz_idx = vel_map.at("base_wz"); - int base_vx_idx = vel_map.at("base_vx"); - int base_vy_idx = vel_map.at("base_vy"); - int base_vz_idx = vel_map.at("base_vz"); + int pelvis_wx_idx = vel_map.at("pelvis_wx"); + int pelvis_wy_idx = vel_map.at("pelvis_wy"); + int pelvis_wz_idx = vel_map.at("pelvis_wz"); + int pelvis_vx_idx = vel_map.at("pelvis_vx"); + int pelvis_vy_idx = vel_map.at("pelvis_vy"); + int pelvis_vz_idx = vel_map.at("pelvis_vz"); int hip_roll_leftdot_idx = vel_map.at("hip_roll_leftdot"); int hip_roll_rightdot_idx = vel_map.at("hip_roll_rightdot"); int hip_yaw_leftdot_idx = vel_map.at("hip_yaw_leftdot"); @@ -467,9 +467,9 @@ void DoMain(double duration, double stride_length, double ground_incline, double s = 1; // scale everything together // Dynamic constraints options_list[i].setDynConstraintScaling( - {base_qw_idx, base_qx_idx, base_qy_idx, base_qz_idx}, s * 1.0 / 30.0); + {pelvis_qw_idx, pelvis_qx_idx, pelvis_qy_idx, pelvis_qz_idx}, s * 1.0 / 30.0); options_list[i].setDynConstraintScaling( - {base_x_idx, base_y_idx, base_z_idx, hip_roll_left_idx, + {pelvis_x_idx, pelvis_y_idx, pelvis_z_idx, hip_roll_left_idx, hip_roll_right_idx, hip_yaw_left_idx, hip_yaw_right_idx, hip_pitch_left_idx, hip_pitch_right_idx, knee_left_idx, knee_right_idx, ankle_joint_left_idx, ankle_joint_right_idx}, @@ -477,8 +477,8 @@ void DoMain(double duration, double stride_length, double ground_incline, options_list[i].setDynConstraintScaling({toe_left_idx, toe_right_idx}, s * 1.0 / 300.0); options_list[i].setDynConstraintScaling( - {n_q + base_wx_idx, n_q + base_wy_idx, n_q + base_wz_idx, - n_q + base_vx_idx, n_q + base_vy_idx, n_q + base_vz_idx, + {n_q + pelvis_wx_idx, n_q + pelvis_wy_idx, n_q + pelvis_wz_idx, + n_q + pelvis_vx_idx, n_q + pelvis_vy_idx, n_q + pelvis_vz_idx, n_q + hip_roll_leftdot_idx, n_q + hip_roll_rightdot_idx, n_q + hip_yaw_leftdot_idx, n_q + hip_yaw_rightdot_idx}, s * 1.0 / 600.0); @@ -503,9 +503,9 @@ void DoMain(double duration, double stride_length, double ground_incline, s * 20); // Impact constraints options_list[i].setImpConstraintScaling( - {base_wx_idx, base_wy_idx, base_wz_idx}, s / 50.0); + {pelvis_wx_idx, pelvis_wy_idx, pelvis_wz_idx}, s / 50.0); options_list[i].setImpConstraintScaling( - {base_vx_idx, base_vy_idx, base_vz_idx}, s / 300.0); + {pelvis_vx_idx, pelvis_vy_idx, pelvis_vz_idx}, s / 300.0); options_list[i].setImpConstraintScaling( {hip_roll_leftdot_idx, hip_roll_rightdot_idx}, s / 24.0); options_list[i].setImpConstraintScaling( @@ -577,23 +577,23 @@ void DoMain(double duration, double stride_length, double ground_incline, } // x position constraint - prog.AddBoundingBoxConstraint(0, 0, x0(pos_map.at("base_x"))); + prog.AddBoundingBoxConstraint(0, 0, x0(pos_map.at("pelvis_x"))); prog.AddBoundingBoxConstraint(stride_length, stride_length, - xf(pos_map.at("base_x"))); + xf(pos_map.at("pelvis_x"))); // height constraint - // prog.AddLinearConstraint(x0(pos_map.at("base_z")) == 1); - // prog.AddLinearConstraint(xf(pos_map.at("base_z")) == 1.1); + // prog.AddLinearConstraint(x0(pos_map.at("pelvis_z")) == 1); + // prog.AddLinearConstraint(xf(pos_map.at("pelvis_z")) == 1.1); // initial pelvis position - // prog.AddLinearConstraint(x0(pos_map.at("base_y")) == 0); + // prog.AddLinearConstraint(x0(pos_map.at("pelvis_y")) == 0); // pelvis pose constraints - // prog.AddConstraintToAllKnotPoints(x(pos_map.at("base_qw")) == - // 1); prog.AddConstraintToAllKnotPoints(x(pos_map.at("base_qx")) + // prog.AddConstraintToAllKnotPoints(x(pos_map.at("pelvis_qw")) == + // 1); prog.AddConstraintToAllKnotPoints(x(pos_map.at("pelvis_qx")) // == 0); - // prog.AddConstraintToAllKnotPoints(x(pos_map.at("base_qy")) == - // 0); prog.AddConstraintToAllKnotPoints(x(pos_map.at("base_qz")) + // prog.AddConstraintToAllKnotPoints(x(pos_map.at("pelvis_qy")) == + // 0); prog.AddConstraintToAllKnotPoints(x(pos_map.at("pelvis_qz")) // == 0); // start/end velocity constraints @@ -601,32 +601,32 @@ void DoMain(double duration, double stride_length, double ground_incline, // prog.AddLinearConstraint(xf.tail(n_v) == VectorXd::Zero(n_v)); // Floating base periodicity - prog.AddLinearConstraint(x0(pos_map.at("base_qw")) == - xf(pos_map.at("base_qw"))); - prog.AddLinearConstraint(x0(pos_map.at("base_qx")) == - -xf(pos_map.at("base_qx"))); - prog.AddLinearConstraint(x0(pos_map.at("base_qy")) == - xf(pos_map.at("base_qy"))); - prog.AddLinearConstraint(x0(pos_map.at("base_qz")) == - -xf(pos_map.at("base_qz"))); - prog.AddLinearConstraint(x0(pos_map.at("base_y")) == - -xf(pos_map.at("base_y"))); + prog.AddLinearConstraint(x0(pos_map.at("pelvis_qw")) == + xf(pos_map.at("pelvis_qw"))); + prog.AddLinearConstraint(x0(pos_map.at("pelvis_qx")) == + -xf(pos_map.at("pelvis_qx"))); + prog.AddLinearConstraint(x0(pos_map.at("pelvis_qy")) == + xf(pos_map.at("pelvis_qy"))); + prog.AddLinearConstraint(x0(pos_map.at("pelvis_qz")) == + -xf(pos_map.at("pelvis_qz"))); + prog.AddLinearConstraint(x0(pos_map.at("pelvis_y")) == + -xf(pos_map.at("pelvis_y"))); if (ground_incline == 0) { - prog.AddLinearConstraint(x0(pos_map.at("base_z")) == - xf(pos_map.at("base_z"))); + prog.AddLinearConstraint(x0(pos_map.at("pelvis_z")) == + xf(pos_map.at("pelvis_z"))); } - prog.AddLinearConstraint(x0(n_q + vel_map.at("base_wx")) == - xf(n_q + vel_map.at("base_wx"))); - prog.AddLinearConstraint(x0(n_q + vel_map.at("base_wy")) == - -xf(n_q + vel_map.at("base_wy"))); - prog.AddLinearConstraint(x0(n_q + vel_map.at("base_wz")) == - xf(n_q + vel_map.at("base_wz"))); - prog.AddLinearConstraint(x0(n_q + vel_map.at("base_vx")) == - xf(n_q + vel_map.at("base_vx"))); - prog.AddLinearConstraint(x0(n_q + vel_map.at("base_vy")) == - -xf(n_q + vel_map.at("base_vy"))); - prog.AddLinearConstraint(x0(n_q + vel_map.at("base_vz")) == - xf(n_q + vel_map.at("base_vz"))); + prog.AddLinearConstraint(x0(n_q + vel_map.at("pelvis_wx")) == + xf(n_q + vel_map.at("pelvis_wx"))); + prog.AddLinearConstraint(x0(n_q + vel_map.at("pelvis_wy")) == + -xf(n_q + vel_map.at("pelvis_wy"))); + prog.AddLinearConstraint(x0(n_q + vel_map.at("pelvis_wz")) == + xf(n_q + vel_map.at("pelvis_wz"))); + prog.AddLinearConstraint(x0(n_q + vel_map.at("pelvis_vx")) == + xf(n_q + vel_map.at("pelvis_vx"))); + prog.AddLinearConstraint(x0(n_q + vel_map.at("pelvis_vy")) == + -xf(n_q + vel_map.at("pelvis_vy"))); + prog.AddLinearConstraint(x0(n_q + vel_map.at("pelvis_vz")) == + xf(n_q + vel_map.at("pelvis_vz"))); // The legs joint positions/velocities/torque should be mirrored between legs // (notice that hip yaw and roll should be asymmetric instead of symmetric.) @@ -719,7 +719,7 @@ void DoMain(double duration, double stride_length, double ground_incline, prog.AddConstraint(right_foot_constraint_z, x_mid.head(n_q)); // Optional -- constraint on initial floating base - prog.AddConstraint(x0(pos_map.at("base_qw")) == 1); + prog.AddConstraint(x0(pos_map.at("pelvis_qw")) == 1); // Optional -- constraint on the forces magnitude /*for (unsigned int mode = 0; mode < num_time_samples.size(); mode++) { for (int index = 0; index < num_time_samples[mode]; index++) { @@ -774,7 +774,7 @@ void DoMain(double duration, double stride_length, double ground_incline, trajopt->ScaleTimeVariables(0.008); // state trajopt->ScaleStateVariables( - {base_qw_idx, base_qx_idx, base_qy_idx, base_qz_idx}, 0.5); + {pelvis_qw_idx, pelvis_qx_idx, pelvis_qy_idx, pelvis_qz_idx}, 0.5); if (s_q_toe > 1) { trajopt->ScaleStateVariables({toe_left_idx, toe_right_idx}, s_q_toe); } @@ -882,7 +882,7 @@ void DoMain(double duration, double stride_length, double ground_incline, } if (w_q_quat_xyz) { for (int i = 0; i < N; i++) { - auto q = trajopt->state(i).segment(base_qx_idx, 3); + auto q = trajopt->state(i).segment(pelvis_qx_idx, 3); prog.AddCost(w_q_quat_xyz * q.transpose() * q); } } @@ -953,23 +953,23 @@ void DoMain(double duration, double stride_length, double ground_incline, // initial condition for post impact velocity auto vp_var = trajopt->v_post_impact_vars_by_mode(0); prog.SetInitialGuess( - vp_var(vel_map.at("base_wx")), - prog.GetInitialGuess(x0(n_q + vel_map.at("base_wx")))); + vp_var(vel_map.at("pelvis_wx")), + prog.GetInitialGuess(x0(n_q + vel_map.at("pelvis_wx")))); prog.SetInitialGuess( - vp_var(vel_map.at("base_wy")), - -prog.GetInitialGuess(x0(n_q + vel_map.at("base_wy")))); + vp_var(vel_map.at("pelvis_wy")), + -prog.GetInitialGuess(x0(n_q + vel_map.at("pelvis_wy")))); prog.SetInitialGuess( - vp_var(vel_map.at("base_wz")), - prog.GetInitialGuess(x0(n_q + vel_map.at("base_wz")))); + vp_var(vel_map.at("pelvis_wz")), + prog.GetInitialGuess(x0(n_q + vel_map.at("pelvis_wz")))); prog.SetInitialGuess( - vp_var(vel_map.at("base_vx")), - prog.GetInitialGuess(x0(n_q + vel_map.at("base_vx")))); + vp_var(vel_map.at("pelvis_vx")), + prog.GetInitialGuess(x0(n_q + vel_map.at("pelvis_vx")))); prog.SetInitialGuess( - vp_var(vel_map.at("base_vy")), - -prog.GetInitialGuess(x0(n_q + vel_map.at("base_vy")))); + vp_var(vel_map.at("pelvis_vy")), + -prog.GetInitialGuess(x0(n_q + vel_map.at("pelvis_vy")))); prog.SetInitialGuess( - vp_var(vel_map.at("base_vz")), - prog.GetInitialGuess(x0(n_q + vel_map.at("base_vz")))); + vp_var(vel_map.at("pelvis_vz")), + prog.GetInitialGuess(x0(n_q + vel_map.at("pelvis_vz")))); for (auto l_r_pair : l_r_pairs) { for (unsigned int i = 0; i < asy_joint_names.size(); i++) { // velocities @@ -994,13 +994,13 @@ void DoMain(double duration, double stride_length, double ground_incline, // produces NAN value in some calculation. for (int i = 0; i < N; i++) { auto xi = trajopt->state(i); - if ((prog.GetInitialGuess(xi.segment<4>(base_qw_idx)).norm() == + if ((prog.GetInitialGuess(xi.segment<4>(pelvis_qw_idx)).norm() == 0) || std::isnan(prog.GetInitialGuess( - xi.segment<4>(base_qw_idx)).norm())) { - prog.SetInitialGuess(xi(pos_map.at("base_qw")), 1); - prog.SetInitialGuess(xi(pos_map.at("base_qx")), 0); - prog.SetInitialGuess(xi(pos_map.at("base_qy")), 0); - prog.SetInitialGuess(xi(pos_map.at("base_qz")), 0); + xi.segment<4>(pelvis_qw_idx)).norm())) { + prog.SetInitialGuess(xi(pos_map.at("pelvis_qw")), 1); + prog.SetInitialGuess(xi(pos_map.at("pelvis_qx")), 0); + prog.SetInitialGuess(xi(pos_map.at("pelvis_qy")), 0); + prog.SetInitialGuess(xi(pos_map.at("pelvis_qz")), 0); } } @@ -1200,7 +1200,7 @@ void DoMain(double duration, double stride_length, double ground_incline, // add cost on quaternion double cost_q_quat_xyz = 0; for (int i = 0; i < N; i++) { - auto q = result.GetSolution(trajopt->state(i).segment(base_qx_idx, 3)); + auto q = result.GetSolution(trajopt->state(i).segment(pelvis_qx_idx, 3)); cost_q_quat_xyz += w_q_quat_xyz * q.transpose() * q; } total_cost += cost_q_quat_xyz; diff --git a/examples/Cassie/visualize_trajectory.cc b/examples/Cassie/visualize_trajectory.cc index ca9e4293f3..83a5f60d2f 100644 --- a/examples/Cassie/visualize_trajectory.cc +++ b/examples/Cassie/visualize_trajectory.cc @@ -72,9 +72,9 @@ int DoMain() { auto mirrored_traj = saved_traj.ReconstructMirrorStateTrajectory(optimal_traj.end_time()); VectorXd x_offset = VectorXd::Zero(nx); - x_offset(pos_map["base_x"]) = - optimal_traj.value(optimal_traj.end_time())(pos_map["base_x"]) - - optimal_traj.value(optimal_traj.start_time())(pos_map["base_x"]); + x_offset(pos_map["pelvis_x"]) = + optimal_traj.value(optimal_traj.end_time())(pos_map["pelvis_x"]) - + optimal_traj.value(optimal_traj.start_time())(pos_map["pelvis_x"]); std::vector x_offset_rep(mirrored_traj.get_segment_times().size(), x_offset); PiecewisePolynomial x_offset_traj = @@ -82,9 +82,9 @@ int DoMain() { mirrored_traj.get_segment_times(), x_offset_rep); optimal_traj.ConcatenateInTime(mirrored_traj + x_offset_traj); - x_offset(pos_map["base_x"]) = - optimal_traj.value(optimal_traj.end_time())(pos_map["base_x"]) - - optimal_traj.value(optimal_traj.start_time())(pos_map["base_x"]); + x_offset(pos_map["pelvis_x"]) = + optimal_traj.value(optimal_traj.end_time())(pos_map["pelvis_x"]) - + optimal_traj.value(optimal_traj.start_time())(pos_map["pelvis_x"]); x_offset_rep = std::vector( optimal_traj.get_segment_times().size(), x_offset); diff --git a/multibody/multibody_utils.cc b/multibody/multibody_utils.cc index 4c1156778f..b603f1861c 100644 --- a/multibody/multibody_utils.cc +++ b/multibody/multibody_utils.cc @@ -4,6 +4,7 @@ #include #include "drake/common/drake_assert.h" +#include "drake/geometry/proximity_properties.h" #include "drake/math/autodiff_gradient.h" namespace dairlib { @@ -22,9 +23,9 @@ using drake::multibody::JointIndex; using drake::multibody::ModelInstanceIndex; using drake::multibody::MultibodyPlant; using drake::systems::Context; -using Eigen::VectorXd; -using Eigen::Vector3d; using Eigen::Vector2d; +using Eigen::Vector3d; +using Eigen::VectorXd; using std::map; using std::string; using std::vector; @@ -121,7 +122,8 @@ void SetInputsIfNew(const MultibodyPlant& plant, template void AddFlatTerrain(MultibodyPlant* plant, SceneGraph* scene_graph, double mu_static, double mu_kinetic, - Eigen::Vector3d normal_W, bool show_ground) { + Eigen::Vector3d normal_W, double stiffness, + double dissipation_rate, bool show_ground) { if (!plant->geometry_source_is_registered()) { plant->RegisterAsSourceForSceneGraph(scene_graph); } @@ -133,8 +135,19 @@ void AddFlatTerrain(MultibodyPlant* plant, SceneGraph* scene_graph, const drake::math::RigidTransformd X_WG( HalfSpace::MakePose(normal_W, point_W)); - plant->RegisterCollisionGeometry(plant->world_body(), X_WG, HalfSpace(), - "collision", friction); + if (stiffness != 0) { + drake::geometry::ProximityProperties props; + props.AddProperty("material", "point_contact_stiffness", stiffness); + props.AddProperty("material", "hunt_crossley_dissipation", + dissipation_rate); + props.AddProperty(drake::geometry::internal::kMaterialGroup, + drake::geometry::internal::kFriction, friction); + plant->RegisterCollisionGeometry(plant->world_body(), X_WG, HalfSpace(), + "collision", props); + } else { + plant->RegisterCollisionGeometry(plant->world_body(), X_WG, HalfSpace(), + "collision", friction); + } // Add visual for the ground. if (show_ground) { @@ -144,10 +157,11 @@ void AddFlatTerrain(MultibodyPlant* plant, SceneGraph* scene_graph, } /// Get the ordered names from a NameTo___Map -vector ExtractOrderedNamesFromMap(const map& map) { +vector ExtractOrderedNamesFromMap(const map& map, + int index_start) { vector names(map.size()); for (const auto& entry : map) { - names[entry.second] = entry.first; + names[entry.second - index_start] = entry.first; } return names; } @@ -188,16 +202,12 @@ map MakeNameToPositionsMap(const MultibodyPlant& plant) { } } - // TODO: once RBT fully deprecated, this block can likely be removed, using - // default coordinate names from Drake. auto floating_bodies = plant.GetFloatingBaseBodies(); - DRAKE_THROW_UNLESS(floating_bodies.size() <= 1); for (auto body_index : floating_bodies) { const auto& body = plant.get_body(body_index); DRAKE_ASSERT(body.has_quaternion_dofs()); int start = body.floating_positions_start(); - // should be body.name() once RBT is deprecated - std::string name = "base"; + std::string name = body.name(); name_to_index_map[name + "_qw"] = start; name_to_index_map[name + "_qx"] = start + 1; name_to_index_map[name + "_qy"] = start + 2; @@ -219,6 +229,64 @@ map MakeNameToPositionsMap(const MultibodyPlant& plant) { return name_to_index_map; } +/// Construct a map between joint names and position indices +/// such that q(index) has the given name +/// -Only accurately includes joints with a single position and single velocity +/// -Others are included as "position[ind]"" +/// -Index mapping can also be used as a state mapping (assumes x = [q;v]) +template +map MakeNameToPositionsMap(const MultibodyPlant& plant, + ModelInstanceIndex model_instance) { + map name_to_index_map; + std::set index_set; + for (auto i : plant.GetJointIndices(model_instance)) { + const drake::multibody::Joint& joint = plant.get_joint(i); + auto name = joint.name(); + + if (joint.num_velocities() == 1 && joint.num_positions() == 1) { + std::vector index_vector{i}; + auto selectorMatrix = plant.MakeStateSelectorMatrix(index_vector); + // find index and add + int selector_index = -1; + for (int j = 0; j < selectorMatrix.cols(); ++j) { + if (selectorMatrix(0, j) == 1) { + if (selector_index == -1) { + selector_index = j; + } else { + throw std::logic_error("Unable to create selector map."); + } + } + } + if (selector_index == -1) { + std::logic_error("Unable to create selector map."); + } + + name_to_index_map[name] = selector_index; + index_set.insert(selector_index); + } + } + + if (plant.HasUniqueFreeBaseBody(model_instance)) { + const auto& body = plant.GetUniqueFreeBaseBodyOrThrow(model_instance); + DRAKE_ASSERT(body.has_quaternion_dofs()); + int start = body.floating_positions_start(); + std::string name = body.name(); + name_to_index_map[name + "_qw"] = start; + name_to_index_map[name + "_qx"] = start + 1; + name_to_index_map[name + "_qy"] = start + 2; + name_to_index_map[name + "_qz"] = start + 3; + name_to_index_map[name + "_x"] = start + 4; + name_to_index_map[name + "_y"] = start + 5; + name_to_index_map[name + "_z"] = start + 6; + for (int i = 0; i < 7; i++) { + index_set.insert(start + i); + } + } + // if index has not already been captured, throw an error + DRAKE_THROW_UNLESS(plant.num_positions(model_instance) == index_set.size()); + + return name_to_index_map; +} /// Construct a map between joint names and velocity indices /// such that v(index) has the given name @@ -233,8 +301,6 @@ map MakeNameToVelocitiesMap(const MultibodyPlant& plant) { for (JointIndex i(0); i < plant.num_joints(); ++i) { const drake::multibody::Joint& joint = plant.get_joint(i); - // TODO(posa): this "dot" should be removed, it's an anachronism from - // RBT auto name = joint.name() + "dot"; if (joint.num_velocities() == 1 && joint.num_positions() == 1) { @@ -261,12 +327,10 @@ map MakeNameToVelocitiesMap(const MultibodyPlant& plant) { } auto floating_bodies = plant.GetFloatingBaseBodies(); - // Remove throw once RBT deprecated - DRAKE_THROW_UNLESS(floating_bodies.size() <= 1); for (auto body_index : floating_bodies) { const auto& body = plant.get_body(body_index); int start = body.floating_velocities_start() - plant.num_positions(); - std::string name = "base"; // should be body.name() once RBT is deprecated + std::string name = body.name(); name_to_index_map[name + "_wx"] = start; name_to_index_map[name + "_wy"] = start + 1; name_to_index_map[name + "_wz"] = start + 2; @@ -285,6 +349,67 @@ map MakeNameToVelocitiesMap(const MultibodyPlant& plant) { } } + // if index has not already been captured, throw an error + DRAKE_THROW_UNLESS(plant.num_velocities() == index_set.size()); + return name_to_index_map; +} + +/// Construct a map between joint names and velocity indices +/// such that v(index) has the given name +/// -Only accurately includes joints with a single position and single velocity +/// -Others are included as "state[ind]" +/// -Index mapping can also be used as a state mapping, AFTER +/// an offset of num_positions is applied (assumes x = [q;v]) +template +map MakeNameToVelocitiesMap(const MultibodyPlant& plant, + ModelInstanceIndex model_instance) { + map name_to_index_map; + std::set index_set; + + for (auto i : plant.GetJointIndices(model_instance)) { + const drake::multibody::Joint& joint = plant.get_joint(i); + auto name = joint.name() + "dot"; + + if (joint.num_velocities() == 1 && joint.num_positions() == 1) { + std::vector index_vector{i}; + auto selectorMatrix = plant.MakeStateSelectorMatrix(index_vector); + // find index and add + int selector_index = -1; + for (int j = 0; j < selectorMatrix.cols(); ++j) { + if (selectorMatrix(1, j) == 1) { + if (selector_index == -1) { + selector_index = j; + } else { + throw std::logic_error("Unable to create selector map."); + } + } + } + if (selector_index == -1) { + throw std::logic_error("Unable to create selector map."); + } + + name_to_index_map[name] = selector_index - plant.num_positions(); + index_set.insert(selector_index - plant.num_positions()); + } + } + + if (plant.HasUniqueFreeBaseBody(model_instance)) { + const auto& body = plant.GetUniqueFreeBaseBodyOrThrow(model_instance); + int start = body.floating_velocities_start() - plant.num_positions(); + std::string name = body.name(); + name_to_index_map[name + "_wx"] = start; + name_to_index_map[name + "_wy"] = start + 1; + name_to_index_map[name + "_wz"] = start + 2; + name_to_index_map[name + "_vx"] = start + 3; + name_to_index_map[name + "_vy"] = start + 4; + name_to_index_map[name + "_vz"] = start + 5; + for (int i = 0; i < 6; i++) { + index_set.insert(start + i); + } + } + + // if index has not already been captured, throw an error + DRAKE_THROW_UNLESS(plant.num_velocities(model_instance) == index_set.size()); return name_to_index_map; } @@ -323,8 +448,7 @@ map MakeNameToActuatorsMap(const MultibodyPlant& plant) { } template -vector CreateStateNameVectorFromMap( - const MultibodyPlant& plant) { +vector CreateStateNameVectorFromMap(const MultibodyPlant& plant) { map pos_map = MakeNameToPositionsMap(plant); map vel_map = MakeNameToVelocitiesMap(plant); vector state_names(pos_map.size() + vel_map.size()); @@ -340,8 +464,7 @@ vector CreateStateNameVectorFromMap( } template -vector CreateActuatorNameVectorFromMap( - const MultibodyPlant& plant) { +vector CreateActuatorNameVectorFromMap(const MultibodyPlant& plant) { map act_map = MakeNameToActuatorsMap(plant); return ExtractOrderedNamesFromMap(act_map); } @@ -453,10 +576,9 @@ Vector3d ReExpressWorldVector3InBodyYawFrame(const MultibodyPlant& plant, const Vector3d& vec) { Vector3d pelvis_x = plant.GetBodyByName(body_name).EvalPoseInWorld(context).rotation().col(0); - double yaw = atan2(pelvis_x(1), pelvis_x(0)); - return Vector3d(cos(yaw) * vec(0) + sin(yaw)*vec(1), - -sin(yaw) * vec(0) + cos(yaw)*vec(1), - vec(2)); + double yaw = atan2(pelvis_x(1), pelvis_x(0)); + return Vector3d(cos(yaw) * vec(0) + sin(yaw) * vec(1), + -sin(yaw) * vec(0) + cos(yaw) * vec(1), vec(2)); } template @@ -466,9 +588,9 @@ Vector2d ReExpressWorldVector2InBodyYawFrame(const MultibodyPlant& plant, const Vector2d& vec) { Vector3d pelvis_x = plant.GetBodyByName(body_name).EvalPoseInWorld(context).rotation().col(0); - double yaw = atan2(pelvis_x(1), pelvis_x(0)); - return Vector2d(cos(yaw) * vec(0) + sin(yaw)*vec(1), - -sin(yaw) * vec(0) + cos(yaw)*vec(1)); + double yaw = atan2(pelvis_x(1), pelvis_x(0)); + return Vector2d(cos(yaw) * vec(0) + sin(yaw) * vec(1), + -sin(yaw) * vec(0) + cos(yaw) * vec(1)); } VectorXd MakeJointPositionOffsetFromMap( @@ -487,21 +609,21 @@ Eigen::MatrixXd WToQuatDotMap(const Eigen::Vector4d& q) { // clang-format off Eigen::MatrixXd ret(4,3); ret << -q(1), -q(2), -q(3), - q(0), q(3), -q(2), - -q(3), q(0), q(1), - q(2), -q(1), q(0); + q(0), q(3), -q(2), + -q(3), q(0), q(1), + q(2), -q(1), q(0); ret *= 0.5; // clang-format on return ret; } -Eigen::MatrixXd JwrtqdotToJwrtv( - const Eigen::VectorXd& q, const Eigen::MatrixXd& Jwrtqdot) { +Eigen::MatrixXd JwrtqdotToJwrtv(const Eigen::VectorXd& q, + const Eigen::MatrixXd& Jwrtqdot) { //[J_1:4, J_5:end] * [WToQuatDotMap, 0] = [J_1:4 * WToQuatDotMap, J_5:end] // [ 0 , I] DRAKE_DEMAND(Jwrtqdot.cols() == q.size()); - Eigen::MatrixXd ret(Jwrtqdot.rows(), q.size() -1); + Eigen::MatrixXd ret(Jwrtqdot.rows(), q.size() - 1); ret << Jwrtqdot.leftCols<4>() * WToQuatDotMap(q.head<4>()), Jwrtqdot.rightCols(q.size() - 4); return ret; @@ -517,8 +639,12 @@ template Vector3d ReExpressWorldVector3InBodyYawFrame(const MultibodyPlant& plant, const Context& context, const std::string& body_name, const Vector2d& vec); //NOLINT template map MakeNameToPositionsMap(const MultibodyPlant& plant); // NOLINT template map MakeNameToPositionsMap(const MultibodyPlant &plant); // NOLINT +template map MakeNameToPositionsMap(const MultibodyPlant& plant, drake::multibody::ModelInstanceIndex); // NOLINT +template map MakeNameToPositionsMap(const MultibodyPlant& plant, drake::multibody::ModelInstanceIndex); // NOLINT template map MakeNameToVelocitiesMap(const MultibodyPlant& plant); // NOLINT template map MakeNameToVelocitiesMap(const MultibodyPlant& plant); // NOLINT +template map MakeNameToVelocitiesMap(const MultibodyPlant& plant, drake::multibody::ModelInstanceIndex); // NOLINT +template map MakeNameToVelocitiesMap(const MultibodyPlant& plant, drake::multibody::ModelInstanceIndex); // NOLINT template map MakeNameToActuatorsMap(const MultibodyPlant& plant); // NOLINT template map MakeNameToActuatorsMap(const MultibodyPlant& plant); // NOLINT template vector CreateStateNameVectorFromMap(const MultibodyPlant& plant); // NOLINT @@ -527,7 +653,7 @@ template vector CreateActuatorNameVectorFromMap(const MultibodyPlant CreateActuatorNameVectorFromMap(const MultibodyPlant& plant); // NOLINT template Eigen::MatrixXd CreateWithSpringsToWithoutSpringsMapPos(const drake::multibody::MultibodyPlant& plant_w_spr, const drake::multibody::MultibodyPlant& plant_wo_spr); // NOLINT template Eigen::MatrixXd CreateWithSpringsToWithoutSpringsMapVel(const drake::multibody::MultibodyPlant& plant_w_spr, const drake::multibody::MultibodyPlant& plant_wo_spr); // NOLINT -template void AddFlatTerrain(MultibodyPlant* plant, SceneGraph* scene_graph, double mu_static, double mu_kinetic, Eigen::Vector3d normal_W, bool show_ground); // NOLINT +template void AddFlatTerrain(MultibodyPlant* plant, SceneGraph* scene_graph, double mu_static, double mu_kinetic, Eigen::Vector3d normal_W, double stiffness, double dissipation_rate, bool show_ground); // NOLINT template VectorX GetInput(const MultibodyPlant& plant, const Context& context); // NOLINT template VectorX GetInput(const MultibodyPlant& plant, const Context& context); // NOLINT template std::unique_ptr> CreateContext(const MultibodyPlant& plant, const Eigen::Ref& state, const Eigen::Ref& input); // NOLINT diff --git a/multibody/multibody_utils.h b/multibody/multibody_utils.h index 03ad94022a..4d829fe985 100644 --- a/multibody/multibody_utils.h +++ b/multibody/multibody_utils.h @@ -68,23 +68,37 @@ void AddFlatTerrain(drake::multibody::MultibodyPlant* plant, drake::geometry::SceneGraph* scene_graph, double mu_static, double mu_kinetic, Eigen::Vector3d normal_W = Eigen::Vector3d(0, 0, 1), + double stiffness = 0, double dissipation_rate = 0, bool show_ground = true); /// Get the ordered names from a NameTo___Map std::vector ExtractOrderedNamesFromMap( - const std::map& map); + const std::map& map, int index_start = 0); /// Given a MultibodyPlant, builds a map from position name to position index template std::map MakeNameToPositionsMap( const drake::multibody::MultibodyPlant& plant); -/// Given a MultiBodyTree, builds a map from velocity name to velocity index +/// Given a MultibodyPlant, builds a map from position name to position index +template +std::map MakeNameToPositionsMap( + const drake::multibody::MultibodyPlant& plant, + drake::multibody::ModelInstanceIndex model_instance_index); + +/// Given a MultiBodyPlant, builds a map from velocity name to velocity index template std::map MakeNameToVelocitiesMap( const drake::multibody::MultibodyPlant& plant); -/// Given a MultiBodyTree, builds a map from actuator name to actuator index + +/// Given a MultiBodyPlant, builds a map from velocity name to velocity index +template +std::map MakeNameToVelocitiesMap( + const drake::multibody::MultibodyPlant& plant, + drake::multibody::ModelInstanceIndex model_instance_index); + +/// Given a MultiBodyPlant, builds a map from actuator name to actuator index template std::map MakeNameToActuatorsMap( const drake::multibody::MultibodyPlant& plant); diff --git a/multibody/multipose_visualizer.cc b/multibody/multipose_visualizer.cc index e11da4ef05..e8e3aa9aa8 100644 --- a/multibody/multipose_visualizer.cc +++ b/multibody/multipose_visualizer.cc @@ -6,6 +6,7 @@ #include "drake/systems/lcm/lcm_interface_system.h" using drake::geometry::DrakeVisualizer; +using drake::geometry::Meshcat; using drake::geometry::SceneGraph; using drake::multibody::MultibodyPlant; using drake::multibody::Parser; @@ -29,7 +30,8 @@ MultiposeVisualizer::MultiposeVisualizer(string model_file, int num_poses, MultiposeVisualizer::MultiposeVisualizer(string model_file, int num_poses, const Eigen::VectorXd& alpha_scale, - string weld_frame_to_world) + string weld_frame_to_world, + std::shared_ptr meshcat) : num_poses_(num_poses) { DRAKE_DEMAND(num_poses == alpha_scale.size()); DiagramBuilder builder; @@ -40,11 +42,10 @@ MultiposeVisualizer::MultiposeVisualizer(string model_file, int num_poses, auto lcm = builder.AddSystem(); Parser parser(plant_, scene_graph); - + parser.SetAutoRenaming(true); // Add num_poses copies of the plant, giving each a unique name for (int i = 0; i < num_poses_; i++) { - auto index = - parser.AddModelFromFile(model_file, "model[" + std::to_string(i) + "]"); + auto index = parser.AddModels(model_file)[0]; model_indices_.push_back(index); if (!weld_frame_to_world.empty()) { plant_->WeldFrames( @@ -88,10 +89,19 @@ MultiposeVisualizer::MultiposeVisualizer(string model_file, int num_poses, } } + if (meshcat == nullptr) { + meshcat_ = std::make_shared(); + } else { + meshcat_ = meshcat; + } + meshcat_visualizer_ = + &drake::geometry::MeshcatVisualizer::AddToBuilder( + &builder, *scene_graph, meshcat_); + DrakeVisualizer::AddToBuilder(&builder, *scene_graph, lcm); + DrakeVisualizer::DispatchLoadMessage(*scene_graph, lcm); diagram_ = builder.Build(); diagram_context_ = diagram_->CreateDefaultContext(); - DrakeVisualizer::DispatchLoadMessage(*scene_graph, lcm); } void MultiposeVisualizer::DrawPoses(MatrixXd poses) { diff --git a/multibody/multipose_visualizer.h b/multibody/multipose_visualizer.h index 3d4f28b0d7..5ae9feb9ac 100644 --- a/multibody/multipose_visualizer.h +++ b/multibody/multipose_visualizer.h @@ -4,6 +4,7 @@ #include #include "drake/geometry/scene_graph.h" +#include "drake/geometry/meshcat_visualizer.h" #include "drake/multibody/parsing/parser.h" #include "drake/multibody/plant/multibody_plant.h" #include "drake/systems/framework/diagram.h" @@ -44,19 +45,27 @@ class MultiposeVisualizer { /// @param alpha_scale Vector, of same length as num_poses. Provideas variable /// scaling of the transparency alpha field of all bodies, indexed by pose /// @param weld_frame_to_world Welds the frame of the given name to the world + /// @param meshcat Pointer to meshcat visualizer for option to attach to an existing meshcat instance MultiposeVisualizer(std::string model_file, int num_poses, const Eigen::VectorXd& alpha_scale, - std::string weld_frame_to_world = ""); + std::string weld_frame_to_world = "", + std::shared_ptr meshcat = nullptr); /// Draws the poses in the given (num_positions x num_poses) matrix /// Note: the matrix can have extra rows (e.g. velocities), which will be /// ignored. void DrawPoses(Eigen::MatrixXd poses); + const std::shared_ptr GetMeshcat(){ + return meshcat_; + } + private: int num_poses_; drake::multibody::MultibodyPlant* plant_; std::unique_ptr> diagram_; + std::shared_ptr meshcat_; + drake::geometry::MeshcatVisualizer* meshcat_visualizer_; std::unique_ptr> diagram_context_; std::vector model_indices_; }; diff --git a/multibody/test/geom_geom_collider_test.cc b/multibody/test/geom_geom_collider_test.cc index bcee569344..09050b0af5 100644 --- a/multibody/test/geom_geom_collider_test.cc +++ b/multibody/test/geom_geom_collider_test.cc @@ -74,12 +74,12 @@ void GeomGeomColliderTest() { q(q_map.at("finger_base_to_upper_joint_240")) = 0; q(q_map.at("finger_upper_to_middle_joint_240")) = -1; q(q_map.at("finger_middle_to_lower_joint_240")) = -1.5; - q(q_map.at("base_qw")) = 1; - q(q_map.at("base_qx")) = 0; - q(q_map.at("base_qz")) = 0; - q(q_map.at("base_x")) = 0; - q(q_map.at("base_y")) = 0; - q(q_map.at("base_z")) = .05; + q(q_map.at("cube_qw")) = 1; + q(q_map.at("cube_qx")) = 0; + q(q_map.at("cube_qz")) = 0; + q(q_map.at("cube_x")) = 0; + q(q_map.at("cube_y")) = 0; + q(q_map.at("cube_z")) = .05; plant.SetPositions(&context, q); diff --git a/multibody/visualization_utils.cc b/multibody/visualization_utils.cc index b5d819fc0c..2c53e7c15f 100644 --- a/multibody/visualization_utils.cc +++ b/multibody/visualization_utils.cc @@ -3,7 +3,6 @@ #include "common/find_resource.h" #include "multibody/com_pose_system.h" #include "systems/primitives/subvector_pass_through.h" -#include "drake/geometry/drake_visualizer.h" #include "drake/multibody/parsing/parser.h" #include "drake/systems/primitives/trajectory_source.h" @@ -37,11 +36,11 @@ void ConnectTrajectoryVisualizer( drake::geometry::SceneGraph* scene_graph, const Trajectory& trajectory) { auto empty_plant = std::make_unique>(0.0); - ConnectTrajectoryVisualizer(plant, builder, scene_graph, trajectory, - *empty_plant); + ConnectTrajectoryVisualizerWithCoM(plant, builder, scene_graph, trajectory, + *empty_plant); } -void ConnectTrajectoryVisualizer( +void ConnectTrajectoryVisualizerWithCoM( const MultibodyPlant* plant, drake::systems::DiagramBuilder* builder, drake::geometry::SceneGraph* scene_graph, @@ -81,7 +80,6 @@ void ConnectTrajectoryVisualizer( scene_graph->get_source_pose_port(ball_plant.get_source_id().value())); } - DrakeVisualizer::AddToBuilder(builder, *scene_graph); } } // namespace multibody diff --git a/multibody/visualization_utils.h b/multibody/visualization_utils.h index 31de4481c2..fba3f5f21a 100644 --- a/multibody/visualization_utils.h +++ b/multibody/visualization_utils.h @@ -27,7 +27,7 @@ void ConnectTrajectoryVisualizer( drake::systems::DiagramBuilder* builder, drake::geometry::SceneGraph* scene_graph, const drake::trajectories::Trajectory& trajectory); -void ConnectTrajectoryVisualizer( +void ConnectTrajectoryVisualizerWithCoM( const drake::multibody::MultibodyPlant* plant, drake::systems::DiagramBuilder* builder, drake::geometry::SceneGraph* scene_graph, diff --git a/systems/robot_lcm_systems.cc b/systems/robot_lcm_systems.cc index b812225a48..d8611caa09 100644 --- a/systems/robot_lcm_systems.cc +++ b/systems/robot_lcm_systems.cc @@ -1,7 +1,7 @@ -#include - #include "robot_lcm_systems.h" +#include "dairlib/lcmt_robot_input.hpp" +#include "dairlib/lcmt_robot_output.hpp" #include "multibody/multibody_utils.h" #include "drake/multibody/plant/multibody_plant.h" @@ -9,9 +9,6 @@ #include "drake/systems/lcm/lcm_subscriber_system.h" #include "drake/systems/primitives/discrete_time_delay.h" -#include "dairlib/lcmt_robot_input.hpp" -#include "dairlib/lcmt_robot_output.hpp" - namespace dairlib { namespace systems { @@ -19,9 +16,9 @@ using drake::multibody::JointActuatorIndex; using drake::multibody::JointIndex; using drake::multibody::MultibodyPlant; using drake::systems::Context; +using drake::systems::LeafSystem; using drake::systems::lcm::LcmPublisherSystem; using drake::systems::lcm::LcmSubscriberSystem; -using drake::systems::LeafSystem; using Eigen::VectorXd; using std::string; using systems::OutputVector; @@ -36,6 +33,10 @@ RobotOutputReceiver::RobotOutputReceiver( num_efforts_ = plant.num_actuators(); position_index_map_ = multibody::MakeNameToPositionsMap(plant); velocity_index_map_ = multibody::MakeNameToVelocitiesMap(plant); + model_instance_ = drake::multibody::ModelInstanceIndex(-1); + + positions_start_idx_ = 0; + velocities_start_idx_ = 0; effort_index_map_ = multibody::MakeNameToActuatorsMap(plant); this->DeclareAbstractInputPort("lcmt_robot_output", drake::Value{}); @@ -46,6 +47,35 @@ RobotOutputReceiver::RobotOutputReceiver( &RobotOutputReceiver::CopyOutput); } +RobotOutputReceiver::RobotOutputReceiver( + const drake::multibody::MultibodyPlant& plant, + drake::multibody::ModelInstanceIndex model_instance) { + model_instance_ = model_instance; + num_positions_ = plant.num_positions(model_instance); + num_velocities_ = plant.num_velocities(model_instance); + num_efforts_ = plant.num_actuators(); + position_index_map_ = + multibody::MakeNameToPositionsMap(plant, model_instance); + velocity_index_map_ = + multibody::MakeNameToVelocitiesMap(plant, model_instance); + + positions_start_idx_ = + plant.get_joint(plant.GetJointIndices(model_instance).front()) + .position_start(); + velocities_start_idx_ = + plant.get_joint(plant.GetJointIndices(model_instance).front()) + .velocity_start(); + effort_index_map_ = multibody::MakeNameToActuatorsMap(plant); + this->DeclareAbstractInputPort("lcmt_robot_output", + drake::Value{}); + this->DeclareVectorOutputPort( + "x, u, t", + OutputVector(plant.num_positions(model_instance), + plant.num_velocities(model_instance), + plant.num_actuators()), + &RobotOutputReceiver::CopyOutput); +} + void RobotOutputReceiver::CopyOutput(const Context& context, OutputVector* output) const { const drake::AbstractValue* input = this->EvalAbstractInput(context, 0); @@ -55,12 +85,12 @@ void RobotOutputReceiver::CopyOutput(const Context& context, VectorXd positions = VectorXd::Zero(num_positions_); for (int i = 0; i < state_msg.num_positions; i++) { int j = position_index_map_.at(state_msg.position_names[i]); - positions(j) = state_msg.position[i]; + positions(j - positions_start_idx_) = state_msg.position[i]; } VectorXd velocities = VectorXd::Zero(num_velocities_); for (int i = 0; i < state_msg.num_velocities; i++) { int j = velocity_index_map_.at(state_msg.velocity_names[i]); - velocities(j) = state_msg.velocity[i]; + velocities(j - velocities_start_idx_) = state_msg.velocity[i]; } VectorXd efforts = VectorXd::Zero(num_efforts_); for (int i = 0; i < state_msg.num_efforts; i++) { @@ -84,16 +114,18 @@ void RobotOutputReceiver::CopyOutput(const Context& context, void RobotOutputReceiver::InitializeSubscriberPositions( const MultibodyPlant& plant, - drake::systems::Context &context) const { + drake::systems::Context& context) const { auto& state_msg = context.get_mutable_abstract_state(0); // using the time from the context state_msg.utime = context.get_time() * 1e6; std::vector ordered_position_names = - multibody::ExtractOrderedNamesFromMap(position_index_map_); + multibody::ExtractOrderedNamesFromMap(position_index_map_, + positions_start_idx_); std::vector ordered_velocity_names = - multibody::ExtractOrderedNamesFromMap(velocity_index_map_); + multibody::ExtractOrderedNamesFromMap(velocity_index_map_, + velocities_start_idx_); std::vector ordered_effort_names = multibody::ExtractOrderedNamesFromMap(effort_index_map_); @@ -110,10 +142,16 @@ void RobotOutputReceiver::InitializeSubscriberPositions( } // Set quaternion w = 1, assumes drake quaternion ordering of wxyz - for (const auto& body_idx : plant.GetFloatingBaseBodies()) { - const auto& body = plant.get_body(body_idx); - if (body.has_quaternion_dofs()) { - state_msg.position[body.floating_positions_start()] = 1; + if (model_instance_ != drake::multibody::ModelInstanceIndex(-1)) { + if (plant.HasUniqueFreeBaseBody(model_instance_)) { + state_msg.position.at(0) = 1; + } + } else { + for (const auto& body_idx : plant.GetFloatingBaseBodies()) { + const auto& body = plant.get_body(body_idx); + if (body.has_quaternion_dofs()) { + state_msg.position.at(body.floating_positions_start()) = 1; + } } } @@ -125,7 +163,6 @@ void RobotOutputReceiver::InitializeSubscriberPositions( /*--------------------------------------------------------------------------*/ // methods implementation for RobotOutputSender. - RobotOutputSender::RobotOutputSender( const drake::multibody::MultibodyPlant& plant, const bool publish_efforts, const bool publish_imu) @@ -138,6 +175,9 @@ RobotOutputSender::RobotOutputSender( velocity_index_map_ = multibody::MakeNameToVelocitiesMap(plant); effort_index_map_ = multibody::MakeNameToActuatorsMap(plant); + positions_start_idx_ = 0; + velocities_start_idx_ = 0; + ordered_position_names_ = multibody::ExtractOrderedNamesFromMap(position_index_map_); ordered_velocity_names_ = @@ -164,6 +204,55 @@ RobotOutputSender::RobotOutputSender( &RobotOutputSender::Output); } +RobotOutputSender::RobotOutputSender( + const drake::multibody::MultibodyPlant& plant, + drake::multibody::ModelInstanceIndex model_instance, + const bool publish_efforts, const bool publish_imu) + : publish_efforts_(publish_efforts), + publish_imu_(publish_imu) { + num_positions_ = plant.num_positions(model_instance); + num_velocities_ = plant.num_velocities(model_instance); + num_efforts_ = plant.num_actuators(); + + position_index_map_ = + multibody::MakeNameToPositionsMap(plant, model_instance); + velocity_index_map_ = + multibody::MakeNameToVelocitiesMap(plant, model_instance); + effort_index_map_ = multibody::MakeNameToActuatorsMap(plant); + + positions_start_idx_ = + plant.get_joint(plant.GetJointIndices(model_instance).front()) + .position_start(); + velocities_start_idx_ = + plant.get_joint(plant.GetJointIndices(model_instance).front()) + .velocity_start(); + + ordered_position_names_ = multibody::ExtractOrderedNamesFromMap( + position_index_map_, positions_start_idx_); + ordered_velocity_names_ = multibody::ExtractOrderedNamesFromMap( + velocity_index_map_, velocities_start_idx_); + ordered_effort_names_ = + multibody::ExtractOrderedNamesFromMap(effort_index_map_); + + state_input_port_ = + this->DeclareVectorInputPort( + "x", BasicVector(num_positions_ + num_velocities_)) + .get_index(); + if (publish_efforts_) { + effort_input_port_ = + this->DeclareVectorInputPort("u", BasicVector(num_efforts_)) + .get_index(); + } + if (publish_imu_) { + imu_input_port_ = + this->DeclareVectorInputPort("imu_acceleration", BasicVector(3)) + .get_index(); + } + + this->DeclareAbstractOutputPort("lcmt_robot_output", + &RobotOutputSender::Output); +} + /// Populate a state message with all states void RobotOutputSender::Output(const Context& context, dairlib::lcmt_robot_output* state_msg) const { @@ -222,9 +311,9 @@ RobotInputReceiver::RobotInputReceiver( actuator_index_map_ = multibody::MakeNameToActuatorsMap(plant); this->DeclareAbstractInputPort("lcmt_robot_input", drake::Value{}); - this->DeclareVectorOutputPort("u, t", - TimestampedVector(num_actuators_), - &RobotInputReceiver::CopyInputOut); + this->DeclareVectorOutputPort( + "u, t", TimestampedVector(num_actuators_), + &RobotInputReceiver::CopyInputOut, {all_sources_ticket()}); } void RobotInputReceiver::CopyInputOut(const Context& context, @@ -284,13 +373,10 @@ void RobotCommandSender::OutputCommand( SubvectorPassThrough* AddActuationRecieverAndStateSenderLcm( drake::systems::DiagramBuilder* builder, const MultibodyPlant& plant, - drake::systems::lcm::LcmInterfaceSystem* lcm, - std::string actuator_channel, - std::string state_channel, - double publish_rate, - bool publish_efforts, - double actuator_delay) { - + drake::systems::lcm::LcmInterfaceSystem* lcm, std::string actuator_channel, + std::string state_channel, double publish_rate, + drake::multibody::ModelInstanceIndex model_instance_index, + bool publish_efforts, double actuator_delay) { // Create LCM input for actuators auto input_sub = builder->AddSystem(LcmSubscriberSystem::Make( @@ -307,31 +393,32 @@ SubvectorPassThrough* AddActuationRecieverAndStateSenderLcm( builder->AddSystem(LcmPublisherSystem::Make( state_channel, lcm, 1.0 / publish_rate)); auto state_sender = builder->AddSystem( - plant, publish_efforts); - builder->Connect(plant.get_state_output_port(), + plant, model_instance_index, publish_efforts); + + builder->Connect(plant.get_state_output_port(model_instance_index), state_sender->get_input_port_state()); // Add delay, if used, and associated connections if (actuator_delay > 0) { - auto discrete_time_delay = - builder->AddSystem( - 1.0 / publish_rate, actuator_delay * publish_rate, - plant.num_actuators()); - builder->Connect(*passthrough, *discrete_time_delay); + auto discrete_time_delay = + builder->AddSystem( + 1.0 / publish_rate, actuator_delay * publish_rate, + plant.num_actuators()); + builder->Connect(*passthrough, *discrete_time_delay); + builder->Connect(discrete_time_delay->get_output_port(), + plant.get_actuation_input_port()); + + if (publish_efforts) { builder->Connect(discrete_time_delay->get_output_port(), - plant.get_actuation_input_port()); - - if (publish_efforts) { - builder->Connect(discrete_time_delay->get_output_port(), - state_sender->get_input_port_effort()); - } + state_sender->get_input_port_effort()); + } } else { + builder->Connect(passthrough->get_output_port(), + plant.get_actuation_input_port()); + if (publish_efforts) { builder->Connect(passthrough->get_output_port(), - plant.get_actuation_input_port()); - if (publish_efforts) { - builder->Connect(passthrough->get_output_port(), - state_sender->get_input_port_effort()); - } + state_sender->get_input_port_effort()); + } } builder->Connect(*state_sender, *state_pub); diff --git a/systems/robot_lcm_systems.h b/systems/robot_lcm_systems.h index 3e1a88c0f6..b280c24c7d 100644 --- a/systems/robot_lcm_systems.h +++ b/systems/robot_lcm_systems.h @@ -29,6 +29,10 @@ class RobotOutputReceiver : public drake::systems::LeafSystem { explicit RobotOutputReceiver( const drake::multibody::MultibodyPlant& plant); + explicit RobotOutputReceiver( + const drake::multibody::MultibodyPlant& plant, + drake::multibody::ModelInstanceIndex model_instance); + /// Convenience function to initialize an lcmt_robot_output subscriber with /// positions and velocities which are all zero except for the quaternion /// positions, which are all 1, 0, 0, 0 @@ -41,6 +45,9 @@ class RobotOutputReceiver : public drake::systems::LeafSystem { private: void CopyOutput(const drake::systems::Context& context, OutputVector* output) const; + drake::multibody::ModelInstanceIndex model_instance_; + int positions_start_idx_; + int velocities_start_idx_; int num_positions_; int num_velocities_; int num_efforts_; @@ -52,6 +59,11 @@ class RobotOutputReceiver : public drake::systems::LeafSystem { /// Converts a OutputVector object to LCM type lcmt_robot_output class RobotOutputSender : public drake::systems::LeafSystem { public: + explicit RobotOutputSender( + const drake::multibody::MultibodyPlant& plant, + drake::multibody::ModelInstanceIndex model_instance_index, + const bool publish_efforts = false, const bool publish_imu = false); + explicit RobotOutputSender( const drake::multibody::MultibodyPlant& plant, const bool publish_efforts = false, const bool publish_imu = false); @@ -73,6 +85,8 @@ class RobotOutputSender : public drake::systems::LeafSystem { void Output(const drake::systems::Context& context, dairlib::lcmt_robot_output* output) const; + int positions_start_idx_; + int velocities_start_idx_; int num_positions_; int num_velocities_; int num_efforts_; @@ -122,7 +136,6 @@ class RobotCommandSender : public drake::systems::LeafSystem { std::map actuator_index_map_; }; - /// /// Convenience method to add and connect leaf systems for controlling /// a MultibodyPlant via LCM. Makes two primary connections: @@ -148,11 +161,10 @@ class RobotCommandSender : public drake::systems::LeafSystem { SubvectorPassThrough* AddActuationRecieverAndStateSenderLcm( drake::systems::DiagramBuilder* builder, const drake::multibody::MultibodyPlant& plant, - drake::systems::lcm::LcmInterfaceSystem* lcm, - std::string actuator_channel, - std::string state_channel, - double publish_rate, - bool publish_efforts = true, - double actuator_delay = 0); + drake::systems::lcm::LcmInterfaceSystem* lcm, std::string actuator_channel, + std::string state_channel, double publish_rate, + drake::multibody::ModelInstanceIndex model_instance_index, + bool publish_efforts = true, double actuator_delay = 0); + } // namespace systems } // namespace dairlib diff --git a/systems/trajectory_optimization/dircon_opt_constraints.cc b/systems/trajectory_optimization/dircon_opt_constraints.cc index 4d0f6866df..f382e347f7 100644 --- a/systems/trajectory_optimization/dircon_opt_constraints.cc +++ b/systems/trajectory_optimization/dircon_opt_constraints.cc @@ -53,10 +53,13 @@ DirconDynamicConstraint::DirconDynamicConstraint( // is located at the first four element of the generalized position if (is_quaternion) { map positions_map = multibody::MakeNameToPositionsMap(plant); - DRAKE_DEMAND(positions_map.at("base_qw") == 0); - DRAKE_DEMAND(positions_map.at("base_qx") == 1); - DRAKE_DEMAND(positions_map.at("base_qy") == 2); - DRAKE_DEMAND(positions_map.at("base_qz") == 3); + auto floating_bodies = plant.GetFloatingBaseBodies(); + const auto& body = plant.get_body(*floating_bodies.begin()); + std::string name = body.name(); + DRAKE_DEMAND(positions_map.at(name + "_qw") == 0); + DRAKE_DEMAND(positions_map.at(name + "_qx") == 1); + DRAKE_DEMAND(positions_map.at(name + "_qy") == 2); + DRAKE_DEMAND(positions_map.at(name + "_qz") == 3); } }