I have written a custom hardware interface for an 8 joint bipedal robot based on the tutorial given by Slate Robotics. I am running ROS kinetic on Ubuntu 16.04, without a rt_preempt kernel. I am using SOEM to communicate with the servo drives of my device.

In the init() of the interface, per joint a joint handle is made and registered who's command memory location is an index of a std::vector<double>. This vector therefore contains all the commands of the 8 joints.This is done as shown below:

JointHandle jointEffortHandle(jointStateHandle, &joint_effort_command_[i]);  
effort_joint_interface_.registerHandle(jointEffortHandle);

This joint handle is used by the controller in the controlmanager to know where it needs to put the command that it generates. This occurs in the update() function of the hardware interface. What i have been noticing is that the values of `jointeffortcommand[i]` changes between when the controller generates it when asked by the control_manager in the update() method and when it is written to the hardware in the write() method. This was done through a realtime publisher. A plot of the data can be seen below:

Figure 1: Difference between update() and write() value of jointeffortcommand_[1], left is controller output, right is value sent to servo drives before limits are applied. Note that the minimum value in the right image is so large that the shape of the left image can no longer easily be spotted.

Note that nothing is being added/changed in the hardware interface to that variable between these methods. While debugging it was found that if at the beginning of the write() function a copy was made of jointeffortcommand_ and that was used instead of the actual values, that the random value changes no longer occurred. This was implemented as shown below. This is not a clean solution, and I don't know why this works, making it potentially dangerous.

