Skip to content

Commit

Permalink
mbp: Propagate de-beetlejuicing
Browse files Browse the repository at this point in the history
  • Loading branch information
EricCousineau-TRI committed Nov 1, 2018
1 parent b5ca63b commit 13788cd
Show file tree
Hide file tree
Showing 20 changed files with 91 additions and 105 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -33,10 +33,10 @@ def main():
builder = DiagramBuilder()
scene_graph = builder.AddSystem(SceneGraph())
cart_pole = builder.AddSystem(MultibodyPlant(time_step=args.time_step))
AddModelFromSdfFile(
file_name=file_name, plant=cart_pole, scene_graph=scene_graph)
cart_pole.RegisterAsSourceForSceneGraph(scene_graph)
AddModelFromSdfFile(file_name=file_name, plant=cart_pole)
cart_pole.AddForceElement(UniformGravityFieldElement([0, 0, -9.81]))
cart_pole.Finalize(scene_graph)
cart_pole.Finalize()
assert cart_pole.geometry_source_is_registered()

builder.Connect(
Expand Down
3 changes: 1 addition & 2 deletions bindings/pydrake/manipulation/test/planner_test.py
Original file line number Diff line number Diff line change
Expand Up @@ -65,8 +65,7 @@ def test_mbp_overloads(self):
file_name = FindResourceOrThrow(
"drake/multibody/benchmarks/acrobot/acrobot.sdf")
plant = MultibodyPlant()
AddModelFromSdfFile(
file_name=file_name, plant=plant, scene_graph=None)
AddModelFromSdfFile(file_name=file_name, plant=plant)
plant.Finalize()

context = plant.CreateDefaultContext()
Expand Down
2 changes: 1 addition & 1 deletion bindings/pydrake/manipulation/test/simple_ui_test.py
Original file line number Diff line number Diff line change
Expand Up @@ -19,7 +19,7 @@ def test_joint_slider(self):
file_name = FindResourceOrThrow(
"drake/multibody/benchmarks/acrobot/acrobot.sdf")
plant = MultibodyPlant()
AddModelFromSdfFile(file_name=file_name, plant=plant, scene_graph=None)
AddModelFromSdfFile(file_name=file_name, plant=plant)
plant.Finalize()

slider = JointSliders(robot=plant, lower_limit=-5., upper_limit=5.,
Expand Down
2 changes: 1 addition & 1 deletion bindings/pydrake/multibody/test/inverse_kinematics_test.py
Original file line number Diff line number Diff line change
Expand Up @@ -27,7 +27,7 @@ def setUp(self):
"drake/bindings/pydrake/multibody/test/two_bodies.sdf")
self.plant = MultibodyPlant(time_step=0.01)
model_instance = AddModelFromSdfFile(
file_name=file_name, plant=self.plant, scene_graph=None)
file_name=file_name, plant=self.plant)
self.plant.Finalize()
self.body1_frame = self.plant.GetBodyByName("body1").body_frame()
self.body2_frame = self.plant.GetBodyByName("body2").body_frame()
Expand Down
13 changes: 5 additions & 8 deletions bindings/pydrake/multibody/test/multibody_tree_test.py
Original file line number Diff line number Diff line change
Expand Up @@ -113,7 +113,7 @@ def test_multibody_plant_api_via_parsing(self):
"drake/multibody/benchmarks/acrobot/acrobot.sdf")
plant = MultibodyPlant(time_step=0.01)
model_instance = AddModelFromSdfFile(
file_name=file_name, plant=plant, scene_graph=None)
file_name=file_name, plant=plant)
self.assertIsInstance(model_instance, ModelInstanceIndex)
plant.Finalize()
benchmark = MakeAcrobotPlant(AcrobotParameters(), True)
Expand Down Expand Up @@ -213,13 +213,12 @@ def test_multibody_plant_parsing(self):
"drake/multibody/benchmarks/acrobot/acrobot.sdf")
plant = MultibodyPlant(time_step=0.01)
model_instance = AddModelFromSdfFile(
file_name=file_name, plant=plant, scene_graph=None)
file_name=file_name, plant=plant)
self.assertIsInstance(model_instance, ModelInstanceIndex)

plant = MultibodyPlant(time_step=0.01)
model_instance = AddModelFromSdfFile(
file_name=file_name, model_name="acrobot", plant=plant,
scene_graph=None)
file_name=file_name, model_name="acrobot", plant=plant)

