Skip to content

Commit

Permalink
Use sdf files for models
Browse files Browse the repository at this point in the history
  • Loading branch information
@sloretz
sloretz committed Nov 9, 2022
1 parent d2c163f commit 728ef17
Show file tree
Hide file tree
Showing 4 changed files with 133 additions and 91 deletions.
9 changes: 9 additions & 0 deletions drake_ros_examples/examples/ufo/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
@@ -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)
Expand All @@ -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/
)
25 changes: 25 additions & 0 deletions drake_ros_examples/examples/ufo/models/ground.sdf
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,25 @@
<?xml version="1.0" ?>
<sdf version="1.9">
<model name="ground">
<static>true</static>
<link name="ground">
<visual name="visual">
<geometry>
<plane>
<size>50 50</size>
</plane>
</geometry>
<material>
<diffuse>0.0 0.5 0.0 1.0</diffuse>
</material>
</visual>
<collision name="collision">
<geometry>
<plane>
<size>50 50</size>
</plane>
</geometry>
</collision>
</link>
</model>
</sdf>
79 changes: 79 additions & 0 deletions drake_ros_examples/examples/ufo/models/ufo.sdf
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,79 @@
<?xml version="1.0" ?>
<sdf version="1.9">
<model name="flying_saucer">
<!-- A great place to put a tractor beam -->
<frame name="bottom_center"/>

<!-- Ellipsoid body of the craft -->
<link name="spacecraft">
<pose relative_to="bottom_center">0 0 0.5 0 0 0</pose>
<visual name="visual">
<geometry>
<ellipsoid>
<radii>2.5 2.5 0.5</radii>
</ellipsoid>
</geometry>
<material>
<diffuse>0.5 0.5 0.5 1.0</diffuse>
</material>
</visual>
<collision name="collision">
<geometry>
<ellipsoid>
<radii>2.5 2.5 0.5</radii>
</ellipsoid>
</geometry>
</collision>
<inertial>
<mass>1000</mass>
<inertia>
<ixx>1300</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>1300</iyy>
<iyz>0</iyz>
<izz>2500</izz>
</inertia>
</inertial>
</link>

<!-- Spherical translucent shell for pilot to peer out of -->
<link name="lookout">
<pose relative_to="spacecraft">0 0 0.49 0 0 0</pose>
<visual name="visual">
<geometry>
<sphere>
<radius>0.98</radius>
</sphere>
</geometry>
<material>
<diffuse>1.0 0.55 0.0 0.35</diffuse>
</material>
</visual>
<collision name="collision">
<geometry>
<sphere>
<radius>0.98</radius>
</sphere>
</geometry>
</collision>
<inertial>
<mass>100</mass>
<inertia>
<ixx>65.34</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>65.34</iyy>
<iyz>0</iyz>
<izz>65.34</izz>
</inertia>
</inertial>
</link>

<!-- Combine links -->
<joint name="weld" type="fixed">
<parent>spacecraft</parent>
<child>lookout</child>
</joint>
</model>
</sdf>
111 changes: 20 additions & 91 deletions drake_ros_examples/examples/ufo/ufo.cc
View file
Original file line number Diff line number Diff line change
@@ -1,6 +1,9 @@
#include <filesystem>
#include <vector>

#include <ament_index_cpp/get_package_share_directory.hpp>
#include <drake/geometry/proximity_properties.h>
#include <drake/multibody/parsing/parser.h>
#include <drake/multibody/plant/externally_applied_spatial_force.h>
#include <drake/multibody/plant/multibody_plant.h>
#include <drake/systems/analysis/simulator.h>
Expand All @@ -18,28 +21,19 @@
#include <drake_ros_viz/rviz_visualizer.h>
#include <geometry_msgs/msg/pose_stamped.hpp>

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<double>;
using ConstantVectorSourced = drake::systems::ConstantVectorSource<double>;
using Contextd = drake::systems::Context<double>;
using CoulombFrictiond = drake::multibody::CoulombFriction<double>;
using Diagramd = drake::systems::Diagram<double>;
using DiagramBuilderd = drake::systems::DiagramBuilder<double>;
using ExternallyAppliedSpatialForced =
Expand All @@ -48,99 +42,34 @@ using LeafSystemd = drake::systems::LeafSystem<double>;
using MultibodyPlantd = drake::multibody::MultibodyPlant<double>;
using Multiplexerd = drake::systems::Multiplexer<double>;
using PidControllerd = drake::systems::controllers::PidController<double>;
using RigidBodyd = drake::multibody::RigidBody<double>;
using RigidTransformd = drake::math::RigidTransform<double>;
using RollPitchYawd = drake::math::RollPitchYaw<double>;
using RotationMatrixd = drake::math::RotationMatrix<double>;
using Simulatord = drake::systems::Simulator<double>;
using SpatialForced = drake::multibody::SpatialForce<double>;
using SpatialInertiad = drake::multibody::SpatialInertia<double>;
using StateInterpolatorWithDiscreteDerivatived =
drake::systems::StateInterpolatorWithDiscreteDerivative<double>;
using Systemd = drake::systems::System<double>;
using UnitInertiad = drake::multibody::UnitInertia<double>;

/// 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 {
Expand Down Expand Up @@ -416,7 +345,7 @@ std::unique_ptr<Diagramd> 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<BodyPoseAtIndex>(ufo_index);
builder.Connect(
plant.get_body_poses_output_port(),
Expand Down Expand Up @@ -472,7 +401,7 @@ std::unique_ptr<Contextd> 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;
}
Expand Down

0 comments on commit 728ef17

Please sign in to comment.