From 7a76e8e42b4c42c642138e194b098de19653a8ab Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Fri, 4 Nov 2022 14:25:24 -0700 Subject: [PATCH 01/17] Add UFO Example --- drake_ros_examples/CMakeLists.txt | 1 + drake_ros_examples/README.md | 3 +- .../examples/ufo/CMakeLists.txt | 20 + drake_ros_examples/examples/ufo/README.md | 33 ++ drake_ros_examples/examples/ufo/ufo.cc | 506 ++++++++++++++++++ drake_ros_examples/examples/ufo/ufo.rviz | 172 ++++++ 6 files changed, 734 insertions(+), 1 deletion(-) create mode 100644 drake_ros_examples/examples/ufo/CMakeLists.txt create mode 100644 drake_ros_examples/examples/ufo/README.md create mode 100644 drake_ros_examples/examples/ufo/ufo.cc create mode 100644 drake_ros_examples/examples/ufo/ufo.rviz diff --git a/drake_ros_examples/CMakeLists.txt b/drake_ros_examples/CMakeLists.txt index bc47751b..016b323b 100644 --- a/drake_ros_examples/CMakeLists.txt +++ b/drake_ros_examples/CMakeLists.txt @@ -18,6 +18,7 @@ find_package(drake_ros_viz REQUIRED) add_subdirectory(examples/iiwa_manipulator) add_subdirectory(examples/multirobot) add_subdirectory(examples/rs_flip_flop) +add_subdirectory(examples/ufo) if(BUILD_TESTING) find_package(ament_cmake_clang_format REQUIRED) diff --git a/drake_ros_examples/README.md b/drake_ros_examples/README.md index 7d19a1f6..6c716059 100644 --- a/drake_ros_examples/README.md +++ b/drake_ros_examples/README.md @@ -38,4 +38,5 @@ Source your workspace. - [RS flip flop](./examples/rs_flip_flop): a latch with a ROS 2 topic interface. - [IIWA manipulator](./examples/iiwa_manipulator): an RViz visualization of a static IIWA arm. -- [Multiple robots](./examples/multirobot): an RViz visualisation of an array of Kuka LBR iiwa manipulators. +- [Multiple robots](./examples/multirobot): an RViz visualization of an array of Kuka LBR iiwa manipulators. +- [UFO](./examples/multirobot): an RViz visualization of a flying object controlled with the Drake systems framework and commandedusing RViz. diff --git a/drake_ros_examples/examples/ufo/CMakeLists.txt b/drake_ros_examples/examples/ufo/CMakeLists.txt new file mode 100644 index 00000000..390213f2 --- /dev/null +++ b/drake_ros_examples/examples/ufo/CMakeLists.txt @@ -0,0 +1,20 @@ +find_package(drake REQUIRED) +find_package(drake_ros_core REQUIRED) +find_package(drake_ros_tf2 REQUIRED) +find_package(drake_ros_viz REQUIRED) +find_package(geometry_msgs REQUIRED) + +add_executable(ufo ufo.cc) +target_link_libraries(ufo PRIVATE + drake::drake + drake_ros_core::drake_ros_core + drake_ros_tf2::drake_ros_tf2 + drake_ros_viz::drake_ros_viz + ${geometry_msgs_TARGETS} +) + +install( + TARGETS + ufo + DESTINATION lib/${PROJECT_NAME} +) diff --git a/drake_ros_examples/examples/ufo/README.md b/drake_ros_examples/examples/ufo/README.md new file mode 100644 index 00000000..a3026e81 --- /dev/null +++ b/drake_ros_examples/examples/ufo/README.md @@ -0,0 +1,33 @@ +# UFO + +## Overview + +The UFO example shows how to use Drake-ROS and the Drake systems framework to +enable controlling a flying object with RViz. + +It publishes the following topics: + +* `/tf` (all scene frames) +* `/scene_markers/collision` (all collision geometries) +* `/scene_markers/visual` (all visual geometries) + +It subscribes to the following topic + +* `/goal_pose` (commands where the object should fly to) + +## How to run the example + +Run the `ufo` executable. + +``` +ros2 run drake_ros_examples ufo +``` + +Run RViz in a different terminal with your ROS installation sourced to visualize the station. + +``` +ros2 run rviz2 rviz2 -d ufo.rviz +``` + +Use the `2D Goal Pose` button in RViz to set the location to which the object +should fly to. diff --git a/drake_ros_examples/examples/ufo/ufo.cc b/drake_ros_examples/examples/ufo/ufo.cc new file mode 100644 index 00000000..c8e92dd9 --- /dev/null +++ b/drake_ros_examples/examples/ufo/ufo.cc @@ -0,0 +1,506 @@ +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using drake::geometry::AddContactMaterial; +using drake::geometry::Ellipsoid; +using drake::geometry::HalfSpace; +using drake::geometry::ProximityProperties; +using drake::geometry::Sphere; +using drake::multibody::BodyIndex; +using drake_ros_core::DrakeRos; +using drake_ros_core::RosInterfaceSystem; +using drake_ros_core::RosSubscriberSystem; +using drake_ros_tf2::SceneTfBroadcasterParams; +using drake_ros_tf2::SceneTfBroadcasterSystem; +using drake_ros_viz::RvizVisualizer; +using drake_ros_viz::RvizVisualizerParams; +using Eigen::Quaterniond; +using Eigen::Vector3d; +using Eigen::Vector4d; +using Eigen::VectorXd; + +using BasicVectord = drake::systems::BasicVector; +using ConstantVectorSourced = drake::systems::ConstantVectorSource; +using Contextd = drake::systems::Context; +using CoulombFrictiond = drake::multibody::CoulombFriction; +using Diagramd = drake::systems::Diagram; +using DiagramBuilderd = drake::systems::DiagramBuilder; +using ExternallyAppliedSpatialForced = + drake::multibody::ExternallyAppliedSpatialForce; +using LeafSystemd = drake::systems::LeafSystem; +using MultibodyPlantd = drake::multibody::MultibodyPlant; +using Multiplexerd = drake::systems::Multiplexer; +using PidControllerd = drake::systems::controllers::PidController; +using RigidBodyd = drake::multibody::RigidBody; +using RigidTransformd = drake::math::RigidTransform; +using RollPitchYawd = drake::math::RollPitchYaw; +using RotationMatrixd = drake::math::RotationMatrix; +using Simulatord = drake::systems::Simulator; +using SpatialForced = drake::multibody::SpatialForce; +using SpatialInertiad = drake::multibody::SpatialInertia; +using StateInterpolatorWithDiscreteDerivatived = + drake::systems::StateInterpolatorWithDiscreteDerivative; +using Systemd = drake::systems::System; +using UnitInertiad = drake::multibody::UnitInertia; + +/// Adds body named FlyingSaucer to the multibody plant. +void AddFlyingSaucer(MultibodyPlantd* plant) { + const double kSaucerRadius = 2.5; + const double kSaucerThickness = 1.0; + const double kLookoutRadius = kSaucerThickness * 0.99; + + const double kA = kSaucerRadius; + const double kB = kSaucerRadius; + const double kC = kSaucerThickness / 2.0; + + const double kSaucerMass = 1000.0; + const double kLookoutMass = kSaucerMass * 0.1; + + auto G_Scm = UnitInertiad::SolidEllipsoid(kA, kB, kC); + auto G_Lcm = UnitInertiad::HollowSphere(kLookoutRadius); + + auto M_Scm = SpatialInertiad(kSaucerMass, Vector3d::Zero(), G_Scm); + auto M_Lcm = SpatialInertiad(kLookoutMass, Vector3d::Zero(), G_Lcm); + + const double kDissipation = 5.0; // s/m + const double kFrictionCoefficient = 0.3; + + const CoulombFrictiond kSurfaceFriction( + kFrictionCoefficient /* static friction */, + kFrictionCoefficient /* dynamic friction */); + + const Vector4d kGray(0.5, 0.5, 0.5, 1); + const Vector4d kTranslucentOrange(1.0, 0.55, 0.0, 0.35); + + auto saucer_geom = Ellipsoid(kA, kB, kC); + auto lookout_geom = Sphere(kLookoutRadius); + + ProximityProperties saucer_props; + ProximityProperties lookout_props; + + AddContactMaterial(kDissipation, {} /* point stiffness */, kSurfaceFriction, + &saucer_props); + AddContactMaterial(kDissipation, {} /* point stiffness */, kSurfaceFriction, + &lookout_props); + + const RigidTransformd X_SS; // identity + // Lookout in Saucer frame + const RigidTransformd X_SL(RollPitchYawd(0.0, 0.0, 0.0), + Vector3d(0.0, 0.0, kC)); + + // Combined Spatial Inertia + auto M_Ccm{M_Scm}; + M_Ccm += M_Lcm.Shift(X_SL.translation()); + + const RigidBodyd& flying_saucer = plant->AddRigidBody("FlyingSaucer", M_Ccm); + + plant->RegisterCollisionGeometry(flying_saucer, X_SS, saucer_geom, + "collision_saucer", saucer_props); + plant->RegisterVisualGeometry(flying_saucer, X_SS, saucer_geom, + "visual_saucer", kGray); + plant->RegisterCollisionGeometry(flying_saucer, X_SL, lookout_geom, + "collision_lookout", lookout_props); + plant->RegisterVisualGeometry(flying_saucer, X_SL, lookout_geom, + "visual_lookout", kTranslucentOrange); +} + +/// Adds Ground geometry to the world in the multibody plant. +void AddGround(MultibodyPlantd* plant) { + const double kDissipation = 5.0; // s/m + const double kFrictionCoefficient = 0.3; + + const CoulombFrictiond kSurfaceFriction( + kFrictionCoefficient /* static friction */, + kFrictionCoefficient /* dynamic friction */); + + const Vector4d kGreen(0.0, 0.5, 0.0, 1); + + RigidTransformd X_WG; // identity + + ProximityProperties ground_props; + AddContactMaterial(kDissipation, {} /* point stiffness */, kSurfaceFriction, + &ground_props); + plant->RegisterCollisionGeometry(plant->world_body(), X_WG, HalfSpace{}, + "collision_ground", std::move(ground_props)); + plant->RegisterVisualGeometry(plant->world_body(), X_WG, HalfSpace{}, + "visual_ground", kGreen); +} + +class SplitRigidTransform : public LeafSystemd { + public: + SplitRigidTransform() { + DeclareAbstractInputPort(kTransformPort, + *drake::AbstractValue::Make(RigidTransformd())); + + DeclareVectorOutputPort(kOrientationPort, BasicVectord(3), + &SplitRigidTransform::CalcOrientation); + + DeclareVectorOutputPort(kPositionPort, BasicVectord(3), + &SplitRigidTransform::CalcPosition); + } + + virtual ~SplitRigidTransform() = default; + + static constexpr const char* kTransformPort{"X_WF"}; + static constexpr const char* kOrientationPort{"R_WF"}; + static constexpr const char* kPositionPort{"p_WF"}; + + private: + void CalcOrientation(const Contextd& context, BasicVectord* output) const { + auto& transform_port = GetInputPort(kTransformPort); + const auto& X_WF = transform_port.Eval(context); + output->SetFromVector(RollPitchYawd(X_WF.rotation()).vector()); + } + + void CalcPosition(const Contextd& context, BasicVectord* output) const { + auto& transform_port = GetInputPort(kTransformPort); + const auto& X_WF = transform_port.Eval(context); + output->SetFromVector(X_WF.translation()); + } +}; + +class UnsplitSpatialForce : public LeafSystemd { + public: + UnsplitSpatialForce() { + DeclareVectorInputPort(kForcesPort, BasicVectord(3)); + DeclareVectorInputPort(kTorquesPort, BasicVectord(3)); + + DeclareAbstractOutputPort(kSpatialForcePort, + &UnsplitSpatialForce::CalcSpatialForce); + } + + virtual ~UnsplitSpatialForce() = default; + + static constexpr const char* kForcesPort{"f_F"}; + static constexpr const char* kTorquesPort{"t_F"}; + static constexpr const char* kSpatialForcePort{"F_F"}; + + private: + void CalcSpatialForce(const Contextd& context, SpatialForced* output) const { + auto& forces_port = GetInputPort(kForcesPort); + auto& torques_port = GetInputPort(kTorquesPort); + + const auto& f_F = forces_port.Eval(context).value(); + const auto& t_F = torques_port.Eval(context).value(); + *output = SpatialForced(t_F, f_F); + } +}; + +std::unique_ptr CreateSaucerController() { + // X_WS = Pose of saucer in world frame + // X_WT = Target saucer pose in world frame + // p_WS = Current saucer position in world frame + // p_WT = Target saucer position in world frame + // f_S_W = force to be applied to saucer in world frame + // t_S_W = torque to be applied to saucer in world frame + // F_S_W = SpatialForce to be applied on saucer in world frame + + // TODO(eric.cousineau): Add orientation controller + + // Not included: glue to MultibodyPlant's vectors of stuff + + DiagramBuilderd builder; + + // Input glue (current pose splitter) + // input: RigidTransform X_WS + // output: Vector3d (Euler) R_WS + // output: Vector3d p_WS + auto* current_pose_glue = builder.AddSystem(); + + // Input glue (target pose splitter) + // input: RigidTransform X_WT + // output: Vector3d (Euler) R_WT + // output: Vector3d p_WT + auto* target_pose_glue = builder.AddSystem(); + + // Zero velocity for target pose + auto* zero_vector3 = + builder.AddSystem(Vector3d{0.0, 0.0, 0.0}); + + // Current position state with derivative + // input: p_WS + // output: p_WS concatenated with v_WS + auto* current_position_interp = + builder.AddSystem(3, 0.01); + builder.Connect( + current_pose_glue->GetOutputPort(SplitRigidTransform::kPositionPort), + current_position_interp->get_input_port()); + + // Target position mux + // input: p_WT + // output: p_WT concatenated with v_WT (zeros) + auto* target_position_mux = + builder.AddSystem(std::vector{3, 3}); + builder.Connect( + target_pose_glue->GetOutputPort(SplitRigidTransform::kPositionPort), + target_position_mux->get_input_port(0)); + builder.Connect(zero_vector3->get_output_port(), + target_position_mux->get_input_port(1)); + + // Forces PidController + // input: estimated state Vector3d p_WS concatenated with v_WS + // input: desired state Vector3d p_WT concatenated with v_WT + // output: Vector3d f_S_W + auto* forces_pid_controller = builder.AddSystem( + Vector3d{100.0f, 100.0f, 2500.0f}, Vector3d{0.0f, 0.0f, 50.0f}, + Vector3d{500.0f, 500.0f, 500.0f}); + builder.Connect(current_position_interp->get_output_port(), + forces_pid_controller->get_input_port_estimated_state()); + builder.Connect(target_position_mux->get_output_port(), + forces_pid_controller->get_input_port_desired_state()); + + // Output Glue + // input: Vector3d f_S_W + // input: Vector3d t_S_W (zeros) + // output: SpacialForce F_S_W + auto* spatial_force_combiner = builder.AddSystem(); + builder.Connect( + forces_pid_controller->get_output_port_control(), + spatial_force_combiner->GetInputPort(UnsplitSpatialForce::kForcesPort)); + builder.Connect( + zero_vector3->get_output_port(), + spatial_force_combiner->GetInputPort(UnsplitSpatialForce::kTorquesPort)); + + // Whole diagram ports + // input: RigidTransform X_WS + // input: RigidTransform X_WT + // output: SpatialForced F_S_W + builder.ExportInput( + current_pose_glue->GetInputPort(SplitRigidTransform::kTransformPort), + "X_WS"); + builder.ExportInput( + target_pose_glue->GetInputPort(SplitRigidTransform::kTransformPort), + "X_WT"); + builder.ExportOutput(spatial_force_combiner->GetOutputPort( + UnsplitSpatialForce::kSpatialForcePort), + "F_S_W"); + + return builder.Build(); +} + +class BodyPoseAtIndex : public LeafSystemd { + public: + explicit BodyPoseAtIndex(BodyIndex index) : index_(index) { + DeclareAbstractInputPort( + kBodyPosesPort, + *drake::AbstractValue::Make(std::vector{})); + + DeclareAbstractOutputPort(kPosePort, &BodyPoseAtIndex::CalcPose); + } + + virtual ~BodyPoseAtIndex() = default; + + static constexpr const char* kBodyPosesPort{"body_poses"}; + static constexpr const char* kPosePort{"pose"}; + + private: + void CalcPose(const Contextd& context, RigidTransformd* output) const { + auto& body_poses_port = GetInputPort(kBodyPosesPort); + + const auto& body_poses = + body_poses_port.Eval>(context); + *output = body_poses.at(index_); + } + + const BodyIndex index_; +}; + +class AppliedSpatialForceVector : public LeafSystemd { + public: + explicit AppliedSpatialForceVector(BodyIndex index) : index_(index) { + DeclareAbstractInputPort(kSpatialForcePort, + *drake::AbstractValue::Make(SpatialForced())); + + DeclareAbstractOutputPort( + kAppliedSpatialForcePort, + &AppliedSpatialForceVector::CalcAppliedSpatialForceVector); + } + + virtual ~AppliedSpatialForceVector() = default; + + static constexpr const char* kSpatialForcePort{"body_poses"}; + static constexpr const char* kAppliedSpatialForcePort{ + "applied_spatial_force"}; + + private: + void CalcAppliedSpatialForceVector( + const Contextd& context, + std::vector* output) const { + auto& input_port = GetInputPort(kSpatialForcePort); + + const auto& F_WB = input_port.Eval(context); + ExternallyAppliedSpatialForced spatial_force; + spatial_force.body_index = index_; + spatial_force.p_BoBq_B = Vector3d{0, 0, 0}; + spatial_force.F_Bq_W = F_WB; + output->clear(); + output->push_back(spatial_force); + } + + const BodyIndex index_; +}; + +class RosPoseGlue : public LeafSystemd { + public: + explicit RosPoseGlue(double extra_z = 0.0) : extra_z_(extra_z) { + DeclareAbstractInputPort( + kRosMsgPort, + *drake::AbstractValue::Make(geometry_msgs::msg::PoseStamped())); + + DeclareAbstractOutputPort(kOutputPort, &RosPoseGlue::CalcDrakeTransform); + } + + virtual ~RosPoseGlue() = default; + + static constexpr const char* kRosMsgPort{"ros_pose"}; + static constexpr const char* kOutputPort{"drake_pose"}; + + private: + void CalcDrakeTransform(const Contextd& context, + RigidTransformd* output) const { + auto& input_port = GetInputPort(kRosMsgPort); + + // TODO(sloretz) use RobotLocomotion/drake-ros#141 + const auto& goal_pose = + input_port.Eval(context); + + Vector3d translation{ + goal_pose.pose.position.x, + goal_pose.pose.position.y, + goal_pose.pose.position.z + extra_z_, + }; + Quaterniond rotation{ + goal_pose.pose.orientation.w, + goal_pose.pose.orientation.x, + goal_pose.pose.orientation.y, + goal_pose.pose.orientation.z, + }; + + output->set_translation(translation); + output->set_rotation(rotation); + } + + const double extra_z_; +}; + +/// Build a simulation and set initial conditions. +std::unique_ptr BuildSimulation() { + DiagramBuilderd builder; + + auto [plant, scene_graph] = + drake::multibody::AddMultibodyPlantSceneGraph(&builder, 0.001); + + AddGround(&plant); + AddFlyingSaucer(&plant); + + plant.Finalize(); + + auto* ufo_controller = builder.AddSystem(CreateSaucerController()); + + // Glue controller to multibody plant + // Get saucer poses X_WS to controller + const BodyIndex ufo_index = plant.GetBodyByName("FlyingSaucer").index(); + auto* body_pose_at_index = builder.AddSystem(ufo_index); + builder.Connect( + plant.get_body_poses_output_port(), + body_pose_at_index->GetInputPort(BodyPoseAtIndex::kBodyPosesPort)); + builder.Connect(body_pose_at_index->GetOutputPort(BodyPoseAtIndex::kPosePort), + ufo_controller->GetInputPort("X_WS")); + + // TODO(sloretz) replace when RobotLocomotion/drake#16923 is solved + auto* apply_force_glue = + builder.AddSystem(ufo_index); + builder.Connect(ufo_controller->GetOutputPort("F_S_W"), + apply_force_glue->get_input_port()); + builder.Connect(apply_force_glue->get_output_port(), + plant.get_applied_spatial_force_input_port()); + + // Add basic system to create a ROS node + auto* ros_interface_system = builder.AddSystem( + std::make_unique("systems_framework_demo")); + + // Add a TF2 broadcaster to provide frame info to ROS + auto* scene_tf_broadcaster = builder.AddSystem( + ros_interface_system->get_ros_interface()); + builder.Connect(scene_graph.get_query_output_port(), + scene_tf_broadcaster->get_graph_query_input_port()); + + // Add a system to output the visualisation markers for RViz + auto* scene_visualizer = builder.AddSystem( + ros_interface_system->get_ros_interface()); + scene_visualizer->RegisterMultibodyPlant(&plant); + builder.Connect(scene_graph.get_query_output_port(), + scene_visualizer->get_graph_query_input_port()); + + // Add system to get goal pose from RViz + auto* goal_sub = builder.AddSystem( + RosSubscriberSystem::Make( + "goal_pose", rclcpp::QoS(1), + ros_interface_system->get_ros_interface())); + auto goal_glue = builder.AddSystem(10.0f); + builder.Connect(goal_sub->get_output_port(), goal_glue->get_input_port()); + builder.Connect(goal_glue->get_output_port(), + ufo_controller->GetInputPort("X_WT")); + + return builder.Build(); +} + +std::unique_ptr SetInitialConditions(Diagramd* diagram) { + std::unique_ptr diagram_context = diagram->CreateDefaultContext(); + + const Systemd& plant_system = diagram->GetSubsystemByName("plant"); + const MultibodyPlantd& plant = + dynamic_cast(plant_system); + Contextd& plant_context = + diagram->GetMutableSubsystemContext(plant, diagram_context.get()); + + RigidTransformd X_WS(RollPitchYawd(0.0, 0.0, 0.0), Vector3d(0.0, 0.0, 0.0)); + plant.SetFreeBodyPose(&plant_context, plant.GetBodyByName("FlyingSaucer"), + X_WS); + return diagram_context; +} + +void RunSimulation(Diagramd* diagram, + std::unique_ptr diagram_context) { + auto simulator = + std::make_unique(*diagram, std::move(diagram_context)); + + Contextd& simulator_context = simulator->get_mutable_context(); + simulator->get_mutable_integrator().set_maximum_step_size(1.0 / 50.0); + simulator->set_target_realtime_rate(1.0); + + simulator->Initialize(); + + while (true) { + simulator->AdvanceTo(simulator_context.get_time() + 0.1); + } +} + +int main() { + drake_ros_core::init(); + + std::unique_ptr diagram = BuildSimulation(); + std::unique_ptr diagram_context = + SetInitialConditions(diagram.get()); + + RunSimulation(diagram.get(), std::move(diagram_context)); + + return 0; +} diff --git a/drake_ros_examples/examples/ufo/ufo.rviz b/drake_ros_examples/examples/ufo/ufo.rviz new file mode 100644 index 00000000..9cb3ae3f --- /dev/null +++ b/drake_ros_examples/examples/ufo/ufo.rviz @@ -0,0 +1,172 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 138 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /TF1 + - /TF1/Frames1 + - /TF1/Tree1 + Splitter Ratio: 0.5 + Tree Height: 730 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 50 + Reference Frame: + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MarkerArray + Namespaces: + plant: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /scene_markers/visual + Value: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + DefaultModelInstance/FlyingSaucer/20: + Value: true + FlyingSaucer: + Value: true + world: + Value: true + Marker Scale: 10 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + world: + DefaultModelInstance/FlyingSaucer/20: + {} + FlyingSaucer: + {} + Update Interval: 0 + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: world + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 54.73565673828125 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.785398006439209 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.785398006439209 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1207 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd0000000400000000000001dc000003c8fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000ab00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000069000003c80000017800fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000014f000003c8fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000069000003c80000012300fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007c300000051fc0100000002fb0000000800540069006d00650100000000000007c30000045300fffffffb0000000800540069006d0065010000000000000450000000000000000000000480000003c800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1987 + X: 144 + Y: 60 From 3bffdbdb8d3e02f739f93f7438b380da06fc94c9 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Fri, 4 Nov 2022 14:29:36 -0700 Subject: [PATCH 02/17] clang-format --- drake_ros_examples/examples/ufo/ufo.cc | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/drake_ros_examples/examples/ufo/ufo.cc b/drake_ros_examples/examples/ufo/ufo.cc index c8e92dd9..7cce023e 100644 --- a/drake_ros_examples/examples/ufo/ufo.cc +++ b/drake_ros_examples/examples/ufo/ufo.cc @@ -112,13 +112,13 @@ void AddFlyingSaucer(MultibodyPlantd* plant) { const RigidBodyd& flying_saucer = plant->AddRigidBody("FlyingSaucer", M_Ccm); plant->RegisterCollisionGeometry(flying_saucer, X_SS, saucer_geom, - "collision_saucer", saucer_props); + "collision_saucer", saucer_props); plant->RegisterVisualGeometry(flying_saucer, X_SS, saucer_geom, - "visual_saucer", kGray); + "visual_saucer", kGray); plant->RegisterCollisionGeometry(flying_saucer, X_SL, lookout_geom, - "collision_lookout", lookout_props); + "collision_lookout", lookout_props); plant->RegisterVisualGeometry(flying_saucer, X_SL, lookout_geom, - "visual_lookout", kTranslucentOrange); + "visual_lookout", kTranslucentOrange); } /// Adds Ground geometry to the world in the multibody plant. @@ -138,9 +138,9 @@ void AddGround(MultibodyPlantd* plant) { AddContactMaterial(kDissipation, {} /* point stiffness */, kSurfaceFriction, &ground_props); plant->RegisterCollisionGeometry(plant->world_body(), X_WG, HalfSpace{}, - "collision_ground", std::move(ground_props)); + "collision_ground", std::move(ground_props)); plant->RegisterVisualGeometry(plant->world_body(), X_WG, HalfSpace{}, - "visual_ground", kGreen); + "visual_ground", kGreen); } class SplitRigidTransform : public LeafSystemd { @@ -498,7 +498,7 @@ int main() { std::unique_ptr diagram = BuildSimulation(); std::unique_ptr diagram_context = - SetInitialConditions(diagram.get()); + SetInitialConditions(diagram.get()); RunSimulation(diagram.get(), std::move(diagram_context)); From 405a951bcc2a7ea2390c1d3661b19ec46b62b1aa Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 9 Nov 2022 10:09:37 -0800 Subject: [PATCH 03/17] word-wrap to 80 chars --- drake_ros_examples/README.md | 26 +++++++++++++++++--------- 1 file changed, 17 insertions(+), 9 deletions(-) diff --git a/drake_ros_examples/README.md b/drake_ros_examples/README.md index 6c716059..317b77e1 100644 --- a/drake_ros_examples/README.md +++ b/drake_ros_examples/README.md @@ -1,25 +1,29 @@ # Drake ROS Examples -This is a collection of examples built around `drake_ros` libraries' C++ and Python APIs. +This is a collection of examples built around `drake_ros` libraries' C++ and +Python APIs. ## Building -This package has been built and tested on Ubuntu Focal with ROS Rolling, using a Drake nightly or any stable releases after 14 Jan 2022. +This package has been built and tested on Ubuntu Focal with ROS Rolling, using a +Drake nightly or any stable releases after 14 Jan 2022. It may work on other versions of ROS and Drake, but it hasn't been tested. To build it: 1. [Install ROS Rolling](https://index.ros.org/doc/ros2/Installation/Rolling/) 1. Source your ROS installation `. /opt/ros/rolling/setup.bash` -1. [Download Drake binary](https://drake.mit.edu/from_binary.html), nightly or any stable releases after 14 Jan 2022. -1. Extract the Drake binary installation, install it's prerequisites, and [use this Python virutalenv trick](https://drake.mit.edu/from_binary.html). +1. [Download Drake binary](https://drake.mit.edu/from_binary.html), nightly or +any stable releases after 14 Jan 2022. +1. Extract the Drake binary installation, install it's prerequisites, and +[use this Python virutalenv trick](https://drake.mit.edu/from_binary.html). 1. Activate the drake virtual environment. 1. Build it using Colcon. **Colcon** 1. Make a workspace `mkdir -p ./ws/src` 1. `cd ./ws/src` - 1. Get this code `git clone https://github.com/RobotLocomotion/drake-ros.git` + 1. Get the code `git clone https://github.com/RobotLocomotion/drake-ros.git` 1. `cd ..` 1. Build this package `colcon build --packages-up-to drake_ros_examples` @@ -32,11 +36,15 @@ Source your workspace. # Also make sure to activate drake virtual environment ``` - Now you can run C++ and Python examples using `ros2 run drake_ros_examples `. + Now you can run C++ and Python examples using `ros2 run drake_ros_examples + `. ## List of examples - [RS flip flop](./examples/rs_flip_flop): a latch with a ROS 2 topic interface. -- [IIWA manipulator](./examples/iiwa_manipulator): an RViz visualization of a static IIWA arm. -- [Multiple robots](./examples/multirobot): an RViz visualization of an array of Kuka LBR iiwa manipulators. -- [UFO](./examples/multirobot): an RViz visualization of a flying object controlled with the Drake systems framework and commandedusing RViz. +- [IIWA manipulator](./examples/iiwa_manipulator): an RViz visualization of a +static IIWA arm. +- [Multiple robots](./examples/multirobot): an RViz visualization of an array of +Kuka LBR iiwa manipulators. +- [UFO](./examples/multirobot): an RViz visualization of a flying object +controlled with the Drake systems framework and commanded using RViz. From b27858fcb2dee4567ad21f292cffc8f52207965e Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 9 Nov 2022 10:10:59 -0800 Subject: [PATCH 04/17] Drake-ROS -> Drake ROS --- drake_ros_examples/examples/ufo/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/drake_ros_examples/examples/ufo/README.md b/drake_ros_examples/examples/ufo/README.md index a3026e81..364a1063 100644 --- a/drake_ros_examples/examples/ufo/README.md +++ b/drake_ros_examples/examples/ufo/README.md @@ -2,7 +2,7 @@ ## Overview -The UFO example shows how to use Drake-ROS and the Drake systems framework to +The UFO example shows how to use Drake ROS and the Drake systems framework to enable controlling a flying object with RViz. It publishes the following topics: From 99e4f3a37824c0adf08bc4b9809c17c7350816ee Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 9 Nov 2022 10:11:10 -0800 Subject: [PATCH 05/17] Wrap to 80 chars --- drake_ros_examples/examples/ufo/README.md | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/drake_ros_examples/examples/ufo/README.md b/drake_ros_examples/examples/ufo/README.md index 364a1063..a6fcf86c 100644 --- a/drake_ros_examples/examples/ufo/README.md +++ b/drake_ros_examples/examples/ufo/README.md @@ -23,7 +23,8 @@ Run the `ufo` executable. ros2 run drake_ros_examples ufo ``` -Run RViz in a different terminal with your ROS installation sourced to visualize the station. +Run RViz in a different terminal with your ROS installation sourced to visualize +the station. ``` ros2 run rviz2 rviz2 -d ufo.rviz From e9e4d7641ac09d83d1ebbccdc0c019fb1c02fa1e Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 9 Nov 2022 12:10:16 -0800 Subject: [PATCH 06/17] Use sdf files for models --- .../examples/ufo/CMakeLists.txt | 9 ++ .../examples/ufo/models/ground.sdf | 25 ++++ .../examples/ufo/models/ufo.sdf | 79 +++++++++++++ drake_ros_examples/examples/ufo/ufo.cc | 111 ++++-------------- 4 files changed, 133 insertions(+), 91 deletions(-) create mode 100644 drake_ros_examples/examples/ufo/models/ground.sdf create mode 100644 drake_ros_examples/examples/ufo/models/ufo.sdf diff --git a/drake_ros_examples/examples/ufo/CMakeLists.txt b/drake_ros_examples/examples/ufo/CMakeLists.txt index 390213f2..59f1c4f8 100644 --- a/drake_ros_examples/examples/ufo/CMakeLists.txt +++ b/drake_ros_examples/examples/ufo/CMakeLists.txt @@ -1,3 +1,4 @@ +find_package(ament_index_cpp REQUIRED) find_package(drake REQUIRED) find_package(drake_ros_core REQUIRED) find_package(drake_ros_tf2 REQUIRED) @@ -6,15 +7,23 @@ find_package(geometry_msgs REQUIRED) add_executable(ufo ufo.cc) target_link_libraries(ufo PRIVATE + ament_index_cpp::ament_index_cpp drake::drake drake_ros_core::drake_ros_core drake_ros_tf2::drake_ros_tf2 drake_ros_viz::drake_ros_viz ${geometry_msgs_TARGETS} ) +target_compile_features(ufo PRIVATE cxx_std_17) install( TARGETS ufo DESTINATION lib/${PROJECT_NAME} ) + +install( + DIRECTORY + models/ + DESTINATION share/${PROJECT_NAME}/models/ +) diff --git a/drake_ros_examples/examples/ufo/models/ground.sdf b/drake_ros_examples/examples/ufo/models/ground.sdf new file mode 100644 index 00000000..5359a2d5 --- /dev/null +++ b/drake_ros_examples/examples/ufo/models/ground.sdf @@ -0,0 +1,25 @@ + + + + true + + + + + 50 50 + + + + 0.0 0.5 0.0 1.0 + + + + + + 50 50 + + + + + + diff --git a/drake_ros_examples/examples/ufo/models/ufo.sdf b/drake_ros_examples/examples/ufo/models/ufo.sdf new file mode 100644 index 00000000..005925fe --- /dev/null +++ b/drake_ros_examples/examples/ufo/models/ufo.sdf @@ -0,0 +1,79 @@ + + + + + + + + + 0 0 0.5 0 0 0 + + + + 2.5 2.5 0.5 + + + + 0.5 0.5 0.5 1.0 + + + + + + 2.5 2.5 0.5 + + + + + 1000 + + 1300 + 0 + 0 + 1300 + 0 + 2500 + + + + + + + 0 0 0.49 0 0 0 + + + + 0.98 + + + + 1.0 0.55 0.0 0.35 + + + + + + 0.98 + + + + + 100 + + 65.34 + 0 + 0 + 65.34 + 0 + 65.34 + + + + + + + spacecraft + lookout + + + diff --git a/drake_ros_examples/examples/ufo/ufo.cc b/drake_ros_examples/examples/ufo/ufo.cc index 7cce023e..47eafe57 100644 --- a/drake_ros_examples/examples/ufo/ufo.cc +++ b/drake_ros_examples/examples/ufo/ufo.cc @@ -1,6 +1,9 @@ +#include #include +#include #include +#include #include #include #include @@ -18,28 +21,19 @@ #include #include -using drake::geometry::AddContactMaterial; -using drake::geometry::Ellipsoid; -using drake::geometry::HalfSpace; -using drake::geometry::ProximityProperties; -using drake::geometry::Sphere; using drake::multibody::BodyIndex; +using drake::multibody::Parser; using drake_ros_core::DrakeRos; using drake_ros_core::RosInterfaceSystem; using drake_ros_core::RosSubscriberSystem; -using drake_ros_tf2::SceneTfBroadcasterParams; using drake_ros_tf2::SceneTfBroadcasterSystem; using drake_ros_viz::RvizVisualizer; -using drake_ros_viz::RvizVisualizerParams; using Eigen::Quaterniond; using Eigen::Vector3d; -using Eigen::Vector4d; -using Eigen::VectorXd; using BasicVectord = drake::systems::BasicVector; using ConstantVectorSourced = drake::systems::ConstantVectorSource; using Contextd = drake::systems::Context; -using CoulombFrictiond = drake::multibody::CoulombFriction; using Diagramd = drake::systems::Diagram; using DiagramBuilderd = drake::systems::DiagramBuilder; using ExternallyAppliedSpatialForced = @@ -48,99 +42,34 @@ using LeafSystemd = drake::systems::LeafSystem; using MultibodyPlantd = drake::multibody::MultibodyPlant; using Multiplexerd = drake::systems::Multiplexer; using PidControllerd = drake::systems::controllers::PidController; -using RigidBodyd = drake::multibody::RigidBody; using RigidTransformd = drake::math::RigidTransform; using RollPitchYawd = drake::math::RollPitchYaw; -using RotationMatrixd = drake::math::RotationMatrix; using Simulatord = drake::systems::Simulator; using SpatialForced = drake::multibody::SpatialForce; -using SpatialInertiad = drake::multibody::SpatialInertia; using StateInterpolatorWithDiscreteDerivatived = drake::systems::StateInterpolatorWithDiscreteDerivative; using Systemd = drake::systems::System; -using UnitInertiad = drake::multibody::UnitInertia; /// Adds body named FlyingSaucer to the multibody plant. void AddFlyingSaucer(MultibodyPlantd* plant) { - const double kSaucerRadius = 2.5; - const double kSaucerThickness = 1.0; - const double kLookoutRadius = kSaucerThickness * 0.99; - - const double kA = kSaucerRadius; - const double kB = kSaucerRadius; - const double kC = kSaucerThickness / 2.0; - - const double kSaucerMass = 1000.0; - const double kLookoutMass = kSaucerMass * 0.1; - - auto G_Scm = UnitInertiad::SolidEllipsoid(kA, kB, kC); - auto G_Lcm = UnitInertiad::HollowSphere(kLookoutRadius); - - auto M_Scm = SpatialInertiad(kSaucerMass, Vector3d::Zero(), G_Scm); - auto M_Lcm = SpatialInertiad(kLookoutMass, Vector3d::Zero(), G_Lcm); - - const double kDissipation = 5.0; // s/m - const double kFrictionCoefficient = 0.3; - - const CoulombFrictiond kSurfaceFriction( - kFrictionCoefficient /* static friction */, - kFrictionCoefficient /* dynamic friction */); - - const Vector4d kGray(0.5, 0.5, 0.5, 1); - const Vector4d kTranslucentOrange(1.0, 0.55, 0.0, 0.35); - - auto saucer_geom = Ellipsoid(kA, kB, kC); - auto lookout_geom = Sphere(kLookoutRadius); - - ProximityProperties saucer_props; - ProximityProperties lookout_props; - - AddContactMaterial(kDissipation, {} /* point stiffness */, kSurfaceFriction, - &saucer_props); - AddContactMaterial(kDissipation, {} /* point stiffness */, kSurfaceFriction, - &lookout_props); - - const RigidTransformd X_SS; // identity - // Lookout in Saucer frame - const RigidTransformd X_SL(RollPitchYawd(0.0, 0.0, 0.0), - Vector3d(0.0, 0.0, kC)); - - // Combined Spatial Inertia - auto M_Ccm{M_Scm}; - M_Ccm += M_Lcm.Shift(X_SL.translation()); - - const RigidBodyd& flying_saucer = plant->AddRigidBody("FlyingSaucer", M_Ccm); - - plant->RegisterCollisionGeometry(flying_saucer, X_SS, saucer_geom, - "collision_saucer", saucer_props); - plant->RegisterVisualGeometry(flying_saucer, X_SS, saucer_geom, - "visual_saucer", kGray); - plant->RegisterCollisionGeometry(flying_saucer, X_SL, lookout_geom, - "collision_lookout", lookout_props); - plant->RegisterVisualGeometry(flying_saucer, X_SL, lookout_geom, - "visual_lookout", kTranslucentOrange); + auto parser = Parser(plant); + std::filesystem::path pkg_share_dir{ + ament_index_cpp::get_package_share_directory("drake_ros_examples") + }; + const char * kUfoPath = "models/ufo.sdf"; + std::string model_file_path = (pkg_share_dir / kUfoPath).string(); + parser.AddModelFromFile(model_file_path, "spacecraft"); } /// Adds Ground geometry to the world in the multibody plant. void AddGround(MultibodyPlantd* plant) { - const double kDissipation = 5.0; // s/m - const double kFrictionCoefficient = 0.3; - - const CoulombFrictiond kSurfaceFriction( - kFrictionCoefficient /* static friction */, - kFrictionCoefficient /* dynamic friction */); - - const Vector4d kGreen(0.0, 0.5, 0.0, 1); - - RigidTransformd X_WG; // identity - - ProximityProperties ground_props; - AddContactMaterial(kDissipation, {} /* point stiffness */, kSurfaceFriction, - &ground_props); - plant->RegisterCollisionGeometry(plant->world_body(), X_WG, HalfSpace{}, - "collision_ground", std::move(ground_props)); - plant->RegisterVisualGeometry(plant->world_body(), X_WG, HalfSpace{}, - "visual_ground", kGreen); + auto parser = Parser(plant); + std::filesystem::path pkg_share_dir{ + ament_index_cpp::get_package_share_directory("drake_ros_examples") + }; + const char * kGroundPath = "models/ground.sdf"; + std::string model_file_path = (pkg_share_dir / kGroundPath).string(); + parser.AddModelFromFile(model_file_path, "ground"); } class SplitRigidTransform : public LeafSystemd { @@ -416,7 +345,7 @@ std::unique_ptr BuildSimulation() { // Glue controller to multibody plant // Get saucer poses X_WS to controller - const BodyIndex ufo_index = plant.GetBodyByName("FlyingSaucer").index(); + const BodyIndex ufo_index = plant.GetBodyByName("spacecraft").index(); auto* body_pose_at_index = builder.AddSystem(ufo_index); builder.Connect( plant.get_body_poses_output_port(), @@ -472,7 +401,7 @@ std::unique_ptr SetInitialConditions(Diagramd* diagram) { diagram->GetMutableSubsystemContext(plant, diagram_context.get()); RigidTransformd X_WS(RollPitchYawd(0.0, 0.0, 0.0), Vector3d(0.0, 0.0, 0.0)); - plant.SetFreeBodyPose(&plant_context, plant.GetBodyByName("FlyingSaucer"), + plant.SetFreeBodyPose(&plant_context, plant.GetBodyByName("spacecraft"), X_WS); return diagram_context; } From 1b6b407946465c7415780a733c51b97e0d174db2 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 9 Nov 2022 12:13:17 -0800 Subject: [PATCH 07/17] SetInitialConditions -> CreateInitialConditions --- drake_ros_examples/examples/ufo/ufo.cc | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/drake_ros_examples/examples/ufo/ufo.cc b/drake_ros_examples/examples/ufo/ufo.cc index 47eafe57..0bc45a6e 100644 --- a/drake_ros_examples/examples/ufo/ufo.cc +++ b/drake_ros_examples/examples/ufo/ufo.cc @@ -391,7 +391,7 @@ std::unique_ptr BuildSimulation() { return builder.Build(); } -std::unique_ptr SetInitialConditions(Diagramd* diagram) { +std::unique_ptr CreateInitialConditions(Diagramd* diagram) { std::unique_ptr diagram_context = diagram->CreateDefaultContext(); const Systemd& plant_system = diagram->GetSubsystemByName("plant"); @@ -400,9 +400,6 @@ std::unique_ptr SetInitialConditions(Diagramd* diagram) { Contextd& plant_context = diagram->GetMutableSubsystemContext(plant, diagram_context.get()); - RigidTransformd X_WS(RollPitchYawd(0.0, 0.0, 0.0), Vector3d(0.0, 0.0, 0.0)); - plant.SetFreeBodyPose(&plant_context, plant.GetBodyByName("spacecraft"), - X_WS); return diagram_context; } @@ -427,7 +424,7 @@ int main() { std::unique_ptr diagram = BuildSimulation(); std::unique_ptr diagram_context = - SetInitialConditions(diagram.get()); + CreateInitialConditions(diagram.get()); RunSimulation(diagram.get(), std::move(diagram_context)); From 848a66bab5e3664963bf0de521bbcb067a49912f Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 9 Nov 2022 16:12:54 -0800 Subject: [PATCH 08/17] Document gains were picked empirically --- drake_ros_examples/examples/ufo/ufo.cc | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/drake_ros_examples/examples/ufo/ufo.cc b/drake_ros_examples/examples/ufo/ufo.cc index 0bc45a6e..8d069533 100644 --- a/drake_ros_examples/examples/ufo/ufo.cc +++ b/drake_ros_examples/examples/ufo/ufo.cc @@ -184,9 +184,10 @@ std::unique_ptr CreateSaucerController() { target_position_mux->get_input_port(1)); // Forces PidController - // input: estimated state Vector3d p_WS concatenated with v_WS - // input: desired state Vector3d p_WT concatenated with v_WT - // output: Vector3d f_S_W + // input: estimated state Vector3d p_WS concatenated with v_WS + // input: desired state Vector3d p_WT concatenated with v_WT + // output: Vector3d f_S_W + // Gains picked through trial and error auto* forces_pid_controller = builder.AddSystem( Vector3d{100.0f, 100.0f, 2500.0f}, Vector3d{0.0f, 0.0f, 50.0f}, Vector3d{500.0f, 500.0f, 500.0f}); From 9132cbc0b56037fa24b82449c7057a9a7c9cb5ac Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 9 Nov 2022 17:23:38 -0800 Subject: [PATCH 09/17] Add gravity feedforward --- drake_ros_examples/examples/ufo/ufo.cc | 51 ++++++++++++++++++-------- 1 file changed, 36 insertions(+), 15 deletions(-) diff --git a/drake_ros_examples/examples/ufo/ufo.cc b/drake_ros_examples/examples/ufo/ufo.cc index 8d069533..ce251777 100644 --- a/drake_ros_examples/examples/ufo/ufo.cc +++ b/drake_ros_examples/examples/ufo/ufo.cc @@ -10,6 +10,7 @@ #include #include #include +#include #include #include #include @@ -22,6 +23,7 @@ #include using drake::multibody::BodyIndex; +using drake::multibody::ModelInstanceIndex; using drake::multibody::Parser; using drake_ros_core::DrakeRos; using drake_ros_core::RosInterfaceSystem; @@ -31,7 +33,9 @@ using drake_ros_viz::RvizVisualizer; using Eigen::Quaterniond; using Eigen::Vector3d; +using Adderd = drake::systems::Adder; using BasicVectord = drake::systems::BasicVector; +using Bodyd = drake::multibody::Body; using ConstantVectorSourced = drake::systems::ConstantVectorSource; using Contextd = drake::systems::Context; using Diagramd = drake::systems::Diagram; @@ -51,14 +55,14 @@ using StateInterpolatorWithDiscreteDerivatived = using Systemd = drake::systems::System; /// Adds body named FlyingSaucer to the multibody plant. -void AddFlyingSaucer(MultibodyPlantd* plant) { +ModelInstanceIndex AddFlyingSaucer(MultibodyPlantd* plant) { auto parser = Parser(plant); std::filesystem::path pkg_share_dir{ ament_index_cpp::get_package_share_directory("drake_ros_examples") }; const char * kUfoPath = "models/ufo.sdf"; std::string model_file_path = (pkg_share_dir / kUfoPath).string(); - parser.AddModelFromFile(model_file_path, "spacecraft"); + return parser.AddModelFromFile(model_file_path, "spacecraft"); } /// Adds Ground geometry to the world in the multibody plant. @@ -132,7 +136,15 @@ class UnsplitSpatialForce : public LeafSystemd { } }; -std::unique_ptr CreateSaucerController() { +/// Create a controller for the flying saucer +/// \param[in] saucer_mass Mass of spacecraft in kg for gravity feedforward +/// controller. +/// \param[in] gravity_vector Acceleration due to gravity expressed in world +/// frame. This assumes a flat planet with gravity that is constant +/// regardless of altitude. +std::unique_ptr CreateSaucerController( + double saucer_mass, Vector3d gravity_vector) +{ // X_WS = Pose of saucer in world frame // X_WT = Target saucer pose in world frame // p_WS = Current saucer position in world frame @@ -183,26 +195,38 @@ std::unique_ptr CreateSaucerController() { builder.Connect(zero_vector3->get_output_port(), target_position_mux->get_input_port(1)); + // Gravity feedforward + // output: Vector3d f_S_W + auto antigravity = + builder.AddSystem( + -1 * saucer_mass * gravity_vector); + // Forces PidController // input: estimated state Vector3d p_WS concatenated with v_WS // input: desired state Vector3d p_WT concatenated with v_WT // output: Vector3d f_S_W // Gains picked through trial and error auto* forces_pid_controller = builder.AddSystem( - Vector3d{100.0f, 100.0f, 2500.0f}, Vector3d{0.0f, 0.0f, 50.0f}, - Vector3d{500.0f, 500.0f, 500.0f}); + Vector3d{100.0f, 0.0f, 2500.0f}, Vector3d{0.0f, 0.0f, 50.0f}, + Vector3d{500.0f, 0.0f, 500.0f}); builder.Connect(current_position_interp->get_output_port(), forces_pid_controller->get_input_port_estimated_state()); builder.Connect(target_position_mux->get_output_port(), forces_pid_controller->get_input_port_desired_state()); + auto force_adder = builder.AddSystem(2, 3); + builder.Connect(antigravity->get_output_port(), + force_adder->get_input_port(0)); + builder.Connect(forces_pid_controller->get_output_port_control(), + force_adder->get_input_port(1)); + // Output Glue // input: Vector3d f_S_W // input: Vector3d t_S_W (zeros) // output: SpacialForce F_S_W auto* spatial_force_combiner = builder.AddSystem(); builder.Connect( - forces_pid_controller->get_output_port_control(), + force_adder->get_output_port(), spatial_force_combiner->GetInputPort(UnsplitSpatialForce::kForcesPort)); builder.Connect( zero_vector3->get_output_port(), @@ -338,11 +362,15 @@ std::unique_ptr BuildSimulation() { drake::multibody::AddMultibodyPlantSceneGraph(&builder, 0.001); AddGround(&plant); - AddFlyingSaucer(&plant); + ModelInstanceIndex saucer_idx = AddFlyingSaucer(&plant); plant.Finalize(); - auto* ufo_controller = builder.AddSystem(CreateSaucerController()); + const Bodyd& saucer_body = plant.GetUniqueFreeBaseBodyOrThrow(saucer_idx); + + auto* ufo_controller = builder.AddSystem( + CreateSaucerController(saucer_body.default_mass(), + plant.gravity_field().gravity_vector())); // Glue controller to multibody plant // Get saucer poses X_WS to controller @@ -394,13 +422,6 @@ std::unique_ptr BuildSimulation() { std::unique_ptr CreateInitialConditions(Diagramd* diagram) { std::unique_ptr diagram_context = diagram->CreateDefaultContext(); - - const Systemd& plant_system = diagram->GetSubsystemByName("plant"); - const MultibodyPlantd& plant = - dynamic_cast(plant_system); - Contextd& plant_context = - diagram->GetMutableSubsystemContext(plant, diagram_context.get()); - return diagram_context; } From d191f59dce67aaf8fcd1c64616db24f5e1b502b0 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Tue, 22 Nov 2022 10:30:40 -0800 Subject: [PATCH 10/17] Inline RunSimulation and CreateInitialConditions --- drake_ros_examples/examples/ufo/ufo.cc | 19 ++++--------------- 1 file changed, 4 insertions(+), 15 deletions(-) diff --git a/drake_ros_examples/examples/ufo/ufo.cc b/drake_ros_examples/examples/ufo/ufo.cc index ce251777..a224876d 100644 --- a/drake_ros_examples/examples/ufo/ufo.cc +++ b/drake_ros_examples/examples/ufo/ufo.cc @@ -420,13 +420,12 @@ std::unique_ptr BuildSimulation() { return builder.Build(); } -std::unique_ptr CreateInitialConditions(Diagramd* diagram) { +int main() { + drake_ros_core::init(); + + std::unique_ptr diagram = BuildSimulation(); std::unique_ptr diagram_context = diagram->CreateDefaultContext(); - return diagram_context; -} -void RunSimulation(Diagramd* diagram, - std::unique_ptr diagram_context) { auto simulator = std::make_unique(*diagram, std::move(diagram_context)); @@ -439,16 +438,6 @@ void RunSimulation(Diagramd* diagram, while (true) { simulator->AdvanceTo(simulator_context.get_time() + 0.1); } -} - -int main() { - drake_ros_core::init(); - - std::unique_ptr diagram = BuildSimulation(); - std::unique_ptr diagram_context = - CreateInitialConditions(diagram.get()); - - RunSimulation(diagram.get(), std::move(diagram_context)); return 0; } From 10f4eb44d2d04770e7b4dff64ec390301dba2b11 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Tue, 22 Nov 2022 11:09:06 -0800 Subject: [PATCH 11/17] Load models from single scene --- .../examples/ufo/models/ufo_scene.sdf | 16 ++++++++++ drake_ros_examples/examples/ufo/ufo.cc | 32 ++++++++----------- 2 files changed, 29 insertions(+), 19 deletions(-) create mode 100644 drake_ros_examples/examples/ufo/models/ufo_scene.sdf diff --git a/drake_ros_examples/examples/ufo/models/ufo_scene.sdf b/drake_ros_examples/examples/ufo/models/ufo_scene.sdf new file mode 100644 index 00000000..04104408 --- /dev/null +++ b/drake_ros_examples/examples/ufo/models/ufo_scene.sdf @@ -0,0 +1,16 @@ + + + + + + package://drake_ros_examples/models/ufo.sdf + + + + + package://drake_ros_examples/models/ground.sdf + + true + + + diff --git a/drake_ros_examples/examples/ufo/ufo.cc b/drake_ros_examples/examples/ufo/ufo.cc index a224876d..e8d77875 100644 --- a/drake_ros_examples/examples/ufo/ufo.cc +++ b/drake_ros_examples/examples/ufo/ufo.cc @@ -54,26 +54,21 @@ using StateInterpolatorWithDiscreteDerivatived = drake::systems::StateInterpolatorWithDiscreteDerivative; using Systemd = drake::systems::System; -/// Adds body named FlyingSaucer to the multibody plant. -ModelInstanceIndex AddFlyingSaucer(MultibodyPlantd* plant) { +/// Adds UFO scene to the multibody plant. +/// \return index of flying saucer +ModelInstanceIndex AddUfoScene(MultibodyPlantd* plant) { auto parser = Parser(plant); - std::filesystem::path pkg_share_dir{ - ament_index_cpp::get_package_share_directory("drake_ros_examples") - }; - const char * kUfoPath = "models/ufo.sdf"; - std::string model_file_path = (pkg_share_dir / kUfoPath).string(); - return parser.AddModelFromFile(model_file_path, "spacecraft"); -} + parser.package_map().Add( + "drake_ros_examples", + ament_index_cpp::get_package_share_directory("drake_ros_examples")); -/// Adds Ground geometry to the world in the multibody plant. -void AddGround(MultibodyPlantd* plant) { - auto parser = Parser(plant); - std::filesystem::path pkg_share_dir{ - ament_index_cpp::get_package_share_directory("drake_ros_examples") + // TODO(sloretz) make Drake Parser to support package:// + std::filesystem::path ufo_path{ + parser.package_map().GetPath("drake_ros_examples") }; - const char * kGroundPath = "models/ground.sdf"; - std::string model_file_path = (pkg_share_dir / kGroundPath).string(); - parser.AddModelFromFile(model_file_path, "ground"); + parser.AddAllModelsFromFile((ufo_path / "models/ufo_scene.sdf").string()); + + return plant->GetModelInstanceByName("spacecraft"); } class SplitRigidTransform : public LeafSystemd { @@ -361,8 +356,7 @@ std::unique_ptr BuildSimulation() { auto [plant, scene_graph] = drake::multibody::AddMultibodyPlantSceneGraph(&builder, 0.001); - AddGround(&plant); - ModelInstanceIndex saucer_idx = AddFlyingSaucer(&plant); + ModelInstanceIndex saucer_idx = AddUfoScene(&plant); plant.Finalize(); From cc5e66b5895bdcc4d5edec5ae9ba8d6be4c31a4e Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 23 Nov 2022 14:49:33 -0800 Subject: [PATCH 12/17] Don't reuse Drake's built-in systems, also add Orientation controller --- drake_ros_examples/examples/ufo/ufo.cc | 307 ++++++++++--------------- 1 file changed, 123 insertions(+), 184 deletions(-) diff --git a/drake_ros_examples/examples/ufo/ufo.cc b/drake_ros_examples/examples/ufo/ufo.cc index e8d77875..210c13c2 100644 --- a/drake_ros_examples/examples/ufo/ufo.cc +++ b/drake_ros_examples/examples/ufo/ufo.cc @@ -2,19 +2,12 @@ #include #include -#include #include #include #include #include -#include #include #include -#include -#include -#include -#include -#include #include #include #include @@ -25,6 +18,7 @@ using drake::multibody::BodyIndex; using drake::multibody::ModelInstanceIndex; using drake::multibody::Parser; +using drake::systems::AbstractStateIndex; using drake_ros_core::DrakeRos; using drake_ros_core::RosInterfaceSystem; using drake_ros_core::RosSubscriberSystem; @@ -33,216 +27,153 @@ using drake_ros_viz::RvizVisualizer; using Eigen::Quaterniond; using Eigen::Vector3d; -using Adderd = drake::systems::Adder; -using BasicVectord = drake::systems::BasicVector; using Bodyd = drake::multibody::Body; -using ConstantVectorSourced = drake::systems::ConstantVectorSource; using Contextd = drake::systems::Context; -using Diagramd = drake::systems::Diagram; using DiagramBuilderd = drake::systems::DiagramBuilder; +using Diagramd = drake::systems::Diagram; using ExternallyAppliedSpatialForced = drake::multibody::ExternallyAppliedSpatialForce; using LeafSystemd = drake::systems::LeafSystem; using MultibodyPlantd = drake::multibody::MultibodyPlant; -using Multiplexerd = drake::systems::Multiplexer; -using PidControllerd = drake::systems::controllers::PidController; using RigidTransformd = drake::math::RigidTransform; -using RollPitchYawd = drake::math::RollPitchYaw; using Simulatord = drake::systems::Simulator; using SpatialForced = drake::multibody::SpatialForce; -using StateInterpolatorWithDiscreteDerivatived = - drake::systems::StateInterpolatorWithDiscreteDerivative; -using Systemd = drake::systems::System; +using Stated = drake::systems::State; /// Adds UFO scene to the multibody plant. /// \return index of flying saucer ModelInstanceIndex AddUfoScene(MultibodyPlantd* plant) { auto parser = Parser(plant); parser.package_map().Add( - "drake_ros_examples", - ament_index_cpp::get_package_share_directory("drake_ros_examples")); + "drake_ros_examples", + ament_index_cpp::get_package_share_directory("drake_ros_examples")); - // TODO(sloretz) make Drake Parser to support package:// + // TODO(sloretz) make Drake Parser support package:// std::filesystem::path ufo_path{ - parser.package_map().GetPath("drake_ros_examples") - }; + parser.package_map().GetPath("drake_ros_examples")}; parser.AddAllModelsFromFile((ufo_path / "models/ufo_scene.sdf").string()); return plant->GetModelInstanceByName("spacecraft"); } -class SplitRigidTransform : public LeafSystemd { +class FlyingSaucerController : public LeafSystemd { public: - SplitRigidTransform() { - DeclareAbstractInputPort(kTransformPort, + FlyingSaucerController(double saucer_mass, Vector3d gravity, Vector3d kp, + Vector3d kd, Vector3d kp_rot, Vector3d kd_rot, + double period = 1.0 / 100.0) + : saucer_mass_(saucer_mass), + gravity_(gravity), + kp_(kp), + kd_(kd), + kp_rot_(kp_rot), + kd_rot_(kd_rot), + period_(period) { + DeclareAbstractInputPort(kCurrentPosePort, + *drake::AbstractValue::Make(RigidTransformd())); + DeclareAbstractInputPort(kTargetPosePort, *drake::AbstractValue::Make(RigidTransformd())); - DeclareVectorOutputPort(kOrientationPort, BasicVectord(3), - &SplitRigidTransform::CalcOrientation); - - DeclareVectorOutputPort(kPositionPort, BasicVectord(3), - &SplitRigidTransform::CalcPosition); - } - - virtual ~SplitRigidTransform() = default; + DeclareAbstractOutputPort(kSpatialForcePort, + &FlyingSaucerController::CalcSpatialForce); - static constexpr const char* kTransformPort{"X_WF"}; - static constexpr const char* kOrientationPort{"R_WF"}; - static constexpr const char* kPositionPort{"p_WF"}; + X_WS_idx_ = + DeclareAbstractState(*drake::AbstractValue::Make(RigidTransformd())); + prev_X_WS_idx_ = + DeclareAbstractState(*drake::AbstractValue::Make(RigidTransformd())); - private: - void CalcOrientation(const Contextd& context, BasicVectord* output) const { - auto& transform_port = GetInputPort(kTransformPort); - const auto& X_WF = transform_port.Eval(context); - output->SetFromVector(RollPitchYawd(X_WF.rotation()).vector()); + DeclarePeriodicUnrestrictedUpdateEvent( + period_, 0., &FlyingSaucerController::UpdateState); } - void CalcPosition(const Contextd& context, BasicVectord* output) const { - auto& transform_port = GetInputPort(kTransformPort); - const auto& X_WF = transform_port.Eval(context); - output->SetFromVector(X_WF.translation()); - } -}; - -class UnsplitSpatialForce : public LeafSystemd { - public: - UnsplitSpatialForce() { - DeclareVectorInputPort(kForcesPort, BasicVectord(3)); - DeclareVectorInputPort(kTorquesPort, BasicVectord(3)); + // Pose of saucer in world frame + static constexpr const char* kCurrentPosePort{"X_WS"}; + // Target saucer pose in world frame + static constexpr const char* kTargetPosePort{"X_WT"}; + // SpatialForce to be applied on saucer in world frame + static constexpr const char* kSpatialForcePort{"F_S_W"}; - DeclareAbstractOutputPort(kSpatialForcePort, - &UnsplitSpatialForce::CalcSpatialForce); + private: + void UpdateState(const Contextd& context, Stated* state) const { + auto& current_pose_port = GetInputPort(kCurrentPosePort); + + // Store current pose as old pose + state->get_mutable_abstract_state(prev_X_WS_idx_) = + state->get_mutable_abstract_state(X_WS_idx_); + + // TODO(sloretz) what happens if UpdateState is called faster than + // X_WS input? + // Store input pose as current pose + state->get_mutable_abstract_state(X_WS_idx_) = + current_pose_port.Eval(context); } - virtual ~UnsplitSpatialForce() = default; - - static constexpr const char* kForcesPort{"f_F"}; - static constexpr const char* kTorquesPort{"t_F"}; - static constexpr const char* kSpatialForcePort{"F_F"}; - - private: void CalcSpatialForce(const Contextd& context, SpatialForced* output) const { - auto& forces_port = GetInputPort(kForcesPort); - auto& torques_port = GetInputPort(kTorquesPort); - - const auto& f_F = forces_port.Eval(context).value(); - const auto& t_F = torques_port.Eval(context).value(); - *output = SpatialForced(t_F, f_F); + // Get target pose + auto& target_pose_port = GetInputPort(kTargetPosePort); + const auto& X_WT = target_pose_port.Eval(context); + + // Get Current and previous pose + const auto& avs = context.get_abstract_state(); + const auto& X_WS = avs.get_value(X_WS_idx_).get_value(); + const auto& prev_X_WS = + avs.get_value(prev_X_WS_idx_).get_value(); + + // force to be applied to saucer in world frame + Vector3d f_S_W{0, 0, 0}; + // torque to be applied to saucer in world frame + Vector3d t_S_W{0, 0, 0}; + + // Translation + // p_WS = Current saucer position in world frame + // p_WT = Target saucer position in world frame + const auto& p_WS = X_WS.translation(); + const auto& p_WT = X_WT.translation(); + + // Translation Error + const auto p_error = p_WT - p_WS; + const auto prev_p_error = p_WT - prev_X_WS.translation(); + + // Translation PD controller - proportional + f_S_W += ((p_error).array() * kp_.array()).matrix(); + + // Translation PD controller - derivative + const auto de_dt = (p_error - prev_p_error) / period_; + f_S_W += (de_dt.array() * kd_.array()).matrix(); + + // Translation PD Gravity feedforward + f_S_W -= saucer_mass_ * gravity_; + + // Orientation + const auto& R_WS = X_WS.rotation(); + const auto& R_WT = X_WT.rotation(); + + // Rotation from current to target orientation + const auto R_error = R_WT * R_WS.inverse(); + const auto prev_R_error = R_WT * prev_X_WS.rotation().inverse(); + + // Orientation PD controller - proportional + t_S_W += + (R_error.ToRollPitchYaw().vector().array() * kp_rot_.array()).matrix(); + + // Orientation PD controller - derivative + const auto rot_de_dt = + (R_error * prev_R_error.inverse()).ToRollPitchYaw().vector() / period_; + t_S_W += (rot_de_dt.array() * kd_rot_.array()).matrix(); + + *output = SpatialForced(t_S_W, f_S_W); } -}; -/// Create a controller for the flying saucer -/// \param[in] saucer_mass Mass of spacecraft in kg for gravity feedforward -/// controller. -/// \param[in] gravity_vector Acceleration due to gravity expressed in world -/// frame. This assumes a flat planet with gravity that is constant -/// regardless of altitude. -std::unique_ptr CreateSaucerController( - double saucer_mass, Vector3d gravity_vector) -{ - // X_WS = Pose of saucer in world frame - // X_WT = Target saucer pose in world frame - // p_WS = Current saucer position in world frame - // p_WT = Target saucer position in world frame - // f_S_W = force to be applied to saucer in world frame - // t_S_W = torque to be applied to saucer in world frame - // F_S_W = SpatialForce to be applied on saucer in world frame - - // TODO(eric.cousineau): Add orientation controller - - // Not included: glue to MultibodyPlant's vectors of stuff + const double saucer_mass_; + const Vector3d gravity_; + const Vector3d kp_; + const Vector3d kd_; + const Vector3d kp_rot_; + const Vector3d kd_rot_; + const double period_; - DiagramBuilderd builder; - - // Input glue (current pose splitter) - // input: RigidTransform X_WS - // output: Vector3d (Euler) R_WS - // output: Vector3d p_WS - auto* current_pose_glue = builder.AddSystem(); - - // Input glue (target pose splitter) - // input: RigidTransform X_WT - // output: Vector3d (Euler) R_WT - // output: Vector3d p_WT - auto* target_pose_glue = builder.AddSystem(); - - // Zero velocity for target pose - auto* zero_vector3 = - builder.AddSystem(Vector3d{0.0, 0.0, 0.0}); - - // Current position state with derivative - // input: p_WS - // output: p_WS concatenated with v_WS - auto* current_position_interp = - builder.AddSystem(3, 0.01); - builder.Connect( - current_pose_glue->GetOutputPort(SplitRigidTransform::kPositionPort), - current_position_interp->get_input_port()); - - // Target position mux - // input: p_WT - // output: p_WT concatenated with v_WT (zeros) - auto* target_position_mux = - builder.AddSystem(std::vector{3, 3}); - builder.Connect( - target_pose_glue->GetOutputPort(SplitRigidTransform::kPositionPort), - target_position_mux->get_input_port(0)); - builder.Connect(zero_vector3->get_output_port(), - target_position_mux->get_input_port(1)); - - // Gravity feedforward - // output: Vector3d f_S_W - auto antigravity = - builder.AddSystem( - -1 * saucer_mass * gravity_vector); - - // Forces PidController - // input: estimated state Vector3d p_WS concatenated with v_WS - // input: desired state Vector3d p_WT concatenated with v_WT - // output: Vector3d f_S_W - // Gains picked through trial and error - auto* forces_pid_controller = builder.AddSystem( - Vector3d{100.0f, 0.0f, 2500.0f}, Vector3d{0.0f, 0.0f, 50.0f}, - Vector3d{500.0f, 0.0f, 500.0f}); - builder.Connect(current_position_interp->get_output_port(), - forces_pid_controller->get_input_port_estimated_state()); - builder.Connect(target_position_mux->get_output_port(), - forces_pid_controller->get_input_port_desired_state()); - - auto force_adder = builder.AddSystem(2, 3); - builder.Connect(antigravity->get_output_port(), - force_adder->get_input_port(0)); - builder.Connect(forces_pid_controller->get_output_port_control(), - force_adder->get_input_port(1)); - - // Output Glue - // input: Vector3d f_S_W - // input: Vector3d t_S_W (zeros) - // output: SpacialForce F_S_W - auto* spatial_force_combiner = builder.AddSystem(); - builder.Connect( - force_adder->get_output_port(), - spatial_force_combiner->GetInputPort(UnsplitSpatialForce::kForcesPort)); - builder.Connect( - zero_vector3->get_output_port(), - spatial_force_combiner->GetInputPort(UnsplitSpatialForce::kTorquesPort)); - - // Whole diagram ports - // input: RigidTransform X_WS - // input: RigidTransform X_WT - // output: SpatialForced F_S_W - builder.ExportInput( - current_pose_glue->GetInputPort(SplitRigidTransform::kTransformPort), - "X_WS"); - builder.ExportInput( - target_pose_glue->GetInputPort(SplitRigidTransform::kTransformPort), - "X_WT"); - builder.ExportOutput(spatial_force_combiner->GetOutputPort( - UnsplitSpatialForce::kSpatialForcePort), - "F_S_W"); - - return builder.Build(); -} + AbstractStateIndex X_WS_idx_; + AbstractStateIndex prev_X_WS_idx_; +}; class BodyPoseAtIndex : public LeafSystemd { public: @@ -360,11 +291,19 @@ std::unique_ptr BuildSimulation() { plant.Finalize(); - const Bodyd& saucer_body = plant.GetUniqueFreeBaseBodyOrThrow(saucer_idx); + double saucer_mass{0.0}; + auto body_idxs = plant.GetBodyIndices(saucer_idx); + for (auto& body_idx : body_idxs) { + const Bodyd& body = plant.get_body(body_idx); + saucer_mass += body.default_mass(); + } - auto* ufo_controller = builder.AddSystem( - CreateSaucerController(saucer_body.default_mass(), - plant.gravity_field().gravity_vector())); + auto ufo_controller = builder.AddSystem( + saucer_mass, plant.gravity_field().gravity_vector(), + Vector3d{100.0, 100.0, 100.0}, // kp & kd chosen by trial and error + Vector3d{500.0, 500.0, 400.0}, + Vector3d{1000.0, 1000.0, 1000.0}, // kp_rot & kd_rot also trial and error + Vector3d{1000.0, 1000.0, 1000.0}); // Glue controller to multibody plant // Get saucer poses X_WS to controller From af2f72b48cf499338d688f194109c846d4d12e3c Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Mon, 28 Nov 2022 15:03:14 -0800 Subject: [PATCH 13/17] RigidTransformPremultiplier --- drake_ros_examples/examples/ufo/ufo.cc | 37 ++++++++++++++++++++++---- 1 file changed, 32 insertions(+), 5 deletions(-) diff --git a/drake_ros_examples/examples/ufo/ufo.cc b/drake_ros_examples/examples/ufo/ufo.cc index 210c13c2..fd1127a7 100644 --- a/drake_ros_examples/examples/ufo/ufo.cc +++ b/drake_ros_examples/examples/ufo/ufo.cc @@ -237,9 +237,32 @@ class AppliedSpatialForceVector : public LeafSystemd { const BodyIndex index_; }; +class RigidTransformPremultiplier : public LeafSystemd { + public: + explicit RigidTransformPremultiplier(const RigidTransformd& X_BC) : X_BC_(X_BC) { + DeclareAbstractInputPort( + kInputPort, + *drake::AbstractValue::Make(RigidTransformd())); + + DeclareAbstractOutputPort(kOutputPort, &RigidTransformPremultiplier::CalcTransform); + } + + static constexpr const char* kInputPort{"X_AB"}; + static constexpr const char* kOutputPort{"X_AC"}; + protected: + void CalcTransform(const Contextd& context, + RigidTransformd* X_AC) const { + auto& input_port = GetInputPort(kInputPort); + auto& X_AB = input_port.Eval(context); + *X_AC = X_AB * X_BC_; + } + + const RigidTransformd X_BC_; +}; + class RosPoseGlue : public LeafSystemd { public: - explicit RosPoseGlue(double extra_z = 0.0) : extra_z_(extra_z) { + explicit RosPoseGlue() { DeclareAbstractInputPort( kRosMsgPort, *drake::AbstractValue::Make(geometry_msgs::msg::PoseStamped())); @@ -264,7 +287,7 @@ class RosPoseGlue : public LeafSystemd { Vector3d translation{ goal_pose.pose.position.x, goal_pose.pose.position.y, - goal_pose.pose.position.z + extra_z_, + goal_pose.pose.position.z, }; Quaterniond rotation{ goal_pose.pose.orientation.w, @@ -276,8 +299,6 @@ class RosPoseGlue : public LeafSystemd { output->set_translation(translation); output->set_rotation(rotation); } - - const double extra_z_; }; /// Build a simulation and set initial conditions. @@ -345,9 +366,15 @@ std::unique_ptr BuildSimulation() { RosSubscriberSystem::Make( "goal_pose", rclcpp::QoS(1), ros_interface_system->get_ros_interface())); - auto goal_glue = builder.AddSystem(10.0f); + auto goal_glue = builder.AddSystem(); builder.Connect(goal_sub->get_output_port(), goal_glue->get_input_port()); + + // RViz goal is on the ground - add system to raise it by 10 meters + auto goal_offsetter = builder.AddSystem( + RigidTransformd(Vector3d{0, 0, 10.0})); builder.Connect(goal_glue->get_output_port(), + goal_offsetter->get_input_port()); + builder.Connect(goal_offsetter->get_output_port(), ufo_controller->GetInputPort("X_WT")); return builder.Build(); From 4c39ea0582f4e770d2e011f2cced5220f1074421 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Mon, 28 Nov 2022 15:08:06 -0800 Subject: [PATCH 14/17] Use drake_ros_core conversions --- drake_ros_examples/examples/ufo/ufo.cc | 16 ++-------------- 1 file changed, 2 insertions(+), 14 deletions(-) diff --git a/drake_ros_examples/examples/ufo/ufo.cc b/drake_ros_examples/examples/ufo/ufo.cc index fd1127a7..2ff629ae 100644 --- a/drake_ros_examples/examples/ufo/ufo.cc +++ b/drake_ros_examples/examples/ufo/ufo.cc @@ -9,6 +9,7 @@ #include #include #include +#include #include #include #include @@ -284,20 +285,7 @@ class RosPoseGlue : public LeafSystemd { const auto& goal_pose = input_port.Eval(context); - Vector3d translation{ - goal_pose.pose.position.x, - goal_pose.pose.position.y, - goal_pose.pose.position.z, - }; - Quaterniond rotation{ - goal_pose.pose.orientation.w, - goal_pose.pose.orientation.x, - goal_pose.pose.orientation.y, - goal_pose.pose.orientation.z, - }; - - output->set_translation(translation); - output->set_rotation(rotation); + *output = drake_ros_core::RosPoseToRigidTransform(goal_pose.pose); } }; From 16102f7276166d1b7de7a9926c9350dd891e2d4c Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Mon, 28 Nov 2022 15:31:37 -0800 Subject: [PATCH 15/17] Replace TODO with comment --- drake_ros_examples/examples/ufo/ufo.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/drake_ros_examples/examples/ufo/ufo.cc b/drake_ros_examples/examples/ufo/ufo.cc index 2ff629ae..2de11ff5 100644 --- a/drake_ros_examples/examples/ufo/ufo.cc +++ b/drake_ros_examples/examples/ufo/ufo.cc @@ -324,7 +324,7 @@ std::unique_ptr BuildSimulation() { builder.Connect(body_pose_at_index->GetOutputPort(BodyPoseAtIndex::kPosePort), ufo_controller->GetInputPort("X_WS")); - // TODO(sloretz) replace when RobotLocomotion/drake#16923 is solved + // Connect UFO Controller's output to applied force accepted by MultibodyPlant auto* apply_force_glue = builder.AddSystem(ufo_index); builder.Connect(ufo_controller->GetOutputPort("F_S_W"), From a52d966464c04f96a83f2e4f1d12adca42b8a729 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 28 Dec 2022 17:05:46 -0800 Subject: [PATCH 16/17] Add FlyingSaucerController::Params struct Signed-off-by: Shane Loretz --- drake_ros_examples/examples/ufo/ufo.cc | 61 ++++++++++++++++---------- 1 file changed, 37 insertions(+), 24 deletions(-) diff --git a/drake_ros_examples/examples/ufo/ufo.cc b/drake_ros_examples/examples/ufo/ufo.cc index 2de11ff5..9f8c7ba1 100644 --- a/drake_ros_examples/examples/ufo/ufo.cc +++ b/drake_ros_examples/examples/ufo/ufo.cc @@ -59,16 +59,24 @@ ModelInstanceIndex AddUfoScene(MultibodyPlantd* plant) { class FlyingSaucerController : public LeafSystemd { public: - FlyingSaucerController(double saucer_mass, Vector3d gravity, Vector3d kp, - Vector3d kd, Vector3d kp_rot, Vector3d kd_rot, - double period = 1.0 / 100.0) - : saucer_mass_(saucer_mass), - gravity_(gravity), - kp_(kp), - kd_(kd), - kp_rot_(kp_rot), - kd_rot_(kd_rot), - period_(period) { + struct Params { + double saucer_mass; + Vector3d gravity; + Vector3d kp; + Vector3d kd; + Vector3d kp_rot; + Vector3d kd_rot; + double period{1.0 / 100.0}; + }; + + explicit FlyingSaucerController(const Params& params) + : saucer_mass_(params.saucer_mass), + gravity_(params.gravity), + kp_(params.kp), + kd_(params.kd), + kp_rot_(params.kp_rot), + kd_rot_(params.kd_rot), + period_(params.period) { DeclareAbstractInputPort(kCurrentPosePort, *drake::AbstractValue::Make(RigidTransformd())); DeclareAbstractInputPort(kTargetPosePort, @@ -240,19 +248,20 @@ class AppliedSpatialForceVector : public LeafSystemd { class RigidTransformPremultiplier : public LeafSystemd { public: - explicit RigidTransformPremultiplier(const RigidTransformd& X_BC) : X_BC_(X_BC) { - DeclareAbstractInputPort( - kInputPort, - *drake::AbstractValue::Make(RigidTransformd())); + explicit RigidTransformPremultiplier(const RigidTransformd& X_BC) + : X_BC_(X_BC) { + DeclareAbstractInputPort(kInputPort, + *drake::AbstractValue::Make(RigidTransformd())); - DeclareAbstractOutputPort(kOutputPort, &RigidTransformPremultiplier::CalcTransform); + DeclareAbstractOutputPort(kOutputPort, + &RigidTransformPremultiplier::CalcTransform); } static constexpr const char* kInputPort{"X_AB"}; static constexpr const char* kOutputPort{"X_AC"}; + protected: - void CalcTransform(const Contextd& context, - RigidTransformd* X_AC) const { + void CalcTransform(const Contextd& context, RigidTransformd* X_AC) const { auto& input_port = GetInputPort(kInputPort); auto& X_AB = input_port.Eval(context); *X_AC = X_AB * X_BC_; @@ -263,7 +272,7 @@ class RigidTransformPremultiplier : public LeafSystemd { class RosPoseGlue : public LeafSystemd { public: - explicit RosPoseGlue() { + RosPoseGlue() { DeclareAbstractInputPort( kRosMsgPort, *drake::AbstractValue::Make(geometry_msgs::msg::PoseStamped())); @@ -307,12 +316,16 @@ std::unique_ptr BuildSimulation() { saucer_mass += body.default_mass(); } - auto ufo_controller = builder.AddSystem( - saucer_mass, plant.gravity_field().gravity_vector(), - Vector3d{100.0, 100.0, 100.0}, // kp & kd chosen by trial and error - Vector3d{500.0, 500.0, 400.0}, - Vector3d{1000.0, 1000.0, 1000.0}, // kp_rot & kd_rot also trial and error - Vector3d{1000.0, 1000.0, 1000.0}); + FlyingSaucerController::Params params; + params.saucer_mass = saucer_mass; + params.gravity = plant.gravity_field().gravity_vector(); + params.kp = + Vector3d{100.0, 100.0, 100.0}; // kp & kd chosen by trial and error + params.kd = Vector3d{500.0, 500.0, 400.0}; + params.kp_rot = + Vector3d{1000.0, 1000.0, 1000.0}; // kp_rot & kd_rot also trial and error + params.kd_rot = Vector3d{1000.0, 1000.0, 1000.0}; + auto ufo_controller = builder.AddSystem(params); // Glue controller to multibody plant // Get saucer poses X_WS to controller From 5211f1700828c55468793c630c137579b61e511e Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Fri, 17 Mar 2023 18:04:29 +0000 Subject: [PATCH 17/17] Update for combined package --- .../examples/ufo/CMakeLists.txt | 10 +++---- drake_ros_examples/examples/ufo/ufo.cc | 26 +++++++++---------- 2 files changed, 17 insertions(+), 19 deletions(-) diff --git a/drake_ros_examples/examples/ufo/CMakeLists.txt b/drake_ros_examples/examples/ufo/CMakeLists.txt index 59f1c4f8..53aa6643 100644 --- a/drake_ros_examples/examples/ufo/CMakeLists.txt +++ b/drake_ros_examples/examples/ufo/CMakeLists.txt @@ -1,17 +1,15 @@ find_package(ament_index_cpp REQUIRED) find_package(drake REQUIRED) -find_package(drake_ros_core REQUIRED) -find_package(drake_ros_tf2 REQUIRED) -find_package(drake_ros_viz REQUIRED) +find_package(drake_ros REQUIRED) find_package(geometry_msgs REQUIRED) add_executable(ufo ufo.cc) target_link_libraries(ufo PRIVATE ament_index_cpp::ament_index_cpp drake::drake - drake_ros_core::drake_ros_core - drake_ros_tf2::drake_ros_tf2 - drake_ros_viz::drake_ros_viz + drake_ros::drake_ros_core + drake_ros::drake_ros_tf2 + drake_ros::drake_ros_viz ${geometry_msgs_TARGETS} ) target_compile_features(ufo PRIVATE cxx_std_17) diff --git a/drake_ros_examples/examples/ufo/ufo.cc b/drake_ros_examples/examples/ufo/ufo.cc index 9f8c7ba1..021d2f66 100644 --- a/drake_ros_examples/examples/ufo/ufo.cc +++ b/drake_ros_examples/examples/ufo/ufo.cc @@ -8,23 +8,23 @@ #include #include #include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include #include using drake::multibody::BodyIndex; using drake::multibody::ModelInstanceIndex; using drake::multibody::Parser; using drake::systems::AbstractStateIndex; -using drake_ros_core::DrakeRos; -using drake_ros_core::RosInterfaceSystem; -using drake_ros_core::RosSubscriberSystem; -using drake_ros_tf2::SceneTfBroadcasterSystem; -using drake_ros_viz::RvizVisualizer; +using drake_ros::core::DrakeRos; +using drake_ros::core::RosInterfaceSystem; +using drake_ros::core::RosSubscriberSystem; +using drake_ros::tf2::SceneTfBroadcasterSystem; +using drake_ros::viz::RvizVisualizer; using Eigen::Quaterniond; using Eigen::Vector3d; @@ -294,7 +294,7 @@ class RosPoseGlue : public LeafSystemd { const auto& goal_pose = input_port.Eval(context); - *output = drake_ros_core::RosPoseToRigidTransform(goal_pose.pose); + *output = drake_ros::core::RosPoseToRigidTransform(goal_pose.pose); } }; @@ -382,7 +382,7 @@ std::unique_ptr BuildSimulation() { } int main() { - drake_ros_core::init(); + drake_ros::core::init(); std::unique_ptr diagram = BuildSimulation(); std::unique_ptr diagram_context = diagram->CreateDefaultContext();