00001 00002 #include <unistd.h> 00003 #include <fcntl.h> 00004 #include <getopt.h> 00005 #include <termios.h> 00006 #include <cmath> 00007 #include <stdio.h> 00008 #include <stdlib.h> 00009 #include <iostream> // std::cout 00010 #include <string> // std::string, std::to_string 00011 #include <vector> 00012 #include <time.h> 00013 #include <hardware_interface/joint_command_interface.h> 00014 #include <hardware_interface/joint_state_interface.h> 00015 00016 #include <hardware_interface/robot_hw.h> 00017 #include <controller_manager/controller_manager.h> 00018 #include <hardware_interface/posvel_command_interface.h> 00019 #include "dynamixel_sdk/dynamixel_sdk.h" // Uses Dynamixel SDK library 00020 #include <ros/package.h> 00021 #include "yaml-cpp/yaml.h" 00022 00023 #include <ros/ros.h> 00024 #include <robotican_hardware_interface/Device.h> 00025 #include <robotican_hardware_interface/RiCBoardManager.h> 00026 00027 #ifdef HAVE_NEW_YAMLCPP 00028 // The >> operator disappeared in yaml-cpp 0.5, so this function is 00029 // added to provide support for code written under the yaml-cpp 0.3 API. 00030 template<typename T> 00031 void operator >> (const YAML::Node& node, T& i) 00032 { 00033 i = node.as<T>(); 00034 } 00035 00036 #endif 00037 00038 struct dynamixel_spec 00039 { 00040 std::string name; 00041 uint16_t model_number; 00042 int cpr; 00043 double rpm_scale_factor; 00044 float torque_constant_a; 00045 float torque_constant_b; 00046 }; 00047 00048 00049 // Control table address FOR MX-28 00050 #define ADDR_MX_MODEL_NUM 30 00051 #define ADDR_MX_TORQUE_ENABLE 64 // Control table address is different in Dynamixel model 00052 #define ADDR_MX_GOAL_POSITION 116 00053 #define ADDR_MX_PROFILE_VELOCITY 112 00054 #define ADDR_MX_PRESENT_POSITION 132 00055 #define ADDR_MX_PRESENT_SPEED 128 00056 #define ADDR_MX_PRESENT_LOAD 126 00057 #define ADDR_MX_PRESENT_TEMPERATURE 146 00058 #define ADDR_MX_MOVING 122 00059 #define ADDR_MX_HARDWARE_ERROR 70 00060 00061 // Control table address FOR Pro 00062 #define ADDR_PRO_MODEL_NUM 0 00063 #define ADDR_PRO_TORQUE_ENABLE 562 // Control table address is different in Dynamixel model 00064 #define ADDR_PRO_GOAL_POSITION 596 00065 #define ADDR_PRO_GOAL_SPEED 600 00066 #define ADDR_PRO_GOAL_ACCELERATION 606 00067 #define ADDR_PRO_PRESENT_POSITION 611 00068 #define ADDR_PRO_PRESENT_SPEED 615 00069 #define ADDR_PRO_PRESENT_CURRENT 621 00070 #define ADDR_PRO_PRESENT_TEMPERATURE 43 00071 #define ADDR_PRO_MOVING 46 00072 #define ADDR_PRO_HARDWARE_ERROR 892 00073 00074 00075 // Control table address FOR XH 00076 #define ADDR_XH_MODEL_NUM 1040 00077 #define ADDR_XH_TORQUE_ENABLE 64 // Control table address is different in Dynamixel model 00078 #define ADDR_XH_GOAL_POSITION 116 00079 #define ADDR_XH_GOAL_SPEED 104 00080 #define ADDR_XH_VELOCITY_LIMIT 44 00081 #define ADDR_XH_PROFILE_VELOCITY 112 00082 #define ADDR_XH_GOAL_ACCELERATION 40 00083 #define ADDR_XH_PRESENT_POSITION 132 00084 #define ADDR_XH_PRESENT_SPEED 128 00085 #define ADDR_XH_PRESENT_CURRENT 126 00086 #define ADDR_XH_PRESENT_TEMPERATURE 146 00087 #define ADDR_XH_MOVING 123 00088 #define ADDR_XH_HARDWARE_ERROR 70 00089 00090 // Protocol version 00091 #define PROTOCOL_VERSION1 1.0 // See which protocol version is used in the Dynamixel 00092 #define PROTOCOL_VERSION2 2.0 00093 00094 00095 struct motor 00096 { 00097 uint8_t id; 00098 uint16_t model; 00099 bool torque; 00100 double position; 00101 double velocity; 00102 double current; 00103 double effort; 00104 double command_position; 00105 double command_velocity; 00106 double pre_command_velocity; 00107 uint8_t error; 00108 int cpr; 00109 double rpm_scale_factor; 00110 float protocol_ver; 00111 std::string joint_name; 00112 std::string interface; 00113 float torque_constant_a; 00114 float torque_constant_b; 00115 bool first_read; 00116 00117 };