dynamixel_interface_driver.h

Go to the documentation of this file.

/* CSIRO Open Source Software License Agreement (variation of the BSD / MIT License)
 * Copyright (c) 2020, Commonwealth Scientific and Industrial Research Organisation (CSIRO) ABN 41 687 119 230.
 * All rights reserved. CSIRO is willing to grant you a license to the dynamixel_actuator ROS packages on the following
 * terms, except where otherwise indicated for third party material. Redistribution and use of this software in source
 * and binary forms, with or without modification, are permitted provided that the following conditions are met:
 * - Redistributions of source code must retain the above copyright notice, this list of conditions and the following
 *   disclaimer.
 * - Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following
 *   disclaimer in the documentation and/or other materials provided with the distribution.
 * - Neither the name of CSIRO nor the names of its contributors may be used to endorse or promote products derived from
 *   this software without specific prior written permission of CSIRO.
 *
 * EXCEPT AS EXPRESSLY STATED IN THIS AGREEMENT AND TO THE FULL EXTENT PERMITTED BY APPLICABLE LAW, THE SOFTWARE IS
 * PROVIDED "AS-IS". CSIRO MAKES NO REPRESENTATIONS, WARRANTIES OR CONDITIONS OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
 * BUT NOT LIMITED TO ANY REPRESENTATIONS, WARRANTIES OR CONDITIONS REGARDING THE CONTENTS OR ACCURACY OF THE SOFTWARE,
 * OR OF TITLE, MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, NON-INFRINGEMENT, THE ABSENCE OF LATENT OR OTHER
 * DEFECTS, OR THE PRESENCE OR ABSENCE OF ERRORS, WHETHER OR NOT DISCOVERABLE.
 * TO THE FULL EXTENT PERMITTED BY APPLICABLE LAW, IN NO EVENT SHALL CSIRO BE LIABLE ON ANY LEGAL THEORY (INCLUDING,
 * WITHOUT LIMITATION, IN AN ACTION FOR BREACH OF CONTRACT, NEGLIGENCE OR OTHERWISE) FOR ANY CLAIM, LOSS, DAMAGES OR
 * OTHER LIABILITY HOWSOEVER INCURRED.  WITHOUT LIMITING THE SCOPE OF THE PREVIOUS SENTENCE THE EXCLUSION OF LIABILITY
 * SHALL INCLUDE: LOSS OF PRODUCTION OR OPERATION TIME, LOSS, DAMAGE OR CORRUPTION OF DATA OR RECORDS; OR LOSS OF
 * ANTICIPATED SAVINGS, OPPORTUNITY, REVENUE, PROFIT OR GOODWILL, OR OTHER ECONOMIC LOSS; OR ANY SPECIAL, INCIDENTAL,
 * INDIRECT, CONSEQUENTIAL, PUNITIVE OR EXEMPLARY DAMAGES, ARISING OUT OF OR IN CONNECTION WITH THIS AGREEMENT, ACCESS
 * OF THE SOFTWARE OR ANY OTHER DEALINGS WITH THE SOFTWARE, EVEN IF CSIRO HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH
 * CLAIM, LOSS, DAMAGES OR OTHER LIABILITY. APPLICABLE LEGISLATION SUCH AS THE AUSTRALIAN CONSUMER LAW MAY APPLY
 * REPRESENTATIONS, WARRANTIES, OR CONDITIONS, OR IMPOSES OBLIGATIONS OR LIABILITY ON CSIRO THAT CANNOT BE EXCLUDED,
 * RESTRICTED OR MODIFIED TO THE FULL EXTENT SET OUT IN THE EXPRESS TERMS OF THIS CLAUSE ABOVE "CONSUMER GUARANTEES".
 * TO THE EXTENT THAT SUCH CONSUMER GUARANTEES CONTINUE TO APPLY, THEN TO THE FULL EXTENT PERMITTED BY THE APPLICABLE
 * LEGISLATION, THE LIABILITY OF CSIRO UNDER THE RELEVANT CONSUMER GUARANTEE IS LIMITED (WHERE PERMITTED AT CSIRO'S
 * OPTION) TO ONE OF FOLLOWING REMEDIES OR SUBSTANTIALLY EQUIVALENT REMEDIES:
 * (a)  THE REPLACEMENT OF THE SOFTWARE, THE SUPPLY OF EQUIVALENT SOFTWARE, OR SUPPLYING RELEVANT SERVICES AGAIN;
 * (b)  THE REPAIR OF THE SOFTWARE;
 * (c)  THE PAYMENT OF THE COST OF REPLACING THE SOFTWARE, OF ACQUIRING EQUIVALENT SOFTWARE, HAVING THE RELEVANT
 *      SERVICES SUPPLIED AGAIN, OR HAVING THE SOFTWARE REPAIRED.
 * IN THIS CLAUSE, CSIRO INCLUDES ANY THIRD PARTY AUTHOR OR OWNER OF ANY PART OF THE SOFTWARE OR MATERIAL DISTRIBUTED
 * WITH IT.  CSIRO MAY ENFORCE ANY RIGHTS ON BEHALF OF THE RELEVANT THIRD PARTY.
 *
 * Third Party Components:
 *
 * The following third party components are distributed with the Software.  You agree to comply with the license terms
 * for these components as part of accessing the Software.  Other third party software may also be identified in
 * separate files distributed with the Software.
 * ___________________________________________________________________
 *
 * dynamixel_interface is forked from projects authored by Brian
 * Axelrod (on behalf of Willow Garage):
 *
 * https://github.com/baxelrod/dynamixel_pro_controller
 * https://github.com/baxelrod/dynamixel_pro_driver
 *
 * Thus they retain the following notice:
 *
 * Software License Agreement (BSD License)
 * Copyright (c) 2013, Willow Garage
 * All rights reserved.
 * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the
 * following conditions are met:
 *  - Redistributions of source code must retain the above copyright notice, this list of conditions and the following
 *    disclaimer.
 *  - Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the
 *    following disclaimer in the documentation and/or other materials provided with the distribution.
 *  - Neither the name of Willow Garage nor the names of its contributors may be used to endorse or promote products
 *    derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
 * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 * ___________________________________________________________________
 */

