denso_cobotta_ros実装調査

cobotta_common.h

# denso_cobotta_ros\denso_cobotta_lib\include\denso_cobotta_lib
namespace cobotta_common
{
static constexpr const char* PATH_DEVFILE = "/dev/denso_cobotta";
static constexpr const char* TEMP_PARAMS_PATH = "/tmp/cobotta_parameters.yaml";
static constexpr uint32_t CONTROL_JOINT_MAX = 6;
static constexpr double SERVO_PERIOD = 0.008;         //[s]
static constexpr double DRIVER_UPDATE_PERIOD = 0.01;  //[s]
static constexpr double COMMAND_CYCLE = 0.004;                  /** Update command rate: 4ms (less than 8ms) */
static constexpr double COMMAND_SHORT_BREAK = SERVO_PERIOD * 2; /** Sleep short time to avoid command overflow */
static constexpr double COMMAND_LONG_BREAK = SERVO_PERIOD * 8;  /** Sleep long time to avoid command overflow */
static ros::Duration getPeriod()
  return ros::Duration(SERVO_PERIOD);

denso_cobotta_control.cpp

# denso_cobotta_ros\denso_cobotta_control\src
int main(int argc, char** argv)
  denso_cobotta_control::DensoCobottaHW cobotta;
  bool success = cobotta.initialize(nh);

denso_cobotta_hw.cpp

# denso_cobotta_ros\denso_cobotta_control\src
bool DensoCobottaHW::initialize(ros::NodeHandle& nh)
  fd_ = open(PATH_DEVFILE, O_RDWR);

bool DensoCobottaHW::sendServoUpdateData()
  SRV_COMM_SEND send_data;
    struct DriverCommandInfo info = Driver::writeHwUpdate(fd_, send_data);

bool DensoCobottaHW::recvEncoderData()
  SRV_COMM_RECV recv_data{ 0 };
    recv_data = Driver::readHwEncoder(fd_, 0);

cobotta_ioctl.h

# denso_cobotta_ros\denso_cobotta_lib\include\denso_cobotta_lib
typedef struct SRV_COMM_SEND {
	long arm_no;
	uint32_t discontinuous;
	uint32_t disable_cur_lim;
	uint32_t stay_here;
	long position[JOINT_MAX];
	uint16_t current_limit[JOINT_MAX];
	int16_t current_offset[JOINT_MAX];
} SRV_COMM_SEND;
typedef struct SRV_COMM_SEND_RECV {
	long result;
	uint32_t buff_state;
} SRV_COMM_SEND_RECV;
typedef struct IOCTL_DATA_UPDATE {
	SRV_COMM_SEND send;
	SRV_COMM_SEND_RECV recv;
} IOCTL_DATA_UPDATE;
typedef struct SRV_COMM_RECV {
	long result;
	uint32_t time_stamp;
	long encoder[JOINT_MAX];
	uint16_t current_q[JOINT_MAX];
	uint16_t current_u[JOINT_MAX];
	uint16_t current_v[JOINT_MAX];
} SRV_COMM_RECV;
typedef struct IOCTL_DATA_GETENC {
	long arm_no;
	SRV_COMM_RECV recv;
} IOCTL_DATA_GETENC;

driver.cpp

# denso_cobotta_ros\denso_cobotta_lib\src
int Driver::openDeviceFile() noexcept(false)
  fd = open(cobotta_common::PATH_DEVFILE, O_RDWR);

struct DriverCommandInfo Driver::writeHwUpdate(int fd, const SRV_COMM_SEND& send_data) noexcept(false)
  IOCTL_DATA_UPDATE dat{ 0 };
  dat.send = send_data;
  int ret = ioctl(fd, COBOTTA_IOCTL_SRV_UPDATE, &dat);

SRV_COMM_RECV Driver::readHwEncoder(int fd, long arm_no) noexcept(false)
  IOCTL_DATA_GETENC dat{ 0 };
  int ret = ioctl(fd, COBOTTA_IOCTL_SRV_GETENC, &dat);

struct DriverCommandInfo Driver::writeHwUpdate(int fd, const SRV_COMM_SEND& send_data) noexcept(false)
      info.stay_here = dat.recv.buff_state && 0x00010000; // これバグ？？？

cobotta.cpp

# denso_cobotta_ros\denso_cobotta_lib\src
const std::unique_ptr<Driver>& Cobotta::getDriver() const
  return driver_;

void Cobotta::open() noexcept(false)
    fd_ = getDriver()->openDeviceFile();

denso_cobotta_driver.cpp

# denso_cobotta_ros\denso_cobotta_driver\src
bool DensoCobottaDriver::recvPulse(long arm_no, std::array<int32_t, JOINT_MAX>& pulse)
  SRV_COMM_RECV recv_data{ 0 };
    recv_data = Driver::readHwEncoder(cobotta_->getFd(), arm_no);

bool DensoCobottaDriver::sendServoUpdateData(const SRV_COMM_SEND& send_data)
    struct DriverCommandInfo info = Driver::writeHwUpdate(cobotta_->getFd(), send_data);
    if (info.result == 0x0F408101)
    {
      // The current number of commands in buffer is 11.
      // To avoid buffer overflow, sleep 8 msec
      ros::Duration(cobotta_common::getPeriod()).sleep();
    }

bool DensoCobottaDriver::sineMove(const MoveParam& move_param)
{
  std::array<int32_t, JOINT_MAX> cur_pulse;             // [pulse]
  std::array<double, CONTROL_JOINT_MAX> start_pos_rad;  // [rad]
  std::array<double, CONTROL_JOINT_MAX> rotation_rad;   // [rad]
  std::array<double, CONTROL_JOINT_MAX> velocity;       // [rad/s]
  int32_t max_count;

  auto is_motor_runnning = [&]() {
    bool is_running = cobotta_->getMotor()->isRunning();
    if (!is_running)
    {
      ROS_WARN("Motor is not running.");
    }
    return is_running;
  };

  // Get current position
  if (is_motor_runnning() && recvPulse(0, cur_pulse))
  {
    for (int i = 0; i < CONTROL_JOINT_MAX; i++)
    {
      start_pos_rad[i] = cur_pulse[i] / ARM_COEFF_OUTPOS_TO_PULSE[i];
    }
  }
  else
  {
    return false;
  }
  // Caluculate rotation angle.
  for (int i = 0; i < CONTROL_JOINT_MAX; i++)
  {
    rotation_rad[i] = move_param.target_position[i] - start_pos_rad[i];
  }
  if (!calculateVelocity(move_param, rotation_rad, velocity, max_count))
  {
    return false;
  }

  // Prepare to move.
  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.current_limit[i] = move_param.current_limit[i];
    send_data.current_offset[i] = move_param.current_offset[i];
  }

  const double move_time = max_count * SERVO_PERIOD;
  for (int count = 0; count <= max_count; count++)
  {
    for (int i = 0; i < CONTROL_JOINT_MAX; i++)
    {
      // Calculate position.
      send_data.position[i] =
          ARM_COEFF_OUTPOS_TO_PULSE[i] *
          (start_pos_rad[i] + (velocity[i] * move_time / M_PI) * (1 - cos(M_PI * count / (double)max_count)));
    }

    if (count == max_count)
    {
      send_data.stay_here = 1;
    }
    if (!(is_motor_runnning() && sendServoUpdateData(send_data)))
    {
      return false;
    }
  }
  return true;
}