From 728ef17957f36d209190eaf3fc4cd36c5d728195 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 9 Nov 2022 12:10:16 -0800 Subject: [PATCH] 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; }