Program Listing for File trossen_arm.hpp

↰ Return to documentation for file (include/libtrossen_arm/trossen_arm.hpp)

// Copyright 2025 Trossen Robotics
//
// 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 the the copyright holder 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 HOLDER 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 LIBTROSSEN_ARM__TROSSEN_ARM_HPP_
#define LIBTROSSEN_ARM__TROSSEN_ARM_HPP_

#include <atomic>
#include <cstdint>
#include <cstring>
#include <map>
#include <mutex>
#include <optional>
#include <string>
#include <thread>
#include <vector>

#include "libtrossen_arm/trossen_arm_config.hpp"
#include "libtrossen_arm/trossen_arm_interpolate.hpp"
#include "libtrossen_arm/trossen_arm_logging.hpp"
#include "libtrossen_arm/trossen_arm_udp_client.hpp"
#include "yaml-cpp/yaml.h"

namespace trossen_arm
{

enum class Mode : uint8_t {
  idle,
  position,
  velocity,
  external_effort,
};

enum class IPMethod : uint8_t {
  manual,
  dhcp,
};

enum class Model : uint8_t {
  wxai_v0,
};

struct JointCharacteristic
{
  float effort_correction;
  float friction_transition_velocity;
  float friction_constant_term;
  float friction_coulomb_coef;
  float friction_viscous_coef;
  float continuity_factor;
};

struct LinkProperties
{
  float mass;
  std::array<float, 9> inertia;
  std::array<float, 3> origin_xyz;
  std::array<float, 3> origin_rpy;
};

struct EndEffectorProperties
{
  LinkProperties palm;

  LinkProperties finger_left;

  LinkProperties finger_right;

  float offset_finger_left;

  float offset_finger_right;

  float t_max_factor;
};

struct StandardEndEffector {
  static constexpr EndEffectorProperties wxai_v0_base{
    .palm = {
      .mass = 0.53780000f,
      .inertia = {
        0.00079919, -0.00000049, 0.00000010,
        -0.00000049, 0.00047274, 0.00000004,
        0.00000010, 0.00000004, 0.00105293
      },
      .origin_xyz = {0.04572768f, -0.00000726f, 0.00001402f},
      .origin_rpy = {0.0f, 0.0f, 0.0f}
    },
    .finger_left = {
      .mass = 0.05945000f,
      .inertia = {
        0.00001875f, 0.00000309f, -0.00000149f,
        0.00000309f, 0.00002614f, -0.00000124f,
        -0.00000149f, -0.00000124f, 0.00002995f
      },
      .origin_xyz = {0.00169016f, -0.00592796f, -0.00365701f},
      .origin_rpy = {0.0f, 0.0f, 0.0f}
    },
    .finger_right = {
      .mass = 0.05945000f,
      .inertia = {
        0.00001930f, -0.00000309f, 0.00000359f,
        -0.00000309f, 0.00002670f, -0.00000064f,
        0.00000359f, -0.00000064f, 0.00002995f
      },
      .origin_xyz = {0.00169015f, 0.00592793f, 0.00201818f},
      .origin_rpy = {0.0f, 0.0f, 0.0f}
    },
    .offset_finger_left = 0.0227f,
    .offset_finger_right = -0.0227f,
    .t_max_factor = 0.5f
  };

  static constexpr EndEffectorProperties wxai_v0_leader{
    .palm = {
      .mass = 0.59570000f,
      .inertia = {
        0.00117653f, -0.00000040f, -0.00005492f,
        -0.00000040f, 0.00085696f, 0.00000074f,
        -0.00005492f, 0.00000074f, 0.00107685f
      },
      .origin_xyz = {0.04454388f, 0.00000506f, -0.00694150f},
      .origin_rpy = {0.0f, 0.0f, 0.0f}
    },
    .finger_left = {
      .mass = 0.06380000f,
      .inertia = {
        0.00003556f, -0.00000249f, 0.00000167f,
        -0.00000249f, 0.00002700f, 0.00000217f,
        0.00000167f, 0.00000217f, 0.00001726f
      },
      .origin_xyz = {-0.00423580f, -0.00167541f, -0.01050810f},
      .origin_rpy = {0.0f, 0.0f, 0.0f}
    },
    .finger_right = {
      .mass = 0.06380000f,
      .inertia = {
        0.00004133f, 0.00000250f, 0.00000517f,
        0.00000250f, 0.00003277f, -0.00000592f,
        0.00000517f, -0.00000592f, 0.00001727f
      },
      .origin_xyz = {-0.00423309f, 0.00167373f, -0.00451087f},
      .origin_rpy = {0.0f, 0.0f, 0.0f}
    },
    .offset_finger_left = 0.0179f,
    .offset_finger_right = -0.0179f,
    .t_max_factor = 0.5f
  };

