From 94862e3725026b3665ede07d7e815e6e1b464c88 Mon Sep 17 00:00:00 2001 From: Evan Drumwright Date: Wed, 24 Apr 2019 13:16:46 -0700 Subject: [PATCH 1/8] Pre-PR commit. --- examples/manipulation_station/BUILD.bazel | 45 +++++++ .../combined_manipulator_and_gripper_model.h | 120 ++++++++++++++++++ 2 files changed, 165 insertions(+) create mode 100644 examples/manipulation_station/combined_manipulator_and_gripper_model.h diff --git a/examples/manipulation_station/BUILD.bazel b/examples/manipulation_station/BUILD.bazel index 899241128344..d04f214c4737 100644 --- a/examples/manipulation_station/BUILD.bazel +++ b/examples/manipulation_station/BUILD.bazel @@ -75,6 +75,51 @@ drake_cc_library( ], ) +drake_cc_library( + name = "combined_iiwa_wsg", + srcs = [ + "combined_iiwa_wsg.cc", + ], + hdrs = [ + "combined_iiwa_wsg.h", + ], + data = [ + "//manipulation/models/iiwa_description:models", + "//manipulation/models/wsg_50_description:models", + ], + visibility = ["//visibility:public"], + deps = [ + ":combined_manipulator_and_gripper_model", + "//common:find_resource", + "//examples/kuka_iiwa_arm:iiwa_lcm", + "//geometry/dev:scene_graph", + "//lcm", + "//manipulation/schunk_wsg:schunk_wsg_constants", + "//manipulation/schunk_wsg:schunk_wsg_lcm", + "//manipulation/schunk_wsg:schunk_wsg_position_controller", + "//multibody/parsing", + "//multibody/plant", + "//systems/controllers:inverse_dynamics_controller", + "//systems/framework", + "//systems/lcm", + "//systems/primitives", + "//systems/sensors:lcm_image_array_to_images", + "//systems/sensors/dev:rgbd_camera", + ], +) + +drake_cc_library( + name = "combined_manipulator_and_gripper_model", + hdrs = [ + "combined_manipulator_and_gripper_model.h", + "manipulation_station_setup.h", + ], + deps = [ + "//multibody/tree", + ], + visibility = ["//visibility:public"], +) + drake_cc_binary( name = "mock_station_simulation", srcs = [ diff --git a/examples/manipulation_station/combined_manipulator_and_gripper_model.h b/examples/manipulation_station/combined_manipulator_and_gripper_model.h new file mode 100644 index 000000000000..af410fd869d0 --- /dev/null +++ b/examples/manipulation_station/combined_manipulator_and_gripper_model.h @@ -0,0 +1,120 @@ +#pragma once + +#include "drake/common/eigen_types.h" +#include "drake/multibody/tree/multibody_tree_indexes.h" +#include "drake/systems/framework/context.h" +#include "drake/systems/framework/diagram.h" + +namespace drake { + +namespace systems { +template +class DiagramBuilder; +} + +namespace multibody { +template +class MultibodyPlant; +} + +namespace examples { +namespace manipulation_station { + +/** + * An abstract class for adding a combined manipulator/gripper model and its + * controller to a Diagram and querying aspects of the model (e.g., the number + * of manipulator generalized positions). + */ +template +class CombinedManipulatorAndGripperModel { + public: + CombinedManipulatorAndGripperModel(multibody::MultibodyPlant* plant) : + plant_(plant) {} + virtual ~CombinedManipulatorAndGripperModel() {} + + /// Gets the number of joints in the manipulator. + virtual int num_manipulator_joints() const = 0; + + /// Gets the number of joints in the gripper. + virtual int num_gripper_joints() const = 0; + + /// Gets the manipulator generalized positions. + virtual VectorX GetManipulatorPositions( + const systems::Context& diagram_context, + const systems::Diagram& diagram) const = 0; + + /// Gets the gripper generalized positions. + virtual VectorX GetGripperPositions( + const systems::Context& diagram_context, + const systems::Diagram& diagram) const = 0; + + /// Sets the manipulator generalized positions. + virtual void SetManipulatorPositions( + const systems::Context& diagram_context, + const Eigen::Ref>& q, + const systems::Diagram& diagram, + systems::State* diagram_state) const = 0; + + /// Sets the gripper generalized positions to the default "open" position. + virtual void SetGripperPositionsToDefaultOpen( + const systems::Context& diagram_context, + const systems::Diagram& diagram, + systems::State* diagram_state) const = 0; + + /// Sets the gripper generalized positions. + virtual void SetGripperPositions( + const systems::Context& diagram_context, + const Eigen::Ref>& q, + const systems::Diagram& diagram, + systems::State* diagram_state) const = 0; + + /// Gets the manipulator generalized velocities. + virtual VectorX GetManipulatorVelocities( + const systems::Context& diagram_context, + const systems::Diagram& diagram) const = 0; + + /// Gets the gripper generalized velocities. + virtual VectorX GetGripperVelocities( + const systems::Context& diagram_context, + const systems::Diagram& diagram) const = 0; + + /// Sets the manipulator generalized velocities. + virtual void SetManipulatorVelocities( + const systems::Context& diagram_context, + const Eigen::Ref>& v, + const systems::Diagram& diagram, + systems::State* diagram_state) const = 0; + + /// Sets the gripper generalized velocities. + virtual void SetGripperVelocities( + const systems::Context& diagram_context, + const Eigen::Ref>& v, + const systems::Diagram& diagram, + systems::State* diagram_state) const = 0; + + /// This method adds the manipulator and gripper models to the internal plant. + virtual void AddRobotModelToMultibodyPlant() = 0; + + /// This method builds the control diagram for the manipulator and gripper + /// and adds it to the given builder for a Diagram. + virtual void BuildControlDiagram(systems::DiagramBuilder* builder) = 0; + + /// Gets a reference to the plant used for control. + virtual const multibody::MultibodyPlant& get_controller_plant() const = 0; + + /// Gets the model instance for the manipulator in the internal plant. + virtual multibody::ModelInstanceIndex manipulator_model_instance() const = 0; + + /// Gets the model instance for the gripper in the internal plant. + virtual multibody::ModelInstanceIndex gripper_model_instance() const = 0; + + protected: + // The MultibodyPlant holding the robot model (and possibly other models as + // well). + multibody::MultibodyPlant* plant_{nullptr}; +}; + +} // namespace manipulation_station +} // namespace examples +} // namespace drake + From a0f0a1e38b9a36b733a0a77152cdd80289455479 Mon Sep 17 00:00:00 2001 From: Evan Drumwright Date: Wed, 24 Apr 2019 13:43:25 -0700 Subject: [PATCH 2/8] Fixed build. --- examples/manipulation_station/BUILD.bazel | 33 ----------------------- 1 file changed, 33 deletions(-) diff --git a/examples/manipulation_station/BUILD.bazel b/examples/manipulation_station/BUILD.bazel index d04f214c4737..7b1413468c97 100644 --- a/examples/manipulation_station/BUILD.bazel +++ b/examples/manipulation_station/BUILD.bazel @@ -75,39 +75,6 @@ drake_cc_library( ], ) -drake_cc_library( - name = "combined_iiwa_wsg", - srcs = [ - "combined_iiwa_wsg.cc", - ], - hdrs = [ - "combined_iiwa_wsg.h", - ], - data = [ - "//manipulation/models/iiwa_description:models", - "//manipulation/models/wsg_50_description:models", - ], - visibility = ["//visibility:public"], - deps = [ - ":combined_manipulator_and_gripper_model", - "//common:find_resource", - "//examples/kuka_iiwa_arm:iiwa_lcm", - "//geometry/dev:scene_graph", - "//lcm", - "//manipulation/schunk_wsg:schunk_wsg_constants", - "//manipulation/schunk_wsg:schunk_wsg_lcm", - "//manipulation/schunk_wsg:schunk_wsg_position_controller", - "//multibody/parsing", - "//multibody/plant", - "//systems/controllers:inverse_dynamics_controller", - "//systems/framework", - "//systems/lcm", - "//systems/primitives", - "//systems/sensors:lcm_image_array_to_images", - "//systems/sensors/dev:rgbd_camera", - ], -) - drake_cc_library( name = "combined_manipulator_and_gripper_model", hdrs = [ From d83fdd394adcc4eded56ba367bc31f4b8aa25d09 Mon Sep 17 00:00:00 2001 From: Evan Drumwright Date: Wed, 24 Apr 2019 13:48:50 -0700 Subject: [PATCH 3/8] Build fix. --- examples/manipulation_station/BUILD.bazel | 1 - 1 file changed, 1 deletion(-) diff --git a/examples/manipulation_station/BUILD.bazel b/examples/manipulation_station/BUILD.bazel index 7b1413468c97..f762b02cad45 100644 --- a/examples/manipulation_station/BUILD.bazel +++ b/examples/manipulation_station/BUILD.bazel @@ -79,7 +79,6 @@ drake_cc_library( name = "combined_manipulator_and_gripper_model", hdrs = [ "combined_manipulator_and_gripper_model.h", - "manipulation_station_setup.h", ], deps = [ "//multibody/tree", From 8cbd98379e051d921585449c56a4b2cf159d2dc3 Mon Sep 17 00:00:00 2001 From: Evan Drumwright Date: Wed, 24 Apr 2019 14:15:21 -0700 Subject: [PATCH 4/8] Lint brush. --- .../combined_manipulator_and_gripper_model.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/examples/manipulation_station/combined_manipulator_and_gripper_model.h b/examples/manipulation_station/combined_manipulator_and_gripper_model.h index af410fd869d0..7a22b322968c 100644 --- a/examples/manipulation_station/combined_manipulator_and_gripper_model.h +++ b/examples/manipulation_station/combined_manipulator_and_gripper_model.h @@ -28,8 +28,8 @@ namespace manipulation_station { template class CombinedManipulatorAndGripperModel { public: - CombinedManipulatorAndGripperModel(multibody::MultibodyPlant* plant) : - plant_(plant) {} + explicit CombinedManipulatorAndGripperModel( + multibody::MultibodyPlant* plant) : plant_(plant) {} virtual ~CombinedManipulatorAndGripperModel() {} /// Gets the number of joints in the manipulator. From 19cf2a1b00d60b444108cfa8431149431f91f964 Mon Sep 17 00:00:00 2001 From: Evan Drumwright Date: Wed, 24 Apr 2019 14:23:23 -0700 Subject: [PATCH 5/8] Buildifier. --- examples/manipulation_station/BUILD.bazel | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/manipulation_station/BUILD.bazel b/examples/manipulation_station/BUILD.bazel index f762b02cad45..91bc0cbfd6fe 100644 --- a/examples/manipulation_station/BUILD.bazel +++ b/examples/manipulation_station/BUILD.bazel @@ -80,10 +80,10 @@ drake_cc_library( hdrs = [ "combined_manipulator_and_gripper_model.h", ], + visibility = ["//visibility:public"], deps = [ "//multibody/tree", ], - visibility = ["//visibility:public"], ) drake_cc_binary( From 5b7e19ccec8176c616de236e548097d8068701c0 Mon Sep 17 00:00:00 2001 From: Evan Drumwright Date: Wed, 8 May 2019 09:53:14 -0700 Subject: [PATCH 6/8] Checkpoint. --- examples/manipulation_station/BUILD.bazel | 4 +- ...del.h => combined_arm_and_gripper_model.h} | 38 +++++++++---------- 2 files changed, 21 insertions(+), 21 deletions(-) rename examples/manipulation_station/{combined_manipulator_and_gripper_model.h => combined_arm_and_gripper_model.h} (75%) diff --git a/examples/manipulation_station/BUILD.bazel b/examples/manipulation_station/BUILD.bazel index 91bc0cbfd6fe..22f82b27fbe3 100644 --- a/examples/manipulation_station/BUILD.bazel +++ b/examples/manipulation_station/BUILD.bazel @@ -76,9 +76,9 @@ drake_cc_library( ) drake_cc_library( - name = "combined_manipulator_and_gripper_model", + name = "combined_arm_and_gripper_model", hdrs = [ - "combined_manipulator_and_gripper_model.h", + "combined_arm_and_gripper_model.h", ], visibility = ["//visibility:public"], deps = [ diff --git a/examples/manipulation_station/combined_manipulator_and_gripper_model.h b/examples/manipulation_station/combined_arm_and_gripper_model.h similarity index 75% rename from examples/manipulation_station/combined_manipulator_and_gripper_model.h rename to examples/manipulation_station/combined_arm_and_gripper_model.h index 7a22b322968c..f8fbc97dd13e 100644 --- a/examples/manipulation_station/combined_manipulator_and_gripper_model.h +++ b/examples/manipulation_station/combined_arm_and_gripper_model.h @@ -21,25 +21,25 @@ namespace examples { namespace manipulation_station { /** - * An abstract class for adding a combined manipulator/gripper model and its + * An abstract class for adding a combined arm/gripper model and its * controller to a Diagram and querying aspects of the model (e.g., the number - * of manipulator generalized positions). + * of arm generalized positions). */ template -class CombinedManipulatorAndGripperModel { +class CombinedArmAndGripperModel { public: - explicit CombinedManipulatorAndGripperModel( + explicit CombinedArmAndGripperModel( multibody::MultibodyPlant* plant) : plant_(plant) {} - virtual ~CombinedManipulatorAndGripperModel() {} + virtual ~CombinedArmAndGripperModel() {} - /// Gets the number of joints in the manipulator. - virtual int num_manipulator_joints() const = 0; + /// Gets the number of joints in the arm. + virtual int num_arm_joints() const = 0; /// Gets the number of joints in the gripper. virtual int num_gripper_joints() const = 0; - /// Gets the manipulator generalized positions. - virtual VectorX GetManipulatorPositions( + /// Gets the arm generalized positions. + virtual VectorX GetArmPositions( const systems::Context& diagram_context, const systems::Diagram& diagram) const = 0; @@ -48,8 +48,8 @@ class CombinedManipulatorAndGripperModel { const systems::Context& diagram_context, const systems::Diagram& diagram) const = 0; - /// Sets the manipulator generalized positions. - virtual void SetManipulatorPositions( + /// Sets the arm generalized positions. + virtual void SetArmPositions( const systems::Context& diagram_context, const Eigen::Ref>& q, const systems::Diagram& diagram, @@ -68,8 +68,8 @@ class CombinedManipulatorAndGripperModel { const systems::Diagram& diagram, systems::State* diagram_state) const = 0; - /// Gets the manipulator generalized velocities. - virtual VectorX GetManipulatorVelocities( + /// Gets the arm generalized velocities. + virtual VectorX GetArmVelocities( const systems::Context& diagram_context, const systems::Diagram& diagram) const = 0; @@ -78,8 +78,8 @@ class CombinedManipulatorAndGripperModel { const systems::Context& diagram_context, const systems::Diagram& diagram) const = 0; - /// Sets the manipulator generalized velocities. - virtual void SetManipulatorVelocities( + /// Sets the arm generalized velocities. + virtual void SetArmVelocities( const systems::Context& diagram_context, const Eigen::Ref>& v, const systems::Diagram& diagram, @@ -92,18 +92,18 @@ class CombinedManipulatorAndGripperModel { const systems::Diagram& diagram, systems::State* diagram_state) const = 0; - /// This method adds the manipulator and gripper models to the internal plant. + /// This method adds the arm and gripper models to the internal plant. virtual void AddRobotModelToMultibodyPlant() = 0; - /// This method builds the control diagram for the manipulator and gripper + /// This method builds the control diagram for the arm and gripper /// and adds it to the given builder for a Diagram. virtual void BuildControlDiagram(systems::DiagramBuilder* builder) = 0; /// Gets a reference to the plant used for control. virtual const multibody::MultibodyPlant& get_controller_plant() const = 0; - /// Gets the model instance for the manipulator in the internal plant. - virtual multibody::ModelInstanceIndex manipulator_model_instance() const = 0; + /// Gets the model instance for the arm in the internal plant. + virtual multibody::ModelInstanceIndex arm_model_instance() const = 0; /// Gets the model instance for the gripper in the internal plant. virtual multibody::ModelInstanceIndex gripper_model_instance() const = 0; From 74b0ce6efef2d7f334d75d30788011c48d4920bd Mon Sep 17 00:00:00 2001 From: Evan Drumwright Date: Thu, 16 May 2019 14:43:09 -0700 Subject: [PATCH 7/8] Removed arm-specific virtual methods for state. --- .../combined_arm_and_gripper_model.h | 29 +------------------ 1 file changed, 1 insertion(+), 28 deletions(-) diff --git a/examples/manipulation_station/combined_arm_and_gripper_model.h b/examples/manipulation_station/combined_arm_and_gripper_model.h index f8fbc97dd13e..87aa03241103 100644 --- a/examples/manipulation_station/combined_arm_and_gripper_model.h +++ b/examples/manipulation_station/combined_arm_and_gripper_model.h @@ -32,29 +32,14 @@ class CombinedArmAndGripperModel { multibody::MultibodyPlant* plant) : plant_(plant) {} virtual ~CombinedArmAndGripperModel() {} - /// Gets the number of joints in the arm. - virtual int num_arm_joints() const = 0; - /// Gets the number of joints in the gripper. virtual int num_gripper_joints() const = 0; - /// Gets the arm generalized positions. - virtual VectorX GetArmPositions( - const systems::Context& diagram_context, - const systems::Diagram& diagram) const = 0; - /// Gets the gripper generalized positions. virtual VectorX GetGripperPositions( const systems::Context& diagram_context, const systems::Diagram& diagram) const = 0; - /// Sets the arm generalized positions. - virtual void SetArmPositions( - const systems::Context& diagram_context, - const Eigen::Ref>& q, - const systems::Diagram& diagram, - systems::State* diagram_state) const = 0; - /// Sets the gripper generalized positions to the default "open" position. virtual void SetGripperPositionsToDefaultOpen( const systems::Context& diagram_context, @@ -68,23 +53,11 @@ class CombinedArmAndGripperModel { const systems::Diagram& diagram, systems::State* diagram_state) const = 0; - /// Gets the arm generalized velocities. - virtual VectorX GetArmVelocities( - const systems::Context& diagram_context, - const systems::Diagram& diagram) const = 0; - /// Gets the gripper generalized velocities. virtual VectorX GetGripperVelocities( const systems::Context& diagram_context, const systems::Diagram& diagram) const = 0; - /// Sets the arm generalized velocities. - virtual void SetArmVelocities( - const systems::Context& diagram_context, - const Eigen::Ref>& v, - const systems::Diagram& diagram, - systems::State* diagram_state) const = 0; - /// Sets the gripper generalized velocities. virtual void SetGripperVelocities( const systems::Context& diagram_context, @@ -93,7 +66,7 @@ class CombinedArmAndGripperModel { systems::State* diagram_state) const = 0; /// This method adds the arm and gripper models to the internal plant. - virtual void AddRobotModelToMultibodyPlant() = 0; + virtual AddRobotModelToMultibodyPlant() = 0; /// This method builds the control diagram for the arm and gripper /// and adds it to the given builder for a Diagram. From 26e0a60c1b5e92885bf317d3dba7bc22e3e8b3bb Mon Sep 17 00:00:00 2001 From: Evan Drumwright Date: Thu, 16 May 2019 14:57:28 -0700 Subject: [PATCH 8/8] Updates from feature review. --- .../combined_arm_and_gripper_model.h | 26 +++++++------------ 1 file changed, 10 insertions(+), 16 deletions(-) diff --git a/examples/manipulation_station/combined_arm_and_gripper_model.h b/examples/manipulation_station/combined_arm_and_gripper_model.h index 87aa03241103..802d140f3004 100644 --- a/examples/manipulation_station/combined_arm_and_gripper_model.h +++ b/examples/manipulation_station/combined_arm_and_gripper_model.h @@ -21,9 +21,8 @@ namespace examples { namespace manipulation_station { /** - * An abstract class for adding a combined arm/gripper model and its - * controller to a Diagram and querying aspects of the model (e.g., the number - * of arm generalized positions). + * A virtual class for adding a combined arm/gripper model and its + * controller to a Diagram and querying aspects of that model. */ template class CombinedArmAndGripperModel { @@ -37,33 +36,28 @@ class CombinedArmAndGripperModel { /// Gets the gripper generalized positions. virtual VectorX GetGripperPositions( - const systems::Context& diagram_context, - const systems::Diagram& diagram) const = 0; + const systems::Context& plant_context) const = 0; /// Sets the gripper generalized positions to the default "open" position. virtual void SetGripperPositionsToDefaultOpen( - const systems::Context& diagram_context, - const systems::Diagram& diagram, - systems::State* diagram_state) const = 0; + const systems::Context& plant_context, + systems::State* plant_state) const = 0; /// Sets the gripper generalized positions. virtual void SetGripperPositions( - const systems::Context& diagram_context, + const systems::Context& plant_context, const Eigen::Ref>& q, - const systems::Diagram& diagram, - systems::State* diagram_state) const = 0; + systems::State* plant_state) const = 0; /// Gets the gripper generalized velocities. virtual VectorX GetGripperVelocities( - const systems::Context& diagram_context, - const systems::Diagram& diagram) const = 0; + const systems::Context& plant_context) const = 0; /// Sets the gripper generalized velocities. virtual void SetGripperVelocities( - const systems::Context& diagram_context, + const systems::Context& plant_context, const Eigen::Ref>& v, - const systems::Diagram& diagram, - systems::State* diagram_state) const = 0; + systems::State* plant_state) const = 0; /// This method adds the arm and gripper models to the internal plant. virtual AddRobotModelToMultibodyPlant() = 0;