#ifndef DYNAMIXEL_INTERFACE_DRIVER_H_
#define DYNAMIXEL_INTERFACE_DRIVER_H_

#include <stdint.h>
#include <map>
#include <string>
#include <unordered_map>
#include <vector>

#include <dynamixel_interface/dynamixel_const.h>
#include <dynamixel_sdk/dynamixel_sdk.h>
#include <ros/ros.h>

namespace dynamixel_interface
{
struct DynamixelSpec
{
  std::string name;              
  uint16_t model_number;         
  DynamixelSeriesType type;      
  bool external_ports;           
  int encoder_cpr;               
  double encoder_range_deg;      
  double velocity_radps_to_reg;  
  double effort_reg_max;         
  double effort_reg_to_mA;       
};


struct SyncData
{
  int id;                     
  DynamixelSeriesType type;   
  bool success;               
  uint8_t error;              
  std::vector<uint8_t> data;  
};

struct DynamixelState : SyncData
{
  int32_t position;  
  int32_t velocity;  
  int32_t effort;    
};

struct DynamixelDiagnostic : SyncData
{
  int32_t voltage;      
  int32_t temperature;  
};

struct DynamixelDataport : SyncData
{
  std::array<uint16_t, 4> port_values;  
};

class DynamixelInterfaceDriver
{
public:
  DynamixelInterfaceDriver(const std::string &device = "/dev/ttyUSB0", int baud = 1000000,
                           bool use_legacy_protocol = false, bool use_group_read = true, bool use_group_write = true);

  ~DynamixelInterfaceDriver();

  bool initialise(void);

  bool ping(int servo_id) const;

  inline const DynamixelSpec *getModelSpec(uint model_number) const
  {
    auto it = model_specs_.find(model_number);
    if (it != model_specs_.end())
    {
      return &(it->second);
    }
    else
    {
      return nullptr;
    }
  };

  // ************************************ GETTERS ***************************************** //

  bool getModelNumber(int servo_id, uint16_t *model_number) const;

  bool getErrorStatus(int servo_id, DynamixelSeriesType type, uint8_t *error) const;

  bool getMaxTorque(int servo_id, DynamixelSeriesType type, uint16_t *max_torque) const;

  bool getTorqueEnabled(int servo_id, DynamixelSeriesType type, bool *torque_enabled) const;

  bool getTargetTorque(int servo_id, DynamixelSeriesType type, int16_t *target_torque) const;

  bool readRegisters(int servo_id, uint16_t address, uint16_t length, std::vector<uint8_t> *response) const;

  // *********************** BULK_READ METHODS *************************** //

  bool getBulkState(std::unordered_map<int, DynamixelState> &state_map) const;

