Skip to content

adding support for model instances in our common utils functions #353

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 4 commits into
base: main
Choose a base branch
from
Open

adding support for model instances in our common utils functions #353

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
8dcb549
adding support for model instances in our common utils functions
yangwill Nov 6, 2023
132cc81
cleaning up code as per comments
yangwill Nov 14, 2023
35d9c45
fixing compile issues, renaming base to pelvis for cassie examples
yangwill Nov 14, 2023
36c292c
renaming hardcoded base link for cassie examples
yangwill Nov 14, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions bindings/pydairlib/multibody/BUILD.bazel
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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",
Expand Down
20 changes: 17 additions & 3 deletions bindings/pydairlib/multibody/multibody_py.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@

#include "multibody/multibody_utils.h"
#include "multibody/multipose_visualizer.h"
#include "multibody/visualization_utils.h"

namespace py = pybind11;

Expand All @@ -22,12 +23,18 @@ PYBIND11_MODULE(multibody, m) {
.def(py::init<std::string, int, std::string>())
.def(py::init<std::string, int, double, std::string>())
.def(py::init<std::string, int, Eigen::VectorXd, std::string>())
.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<double>, py::arg("plant"))
py::overload_cast<const drake::multibody::MultibodyPlant<double>&>(&dairlib::multibody::MakeNameToPositionsMap<double>),
py::arg("plant"))
.def("MakeNameToVelocitiesMap",
&dairlib::multibody::MakeNameToVelocitiesMap<double>,
py::overload_cast<const drake::multibody::MultibodyPlant<double>&>(&dairlib::multibody::MakeNameToVelocitiesMap<double>),
py::arg("plant"))
.def("MakeNameToActuatorsMap",
&dairlib::multibody::MakeNameToActuatorsMap<double>,
Expand All @@ -38,10 +45,17 @@ PYBIND11_MODULE(multibody, m) {
.def("CreateActuatorNameVectorFromMap",
&dairlib::multibody::CreateActuatorNameVectorFromMap<double>,
py::arg("plant"))
.def("CreateWithSpringsToWithoutSpringsMapPos",
&dairlib::multibody::CreateWithSpringsToWithoutSpringsMapPos<double>,
py::arg("plant_w_spr"), py::arg("plant_wo_spr"))
.def("CreateWithSpringsToWithoutSpringsMapVel",
&dairlib::multibody::CreateWithSpringsToWithoutSpringsMapVel<double>,
py::arg("plant_w_spr"), py::arg("plant_wo_spr"))
.def("AddFlatTerrain", &dairlib::multibody::AddFlatTerrain<double>,
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);
}

Expand Down
29 changes: 19 additions & 10 deletions bindings/pydairlib/systems/robot_lcm_systems_py.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 {
Expand All @@ -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_<systems::RobotOutputReceiver, drake::systems::LeafSystem<double>>(
m, "RobotOutputReceiver")
.def(py::init<const MultibodyPlant<double>&>());
.def(py::init<const MultibodyPlant<double>&>())
.def(py::init<const MultibodyPlant<double>&,
drake::multibody::ModelInstanceIndex>());
py::class_<systems::RobotInputReceiver, drake::systems::LeafSystem<double>>(
m, "RobotInputReceiver")
.def(py::init<const MultibodyPlant<double>&>());
py::class_<RobotOutputSender, drake::systems::LeafSystem<double>>(
m, "RobotOutputSender")
.def(py::init<const MultibodyPlant<double>&, bool>())
.def(py::init<const MultibodyPlant<double>&, bool, bool>())
.def(py::init<const MultibodyPlant<double>&,
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_<systems::RobotCommandSender, drake::systems::LeafSystem<double>>(
m, "RobotCommandSender")
.def(py::init<const MultibodyPlant<double>&>());
m.def("AddActuationRecieverAndStateSenderLcm",
&dairlib::systems::AddActuationRecieverAndStateSenderLcm,
py::overload_cast<drake::systems::DiagramBuilder<double>*,
const MultibodyPlant<double>&,
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
Expand Down
42 changes: 21 additions & 21 deletions examples/Cassie/cassie_fixed_point_solver.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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")) ==
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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")) ==
Expand Down
30 changes: 15 additions & 15 deletions examples/Cassie/cassie_state_estimator.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -435,20 +435,20 @@ void CassieStateEstimator::AssignNonFloatingBaseStateToOutputVector(

void CassieStateEstimator::AssignFloatingBaseStateToOutputVector(
const VectorXd& est_fb_state, OutputVector<double>* 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.
Expand Down Expand Up @@ -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
Expand Down
12 changes: 6 additions & 6 deletions examples/Cassie/run_dircon_jumping.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -442,9 +442,9 @@ void SetKinematicConstraints(Dircon<double>* 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);
Expand Down Expand Up @@ -472,13 +472,13 @@ void SetKinematicConstraints(Dircon<double>* 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));
Expand Down
Loading