  static constexpr EndEffectorProperties wxai_v0_follower{
    .palm = {
      .mass = 0.64230000f,
      .inertia = {
        0.00108484f, 0.00000063f, -0.00004180f,
        0.00000063f, 0.00075170f, -0.00001558f,
        -0.00004180f, -0.00001558f, 0.00110994f
      },
      .origin_xyz = {0.04699592f, 0.00045936f, 0.00827772f},
      .origin_rpy = {0.0f, 0.0f, 0.0f}
    },
    .finger_left = {
      .mass = 0.05945000f,
      .inertia = {
        0.00001875f, 0.00000309f, -0.00000149f,
        0.00000309f, 0.00002614f, -0.00000124f,
        -0.00000149f, -0.00000124f, 0.00002995f
      },
      .origin_xyz = {0.00169016f, -0.00592796f, -0.00365701f},
      .origin_rpy = {0.0f, 0.0f, 0.0f}
    },
    .finger_right = {
      .mass = 0.05945000f,
      .inertia = {
        0.00001930f, -0.00000309f, 0.00000359f,
        -0.00000309f, 0.00002670f, -0.00000064f,
        0.00000359f, -0.00000064f, 0.00002995f
      },
      .origin_xyz = {0.00169015f, 0.00592793f, 0.00201818f},
      .origin_rpy = {0.0f, 0.0f, 0.0f}
    },
    .offset_finger_left = 0.0227f,
    .offset_finger_right = -0.0227f,
    .t_max_factor = 0.5f
  };
};

class TrossenArmDriver
{
public:
  ~TrossenArmDriver();

  void configure(
    Model model,
    EndEffectorProperties end_effector,
    const std::string serv_ip,
    bool clear_error
  );

  void cleanup();

  void set_all_positions(
    const std::vector<float> & goal_positions,
    float goal_time = 2.0f,
    bool blocking = true,
    const std::optional<std::vector<float>> & goal_feedforward_velocities = std::nullopt,
    const std::optional<std::vector<float>> & goal_feedforward_accelerations = std::nullopt);

  void set_arm_positions(
    const std::vector<float> & goal_positions,
    float goal_time = 2.0f,
    bool blocking = true,
    const std::optional<std::vector<float>> & goal_feedforward_velocities = std::nullopt,
    const std::optional<std::vector<float>> & goal_feedforward_accelerations = std::nullopt);

  void set_gripper_position(
    float goal_position,
    float goal_time = 2.0f,
    bool blocking = true,
    float goal_feedforward_velocity = 0.0f,
    float goal_feedforward_acceleration = 0.0f);

  void set_joint_position(
    uint8_t joint_index,
    float goal_position,
    float goal_time = 2.0f,
    bool blocking = true,
    float goal_feedforward_velocity = 0.0f,
    float goal_feedforward_acceleration = 0.0f
  );

  void set_all_velocities(
    const std::vector<float> & goal_velocities,
    float goal_time = 2.0f,
    bool blocking = true,
    const std::optional<std::vector<float>> & goal_feedforward_accelerations = std::nullopt);

  void set_arm_velocities(
    const std::vector<float> & goal_velocities,
    float goal_time = 2.0f,
    bool blocking = true,
    const std::optional<std::vector<float>> & goal_feedforward_accelerations = std::nullopt);

  void set_gripper_velocity(
    float goal_velocity,
    float goal_time = 2.0f,
    bool blocking = true,
    float goal_feedforward_acceleration = 0.0f
  );

  void set_joint_velocity(
    uint8_t joint_index,
    float goal_velocity,
    float goal_time = 2.0f,
    bool blocking = true,
    float goal_feedforward_acceleration = 0.0f
  );

  void set_all_external_efforts(
    const std::vector<float> & goal_external_efforts,
    float goal_time = 2.0f,
    bool blocking = true
  );

  void set_arm_external_efforts(
    const std::vector<float> & goal_external_efforts,
    float goal_time = 2.0f,
    bool blocking = true
  );

  void set_gripper_external_effort(
    float goal_external_effort,
    float goal_time = 2.0f,
    bool blocking = true
  );

  void set_joint_external_effort(
    uint8_t joint_index,
    float goal_external_effort,
    float goal_time = 2.0f,
    bool blocking = true
  );