void MarchHardwareInterface::write(ros::Duration elapsed_time)
{
  after_limit_command_pub_->msg_.name.clear();
  after_limit_command_pub_->msg_.position_command.clear();
  after_limit_command_pub_->msg_.effort_command.clear();
  after_limit_command_pub_->msg_.before_limit.clear();
    after_limit_command_pub_->msg_.before_multiply.clear();
    after_limit_command_pub_->msg_.start_write.clear();
    after_limit_command_pub_->msg_.start_write = joint_effort_command_;

    joint_effort_command_copy.clear();
    joint_effort_command_copy.resize(joint_effort_command_.size());
    joint_effort_command_copy = joint_effort_command_;

  for (int i = 0; i < num_joints_; i++)
  {
    march4cpp::Joint singleJoint = marchRobot.getJoint(joint_names_[i]);
    if (singleJoint.canActuate())
    {

      after_limit_command_pub_->msg_.name.push_back(singleJoint.getName());

      joint_effort_command_[i] = joint_effort_command_copy[i];

      after_limit_command_pub_->msg_.before_limit.push_back(joint_effort_command_[i]);

      effortJointSoftLimitsInterface.enforceLimits(elapsed_time);
      positionJointSoftLimitsInterface.enforceLimits(elapsed_time);

      if (singleJoint.getActuationMode() == march4cpp::ActuationMode::position)
      {
        singleJoint.actuateRad(static_cast<float>(joint_position_command_[i]));
        after_limit_command_pub_->msg_.position_command.push_back(joint_position_command_[i]);
        after_limit_command_pub_->msg_.effort_command.push_back(0);
      }
      else if (singleJoint.getActuationMode() == march4cpp::ActuationMode::torque)
      {
        singleJoint.actuateTorque(static_cast<int>(joint_effort_command_[i]));
        after_limit_command_pub_->msg_.position_command.push_back(0);
        after_limit_command_pub_->msg_.effort_command.push_back(joint_effort_command_[i]);
      }
    }
    else ROS_INFO("this joint cannot actuate");
  } 

Considering that nothing is changed by the hardware interface to that memory location, I have a couple of questions:

• Are there other processes that run parallel to the hardware interface that interact with the joint handles, that can change the command of the joint handle?
• Could there be a timing misalignment, causing the command to be read while also being written to? How could I prevent this from happening?
• How much effect would a realtime kernel have on the hardware interface? Would it help mitigate this problem?

Below a thinned out version of the hardware interface can be found, with the components that I think are relevant for this case. The full code can be found here.

#include <joint_limits_interface/joint_limits.h>
#include <joint_limits_interface/joint_limits_interface.h>
#include <joint_limits_interface/joint_limits_urdf.h>

#include <sstream>

#include <march_hardware/ActuationMode.h>
#include <march_hardware/Joint.h>
#include <march_hardware/MarchRobot.h>
#include <march_shared_resources/AfterLimitJointCommand.h>

#include <march_hardware_interface/PowerNetOnOffCommand.h>
#include <march_hardware_interface/march_hardware_interface.h>

#include <urdf/model.h>

using joint_limits_interface::JointLimits;
using joint_limits_interface::EffortJointSoftLimitsHandle;
using joint_limits_interface::EffortJointSoftLimitsInterface;
using joint_limits_interface::SoftJointLimits;

namespace march_hardware_interface
{
MarchHardwareInterface::MarchHardwareInterface(ros::NodeHandle& nh, AllowedRobot robotName)
  : nh_(nh), marchRobot(HardwareBuilder(robotName).createMarchRobot())
{
  init();
  controller_manager_.reset(new controller_manager::ControllerManager(this, nh_));
  nh_.param("/march_hardware_interface/loop_hz", loop_hz_, 100.0);
  ros::Duration update_freq = ros::Duration(1.0 / loop_hz_);
  non_realtime_loop_ = nh_.createTimer(update_freq, &MarchHardwareInterface::update, this);
}

MarchHardwareInterface::~MarchHardwareInterface()
{
  this->marchRobot.stopEtherCAT();
}

void MarchHardwareInterface::init()
{
  // Initialize realtime publisher for the IMotionCube states

  after_limit_command_pub_ = RtPublisherAfterLimitCommandPtr(
      new realtime_tools::RealtimePublisher<march_shared_resources::AfterLimitJointCommand>(
          this->nh_, "/march/after_limit_command/", 4));

  // Start ethercat cycle in the hardware
  this->marchRobot.startEtherCAT();

  urdf::Model model;
  if (!model.initParam("/robot_description"))
  {
    ROS_ERROR("Failed to read the urdf from the parameter server.");
    throw std::runtime_error("Failed to read the urdf from the parameter server.");
  }

  // Get joint names from urdf
  for (auto const& urdfJoint : model.joints_)
  {
    if (urdfJoint.second->type != urdf::Joint::FIXED)
    {
      joint_names_.push_back(urdfJoint.first);
    }
  }

  nh_.setParam("/march/joint_names", joint_names_);

  num_joints_ = joint_names_.size();

  // Resize vectors
  joint_position_.resize(num_joints_);
  joint_velocity_.resize(num_joints_);
  joint_effort_.resize(num_joints_);
  joint_temperature_.resize(num_joints_);
  joint_temperature_variance_.resize(num_joints_);
  joint_position_command_.resize(num_joints_);
  joint_velocity_command_.resize(num_joints_);
  joint_effort_command_.resize(num_joints_);
  soft_limits_.resize(num_joints_);
  joint_limits_.resize(num_joints_);

  for (int i = 0; i < num_joints_; ++i)
  {
    SoftJointLimits soft_limits;
    getSoftJointLimits(model.getJoint(joint_names_[i]), soft_limits);
    ROS_INFO("%s soft_limits_ (%f, %f).", joint_names_[i].c_str(), soft_limits.min_position, soft_limits.max_position);
    soft_limits_[i] = soft_limits;
  }

  for (int i = 0; i < num_joints_; ++i)
  {
    JointLimits joint_limits;
    getJointLimits(model.getJoint(joint_names_[i]), joint_limits);
    ROS_INFO("max_velocity: %f, max_effort: %f.", joint_limits.max_velocity, joint_limits.max_effort);
    joint_limits_[i] = joint_limits;
  }

  registerInterface(&joint_state_interface_);
  registerInterface(&position_joint_interface_);
  registerInterface(&effort_joint_interface_);
  registerInterface(&positionJointSoftLimitsInterface);
  registerInterface(&effortJointSoftLimitsInterface);

  // Initialize interfaces for each joint
  for (int i = 0; i < num_joints_; ++i)
  {
    march4cpp::Joint joint = marchRobot.getJoint(joint_names_[i]);
    // Create joint state interface
    JointStateHandle jointStateHandle(joint.getName(), &joint_position_[i], &joint_velocity_[i], &joint_effort_[i]);
    joint_state_interface_.registerHandle(jointStateHandle);

    // Retrieve joint (soft) limits from the urdf
    JointLimits limits;
    getJointLimits(model.getJoint(joint.getName()), limits);

    // Dependently on which actuationMode is chosen, the state and limit handles must be selected
    if (marchRobot.getJoint(joint_names_[i]).getActuationMode() == march4cpp::ActuationMode::position)
    {
      // Create position joint interface
      JointHandle jointPositionHandle(jointStateHandle, &joint_position_command_[i]);
      position_joint_interface_.registerHandle(jointPositionHandle);

      //     Create joint limit interface
      PositionJointSoftLimitsHandle jointPositionLimitsHandle(jointPositionHandle, limits, soft_limits_[i]);
      positionJointSoftLimitsInterface.registerHandle(jointPositionLimitsHandle);
    }
    else if (marchRobot.getJoint(joint_names_[i]).getActuationMode() == march4cpp::ActuationMode::torque)
    {
      // Create effort joint interface
      JointHandle jointEffortHandle(jointStateHandle, &joint_effort_command_[i]);
      effort_joint_interface_.registerHandle(jointEffortHandle);

      // Create joint effort limit interface
      EffortJointSoftLimitsHandle jointEffortLimitsHandle(jointEffortHandle, limits, soft_limits_[i]);
      effortJointSoftLimitsInterface.registerHandle(jointEffortLimitsHandle);
    }

    // Set the first target as the current position
    this->read();
    joint_velocity_[i] = 0;
    joint_effort_[i] = 0;
    joint_position_command_[i] = joint_position_[i];

    // Create velocity joint interface
    JointHandle jointVelocityHandle(jointStateHandle, &joint_velocity_command_[i]);
    velocity_joint_interface_.registerHandle(jointVelocityHandle);
  }
}

void MarchHardwareInterface::update(const ros::TimerEvent& e)
{
  std::vector<double> before_write;
  before_write.resize(joint_effort_command_.size());

  elapsed_time_ = ros::Duration(e.current_real - e.last_real);
  after_limit_command_pub_->msg_.before_write.clear();

  read(elapsed_time_);

  controller_manager_->update(ros::Time::now(), elapsed_time_);

  before_write = joint_effort_command_;
  after_limit_command_pub_->msg_.before_write = before_write;

  write(elapsed_time_);
}

void MarchHardwareInterface::write(ros::Duration elapsed_time)
{
  after_limit_command_pub_->msg_.name.clear();
  after_limit_command_pub_->msg_.position_command.clear();
  after_limit_command_pub_->msg_.effort_command.clear();
  after_limit_command_pub_->msg_.before_limit.clear();
  after_limit_command_pub_->msg_.start_write.clear();

  after_limit_command_pub_->msg_.start_write = joint_effort_command_;

  for (int i = 0; i < num_joints_; i++)
  {
    march4cpp::Joint singleJoint = marchRobot.getJoint(joint_names_[i]);
    if (singleJoint.canActuate())
    {
      after_limit_command_pub_->msg_.name.push_back(singleJoint.getName());

      after_limit_command_pub_->msg_.before_limit.push_back(joint_effort_command_[i]);

      effortJointSoftLimitsInterface.enforceLimits(elapsed_time);
      positionJointSoftLimitsInterface.enforceLimits(elapsed_time);

      if (singleJoint.getActuationMode() == march4cpp::ActuationMode::position)
  {
    singleJoint.actuateRad(static_cast<float>(joint_position_command_[i]));
    after_limit_command_pub_->msg_.position_command.push_back(joint_position_command_[i]);
    after_limit_command_pub_->msg_.effort_command.push_back(0);
  }
  else if (singleJoint.getActuationMode() == march4cpp::ActuationMode::torque)
  {
    singleJoint.actuateTorque(static_cast<int>(joint_effort_command_[i]));
    after_limit_command_pub_->msg_.position_command.push_back(0);
    after_limit_command_pub_->msg_.effort_command.push_back(joint_effort_command_[i]);
  }
    }
  }
  else ROS_INFO("this joint cannot actuate");
}

if (after_limit_command_pub_->trylock())
{
  after_limit_command_pub_->unlockAndPublish();
}
}  // namespace march_hardware_interface