Jerky and slow motion using moveit_servo

[ILoveCorn]

Hi there, I am working on a project that requires Cobotta to follow end-effector velocity command. As there is no twist controller implemented in denso_cobotta_ros, I use moveit_servo to first convert end-effector velocity command into joint position command and then use position_controllers/JointGroupPositionControllers from ros_control to move the robot.

However, the robot's motion is very jerky and much slower than the given command, despite the given velocity is very small (~1cm/s).

I then tested the same framework (moveit_servo+position_controllers/JointGroupPositionControllers) on a UR3 robot with a communication frequency of ~125Hz and it worked perfectly.

So I checked the code of denso_cobotta_control and found something weird:

• In denso_cobotta_control.cpp the main loop frequency is set by rate(1.0 / cobotta_common::COMMAND_CYCLE), i.e. 250Hz since COMMAND_CYCLE is defined as 0.004 in cobotta_common.h.
• According to Code API of controller_manager, controller_manager should be updated with the period of the main loop. But cm.update(now, dt); is used with dt=0.008.

  controller_manager::ControllerManager cm(&cobotta, nh);
  ros::Rate rate(1.0 / cobotta_common::COMMAND_CYCLE);
  ros::AsyncSpinner spinner(1);
  spinner.start();

  ros::Time start = ros::Time::now();
  ros::Duration dt = cobotta_common::getPeriod();
  while (ros::ok())
  {
    ros::Time now = ros::Time::now();

    success = cobotta.read(now, dt);
    if (!success)
    {
      break;
    }

    if (cobotta.shouldReset())
    {
      cm.update(now, ros::Duration(0), true);
      continue;
    }

    cm.update(now, dt);
    if (cobotta.isMotorOn())
    {
      success = cobotta.write(now, dt);
      if (!success)
        break;
    }
    rate.sleep();
  }

Although the above code looks weird, I guess the jerky motion is actually caused by the sleep defined in denso_cobotta_hw.cpp. I tried to adjust cobotta_common::COMMAND_SHORT_BREAK but the motion was still jerky and the robot would display "Servo command buffer full" and shut off the motors if the parameter was too low.

Any help would be appreciated.

bool DensoCobottaHW::sendServoUpdateData()
{
  SRV_COMM_SEND send_data;
  send_data.arm_no = 0;
  send_data.discontinuous = 0;
  send_data.disable_cur_lim = 0;
  send_data.stay_here = 0;

  for (int i = 0; i < CONTROL_JOINT_MAX; i++)
  {
    send_data.position[i] = ARM_COEFF_OUTPOS_TO_PULSE[i] * cmd_[i] - pulse_offset_[i];
    send_data.current_limit[i] = 0x0FFF;
    send_data.current_offset[i] = 0;
  }

  try
  {
    struct DriverCommandInfo info = Driver::writeHwUpdate(fd_, send_data);
    // ROS_DEBUG("result:%08X queue:%d stay_here:%d", info.result, info.queue_num, info.stay_here);
    if (info.result == 0x0F408101)
    {
      // The current number of commands in buffer is 11.
      // To avoid buffer overflow, sleep short time
      ros::Duration(cobotta_common::COMMAND_SHORT_BREAK).sleep();
    }
    else if (info.result == 0x84400502)
    {
      // buffer full
      // To avoid buffer overflow, sleep long time
      ros::Duration(cobotta_common::COMMAND_LONG_BREAK).sleep();
    }
  }
  catch (const std::exception& e)
  {
    ROS_ERROR_STREAM(e.what());
    return false;
  }
  return true;
}