Pre-ramble

I have been wanting to give drake a try for a while now, and after running into some issues with Gazebo, I decided it was a good chance to finally get around to trying Drake. There were some hurdles along the way, but truthfully it was less painful than I was expecting. The end result is drake as a stand-in replacement for Gazebo, publishing to /joint_states,/imu, and subscribing to /cmd_vel for joint commands. This has allowed me to retain usage of the rest of my stack, simply pulling out Gazebo, and replacing it with Drake. At a high level, the software stack can be viewed as:

Some thoughts on Drake

In a few of the resources I’ve seen providing an introduction to Drake, they’ll describe it as “Kind of like Simulink”. I’ve never liked this as a description, mostly as it requires one to have used Simulink, but moreso those who have scratched the surface of Simulink likely won’t understand what is implied by this description. I’d like to try and break down what this actually means.

Drake follows a Diagram based modelling paradigm. The defining feature of this is that you define your components as separated blocks, with the flow of information represented by inputs and outputs. If you’d like you implement a block into your diagram, you do so by defining connections for the inputs, and optionally for the output if you need it. Below is a generated diagram of KIWI’s architecture in Drake, in it you can see the flow of information between the different nodes in the form of inputs and outputs.

I feel the nuance of this paradigm isn’t revealed until we look at the implementation of a node. In c++, we utilise class inheritance to define the structure of our node. A look at a minimal LeafNode:

template <typename T>
class ExampleLeafNode : public drake::systems::LeafSystem<T> {
public:
  ExampleLeafNode() {
    this->DeclareVectorOutputPort("example_port",
                                  drake::systems::BasicVector<T>(2),
                                  &ArrayInputSystem::CalcOutput);
  }

private:
  void CalcOutput(const drake::systems::Context<double> & /*context*/,
                  drake::systems::BasicVector<T> *output) const {
    output->set_value(Eigen::Vector2d{1.0, -1.0});
  }
};

This is about as basic as you can get. We in our constructor we declare an output port, which simply returns a vector of {1.0, -1.0}. The two things to note:

1. The class is templated: Drake will infer the appropriate type based on the connected inputs/outputs.
2. The CalcOutput() receives a drake::systems::Context, which is the mechanism by which one can read the inputs. A look at the JointStatePublisherConverter posted below demonstrates how one can read this input. To instantiate and attach this node:

auto example_node = builder.AddSystem<ExampleLeafNode>();
builder.Connect(example_node->get_output_port(), other_node->get_input_port());

This may seem somewhat obtuse for someone background in purely software, where you may be looking to directly call a components ‘getter’ function yourself to acquire the output. For the control theorist, however, this is the form of an old friend. The diagram architecture is wonderful for visualising the flow of information. When we want to add a component in Drake, we first think about what we want as an input, and what we expect as an output of the block. This requires a slight change in thinking, but it feels relatively low friction once you get the mental model formed.

The choice to employ this type of architecture follows naturally from the “mission statement” of Drake, which is to expose the governing structure of the dynamics that is being simulated. Instead of being faced with a “black box” in the simulation, the user can access the internals of these dynamics and define algorithms to manipulate them.

Drake_ROS

The migration to Drake is almost entirely enabled by the work done by TRI on the drake_ros package, which provides a number of faces to the ROS ecosystem. This includes things like publishers, subscribers and a TF tree publisher, among other things. The examples are unfortuantely bare-bones, and I found myself doing a bit of leg-work to get a basic ‘hello-world’ going with KIWI. Though, I am happy to report that I had no issues with the installation of Drake in my container, nor with the linking of drake_ros as a dependency. Sounds like a silly statement, but I consider it to be high praise.

1: Installing Drake to our Docker Container.

Standard Ubuntu procedure of adding the repositories to our apt packages, and installing. Then we’re adding drake_ros to our deps folder, which will be linked by Colcon when we compile.

# Drake
RUN wget -qO- https://drake-apt.csail.mit.edu/drake.asc | gpg --dearmor - \
  | tee /etc/apt/trusted.gpg.d/drake.gpg >/dev/null

RUN echo "deb [arch=amd64] https://drake-apt.csail.mit.edu/$(lsb_release -cs) $(lsb_release -cs) main" \
  | tee /etc/apt/sources.list.d/drake.list >/dev/null

RUN apt update && apt install -qqy drake-dev

# Drake Ros
RUN mkdir -p /home/kiwi/kiwi_deps/src \
    && cd /home/kiwi/kiwi_deps/src \
    && git clone https://github.com/RobotLocomotion/drake-ros.git

USER root

RUN cd /home/kiwi/kiwi_deps/src && rosdep install --from-paths ./ -i -y --rosdistro humble

2: An overview of our Drake Node