def test_multibody_tree_kinematics(self):
file_name = FindResourceOrThrow(
Expand Down Expand Up @@ -327,11 +326,9 @@ def test_model_instance_state_access(self):
plant = MultibodyPlant(timestep)

iiwa_model = AddModelFromSdfFile(
file_name=iiwa_sdf_path, model_name='robot',
scene_graph=None, plant=plant)
file_name=iiwa_sdf_path, model_name='robot', plant=plant)
gripper_model = AddModelFromSdfFile(
file_name=wsg50_sdf_path, model_name='gripper',
scene_graph=None, plant=plant)
file_name=wsg50_sdf_path, model_name='gripper', plant=plant)

# Weld the base of arm and gripper to reduce the number of states.
X_EeGripper = Isometry3.Identity()
Expand Down
6 changes: 3 additions & 3 deletions bindings/pydrake/systems/drawing_graphviz_example.py
Original file line number Diff line number Diff line change
Expand Up @@ -24,10 +24,10 @@
builder = DiagramBuilder()
scene_graph = builder.AddSystem(SceneGraph())
cart_pole = builder.AddSystem(MultibodyPlant())
AddModelFromSdfFile(
file_name=file_name, plant=cart_pole, scene_graph=scene_graph)
cart_pole.RegisterAsSourceForSceneGraph(scene_graph)
AddModelFromSdfFile(file_name=file_name, plant=cart_pole)
cart_pole.AddForceElement(UniformGravityFieldElement([0, 0, -9.81]))
cart_pole.Finalize(scene_graph)
cart_pole.Finalize()
assert cart_pole.geometry_source_is_registered()

