motor_base.hpp#

base class of the dynamixel motor More...
Namespaces#

Name
dynamixel_hardware_interface
Classes#

	Name
struct	dynamixel_hardware_interface::Result Struct describes the command result.
class	dynamixel_hardware_interface::MotorBase Base class for controlling dynamixel motor.
Detailed Description#

base class of the dynamixel motor
Author: Masaya Kataoka (ms.kataoka@gmail.com)
Version: 0.1
Date: 2021-05-01
Source code#

// Copyright (c) 2021 OUXT Polaris
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef DYNAMIXEL_HARDWARE_INTERFACE__MOTOR_BASE_HPP_
#define DYNAMIXEL_HARDWARE_INTERFACE__MOTOR_BASE_HPP_

#include <dynamixel_sdk/dynamixel_sdk.h>

#include <dynamixel_hardware_interface/address_table_base.hpp>
#include <dynamixel_hardware_interface/constants.hpp>
#if defined(GALACTIC) || defined(HUMBLE)
#include <hardware_interface/system_interface.hpp>
#else
#include <hardware_interface/base_interface.hpp>
#endif
#include <hardware_interface/handle.hpp>
#include <hardware_interface/hardware_info.hpp>
#include <hardware_interface/system_interface.hpp>
#include <hardware_interface/types/hardware_interface_return_values.hpp>
#if defined(GALACTIC) || defined(HUMBLE)
#include <hardware_interface/types/hardware_interface_type_values.hpp>
#else
#include <hardware_interface/types/hardware_interface_status_values.hpp>
#endif
#include <limits>
#include <memory>
#include <rclcpp/rclcpp.hpp>
#include <string>
#include <vector>

namespace dynamixel_hardware_interface
{
struct Result
{
  const std::string description;
  const bool success;
  Result(const std::string & description, bool success) : description(description), success(success)
  {
  }
};

class MotorBase
{
public:
  const SupportedMotors motor_type;
  const std::string joint_name;
  const bool enable_dummy;
  const int baudrate;
  const uint8_t id;

  const double max_joint_limit;
  const double min_joint_limit;
  template <typename AddressTable>
  MotorBase(
    const SupportedMotors & motor_type, const std::string & joint_name, const bool enable_dummy,
    const AddressTable & table, int baudrate, uint8_t id, double max_joint_limit,
    double min_joint_limit, std::shared_ptr<dynamixel::PortHandler> port_handler,
    std::shared_ptr<dynamixel::PacketHandler> packet_handler)
  : motor_type(motor_type),
    joint_name(joint_name),
    enable_dummy(enable_dummy),
    baudrate(baudrate),
    id(id),
    max_joint_limit(max_joint_limit),
    min_joint_limit(min_joint_limit),
    port_handler_(port_handler),
    packet_handler_(packet_handler),
    joint_position_(std::numeric_limits<double>::quiet_NaN()),
    goal_position_(std::numeric_limits<double>::quiet_NaN())
  {
    RCLCPP_INFO_STREAM(
      rclcpp::get_logger("dynamixel_hardware_interface"), "start constructing motor instance");
    RCLCPP_INFO_STREAM(
      rclcpp::get_logger("dynamixel_hardware_interface"), "joint_name : " << joint_name);
    address_table_ = std::make_shared<AddressTableBase>(table);
    RCLCPP_INFO_STREAM(
      rclcpp::get_logger("dynamixel_hardware_interface"), "end constructing motor instance");
  }
  ~MotorBase();
  bool operationSupports(const Operation & operation);
  virtual std::vector<Operation> getSupportedOperations();
  virtual Result configure();
  virtual Result torqueEnable(bool enable);
  virtual Result setGoalPosition(double goal_position);
  virtual Result setCurrentGoalPosition() { return setGoalPosition(goal_position_); }
  virtual double getJointPosition() const { return joint_position_; }
  virtual double getGoalPosition() const { return goal_position_; }
  virtual Result updateJointPosition();
  virtual Result updateJointVelocity();
  virtual Result updatePresentTemperature();
  virtual void appendStateInterfaces(std::vector<hardware_interface::StateInterface> & interfaces);
  virtual void appendCommandInterfaces(
    std::vector<hardware_interface::CommandInterface> & interfaces);

  virtual Result setJointPositionLimit(double max_joint_limit, double min_joint_limit);

protected:
  MotorBase() = delete;
  Result getResult(int communication_result, uint8_t packet_error);
  template <typename T>
  T radianToPosition(double radian) const
  {
    T value;
    radianToPosition(radian, value);
    return value;
  }
  uint16_t radianToPosition(double radian) const;
  virtual double positionToRadian(const uint8_t position) const;
  virtual double positionToRadian(const uint16_t position) const;
  virtual double positionToRadian(const uint32_t position) const;
  virtual void radianToPosition(double radian, uint8_t & value) const;
  virtual void radianToPosition(double radian, uint16_t & value) const;
  virtual void radianToPosition(double radian, uint32_t & value) const;
  void rpmToVelocity(double rpm, double & radian) const { radian = rpm / 60.0 * 2 * M_PI; }
  virtual double valueToRpm(uint8_t value) const;
  virtual double valueToRpm(uint16_t value) const;
  virtual double valueToRpm(uint32_t value) const;
  virtual double valueToTemperature(uint8_t value) const;
  virtual double valueToTemperature(uint16_t value) const;
  virtual double valueToTemperature(uint32_t value) const;

  std::shared_ptr<AddressTableBase> address_table_;
  std::shared_ptr<dynamixel::PortHandler> port_handler_;
  std::shared_ptr<dynamixel::PacketHandler> packet_handler_;
  double joint_position_;
  double goal_position_;
  double joint_velocity_;
  double goal_velocity_;
  double present_temperature_;
};
}  //  namespace dynamixel_hardware_interface

#endif  // DYNAMIXEL_HARDWARE_INTERFACE__MOTOR_BASE_HPP_
Updated on 17 July 2023 at 01:42:11 UTC