Now we have Drake installed, we now turn to our kiwi_drake.cpp. This node is our one-stop shop for launching our simulation, handling the instantiation of Drake, Meshcat, and the “hardware interface” of KIWI, which will subscribe to /cmd_vel, and publish to our sensor topics. You can view the full file here

3: Instantiate the meshcat visualiser

Drake supports using the Meshcat visualiser to render the simulated system. Thus far I am impressed with its render quality and performance, further, the workflow of simply having an always-open browser window that automatically updates and connects on restart feels like a good move. I’ve not ran into any issues with it yet, although I’m sure they will emerge as I continue adding new things.

drake::geometry::MeshcatVisualizerParams params;
params.publish_period = 1.0 / 60.0; // 60hz update rate
auto meshcat = std::make_shared<drake::geometry::Meshcat>();
drake::geometry::MeshcatVisualizer<double>::AddToBuilder(
    &builder, scene_graph, meshcat, std::move(params));

4: Loading our URDF

Loading the model is thankfully trivial: The parser reads the URDF as a string, after which we then set our initial pose as the RigidTransform X_WB. Slight aside: The naming of X_WB is an example of drakes monogram notation

auto model_file = exec("xacro /home/kiwi/kiwi_ws/src/kiwi_gazebo/urdf/kiwi.xacro.urdf");
auto model_instance = parser.AddModelsFromString(model_file, "urdf")[0];

// Setting initial pose.
auto X_WB = drake::math::RigidTransform(drake::Vector3<double>{
    static_cast<double>(0.), static_cast<double>(0.), 1.3});
plant.SetDefaultFreeBodyPose(plant.GetBodyByName("base_link", model_instance),
                              X_WB);

There is one potential gotcha, which is that Drake expects there to be a transmission interface for the joints you would like to control. These can be simply added like so:

<transmission name="left_wheel_transmission">
  <type>transmission_interface/SimpleTransmission</type>
  <joint name="left_wheel_joint">
    <hardwareInterface>EffortJointInterface</hardwareInterface>
  </joint>
  <actuator name="left_motor">
    <mechanicalReduction>1</mechanicalReduction>
  </actuator>
</transmission>

And with that, you should have your robot model spawning.

5: Subscribing to /cmd_vel

A lot of code for a very simple idea. We subscribe to /cmd_vel, with the callback storing the recieved command in the member variable ‘cmd_vel_’. We also create a leaf node, which doesn’t use any inputs, but outputs directly to the actuators of KIWI. Every timestep, the leaf node reads ‘cmd_vel_’, clamps it, and passes the command directly to the actuators.

// Leaf node to apply the torque specified in cmd_vel_
class ArrayInputSystem : public drake::systems::LeafSystem<double> {
public:
  ArrayInputSystem(const std::array<double, 2> &cmd_vel) : cmd_vel_(cmd_vel) {
    this->DeclareVectorOutputPort("torque",
                                  drake::systems::BasicVector<double>(2),
                                  &ArrayInputSystem::CalcOutput);
  }

private:
  void CalcOutput(const drake::systems::Context<double> & /*context*/,
                  drake::systems::BasicVector<double> *output) const {
    output->set_value(Eigen::Vector2d{std::clamp(cmd_vel_[0], -10.0, 10.0),
                                      std::clamp(cmd_vel_[1], -10.0, 10.0)});
  }

  const std::array<double, 2> &cmd_vel_;
};
// Subscriber to cmd_vel. Populates cmd_vel_
auto cmd_vel_sub =
    direct_ros_node->create_subscription<std_msgs::msg::Float64MultiArray>(
        "/effort_controllers/commands", 10,
        [&](const std_msgs::msg::Float64MultiArray::SharedPtr msg) {
          auto t_arr = std::array{msg->data[0], msg->data[1]};
          std::swap(t_arr, cmd_vel_);
        });
// Instantiate the Array input system
auto array_input = builder.AddSystem<ArrayInputSystem>(cmd_vel_);
// Connect the array input to KIWI's actuation input
builder.Connect(array_input->get_output_port(),
                plant.get_actuation_input_port(model_instance));

6: Publishing to /imu

I didn’t write this, I copied it from here. Admittedly, this one took a lot more work than the others, and I am a little surprised that there isn’t a pre-built module for this (So I stole someone else’s implementation). The gist of it is the IMU topic gets published by an orientation sensor node. Among other things, this node reads the motions of the torso, and publishes them to the /imu/data topic. An additional requirement is to transform the data from a BasicVector (The output of the orientation sensor) to an Abstract data type, which is used by the ROS publisher leaf node. I opted to keep this leaf separate from the IMU class for no real reason other than it feeling like better practice to keep the module itself “ROS agnostic” and attach a conversion leaf node to its output. The interesting parts:

// Transforming from  body orientation to quaternion
template <typename T>
void OrientationSensor<T>::CalcOutput(const Context<T> &context,
                                      BasicVector<T> *output) const{
    const auto &X_WB =
        get_body_poses_input_port().template Eval<std::vector<RigidTransform<T>>>(
            context)[body_index_];

    const auto R_SB = X_BS_.rotation().matrix().template cast<T>().transpose();
    const auto R_BW = X_WB.rotation().matrix().transpose();
    const auto R_SW = R_SB * R_BW;

    output->SetFromVector(
        RotationMatrix<T>(R_SW.transpose()).ToQuaternionAsVector4());
}

// Quaternion to ros message type
template <typename T>
class RosQuaternionConverter : public drake::systems::LeafSystem<T> {
public:
  RosQuaternionConverter() {
    this->DeclareVectorInputPort("drake_quat",
                                 drake::systems::BasicVector<T>(4));

    this->DeclareAbstractOutputPort("ros_quat",
                                    &RosQuaternionConverter::CalcOutput);
  }

private:
  void CalcOutput(const drake::systems::Context<double> &context,
                  sensor_msgs::msg::Imu *output) const {
    const auto current_rot = this->get_input_port(input_port_).Eval(context);
    output->orientation.w = current_rot[0];
    output->orientation.x = current_rot[1];
    output->orientation.y = current_rot[2];
    output->orientation.z = current_rot[3];
  }
  int input_port_ = 0;
};

7: Publishing to /joint_states

Very similar to the to the cmd\_vel, just the other way around. I construct a conversion from Drake’s context vector, using it to populate a sensor\_msgs/JointState message. The output from this is simply passed to a ROS publisher.

template <typename T>
class JointStatePublisherConverter : public drake::systems::LeafSystem<T> {
public:
  JointStatePublisherConverter(const drake::multibody::MultibodyPlant<T>& plant,
                               const std::vector<std::string>& joint_names)
      : plant_(plant), joint_names_(joint_names) {
    this->DeclareVectorInputPort("plant_state",
                                 plant.num_multibody_states());
    this->DeclareAbstractOutputPort("joint_states",
                                    &JointStatePublisherConverter::CalcOutput);

    auto num_positions = plant.GetPositionNames().size();
    // Store the position and velocity start indices for each joint
    for (const auto& name : joint_names) {
      const auto& joint = plant_.GetJointByName(name);
      joint_indices_.push_back({joint.position_start(), joint.velocity_start()+num_positions});
    }
  }

private:
  void CalcOutput(const drake::systems::Context<T>& context,
                  sensor_msgs::msg::JointState* output) const {
    const Eigen::VectorXd current_state = this->get_input_port().Eval(context);

    output->name = joint_names_;
    output->position.resize(joint_names_.size());
    output->velocity.resize(joint_names_.size());
    output->effort.resize(joint_names_.size(), 0.0);

    for (size_t i = 0; i < joint_names_.size(); ++i) {
      output->position[i] = current_state[joint_indices_[i].first];
      output->velocity[i] = current_state[joint_indices_[i].second];
    }
  }

  const drake::multibody::MultibodyPlant<T>& plant_;
  std::vector<std::string> joint_names_;
  std::vector<std::pair<int, int>> joint_indices_;  // Pairs of (position_index, velocity_index)
};

std::vector<std::string> target_joints = {"left_wheel_joint", "right_wheel_joint"};
auto js_translator = builder.AddSystem<JointStatePublisherConverter<double>>(plant, target_joints);
builder.Connect(plant.get_state_output_port(), js_translator->get_input_port());

Results

To the surprise of no-one, I did end up having to do some slight tuning of my controllers to re-achieve the balancing I had before the migration. I didn’t spend time getting things as clean as they were before, but I only really had to tweak the low-pass filters, due to Drake publishing the sensor data at a much faster rate than was being published with Gazebo.

Thus far, I am extremely impressed with Drake as a piece of software. It’s easy to interface with, and performs very well. It’s too early to say whether I’ll be emphatically recommending it, we will need to wait and see how well handles both scaling of software complexity, and the addition/integration of sensors.

A useful code snippet

While debugging, I found it Drake’s diagram output to GraphViz to be extremely useful. This generates a visual diagram of the current architecture, with the inputs and outputs of each block. The output of this is the diagram shown earlier in this post, where you can see the graph of all the entities in the simulation. This is just a very handy snippet to keep at the ready.

std::ofstream dot_file("diagram.dot", std::ofstream::out);
dot_file << diagram->GetGraphvizString();
dot_file.close();

Then, in bash.

$dot -Tpng diagram.dot > output.png