  void load_configs_from_file(const std::string & file_path);

  void set_factory_reset_flag(bool flag = true);

  void set_ip_method(IPMethod method = IPMethod::manual);

  void set_manual_ip(const std::string manual_ip = "192.168.1.2");

  void set_dns(const std::string dns = "8.8.8.8");

  void set_gateway(const std::string gateway = "192.168.1.1");

  void set_subnet(const std::string subnet = "255.255.255.0");

  void set_joint_characteristics(const std::vector<JointCharacteristic> & joint_characteristics);

  void set_effort_corrections(const std::vector<float> & effort_corrections);

  void set_friction_transition_velocities(
    const std::vector<float> & friction_transition_velocities
  );

  void set_friction_constant_terms(const std::vector<float> & friction_constant_terms);

  void set_friction_coulomb_coefs(const std::vector<float> & friction_coulomb_coefs);

  void set_friction_viscous_coefs(const std::vector<float> & friction_viscous_coefs);

  void set_continuity_factors(const std::vector<float> & continuity_factors);

  void set_joint_modes(const std::vector<Mode> & modes);

  void set_all_modes(Mode mode = Mode::idle);

  void set_arm_modes(Mode mode = Mode::idle);

  void set_gripper_mode(Mode mode = Mode::idle);

  void set_end_effector(const EndEffectorProperties & end_effector);

  void set_gripper_force_limit_scaling_factor(float scaling_factor = 0.5f);

  uint8_t get_num_joints() const;

  std::string get_driver_version() const;

  std::string get_controller_version() const;

  std::vector<float> get_positions();

  std::vector<float> get_velocities();

  std::vector<float> get_efforts();

  std::vector<float> get_external_efforts();

  std::vector<float> get_compensation_efforts();

  void save_configs_to_file(const std::string & file_path);

  bool get_factory_reset_flag();

  IPMethod get_ip_method();

  std::string get_manual_ip();

  std::string get_dns();

  std::string get_gateway();

  std::string get_subnet();

  std::vector<JointCharacteristic> get_joint_characteristics();

  std::vector<float> get_effort_corrections();

  std::vector<float> get_friction_transition_velocities();

  std::vector<float> get_friction_constant_terms();

  std::vector<float> get_friction_coulomb_coefs();

  std::vector<float> get_friction_viscous_coefs();

  std::vector<float> get_continuity_factors();

  std::string get_error_information();

  std::vector<Mode> get_modes();

  EndEffectorProperties get_end_effector();

  float get_gripper_force_limit_scaling_factor();

private:
  // Raw counterparts of LinkProperties and EndEffectorProperties
  struct LinkRaw
  {
    float mass;
    float inertia[9];
    float origin_xyz[3];
    float origin_rpy[3];
  };
  struct EndEffectorRaw
  {
    LinkRaw palm;
    LinkRaw finger_left;
    LinkRaw finger_right;
    float offset_finger_left;
    float offset_finger_right;
    float t_max_factor;
  };

  struct JointInput
  {
    Mode mode{Mode::idle};
    union {
      struct {
        float position;
        float feedforward_velocity;
        float feedforward_acceleration;
      } position{0.0f, 0.0f, 0.0f};
      struct {
        float velocity;
        float feedforward_acceleration;
      } velocity;
      struct {
        float external_effort;
      } external_effort;
    };
  };

  struct JointOutput
  {
    float position;
    float velocity;
    float effort;
    float external_effort;
  };

  // Robot command indicators
  enum class RobotCommandIndicator : uint8_t
  {
    handshake,
    set_joint_inputs,
    get_joint_outputs,
    set_home,
    set_configuration,
    get_configuration,
    get_log,
  };

  // ErrorState
  enum class ErrorState : uint8_t {
    // No error
    none,
    // Controller's UDP interface failed to initialize
    udp_init_failed,
    // Controller's CAN interface failed to initialize
    can_init_failed,
    // Controller's CAN interface failed to send a message
    joint_command_failed,
    // Controller's CAN interface failed to receive a message
    joint_feedback_failed,
    // Joint clear error command failed
    joint_clear_error_failed,
    // Joint enable command failed
    joint_enable_failed,
    // Joint disable command failed
    joint_disable_failed,
    // Joint home calibration command failed
    joint_set_home_failed,
    // Joint disabled unexpectedly
    joint_disabled_unexpectedly,
    // Joint overheated
    joint_overheated,
    // Invalid mode command received
    invalid_mode,
    // Invalid robot command indicator received
    invalid_robot_command,
    // Robot command with unexpected size received
    invalid_robot_command_size,
    // Invalid configuration address
    invalid_configuration_address,
    // Invalid pending command
    invalid_pending_command,
    // Robot input with modes different than configured modes received
    robot_input_mode_mismatch,
    // Discontinuous robot input received
    robot_input_discontinous,
  };