builder.Connect(
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -64,6 +64,7 @@ void DoMain() {

MultibodyPlant<double>& plant =
*builder.AddSystem<MultibodyPlant>(FLAGS_max_time_step);
plant.RegisterAsSourceForSceneGraph(&scene_graph);
std::string hand_model_path;
if (FLAGS_use_right_hand)
hand_model_path = FindResourceOrThrow(
Expand All @@ -76,10 +77,8 @@ void DoMain() {

const std::string object_model_path = FindResourceOrThrow(
"drake/examples/allegro_hand/joint_control/simple_mug.sdf");
multibody::parsing::AddModelFromSdfFile(hand_model_path, &plant,
&scene_graph);
multibody::parsing::AddModelFromSdfFile(object_model_path, &plant,
&scene_graph);
multibody::parsing::AddModelFromSdfFile(hand_model_path, &plant);
multibody::parsing::AddModelFromSdfFile(object_model_path, &plant);

// Weld the hand to the world frame
const auto& joint_hand_root = plant.GetBodyByName("hand_root");
Expand All @@ -93,7 +92,7 @@ void DoMain() {
-9.81 * Eigen::Vector3d::UnitZ());

// Finished building the plant
plant.Finalize(&scene_graph);
plant.Finalize();

// Visualization
geometry::ConnectDrakeVisualizer(&builder, scene_graph);
Expand Down
6 changes: 3 additions & 3 deletions examples/allegro_hand/run_allegro_constant_load_demo.cc
Original file line number Diff line number Diff line change
Expand Up @@ -60,6 +60,7 @@ void DoMain() {

MultibodyPlant<double>& plant = *builder.AddSystem<MultibodyPlant>
(FLAGS_max_time_step);
plant.RegisterAsSourceForSceneGraph(&scene_graph);
std::string full_name;
if (FLAGS_use_right_hand)
full_name = FindResourceOrThrow("drake/manipulation/models/"
Expand All @@ -68,8 +69,7 @@ void DoMain() {
full_name = FindResourceOrThrow("drake/manipulation/models/"
"allegro_hand_description/sdf/allegro_hand_description_left.sdf");

multibody::parsing::AddModelFromSdfFile(
full_name, &plant, &scene_graph);
multibody::parsing::AddModelFromSdfFile(full_name, &plant);

// Weld the hand to the world frame
const auto& joint_hand_root = plant.GetBodyByName("hand_root");
Expand All @@ -82,7 +82,7 @@ void DoMain() {
-9.81 * Eigen::Vector3d::UnitZ());

// Now the model is complete.
plant.Finalize(&scene_graph);
plant.Finalize();

DRAKE_DEMAND(plant.num_actuators() == 16);
DRAKE_DEMAND(plant.num_actuated_dofs() == 16);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -63,7 +63,7 @@ int DoMain() {
-9.81 * Vector3<double>::UnitZ());

// Now the model is complete.
kuka_plant.Finalize(&scene_graph);
kuka_plant.Finalize();
DRAKE_THROW_UNLESS(kuka_plant.num_positions() == 7);
// Sanity check on the availability of the optional source id before using it.
DRAKE_DEMAND(!!kuka_plant.get_source_id());
Expand Down
11 changes: 6 additions & 5 deletions examples/manipulation_station/manipulation_station.cc
Original file line number Diff line number Diff line change
Expand Up @@ -114,14 +114,15 @@ ManipulationStation<T>::ManipulationStation(double time_step)
// the raw pointers, which should stay valid for the lifetime of the Diagram.
plant_ = owned_plant_.get();
scene_graph_ = owned_scene_graph_.get();
plant_->RegisterAsSourceForSceneGraph(scene_graph_);

// Add the table and 80/20 workcell frame.
const double dx_table_center_to_robot_base = 0.3257;
const double dz_table_top_robot_base = 0.0127;
const std::string table_sdf_path = FindResourceOrThrow(
"drake/examples/manipulation_station/models/amazon_table_simplified.sdf");
const auto table =
AddModelFromSdfFile(table_sdf_path, "table", plant_, scene_graph_);
AddModelFromSdfFile(table_sdf_path, "table", plant_);
plant_->WeldFrames(
plant_->world_frame(), plant_->GetFrameByName("amazon_table", table),
RigidTransform<double>(
Expand All @@ -133,7 +134,7 @@ ManipulationStation<T>::ManipulationStation(double time_step)
"drake/manipulation/models/iiwa_description/"
"sdf/iiwa14_no_collision.sdf");
iiwa_model_ =
AddModelFromSdfFile(iiwa_sdf_path, "iiwa", plant_, scene_graph_);
AddModelFromSdfFile(iiwa_sdf_path, "iiwa", plant_);
plant_->WeldFrames(plant_->world_frame(),
plant_->GetFrameByName("iiwa_link_0", iiwa_model_));

Expand All @@ -142,7 +143,7 @@ ManipulationStation<T>::ManipulationStation(double time_step)
"drake/manipulation/models/"
"wsg_50_description/sdf/schunk_wsg_50.sdf");
wsg_model_ =
AddModelFromSdfFile(wsg_sdf_path, "gripper", plant_, scene_graph_);
AddModelFromSdfFile(wsg_sdf_path, "gripper", plant_);
const Isometry3d wsg_pose =
RigidTransform<double>(
RollPitchYaw<double>(M_PI_2, 0, M_PI_2).ToRotationMatrix(),
Expand Down Expand Up @@ -191,7 +192,7 @@ void ManipulationStation<T>::AddCupboard() {
const std::string sdf_path = FindResourceOrThrow(
"drake/examples/manipulation_station/models/cupboard.sdf");
const auto cupboard =
AddModelFromSdfFile(sdf_path, "cupboard", plant_, scene_graph_);
AddModelFromSdfFile(sdf_path, "cupboard", plant_);
plant_->WeldFrames(
plant_->world_frame(), plant_->GetFrameByName("cupboard_body", cupboard),
RigidTransform<double>(
Expand All @@ -208,7 +209,7 @@ void ManipulationStation<T>::Finalize() {
// - cannot finalize plant until all of my objects are added, and
// - cannot wire up my diagram until we have finalized the plant.

plant_->Finalize(scene_graph_);
plant_->Finalize();

systems::DiagramBuilder<T> builder;

Expand Down
2 changes: 1 addition & 1 deletion examples/multibody/acrobot/run_lqr.cc
Original file line number Diff line number Diff line change
Expand Up @@ -115,7 +115,7 @@ int do_main() {
-9.81 * Vector3<double>::UnitZ());

// We are done defining the model.
acrobot.Finalize(&scene_graph);
acrobot.Finalize();

DRAKE_DEMAND(acrobot.num_actuators() == 1);
DRAKE_DEMAND(acrobot.num_actuated_dofs() == 1);
Expand Down
11 changes: 5 additions & 6 deletions examples/multibody/bouncing_ball/make_bouncing_ball_plant.cc
Original file line number Diff line number Diff line change
Expand Up @@ -39,34 +39,33 @@ MakeBouncingBallPlant(double radius, double mass,
// A half-space for the ground geometry.
plant->RegisterCollisionGeometry(
plant->world_body(), HalfSpace::MakePose(normal_W, point_W),
HalfSpace(), "collision", surface_friction, scene_graph);
HalfSpace(), "collision", surface_friction);

// Add visual for the ground.
plant->RegisterVisualGeometry(
plant->world_body(), HalfSpace::MakePose(normal_W, point_W),
HalfSpace(), "visual", scene_graph);
HalfSpace(), "visual");

// Add sphere geometry for the ball.
plant->RegisterCollisionGeometry(
ball,
/* Pose X_BG of the geometry frame G in the ball frame B. */
Isometry3<double>::Identity(), Sphere(radius), "collision",
surface_friction, scene_graph);
surface_friction);

// Add visual for the ball.
const VisualMaterial orange(Vector4<double>(1.0, 0.55, 0.0, 1.0));
plant->RegisterVisualGeometry(
ball,
/* Pose X_BG of the geometry frame G in the ball frame B. */
Isometry3<double>::Identity(), Sphere(radius), "visual", orange,
scene_graph);
Isometry3<double>::Identity(), Sphere(radius), "visual", orange);
}

// Gravity acting in the -z direction.
plant->AddForceElement<UniformGravityFieldElement>(gravity_W);

// We are done creating the plant.
plant->Finalize(scene_graph);
plant->Finalize();

return plant;
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -62,7 +62,7 @@ int do_main() {
-9.81 * Vector3<double>::UnitZ());

// Now the model is complete.
cart_pole.Finalize(&scene_graph);
cart_pole.Finalize();

// Sanity check on the availability of the optional source id before using it.
DRAKE_DEMAND(cart_pole.geometry_source_is_registered());
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -21,8 +21,7 @@ using drake::multibody::UnitInertia;
using Eigen::Isometry3d;

void AddCylinderWithMultiContact(
MultibodyPlant<double>* plant, SceneGraph<double>* scene_graph,
const RigidBody<double>& body,
MultibodyPlant<double>* plant, const RigidBody<double>& body,
double radius, double length, const CoulombFriction<double>& friction,
double contact_spheres_radius, int num_contacts) {
const VisualMaterial orange(Vector4<double>(1.0, 0.55, 0.0, 1.0));
Expand All @@ -32,8 +31,7 @@ void AddCylinderWithMultiContact(
plant->RegisterVisualGeometry(
body,
/* Pose X_BG of the geometry frame G in the cylinder body frame B. */
Isometry3d::Identity(), Cylinder(radius, length), "visual", orange,
scene_graph);
Isometry3d::Identity(), Cylinder(radius, length), "visual", orange);

// Add a bunch of little spheres to simulate "multi-contact".
for (int i = 0; i < num_contacts; ++i) {
Expand All @@ -48,25 +46,24 @@ void AddCylinderWithMultiContact(
body,
X_BG,
Sphere(contact_spheres_radius), "collision_top_" + std::to_string(i),
friction, scene_graph);
friction);
plant->RegisterVisualGeometry(
body,
X_BG,
Sphere(contact_spheres_radius), "visual_top_" + std::to_string(i), red,
scene_graph);
Sphere(contact_spheres_radius), "visual_top_" + std::to_string(i), red);

// Bottom spheres:
X_BG.translation() << x, y, -length / 2;
plant->RegisterCollisionGeometry(
body,
X_BG,
Sphere(contact_spheres_radius), "collision_bottom_" + std::to_string(i),
friction, scene_graph);
friction);
plant->RegisterVisualGeometry(
body,
X_BG,
Sphere(contact_spheres_radius), "visual_bottom_" + std::to_string(i),
red, scene_graph);
red);
}
}

Expand Down Expand Up @@ -95,7 +92,7 @@ MakeCylinderPlant(double radius, double length, double mass,

// Add geometry to the cylinder for both contact and visualization.
AddCylinderWithMultiContact(
plant.get(), scene_graph,
plant.get(),
cylinder, radius, length, surface_friction,
contact_radius, num_contact_spheres);

Expand All @@ -107,17 +104,17 @@ MakeCylinderPlant(double radius, double length, double mass,
plant->RegisterCollisionGeometry(
plant->world_body(),
HalfSpace::MakePose(normal_W, point_W), HalfSpace(), "collision",
surface_friction, scene_graph);
surface_friction);

// Add visual for the ground.
plant->RegisterVisualGeometry(
plant->world_body(), HalfSpace::MakePose(normal_W, point_W),
HalfSpace(), "visual", scene_graph);
HalfSpace(), "visual");

plant->AddForceElement<UniformGravityFieldElement>(gravity_W);

// We are done creating the plant.
plant->Finalize(scene_graph);
plant->Finalize();

return plant;
}
Expand Down
Loading

0 comments on commit 13788cd

Please sign in to comment.