  bool getBulkDataportInfo(std::unordered_map<int, DynamixelDataport> &data_map) const;

  bool getBulkDiagnosticInfo(std::unordered_map<int, DynamixelDiagnostic> &diag_map) const;

  // **************************** SETTERS ******************************** //

  bool setOperatingMode(int servo_id, DynamixelSeriesType type, DynamixelControlMode operating_mode) const;

  bool setAngleLimits(int servo_id, DynamixelSeriesType type, int32_t min_angle, int32_t max_angle) const;

  bool setMinAngleLimit(int servo_id, DynamixelSeriesType type, int32_t angle) const;

  bool setMaxAngleLimit(int servo_id, DynamixelSeriesType type, int32_t angle) const;

  bool setMaxTorque(int servo_id, DynamixelSeriesType type, uint16_t max_torque) const;

  bool setMaxVelocity(int servo_id, DynamixelSeriesType type, uint32_t max_vel) const;

  bool setTorqueEnabled(int servo_id, DynamixelSeriesType type, bool on) const;

  bool setTorqueControlEnabled(int servo_id, DynamixelSeriesType type, bool on) const;


  bool setPositionPIDGains(int servo_id, DynamixelSeriesType type, double p_gain, double i_gain, double d_gain) const;

  bool setPositionProportionalGain(int servo_id, DynamixelSeriesType type, uint16_t gain) const;

  bool setPositionIntegralGain(int servo_id, DynamixelSeriesType type, uint16_t gain) const;

  bool setPositionDerivativeGain(int servo_id, DynamixelSeriesType type, uint16_t gain) const;

  bool setVelocityPIDGains(int servo_id, DynamixelSeriesType type, double p_gain, double i_gain) const;

  bool setVelocityProportionalGain(int servo_id, DynamixelSeriesType type, uint16_t gain) const;

  bool setVelocityIntegralGain(int servo_id, DynamixelSeriesType type, uint16_t gain) const;

  bool setProfileVelocity(int servo_id, DynamixelSeriesType type, int32_t velocity) const;

  bool setTorque(int servo_id, DynamixelSeriesType type, int16_t torque) const;

  bool writeRegisters(int servo_id, uint16_t address, uint16_t length, uint8_t *data) const;

  // *********************** SYNC_WRITE METHODS *************************** //

  bool setMultiPosition(std::unordered_map<int, SyncData> &position_data) const;

  bool setMultiVelocity(std::unordered_map<int, SyncData> &velocity_data) const;

  bool setMultiProfileVelocity(std::unordered_map<int, SyncData> &velocity_data) const;

  bool setMultiTorque(std::unordered_map<int, SyncData> &torque_data) const;

  inline std::string getSeriesName(DynamixelSeriesType type) const
  {
    std::string series_name = "undefined";
    switch (type)
    {
      case kSeriesAX:
        series_name = "AX";
        break;
      case kSeriesRX:
        series_name = "RX";
        break;
      case kSeriesDX:
        series_name = "DX";
        break;
      case kSeriesEX:
        series_name = "EX";
        break;
      case kSeriesLegacyMX:
        series_name = "Legacy MX";
        break;
      case kSeriesMX:
        series_name = "MX";
        break;
      case kSeriesX:
        series_name = "X";
        break;
      case kSeriesP:
        series_name = "P";
        break;
      case kSeriesLegacyPro:
        series_name = "Legacy Pro";
        break;
    }
    return series_name;
  };

private:
  bool loadMotorData(void);

  bool bulkRead(std::unordered_map<int, SyncData*> &read_data, uint16_t address, uint16_t length) const;

  bool syncRead(std::unordered_map<int, SyncData*> &read_data, uint16_t address, uint16_t length) const;

  bool syncWrite(std::unordered_map<int, SyncData> &write_data, uint16_t address, uint16_t length) const;

private:
  std::unique_ptr<dynamixel::PortHandler> portHandler_;  
  std::unique_ptr<dynamixel::PacketHandler> packetHandler_;  

  std::string device_;  
  int baud_rate_;       

  bool use_legacy_protocol_;  
  bool use_group_read_;       
  bool use_group_write_;      

  bool initialised_ = false;              
  uint8_t single_read_fallback_counter_;  

  std::unordered_map<uint16_t, const DynamixelSpec> model_specs_;  
  std::unordered_map<int, std::vector<uint8_t>> raw_read_map_;     
};

}  // namespace dynamixel_interface

#endif  // DYNAMIXEL_INTERFACE_DRIVER_H_