  // Configuration addresses
  enum class ConfigurationAddress : uint8_t {
    factory_reset_flag,
    ip_method,
    manual_ip,
    dns,
    gateway,
    subnet,
    joint_characteristics,
    error_state,
    modes,
    end_effector
  };

  // UDP port
  static constexpr uint16_t PORT{50000};

  // Timeout in microseconds for receiving UDP packets
  static constexpr uint32_t TIMEOUT_US{1000};

  // Maximum retransmission attempts
  static constexpr uint8_t MAX_RETRANSMISSION_ATTEMPTS{100};

  // Model to number of joints mapping
  static const std::map<Model, uint8_t> MODEL_NUM_JOINTS;

  // Error information
  static const std::map<ErrorState, std::string> ERROR_INFORMATION;

  // Model name
  static const std::map<Model, std::string> MODEL_NAME;

  // Mode name
  static const std::map<Mode, std::string> MODE_NAME;

  // Configuration name
  static const std::map<ConfigurationAddress, std::string> CONFIGURATION_NAME;

  // Joint characteristic name
  static const struct JointCharacteristicName
  {
    std::string effort_correction;
    std::string friction_transition_velocity;
    std::string friction_constant_term;
    std::string friction_coulomb_coef;
    std::string friction_viscous_coef;
    std::string continuity_factor;
  } JOINT_CHARACTERISTIC_NAME;

  // Interpolators for joint trajectories
  std::vector<QuinticHermiteInterpolator> trajectories_;

  // Trajectory start time
  std::vector<std::chrono::time_point<std::chrono::steady_clock>> trajectory_start_times_;

  // Robot input
  std::vector<JointInput> joint_inputs_;

  // Joint outputs
  std::vector<JointOutput> joint_outputs_;

  // Number of joints
  uint8_t num_joints_{0};

  // Driver version
  std::string driver_version_;

  // Controller firmware version
  std::string controller_version_;

  // Whether the driver is properly configured for the robot to be controlled
  // true if configured, false if not configured
  bool configured_{false};

  // UDP client
  UDP_Client udp_client_;

  // Atomic flag for maintaining and stopping the daemon thread
  std::atomic<bool> activated_{false};

  // Multithreading design
  //
  // Goal
  //
  // - only one thread can run at a time
  // - another thread cannot cut in until the full communication cycle is completed
  //   for example, set_joint_inputs --nothing-in-between--> receive_joint_outputs
  // - the other thread has priority to run after the current thread finishes
  //
  // Mutex ownership
  //
  // call mutex_preempt_ 1 and mutex_data_ 2 for simplicity
  // daemon: |-|-1-|-12-|-2-|--------|-1-|-12-|-2-|-|
  // main:   |------------|-1-|-12-|-2-|------------|
  //
  // Exception handling
  //
  // - if an exception is thrown in the main thread
  //   - the daemon thread gets std::terminate
  //   - the main thread unwind the stack: ~TrossenArmDriver() -> cleanup()
  // - if an exception is thrown in the daemon thread
  //   - the exception is stored in exception_ptr_
  //   - the daemon thread returns
  //   - the main thread gets the exception and rethrows it at the next operation
  //   - the main thread unwind the stack: ~TrossenArmDriver() -> cleanup()
  //
  // Notes
  //
  // - the mutex claiming cannot be nested or there will be deadlocks, i.e., |-1-|-12-|-2-|-12-|-2-|
  //   is not allowed
  // - when an exception is thrown by the main thread, the program is expected to terminate either
  //   immediately or right after cleaning up the resources not related to the driver

  // Daemon thread
  std::thread daemon_thread_;

  // Mutex for data access
  std::mutex mutex_data_;

  // Mutex for preempting the next slot to run
  std::mutex mutex_preempt_;

  // Shared exception pointer
  std::exception_ptr exception_ptr_;

  void set_joint_inputs();

  bool receive_joint_outputs();

  void get_configuration(ConfigurationAddress configuration_address);

  void check_error_state(bool clear_error);

  void reset_error_state();

  std::string get_detailed_log();

  void daemon();
};

}  // namespace trossen_arm

#endif  // LIBTROSSEN_ARM__TROSSEN_ARM_HPP_