Joint Handle Command unintentional change in Hardware Interface (HI) between Controller update() and HI write()

edit

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 control_manager 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 joint_effort_command_[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 joint_effort_command_[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 joint_effort_command_ 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 ...