From 587ba84c709e33b07990ef40ddc1fa0a58083397 Mon Sep 17 00:00:00 2001 From: ROBOTIS Date: Fri, 4 Mar 2016 21:01:35 +0900 Subject: [PATCH 001/238] - renewal --- README.md | 2 + dynamixel_sdk/CMakeLists.txt | 38 + .../include/dynamixel_sdk/GroupBulkRead.h | 59 + .../include/dynamixel_sdk/GroupBulkWrite.h | 55 + .../include/dynamixel_sdk/GroupSyncRead.h | 59 + .../include/dynamixel_sdk/GroupSyncWrite.h | 54 + .../include/dynamixel_sdk/PacketHandler.h | 130 ++ .../include/dynamixel_sdk/PortHandler.h | 51 + .../dynamixel_sdk/Protocol1PacketHandler.h | 95 ++ .../dynamixel_sdk/Protocol2PacketHandler.h | 100 ++ .../include/dynamixel_sdk/RobotisDef.h | 21 + .../dynamixel_sdk_linux/PortHandlerLinux.h | 62 + dynamixel_sdk/package.xml | 15 + .../src/dynamixel_sdk/GroupBulkRead.cpp | 202 +++ .../src/dynamixel_sdk/GroupBulkWrite.cpp | 133 ++ .../src/dynamixel_sdk/GroupSyncRead.cpp | 177 +++ .../src/dynamixel_sdk/GroupSyncWrite.cpp | 110 ++ .../src/dynamixel_sdk/PacketHandler.cpp | 22 + .../src/dynamixel_sdk/PortHandler.cpp | 31 + .../dynamixel_sdk/Protocol1PacketHandler.cpp | 562 +++++++++ .../dynamixel_sdk/Protocol2PacketHandler.cpp | 898 +++++++++++++ .../dynamixel_sdk_linux/PortHandlerLinux.cpp | 253 ++++ robotis_controller/CMakeLists.txt | 55 + .../robotis_controller/RobotisController.h | 124 ++ robotis_controller/package.xml | 38 + .../robotis_controller/RobotisController.cpp | 1112 +++++++++++++++++ robotis_controller_msgs/CMakeLists.txt | 47 + .../msg/JointCtrlModule.msg | 2 + robotis_controller_msgs/msg/SyncWriteItem.msg | 3 + robotis_controller_msgs/package.xml | 30 + .../srv/GetJointModule.srv | 4 + robotis_device/CMakeLists.txt | 61 + .../devices/dynamixel/GRIPPER.device | 70 ++ .../devices/dynamixel/H42-20-S300-R.device | 71 ++ .../devices/dynamixel/H54-100-S500-R.device | 71 ++ .../devices/dynamixel/H54-200-B500-R.device | 71 ++ .../devices/dynamixel/H54-200-S500-R.device | 71 ++ .../devices/dynamixel/L42-10-S300-R.device | 70 ++ .../devices/dynamixel/L54-30-S400-R.device | 71 ++ .../devices/dynamixel/L54-30-S500-R.device | 71 ++ .../devices/dynamixel/L54-50-S290-R.device | 71 ++ .../devices/dynamixel/L54-50-S500-R.device | 71 ++ .../devices/dynamixel/M42-10-S260-R.device | 71 ++ .../devices/dynamixel/M54-40-S250-R.device | 71 ++ .../devices/dynamixel/M54-60-S250-R.device | 71 ++ robotis_device/devices/dynamixel/MX-28.device | 59 + .../include/robotis_device/ControlTableItem.h | 52 + .../include/robotis_device/Dynamixel.h | 60 + .../include/robotis_device/DynamixelState.h | 76 ++ robotis_device/include/robotis_device/Robot.h | 35 + .../include/robotis_device/Sensor.h | 34 + robotis_device/package.xml | 34 + .../src/robotis_device/Dynamixel.cpp | 37 + robotis_device/src/robotis_device/Robot.cpp | 227 ++++ robotis_framework_common/CMakeLists.txt | 186 +++ .../robotis_framework_common/MotionModule.h | 53 + .../robotis_framework_common/RobotisDef.h | 21 + robotis_framework_common/package.xml | 52 + 58 files changed, 6452 insertions(+) create mode 100644 README.md create mode 100644 dynamixel_sdk/CMakeLists.txt create mode 100644 dynamixel_sdk/include/dynamixel_sdk/GroupBulkRead.h create mode 100644 dynamixel_sdk/include/dynamixel_sdk/GroupBulkWrite.h create mode 100644 dynamixel_sdk/include/dynamixel_sdk/GroupSyncRead.h create mode 100644 dynamixel_sdk/include/dynamixel_sdk/GroupSyncWrite.h create mode 100644 dynamixel_sdk/include/dynamixel_sdk/PacketHandler.h create mode 100644 dynamixel_sdk/include/dynamixel_sdk/PortHandler.h create mode 100644 dynamixel_sdk/include/dynamixel_sdk/Protocol1PacketHandler.h create mode 100644 dynamixel_sdk/include/dynamixel_sdk/Protocol2PacketHandler.h create mode 100644 dynamixel_sdk/include/dynamixel_sdk/RobotisDef.h create mode 100644 dynamixel_sdk/include/dynamixel_sdk_linux/PortHandlerLinux.h create mode 100644 dynamixel_sdk/package.xml create mode 100644 dynamixel_sdk/src/dynamixel_sdk/GroupBulkRead.cpp create mode 100644 dynamixel_sdk/src/dynamixel_sdk/GroupBulkWrite.cpp create mode 100644 dynamixel_sdk/src/dynamixel_sdk/GroupSyncRead.cpp create mode 100644 dynamixel_sdk/src/dynamixel_sdk/GroupSyncWrite.cpp create mode 100644 dynamixel_sdk/src/dynamixel_sdk/PacketHandler.cpp create mode 100644 dynamixel_sdk/src/dynamixel_sdk/PortHandler.cpp create mode 100644 dynamixel_sdk/src/dynamixel_sdk/Protocol1PacketHandler.cpp create mode 100644 dynamixel_sdk/src/dynamixel_sdk/Protocol2PacketHandler.cpp create mode 100644 dynamixel_sdk/src/dynamixel_sdk_linux/PortHandlerLinux.cpp create mode 100644 robotis_controller/CMakeLists.txt create mode 100644 robotis_controller/include/robotis_controller/RobotisController.h create mode 100644 robotis_controller/package.xml create mode 100644 robotis_controller/src/robotis_controller/RobotisController.cpp create mode 100644 robotis_controller_msgs/CMakeLists.txt create mode 100644 robotis_controller_msgs/msg/JointCtrlModule.msg create mode 100644 robotis_controller_msgs/msg/SyncWriteItem.msg create mode 100644 robotis_controller_msgs/package.xml create mode 100644 robotis_controller_msgs/srv/GetJointModule.srv create mode 100644 robotis_device/CMakeLists.txt create mode 100644 robotis_device/devices/dynamixel/GRIPPER.device create mode 100644 robotis_device/devices/dynamixel/H42-20-S300-R.device create mode 100644 robotis_device/devices/dynamixel/H54-100-S500-R.device create mode 100644 robotis_device/devices/dynamixel/H54-200-B500-R.device create mode 100644 robotis_device/devices/dynamixel/H54-200-S500-R.device create mode 100644 robotis_device/devices/dynamixel/L42-10-S300-R.device create mode 100644 robotis_device/devices/dynamixel/L54-30-S400-R.device create mode 100644 robotis_device/devices/dynamixel/L54-30-S500-R.device create mode 100644 robotis_device/devices/dynamixel/L54-50-S290-R.device create mode 100644 robotis_device/devices/dynamixel/L54-50-S500-R.device create mode 100644 robotis_device/devices/dynamixel/M42-10-S260-R.device create mode 100644 robotis_device/devices/dynamixel/M54-40-S250-R.device create mode 100644 robotis_device/devices/dynamixel/M54-60-S250-R.device create mode 100644 robotis_device/devices/dynamixel/MX-28.device create mode 100644 robotis_device/include/robotis_device/ControlTableItem.h create mode 100644 robotis_device/include/robotis_device/Dynamixel.h create mode 100644 robotis_device/include/robotis_device/DynamixelState.h create mode 100644 robotis_device/include/robotis_device/Robot.h create mode 100644 robotis_device/include/robotis_device/Sensor.h create mode 100644 robotis_device/package.xml create mode 100644 robotis_device/src/robotis_device/Dynamixel.cpp create mode 100644 robotis_device/src/robotis_device/Robot.cpp create mode 100644 robotis_framework_common/CMakeLists.txt create mode 100644 robotis_framework_common/include/robotis_framework_common/MotionModule.h create mode 100644 robotis_framework_common/include/robotis_framework_common/RobotisDef.h create mode 100644 robotis_framework_common/package.xml diff --git a/README.md b/README.md new file mode 100644 index 0000000..73be6ba --- /dev/null +++ b/README.md @@ -0,0 +1,2 @@ +# ROBOTIS-Framework +ROBOTIS Framework GIT REP MAIN diff --git a/dynamixel_sdk/CMakeLists.txt b/dynamixel_sdk/CMakeLists.txt new file mode 100644 index 0000000..4f16e17 --- /dev/null +++ b/dynamixel_sdk/CMakeLists.txt @@ -0,0 +1,38 @@ +cmake_minimum_required(VERSION 2.8.3) +project(dynamixel_sdk) + +find_package(catkin REQUIRED COMPONENTS + roscpp +) + +catkin_package( + INCLUDE_DIRS include +# LIBRARIES dynamixel_sdk +# CATKIN_DEPENDS roscpp +# DEPENDS system_lib +) + +# include_directories(include) +include_directories( + include + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +add_library(dynamixel_sdk + src/${PROJECT_NAME}/PacketHandler.cpp + src/${PROJECT_NAME}/Protocol1PacketHandler.cpp + src/${PROJECT_NAME}/Protocol2PacketHandler.cpp + src/${PROJECT_NAME}/GroupSyncRead.cpp + src/${PROJECT_NAME}/GroupSyncWrite.cpp + src/${PROJECT_NAME}/GroupBulkRead.cpp + src/${PROJECT_NAME}/GroupBulkWrite.cpp + src/${PROJECT_NAME}/PortHandler.cpp + src/${PROJECT_NAME}_linux/PortHandlerLinux.cpp +) + +## Specify libraries to link a library or executable target against +target_link_libraries(dynamixel_sdk + ${catkin_LIBRARIES} +) + diff --git a/dynamixel_sdk/include/dynamixel_sdk/GroupBulkRead.h b/dynamixel_sdk/include/dynamixel_sdk/GroupBulkRead.h new file mode 100644 index 0000000..2447f87 --- /dev/null +++ b/dynamixel_sdk/include/dynamixel_sdk/GroupBulkRead.h @@ -0,0 +1,59 @@ +/* + * GroupBulkRead.h + * + * Created on: 2016. 1. 28. + * Author: zerom + */ + +#ifndef ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPBULKREAD_H_ +#define ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPBULKREAD_H_ + + +#include +#include +#include "RobotisDef.h" +#include "PortHandler.h" +#include "PacketHandler.h" + +namespace ROBOTIS +{ + +class GroupBulkRead +{ +private: + PortHandler *port_; + PacketHandler *ph_; + + std::vector id_list_; + std::map address_list_; // + std::map length_list_; // + std::map data_list_; // + + UINT8_T *param_; + + void MakeParam(); + +public: + GroupBulkRead(PortHandler *port, PacketHandler *ph); + ~GroupBulkRead() { ClearParam(); } + + PortHandler *GetPortHandler() { return port_; } + PacketHandler *GetPacketHandler() { return ph_; } + + bool AddParam (UINT8_T id, UINT16_T start_address, UINT16_T data_length); + void RemoveParam (UINT8_T id); + void ClearParam (); + + int TxPacket(); + int RxPacket(); + int TxRxPacket(); + + bool GetData(UINT8_T id, UINT16_T address, UINT8_T *data); + bool GetData(UINT8_T id, UINT16_T address, UINT16_T *data); + bool GetData(UINT8_T id, UINT16_T address, UINT32_T *data); +}; + +} + + +#endif /* ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPBULKREAD_H_ */ diff --git a/dynamixel_sdk/include/dynamixel_sdk/GroupBulkWrite.h b/dynamixel_sdk/include/dynamixel_sdk/GroupBulkWrite.h new file mode 100644 index 0000000..510af24 --- /dev/null +++ b/dynamixel_sdk/include/dynamixel_sdk/GroupBulkWrite.h @@ -0,0 +1,55 @@ +/* + * GroupBulkWrite.h + * + * Created on: 2016. 2. 2. + * Author: zerom + */ + +#ifndef ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPBULKWRITE_H_ +#define ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPBULKWRITE_H_ + + +#include +#include +#include "RobotisDef.h" +#include "PortHandler.h" +#include "PacketHandler.h" + +namespace ROBOTIS +{ + +class GroupBulkWrite +{ +private: + PortHandler *port_; + PacketHandler *ph_; + + std::vector id_list_; + std::map address_list_; // + std::map length_list_; // + std::map data_list_; // + + UINT8_T *param_; + UINT16_T param_length_; + + void MakeParam(); + +public: + GroupBulkWrite(PortHandler *port, PacketHandler *ph); + ~GroupBulkWrite() { ClearParam(); } + + PortHandler *GetPortHandler() { return port_; } + PacketHandler *GetPacketHandler() { return ph_; } + + bool AddParam (UINT8_T id, UINT16_T start_address, UINT16_T data_length, UINT8_T *data); + void RemoveParam (UINT8_T id); + bool ChangeParam (UINT8_T id, UINT16_T start_address, UINT16_T data_length, UINT8_T *data); + void ClearParam (); + + int TxPacket(); +}; + +} + + +#endif /* ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPBULKWRITE_H_ */ diff --git a/dynamixel_sdk/include/dynamixel_sdk/GroupSyncRead.h b/dynamixel_sdk/include/dynamixel_sdk/GroupSyncRead.h new file mode 100644 index 0000000..900cd6f --- /dev/null +++ b/dynamixel_sdk/include/dynamixel_sdk/GroupSyncRead.h @@ -0,0 +1,59 @@ +/* + * GroupSyncRead.h + * + * Created on: 2016. 2. 2. + * Author: zerom + */ + +#ifndef ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPSYNCREAD_H_ +#define ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPSYNCREAD_H_ + + +#include +#include +#include "RobotisDef.h" +#include "PortHandler.h" +#include "PacketHandler.h" + +namespace ROBOTIS +{ + +class GroupSyncRead +{ +private: + PortHandler *port_; + PacketHandler *ph_; + + std::vector id_list_; + std::map data_list_; // + + UINT8_T *param_; + UINT16_T start_address_; + UINT16_T data_length_; + + void MakeParam(); + +public: + GroupSyncRead(PortHandler *port, PacketHandler *ph, UINT16_T start_address, UINT16_T data_length); + ~GroupSyncRead() { ClearParam(); } + + PortHandler *GetPortHandler() { return port_; } + PacketHandler *GetPacketHandler() { return ph_; } + + bool AddParam (UINT8_T id); + void RemoveParam (UINT8_T id); + void ClearParam (); + + int TxPacket(); + int RxPacket(); + int TxRxPacket(); + + bool GetData(UINT8_T id, UINT16_T address, UINT8_T *data); + bool GetData(UINT8_T id, UINT16_T address, UINT16_T *data); + bool GetData(UINT8_T id, UINT16_T address, UINT32_T *data); +}; + +} + + +#endif /* ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPSYNCREAD_H_ */ diff --git a/dynamixel_sdk/include/dynamixel_sdk/GroupSyncWrite.h b/dynamixel_sdk/include/dynamixel_sdk/GroupSyncWrite.h new file mode 100644 index 0000000..720e60a --- /dev/null +++ b/dynamixel_sdk/include/dynamixel_sdk/GroupSyncWrite.h @@ -0,0 +1,54 @@ +/* + * GroupSyncWrite.h + * + * Created on: 2016. 1. 28. + * Author: zerom + */ + +#ifndef ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPSYNCWRITE_H_ +#define ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPSYNCWRITE_H_ + + +#include +#include +#include "RobotisDef.h" +#include "PortHandler.h" +#include "PacketHandler.h" + +namespace ROBOTIS +{ + +class GroupSyncWrite +{ +private: + PortHandler *port_; + PacketHandler *ph_; + + std::vector id_list_; + std::map data_list_; // + + UINT8_T *param_; + UINT16_T start_address_; + UINT16_T data_length_; + + void MakeParam(); + +public: + GroupSyncWrite(PortHandler *port, PacketHandler *ph, UINT16_T start_address, UINT16_T data_length); + ~GroupSyncWrite() { ClearParam(); } + + PortHandler *GetPortHandler() { return port_; } + PacketHandler *GetPacketHandler() { return ph_; } + + bool AddParam (UINT8_T id, UINT8_T *data); + void RemoveParam (UINT8_T id); + bool ChangeParam (UINT8_T id, UINT8_T *data); + void ClearParam (); + + int TxPacket(); +}; + +} + + +#endif /* ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPSYNCWRITE_H_ */ diff --git a/dynamixel_sdk/include/dynamixel_sdk/PacketHandler.h b/dynamixel_sdk/include/dynamixel_sdk/PacketHandler.h new file mode 100644 index 0000000..3746831 --- /dev/null +++ b/dynamixel_sdk/include/dynamixel_sdk/PacketHandler.h @@ -0,0 +1,130 @@ +/* + * PacketHandler.h + * + * Created on: 2016. 1. 26. + * Author: zerom + */ + +#ifndef ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PACKETHANDLER_H_ +#define ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PACKETHANDLER_H_ + + +#include +#include "RobotisDef.h" +#include "PortHandler.h" + +#define BROADCAST_ID 0xFE // 254 +#define MAX_ID 0xFC // 252 + +/* Macro for Control Table Value */ +#define DXL_MAKEWORD(a, b) ((unsigned short)(((unsigned char)(((unsigned long)(a)) & 0xff)) | ((unsigned short)((unsigned char)(((unsigned long)(b)) & 0xff))) << 8)) +#define DXL_MAKEDWORD(a, b) ((unsigned int)(((unsigned short)(((unsigned long)(a)) & 0xffff)) | ((unsigned int)((unsigned short)(((unsigned long)(b)) & 0xffff))) << 16)) +#define DXL_LOWORD(l) ((unsigned short)(((unsigned long)(l)) & 0xffff)) +#define DXL_HIWORD(l) ((unsigned short)((((unsigned long)(l)) >> 16) & 0xffff)) +#define DXL_LOBYTE(w) ((unsigned char)(((unsigned long)(w)) & 0xff)) +#define DXL_HIBYTE(w) ((unsigned char)((((unsigned long)(w)) >> 8) & 0xff)) + +/* Instruction for DXL Protocol */ +#define INST_PING 1 +#define INST_READ 2 +#define INST_WRITE 3 +#define INST_REG_WRITE 4 +#define INST_ACTION 5 +#define INST_FACTORY_RESET 6 +#define INST_SYNC_WRITE 131 // 0x83 +#define INST_BULK_READ 146 // 0x92 +// --- Only for 2.0 --- // +#define INST_REBOOT 8 +#define INST_STATUS 85 // 0x55 +#define INST_SYNC_READ 130 // 0x82 +#define INST_BULK_WRITE 147 // 0x93 + +// Communication Result +#define COMM_SUCCESS 0 // tx or rx packet communication success +#define COMM_PORT_BUSY -1000 // Port is busy (in use) +#define COMM_TX_FAIL -1001 // Failed transmit instruction packet +#define COMM_RX_FAIL -1002 // Failed get status packet +#define COMM_TX_ERROR -2000 // Incorrect instruction packet +#define COMM_RX_WAITING -3000 // Now recieving status packet +#define COMM_RX_TIMEOUT -3001 // There is no status packet +#define COMM_RX_CORRUPT -3002 // Incorrect status packet +#define COMM_NOT_AVAILABLE -9000 // + +namespace ROBOTIS +{ + +class PacketHandler +{ +protected: + PacketHandler() { } + +public: + static PacketHandler *GetPacketHandler(float protocol_version = 2.0); + + virtual ~PacketHandler() { } + + virtual float GetProtocolVersion() = 0; + + virtual int TxPacket (PortHandler *port, UINT8_T *txpacket) = 0; + virtual int RxPacket (PortHandler *port, UINT8_T *rxpacket) = 0; + virtual int TxRxPacket (PortHandler *port, UINT8_T *txpacket, UINT8_T *rxpacket, UINT8_T *error = 0) = 0; + + virtual int Ping (PortHandler *port, UINT8_T id, UINT8_T *error = 0) = 0; + virtual int Ping (PortHandler *port, UINT8_T id, UINT16_T *model_number, UINT8_T *error = 0) = 0; + + // BroadcastPing + virtual int BroadcastPing (PortHandler *port, std::vector &id_list) = 0; + + virtual int Action (PortHandler *port, UINT8_T id) = 0; + virtual int Reboot (PortHandler *port, UINT8_T id, UINT8_T *error = 0) = 0; + virtual int FactoryReset (PortHandler *port, UINT8_T id, UINT8_T option = 0, UINT8_T *error = 0) = 0; + + + virtual int ReadTx (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length) = 0; + virtual int ReadRx (PortHandler *port, UINT16_T length, UINT8_T *data, UINT8_T *error = 0) = 0; + virtual int ReadTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error = 0) = 0; + + virtual int Read1ByteTx (PortHandler *port, UINT8_T id, UINT16_T address) = 0; + virtual int Read1ByteRx (PortHandler *port, UINT8_T *data, UINT8_T *error = 0) = 0; + virtual int Read1ByteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T *data, UINT8_T *error = 0) = 0; + + virtual int Read2ByteTx (PortHandler *port, UINT8_T id, UINT16_T address) = 0; + virtual int Read2ByteRx (PortHandler *port, UINT16_T *data, UINT8_T *error = 0) = 0; + virtual int Read2ByteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T *data, UINT8_T *error = 0) = 0; + + virtual int Read4ByteTx (PortHandler *port, UINT8_T id, UINT16_T address) = 0; + virtual int Read4ByteRx (PortHandler *port, UINT32_T *data, UINT8_T *error = 0) = 0; + virtual int Read4ByteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T *data, UINT8_T *error = 0) = 0; + + virtual int WriteTxOnly (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data) = 0; + virtual int WriteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error = 0) = 0; + + virtual int Write1ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T data) = 0; + virtual int Write1ByteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T data, UINT8_T *error = 0) = 0; + + virtual int Write2ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T data) = 0; + virtual int Write2ByteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T data, UINT8_T *error = 0) = 0; + + virtual int Write4ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T data) = 0; + virtual int Write4ByteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T data, UINT8_T *error = 0) = 0; + + virtual int RegWriteTxOnly (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data) = 0; + virtual int RegWriteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error = 0) = 0; + + virtual int SyncReadTx (PortHandler *port, UINT16_T start_address, UINT16_T data_length, UINT8_T *param, UINT16_T param_length) = 0; + // SyncReadRx -> GroupSyncRead class + // SyncReadTxRx -> GroupSyncRead class + + virtual int SyncWriteTxOnly (PortHandler *port, UINT16_T start_address, UINT16_T data_length, UINT8_T *param, UINT16_T param_length) = 0; + + virtual int BulkReadTx (PortHandler *port, UINT8_T *param, UINT16_T param_length) = 0; + // BulkReadRx -> GroupBulkRead class + // BulkReadTxRx -> GroupBulkRead class + + virtual int BulkWriteTxOnly (PortHandler *port, UINT8_T *param, UINT16_T param_length) = 0; +}; + +} + + +#endif /* ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PACKETHANDLER_H_ */ diff --git a/dynamixel_sdk/include/dynamixel_sdk/PortHandler.h b/dynamixel_sdk/include/dynamixel_sdk/PortHandler.h new file mode 100644 index 0000000..4406f50 --- /dev/null +++ b/dynamixel_sdk/include/dynamixel_sdk/PortHandler.h @@ -0,0 +1,51 @@ +/* + * PortHandler.h + * + * Created on: 2016. 1. 26. + * Author: zerom + */ + +#ifndef ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PORTHANDLER_H_ +#define ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PORTHANDLER_H_ + + +#include "RobotisDef.h" + +namespace ROBOTIS +{ + +class PortHandler +{ +public: + static const int DEFAULT_BAUDRATE = 1000000; + + static PortHandler *GetPortHandler(const char *port_name); + + bool is_using; + + virtual ~PortHandler() { } + + virtual bool OpenPort() = 0; + virtual void ClosePort() = 0; + virtual void ClearPort() = 0; + + virtual void SetPortName(const char* port_name) = 0; + virtual char *GetPortName() = 0; + + virtual bool SetBaudRate(const int baudrate) = 0; + virtual int GetBaudRate() = 0; + + virtual int GetBytesAvailable() = 0; + + virtual int ReadPort(UINT8_T *packet, int length) = 0; + virtual int WritePort(UINT8_T *packet, int length) = 0; + + virtual void SetPacketTimeout(UINT16_T packet_length) = 0; + virtual void SetPacketTimeout(double msec) = 0; + virtual bool IsPacketTimeout() = 0; +}; + +} + + +#endif /* ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PORTHANDLER_H_ */ diff --git a/dynamixel_sdk/include/dynamixel_sdk/Protocol1PacketHandler.h b/dynamixel_sdk/include/dynamixel_sdk/Protocol1PacketHandler.h new file mode 100644 index 0000000..a1a9634 --- /dev/null +++ b/dynamixel_sdk/include/dynamixel_sdk/Protocol1PacketHandler.h @@ -0,0 +1,95 @@ +/* + * Protocol1PacketHandler.h + * + * Created on: 2016. 1. 26. + * Author: zerom + */ + +#ifndef ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL1PACKETHANDLER_H_ +#define ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL1PACKETHANDLER_H_ + + +#include "PacketHandler.h" + +namespace ROBOTIS +{ + +class Protocol1PacketHandler : public PacketHandler +{ +private: + static Protocol1PacketHandler *unique_instance_; + + Protocol1PacketHandler(); + +public: + static Protocol1PacketHandler *GetInstance() { return unique_instance_; } + + virtual ~Protocol1PacketHandler() { } + + float GetProtocolVersion() { return 1.0; } + + int TxPacket (PortHandler *port, UINT8_T *txpacket); + int RxPacket (PortHandler *port, UINT8_T *rxpacket); + int TxRxPacket (PortHandler *port, UINT8_T *txpacket, UINT8_T *rxpacket, UINT8_T *error = 0); + + int Ping (PortHandler *port, UINT8_T id, UINT8_T *error = 0); + int Ping (PortHandler *port, UINT8_T id, UINT16_T *model_number, UINT8_T *error = 0); + + // BroadcastPing + int BroadcastPing (PortHandler *port, std::vector &id_list); + + int Action (PortHandler *port, UINT8_T id); + int Reboot (PortHandler *port, UINT8_T id, UINT8_T *error = 0); + int FactoryReset (PortHandler *port, UINT8_T id, UINT8_T option, UINT8_T *error = 0); + + + int ReadTx (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length); + int ReadRx (PortHandler *port, UINT16_T length, UINT8_T *data, UINT8_T *error = 0); + int ReadTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error = 0); + + int Read1ByteTx (PortHandler *port, UINT8_T id, UINT16_T address); + int Read1ByteRx (PortHandler *port, UINT8_T *data, UINT8_T *error = 0); + int Read1ByteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T *data, UINT8_T *error = 0); + + int Read2ByteTx (PortHandler *port, UINT8_T id, UINT16_T address); + int Read2ByteRx (PortHandler *port, UINT16_T *data, UINT8_T *error = 0); + int Read2ByteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T *data, UINT8_T *error = 0); + + int Read4ByteTx (PortHandler *port, UINT8_T id, UINT16_T address); + int Read4ByteRx (PortHandler *port, UINT32_T *data, UINT8_T *error = 0); + int Read4ByteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T *data, UINT8_T *error = 0); + + int WriteTxOnly (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data); + int WriteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error = 0); + + int Write1ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T data); + int Write1ByteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T data, UINT8_T *error = 0); + + int Write2ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T data); + int Write2ByteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T data, UINT8_T *error = 0); + + int Write4ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T data); + int Write4ByteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T data, UINT8_T *error = 0); + + int RegWriteTxOnly (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data); + int RegWriteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error = 0); + + int SyncReadTx (PortHandler *port, UINT16_T start_address, UINT16_T data_length, UINT8_T *param, UINT16_T param_length); + // SyncReadRx -> GroupSyncRead class + // SyncReadTxRx -> GroupSyncRead class + + // param : ID1 DATA0 DATA1 ... DATAn ID2 DATA0 DATA1 ... DATAn ID3 DATA0 DATA1 ... DATAn + int SyncWriteTxOnly (PortHandler *port, UINT16_T start_address, UINT16_T data_length, UINT8_T *param, UINT16_T param_length); + + // param : LEN1 ID1 ADDR1 LEN2 ID2 ADDR2 ... + int BulkReadTx (PortHandler *port, UINT8_T *param, UINT16_T param_length); + // BulkReadRx -> GroupBulkRead class + // BulkReadTxRx -> GroupBulkRead class + + int BulkWriteTxOnly (PortHandler *port, UINT8_T *param, UINT16_T param_length); +}; + +} + + +#endif /* ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL1PACKETHANDLER_H_ */ diff --git a/dynamixel_sdk/include/dynamixel_sdk/Protocol2PacketHandler.h b/dynamixel_sdk/include/dynamixel_sdk/Protocol2PacketHandler.h new file mode 100644 index 0000000..f37e9a9 --- /dev/null +++ b/dynamixel_sdk/include/dynamixel_sdk/Protocol2PacketHandler.h @@ -0,0 +1,100 @@ +/* + * Protocol2PacketHandler.h + * + * Created on: 2016. 1. 26. + * Author: zerom + */ + +#ifndef ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL2PACKETHANDLER_H_ +#define ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL2PACKETHANDLER_H_ + + +#include "PacketHandler.h" + +namespace ROBOTIS +{ + +class Protocol2PacketHandler : public PacketHandler +{ +private: + static Protocol2PacketHandler *unique_instance_; + + Protocol2PacketHandler(); + + UINT16_T UpdateCRC(UINT16_T crc_accum, UINT8_T *data_blk_ptr, UINT16_T data_blk_size); + void AddStuffing(UINT8_T *packet); + void RemoveStuffing(UINT8_T *packet); + +public: + static Protocol2PacketHandler *GetInstance() { return unique_instance_; } + + virtual ~Protocol2PacketHandler() { } + + float GetProtocolVersion() { return 2.0; } + + int TxPacket (PortHandler *port, UINT8_T *txpacket); + int RxPacket (PortHandler *port, UINT8_T *rxpacket); + int TxRxPacket (PortHandler *port, UINT8_T *txpacket, UINT8_T *rxpacket, UINT8_T *error = 0); + + int Ping (PortHandler *port, UINT8_T id, UINT8_T *error = 0); + int Ping (PortHandler *port, UINT8_T id, UINT16_T *model_number, UINT8_T *error = 0); + + // BroadcastPing + int BroadcastPing (PortHandler *port, std::vector &id_list); + + int Action (PortHandler *port, UINT8_T id); + int Reboot (PortHandler *port, UINT8_T id, UINT8_T *error = 0); + int FactoryReset (PortHandler *port, UINT8_T id, UINT8_T option, UINT8_T *error = 0); + + + int ReadTx (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length); + int ReadRx (PortHandler *port, UINT16_T length, UINT8_T *data, UINT8_T *error = 0); + int ReadTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error = 0); + + int Read1ByteTx (PortHandler *port, UINT8_T id, UINT16_T address); + int Read1ByteRx (PortHandler *port, UINT8_T *data, UINT8_T *error = 0); + int Read1ByteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T *data, UINT8_T *error = 0); + + int Read2ByteTx (PortHandler *port, UINT8_T id, UINT16_T address); + int Read2ByteRx (PortHandler *port, UINT16_T *data, UINT8_T *error = 0); + int Read2ByteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T *data, UINT8_T *error = 0); + + int Read4ByteTx (PortHandler *port, UINT8_T id, UINT16_T address); + int Read4ByteRx (PortHandler *port, UINT32_T *data, UINT8_T *error = 0); + int Read4ByteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T *data, UINT8_T *error = 0); + + int WriteTxOnly (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data); + int WriteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error = 0); + + int Write1ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T data); + int Write1ByteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T data, UINT8_T *error = 0); + + int Write2ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T data); + int Write2ByteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T data, UINT8_T *error = 0); + + int Write4ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T data); + int Write4ByteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T data, UINT8_T *error = 0); + + int RegWriteTxOnly (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data); + int RegWriteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error = 0); + + int SyncReadTx (PortHandler *port, UINT16_T start_address, UINT16_T data_length, UINT8_T *param, UINT16_T param_length); + // SyncReadRx -> GroupSyncRead class + // SyncReadTxRx -> GroupSyncRead class + + // param : ID1 DATA0 DATA1 ... DATAn ID2 DATA0 DATA1 ... DATAn ID3 DATA0 DATA1 ... DATAn + int SyncWriteTxOnly (PortHandler *port, UINT16_T start_address, UINT16_T data_length, UINT8_T *param, UINT16_T param_length); + + // param : ID1 ADDR_L1 ADDR_H1 LEN_L1 LEN_H1 ID2 ADDR_L2 ADDR_H2 LEN_L2 LEN_H2 ... + int BulkReadTx (PortHandler *port, UINT8_T *param, UINT16_T param_length); + // BulkReadRx -> GroupBulkRead class + // BulkReadTxRx -> GroupBulkRead class + + // param : ID1 START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H DATA0 DATA1 ... DATAn ID2 START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H DATA0 DATA1 ... DATAn + int BulkWriteTxOnly (PortHandler *port, UINT8_T *param, UINT16_T param_length); +}; + +} + + +#endif /* ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL2PACKETHANDLER_H_ */ diff --git a/dynamixel_sdk/include/dynamixel_sdk/RobotisDef.h b/dynamixel_sdk/include/dynamixel_sdk/RobotisDef.h new file mode 100644 index 0000000..a860e9d --- /dev/null +++ b/dynamixel_sdk/include/dynamixel_sdk/RobotisDef.h @@ -0,0 +1,21 @@ +/* + * RobotisDef.h + * + * Created on: 2016. 1. 27. + * Author: zerom + */ + +#ifndef ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_ROBOTISDEF_H_ +#define ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_ROBOTISDEF_H_ + + +typedef char INT8_T; +typedef short int INT16_T; +typedef int INT32_T; + +typedef unsigned char UINT8_T; +typedef unsigned short int UINT16_T; +typedef unsigned int UINT32_T; + + +#endif /* ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_ROBOTISDEF_H_ */ diff --git a/dynamixel_sdk/include/dynamixel_sdk_linux/PortHandlerLinux.h b/dynamixel_sdk/include/dynamixel_sdk_linux/PortHandlerLinux.h new file mode 100644 index 0000000..2f3d487 --- /dev/null +++ b/dynamixel_sdk/include/dynamixel_sdk_linux/PortHandlerLinux.h @@ -0,0 +1,62 @@ +/* + * PortHandlerLinux.h + * + * Created on: 2016. 1. 26. + * Author: zerom + */ + +#ifndef ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_LINUX_PORTHANDLERLINUX_H_ +#define ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_LINUX_PORTHANDLERLINUX_H_ + + +#include "dynamixel_sdk/PortHandler.h" + +namespace ROBOTIS +{ + +class PortHandlerLinux : public PortHandler +{ +private: + int socket_fd_; + int baudrate_; + char port_name_[30]; + + double packet_start_time_; + double packet_timeout_; + double tx_time_per_byte; + + bool SetupPort(const int cflag_baud); + bool SetCustomBaudrate(int speed); + int GetCFlagBaud(const int baudrate); + + double GetCurrentTime(); + double GetTimeSinceStart(); + +public: + PortHandlerLinux(const char *port_name); + virtual ~PortHandlerLinux() { ClosePort(); } + + bool OpenPort(); + void ClosePort(); + void ClearPort(); + + void SetPortName(const char *port_name); + char *GetPortName(); + + bool SetBaudRate(const int baudrate); + int GetBaudRate(); + + int GetBytesAvailable(); + + int ReadPort(UINT8_T *packet, int length); + int WritePort(UINT8_T *packet, int length); + + void SetPacketTimeout(UINT16_T packet_length); + void SetPacketTimeout(double msec); + bool IsPacketTimeout(); +}; + +} + + +#endif /* ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_LINUX_PORTHANDLERLINUX_H_ */ diff --git a/dynamixel_sdk/package.xml b/dynamixel_sdk/package.xml new file mode 100644 index 0000000..f21a3d3 --- /dev/null +++ b/dynamixel_sdk/package.xml @@ -0,0 +1,15 @@ + + + dynamixel_sdk + 0.1.0 + The dynamixel_sdk package + + robotis + GPLv2 + + ROBOTIS + + catkin + roscpp + roscpp + \ No newline at end of file diff --git a/dynamixel_sdk/src/dynamixel_sdk/GroupBulkRead.cpp b/dynamixel_sdk/src/dynamixel_sdk/GroupBulkRead.cpp new file mode 100644 index 0000000..a97c4cc --- /dev/null +++ b/dynamixel_sdk/src/dynamixel_sdk/GroupBulkRead.cpp @@ -0,0 +1,202 @@ +/* + * GroupBulkRead.cpp + * + * Created on: 2016. 1. 28. + * Author: zerom + */ + +#include +#include +#include "dynamixel_sdk/GroupBulkRead.h" + +using namespace ROBOTIS; + +GroupBulkRead::GroupBulkRead(PortHandler *port, PacketHandler *ph) + : port_(port), + ph_(ph), + param_(0) +{ + ClearParam(); +} + +void GroupBulkRead::MakeParam() +{ + if(id_list_.size() == 0) + return; + + if(param_ != 0) + delete[] param_; + param_ = 0; + + if(ph_->GetProtocolVersion() == 1.0) + param_ = new UINT8_T[id_list_.size() * 3]; // ID(1) + ADDR(1) + LENGTH(1) + else // 2.0 + param_ = new UINT8_T[id_list_.size() * 5]; // ID(1) + ADDR(2) + LENGTH(2) + + int _idx = 0; + for(int _i = 0; _i < id_list_.size(); _i++) + { + UINT8_T _id = id_list_[_i]; + if(ph_->GetProtocolVersion() == 1.0) + { + param_[_idx++] = (UINT8_T)length_list_[_id]; // LEN + param_[_idx++] = _id; // ID + param_[_idx++] = (UINT8_T)address_list_[_id]; // ADDR + } + else // 2.0 + { + param_[_idx++] = _id; // ID + param_[_idx++] = DXL_LOBYTE(address_list_[_id]); // ADDR_L + param_[_idx++] = DXL_HIBYTE(address_list_[_id]); // ADDR_H + param_[_idx++] = DXL_LOBYTE(length_list_[_id]); // LEN_L + param_[_idx++] = DXL_HIBYTE(length_list_[_id]); // LEN_H + } + } +} + +bool GroupBulkRead::AddParam(UINT8_T id, UINT16_T start_address, UINT16_T data_length) +{ + if(std::find(id_list_.begin(), id_list_.end(), id) != id_list_.end()) // id already exist + return false; + + id_list_.push_back(id); + length_list_[id] = data_length; + address_list_[id] = start_address; + data_list_[id] = new UINT8_T[data_length]; + + MakeParam(); + return true; +} + +void GroupBulkRead::RemoveParam(UINT8_T id) +{ + std::vector::iterator it = std::find(id_list_.begin(), id_list_.end(), id); + if(it == id_list_.end()) // NOT exist + return; + + id_list_.erase(it); + address_list_.erase(id); + length_list_.erase(id); + delete[] data_list_[id]; + data_list_.erase(id); + + MakeParam(); +} + +void GroupBulkRead::ClearParam() +{ + if(id_list_.size() != 0) + { + for(int _i = 0; _i < id_list_.size(); _i++) + delete[] data_list_[id_list_[_i]]; + } + + id_list_.clear(); + address_list_.clear(); + length_list_.clear(); + data_list_.clear(); + if(param_ != 0) + delete[] param_; + param_ = 0; +} + +int GroupBulkRead::TxPacket() +{ + if(id_list_.size() == 0) + return COMM_NOT_AVAILABLE; + + if(ph_->GetProtocolVersion() == 1.0) + return ph_->BulkReadTx(port_, param_, id_list_.size() * 3); + else // 2.0 + return ph_->BulkReadTx(port_, param_, id_list_.size() * 5); +} + +int GroupBulkRead::RxPacket() +{ + int _cnt = id_list_.size(); + int _result = COMM_RX_FAIL; + + if(_cnt == 0) + return COMM_NOT_AVAILABLE; + + for(int _i = 0; _i < _cnt; _i++) + { + UINT8_T _id = id_list_[_i]; + + _result = ph_->ReadRx(port_, length_list_[_id], data_list_[_id]); + if(_result != COMM_SUCCESS) + { + fprintf(stderr, "[GroupBulkRead::RxPacket] ID %d result : %d !!!!!!!!!!\n", _id, _result); + return _result; + } + } + + return _result; +} + +int GroupBulkRead::TxRxPacket() +{ + int _result = COMM_TX_FAIL; + + _result = TxPacket(); + if(_result != COMM_SUCCESS) + return _result; + + return RxPacket(); +} + +bool GroupBulkRead::GetData(UINT8_T id, UINT16_T address, UINT8_T *data) +{ + UINT16_T _start_addr, _data_length; + + if(data_list_.find(id) == data_list_.end()) + return false; + + _start_addr = address_list_[id]; + _data_length = length_list_[id]; + + if(address < _start_addr || _start_addr + _data_length - 1 < address) + return false; + + *data = data_list_[id][address - _start_addr]; + + return true; +} + +bool GroupBulkRead::GetData(UINT8_T id, UINT16_T address, UINT16_T *data) +{ + UINT16_T _start_addr, _data_length; + + if(data_list_.find(id) == data_list_.end()) + return false; + + _start_addr = address_list_[id]; + _data_length = length_list_[id]; + + if(address < _start_addr || _start_addr + _data_length - 2 < address) + return false; + + *data = DXL_MAKEWORD(data_list_[id][address - _start_addr], data_list_[id][address - _start_addr + 1]); + + return true; +} + +bool GroupBulkRead::GetData(UINT8_T id, UINT16_T address, UINT32_T *data) +{ + UINT16_T _start_addr, _data_length; + + if(data_list_.find(id) == data_list_.end()) + return false; + + _start_addr = address_list_[id]; + _data_length = length_list_[id]; + + if(address < _start_addr || _start_addr + _data_length - 4 < address) + return false; + + *data = DXL_MAKEDWORD(DXL_MAKEWORD(data_list_[id][address - _start_addr + 0], data_list_[id][address - _start_addr + 1]), + DXL_MAKEWORD(data_list_[id][address - _start_addr + 2], data_list_[id][address - _start_addr + 3])); + + return true; +} + diff --git a/dynamixel_sdk/src/dynamixel_sdk/GroupBulkWrite.cpp b/dynamixel_sdk/src/dynamixel_sdk/GroupBulkWrite.cpp new file mode 100644 index 0000000..7096463 --- /dev/null +++ b/dynamixel_sdk/src/dynamixel_sdk/GroupBulkWrite.cpp @@ -0,0 +1,133 @@ +/* + * GroupBulkWrite.cpp + * + * Created on: 2016. 2. 2. + * Author: zerom + */ + +#include +#include "dynamixel_sdk/GroupBulkWrite.h" + +using namespace ROBOTIS; + +GroupBulkWrite::GroupBulkWrite(PortHandler *port, PacketHandler *ph) + : port_(port), + ph_(ph), + param_(0), + param_length_(0) +{ + ClearParam(); +} + +void GroupBulkWrite::MakeParam() +{ + if(ph_->GetProtocolVersion() == 1.0 || id_list_.size() == 0) + return; + + if(param_ != 0) + delete[] param_; + param_ = 0; + + param_length_ = 0; + for(int _i = 0; _i < id_list_.size(); _i++) + param_length_ += 1 + 2 + 2 + length_list_[id_list_[_i]]; + + param_ = new UINT8_T[param_length_]; + + int _idx = 0; + for(int _i = 0; _i < id_list_.size(); _i++) + { + UINT8_T _id = id_list_[_i]; + if(data_list_[_id] == 0) + return; + + param_[_idx++] = _id; + param_[_idx++] = DXL_LOBYTE(address_list_[_id]); + param_[_idx++] = DXL_HIBYTE(address_list_[_id]); + param_[_idx++] = DXL_LOBYTE(length_list_[_id]); + param_[_idx++] = DXL_HIBYTE(length_list_[_id]); + for(int _c = 0; _c < length_list_[_id]; _c++) + param_[_idx++] = (data_list_[_id])[_c]; + } +} + +bool GroupBulkWrite::AddParam(UINT8_T id, UINT16_T start_address, UINT16_T data_length, UINT8_T *data) +{ + if(ph_->GetProtocolVersion() == 1.0) + return false; + + if(std::find(id_list_.begin(), id_list_.end(), id) != id_list_.end()) // id already exist + return false; + + id_list_.push_back(id); + address_list_[id] = start_address; + length_list_[id] = data_length; + data_list_[id] = new UINT8_T[data_length]; + for(int _c = 0; _c < data_length; _c++) + data_list_[id][_c] = data[_c]; + + MakeParam(); + return true; +} +void GroupBulkWrite::RemoveParam(UINT8_T id) +{ + if(ph_->GetProtocolVersion() == 1.0) + return; + + std::vector::iterator it = std::find(id_list_.begin(), id_list_.end(), id); + if(it == id_list_.end()) // NOT exist + return; + + id_list_.erase(it); + address_list_.erase(id); + length_list_.erase(id); + delete[] data_list_[id]; + data_list_.erase(id); + + MakeParam(); +} +bool GroupBulkWrite::ChangeParam(UINT8_T id, UINT16_T start_address, UINT16_T data_length, UINT8_T *data) +{ + if(ph_->GetProtocolVersion() == 1.0) + return false; + + std::vector::iterator it = std::find(id_list_.begin(), id_list_.end(), id); + if(it == id_list_.end()) // NOT exist + return false; + + address_list_[id] = start_address; + length_list_[id] = data_length; + delete[] data_list_[id]; + data_list_[id] = new UINT8_T[data_length]; + for(int _c = 0; _c < data_length; _c++) + data_list_[id][_c] = data[_c]; + + MakeParam(); + return true; +} +void GroupBulkWrite::ClearParam() +{ + if(ph_->GetProtocolVersion() == 1.0) + return; + + if(id_list_.size() != 0) + { + for(int _i = 0; _i < id_list_.size(); _i++) + delete[] data_list_[id_list_[_i]]; + } + + id_list_.clear(); + address_list_.clear(); + length_list_.clear(); + data_list_.clear(); + if(param_ != 0) + delete[] param_; + param_ = 0; +} +int GroupBulkWrite::TxPacket() +{ + if(ph_->GetProtocolVersion() == 1.0 || id_list_.size() == 0) + return COMM_NOT_AVAILABLE; + + return ph_->BulkWriteTxOnly(port_, param_, param_length_); +} diff --git a/dynamixel_sdk/src/dynamixel_sdk/GroupSyncRead.cpp b/dynamixel_sdk/src/dynamixel_sdk/GroupSyncRead.cpp new file mode 100644 index 0000000..5006a49 --- /dev/null +++ b/dynamixel_sdk/src/dynamixel_sdk/GroupSyncRead.cpp @@ -0,0 +1,177 @@ +/* + * GroupSyncRead.cpp + * + * Created on: 2016. 2. 2. + * Author: zerom + */ + +#include +#include "dynamixel_sdk/GroupSyncRead.h" + +using namespace ROBOTIS; + +GroupSyncRead::GroupSyncRead(PortHandler *port, PacketHandler *ph, UINT16_T start_address, UINT16_T data_length) + : port_(port), + ph_(ph), + param_(0), + start_address_(start_address), + data_length_(data_length) +{ + ClearParam(); +} + +void GroupSyncRead::MakeParam() +{ + if(ph_->GetProtocolVersion() == 1.0 || id_list_.size() == 0) + return; + + if(param_ != 0) + delete[] param_; + param_ = 0; + + param_ = new UINT8_T[id_list_.size() * 1]; // ID(1) + + int _idx = 0; + for(int _i = 0; _i < id_list_.size(); _i++) + param_[_idx++] = id_list_[_i]; +} + +bool GroupSyncRead::AddParam(UINT8_T id) +{ + if(ph_->GetProtocolVersion() == 1.0) + return false; + + if(std::find(id_list_.begin(), id_list_.end(), id) != id_list_.end()) // id already exist + return false; + + id_list_.push_back(id); + data_list_[id] = new UINT8_T[data_length_]; + + MakeParam(); + return true; +} +void GroupSyncRead::RemoveParam(UINT8_T id) +{ + if(ph_->GetProtocolVersion() == 1.0) + return; + + std::vector::iterator it = std::find(id_list_.begin(), id_list_.end(), id); + if(it == id_list_.end()) // NOT exist + return; + + id_list_.erase(it); + delete[] data_list_[id]; + data_list_.erase(id); + + MakeParam(); +} +void GroupSyncRead::ClearParam() +{ + if(ph_->GetProtocolVersion() == 1.0) + return; + + if(id_list_.size() != 0) + { + for(int _i = 0; _i < id_list_.size(); _i++) + delete[] data_list_[id_list_[_i]]; + } + + id_list_.clear(); + data_list_.clear(); + if(param_ != 0) + delete[] param_; + param_ = 0; +} + +int GroupSyncRead::TxPacket() +{ + if(ph_->GetProtocolVersion() == 1.0 || id_list_.size() == 0) + return COMM_NOT_AVAILABLE; + + return ph_->SyncReadTx(port_, start_address_, data_length_, param_, (UINT16_T)id_list_.size() * 1); +} + +int GroupSyncRead::RxPacket() +{ + if(ph_->GetProtocolVersion() == 1.0) + return COMM_NOT_AVAILABLE; + + int _cnt = id_list_.size(); + int _result = COMM_RX_FAIL; + + if(_cnt == 0) + return COMM_NOT_AVAILABLE; + + for(int _i = 0; _i < _cnt; _i++) + { + UINT8_T _id = id_list_[_i]; + + _result = ph_->ReadRx(port_, data_length_, data_list_[_id]); + if(_result != COMM_SUCCESS) + return _result; + } + + return _result; +} + +int GroupSyncRead::TxRxPacket() +{ + if(ph_->GetProtocolVersion() == 1.0) + return COMM_NOT_AVAILABLE; + + int _result = COMM_TX_FAIL; + + _result = TxPacket(); + if(_result != COMM_SUCCESS) + return _result; + + return RxPacket(); +} + +bool GroupSyncRead::GetData(UINT8_T id, UINT16_T address, UINT8_T *data) +{ + if(ph_->GetProtocolVersion() == 1.0) + return false; + + if(data_list_.find(id) == data_list_.end()) + return false; + + if(address < start_address_ || start_address_ + data_length_ - 1 < address) + return false; + + *data = data_list_[id][address - start_address_]; + + return true; +} +bool GroupSyncRead::GetData(UINT8_T id, UINT16_T address, UINT16_T *data) +{ + if(ph_->GetProtocolVersion() == 1.0) + return false; + + if(data_list_.find(id) == data_list_.end()) + return false; + + if(address < start_address_ || start_address_ + data_length_ - 2 < address) + return false; + + *data = DXL_MAKEWORD(data_list_[id][address - start_address_], data_list_[id][address - start_address_ + 1]); + + return true; +} +bool GroupSyncRead::GetData(UINT8_T id, UINT16_T address, UINT32_T *data) +{ + if(ph_->GetProtocolVersion() == 1.0) + return false; + + if(data_list_.find(id) == data_list_.end()) + return false; + + if(address < start_address_ || start_address_ + data_length_ - 4 < address) + return false; + + *data = DXL_MAKEDWORD(DXL_MAKEWORD(data_list_[id][address - start_address_ + 0], data_list_[id][address - start_address_ + 1]), + DXL_MAKEWORD(data_list_[id][address - start_address_ + 2], data_list_[id][address - start_address_ + 3])); + + return true; +} + diff --git a/dynamixel_sdk/src/dynamixel_sdk/GroupSyncWrite.cpp b/dynamixel_sdk/src/dynamixel_sdk/GroupSyncWrite.cpp new file mode 100644 index 0000000..ac17423 --- /dev/null +++ b/dynamixel_sdk/src/dynamixel_sdk/GroupSyncWrite.cpp @@ -0,0 +1,110 @@ +/* + * GroupSyncWrite.cpp + * + * Created on: 2016. 1. 28. + * Author: zerom + */ + +#include +#include "dynamixel_sdk/GroupSyncWrite.h" + +using namespace ROBOTIS; + +GroupSyncWrite::GroupSyncWrite(PortHandler *port, PacketHandler *ph, UINT16_T start_address, UINT16_T data_length) + : port_(port), + ph_(ph), + param_(0), + start_address_(start_address), + data_length_(data_length) +{ + ClearParam(); +} + +void GroupSyncWrite::MakeParam() +{ + if(id_list_.size() == 0) + return; + + if(param_ != 0) + delete[] param_; + param_ = 0; + + param_ = new UINT8_T[id_list_.size() * (1 + data_length_)]; // ID(1) + DATA(data_length) + + int _idx = 0; + for(int _i = 0; _i < id_list_.size(); _i++) + { + UINT8_T _id = id_list_[_i]; + if(data_list_[_id] == 0) + return; + + param_[_idx++] = _id; + for(int _c = 0; _c < data_length_; _c++) + param_[_idx++] = (data_list_[_id])[_c]; + } +} + +bool GroupSyncWrite::AddParam(UINT8_T id, UINT8_T *data) +{ + if(std::find(id_list_.begin(), id_list_.end(), id) != id_list_.end()) // id already exist + return false; + + id_list_.push_back(id); + data_list_[id] = new UINT8_T[data_length_]; + for(int _c = 0; _c < data_length_; _c++) + data_list_[id][_c] = data[_c]; + + MakeParam(); + return true; +} + +void GroupSyncWrite::RemoveParam(UINT8_T id) +{ + std::vector::iterator it = std::find(id_list_.begin(), id_list_.end(), id); + if(it == id_list_.end()) // NOT exist + return; + + id_list_.erase(it); + delete[] data_list_[id]; + data_list_.erase(id); + + MakeParam(); +} + +bool GroupSyncWrite::ChangeParam(UINT8_T id, UINT8_T *data) +{ + std::vector::iterator it = std::find(id_list_.begin(), id_list_.end(), id); + if(it == id_list_.end()) // NOT exist + return false; + + delete[] data_list_[id]; + data_list_[id] = new UINT8_T[data_length_]; + for(int _c = 0; _c < data_length_; _c++) + data_list_[id][_c] = data[_c]; + + MakeParam(); + return true; +} + +void GroupSyncWrite::ClearParam() +{ + if(id_list_.size() != 0) + { + for(int _i = 0; _i < id_list_.size(); _i++) + delete[] data_list_[id_list_[_i]]; + } + + id_list_.clear(); + data_list_.clear(); + if(param_ != 0) + delete[] param_; + param_ = 0; +} + +int GroupSyncWrite::TxPacket() +{ + if(id_list_.size() == 0) + return COMM_NOT_AVAILABLE; + + return ph_->SyncWriteTxOnly(port_, start_address_, data_length_, param_, id_list_.size() * (1 + data_length_)); +} diff --git a/dynamixel_sdk/src/dynamixel_sdk/PacketHandler.cpp b/dynamixel_sdk/src/dynamixel_sdk/PacketHandler.cpp new file mode 100644 index 0000000..57579b0 --- /dev/null +++ b/dynamixel_sdk/src/dynamixel_sdk/PacketHandler.cpp @@ -0,0 +1,22 @@ +/* + * PacketHandler.cpp + * + * Created on: 2016. 1. 26. + * Author: zerom + */ + +#include "dynamixel_sdk/PacketHandler.h" +#include "dynamixel_sdk/Protocol1PacketHandler.h" +#include "dynamixel_sdk/Protocol2PacketHandler.h" + +using namespace ROBOTIS; + +PacketHandler *PacketHandler::GetPacketHandler(float protocol_version) +{ + if(protocol_version == 1.0) + return (PacketHandler *)(Protocol1PacketHandler::GetInstance()); + else if(protocol_version == 2.0) + return (PacketHandler *)(Protocol2PacketHandler::GetInstance()); + + return (PacketHandler *)(Protocol2PacketHandler::GetInstance()); +} diff --git a/dynamixel_sdk/src/dynamixel_sdk/PortHandler.cpp b/dynamixel_sdk/src/dynamixel_sdk/PortHandler.cpp new file mode 100644 index 0000000..2f1b5b0 --- /dev/null +++ b/dynamixel_sdk/src/dynamixel_sdk/PortHandler.cpp @@ -0,0 +1,31 @@ +/* + * PortHandler.cpp + * + * Created on: 2016. 2. 5. + * Author: zerom + */ + +#include "dynamixel_sdk/PortHandler.h" + +#ifdef __linux__ + #include "dynamixel_sdk_linux/PortHandlerLinux.h" +#endif + +#ifdef _WIN32 || _WIN64 + #include "dynamixel_sdk_windows/PortHandlerWindows.h" +#endif + +using namespace ROBOTIS; + +PortHandler *PortHandler::GetPortHandler(const char *port_name) +{ +#ifdef __linux__ + return (PortHandler *)(new PortHandlerLinux(port_name)); +#endif + +#ifdef _WIN32 || _WIN64 + return (PortHandler *)(new PortHandlerWindows(port_name)); +#endif +} + + diff --git a/dynamixel_sdk/src/dynamixel_sdk/Protocol1PacketHandler.cpp b/dynamixel_sdk/src/dynamixel_sdk/Protocol1PacketHandler.cpp new file mode 100644 index 0000000..a1a52b6 --- /dev/null +++ b/dynamixel_sdk/src/dynamixel_sdk/Protocol1PacketHandler.cpp @@ -0,0 +1,562 @@ +/* + * Protocol1PacketHandler.cpp + * + * Created on: 2016. 1. 26. + * Author: zerom + */ + +#include +#include +#include "dynamixel_sdk/Protocol1PacketHandler.h" + +#define TXPACKET_MAX_LEN (250) +#define RXPACKET_MAX_LEN (250) + +///////////////// for Protocol 1.0 Packet ///////////////// +#define PKT_HEADER0 0 +#define PKT_HEADER1 1 +#define PKT_ID 2 +#define PKT_LENGTH 3 +#define PKT_INSTRUCTION 4 +#define PKT_ERROR 4 +#define PKT_PARAMETER0 5 + +///////////////// Protocol 1.0 Error bit ///////////////// +#define ERRBIT_VOLTAGE 1 // Supplied voltage is out of the range (operating volatage set in the control table) +#define ERRBIT_ANGLE 2 // Goal position is written out of the range (from CW angle limit to CCW angle limit) +#define ERRBIT_OVERHEAT 4 // Temperature is out of the range (operating temperature set in the control table) +#define ERRBIT_RANGE 8 // Command(setting value) is out of the range for use. +#define ERRBIT_CHECKSUM 16 // Instruction packet checksum is incorrect. +#define ERRBIT_OVERLOAD 32 // The current load cannot be controlled by the set torque. +#define ERRBIT_INSTRUCTION 64 // Undefined instruction or delivering the action command without the reg_write command. + +using namespace ROBOTIS; + +Protocol1PacketHandler *Protocol1PacketHandler::unique_instance_ = new Protocol1PacketHandler(); + +Protocol1PacketHandler::Protocol1PacketHandler() { } + +int Protocol1PacketHandler::TxPacket(PortHandler *port, UINT8_T *txpacket) +{ + UINT8_T _checksum = 0; + UINT8_T _total_packet_length = txpacket[PKT_LENGTH] + 4; // 4: HEADER0 HEADER1 ID LENGTH + UINT8_T _written_packet_length = 0; + + if(port->is_using) + return COMM_PORT_BUSY; + port->is_using = true; + + // check max packet length + if(_total_packet_length > TXPACKET_MAX_LEN) + { + port->is_using = false; + return COMM_TX_ERROR; + } + + // make packet header + txpacket[PKT_HEADER0] = 0xFF; + txpacket[PKT_HEADER1] = 0xFF; + + // add a checksum to the packet + for(int _idx = 2; _idx < _total_packet_length - 1; _idx++) // except header, checksum + _checksum += txpacket[_idx]; + txpacket[_total_packet_length - 1] = ~_checksum; + + // tx packet + port->ClearPort(); + _written_packet_length = port->WritePort(txpacket, _total_packet_length); + if(_total_packet_length != _written_packet_length) + { + port->is_using = false; + return COMM_TX_FAIL; + } + + return COMM_SUCCESS; +} + +int Protocol1PacketHandler::RxPacket(PortHandler *port, UINT8_T *rxpacket) +{ + int _result = COMM_TX_FAIL; + + UINT8_T _checksum = 0; + UINT8_T _rx_length = 0; + UINT8_T _wait_length = 6; // minimum length ( HEADER0 HEADER1 ID LENGTH ERROR CHKSUM ) + + while(true) + { + _rx_length += port->ReadPort(&rxpacket[_rx_length], _wait_length - _rx_length); + if(_rx_length >= _wait_length) + { + UINT8_T _idx = 0; + + // find packet header + for(_idx = 0; _idx < (_rx_length - 1); _idx++) + { + if(rxpacket[_idx] == 0xFF && rxpacket[_idx+1] == 0xFF) + break; + } + + if(_idx == 0) // found at the beginning of the packet + { + // re-calculate the exact length of the rx packet + _wait_length = rxpacket[PKT_LENGTH] + PKT_LENGTH + 1; + if(_rx_length < _wait_length) + { + // check timeout + if(port->IsPacketTimeout() == true) + { + if(_rx_length == 0) + _result = COMM_RX_TIMEOUT; + else + _result = COMM_RX_CORRUPT; + break; + } + else + continue; + } + + // calculate checksum + for(int _i = 2; _i < _wait_length - 1; _i++) // except header, checksum + _checksum += rxpacket[_i]; + _checksum = ~_checksum; + + // verify checksum + if(rxpacket[_wait_length - 1] == _checksum) + _result = COMM_SUCCESS; + else + _result = COMM_RX_CORRUPT; + break; + } + else + { + // remove unnecessary packets + for(UINT8_T _s = 0; _s < _rx_length - _idx; _s++) + rxpacket[_s] = rxpacket[_idx + _s]; + //memcpy(&rxpacket[0], &rxpacket[_idx], _rx_length - _idx); + _rx_length -= _idx; + } + } + else + { + // check timeout + if(port->IsPacketTimeout() == true) + { + if(_rx_length == 0) + _result = COMM_RX_TIMEOUT; + else + _result = COMM_RX_CORRUPT; + break; + } + } + } + port->is_using = false; + + return _result; +} + +// NOT for BulkRead instruction +int Protocol1PacketHandler::TxRxPacket(PortHandler *port, UINT8_T *txpacket, UINT8_T *rxpacket, UINT8_T *error) +{ + int _result = COMM_TX_FAIL; + + // tx packet + _result = TxPacket(port, txpacket); + if(_result != COMM_SUCCESS) + return _result; + + // (ID == Broadcast ID && NOT BulkRead) == no need to wait for status packet + // (Instruction == Action) == no need to wait for status packet + if((txpacket[PKT_ID] == BROADCAST_ID && txpacket[PKT_INSTRUCTION] != INST_BULK_READ) || + (txpacket[PKT_INSTRUCTION] == INST_ACTION)) + { + port->is_using = false; + return _result; + } + + // set packet timeout + if(txpacket[PKT_INSTRUCTION] == INST_READ) + port->SetPacketTimeout((UINT16_T)(txpacket[PKT_PARAMETER0+1] + 6)); + else + port->SetPacketTimeout((UINT16_T)6); + + // rx packet + _result = RxPacket(port, rxpacket); + // TODO: check txpacket ID == rxpacket ID + + if(_result == COMM_SUCCESS && txpacket[PKT_ID] != BROADCAST_ID) + { + if(error != 0) + *error = (UINT8_T)rxpacket[PKT_ERROR]; + } + + return _result; +} + +int Protocol1PacketHandler::Ping(PortHandler *port, UINT8_T id, UINT8_T *error) +{ + return Ping(port, id, 0, error); +} + +int Protocol1PacketHandler::Ping(PortHandler *port, UINT8_T id, UINT16_T *model_number, UINT8_T *error) +{ + int _result = COMM_TX_FAIL; + + UINT8_T txpacket[6] = {0}; + UINT8_T rxpacket[6] = {0}; + + if(id >= BROADCAST_ID) + return COMM_NOT_AVAILABLE; + + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH] = 2; + txpacket[PKT_INSTRUCTION] = INST_PING; + + _result = TxRxPacket(port, txpacket, rxpacket, error); + if(_result == COMM_SUCCESS && model_number != 0) + { + UINT8_T _data[2] = {0}; + _result = ReadTxRx(port, id, 0, 2, _data); // Address 0 : Model Number + if(_result == COMM_SUCCESS) + *model_number = DXL_MAKEWORD(_data[0], _data[1]); + } + + return _result; +} + +int Protocol1PacketHandler::BroadcastPing(PortHandler *port, std::vector &id_list) +{ + return COMM_NOT_AVAILABLE; +} + +int Protocol1PacketHandler::Action(PortHandler *port, UINT8_T id) +{ + UINT8_T txpacket[6] = {0}; + + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH] = 2; + txpacket[PKT_INSTRUCTION] = INST_ACTION; + + return TxRxPacket(port, txpacket, 0); +} + +int Protocol1PacketHandler::Reboot(PortHandler *port, UINT8_T id, UINT8_T *error) +{ + return COMM_NOT_AVAILABLE; +} + +int Protocol1PacketHandler::FactoryReset(PortHandler *port, UINT8_T id, UINT8_T option, UINT8_T *error) +{ + UINT8_T txpacket[6] = {0}; + UINT8_T rxpacket[6] = {0}; + + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH] = 2; + txpacket[PKT_INSTRUCTION] = INST_FACTORY_RESET; + + return TxRxPacket(port, txpacket, rxpacket, error); +} + +int Protocol1PacketHandler::ReadTx(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length) +{ + int _result = COMM_TX_FAIL; + + UINT8_T txpacket[8] = {0}; + + if(id >= BROADCAST_ID) + return COMM_NOT_AVAILABLE; + + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH] = 4; + txpacket[PKT_INSTRUCTION] = INST_READ; + txpacket[PKT_PARAMETER0+0] = (UINT8_T)address; + txpacket[PKT_PARAMETER0+1] = (UINT8_T)length; + + _result = TxPacket(port, txpacket); + + // set packet timeout + if(_result == COMM_SUCCESS) + port->SetPacketTimeout((UINT16_T)(length+6)); + + return _result; +} + +int Protocol1PacketHandler::ReadRx(PortHandler *port, UINT16_T length, UINT8_T *data, UINT8_T *error) +{ + int _result = COMM_TX_FAIL; + UINT8_T *rxpacket = (UINT8_T *)malloc(RXPACKET_MAX_LEN);//(length+6); + //UINT8_T *rxpacket = new UINT8_T[length+6]; + + _result = RxPacket(port, rxpacket); + if(_result == COMM_SUCCESS) + { + if(error != 0) + *error = (UINT8_T)rxpacket[PKT_ERROR]; + for(UINT8_T _s = 0; _s < length; _s++) + data[_s] = rxpacket[PKT_PARAMETER0 + _s]; + //memcpy(data, &rxpacket[PKT_PARAMETER0], length); + } + + free(rxpacket); + //delete[] rxpacket; + return _result; +} + +int Protocol1PacketHandler::ReadTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error) +{ + int _result = COMM_TX_FAIL; + + _result = ReadTx(port, id, address, length); + if(_result != COMM_SUCCESS) + return _result; + + return ReadRx(port, length, data, error); +} + +int Protocol1PacketHandler::Read1ByteTx(PortHandler *port, UINT8_T id, UINT16_T address) +{ + return ReadTx(port, id, address, 1); +} +int Protocol1PacketHandler::Read1ByteRx(PortHandler *port, UINT8_T *data, UINT8_T *error) +{ + UINT8_T _data[1] = {0}; + int _result = ReadRx(port, 1, _data, error); + *data = _data[0]; + return _result; +} +int Protocol1PacketHandler::Read1ByteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T *data, UINT8_T *error) +{ + int _result = COMM_TX_FAIL; + + _result = Read1ByteTx(port, id, address); + if(_result != COMM_SUCCESS) + return _result; + + return Read1ByteRx(port, data, error); +} + +int Protocol1PacketHandler::Read2ByteTx(PortHandler *port, UINT8_T id, UINT16_T address) +{ + return ReadTx(port, id, address, 2); +} +int Protocol1PacketHandler::Read2ByteRx(PortHandler *port, UINT16_T *data, UINT8_T *error) +{ + UINT8_T _data[2] = {0}; + int _result = ReadRx(port, 2, _data, error); + *data = DXL_MAKEWORD(_data[0], _data[1]); + return _result; +} +int Protocol1PacketHandler::Read2ByteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T *data, UINT8_T *error) +{ + int _result = COMM_TX_FAIL; + + _result = Read2ByteTx(port, id, address); + if(_result != COMM_SUCCESS) + return _result; + + return Read2ByteRx(port, data, error); +} + +int Protocol1PacketHandler::Read4ByteTx(PortHandler *port, UINT8_T id, UINT16_T address) +{ + return COMM_NOT_AVAILABLE; +} +int Protocol1PacketHandler::Read4ByteRx(PortHandler *port, UINT32_T *data, UINT8_T *error) +{ + return COMM_NOT_AVAILABLE; +} +int Protocol1PacketHandler::Read4ByteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T *data, UINT8_T *error) +{ + return COMM_NOT_AVAILABLE; +} + +int Protocol1PacketHandler::WriteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data) +{ + int _result = COMM_TX_FAIL; + + UINT8_T *txpacket = (UINT8_T *)malloc(length+7); + //UINT8_T *txpacket = new UINT8_T[length+7]; + + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH] = length+3; + txpacket[PKT_INSTRUCTION] = INST_WRITE; + txpacket[PKT_PARAMETER0] = (UINT8_T)address; + + for(UINT8_T _s = 0; _s < length; _s++) + txpacket[PKT_PARAMETER0+1+_s] = data[_s]; + //memcpy(&txpacket[PKT_PARAMETER0+1], data, length); + + _result = TxPacket(port, txpacket); + port->is_using = false; + + free(txpacket); + //delete[] txpacket; + return _result; +} + +int Protocol1PacketHandler::WriteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error) +{ + int _result = COMM_TX_FAIL; + + UINT8_T *txpacket = (UINT8_T *)malloc(length+6); + //UINT8_T *txpacket = new UINT8_T[length+6]; + UINT8_T rxpacket[6] = {0}; + + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH] = length+3; + txpacket[PKT_INSTRUCTION] = INST_WRITE; + txpacket[PKT_PARAMETER0] = (UINT8_T)address; + + for(UINT8_T _s = 0; _s < length; _s++) + txpacket[PKT_PARAMETER0+1+_s] = data[_s]; + //memcpy(&txpacket[PKT_PARAMETER0+1], data, length); + + _result = TxRxPacket(port, txpacket, rxpacket, error); + + free(txpacket); + //delete[] txpacket; + return _result; +} + +int Protocol1PacketHandler::Write1ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T data) +{ + UINT8_T _data[1] = { data }; + return WriteTxOnly(port, id, address, 1, _data); +} +int Protocol1PacketHandler::Write1ByteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T data, UINT8_T *error) +{ + UINT8_T _data[1] = { data }; + return WriteTxRx(port, id, address, 1, _data, error); +} + +int Protocol1PacketHandler::Write2ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T data) +{ + UINT8_T _data[2] = { DXL_LOBYTE(data), DXL_HIBYTE(data) }; + return WriteTxOnly(port, id, address, 2, _data); +} +int Protocol1PacketHandler::Write2ByteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T data, UINT8_T *error) +{ + UINT8_T _data[2] = { DXL_LOBYTE(data), DXL_HIBYTE(data) }; + return WriteTxRx(port, id, address, 2, _data, error); +} + +int Protocol1PacketHandler::Write4ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T data) +{ + return COMM_NOT_AVAILABLE; +} +int Protocol1PacketHandler::Write4ByteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T data, UINT8_T *error) +{ + return COMM_NOT_AVAILABLE; +} + +int Protocol1PacketHandler::RegWriteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data) +{ + int _result = COMM_TX_FAIL; + + UINT8_T *txpacket = (UINT8_T *)malloc(length+6); + //UINT8_T *txpacket = new UINT8_T[length+6]; + + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH] = length+3; + txpacket[PKT_INSTRUCTION] = INST_REG_WRITE; + txpacket[PKT_PARAMETER0] = (UINT8_T)address; + + for(UINT8_T _s = 0; _s < length; _s++) + txpacket[PKT_PARAMETER0+1+_s] = data[_s]; + //memcpy(&txpacket[PKT_PARAMETER0+1], data, length); + + _result = TxPacket(port, txpacket); + port->is_using = false; + + free(txpacket); + //delete[] txpacket; + return _result; +} + +int Protocol1PacketHandler::RegWriteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error) +{ + int _result = COMM_TX_FAIL; + + UINT8_T *txpacket = (UINT8_T *)malloc(length+6); + //UINT8_T *txpacket = new UINT8_T[length+6]; + UINT8_T rxpacket[6] = {0}; + + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH] = length+3; + txpacket[PKT_INSTRUCTION] = INST_REG_WRITE; + txpacket[PKT_PARAMETER0] = (UINT8_T)address; + + for(UINT8_T _s = 0; _s < length; _s++) + txpacket[PKT_PARAMETER0+1+_s] = data[_s]; + //memcpy(&txpacket[PKT_PARAMETER0+1], data, length); + + _result = TxRxPacket(port, txpacket, rxpacket, error); + + free(txpacket); + //delete[] txpacket; + return _result; +} + +int Protocol1PacketHandler::SyncReadTx(PortHandler *port, UINT16_T start_address, UINT16_T data_length, UINT8_T *param, UINT16_T param_length) +{ + return COMM_NOT_AVAILABLE; +} + +int Protocol1PacketHandler::SyncWriteTxOnly(PortHandler *port, UINT16_T start_address, UINT16_T data_length, UINT8_T *param, UINT16_T param_length) +{ + int _result = COMM_TX_FAIL; + + UINT8_T *txpacket = (UINT8_T *)malloc(param_length+8); // 8: HEADER0 HEADER1 ID LEN INST START_ADDR DATA_LEN ... CHKSUM + //UINT8_T *txpacket = new UINT8_T[param_length + 8]; // 8: HEADER0 HEADER1 ID LEN INST START_ADDR DATA_LEN ... CHKSUM + + txpacket[PKT_ID] = BROADCAST_ID; + txpacket[PKT_LENGTH] = param_length + 4; // 4: INST START_ADDR DATA_LEN ... CHKSUM + txpacket[PKT_INSTRUCTION] = INST_SYNC_WRITE; + txpacket[PKT_PARAMETER0+0] = start_address; + txpacket[PKT_PARAMETER0+1] = data_length; + + for(UINT8_T _s = 0; _s < param_length; _s++) + txpacket[PKT_PARAMETER0+2+_s] = param[_s]; + //memcpy(&txpacket[PKT_PARAMETER0+2], param, param_length); + + _result = TxRxPacket(port, txpacket, 0, 0); + + free(txpacket); + //delete[] txpacket; + return _result; +} + +int Protocol1PacketHandler::BulkReadTx(PortHandler *port, UINT8_T *param, UINT16_T param_length) +{ + int _result = COMM_TX_FAIL; + + UINT8_T *txpacket = (UINT8_T *)malloc(param_length+7); // 7: HEADER0 HEADER1 ID LEN INST 0x00 ... CHKSUM + //UINT8_T *txpacket = new UINT8_T[param_length + 7]; // 7: HEADER0 HEADER1 ID LEN INST 0x00 ... CHKSUM + + txpacket[PKT_ID] = BROADCAST_ID; + txpacket[PKT_LENGTH] = param_length + 3; // 3: INST 0x00 ... CHKSUM + txpacket[PKT_INSTRUCTION] = INST_BULK_READ; + txpacket[PKT_PARAMETER0+0] = 0x00; + + for(UINT8_T _s = 0; _s < param_length; _s++) + txpacket[PKT_PARAMETER0+1+_s] = param[_s]; + //memcpy(&txpacket[PKT_PARAMETER0+1], param, param_length); + + _result = TxPacket(port, txpacket); + if(_result == COMM_SUCCESS) + { + int _wait_length = 0; + for(int _i = 0; _i < param_length; _i += 3) + _wait_length += param[_i] + 7; + port->SetPacketTimeout((UINT16_T)_wait_length); + } + + free(txpacket); + //delete[] txpacket; + return _result; +} + +int Protocol1PacketHandler::BulkWriteTxOnly(PortHandler *port, UINT8_T *param, UINT16_T param_length) +{ + return COMM_NOT_AVAILABLE; +} + diff --git a/dynamixel_sdk/src/dynamixel_sdk/Protocol2PacketHandler.cpp b/dynamixel_sdk/src/dynamixel_sdk/Protocol2PacketHandler.cpp new file mode 100644 index 0000000..3cd3200 --- /dev/null +++ b/dynamixel_sdk/src/dynamixel_sdk/Protocol2PacketHandler.cpp @@ -0,0 +1,898 @@ +/* + * Protocol2PacketHandler.cpp + * + * Created on: 2016. 1. 26. + * Author: zerom + */ + +#include +#include +#include +#include "dynamixel_sdk/Protocol2PacketHandler.h" + +#define TXPACKET_MAX_LEN (4*1024) +#define RXPACKET_MAX_LEN (4*1024) + +///////////////// for Protocol 2.0 Packet ///////////////// +#define PKT_HEADER0 0 +#define PKT_HEADER1 1 +#define PKT_HEADER2 2 +#define PKT_RESERVED 3 +#define PKT_ID 4 +#define PKT_LENGTH_L 5 +#define PKT_LENGTH_H 6 +#define PKT_INSTRUCTION 7 +#define PKT_ERROR 8 +#define PKT_PARAMETER0 8 + +///////////////// Protocol 2.0 Error bit ///////////////// +#define ERRNUM_RESULT_FAIL 1 // Failed to process the instruction packet. +#define ERRNUM_INSTRUCTION 2 // Instruction error +#define ERRNUM_CRC 3 // CRC check error +#define ERRNUM_DATA_RANGE 4 // Data range error +#define ERRNUM_DATA_LENGTH 5 // Data length error +#define ERRNUM_DATA_LIMIT 6 // Data limit error +#define ERRNUM_ACCESS 7 // Access error + +#define ERRBIT_ALERT 128 //When the device has a problem, this bit is set to 1. Check "Device Status Check" value. + +using namespace ROBOTIS; + +Protocol2PacketHandler *Protocol2PacketHandler::unique_instance_ = new Protocol2PacketHandler(); + +Protocol2PacketHandler::Protocol2PacketHandler() { } + +unsigned short Protocol2PacketHandler::UpdateCRC(UINT16_T crc_accum, UINT8_T *data_blk_ptr, UINT16_T data_blk_size) +{ + UINT16_T i, j; + UINT16_T crc_table[256] = {0x0000, + 0x8005, 0x800F, 0x000A, 0x801B, 0x001E, 0x0014, 0x8011, + 0x8033, 0x0036, 0x003C, 0x8039, 0x0028, 0x802D, 0x8027, + 0x0022, 0x8063, 0x0066, 0x006C, 0x8069, 0x0078, 0x807D, + 0x8077, 0x0072, 0x0050, 0x8055, 0x805F, 0x005A, 0x804B, + 0x004E, 0x0044, 0x8041, 0x80C3, 0x00C6, 0x00CC, 0x80C9, + 0x00D8, 0x80DD, 0x80D7, 0x00D2, 0x00F0, 0x80F5, 0x80FF, + 0x00FA, 0x80EB, 0x00EE, 0x00E4, 0x80E1, 0x00A0, 0x80A5, + 0x80AF, 0x00AA, 0x80BB, 0x00BE, 0x00B4, 0x80B1, 0x8093, + 0x0096, 0x009C, 0x8099, 0x0088, 0x808D, 0x8087, 0x0082, + 0x8183, 0x0186, 0x018C, 0x8189, 0x0198, 0x819D, 0x8197, + 0x0192, 0x01B0, 0x81B5, 0x81BF, 0x01BA, 0x81AB, 0x01AE, + 0x01A4, 0x81A1, 0x01E0, 0x81E5, 0x81EF, 0x01EA, 0x81FB, + 0x01FE, 0x01F4, 0x81F1, 0x81D3, 0x01D6, 0x01DC, 0x81D9, + 0x01C8, 0x81CD, 0x81C7, 0x01C2, 0x0140, 0x8145, 0x814F, + 0x014A, 0x815B, 0x015E, 0x0154, 0x8151, 0x8173, 0x0176, + 0x017C, 0x8179, 0x0168, 0x816D, 0x8167, 0x0162, 0x8123, + 0x0126, 0x012C, 0x8129, 0x0138, 0x813D, 0x8137, 0x0132, + 0x0110, 0x8115, 0x811F, 0x011A, 0x810B, 0x010E, 0x0104, + 0x8101, 0x8303, 0x0306, 0x030C, 0x8309, 0x0318, 0x831D, + 0x8317, 0x0312, 0x0330, 0x8335, 0x833F, 0x033A, 0x832B, + 0x032E, 0x0324, 0x8321, 0x0360, 0x8365, 0x836F, 0x036A, + 0x837B, 0x037E, 0x0374, 0x8371, 0x8353, 0x0356, 0x035C, + 0x8359, 0x0348, 0x834D, 0x8347, 0x0342, 0x03C0, 0x83C5, + 0x83CF, 0x03CA, 0x83DB, 0x03DE, 0x03D4, 0x83D1, 0x83F3, + 0x03F6, 0x03FC, 0x83F9, 0x03E8, 0x83ED, 0x83E7, 0x03E2, + 0x83A3, 0x03A6, 0x03AC, 0x83A9, 0x03B8, 0x83BD, 0x83B7, + 0x03B2, 0x0390, 0x8395, 0x839F, 0x039A, 0x838B, 0x038E, + 0x0384, 0x8381, 0x0280, 0x8285, 0x828F, 0x028A, 0x829B, + 0x029E, 0x0294, 0x8291, 0x82B3, 0x02B6, 0x02BC, 0x82B9, + 0x02A8, 0x82AD, 0x82A7, 0x02A2, 0x82E3, 0x02E6, 0x02EC, + 0x82E9, 0x02F8, 0x82FD, 0x82F7, 0x02F2, 0x02D0, 0x82D5, + 0x82DF, 0x02DA, 0x82CB, 0x02CE, 0x02C4, 0x82C1, 0x8243, + 0x0246, 0x024C, 0x8249, 0x0258, 0x825D, 0x8257, 0x0252, + 0x0270, 0x8275, 0x827F, 0x027A, 0x826B, 0x026E, 0x0264, + 0x8261, 0x0220, 0x8225, 0x822F, 0x022A, 0x823B, 0x023E, + 0x0234, 0x8231, 0x8213, 0x0216, 0x021C, 0x8219, 0x0208, + 0x820D, 0x8207, 0x0202 }; + + for(j = 0; j < data_blk_size; j++) + { + i = ((UINT16_T)(crc_accum >> 8) ^ *data_blk_ptr++) & 0xFF; + crc_accum = (crc_accum << 8) ^ crc_table[i]; + } + + return crc_accum; +} + +void Protocol2PacketHandler::AddStuffing(UINT8_T *packet) +{ + int i = 0, index = 0; + int packet_length_in = DXL_MAKEWORD(packet[PKT_LENGTH_L], packet[PKT_LENGTH_H]); + int packet_length_out = packet_length_in; + UINT8_T temp[TXPACKET_MAX_LEN] = {0}; + + for(UINT8_T _s = PKT_HEADER0; _s <= PKT_LENGTH_H; _s++) + temp[_s] = packet[_s]; // FF FF FD XX ID LEN_L LEN_H + //memcpy(temp, packet, PKT_LENGTH_H+1); + index = PKT_INSTRUCTION; + for( i = 0; i < packet_length_in - 2; i++) // except CRC + { + temp[index++] = packet[i+PKT_INSTRUCTION]; + if(packet[i+PKT_INSTRUCTION] == 0xFD && packet[i+PKT_INSTRUCTION-1] == 0xFF && packet[i+PKT_INSTRUCTION-2] == 0xFF) + { // FF FF FD + temp[index++] = 0xFD; + packet_length_out++; + } + } + temp[index++] = packet[PKT_INSTRUCTION+packet_length_in-2]; + temp[index++] = packet[PKT_INSTRUCTION+packet_length_in-1]; + + + ////////////////////////// + if(packet_length_in != packet_length_out) + packet = (UINT8_T *)realloc(packet, index * sizeof(UINT8_T)); + + /////////////////////////// + + for(UINT8_T _s = 0; _s < index; _s++) + packet[_s] = temp[_s]; + //memcpy(packet, temp, index); + packet[PKT_LENGTH_L] = DXL_LOBYTE(packet_length_out); + packet[PKT_LENGTH_H] = DXL_HIBYTE(packet_length_out); +} + +void Protocol2PacketHandler::RemoveStuffing(UINT8_T *packet) +{ + int i = 0, index = 0; + int packet_length_in = DXL_MAKEWORD(packet[PKT_LENGTH_L], packet[PKT_LENGTH_H]); + int packet_length_out = packet_length_in; + + index = PKT_INSTRUCTION; + for( i = 0; i < packet_length_in - 2; i++) // except CRC + { + if(packet[i+PKT_INSTRUCTION] == 0xFD && packet[i+PKT_INSTRUCTION+1] == 0xFD && packet[i+PKT_INSTRUCTION-1] == 0xFF && packet[i+PKT_INSTRUCTION-2] == 0xFF) + { // FF FF FD FD + packet_length_out--; + i++; + } + packet[index++] = packet[i+PKT_INSTRUCTION]; + } + packet[index++] = packet[PKT_INSTRUCTION+packet_length_in-2]; + packet[index++] = packet[PKT_INSTRUCTION+packet_length_in-1]; + + packet[PKT_LENGTH_L] = DXL_LOBYTE(packet_length_out); + packet[PKT_LENGTH_H] = DXL_HIBYTE(packet_length_out); +} + +int Protocol2PacketHandler::TxPacket(PortHandler *port, UINT8_T *txpacket) +{ + UINT16_T _total_packet_length = 0; + UINT16_T _written_packet_length = 0; + + if(port->is_using) + return COMM_PORT_BUSY; + port->is_using = true; + + // byte stuffing for header + AddStuffing(txpacket); + + // check max packet length + _total_packet_length = DXL_MAKEWORD(txpacket[PKT_LENGTH_L], txpacket[PKT_LENGTH_H]) + 7; + // 7: HEADER0 HEADER1 HEADER2 RESERVED ID LENGTH_L LENGTH_H + if(_total_packet_length > TXPACKET_MAX_LEN) + { + port->is_using = false; + return COMM_TX_ERROR; + } + + // make packet header + txpacket[PKT_HEADER0] = 0xFF; + txpacket[PKT_HEADER1] = 0xFF; + txpacket[PKT_HEADER2] = 0xFD; + txpacket[PKT_RESERVED] = 0x00; + + // add CRC16 + UINT16_T crc = UpdateCRC(0, txpacket, _total_packet_length - 2); // 2: CRC16 + txpacket[_total_packet_length - 2] = DXL_LOBYTE(crc); + txpacket[_total_packet_length - 1] = DXL_HIBYTE(crc); + + // tx packet + port->ClearPort(); + _written_packet_length = port->WritePort(txpacket, _total_packet_length); + if(_total_packet_length != _written_packet_length) + { + port->is_using = false; + return COMM_TX_FAIL; + } + + return COMM_SUCCESS; +} + +int Protocol2PacketHandler::RxPacket(PortHandler *port, UINT8_T *rxpacket) +{ + int _result = COMM_TX_FAIL; + + UINT16_T _rx_length = 0; + UINT16_T _wait_length = 11; + // minimum length ( HEADER0 HEADER1 HEADER2 RESERVED ID LENGTH_L LENGTH_H INST ERROR CRC16_L CRC16_H ) + + while(true) + { + _rx_length += port->ReadPort(&rxpacket[_rx_length], _wait_length - _rx_length); + if(_rx_length >= _wait_length) + { + UINT16_T _idx = 0; + + // find packet header +// for(_idx = 0; _idx < (_rx_length - 2); _idx++) +// { +// if(rxpacket[_idx] == 0xFF && rxpacket[_idx+1] == 0xFF && rxpacket[_idx+2] == 0xFD) +// break; +// } + for(_idx = 0; _idx < (_rx_length - 3); _idx++) + { + if((rxpacket[_idx] == 0xFF) && (rxpacket[_idx+1] == 0xFF) && (rxpacket[_idx+2] == 0xFD) && (rxpacket[_idx+3] != 0xFD)) + break; + } + + if(_idx == 0) // found at the beginning of the packet + { + // re-calculate the exact length of the rx packet + _wait_length = DXL_MAKEWORD(rxpacket[PKT_LENGTH_L], rxpacket[PKT_LENGTH_H]) + PKT_LENGTH_H + 1; + if(_wait_length > 100) + fprintf(stderr, "id : %d , _wait_length : %d \n", rxpacket[PKT_ID], _wait_length); + + if(_wait_length > RXPACKET_MAX_LEN - 10) + { + _result = COMM_RX_CORRUPT; + break; + } + + if(_rx_length < _wait_length) + { + // check timeout + if(port->IsPacketTimeout() == true) + { + if(_rx_length == 0) + _result = COMM_RX_TIMEOUT; + else { + _result = COMM_RX_CORRUPT; +// fprintf(stderr, "Fail Rx Packet(short_length1) : "); +// for(int _i = 0; _i < _rx_length + 11; _i++) +// { +// fprintf(stderr, " %x", rxpacket[_i]); +// } +// fprintf(stderr, "\n"); + } + break; + } + + else + continue; + } + + // verify CRC16 + UINT16_T crc = DXL_MAKEWORD(rxpacket[_wait_length-2], rxpacket[_wait_length-1]); + if(UpdateCRC(0, rxpacket, _wait_length - 2) == crc) + _result = COMM_SUCCESS; + else { + _result = COMM_RX_CORRUPT; +// fprintf(stderr, "Rx Packet(crc_error) : "); +// for(int _i = 0; _i < _rx_length + 11; _i++) +// { +// fprintf(stderr, " %x", rxpacket[_i]); +// } +// fprintf(stderr, "\n"); + } + break; + } + else + { + // remove unnecessary packets + for(UINT8_T _s = 0; _s < _rx_length - _idx; _s++) + rxpacket[_s] = rxpacket[_idx + _s]; + //memcpy(&rxpacket[0], &rxpacket[_idx], _rx_length - _idx); + _rx_length -= _idx; + } + } + else + { + // check timeout + if(port->IsPacketTimeout() == true) + { + if(_rx_length == 0) + _result = COMM_RX_TIMEOUT; + else { + _result = COMM_RX_CORRUPT; +// fprintf(stderr, "Fail Rx Packet(short_length2): "); +// for(int _i = 0; _i < _rx_length + 11; _i++) +// { +// fprintf(stderr, " %x", rxpacket[_i]); +// } +// fprintf(stderr, "\n"); + } + break; + } + } + } + port->is_using = false; + + if(_result == COMM_SUCCESS) + RemoveStuffing(rxpacket); + + return _result; +} + +// NOT for BulkRead / SyncRead instruction +int Protocol2PacketHandler::TxRxPacket(PortHandler *port, UINT8_T *txpacket, UINT8_T *rxpacket, UINT8_T *error) +{ + int _result = COMM_TX_FAIL; + + // tx packet + _result = TxPacket(port, txpacket); + if(_result != COMM_SUCCESS) + return _result; + + // (ID == Broadcast ID && NOT BulkRead) == no need to wait for status packet + // (Instruction == Action) == no need to wait for status packet + if((txpacket[PKT_ID] == BROADCAST_ID && txpacket[PKT_INSTRUCTION] != INST_BULK_READ) || + (txpacket[PKT_ID] == BROADCAST_ID && txpacket[PKT_INSTRUCTION] != INST_SYNC_READ) || + (txpacket[PKT_INSTRUCTION] == INST_ACTION)) + { + port->is_using = false; + return _result; + } + + // set packet timeout + if(txpacket[PKT_INSTRUCTION] == INST_READ) + port->SetPacketTimeout((UINT16_T)(DXL_MAKEWORD(txpacket[PKT_PARAMETER0+2], txpacket[PKT_PARAMETER0+3]) + 11)); + else + port->SetPacketTimeout((UINT16_T)11); // HEADER0 HEADER1 HEADER2 RESERVED ID LENGTH_L LENGTH_H INST ERROR CRC16_L CRC16_H + + // rx packet + _result = RxPacket(port, rxpacket); + // TODO: check txpacket ID == rxpacket ID + + if(_result == COMM_SUCCESS && txpacket[PKT_ID] != BROADCAST_ID) + { + if(error != 0) + *error = (UINT8_T)rxpacket[PKT_ERROR]; + } + + return _result; +} + +int Protocol2PacketHandler::Ping(PortHandler *port, UINT8_T id, UINT8_T *error) +{ + return Ping(port, id, 0, error); +} + +int Protocol2PacketHandler::Ping(PortHandler *port, UINT8_T id, UINT16_T *model_number, UINT8_T *error) +{ + int _result = COMM_TX_FAIL; + + UINT8_T txpacket[10] = {0}; + UINT8_T rxpacket[14] = {0}; + + if(id >= BROADCAST_ID) + return COMM_NOT_AVAILABLE; + + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH_L] = 3; + txpacket[PKT_LENGTH_H] = 0; + txpacket[PKT_INSTRUCTION] = INST_PING; + + _result = TxRxPacket(port, txpacket, rxpacket, error); + if(_result == COMM_SUCCESS && model_number != 0) + *model_number = DXL_MAKEWORD(rxpacket[PKT_PARAMETER0+1], rxpacket[PKT_PARAMETER0+2]); + + return _result; +} + +int Protocol2PacketHandler::BroadcastPing(PortHandler *port, std::vector &id_list) +{ + const int STATUS_LENGTH = 14; + int _result = COMM_TX_FAIL; + + id_list.clear(); + + UINT16_T _rx_length = 0; + UINT16_T _wait_length = STATUS_LENGTH * MAX_ID; + + UINT8_T txpacket[10] = {0}; + UINT8_T rxpacket[STATUS_LENGTH * MAX_ID] = {0}; + + txpacket[PKT_ID] = BROADCAST_ID; + txpacket[PKT_LENGTH_L] = 3; + txpacket[PKT_LENGTH_H] = 0; + txpacket[PKT_INSTRUCTION] = INST_PING; + + _result = TxPacket(port, txpacket); + if(_result != COMM_SUCCESS) + { + port->is_using = false; + return _result; + } + + // set rx timeout + port->SetPacketTimeout((UINT16_T)(_wait_length * 30)); + + while(1) + { + _rx_length += port->ReadPort(&rxpacket[_rx_length], _wait_length - _rx_length); + if(port->IsPacketTimeout() == true)// || _rx_length >= _wait_length) + break; + } + + port->is_using = false; + + if(_rx_length == 0) + return COMM_RX_TIMEOUT; + + while(1) + { + if(_rx_length < STATUS_LENGTH) + return COMM_RX_CORRUPT; + + UINT16_T _idx = 0; + + // find packet header + for(_idx = 0; _idx < (_rx_length - 2); _idx++) + { + if(rxpacket[_idx] == 0xFF && rxpacket[_idx+1] == 0xFF && rxpacket[_idx+2] == 0xFD) + break; + } + + if(_idx == 0) // found at the beginning of the packet + { + // verify CRC16 + UINT16_T crc = DXL_MAKEWORD(rxpacket[STATUS_LENGTH-2], rxpacket[STATUS_LENGTH-1]); + + if(UpdateCRC(0, rxpacket, STATUS_LENGTH - 2) == crc) + { + _result = COMM_SUCCESS; + + id_list.push_back(rxpacket[PKT_ID]); + + for(UINT8_T _s = 0; _s < _rx_length - STATUS_LENGTH; _s++) + rxpacket[_s] = rxpacket[STATUS_LENGTH + _s]; + _rx_length -= STATUS_LENGTH; + + if(_rx_length == 0) + return _result; + } + else + { + _result = COMM_RX_CORRUPT; + + // remove header (0xFF 0xFF 0xFD) + for(UINT8_T _s = 0; _s < _rx_length - 3; _s++) + rxpacket[_s] = rxpacket[3 + _s]; + _rx_length -= 3; + } + } + else + { + // remove unnecessary packets + for(UINT8_T _s = 0; _s < _rx_length - _idx; _s++) + rxpacket[_s] = rxpacket[_idx + _s]; + _rx_length -= _idx; + } + } + + return _result; +} + +int Protocol2PacketHandler::Action(PortHandler *port, UINT8_T id) +{ + int _result = COMM_TX_FAIL; + + UINT8_T txpacket[10] = {0}; + + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH_L] = 3; + txpacket[PKT_LENGTH_H] = 0; + txpacket[PKT_INSTRUCTION] = INST_ACTION; + + return TxRxPacket(port, txpacket, 0); +} + +int Protocol2PacketHandler::Reboot(PortHandler *port, UINT8_T id, UINT8_T *error) +{ + UINT8_T txpacket[10] = {0}; + UINT8_T rxpacket[11] = {0}; + + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH_L] = 3; + txpacket[PKT_LENGTH_H] = 0; + txpacket[PKT_INSTRUCTION] = INST_REBOOT; + + return TxRxPacket(port, txpacket, rxpacket, error); +} + +int Protocol2PacketHandler::FactoryReset(PortHandler *port, UINT8_T id, UINT8_T option, UINT8_T *error) +{ + UINT8_T txpacket[11] = {0}; + UINT8_T rxpacket[11] = {0}; + + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH_L] = 4; + txpacket[PKT_LENGTH_H] = 0; + txpacket[PKT_INSTRUCTION] = INST_FACTORY_RESET; + txpacket[PKT_PARAMETER0] = option; + + return TxRxPacket(port, txpacket, rxpacket, error); +} + +int Protocol2PacketHandler::ReadTx(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length) +{ + int _result = COMM_TX_FAIL; + + UINT8_T txpacket[14] = {0}; + + if(id >= BROADCAST_ID) + return COMM_NOT_AVAILABLE; + + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH_L] = 7; + txpacket[PKT_LENGTH_H] = 0; + txpacket[PKT_INSTRUCTION] = INST_READ; + txpacket[PKT_PARAMETER0+0] = (UINT8_T)DXL_LOBYTE(address); + txpacket[PKT_PARAMETER0+1] = (UINT8_T)DXL_HIBYTE(address); + txpacket[PKT_PARAMETER0+2] = (UINT8_T)DXL_LOBYTE(length); + txpacket[PKT_PARAMETER0+3] = (UINT8_T)DXL_HIBYTE(length); + + _result = TxPacket(port, txpacket); + + // set packet timeout + if(_result == COMM_SUCCESS) + port->SetPacketTimeout((UINT16_T)(length + 11)); + + return _result; +} + +int Protocol2PacketHandler::ReadRx(PortHandler *port, UINT16_T length, UINT8_T *data, UINT8_T *error) +{ + int _result = COMM_TX_FAIL; + //UINT8_T *rxpacket = (UINT8_T *)malloc(RXPACKET_MAX_LEN);//(length + 11 + (length/3)); // (length/3): consider stuffing + UINT8_T *rxpacket = ( UINT8_T* )calloc(RXPACKET_MAX_LEN, sizeof(UINT8_T));//(length + 11 + (length/3)); // (length/3): consider stuffing + //UINT8_T *rxpacket = new UINT8_T[length + 11 + (length/3)]; // (length/3): consider stuffing + + _result = RxPacket(port, rxpacket); + if(_result == COMM_SUCCESS) + { + if(error != 0) + *error = (UINT8_T)rxpacket[PKT_ERROR]; + for(UINT8_T _s = 0; _s < length; _s++) + data[_s] = rxpacket[PKT_PARAMETER0 + 1 + _s]; + //memcpy(data, &rxpacket[PKT_PARAMETER0+1], length); + } +// else +// { +// fprintf(stderr, "Fail Rx Packet : "); +// for(int _i = 0; _i < length + 11; _i++) +// { +// fprintf(stderr, " %x", rxpacket[_i]); +// } +// fprintf(stderr, "\n"); +// } + + free(rxpacket); + //delete[] rxpacket; + return _result; +} + +int Protocol2PacketHandler::ReadTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error) +{ + int _result = COMM_TX_FAIL; + + _result = ReadTx(port, id, address, length); + if(_result != COMM_SUCCESS) + return _result; + + return ReadRx(port, length, data, error); +} + +int Protocol2PacketHandler::Read1ByteTx(PortHandler *port, UINT8_T id, UINT16_T address) +{ + return ReadTx(port, id, address, 1); +} +int Protocol2PacketHandler::Read1ByteRx(PortHandler *port, UINT8_T *data, UINT8_T *error) +{ + UINT8_T _data[1] = {0}; + int _result = ReadRx(port, 1, _data, error); + *data = _data[0]; + return _result; +} +int Protocol2PacketHandler::Read1ByteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T *data, UINT8_T *error) +{ + int _result = COMM_TX_FAIL; + + _result = Read1ByteTx(port, id, address); + if(_result != COMM_SUCCESS) + return _result; + + return Read1ByteRx(port, data, error); +} + +int Protocol2PacketHandler::Read2ByteTx(PortHandler *port, UINT8_T id, UINT16_T address) +{ + return ReadTx(port, id, address, 2); +} +int Protocol2PacketHandler::Read2ByteRx(PortHandler *port, UINT16_T *data, UINT8_T *error) +{ + UINT8_T _data[2] = {0}; + int _result = ReadRx(port, 2, _data, error); + *data = DXL_MAKEWORD(_data[0], _data[1]); + return _result; +} +int Protocol2PacketHandler::Read2ByteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T *data, UINT8_T *error) +{ + int _result = COMM_TX_FAIL; + + _result = Read2ByteTx(port, id, address); + if(_result != COMM_SUCCESS) + return _result; + + return Read2ByteRx(port, data, error); +} + +int Protocol2PacketHandler::Read4ByteTx(PortHandler *port, UINT8_T id, UINT16_T address) +{ + return ReadTx(port, id, address, 4); +} +int Protocol2PacketHandler::Read4ByteRx(PortHandler *port, UINT32_T *data, UINT8_T *error) +{ + UINT8_T _data[4] = {0}; + int _result = ReadRx(port, 4, _data, error); + *data = DXL_MAKEDWORD(DXL_MAKEWORD(_data[0], _data[1]), DXL_MAKEWORD(_data[2], _data[3])); + return _result; +} +int Protocol2PacketHandler::Read4ByteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T *data, UINT8_T *error) +{ + int _result = COMM_TX_FAIL; + + _result = Read4ByteTx(port, id, address); + if(_result != COMM_SUCCESS) + return _result; + + return Read4ByteRx(port, data, error); +} + + +int Protocol2PacketHandler::WriteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data) +{ + int _result = COMM_TX_FAIL; + + UINT8_T *txpacket = (UINT8_T *)malloc(length+12); + //UINT8_T *txpacket = new UINT8_T[length+12]; + + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length+5); + txpacket[PKT_LENGTH_H] = DXL_HIBYTE(length+5); + txpacket[PKT_INSTRUCTION] = INST_WRITE; + txpacket[PKT_PARAMETER0+0] = (UINT8_T)DXL_LOBYTE(address); + txpacket[PKT_PARAMETER0+1] = (UINT8_T)DXL_HIBYTE(address); + + for(UINT8_T _s = 0; _s < length; _s++) + txpacket[PKT_PARAMETER0+2+_s] = data[_s]; + //memcpy(&txpacket[PKT_PARAMETER0+2], data, length); + + _result = TxPacket(port, txpacket); + port->is_using = false; + + free(txpacket); + //delete[] txpacket; + return _result; +} + +int Protocol2PacketHandler::WriteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error) +{ + int _result = COMM_TX_FAIL; + + UINT8_T *txpacket = (UINT8_T *)malloc(length + 12); + //UINT8_T *txpacket = new UINT8_T[length+12]; + UINT8_T rxpacket[11] = {0}; + + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length+5); + txpacket[PKT_LENGTH_H] = DXL_HIBYTE(length+5); + txpacket[PKT_INSTRUCTION] = INST_WRITE; + txpacket[PKT_PARAMETER0+0] = (UINT8_T)DXL_LOBYTE(address); + txpacket[PKT_PARAMETER0+1] = (UINT8_T)DXL_HIBYTE(address); + + for(UINT8_T _s = 0; _s < length; _s++) + txpacket[PKT_PARAMETER0+2+_s] = data[_s]; + //memcpy(&txpacket[PKT_PARAMETER0+2], data, length); + + _result = TxRxPacket(port, txpacket, rxpacket, error); + + free(txpacket); + //delete[] txpacket; + return _result; +} + +int Protocol2PacketHandler::Write1ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T data) +{ + UINT8_T _data[1] = { data }; + return WriteTxOnly(port, id, address, 1, _data); +} +int Protocol2PacketHandler::Write1ByteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T data, UINT8_T *error) +{ + UINT8_T _data[1] = { data }; + return WriteTxRx(port, id, address, 1, _data, error); +} + +int Protocol2PacketHandler::Write2ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T data) +{ + UINT8_T _data[2] = { DXL_LOBYTE(data), DXL_HIBYTE(data) }; + return WriteTxOnly(port, id, address, 2, _data); +} +int Protocol2PacketHandler::Write2ByteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T data, UINT8_T *error) +{ + UINT8_T _data[2] = { DXL_LOBYTE(data), DXL_HIBYTE(data) }; + return WriteTxRx(port, id, address, 2, _data, error); +} + +int Protocol2PacketHandler::Write4ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T data) +{ + UINT8_T _data[4] = { DXL_LOBYTE(DXL_LOWORD(data)), DXL_HIBYTE(DXL_LOWORD(data)), DXL_LOBYTE(DXL_HIWORD(data)), DXL_HIBYTE(DXL_HIWORD(data)) }; + return WriteTxOnly(port, id, address, 4, _data); +} +int Protocol2PacketHandler::Write4ByteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T data, UINT8_T *error) +{ + UINT8_T _data[4] = { DXL_LOBYTE(DXL_LOWORD(data)), DXL_HIBYTE(DXL_LOWORD(data)), DXL_LOBYTE(DXL_HIWORD(data)), DXL_HIBYTE(DXL_HIWORD(data)) }; + return WriteTxRx(port, id, address, 4, _data, error); +} + +int Protocol2PacketHandler::RegWriteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data) +{ + int _result = COMM_TX_FAIL; + + UINT8_T *txpacket = (UINT8_T *)malloc(length + 12); + //UINT8_T *txpacket = new UINT8_T[length+12]; + + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length+5); + txpacket[PKT_LENGTH_H] = DXL_HIBYTE(length+5); + txpacket[PKT_INSTRUCTION] = INST_REG_WRITE; + txpacket[PKT_PARAMETER0+0] = (UINT8_T)DXL_LOBYTE(address); + txpacket[PKT_PARAMETER0+1] = (UINT8_T)DXL_HIBYTE(address); + + for(UINT8_T _s = 0; _s < length; _s++) + txpacket[PKT_PARAMETER0+2+_s] = data[_s]; + //memcpy(&txpacket[PKT_PARAMETER0+2], data, length); + + _result = TxPacket(port, txpacket); + port->is_using = false; + + free(txpacket); + //delete[] txpacket; + return _result; +} + +int Protocol2PacketHandler::RegWriteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error) +{ + int _result = COMM_TX_FAIL; + + UINT8_T *txpacket = (UINT8_T *)malloc(length + 12); + //UINT8_T *txpacket = new UINT8_T[length+12]; + UINT8_T rxpacket[11] = {0}; + + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length+5); + txpacket[PKT_LENGTH_H] = DXL_HIBYTE(length+5); + txpacket[PKT_INSTRUCTION] = INST_REG_WRITE; + txpacket[PKT_PARAMETER0+0] = (UINT8_T)DXL_LOBYTE(address); + txpacket[PKT_PARAMETER0+1] = (UINT8_T)DXL_HIBYTE(address); + + for(UINT8_T _s = 0; _s < length; _s++) + txpacket[PKT_PARAMETER0+2+_s] = data[_s]; + //memcpy(&txpacket[PKT_PARAMETER0+2], data, length); + + _result = TxRxPacket(port, txpacket, rxpacket, error); + + free(txpacket); + //delete[] txpacket; + return _result; +} + +int Protocol2PacketHandler::SyncReadTx(PortHandler *port, UINT16_T start_address, UINT16_T data_length, UINT8_T *param, UINT16_T param_length) +{ + int _result = COMM_TX_FAIL; + + UINT8_T *txpacket = (UINT8_T *)malloc(param_length + 14); + // 14: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H + + txpacket[PKT_ID] = BROADCAST_ID; + txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H + txpacket[PKT_LENGTH_H] = DXL_HIBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H + txpacket[PKT_INSTRUCTION] = INST_SYNC_READ; + txpacket[PKT_PARAMETER0+0] = DXL_LOBYTE(start_address); + txpacket[PKT_PARAMETER0+1] = DXL_HIBYTE(start_address); + txpacket[PKT_PARAMETER0+2] = DXL_LOBYTE(data_length); + txpacket[PKT_PARAMETER0+3] = DXL_HIBYTE(data_length); + + for(UINT8_T _s = 0; _s < param_length; _s++) + txpacket[PKT_PARAMETER0+4+_s] = param[_s]; + //memcpy(&txpacket[PKT_PARAMETER0+4], param, param_length); + + _result = TxPacket(port, txpacket); + if(_result == COMM_SUCCESS) + port->SetPacketTimeout((UINT16_T)((11 + data_length) * param_length)); + + free(txpacket); + return _result; +} + +int Protocol2PacketHandler::SyncWriteTxOnly(PortHandler *port, UINT16_T start_address, UINT16_T data_length, UINT8_T *param, UINT16_T param_length) +{ + int _result = COMM_TX_FAIL; + + UINT8_T *txpacket = (UINT8_T *)malloc(param_length + 14); + //UINT8_T *txpacket = new UINT8_T[param_length + 14]; + // 14: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H + + txpacket[PKT_ID] = BROADCAST_ID; + txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H + txpacket[PKT_LENGTH_H] = DXL_HIBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H + txpacket[PKT_INSTRUCTION] = INST_SYNC_WRITE; + txpacket[PKT_PARAMETER0+0] = DXL_LOBYTE(start_address); + txpacket[PKT_PARAMETER0+1] = DXL_HIBYTE(start_address); + txpacket[PKT_PARAMETER0+2] = DXL_LOBYTE(data_length); + txpacket[PKT_PARAMETER0+3] = DXL_HIBYTE(data_length); + + for(UINT8_T _s = 0; _s < param_length; _s++) + txpacket[PKT_PARAMETER0+4+_s] = param[_s]; + //memcpy(&txpacket[PKT_PARAMETER0+4], param, param_length); + + _result = TxRxPacket(port, txpacket, 0, 0); + + free(txpacket); + //delete[] txpacket; + return _result; +} + +int Protocol2PacketHandler::BulkReadTx(PortHandler *port, UINT8_T *param, UINT16_T param_length) +{ + int _result = COMM_TX_FAIL; + + UINT8_T *txpacket = (UINT8_T *)malloc(param_length + 10); + //UINT8_T *txpacket = new UINT8_T[param_length + 10]; + // 10: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST CRC16_L CRC16_H + + txpacket[PKT_ID] = BROADCAST_ID; + txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H + txpacket[PKT_LENGTH_H] = DXL_HIBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H + txpacket[PKT_INSTRUCTION] = INST_BULK_READ; + + for(UINT8_T _s = 0; _s < param_length; _s++) + txpacket[PKT_PARAMETER0+_s] = param[_s]; + //memcpy(&txpacket[PKT_PARAMETER0], param, param_length); + + _result = TxPacket(port, txpacket); + if(_result == COMM_SUCCESS) + { + int _wait_length = 0; + for(int _i = 0; _i < param_length; _i += 5) + _wait_length += DXL_MAKEWORD(param[_i+3], param[_i+4]) + 10; + port->SetPacketTimeout((UINT16_T)_wait_length); + } + + free(txpacket); + //delete[] txpacket; + return _result; +} + +int Protocol2PacketHandler::BulkWriteTxOnly(PortHandler *port, UINT8_T *param, UINT16_T param_length) +{ + int _result = COMM_TX_FAIL; + + UINT8_T *txpacket = (UINT8_T *)malloc(param_length + 10); + //UINT8_T *txpacket = new UINT8_T[param_length + 10]; + // 10: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST CRC16_L CRC16_H + + txpacket[PKT_ID] = BROADCAST_ID; + txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H + txpacket[PKT_LENGTH_H] = DXL_HIBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H + txpacket[PKT_INSTRUCTION] = INST_BULK_WRITE; + + for(UINT8_T _s = 0; _s < param_length; _s++) + txpacket[PKT_PARAMETER0+_s] = param[_s]; + //memcpy(&txpacket[PKT_PARAMETER0], param, param_length); + + _result = TxRxPacket(port, txpacket, 0, 0); + + free(txpacket); + //delete[] txpacket; + return _result; +} diff --git a/dynamixel_sdk/src/dynamixel_sdk_linux/PortHandlerLinux.cpp b/dynamixel_sdk/src/dynamixel_sdk_linux/PortHandlerLinux.cpp new file mode 100644 index 0000000..6c6a40b --- /dev/null +++ b/dynamixel_sdk/src/dynamixel_sdk_linux/PortHandlerLinux.cpp @@ -0,0 +1,253 @@ +/* + * PortHandlerLinux.cpp + * + * Created on: 2016. 1. 26. + * Author: zerom + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "dynamixel_sdk_linux/PortHandlerLinux.h" + +#define LATENCY_TIMER 4 // msec (USB latency timer) + +using namespace ROBOTIS; + +PortHandlerLinux::PortHandlerLinux(const char *port_name) + : socket_fd_(-1), + baudrate_(DEFAULT_BAUDRATE), + packet_start_time_(0.0), + packet_timeout_(0.0), + tx_time_per_byte(0.0) +{ + is_using = false; + SetPortName(port_name); +} + +bool PortHandlerLinux::OpenPort() +{ + return SetBaudRate(baudrate_); +} + +void PortHandlerLinux::ClosePort() +{ + if(socket_fd_ != -1) + close(socket_fd_); + socket_fd_ = -1; +} + +void PortHandlerLinux::ClearPort() +{ + tcflush(socket_fd_, TCIOFLUSH); +} + +void PortHandlerLinux::SetPortName(const char *port_name) +{ + strcpy(port_name_, port_name); +} + +char *PortHandlerLinux::GetPortName() +{ + return port_name_; +} + +// TODO: baud number ?? +bool PortHandlerLinux::SetBaudRate(const int baudrate) +{ + int _baud = GetCFlagBaud(baudrate); + + ClosePort(); + + if(_baud <= 0) // custom baudrate + { + SetupPort(B38400); + baudrate_ = baudrate; + return SetCustomBaudrate(baudrate); + } + else + { + baudrate_ = baudrate; + return SetupPort(_baud); + } + + return true; +} + +int PortHandlerLinux::GetBaudRate() +{ + return baudrate_; +} + +int PortHandlerLinux::GetBytesAvailable() +{ + int _bytes_available; + ioctl(socket_fd_, FIONREAD, &_bytes_available); + return _bytes_available; +} + +int PortHandlerLinux::ReadPort(UINT8_T *packet, int length) +{ + return read(socket_fd_, packet, length); +} + +int PortHandlerLinux::WritePort(UINT8_T *packet, int length) +{ + return write(socket_fd_, packet, length); +} + +void PortHandlerLinux::SetPacketTimeout(UINT16_T packet_length) +{ + packet_start_time_ = GetCurrentTime(); + packet_timeout_ = (tx_time_per_byte * (double)packet_length) + (LATENCY_TIMER * 2.0) + 2.0; +} + +void PortHandlerLinux::SetPacketTimeout(double msec) +{ + packet_start_time_ = GetCurrentTime(); + packet_timeout_ = msec; +} + +bool PortHandlerLinux::IsPacketTimeout() +{ + if(GetTimeSinceStart() > packet_timeout_) + { + packet_timeout_ = 0; + return true; + } + return false; +} + +double PortHandlerLinux::GetCurrentTime() +{ +// struct timeval _tv; +// +// gettimeofday(&_tv, 0); +// +// return ((double)_tv.tv_sec * 1000.0 + (double)_tv.tv_usec / 1000.0); + + struct timespec _tv; + clock_gettime( CLOCK_REALTIME, &_tv); + return ((double)_tv.tv_sec*1000.0 + (double)_tv.tv_nsec*0.001*0.001); +} + +double PortHandlerLinux::GetTimeSinceStart() +{ + double _time; + + _time = GetCurrentTime() - packet_start_time_; + if(_time < 0.0) + packet_start_time_ = GetCurrentTime(); + + return _time; +} + +bool PortHandlerLinux::SetupPort(int cflag_baud) +{ + struct termios newtio; + + socket_fd_ = open(port_name_, O_RDWR|O_NOCTTY|O_NONBLOCK); + if(socket_fd_ < 0) + { + printf("[PortHandlerLinux::SetupPort] Error opening serial port!\n"); + return false; + } + + bzero(&newtio, sizeof(newtio)); // clear struct for new port settings + + newtio.c_cflag = cflag_baud | CS8 | CLOCAL | CREAD; + newtio.c_iflag = IGNPAR; + newtio.c_oflag = 0; + newtio.c_lflag = 0; + newtio.c_cc[VTIME] = 0; + newtio.c_cc[VMIN] = 0; + + // clean the buffer and activate the settings for the port + tcflush(socket_fd_, TCIFLUSH); + tcsetattr(socket_fd_, TCSANOW, &newtio); + + tx_time_per_byte = (1000.0 / (double)baudrate_) * 10.0; + return true; +} + +bool PortHandlerLinux::SetCustomBaudrate(int speed) +{ + // try to set a custom divisor + struct serial_struct ss; + if(ioctl(socket_fd_, TIOCGSERIAL, &ss) != 0) + { + printf("[PortHandlerLinux::SetCustomBaudrate] TIOCGSERIAL failed!\n"); + return false; + } + + ss.flags = (ss.flags & ~ASYNC_SPD_MASK) | ASYNC_SPD_CUST; + ss.custom_divisor = (ss.baud_base + (speed / 2)) / speed; + int closest_speed = ss.baud_base / ss.custom_divisor; + + if(closest_speed < speed * 98 / 100 || closest_speed > speed * 102 / 100) + { + printf("[PortHandlerLinux::SetCustomBaudrate] Cannot set speed to %d, closest is %d \n", speed, closest_speed); + return false; + } + + if(ioctl(socket_fd_, TIOCSSERIAL, &ss) < 0) + { + printf("[PortHandlerLinux::SetCustomBaudrate] TIOCSSERIAL failed!\n"); + return false; + } + + tx_time_per_byte = (1000.0 / (double)speed) * 10.0; + return true; +} + +int PortHandlerLinux::GetCFlagBaud(int baudrate) +{ + switch(baudrate) + { + case 9600: + return B9600; + case 19200: + return B19200; + case 38400: + return B38400; + case 57600: + return B57600; + case 115200: + return B115200; + case 230400: + return B230400; + case 460800: + return B460800; + case 500000: + return B500000; + case 576000: + return B576000; + case 921600: + return B921600; + case 1000000: + return B1000000; + case 1152000: + return B1152000; + case 1500000: + return B1500000; + case 2000000: + return B2000000; + case 2500000: + return B2500000; + case 3000000: + return B3000000; + case 3500000: + return B3500000; + case 4000000: + return B4000000; + default: + return -1; + } +} diff --git a/robotis_controller/CMakeLists.txt b/robotis_controller/CMakeLists.txt new file mode 100644 index 0000000..a41b0d8 --- /dev/null +++ b/robotis_controller/CMakeLists.txt @@ -0,0 +1,55 @@ +cmake_minimum_required(VERSION 2.8.3) +project(robotis_controller) + +find_package(catkin REQUIRED COMPONENTS + roscpp + roslib + sensor_msgs + std_msgs + robotis_device + robotis_controller_msgs + robotis_framework_common + cmake_modules + dynamixel_sdk +) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if you package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( + INCLUDE_DIRS include +# LIBRARIES robotis_controller + CATKIN_DEPENDS roscpp roslib sensor_msgs std_msgs +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +# include_directories(include) +include_directories( + include + ${catkin_INCLUDE_DIRS} +) + +## Declare a cpp library +add_library(robotis_controller + src/robotis_controller/RobotisController.cpp +) + +## Specify libraries to link a library or executable target against +target_link_libraries(robotis_controller + yaml-cpp + robotis_device + dynamixel_sdk + ${catkin_LIBRARIES} +) diff --git a/robotis_controller/include/robotis_controller/RobotisController.h b/robotis_controller/include/robotis_controller/RobotisController.h new file mode 100644 index 0000000..f855a38 --- /dev/null +++ b/robotis_controller/include/robotis_controller/RobotisController.h @@ -0,0 +1,124 @@ +/* + * RobotisController.h + * + * Created on: 2016. 1. 15. + * Author: zerom + */ + +#ifndef ROBOTIS_FRAMEWORK_ROBOTIS_CONTROLLER_INCLUDE_ROBOTIS_CONTROLLER_ROBOTISCONTROLLER_H_ +#define ROBOTIS_FRAMEWORK_ROBOTIS_CONTROLLER_INCLUDE_ROBOTIS_CONTROLLER_ROBOTISCONTROLLER_H_ + + +#include +#include +#include +#include +#include + +#include "robotis_device/Robot.h" +#include "robotis_framework_common/MotionModule.h" + +#include "robotis_controller_msgs/SyncWriteItem.h" +#include "robotis_controller_msgs/JointCtrlModule.h" +#include "robotis_controller_msgs/GetJointModule.h" + +// TODO: TEMPORARY CODE !! +#include "dynamixel_sdk/GroupBulkRead.h" +#include "dynamixel_sdk/GroupSyncWrite.h" + +namespace ROBOTIS +{ + +enum CONTROLLER_MODE +{ + MOTION_MODULE_MODE, + DIRECT_CONTROL_MODE +}; + +class RobotisController +{ +private: + static RobotisController *unique_instance_; + + boost::thread queue_thread_; + boost::mutex queue_mutex_; + + bool is_timer_running_; + bool stop_timer_; + pthread_t timer_thread_; + CONTROLLER_MODE controller_mode_; + + std::list modules_; + std::vector direct_sync_write_; + + RobotisController(); + + void QueueThread(); + + bool CheckTimerStop(); + void InitSyncWrite(); + +public: + static const int CONTROL_CYCLE_MSEC = 8; // 8 msec + + bool DEBUG_PRINT; + Robot *robot; + + // TODO: TEMPORARY CODE !! + std::map port_to_bulk_read; + std::map port_to_sync_write; + + /* publisher */ + ros::Publisher goal_joint_state_pub; + ros::Publisher present_joint_state_pub; + ros::Publisher current_module_pub; + + static void *ThreadProc(void *param); + static RobotisController *GetInstance() { return unique_instance_; } + + bool Initialize(const std::string robot_file_path, const std::string init_file_path); + void Process(); + void AddModule(MotionModule *module); + void RemoveModule(MotionModule *module); + + void StartTimer(); + void StopTimer(); + bool IsTimerRunning(); + + void LoadOffset(const std::string path); + + /* ROS Topic Callback Functions */ + void SyncWriteItemCallback(const robotis_controller_msgs::SyncWriteItem::ConstPtr &msg); + void SetControllerModeCallback(const std_msgs::String::ConstPtr &msg); + void SetJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg); + void SetCtrlModuleCallback(const robotis_controller_msgs::JointCtrlModule::ConstPtr &msg); + bool GetCtrlModuleCallback(robotis_controller_msgs::GetJointModule::Request &req, robotis_controller_msgs::GetJointModule::Response &res); + + int Ping (const std::string joint_name, UINT8_T *error = 0); + int Ping (const std::string joint_name, UINT16_T* model_number, UINT8_T *error = 0); + + int Action (const std::string joint_name); + int Reboot (const std::string joint_name, UINT8_T *error = 0); + int FactoryReset(const std::string joint_name, UINT8_T option = 0, UINT8_T *error = 0); + + int Read (const std::string joint_name, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error = 0); + int ReadCtrlItem(const std::string joint_name, const std::string item_name, UINT32_T *data, UINT8_T *error = 0); + + int Read1Byte (const std::string joint_name, UINT16_T address, UINT8_T *data, UINT8_T *error = 0); + int Read2Byte (const std::string joint_name, UINT16_T address, UINT16_T *data, UINT8_T *error = 0); + int Read4Byte (const std::string joint_name, UINT16_T address, UINT32_T *data, UINT8_T *error = 0); + + int Write (const std::string joint_name, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error = 0); + int WriteCtrlItem(const std::string joint_name, const std::string item_name, UINT32_T data, UINT8_T *error = 0); + + int Write1Byte (const std::string joint_name, UINT16_T address, UINT8_T data, UINT8_T *error = 0); + int Write2Byte (const std::string joint_name, UINT16_T address, UINT16_T data, UINT8_T *error = 0); + int Write4Byte (const std::string joint_name, UINT16_T address, UINT32_T data, UINT8_T *error = 0); + + int RegWrite (const std::string joint_name, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error = 0); +}; + +} + + +#endif /* ROBOTIS_FRAMEWORK_ROBOTIS_CONTROLLER_INCLUDE_ROBOTIS_CONTROLLER_ROBOTISCONTROLLER_H_ */ diff --git a/robotis_controller/package.xml b/robotis_controller/package.xml new file mode 100644 index 0000000..c504764 --- /dev/null +++ b/robotis_controller/package.xml @@ -0,0 +1,38 @@ + + + robotis_controller + 0.1.1 + The robotis_controller package + + ROBOTIS + + + GPLv2 + + + + + + ROBOTIS + + + catkin + + roscpp + roslib + std_msgs + sensor_msgs + dynamixel_sdk + robotis_device + robotis_controller_msgs + robotis_framework_common + + roscpp + roslib + std_msgs + sensor_msgs + dynamixel_sdk + robotis_device + robotis_controller_msgs + + diff --git a/robotis_controller/src/robotis_controller/RobotisController.cpp b/robotis_controller/src/robotis_controller/RobotisController.cpp new file mode 100644 index 0000000..aa5bf7c --- /dev/null +++ b/robotis_controller/src/robotis_controller/RobotisController.cpp @@ -0,0 +1,1112 @@ +/* + * RobotisController.cpp + * + * Created on: 2016. 1. 15. + * Author: zerom + */ + +#include +#include + +#include "robotis_controller/RobotisController.h" + +#define INDIRECT_ADDRESS "indirect_address_1" + +using namespace ROBOTIS; + +RobotisController *RobotisController::unique_instance_ = new RobotisController(); + +// INDIRECT ADDR : 634 -> Present Position (4 Byte) +// INDIRECT ADDR : 638 -> Present Velocity (4 Byte) +// INDIRECT ADDR : 642 -> Present Current (2 Byte) +// INDIRECT ADDR : 644 -> Input Voltage (2 Byte) +// INDIRECT ADDR : 646 -> Temperature (1 byte) +// INDIRECT ADDR : 647 -> External Port Data 1 (2 byte) +// INDIRECT ADDR : 649 -> External Port Data 2 (2 byte) +// INDIRECT ADDR : 651 -> External Port Data 3 (2 byte) +// INDIRECT ADDR : 653 -> External Port Data 4 (2 byte) +const UINT16_T PRESENT_POSITION_ADDR = 634; +const UINT16_T TORQUE_ENABLE_ADDR = 562; +const UINT16_T GOAL_POSITION_ADDR = 596; +const UINT16_T GOAL_VELOCITY_ADDR = 600; +const UINT16_T GOAL_ACCELERATION_ADDR = 606; +const UINT16_T EXT_PORT_DATA1_ADDR = 647; +const UINT16_T EXT_PORT_DATA2_ADDR = 649; +const UINT16_T EXT_PORT_DATA3_ADDR = 651; +const UINT16_T EXT_PORT_DATA4_ADDR = 653; + +RobotisController::RobotisController() +: is_timer_running_(false), + stop_timer_(false), + timer_thread_(0), + controller_mode_(MOTION_MODULE_MODE), + DEBUG_PRINT(false), + robot(0) +{ + direct_sync_write_.clear(); +} + +void RobotisController::InitSyncWrite() +{ + ROS_INFO("FIRST BULKREAD"); + // bulkread twice + for(std::map::iterator _it = port_to_bulk_read.begin(); _it != port_to_bulk_read.end(); _it++) + _it->second->TxRxPacket(); + usleep(50*1000); + for(std::map::iterator _it = port_to_bulk_read.begin(); _it != port_to_bulk_read.end(); _it++) + _it->second->TxRxPacket(); + ROS_INFO("FIRST BULKREAD END"); + + // clear syncwrite param setting + for(std::map::iterator _it = port_to_sync_write.begin(); _it != port_to_sync_write.end(); _it++) + _it->second->ClearParam(); + + // set init syncwrite param(from data of bulkread) + for(std::map::iterator _it = robot->dxls.begin(); _it != robot->dxls.end(); _it++) + { + INT32_T _pos = 0; + + std::string _joint_name = _it->first; + Dynamixel *_dxl = _it->second; + + bool _res = false; + _res = port_to_bulk_read[_dxl->port_name]->GetData(_dxl->id, PRESENT_POSITION_ADDR, (UINT32_T*)&_pos); + if(_res == true) + { + _dxl->dxl_state->present_position = _dxl->ConvertValue2Radian(_pos) - _dxl->dxl_state->position_offset; // remove offset; + _dxl->dxl_state->goal_position = _dxl->dxl_state->present_position; + } + + UINT8_T _sync_write_data[4]; + _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_pos)); + _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_pos)); + _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_pos)); + _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_pos)); + + port_to_sync_write[_dxl->port_name]->AddParam(_dxl->id, _sync_write_data); + } +} + +bool RobotisController::Initialize(const std::string robot_file_path, const std::string init_file_path) +{ + std::string _dev_desc_dir_path = ros::package::getPath("robotis_device") + "/devices"; + + // load robot info : port , device + robot = new Robot(robot_file_path, _dev_desc_dir_path); + // TODO: fill joint_ctrl_module_ + + + // TODO: TEMPORARY CODE !! + /* temporary code start */ + PacketHandler *_protocol2_handler = PacketHandler::GetPacketHandler(2.0); + + for(std::map::iterator _it = robot->ports.begin(); _it != robot->ports.end(); _it++) + { + port_to_bulk_read[_it->first] = new GroupBulkRead(_it->second, _protocol2_handler); + port_to_sync_write[_it->first] = new GroupSyncWrite(_it->second, _protocol2_handler, GOAL_POSITION_ADDR, 4); + } + + + // TODO: + // for loop ping --> no response : return false + for(std::map::iterator _it = robot->dxls.begin(); _it != robot->dxls.end(); _it++) + { + std::string _joint_name = _it->first; + Dynamixel *_dxl = _it->second; + if(Ping(_joint_name) != 0) + { + usleep(10*1000); + if(Ping(_joint_name) != 0) + ROS_ERROR("JOINT[%s] does NOT response!!", _joint_name.c_str()); + } + } + + // joint(dynamixel) initialize + if(DEBUG_PRINT) ROS_WARN("INIT FILE LOAD"); + + YAML::Node _doc; + try{ + _doc = YAML::LoadFile(init_file_path.c_str()); + + for(YAML::const_iterator _it_doc = _doc.begin(); _it_doc != _doc.end(); _it_doc++) + { + std::string _joint_name = _it_doc->first.as(); + + YAML::Node _joint_node = _doc[_joint_name]; + if(_joint_node.size() == 0) + continue; + + Dynamixel *_dxl = NULL; + std::map::iterator _dxl_it = robot->dxls.find(_joint_name); + if(_dxl_it != robot->dxls.end()) + _dxl = _dxl_it->second; + + if(_dxl == NULL) + { + ROS_WARN("Joint [%s] does not found.", _joint_name.c_str()); + continue; + } + if(DEBUG_PRINT) ROS_INFO("JOINT_NAME: %s", _joint_name.c_str()); + + for(YAML::const_iterator _it_joint = _joint_node.begin(); _it_joint != _joint_node.end(); _it_joint++) + { + std::string _item_name = _it_joint->first.as(); + + if(DEBUG_PRINT) ROS_INFO(" ITEM_NAME: %s", _item_name.c_str()); + // indirect address setting + if(_item_name == INDIRECT_ADDRESS) + { + YAML::Node _indirect_node = _joint_node[_item_name]; + if(_indirect_node.size() == 0) + continue; + + for(YAML::const_iterator _it_idx = _indirect_node.begin(); _it_idx != _indirect_node.end(); _it_idx++) + { + int _start_idx = _it_idx->first.as(); + if(_start_idx < 1 || 256 < _start_idx) + ROS_WARN("[%s] INDIRECT ADDRESS start index is out of range. (%d)", _joint_name.c_str(), _start_idx); + if(DEBUG_PRINT) ROS_INFO(" START_IDX: %d", _start_idx); + + YAML::Node _indirect_item_node = _indirect_node[_start_idx]; + if(_indirect_item_node.size() == 0) + continue; + + int _start_address = _dxl->ctrl_table[INDIRECT_ADDRESS]->address + _start_idx - 1; + for(unsigned int _i = 0; _i < _indirect_item_node.size(); _i++) + { + std::string _indir_item_name = _indirect_item_node[_i].as().c_str(); + int _addr_leng = _dxl->ctrl_table[_indir_item_name]->data_length; + for(int _l = 0; _l < _addr_leng; _l++) + { + if(DEBUG_PRINT) ROS_INFO(" - INDIR_ADDR[%d] : %d", _start_address, _dxl->ctrl_table[_indir_item_name]->address + _l); + Write2Byte(_joint_name, _start_address, _dxl->ctrl_table[_indir_item_name]->address + _l); + _start_address += 2; + } + } + } + } + // other items setting + else + { + UINT32_T _value = _it_joint->second.as(); + + ControlTableItem *_item = _dxl->ctrl_table[_item_name]; + if(_item == NULL) + { + ROS_WARN("Control Item [%s] does not found.", _item_name.c_str()); + continue; + } + + switch(_item->data_length) + { + case 1: + Write1Byte(_joint_name, _item->address, (UINT8_T)_value); + break; + case 2: + Write2Byte(_joint_name, _item->address, (UINT16_T)_value); + break; + case 4: + Write4Byte(_joint_name, _item->address, _value); + break; + default: + break; + } + } + } + } + } catch(const std::exception& e) { + ROS_INFO("Dynamixel Init file not found."); + } + + + // [ BulkRead ] StartAddress : Present Position , Length : 10 ( Position/Velocity/Current ) + for(std::map::iterator _it = robot->dxls.begin(); _it != robot->dxls.end(); _it++) + { + std::string _joint_name = _it->first; + Dynamixel *_dxl = _it->second; + + if(_dxl == NULL) + continue; + + UINT16_T _data_length = 0; + if(_dxl->id == 11 || _dxl->id == 12 || _dxl->id == 23 || _dxl->id == 24) + _data_length = 17; + else if(_dxl->id == 13 || _dxl->id == 14 || _dxl->id == 25 || _dxl->id == 26) + _data_length = 21; + else + _data_length = 13; + port_to_bulk_read[_dxl->port_name]->AddParam(robot->dxls[_joint_name]->id, PRESENT_POSITION_ADDR, _data_length); + + // Torque ON + _protocol2_handler->Write1ByteTxRx(robot->ports[_dxl->port_name], robot->dxls[_joint_name]->id, TORQUE_ENABLE_ADDR, 1); + } + + //InitSyncWrite(); + + for(std::map::iterator _it = robot->ports.begin(); _it != robot->ports.end(); _it++) + { + // set goal velocity = 0 + _protocol2_handler->Write4ByteTxOnly(_it->second, BROADCAST_ID, GOAL_VELOCITY_ADDR, 0); + // set goal acceleration = 0 + _protocol2_handler->Write4ByteTxOnly(_it->second, BROADCAST_ID, GOAL_ACCELERATION_ADDR, 0); + } + + /* temporary code end */ + + queue_thread_ = boost::thread(boost::bind(&RobotisController::QueueThread, this)); + return true; +} + +void RobotisController::QueueThread() +{ + ros::NodeHandle _ros_node; + ros::CallbackQueue _callback_queue; + + _ros_node.setCallbackQueue(&_callback_queue); + + /* subscriber */ + ros::Subscriber _sync_write_item_sub= _ros_node.subscribe("/robotis/sync_write_item", 10, &RobotisController::SyncWriteItemCallback, this); + ros::Subscriber _ctrl_module_sub = _ros_node.subscribe("/robotis/set_ctrl_module", 10, &RobotisController::SetCtrlModuleCallback, this); + ros::Subscriber _controller_mode_sub= _ros_node.subscribe("/robotis/set_controller_mode", 10, &RobotisController::SetControllerModeCallback, this); + ros::Subscriber _direct_control_sub = _ros_node.subscribe("/robotis/set_joint_states", 10, &RobotisController::SetJointStatesCallback, this); + + /* publisher */ + goal_joint_state_pub = _ros_node.advertise("/robotis/goal_joint_states", 10); + present_joint_state_pub = _ros_node.advertise("/robotis/present_joint_states", 10); + current_module_pub = _ros_node.advertise("/robotis/current_ctrl_module", 10); + + /* service */ + ros::ServiceServer _joint_module_server = _ros_node.advertiseService("/robotis/get_ctrl_module", &RobotisController::GetCtrlModuleCallback, this); + + while(_ros_node.ok()) + { + _callback_queue.callAvailable(); + } +} + +void *RobotisController::ThreadProc(void *param) +{ + RobotisController *controller = (RobotisController *)param; + static struct timespec next_time; + static struct timespec curr_time; + + ROS_INFO("controller::thread_proc"); + + clock_gettime(CLOCK_MONOTONIC, &next_time); + + while(!controller->stop_timer_) + { + next_time.tv_sec += (next_time.tv_nsec + CONTROL_CYCLE_MSEC * 1000000) / 1000000000; + next_time.tv_nsec = (next_time.tv_nsec + CONTROL_CYCLE_MSEC * 1000000) % 1000000000; + + controller->Process(); + + clock_gettime(CLOCK_MONOTONIC, &curr_time); + long delta_nsec = (next_time.tv_sec - curr_time.tv_sec) * 1000000000 + (next_time.tv_nsec - curr_time.tv_nsec); + if(delta_nsec < -0.3 ) + { + if(controller->DEBUG_PRINT == true) + ROS_WARN("[RobotisController::ThreadProc] NEXT TIME < CURR TIME.. (%f)[%ld.%09ld / %ld.%09ld]", delta_nsec/1000000.0, (long)next_time.tv_sec, (long)next_time.tv_nsec, (long)curr_time.tv_sec, (long)curr_time.tv_nsec); + // next_time = curr_time + 3 msec + next_time.tv_sec = curr_time.tv_sec + (curr_time.tv_nsec + 3000000) / 1000000000; + next_time.tv_nsec = (curr_time.tv_nsec + 3000000) % 1000000000; + } + + clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &next_time, NULL); + } + return 0; +} + +void RobotisController::StartTimer() +{ + if(this->is_timer_running_ == true) + return; + + InitSyncWrite(); + + for(std::map::iterator _it = port_to_bulk_read.begin(); _it != port_to_bulk_read.end(); _it++) + _it->second->TxPacket(); + + int error; + struct sched_param param; + pthread_attr_t attr; + + pthread_attr_init(&attr); + + error = pthread_attr_setschedpolicy(&attr, SCHED_RR); + if(error != 0) + ROS_ERROR("pthread_attr_setschedpolicy error = %d\n",error); + error = pthread_attr_setinheritsched(&attr,PTHREAD_EXPLICIT_SCHED); + if(error != 0) + ROS_ERROR("pthread_attr_setinheritsched error = %d\n",error); + + memset(¶m, 0, sizeof(param)); + param.sched_priority = 31;// RT + error = pthread_attr_setschedparam(&attr, ¶m); + if(error != 0) + ROS_ERROR("pthread_attr_setschedparam error = %d\n",error); + + // create and start the thread + if((error = pthread_create(&this->timer_thread_, &attr, this->ThreadProc, this))!= 0) { + ROS_ERROR("timer thread create fail!!"); + exit(-1); + } + + this->is_timer_running_ = true; +} + +void RobotisController::StopTimer() +{ + int error = 0; + + // set the flag to stop the thread + if(this->is_timer_running_) + { + this->stop_timer_ = true; + + // wait until the thread is stopped. + if((error = pthread_join(this->timer_thread_, NULL)) != 0) + exit(-1); + + for(std::map::iterator _it = port_to_bulk_read.begin(); _it != port_to_bulk_read.end(); _it++) + _it->second->RxPacket(); + + for(std::map::iterator _it = port_to_sync_write.begin(); _it != port_to_sync_write.end(); _it++) + _it->second->ClearParam(); + + this->stop_timer_ = false; + this->is_timer_running_ = false; + } +} + +bool RobotisController::IsTimerRunning() +{ + return this->is_timer_running_; +} + +void RobotisController::LoadOffset(const std::string path) +{ + YAML::Node _doc; + try{ + _doc = YAML::LoadFile(path.c_str()); + } catch(const std::exception& e) { + ROS_ERROR("Fail to load offset yaml."); + return; + } + + YAML::Node _offset_node = _doc["offset"]; + if(_offset_node.size() == 0) + return; + + ROS_INFO("Load offsets..."); + for(YAML::const_iterator _it = _offset_node.begin(); _it != _offset_node.end(); _it++) + { + std::string _joint_name = _it->first.as(); + double _offset = _it->second.as(); + + std::map::iterator _dxl_it = robot->dxls.find(_joint_name); + if(_dxl_it != robot->dxls.end()) + _dxl_it->second->dxl_state->position_offset = _offset; + } +} + +void RobotisController::Process() +{ + static bool _is_process_running = false; + + if(_is_process_running == true) + return; + _is_process_running = true; + + // ROS_INFO("Controller::Process()"); + + sensor_msgs::JointState _present_state; + sensor_msgs::JointState _goal_state; + + ros::Time _now = ros::Time::now(); + + + // TODO: BulkRead Rx + bool _do_sync_write = false; + + for(std::map::iterator _it = port_to_bulk_read.begin(); _it != port_to_bulk_read.end(); _it++) + _it->second->RxPacket(); + + ros::Duration _dur = ros::Time::now() - _now; + double _msec = _dur.nsec * 0.000001; + + if(_msec > 8) std::cout << "Process duration : " << _msec << std::endl; + + // -> save to Robot->dxls[]->dynamixel_state.present_position + // -> update time stamp to Robot->dxls[]->dynamixel_state.update_time_stamp + for(std::map::iterator dxl_it = robot->dxls.begin(); dxl_it != robot->dxls.end(); dxl_it++) + { + UINT32_T _pos = 0; + + std::string _joint_name = dxl_it->first; + Dynamixel *_dxl = dxl_it->second; + + if(port_to_bulk_read[_dxl->port_name]->GetData(_dxl->id, PRESENT_POSITION_ADDR, &_pos) == true) + _dxl->dxl_state->present_position = _dxl->ConvertValue2Radian(_pos) - _dxl->dxl_state->position_offset; // remove offset + + UINT16_T _ext_data = 0; + if(_dxl->id == 11 || _dxl->id == 12 || _dxl->id == 23 || _dxl->id == 24) + { + if(port_to_bulk_read[_dxl->port_name]->GetData(_dxl->id, EXT_PORT_DATA1_ADDR, &_ext_data) == true) + _dxl->dxl_state->ext_port_data[0] = _ext_data; + if(port_to_bulk_read[_dxl->port_name]->GetData(_dxl->id, EXT_PORT_DATA2_ADDR, &_ext_data) == true) + _dxl->dxl_state->ext_port_data[1] = _ext_data; + } + else if(_dxl->id == 13 || _dxl->id == 14 || _dxl->id == 25 || _dxl->id == 26) + { + if(port_to_bulk_read[_dxl->port_name]->GetData(_dxl->id, EXT_PORT_DATA1_ADDR, &_ext_data) == true) + _dxl->dxl_state->ext_port_data[0] = _ext_data; + if(port_to_bulk_read[_dxl->port_name]->GetData(_dxl->id, EXT_PORT_DATA2_ADDR, &_ext_data) == true) + _dxl->dxl_state->ext_port_data[1] = _ext_data; + if(port_to_bulk_read[_dxl->port_name]->GetData(_dxl->id, EXT_PORT_DATA3_ADDR, &_ext_data) == true) + _dxl->dxl_state->ext_port_data[2] = _ext_data; + if(port_to_bulk_read[_dxl->port_name]->GetData(_dxl->id, EXT_PORT_DATA4_ADDR, &_ext_data) == true) + _dxl->dxl_state->ext_port_data[3] = _ext_data; + } + + _present_state.name.push_back(_joint_name); + // TODO: should be converted to rad, rad/s, Nm + _present_state.position.push_back(_dxl->dxl_state->present_position); + _present_state.velocity.push_back(_dxl->dxl_state->present_velocity); + _present_state.effort.push_back(_dxl->dxl_state->present_load); + + _goal_state.name.push_back(_joint_name); + _goal_state.position.push_back(_dxl->dxl_state->goal_position); + _goal_state.velocity.push_back(_dxl->dxl_state->goal_velocity); + _goal_state.effort.push_back(_dxl->dxl_state->goal_torque); + } + // -> publish present joint_states topic + _present_state.header.stamp = ros::Time::now(); + present_joint_state_pub.publish(_present_state); + + // -> publish goal joint_states topic + _goal_state.header.stamp = _present_state.header.stamp; + goal_joint_state_pub.publish(_goal_state); + + if(controller_mode_ == MOTION_MODULE_MODE) + { + // TODO: Call MotionModule Process() + // -> for loop : call MotionModule list -> Process() + if(modules_.size() > 0) + { + queue_mutex_.lock(); + + for(std::list::iterator module_it = modules_.begin(); module_it != modules_.end(); module_it++) + { + // ros::Time _before = ros::Time::now(); + + (*module_it)->Process(robot->dxls); + + // ros::Duration _dur = ros::Time::now() - _before; + // double _msec = _dur.nsec * 0.000001; + + // std::cout << "Process duration ["<< (*module_it)->module_name << "] : " << _msec << std::endl; + + // for loop : joint list + for(std::map::iterator dxl_it = robot->dxls.begin(); dxl_it != robot->dxls.end(); dxl_it++) + { + std::string _joint_name = dxl_it->first; + Dynamixel *_dxl = dxl_it->second; + + DynamixelState *_dxl_state = _dxl->dxl_state; + if(_dxl->ctrl_module_name == (*module_it)->module_name) + { + _do_sync_write = true; + // ROS_INFO("Set goal value"); + DynamixelState *_result_state = (*module_it)->result[_joint_name]; + + if(_result_state == NULL) { + ROS_INFO("[%s] %s", (*module_it)->module_name.c_str(), _joint_name.c_str()); + continue; + } + + // TODO: check update time stamp ? + + if((*module_it)->control_mode == POSITION_CONTROL) + { +// if(_result_state->goal_position == 0 && _dxl->id == 3) +// ROS_INFO("[MODULE:%s][ID:3] goal position = %f", (*module_it)->module_name.c_str(), _dxl_state->goal_position); + _dxl_state->goal_position = _result_state->goal_position; + + // add offset + UINT32_T _pos_data = _dxl->ConvertRadian2Value(_dxl_state->goal_position + _dxl_state->position_offset); + UINT8_T _sync_write_data[4]; + _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_pos_data)); + _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_pos_data)); + _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_pos_data)); + _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_pos_data)); + + if(abs(_pos_data) > 151800) + printf("goal_pos : %f | position_offset : %f | pos_data : %d\n", _dxl_state->goal_position , _dxl_state->position_offset, _pos_data); + + port_to_sync_write[_dxl->port_name]->ChangeParam(_dxl->id, _sync_write_data); + } + else if((*module_it)->control_mode == VELOCITY_CONTROL) + { + _dxl_state->goal_velocity = _result_state->goal_velocity; + } + else if((*module_it)->control_mode == TORQUE_CONTROL) + { + _dxl_state->goal_torque = _result_state->goal_torque; + } + } + } + } + + // std::cout << " ------------------------------------------- " << std::endl; + queue_mutex_.unlock(); + } + + // TODO: Combine the result && SyncWrite + // -> SyncWrite + if(_do_sync_write) { + for(std::map::iterator _it = port_to_sync_write.begin(); _it != port_to_sync_write.end(); _it++) + _it->second->TxPacket(); + } + } + else if(controller_mode_ == DIRECT_CONTROL_MODE) + { + queue_mutex_.lock(); + + for(std::map::iterator _it = port_to_sync_write.begin(); _it != port_to_sync_write.end(); _it++) + { + _it->second->TxPacket(); + _it->second->ClearParam(); + } + + if(direct_sync_write_.size() > 0) + { + for(int _i = 0; _i < direct_sync_write_.size(); _i++) + { + direct_sync_write_[_i]->TxPacket(); + direct_sync_write_[_i]->ClearParam(); + } + direct_sync_write_.clear(); + } + + queue_mutex_.unlock(); + } + + // TODO: User Read/Write + + // TODO: BulkRead Tx + for(std::map::iterator _it = port_to_bulk_read.begin(); _it != port_to_bulk_read.end(); _it++) + _it->second->TxPacket(); + + // for test : goal to present + // for(std::map::iterator dxl_it = robot_->dxls.begin(); dxl_it != robot_->dxls.end(); dxl_it++) + // { + // dxl_it->second->dxl_state->present_position = dxl_it->second->dxl_state->goal_position; + // dxl_it->second->dxl_state->present_velocity = dxl_it->second->dxl_state->goal_velocity; + // } + + +// ros::Duration _dur = ros::Time::now() - _now; +// double _msec = _dur.nsec * 0.000001; +// +// if(_msec > 8) std::cout << "Process duration : " << _msec << std::endl; + + _is_process_running = false; +} + +void RobotisController::AddModule(MotionModule *module) +{ + module->Initialize(CONTROL_CYCLE_MSEC); + modules_.push_back(module); + modules_.unique(); +} + +void RobotisController::RemoveModule(MotionModule *module) +{ + modules_.remove(module); +} + +void RobotisController::SyncWriteItemCallback(const robotis_controller_msgs::SyncWriteItem::ConstPtr &msg) +{ + for(int _i = 0; _i < msg->joint_name.size(); _i++) + { + Dynamixel *_dxl = robot->dxls[msg->joint_name[_i]]; + ControlTableItem *_item = _dxl->ctrl_table[msg->item_name]; + + PortHandler *_port = robot->ports[_dxl->port_name]; + PacketHandler *_packet_handler= PacketHandler::GetPacketHandler(_dxl->protocol_version); + + if(_item->access_type == READ) + continue; + + int _idx = 0; + if(direct_sync_write_.size() == 0) + { + direct_sync_write_.push_back(new GroupSyncWrite(_port, _packet_handler, _item->address, _item->data_length)); + _idx = 0; + } + else + { + for(_idx = 0; _idx < direct_sync_write_.size(); _idx++) + { + if(direct_sync_write_[_idx]->GetPortHandler() == _port && + direct_sync_write_[_idx]->GetPacketHandler() == _packet_handler) + break; + } + + if(_idx == direct_sync_write_.size()) + direct_sync_write_.push_back(new GroupSyncWrite(_port, _packet_handler, _item->address, _item->data_length)); + } + + UINT8_T *_data = new UINT8_T[_item->data_length]; + if(_item->data_length == 1) + _data[0] = (UINT8_T)msg->value[_i]; + else if(_item->data_length == 2) + { + _data[0] = DXL_LOBYTE((UINT16_T)msg->value[_i]); + _data[1] = DXL_HIBYTE((UINT16_T)msg->value[_i]); + } + else if(_item->data_length == 4) + { + _data[0] = DXL_LOBYTE(DXL_LOWORD((UINT32_T)msg->value[_i])); + _data[1] = DXL_HIBYTE(DXL_LOWORD((UINT32_T)msg->value[_i])); + _data[2] = DXL_LOBYTE(DXL_HIWORD((UINT32_T)msg->value[_i])); + _data[3] = DXL_HIBYTE(DXL_HIWORD((UINT32_T)msg->value[_i])); + } + direct_sync_write_[_idx]->AddParam(_dxl->id, _data); + delete[] _data; + } +} + +void RobotisController::SetControllerModeCallback(const std_msgs::String::ConstPtr &msg) +{ + if(msg->data == "DIRECT_CONTROL_MODE") + controller_mode_ = DIRECT_CONTROL_MODE; + else if(msg->data == "MOTION_MODULE_MODE") + controller_mode_ = MOTION_MODULE_MODE; +} + +void RobotisController::SetJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg) +{ + if(controller_mode_ != DIRECT_CONTROL_MODE) + return; + + queue_mutex_.lock(); + + for(int _i = 0; _i < msg->name.size(); _i++) + { + INT32_T _pos = 0; + + Dynamixel *_dxl = robot->dxls[msg->name[_i]]; + if(_dxl == NULL) + continue; + + _dxl->dxl_state->goal_position = msg->position[_i]; + _pos = _dxl->ConvertRadian2Value((double)msg->position[_i]); + + UINT8_T _sync_write_data[4]; + _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_pos)); + _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_pos)); + _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_pos)); + _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_pos)); + + port_to_sync_write[_dxl->port_name]->AddParam(_dxl->id, _sync_write_data); + } + + queue_mutex_.unlock(); +} + +void RobotisController::SetCtrlModuleCallback(const robotis_controller_msgs::JointCtrlModule::ConstPtr &msg) +{ + if(msg->joint_name.size() != msg->module_name.size()) + return; + + queue_mutex_.lock(); + + for(unsigned int idx = 0; idx < msg->joint_name.size(); idx++) + { + Dynamixel *_dxl = NULL; + std::map::iterator _dxl_it = robot->dxls.find((std::string)(msg->joint_name[idx])); + if(_dxl_it != robot->dxls.end()) + _dxl = _dxl_it->second; + else + continue; + + // none + if(msg->module_name[idx] == "" || msg->module_name[idx] == "none") + { + _dxl->ctrl_module_name = msg->module_name[idx]; + continue; + } + + // check whether the module is using this joint + for(std::list::iterator _m_it = modules_.begin(); _m_it != modules_.end(); _m_it++) + { + if((*_m_it)->module_name == msg->module_name[idx]) + { + if((*_m_it)->result.find(msg->joint_name[idx]) != (*_m_it)->result.end()) + { + _dxl->ctrl_module_name = msg->module_name[idx]; + break; + } + } + } + } + + for(std::list::iterator _m_it = modules_.begin(); _m_it != modules_.end(); _m_it++) + { + // set all modules -> disable + (*_m_it)->enable = false; + + // set all used modules -> enable + for(std::map::iterator _d_it = robot->dxls.begin(); _d_it != robot->dxls.end(); _d_it++) + { + if(_d_it->second->ctrl_module_name == (*_m_it)->module_name) + { + (*_m_it)->enable = true; + break; + } + } + } + + // TODO: set indirect address + // -> check module's control_mode + + queue_mutex_.unlock(); + + robotis_controller_msgs::JointCtrlModule _current_module_msg; + for(std::map::iterator _dxl_iter = robot->dxls.begin(); _dxl_iter != robot->dxls.end(); ++_dxl_iter) + { + _current_module_msg.joint_name.push_back(_dxl_iter->first); + _current_module_msg.module_name.push_back(_dxl_iter->second->ctrl_module_name); + } + + if(_current_module_msg.joint_name.size() == _current_module_msg.module_name.size()) + current_module_pub.publish(_current_module_msg); +} + +bool RobotisController::GetCtrlModuleCallback(robotis_controller_msgs::GetJointModule::Request &req, robotis_controller_msgs::GetJointModule::Response &res) +{ + for(unsigned int idx = 0; idx < req.joint_name.size(); idx++) + { + std::map::iterator _d_it = robot->dxls.find((std::string)(req.joint_name[idx])); + if(_d_it != robot->dxls.end()) + { + res.joint_name.push_back(req.joint_name[idx]); + res.module_name.push_back(_d_it->second->ctrl_module_name); + } + } + + if(res.joint_name.size() == 0) return false; + + return true; +} + +bool RobotisController::CheckTimerStop() +{ + if(this->is_timer_running_) + { + if(DEBUG_PRINT == true) + ROS_WARN("Process Timer is running.. STOP the timer first."); + return false; + } + return true; +} + +int RobotisController::Ping(const std::string joint_name, UINT8_T *error) +{ + if(CheckTimerStop() == false) + return COMM_PORT_BUSY; + + Dynamixel *_dxl = robot->dxls[joint_name]; + if(_dxl == NULL) + return COMM_NOT_AVAILABLE; + + PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); + PortHandler *_port_handler = robot->ports[_dxl->port_name]; + + return _pkt_handler->Ping(_port_handler, _dxl->id, error); +} +int RobotisController::Ping(const std::string joint_name, UINT16_T* model_number, UINT8_T *error) +{ + if(CheckTimerStop() == false) + return COMM_PORT_BUSY; + + Dynamixel *_dxl = robot->dxls[joint_name]; + if(_dxl == NULL) + return COMM_NOT_AVAILABLE; + + PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); + PortHandler *_port_handler = robot->ports[_dxl->port_name]; + + return _pkt_handler->Ping(_port_handler, _dxl->id, model_number, error); +} + +int RobotisController::Action(const std::string joint_name) +{ + if(CheckTimerStop() == false) + return COMM_PORT_BUSY; + + Dynamixel *_dxl = robot->dxls[joint_name]; + if(_dxl == NULL) + return COMM_NOT_AVAILABLE; + + PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); + PortHandler *_port_handler = robot->ports[_dxl->port_name]; + + return _pkt_handler->Action(_port_handler, _dxl->id); +} +int RobotisController::Reboot(const std::string joint_name, UINT8_T *error) +{ + if(CheckTimerStop() == false) + return COMM_PORT_BUSY; + + Dynamixel *_dxl = robot->dxls[joint_name]; + if(_dxl == NULL) + return COMM_NOT_AVAILABLE; + + PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); + PortHandler *_port_handler = robot->ports[_dxl->port_name]; + + return _pkt_handler->Reboot(_port_handler, _dxl->id, error); +} +int RobotisController::FactoryReset(const std::string joint_name, UINT8_T option, UINT8_T *error) +{ + if(CheckTimerStop() == false) + return COMM_PORT_BUSY; + + Dynamixel *_dxl = robot->dxls[joint_name]; + if(_dxl == NULL) + return COMM_NOT_AVAILABLE; + + PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); + PortHandler *_port_handler = robot->ports[_dxl->port_name]; + + return _pkt_handler->FactoryReset(_port_handler, _dxl->id, option, error); +} + +int RobotisController::Read(const std::string joint_name, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error) +{ + if(CheckTimerStop() == false) + return COMM_PORT_BUSY; + + Dynamixel *_dxl = robot->dxls[joint_name]; + if(_dxl == NULL) + return COMM_NOT_AVAILABLE; + + PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); + PortHandler *_port_handler = robot->ports[_dxl->port_name]; + + return _pkt_handler->ReadTxRx(_port_handler, _dxl->id, address, length, data, error); +} + +int RobotisController::ReadCtrlItem(const std::string joint_name, const std::string item_name, UINT32_T *data, UINT8_T *error) +{ + if(CheckTimerStop() == false) + return COMM_PORT_BUSY; + + Dynamixel *_dxl = robot->dxls[joint_name]; + if(_dxl == NULL) + return COMM_NOT_AVAILABLE; + + ControlTableItem*_item = _dxl->ctrl_table[item_name]; + if(_item == NULL) + return COMM_NOT_AVAILABLE; + + PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); + PortHandler *_port_handler = robot->ports[_dxl->port_name]; + + int _result = COMM_NOT_AVAILABLE; + switch(_item->data_length) + { + case 1: + { + UINT8_T _data = 0; + _result = _pkt_handler->Read1ByteTxRx(_port_handler, _dxl->id, _item->address, &_data, error); + if(_result == COMM_SUCCESS) + *data = _data; + break; + } + case 2: + { + UINT16_T _data = 0; + _result = _pkt_handler->Read2ByteTxRx(_port_handler, _dxl->id, _item->address, &_data, error); + if(_result == COMM_SUCCESS) + *data = _data; + break; + } + case 4: + { + UINT32_T _data = 0; + _result = _pkt_handler->Read4ByteTxRx(_port_handler, _dxl->id, _item->address, &_data, error); + if(_result == COMM_SUCCESS) + *data = _data; + break; + } + default: + break; + } + return _result; +} + +int RobotisController::Read1Byte(const std::string joint_name, UINT16_T address, UINT8_T *data, UINT8_T *error) +{ + if(CheckTimerStop() == false) + return COMM_PORT_BUSY; + + Dynamixel *_dxl = robot->dxls[joint_name]; + if(_dxl == NULL) + return COMM_NOT_AVAILABLE; + + PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); + PortHandler *_port_handler = robot->ports[_dxl->port_name]; + + return _pkt_handler->Read1ByteTxRx(_port_handler, _dxl->id, address, data, error); +} + +int RobotisController::Read2Byte(const std::string joint_name, UINT16_T address, UINT16_T *data, UINT8_T *error) +{ + if(CheckTimerStop() == false) + return COMM_PORT_BUSY; + + Dynamixel *_dxl = robot->dxls[joint_name]; + if(_dxl == NULL) + return COMM_NOT_AVAILABLE; + + PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); + PortHandler *_port_handler = robot->ports[_dxl->port_name]; + + return _pkt_handler->Read2ByteTxRx(_port_handler, _dxl->id, address, data, error); +} + +int RobotisController::Read4Byte(const std::string joint_name, UINT16_T address, UINT32_T *data, UINT8_T *error) +{ + if(CheckTimerStop() == false) + return COMM_PORT_BUSY; + + Dynamixel *_dxl = robot->dxls[joint_name]; + if(_dxl == NULL) + return COMM_NOT_AVAILABLE; + + PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); + PortHandler *_port_handler = robot->ports[_dxl->port_name]; + + return _pkt_handler->Read4ByteTxRx(_port_handler, _dxl->id, address, data, error); +} + +int RobotisController::Write(const std::string joint_name, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error) +{ + if(CheckTimerStop() == false) + return COMM_PORT_BUSY; + + Dynamixel *_dxl = robot->dxls[joint_name]; + if(_dxl == NULL) + return COMM_NOT_AVAILABLE; + + PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); + PortHandler *_port_handler = robot->ports[_dxl->port_name]; + + return _pkt_handler->WriteTxRx(_port_handler, _dxl->id, address, length, data, error); +} + +int RobotisController::WriteCtrlItem(const std::string joint_name, const std::string item_name, UINT32_T data, UINT8_T *error) +{ + if(CheckTimerStop() == false) + return COMM_PORT_BUSY; + + Dynamixel *_dxl = robot->dxls[joint_name]; + if(_dxl == NULL) + return COMM_NOT_AVAILABLE; + + ControlTableItem*_item = _dxl->ctrl_table[item_name]; + if(_item == NULL) + return COMM_NOT_AVAILABLE; + + PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); + PortHandler *_port_handler = robot->ports[_dxl->port_name]; + + int _result = COMM_NOT_AVAILABLE; + UINT8_T *_data = new UINT8_T[_item->data_length]; + if(_item->data_length == 1) + { + _data[0] = (UINT8_T)data; + _result = _pkt_handler->Write1ByteTxRx(_port_handler, _dxl->id, _item->address, data, error); + } + else if(_item->data_length == 2) + { + _data[0] = DXL_LOBYTE((UINT16_T)data); + _data[1] = DXL_HIBYTE((UINT16_T)data); + _result = _pkt_handler->Write2ByteTxRx(_port_handler, _dxl->id, _item->address, data, error); + } + else if(_item->data_length == 4) + { + _data[0] = DXL_LOBYTE(DXL_LOWORD((UINT32_T)data)); + _data[1] = DXL_HIBYTE(DXL_LOWORD((UINT32_T)data)); + _data[2] = DXL_LOBYTE(DXL_HIWORD((UINT32_T)data)); + _data[3] = DXL_HIBYTE(DXL_HIWORD((UINT32_T)data)); + _result = _pkt_handler->Write4ByteTxRx(_port_handler, _dxl->id, _item->address, data, error); + } + delete[] _data; + return _result; +} + +int RobotisController::Write1Byte(const std::string joint_name, UINT16_T address, UINT8_T data, UINT8_T *error) +{ + if(CheckTimerStop() == false) + return COMM_PORT_BUSY; + + Dynamixel *_dxl = robot->dxls[joint_name]; + if(_dxl == NULL) + return COMM_NOT_AVAILABLE; + + PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); + PortHandler *_port_handler = robot->ports[_dxl->port_name]; + + return _pkt_handler->Write1ByteTxRx(_port_handler, _dxl->id, address, data, error); +} + +int RobotisController::Write2Byte(const std::string joint_name, UINT16_T address, UINT16_T data, UINT8_T *error) +{ + if(CheckTimerStop() == false) + return COMM_PORT_BUSY; + + Dynamixel *_dxl = robot->dxls[joint_name]; + if(_dxl == NULL) + return COMM_NOT_AVAILABLE; + + PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); + PortHandler *_port_handler = robot->ports[_dxl->port_name]; + + return _pkt_handler->Write2ByteTxRx(_port_handler, _dxl->id, address, data, error); +} + +int RobotisController::Write4Byte(const std::string joint_name, UINT16_T address, UINT32_T data, UINT8_T *error) +{ + if(CheckTimerStop() == false) + return COMM_PORT_BUSY; + + Dynamixel *_dxl = robot->dxls[joint_name]; + if(_dxl == NULL) + return COMM_NOT_AVAILABLE; + + PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); + PortHandler *_port_handler = robot->ports[_dxl->port_name]; + + return _pkt_handler->Write4ByteTxRx(_port_handler, _dxl->id, address, data, error); +} + +int RobotisController::RegWrite(const std::string joint_name, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error) +{ + if(CheckTimerStop() == false) + return COMM_PORT_BUSY; + + Dynamixel *_dxl = robot->dxls[joint_name]; + if(_dxl == NULL) + return COMM_NOT_AVAILABLE; + + PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); + PortHandler *_port_handler = robot->ports[_dxl->port_name]; + + return _pkt_handler->RegWriteTxRx(_port_handler, _dxl->id, address, length, data, error); +} + diff --git a/robotis_controller_msgs/CMakeLists.txt b/robotis_controller_msgs/CMakeLists.txt new file mode 100644 index 0000000..221d97d --- /dev/null +++ b/robotis_controller_msgs/CMakeLists.txt @@ -0,0 +1,47 @@ +cmake_minimum_required(VERSION 2.8.3) +project(robotis_controller_msgs) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + sensor_msgs + std_msgs + message_generation +) + +## Generate messages in the 'msg' folder +add_message_files( + FILES + SyncWriteItem.msg + JointCtrlModule.msg +) + +## Generate services in the 'srv' folder +add_service_files( + FILES + GetJointModule.srv +) + # Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +generate_messages( + DEPENDENCIES + std_msgs + sensor_msgs +) + +catkin_package( +# INCLUDE_DIRS include + CATKIN_DEPENDS sensor_msgs std_msgs +) + diff --git a/robotis_controller_msgs/msg/JointCtrlModule.msg b/robotis_controller_msgs/msg/JointCtrlModule.msg new file mode 100644 index 0000000..b91eb4d --- /dev/null +++ b/robotis_controller_msgs/msg/JointCtrlModule.msg @@ -0,0 +1,2 @@ +string[] joint_name +string[] module_name \ No newline at end of file diff --git a/robotis_controller_msgs/msg/SyncWriteItem.msg b/robotis_controller_msgs/msg/SyncWriteItem.msg new file mode 100644 index 0000000..4d602b6 --- /dev/null +++ b/robotis_controller_msgs/msg/SyncWriteItem.msg @@ -0,0 +1,3 @@ +string item_name +string[] joint_name +uint32[] value \ No newline at end of file diff --git a/robotis_controller_msgs/package.xml b/robotis_controller_msgs/package.xml new file mode 100644 index 0000000..88a4148 --- /dev/null +++ b/robotis_controller_msgs/package.xml @@ -0,0 +1,30 @@ + + + robotis_controller_msgs + 0.1.0 + The robotis_controller_msgs package + robotis + + GPLv2 + + + + robotis --> + + catkin + + sensor_msgs + std_msgs + message_generation + + sensor_msgs + std_msgs + message_runtime + + + + + + + + \ No newline at end of file diff --git a/robotis_controller_msgs/srv/GetJointModule.srv b/robotis_controller_msgs/srv/GetJointModule.srv new file mode 100644 index 0000000..bedde91 --- /dev/null +++ b/robotis_controller_msgs/srv/GetJointModule.srv @@ -0,0 +1,4 @@ +string[] joint_name +--- +string[] joint_name +string[] module_name \ No newline at end of file diff --git a/robotis_device/CMakeLists.txt b/robotis_device/CMakeLists.txt new file mode 100644 index 0000000..6580f81 --- /dev/null +++ b/robotis_device/CMakeLists.txt @@ -0,0 +1,61 @@ +cmake_minimum_required(VERSION 2.8.3) +project(robotis_device) + +find_package(catkin REQUIRED COMPONENTS + roscpp + rospy + dynamixel_sdk + robotis_framework_common +) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if you package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( + INCLUDE_DIRS include +# LIBRARIES robotis_device + CATKIN_DEPENDS roscpp rospy +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +# include_directories(include) +include_directories( + include + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +add_library(robotis_device + src/robotis_device/Robot.cpp + src/robotis_device/Dynamixel.cpp +) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +add_dependencies(robotis_device ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +# add_executable(robotis_device_node src/robotis_device_node.cpp) + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(robotis_device_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +target_link_libraries(robotis_device + dynamixel_sdk + ${catkin_LIBRARIES} +) diff --git a/robotis_device/devices/dynamixel/GRIPPER.device b/robotis_device/devices/dynamixel/GRIPPER.device new file mode 100644 index 0000000..42de920 --- /dev/null +++ b/robotis_device/devices/dynamixel/GRIPPER.device @@ -0,0 +1,70 @@ +[device info] +model_name = GRIPPER +device_type = Dynamixel + +[type info] +0_radian_position_value = 0 +min_radian_position_value = 0 +max_radian_position_value = 750 +min_radian = 0 +max_radian = 1.150767 +torque_enable_address = 562 +position_d_gain_address = +position_i_gain_address = +position_p_gain_address = 594 +goal_position_address = 596 +goal_velocity_address = 600 +goal_torque_address = 604 +present_position_address = 611 +present_velocity_address = 615 +present_load_address = 621 +is_moving_address = 610 + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 30 | torque_limit | 2 | RW | EEPROM | 0 | 32767 | N + 32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 562 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 586 | velocity_I_gain | 2 | RW | RAM | 0 | 2047 | N + 588 | velocity_P_gain | 2 | RW | RAM | 0 | 2047 | N + 594 | position_P_gain | 2 | RW | RAM | 0 | 2047 | N + 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y + 606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N + 610 | is_moving | 1 | R | RAM | 0 | 1 | N + 611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 621 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 623 | present_voltage | 2 | R | RAM | 0 | 500 | N + 625 | present_temperature | 1 | R | RAM | 0 | 200 | N + 626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 891 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N diff --git a/robotis_device/devices/dynamixel/H42-20-S300-R.device b/robotis_device/devices/dynamixel/H42-20-S300-R.device new file mode 100644 index 0000000..f5ae090 --- /dev/null +++ b/robotis_device/devices/dynamixel/H42-20-S300-R.device @@ -0,0 +1,71 @@ +[device info] +model_name = H42-20-S300-R +device_type = Dynamixel + +[type info] +0_radian_position_value = 0 +min_radian_position_value = -151900 +max_radian_position_value = 151900 +min_radian = -3.14159265 +max_radian = 3.14159265 +torque_enable_address = 562 +position_d_gain_address = +position_i_gain_address = +position_p_gain_address = 594 +goal_position_address = 596 +goal_velocity_address = 600 +goal_torque_address = 604 +present_position_address = 611 +present_velocity_address = 615 +present_load_address = 621 +is_moving_address = 610 + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 13 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 30 | torque_limit | 2 | RW | EEPROM | 0 | 32767 | N + 32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 562 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 586 | velocity_I_gain | 2 | RW | RAM | 0 | 2047 | N + 588 | velocity_P_gain | 2 | RW | RAM | 0 | 2047 | N + 594 | position_P_gain | 2 | RW | RAM | 0 | 2047 | N + 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y + 606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N + 610 | is_moving | 1 | R | RAM | 0 | 1 | N + 611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 621 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 623 | present_voltage | 2 | R | RAM | 0 | 500 | N + 625 | present_temperature | 1 | R | RAM | 0 | 200 | N + 626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 891 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N diff --git a/robotis_device/devices/dynamixel/H54-100-S500-R.device b/robotis_device/devices/dynamixel/H54-100-S500-R.device new file mode 100644 index 0000000..4043cad --- /dev/null +++ b/robotis_device/devices/dynamixel/H54-100-S500-R.device @@ -0,0 +1,71 @@ +[device info] +model_name = H54-100-S500-R +device_type = Dynamixel + +[type info] +0_radian_position_value = 0 +min_radian_position_value = -250950 +max_radian_position_value = 250950 +min_radian = -3.14159265 +max_radian = 3.14159265 +torque_enable_address = 562 +position_d_gain_address = +position_i_gain_address = +position_p_gain_address = 594 +goal_position_address = 596 +goal_velocity_address = 600 +goal_torque_address = 604 +present_position_address = 611 +present_velocity_address = 615 +present_load_address = 621 +is_moving_address = 610 + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 13 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 30 | torque_limit | 2 | RW | EEPROM | 0 | 32767 | N + 32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 562 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 586 | velocity_I_gain | 2 | RW | RAM | 0 | 2047 | N + 588 | velocity_P_gain | 2 | RW | RAM | 0 | 2047 | N + 594 | position_P_gain | 2 | RW | RAM | 0 | 2047 | N + 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y + 606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N + 610 | is_moving | 1 | R | RAM | 0 | 1 | N + 611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 621 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 623 | present_voltage | 2 | R | RAM | 0 | 500 | N + 625 | present_temperature | 1 | R | RAM | 0 | 200 | N + 626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 891 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N diff --git a/robotis_device/devices/dynamixel/H54-200-B500-R.device b/robotis_device/devices/dynamixel/H54-200-B500-R.device new file mode 100644 index 0000000..c3f82c5 --- /dev/null +++ b/robotis_device/devices/dynamixel/H54-200-B500-R.device @@ -0,0 +1,71 @@ +[device info] +model_name = H54-200-B500-R +device_type = Dynamixel + +[type info] +0_radian_position_value = 0 +min_radian_position_value = -250950 +max_radian_position_value = 250950 +min_radian = -3.14159265 +max_radian = 3.14159265 +torque_enable_address = 562 +position_d_gain_address = +position_i_gain_address = +position_p_gain_address = 594 +goal_position_address = 596 +goal_velocity_address = 600 +goal_torque_address = 604 +present_position_address = 611 +present_velocity_address = 615 +present_load_address = 621 +is_moving_address = 610 + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 13 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 30 | torque_limit | 2 | RW | EEPROM | 0 | 32767 | N + 32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 562 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 586 | velocity_I_gain | 2 | RW | RAM | 0 | 2047 | N + 588 | velocity_P_gain | 2 | RW | RAM | 0 | 2047 | N + 594 | position_P_gain | 2 | RW | RAM | 0 | 2047 | N + 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y + 606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N + 610 | is_moving | 1 | R | RAM | 0 | 1 | N + 611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 621 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 623 | present_voltage | 2 | R | RAM | 0 | 500 | N + 625 | present_temperature | 1 | R | RAM | 0 | 200 | N + 626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 891 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N diff --git a/robotis_device/devices/dynamixel/H54-200-S500-R.device b/robotis_device/devices/dynamixel/H54-200-S500-R.device new file mode 100644 index 0000000..e29ba3e --- /dev/null +++ b/robotis_device/devices/dynamixel/H54-200-S500-R.device @@ -0,0 +1,71 @@ +[device info] +model_name = H54-200-S500-R +device_type = Dynamixel + +[type info] +0_radian_position_value = 0 +min_radian_position_value = -250950 +max_radian_position_value = 250950 +min_radian = -3.14159265 +max_radian = 3.14159265 +torque_enable_address = 562 +position_d_gain_address = +position_i_gain_address = +position_p_gain_address = 594 +goal_position_address = 596 +goal_velocity_address = 600 +goal_torque_address = 604 +present_position_address = 611 +present_velocity_address = 615 +present_load_address = 621 +is_moving_address = 610 + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 13 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 30 | torque_limit | 2 | RW | EEPROM | 0 | 32767 | N + 32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 562 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 586 | velocity_I_gain | 2 | RW | RAM | 0 | 2047 | N + 588 | velocity_P_gain | 2 | RW | RAM | 0 | 2047 | N + 594 | position_P_gain | 2 | RW | RAM | 0 | 2047 | N + 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y + 606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N + 610 | is_moving | 1 | R | RAM | 0 | 1 | N + 611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 621 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 623 | present_voltage | 2 | R | RAM | 0 | 500 | N + 625 | present_temperature | 1 | R | RAM | 0 | 200 | N + 626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 891 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N diff --git a/robotis_device/devices/dynamixel/L42-10-S300-R.device b/robotis_device/devices/dynamixel/L42-10-S300-R.device new file mode 100644 index 0000000..057d9a6 --- /dev/null +++ b/robotis_device/devices/dynamixel/L42-10-S300-R.device @@ -0,0 +1,70 @@ +[device info] +model_name = L42-10-S300-R +device_type = Dynamixel + +[type info] +0_radian_position_value = 0 +min_radian_position_value = -2047 +max_radian_position_value = 2048 +min_radian = -3.14159265 +max_radian = 3.14159265 +torque_enable_address = 562 +position_d_gain_address = +position_i_gain_address = +position_p_gain_address = 594 +goal_position_address = 596 +goal_velocity_address = 600 +goal_torque_address = 604 +present_position_address = 611 +present_velocity_address = 615 +present_load_address = 621 +is_moving_address = 610 + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 30 | torque_limit | 2 | RW | EEPROM | 0 | 32767 | N + 32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 562 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 586 | velocity_I_gain | 2 | RW | RAM | 0 | 2047 | N + 588 | velocity_P_gain | 2 | RW | RAM | 0 | 2047 | N + 594 | position_P_gain | 2 | RW | RAM | 0 | 2047 | N + 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y + 606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N + 610 | is_moving | 1 | R | RAM | 0 | 1 | N + 611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 621 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 623 | present_voltage | 2 | R | RAM | 0 | 500 | N + 625 | present_temperature | 1 | R | RAM | 0 | 200 | N + 626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 891 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N diff --git a/robotis_device/devices/dynamixel/L54-30-S400-R.device b/robotis_device/devices/dynamixel/L54-30-S400-R.device new file mode 100644 index 0000000..13e8d55 --- /dev/null +++ b/robotis_device/devices/dynamixel/L54-30-S400-R.device @@ -0,0 +1,71 @@ +[device info] +model_name = L54-30-S400-R +device_type = Dynamixel + +[type info] +0_radian_position_value = 0 +min_radian_position_value = -144198 +max_radian_position_value = 144198 +min_radian = -3.14159265 +max_radian = 3.14159265 +torque_enable_address = 562 +position_d_gain_address = +position_i_gain_address = +position_p_gain_address = 594 +goal_position_address = 596 +goal_velocity_address = 600 +goal_torque_address = 604 +present_position_address = 611 +present_velocity_address = 615 +present_load_address = 621 +is_moving_address = 610 + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 13 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 30 | torque_limit | 2 | RW | EEPROM | 0 | 32767 | N + 32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 562 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 586 | velocity_I_gain | 2 | RW | RAM | 0 | 2047 | N + 588 | velocity_P_gain | 2 | RW | RAM | 0 | 2047 | N + 594 | position_P_gain | 2 | RW | RAM | 0 | 2047 | N + 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y + 606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N + 610 | is_moving | 1 | R | RAM | 0 | 1 | N + 611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 621 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 623 | present_voltage | 2 | R | RAM | 0 | 500 | N + 625 | present_temperature | 1 | R | RAM | 0 | 200 | N + 626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 891 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N diff --git a/robotis_device/devices/dynamixel/L54-30-S500-R.device b/robotis_device/devices/dynamixel/L54-30-S500-R.device new file mode 100644 index 0000000..000fdb9 --- /dev/null +++ b/robotis_device/devices/dynamixel/L54-30-S500-R.device @@ -0,0 +1,71 @@ +[device info] +model_name = L54-30-S500-R +device_type = Dynamixel + +[type info] +0_radian_position_value = 0 +min_radian_position_value = -180684 +max_radian_position_value = 180684 +min_radian = -3.14159265 +max_radian = 3.14159265 +torque_enable_address = 562 +position_d_gain_address = +position_i_gain_address = +position_p_gain_address = 594 +goal_position_address = 596 +goal_velocity_address = 600 +goal_torque_address = 604 +present_position_address = 611 +present_velocity_address = 615 +present_load_address = 621 +is_moving_address = 610 + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 13 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 30 | torque_limit | 2 | RW | EEPROM | 0 | 32767 | N + 32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 562 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 586 | velocity_I_gain | 2 | RW | RAM | 0 | 2047 | N + 588 | velocity_P_gain | 2 | RW | RAM | 0 | 2047 | N + 594 | position_P_gain | 2 | RW | RAM | 0 | 2047 | N + 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y + 606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N + 610 | is_moving | 1 | R | RAM | 0 | 1 | N + 611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 621 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 623 | present_voltage | 2 | R | RAM | 0 | 500 | N + 625 | present_temperature | 1 | R | RAM | 0 | 200 | N + 626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 891 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N diff --git a/robotis_device/devices/dynamixel/L54-50-S290-R.device b/robotis_device/devices/dynamixel/L54-50-S290-R.device new file mode 100644 index 0000000..1bf2115 --- /dev/null +++ b/robotis_device/devices/dynamixel/L54-50-S290-R.device @@ -0,0 +1,71 @@ +[device info] +model_name = L54-50-S290-R +device_type = Dynamixel + +[type info] +0_radian_position_value = 0 +min_radian_position_value = -103860 +max_radian_position_value = 103860 +min_radian = -3.14159265 +max_radian = 3.14159265 +torque_enable_address = 562 +position_d_gain_address = +position_i_gain_address = +position_p_gain_address = 594 +goal_position_address = 596 +goal_velocity_address = 600 +goal_torque_address = 604 +present_position_address = 611 +present_velocity_address = 615 +present_load_address = 621 +is_moving_address = 610 + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 13 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 30 | torque_limit | 2 | RW | EEPROM | 0 | 32767 | N + 32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 562 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 586 | velocity_I_gain | 2 | RW | RAM | 0 | 2047 | N + 588 | velocity_P_gain | 2 | RW | RAM | 0 | 2047 | N + 594 | position_P_gain | 2 | RW | RAM | 0 | 2047 | N + 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y + 606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N + 610 | is_moving | 1 | R | RAM | 0 | 1 | N + 611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 621 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 623 | present_voltage | 2 | R | RAM | 0 | 500 | N + 625 | present_temperature | 1 | R | RAM | 0 | 200 | N + 626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 891 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N diff --git a/robotis_device/devices/dynamixel/L54-50-S500-R.device b/robotis_device/devices/dynamixel/L54-50-S500-R.device new file mode 100644 index 0000000..8fcd40c --- /dev/null +++ b/robotis_device/devices/dynamixel/L54-50-S500-R.device @@ -0,0 +1,71 @@ +[device info] +model_name = L54-50-S500-R +device_type = Dynamixel + +[type info] +0_radian_position_value = 0 +min_radian_position_value = -180684 +max_radian_position_value = 180684 +min_radian = -3.14159265 +max_radian = 3.14159265 +torque_enable_address = 562 +position_d_gain_address = +position_i_gain_address = +position_p_gain_address = 594 +goal_position_address = 596 +goal_velocity_address = 600 +goal_torque_address = 604 +present_position_address = 611 +present_velocity_address = 615 +present_load_address = 621 +is_moving_address = 610 + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 13 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 30 | torque_limit | 2 | RW | EEPROM | 0 | 32767 | N + 32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 562 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 586 | velocity_I_gain | 2 | RW | RAM | 0 | 2047 | N + 588 | velocity_P_gain | 2 | RW | RAM | 0 | 2047 | N + 594 | position_P_gain | 2 | RW | RAM | 0 | 2047 | N + 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y + 606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N + 610 | is_moving | 1 | R | RAM | 0 | 1 | N + 611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 621 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 623 | present_voltage | 2 | R | RAM | 0 | 500 | N + 625 | present_temperature | 1 | R | RAM | 0 | 200 | N + 626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 891 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N diff --git a/robotis_device/devices/dynamixel/M42-10-S260-R.device b/robotis_device/devices/dynamixel/M42-10-S260-R.device new file mode 100644 index 0000000..d9836d8 --- /dev/null +++ b/robotis_device/devices/dynamixel/M42-10-S260-R.device @@ -0,0 +1,71 @@ +[device info] +model_name = M42-10-S260-R +device_type = Dynamixel + +[type info] +0_radian_position_value = 0 +min_radian_position_value = -131584 +max_radian_position_value = 131584 +min_radian = -3.14159265 +max_radian = 3.14159265 +torque_enable_address = 562 +position_d_gain_address = +position_i_gain_address = +position_p_gain_address = 594 +goal_position_address = 596 +goal_velocity_address = 600 +goal_torque_address = 604 +present_position_address = 611 +present_velocity_address = 615 +present_load_address = 621 +is_moving_address = 610 + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 13 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 30 | torque_limit | 2 | RW | EEPROM | 0 | 32767 | N + 32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 562 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 586 | velocity_I_gain | 2 | RW | RAM | 0 | 2047 | N + 588 | velocity_P_gain | 2 | RW | RAM | 0 | 2047 | N + 594 | position_P_gain | 2 | RW | RAM | 0 | 2047 | N + 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y + 606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N + 610 | is_moving | 1 | R | RAM | 0 | 1 | N + 611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 621 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 623 | present_voltage | 2 | R | RAM | 0 | 500 | N + 625 | present_temperature | 1 | R | RAM | 0 | 200 | N + 626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 891 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N diff --git a/robotis_device/devices/dynamixel/M54-40-S250-R.device b/robotis_device/devices/dynamixel/M54-40-S250-R.device new file mode 100644 index 0000000..ba3d1ac --- /dev/null +++ b/robotis_device/devices/dynamixel/M54-40-S250-R.device @@ -0,0 +1,71 @@ +[device info] +model_name = M54-40-S250-R +device_type = Dynamixel + +[type info] +0_radian_position_value = 0 +min_radian_position_value = -125700 +max_radian_position_value = 125700 +min_radian = -3.14159265 +max_radian = 3.14159265 +torque_enable_address = 562 +position_d_gain_address = +position_i_gain_address = +position_p_gain_address = 594 +goal_position_address = 596 +goal_velocity_address = 600 +goal_torque_address = 604 +present_position_address = 611 +present_velocity_address = 615 +present_load_address = 621 +is_moving_address = 610 + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 13 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 30 | torque_limit | 2 | RW | EEPROM | 0 | 32767 | N + 32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 562 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 586 | velocity_I_gain | 2 | RW | RAM | 0 | 2047 | N + 588 | velocity_P_gain | 2 | RW | RAM | 0 | 2047 | N + 594 | position_P_gain | 2 | RW | RAM | 0 | 2047 | N + 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y + 606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N + 610 | is_moving | 1 | R | RAM | 0 | 1 | N + 611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 621 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 623 | present_voltage | 2 | R | RAM | 0 | 500 | N + 625 | present_temperature | 1 | R | RAM | 0 | 200 | N + 626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 891 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N diff --git a/robotis_device/devices/dynamixel/M54-60-S250-R.device b/robotis_device/devices/dynamixel/M54-60-S250-R.device new file mode 100644 index 0000000..ae9cbed --- /dev/null +++ b/robotis_device/devices/dynamixel/M54-60-S250-R.device @@ -0,0 +1,71 @@ +[device info] +model_name = M54-60-S250-R +device_type = Dynamixel + +[type info] +0_radian_position_value = 0 +min_radian_position_value = -125700 +max_radian_position_value = 125700 +min_radian = -3.14159265 +max_radian = 3.14159265 +torque_enable_address = 562 +position_d_gain_address = +position_i_gain_address = +position_p_gain_address = 594 +goal_position_address = 596 +goal_velocity_address = 600 +goal_torque_address = 604 +present_position_address = 611 +present_velocity_address = 615 +present_load_address = 621 +is_moving_address = 610 + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 13 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 30 | torque_limit | 2 | RW | EEPROM | 0 | 32767 | N + 32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 562 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 586 | velocity_I_gain | 2 | RW | RAM | 0 | 2047 | N + 588 | velocity_P_gain | 2 | RW | RAM | 0 | 2047 | N + 594 | position_P_gain | 2 | RW | RAM | 0 | 2047 | N + 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y + 606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N + 610 | is_moving | 1 | R | RAM | 0 | 1 | N + 611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 621 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 623 | present_voltage | 2 | R | RAM | 0 | 500 | N + 625 | present_temperature | 1 | R | RAM | 0 | 200 | N + 626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 891 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N diff --git a/robotis_device/devices/dynamixel/MX-28.device b/robotis_device/devices/dynamixel/MX-28.device new file mode 100644 index 0000000..ef59e83 --- /dev/null +++ b/robotis_device/devices/dynamixel/MX-28.device @@ -0,0 +1,59 @@ +[device info] +model_name = MX-28 +device_type = Dynamixel + +[type info] +0_radian_position_value = 2048 +min_radian_position_value = 0 +max_radian_position_value = 4095 +min_radian = -3.14159265 +max_radian = 3.14159265 +torque_enable_address = 24 +position_d_gain_address = 26 +position_i_gain_address = 27 +position_p_gain_address = 28 +goal_position_address = 30 +goal_velocity_address = 32 +goal_torque_address = 34 +present_position_address = 36 +present_velocity_address = 38 +present_load_address = 40 +is_moving_address = 46 + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 2 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 3 | ID | 1 | RW | EEPROM | 0 | 252 | N + 4 | baudrate | 1 | RW | EEPROM | 0 | 252 | N + 5 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 6 | CW_angle_limit | 2 | RW | EEPROM | 0 | 4095 | N + 8 | CCW_angle_limit | 2 | RW | EEPROM | 0 | 4095 | N + 10 | drive_mode | 1 | RW | EEPROM | 0 | 3 | N + 11 | max_temperature_limit | 1 | RW | EEPROM | 0 | 99 | N + 12 | min_voltage_limit | 1 | RW | EEPROM | 0 | 250 | N + 13 | max_voltage_limit | 1 | RW | EEPROM | 0 | 250 | N + 14 | max_torque | 2 | RW | EEPROM | 0 | 1023 | N + 16 | status_return_level | 1 | RW | EEPROM | 0 | 2 | N + 17 | alarm_LED | 1 | RW | EEPROM | 0 | 127 | N + 18 | alarm_shutdown | 1 | RW | EEPROM | 0 | 127 | N + 20 | multi_turn_offset | 2 | RW | EEPROM | -26624 | 26624 | Y + 22 | resolution_dividor | 1 | RW | EEPROM | 1 | 255 | N + 24 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 25 | LED | 1 | RW | RAM | 0 | 1 | N + 26 | position_D_gain | 1 | RW | RAM | 0 | 254 | N + 27 | position_I_gain | 1 | RW | RAM | 0 | 254 | N + 28 | position_P_gain | 1 | RW | RAM | 0 | 254 | N + 30 | goal_position | 2 | RW | RAM | -28672 | 28672 | Y + 32 | goal_velocity | 2 | RW | RAM | 0 | 1023 | N + 34 | goal_torque | 2 | RW | RAM | 0 | 1023 | N + 36 | present_position | 2 | R | RAM | 0 | 4095 | N + 38 | present_velocity | 2 | R | RAM | 0 | 2048 | N + 40 | present_load | 2 | R | RAM | 0 | 2048 | N + 42 | present_voltage | 1 | R | RAM | 50 | 250 | N + 43 | present_temperature | 1 | R | RAM | 0 | 99 | N + 44 | registered_instruction | 1 | R | RAM | 0 | 1 | N + 46 | is_moving | 1 | R | RAM | 0 | 1 | N + 47 | EEPROM_lock | 1 | RW | RAM | 0 | 1 | N + 48 | punch | 2 | RW | RAM | 0 | 1023 | N + 73 | goal_acceleration | 1 | RW | RAM | 0 | 254 | N diff --git a/robotis_device/include/robotis_device/ControlTableItem.h b/robotis_device/include/robotis_device/ControlTableItem.h new file mode 100644 index 0000000..5a48bab --- /dev/null +++ b/robotis_device/include/robotis_device/ControlTableItem.h @@ -0,0 +1,52 @@ +/* + * ControlTableItem.h + * + * Created on: 2015. 12. 16. + * Author: zerom + */ + +#ifndef ROBOTIS_FRAMEWORK_ROBOTIS_DEVICE_INCLUDE_DEVICE_CONTROLTABLEITEM_H_ +#define ROBOTIS_FRAMEWORK_ROBOTIS_DEVICE_INCLUDE_DEVICE_CONTROLTABLEITEM_H_ + + +#include + +namespace ROBOTIS +{ + +enum ACCESS_TYPE { + READ, + READ_WRITE +}; + +enum MEMORY_TYPE { + EEPROM, + RAM +}; + +class ControlTableItem +{ +public: + UINT16_T address; + ACCESS_TYPE access_type; + MEMORY_TYPE memory_type; + UINT8_T data_length; + INT32_T data_min_value; + INT32_T data_max_value; + bool is_signed; + + ControlTableItem() + : address(0), + access_type(READ), + memory_type(RAM), + data_length(0), + data_min_value(0), + data_max_value(0), + is_signed(false) + { } +}; + +} + + +#endif /* ROBOTIS_FRAMEWORK_ROBOTIS_DEVICE_INCLUDE_DEVICE_CONTROLTABLEITEM_H_ */ diff --git a/robotis_device/include/robotis_device/Dynamixel.h b/robotis_device/include/robotis_device/Dynamixel.h new file mode 100644 index 0000000..4649044 --- /dev/null +++ b/robotis_device/include/robotis_device/Dynamixel.h @@ -0,0 +1,60 @@ +/* + * Dynamixel.h + * + * Created on: 2015. 12. 8. + * Author: zerom + */ + +#ifndef ROBOTIS_FRAMEWORK_ROBOTIS_CONTROLLER_INCLUDE_DEVICE_DYNAMIXEL_H_ +#define ROBOTIS_FRAMEWORK_ROBOTIS_CONTROLLER_INCLUDE_DEVICE_DYNAMIXEL_H_ + + +#include +#include +#include "DynamixelState.h" +#include "ControlTableItem.h" + +namespace ROBOTIS +{ + +class Dynamixel +{ +public: + UINT8_T id; + std::string model_name; + float protocol_version; + + std::map ctrl_table; // string: item name + + std::string port_name; + std::string ctrl_module_name; + DynamixelState *dxl_state; + + INT32_T zero_radian_position_value; + INT32_T min_radian_position_value; + INT32_T max_radian_position_value; + double min_radian; + double max_radian; + + UINT16_T torque_enable_address; + UINT16_T position_d_gain_address; + UINT16_T position_i_gain_address; + UINT16_T position_p_gain_address; + UINT16_T goal_position_address; + UINT16_T goal_velocity_address; + UINT16_T goal_torque_address; + UINT16_T present_position_address; + UINT16_T present_velocity_address; + UINT16_T present_load_address; + UINT16_T is_moving_address; + + Dynamixel(int id, std::string model_name, float protocol_version); + + double ConvertValue2Radian(int32_t value) { return value * max_radian / max_radian_position_value; } + INT32_T ConvertRadian2Value(double radian) { return radian * max_radian_position_value / max_radian; } +}; + +} + + +#endif /* ROBOTIS_FRAMEWORK_ROBOTIS_CONTROLLER_INCLUDE_DEVICE_DYNAMIXEL_H_ */ diff --git a/robotis_device/include/robotis_device/DynamixelState.h b/robotis_device/include/robotis_device/DynamixelState.h new file mode 100644 index 0000000..7917aed --- /dev/null +++ b/robotis_device/include/robotis_device/DynamixelState.h @@ -0,0 +1,76 @@ +/* + * DynamixelState.h + * + * Created on: 2015. 12. 8. + * Author: zerom + */ + +#ifndef ROBOTIS_FRAMEWORK_ROBOTIS_CONTROLLER_INCLUDE_DEVICE_DYNAMIXELSTATE_H_ +#define ROBOTIS_FRAMEWORK_ROBOTIS_CONTROLLER_INCLUDE_DEVICE_DYNAMIXELSTATE_H_ + +#include +#include + +namespace ROBOTIS +{ + +class TimeStamp { +public: + long sec; + long nsec; + TimeStamp(long sec, long nsec) : sec(sec), nsec(nsec) { } +}; + +class DynamixelState +{ +public: + TimeStamp update_time_stamp; + + bool torque_enable; + UINT16_T position_d_gain; + UINT16_T position_i_gain; + UINT16_T position_p_gain; + UINT16_T velocity_d_gain; + UINT16_T velocity_i_gain; + UINT16_T velocity_p_gain; + double goal_position; + double goal_velocity; + double goal_torque; + double present_position; + double present_velocity; + double present_load; + bool is_moving; + + UINT16_T ext_port_data[4]; + + double position_offset; + + DynamixelState() + : update_time_stamp(0, 0), + torque_enable(false), + position_d_gain(0), + position_i_gain(0), + position_p_gain(0), + velocity_d_gain(0), + velocity_i_gain(0), + velocity_p_gain(0), + goal_position(0.0), + goal_velocity(0.0), + goal_torque(0.0), + present_position(0.0), + present_velocity(0.0), + present_load(0.0), + is_moving(false), + position_offset(0.0) + { + ext_port_data[0] = 2048; + ext_port_data[1] = 2048; + ext_port_data[2] = 2048; + ext_port_data[3] = 2048; + } +}; + +} + + +#endif /* ROBOTIS_FRAMEWORK_ROBOTIS_CONTROLLER_INCLUDE_DEVICE_DYNAMIXELSTATE_H_ */ diff --git a/robotis_device/include/robotis_device/Robot.h b/robotis_device/include/robotis_device/Robot.h new file mode 100644 index 0000000..121d385 --- /dev/null +++ b/robotis_device/include/robotis_device/Robot.h @@ -0,0 +1,35 @@ +/* + * Robot.h + * + * Created on: 2015. 12. 11. + * Author: zerom + */ + +#ifndef ROBOTIS_FRAMEWORK_ROBOTIS_DEVICE_INCLUDE_DEVICE_ROBOT_H_ +#define ROBOTIS_FRAMEWORK_ROBOTIS_DEVICE_INCLUDE_DEVICE_ROBOT_H_ + + +#include +#include "Sensor.h" +#include "Dynamixel.h" +#include "dynamixel_sdk/PortHandler.h" + +namespace ROBOTIS +{ + +class Robot +{ +public: + std::map ports; // string: port name + std::map dxls; // string: joint name + std::map sensors; // string: sensor name + + Robot(std::string robot_file_path, std::string dev_desc_dir_path); + + Dynamixel *getDynamixel(std::string path, int id, std::string port, float protocol_version); +}; + +} + + +#endif /* ROBOTIS_FRAMEWORK_ROBOTIS_DEVICE_INCLUDE_DEVICE_ROBOT_H_ */ diff --git a/robotis_device/include/robotis_device/Sensor.h b/robotis_device/include/robotis_device/Sensor.h new file mode 100644 index 0000000..ca3e28a --- /dev/null +++ b/robotis_device/include/robotis_device/Sensor.h @@ -0,0 +1,34 @@ +/* + * Sensor.h + * + * Created on: 2015. 12. 16. + * Author: zerom + */ + +#ifndef ROBOTIS_FRAMEWORK_ROBOTIS_DEVICE_INCLUDE_DEVICE_SENSOR_H_ +#define ROBOTIS_FRAMEWORK_ROBOTIS_DEVICE_INCLUDE_DEVICE_SENSOR_H_ + +#include +#include +#include +#include "ControlTableItem.h" + +namespace ROBOTIS +{ + +class Sensor +{ +public: + UINT8_T id; + std::string model_name; + float protocol_version; + + std::map ctrl_table; + + Sensor(int id, std::string model_name, float protocol_version); +}; + +} + + +#endif /* ROBOTIS_FRAMEWORK_ROBOTIS_DEVICE_INCLUDE_DEVICE_SENSOR_H_ */ diff --git a/robotis_device/package.xml b/robotis_device/package.xml new file mode 100644 index 0000000..06de7e5 --- /dev/null +++ b/robotis_device/package.xml @@ -0,0 +1,34 @@ + + + robotis_device + 0.1.0 + The robotis_device package + + robotis + + + GPLv2 + + + + + + robotis + + + catkin + + roscpp + rospy + dynamixel_sdk + robotis_framework_common + + roscpp + rospy + dynamixel_sdk + + + + + + \ No newline at end of file diff --git a/robotis_device/src/robotis_device/Dynamixel.cpp b/robotis_device/src/robotis_device/Dynamixel.cpp new file mode 100644 index 0000000..baafc2e --- /dev/null +++ b/robotis_device/src/robotis_device/Dynamixel.cpp @@ -0,0 +1,37 @@ +/* + * Dynamixel.cpp + * + * Created on: 2015. 12. 8. + * Author: zerom + */ + +#include "../include/robotis_device/Dynamixel.h" + +using namespace ROBOTIS; + +Dynamixel::Dynamixel(int id, std::string model_name, float protocol_version) + : id(id), + model_name(model_name), + port_name(""), + ctrl_module_name("none"), + protocol_version(protocol_version), + zero_radian_position_value(0), + min_radian_position_value(0), + max_radian_position_value(0), + min_radian(-3.14), + max_radian(3.14), + torque_enable_address(0), + position_d_gain_address(0), + position_i_gain_address(0), + position_p_gain_address(0), + goal_position_address(0), + goal_velocity_address(0), + goal_torque_address(0), + present_position_address(0), + present_velocity_address(0), + present_load_address(0), + is_moving_address(0) +{ + ctrl_table.clear(); + dxl_state = new DynamixelState(); +} diff --git a/robotis_device/src/robotis_device/Robot.cpp b/robotis_device/src/robotis_device/Robot.cpp new file mode 100644 index 0000000..43d2cb5 --- /dev/null +++ b/robotis_device/src/robotis_device/Robot.cpp @@ -0,0 +1,227 @@ +/* + * Robot.cpp + * + * Created on: 2015. 12. 11. + * Author: zerom + */ + +#include +#include +#include +#include "../include/robotis_device/Robot.h" + +using namespace ROBOTIS; + +static inline std::string <rim(std::string &s) { + s.erase(s.begin(), std::find_if(s.begin(), s.end(), std::not1(std::ptr_fun(std::isspace)))); + return s; +} +static inline std::string &rtrim(std::string &s) { + s.erase(std::find_if(s.rbegin(), s.rend(), std::not1(std::ptr_fun(std::isspace))).base(), s.end()); + return s; +} +static inline std::string &trim(std::string &s) { + return ltrim(rtrim(s)); +} + +static inline std::vector split(const std::string &text, char sep) { + std::vector tokens; + std::size_t start = 0, end = 0; + while((end = text.find(sep, start)) != (std::string::npos)) { + tokens.push_back(text.substr(start, end - start)); + trim(tokens.back()); + start = end + 1; + } + tokens.push_back(text.substr(start)); + trim(tokens.back()); + return tokens; +} + +Robot::Robot(std::string robot_file_path, std::string dev_desc_dir_path) +{ + if(dev_desc_dir_path.compare(dev_desc_dir_path.size()-1, 1, "/") != 0) + dev_desc_dir_path += "/"; + + std::ifstream file(robot_file_path.c_str()); + if(file.is_open()) + { + std::string session = ""; + std::string input_str; + while(!file.eof()) + { + std::getline(file, input_str); + + // remove comment ( # ) + std::size_t pos = input_str.find("#"); + if(pos != std::string::npos) + input_str = input_str.substr(0, pos); + + // trim + input_str = trim(input_str); + + // find session + if(!input_str.compare(0, 1, "[") && !input_str.compare(input_str.size()-1, 1, "]")) + { + input_str = input_str.substr(1, input_str.size()-2); + std::transform(input_str.begin(), input_str.end(), input_str.begin(), ::tolower); + session = trim(input_str); + continue; + } + + if(session == "port info") + { + std::vector tokens = split(input_str, '|'); + if(tokens.size() != 2) + continue; + + std::cout << tokens[0] << " added. (baudrate: " << tokens[1] << ")" << std::endl; + + ports[tokens[0]] = (PortHandler*)PortHandler::GetPortHandler(tokens[0].c_str()); + bool _port_result = ports[tokens[0]]->SetBaudRate(std::atoi(tokens[1].c_str())); + if(_port_result == false) + exit(-1); + } + else if(session == "device info") + { + std::vector tokens = split(input_str, '|'); + if(tokens.size() != 6) + continue; + + if(tokens[0] == "dynamixel") + { + std::string _file_path = dev_desc_dir_path + tokens[0] + "/" + tokens[3] + ".device"; + int _id = std::atoi(tokens[2].c_str()); + std::string _port = tokens[1]; + float _protocol = std::atof(tokens[4].c_str()); + std::string _dev_name = tokens[5]; + + dxls[_dev_name] = getDynamixel(_file_path, _id, _port, _protocol); + } + } + } + file.close(); + } + else + { + std::cout << "Unable to open file : " + robot_file_path << std::endl; + } +} + +Dynamixel *Robot::getDynamixel(std::string path, int id, std::string port, float protocol_version) +{ + Dynamixel *dxl; + // load file model_name.device + std::ifstream file(path.c_str()); + if(file.is_open()) + { + std::string session = ""; + std::string input_str; + while(!file.eof()) + { + std::getline(file, input_str); + + // remove comment ( # ) + std::size_t pos = input_str.find("#"); + if(pos != std::string::npos) + input_str = input_str.substr(0, pos); + + // trim + input_str = trim(input_str); + if(input_str == "") + continue; + + // find session + if(!input_str.compare(0, 1, "[") && !input_str.compare(input_str.size()-1, 1, "]")) + { + input_str = input_str.substr(1, input_str.size()-2); + std::transform(input_str.begin(), input_str.end(), input_str.begin(), ::tolower); + session = trim(input_str); + continue; + } + + if(session == "device info") + { + std::vector tokens = split(input_str, '='); + if(tokens.size() != 2) + continue; + + if(tokens[0] == "model_name") + dxl = new Dynamixel(id, tokens[1], protocol_version); + } + else if(session == "type info") + { + std::vector tokens = split(input_str, '='); + if(tokens.size() != 2) + continue; + + if(tokens[0] == "0_radian_position_value") + dxl->zero_radian_position_value = std::atoi(tokens[1].c_str()); + else if(tokens[0] == "min_radian_position_value") + dxl->min_radian_position_value = std::atoi(tokens[1].c_str()); + else if(tokens[0] == "max_radian_position_value") + dxl->max_radian_position_value = std::atoi(tokens[1].c_str()); + else if(tokens[0] == "min_radian") + dxl->min_radian = std::atof(tokens[1].c_str()); + else if(tokens[0] == "max_radian") + dxl->max_radian = std::atof(tokens[1].c_str()); + else if(tokens[0] == "torque_enable_address") + dxl->torque_enable_address = std::atoi(tokens[1].c_str()); + else if(tokens[0] == "position_d_gain_address") + dxl->position_d_gain_address = std::atoi(tokens[1].c_str()); + else if(tokens[0] == "position_i_gain_address") + dxl->position_i_gain_address = std::atoi(tokens[1].c_str()); + else if(tokens[0] == "position_p_gain_address") + dxl->position_p_gain_address = std::atoi(tokens[1].c_str()); + else if(tokens[0] == "goal_position_address") + dxl->goal_position_address = std::atoi(tokens[1].c_str()); + else if(tokens[0] == "goal_velocity_address") + dxl->goal_velocity_address = std::atoi(tokens[1].c_str()); + else if(tokens[0] == "goal_torque_address") + dxl->goal_torque_address = std::atoi(tokens[1].c_str()); + else if(tokens[0] == "present_position_address") + dxl->present_position_address = std::atoi(tokens[1].c_str()); + else if(tokens[0] == "present_velocity_address") + dxl->present_velocity_address = std::atoi(tokens[1].c_str()); + else if(tokens[0] == "present_load_address") + dxl->present_load_address = std::atoi(tokens[1].c_str()); + else if(tokens[0] == "is_moving_address") + dxl->is_moving_address = std::atoi(tokens[1].c_str()); + } + else if(session == "control table") + { + std::vector tokens = split(input_str, '|'); + if(tokens.size() != 8) + continue; + + ControlTableItem *item = new ControlTableItem(); + item->address = std::atoi(tokens[0].c_str()); + item->data_length = std::atoi(tokens[2].c_str()); + if(tokens[3] == "R") + item->access_type = READ; + else if(tokens[3] == "RW") + item->access_type = READ_WRITE; + if(tokens[4] == "EEPROM") + item->memory_type = EEPROM; + else if(tokens[4] == "RAM") + item->memory_type = RAM; + item->data_min_value = std::atol(tokens[5].c_str()); + item->data_max_value = std::atol(tokens[6].c_str()); + if(tokens[7] == "Y") + item->is_signed = true; + else if(tokens[7] == "N") + item->is_signed = false; + dxl->ctrl_table[tokens[1]] = item; + } + } + dxl->port_name = port; + + fprintf(stderr, "(%s) [ID:%3d] %14s added. \n", port.c_str(), dxl->id, dxl->model_name.c_str()); + //std::cout << "[ID:" << (int)(dxl->id) << "] " << dxl->model_name << " added. (" << port << ")" << std::endl; + file.close(); + } + else + std::cout << "Unable to open file : " + path << std::endl; + + return dxl; +} + diff --git a/robotis_framework_common/CMakeLists.txt b/robotis_framework_common/CMakeLists.txt new file mode 100644 index 0000000..2a38558 --- /dev/null +++ b/robotis_framework_common/CMakeLists.txt @@ -0,0 +1,186 @@ +cmake_minimum_required(VERSION 2.8.3) +project(robotis_framework_common) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + roscpp +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a run_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# std_msgs # Or other packages containing msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a run_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if you package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( + INCLUDE_DIRS include +# LIBRARIES robotis_framework_common +# CATKIN_DEPENDS roscpp +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +# include_directories(include) +include_directories( + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +# add_library(robotis_framework_common +# src/${PROJECT_NAME}/robotis_framework_common.cpp +# ) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(robotis_framework_common ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +# add_executable(robotis_framework_common_node src/robotis_framework_common_node.cpp) + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(robotis_framework_common_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(robotis_framework_common_node +# ${catkin_LIBRARIES} +# ) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# install(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables and/or libraries for installation +# install(TARGETS robotis_framework_common robotis_framework_common_node +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_robotis_framework_common.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/robotis_framework_common/include/robotis_framework_common/MotionModule.h b/robotis_framework_common/include/robotis_framework_common/MotionModule.h new file mode 100644 index 0000000..8237032 --- /dev/null +++ b/robotis_framework_common/include/robotis_framework_common/MotionModule.h @@ -0,0 +1,53 @@ +/* + * MotionModule.h + * + * Created on: 2016. 1. 15. + * Author: zerom + */ + +#ifndef ROBOTIS_FRAMEWORK_ROBOTIS_CONTROLLER_MSGS_INCLUDE_MOTIONMODULE_H_ +#define ROBOTIS_FRAMEWORK_ROBOTIS_CONTROLLER_MSGS_INCLUDE_MOTIONMODULE_H_ + + +#include +#include + +#include "robotis_device/Dynamixel.h" +#include "robotis_device/DynamixelState.h" + +namespace ROBOTIS +{ + +enum CONTROL_MODE +{ + POSITION_CONTROL, + VELOCITY_CONTROL, + TORQUE_CONTROL +}; + +class MotionModule +{ +public: + bool enable; + std::string module_name; + CONTROL_MODE control_mode; + + std::map result; + + virtual ~MotionModule() { } + + virtual void Initialize(const int control_cycle_msec) = 0; + virtual void Process(std::map dxls) = 0; + + inline double powDI(double a, int b); +}; + +inline double MotionModule::powDI(double a, int b) +{ + return (b == 0 ? 1 : (b > 0 ? a * powDI(a, b - 1) : 1 / powDI(a, -b))); +} + +} + + +#endif /* ROBOTIS_FRAMEWORK_ROBOTIS_CONTROLLER_MSGS_INCLUDE_MOTIONMODULE_H_ */ diff --git a/robotis_framework_common/include/robotis_framework_common/RobotisDef.h b/robotis_framework_common/include/robotis_framework_common/RobotisDef.h new file mode 100644 index 0000000..a860e9d --- /dev/null +++ b/robotis_framework_common/include/robotis_framework_common/RobotisDef.h @@ -0,0 +1,21 @@ +/* + * RobotisDef.h + * + * Created on: 2016. 1. 27. + * Author: zerom + */ + +#ifndef ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_ROBOTISDEF_H_ +#define ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_ROBOTISDEF_H_ + + +typedef char INT8_T; +typedef short int INT16_T; +typedef int INT32_T; + +typedef unsigned char UINT8_T; +typedef unsigned short int UINT16_T; +typedef unsigned int UINT32_T; + + +#endif /* ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_ROBOTISDEF_H_ */ diff --git a/robotis_framework_common/package.xml b/robotis_framework_common/package.xml new file mode 100644 index 0000000..7863f09 --- /dev/null +++ b/robotis_framework_common/package.xml @@ -0,0 +1,52 @@ + + + robotis_framework_common + 0.0.0 + The robotis_framework_common package + + + + + robotis + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + roscpp + roscpp + + + + + + + + \ No newline at end of file From fe533d50db97abbd4ee2d6912c6d6f7a6fa187b0 Mon Sep 17 00:00:00 2001 From: ROBOTIS Date: Mon, 7 Mar 2016 14:40:32 +0900 Subject: [PATCH 002/238] bug fix --- robotis_controller/src/robotis_controller/RobotisController.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/robotis_controller/src/robotis_controller/RobotisController.cpp b/robotis_controller/src/robotis_controller/RobotisController.cpp index aa5bf7c..edd2269 100644 --- a/robotis_controller/src/robotis_controller/RobotisController.cpp +++ b/robotis_controller/src/robotis_controller/RobotisController.cpp @@ -303,7 +303,7 @@ void *RobotisController::ThreadProc(void *param) clock_gettime(CLOCK_MONOTONIC, &curr_time); long delta_nsec = (next_time.tv_sec - curr_time.tv_sec) * 1000000000 + (next_time.tv_nsec - curr_time.tv_nsec); - if(delta_nsec < -0.3 ) + if(delta_nsec < -100000 ) { if(controller->DEBUG_PRINT == true) ROS_WARN("[RobotisController::ThreadProc] NEXT TIME < CURR TIME.. (%f)[%ld.%09ld / %ld.%09ld]", delta_nsec/1000000.0, (long)next_time.tv_sec, (long)next_time.tv_nsec, (long)curr_time.tv_sec, (long)curr_time.tv_nsec); From 825616ad5db158588bfac7a69bfaff63f17101e4 Mon Sep 17 00:00:00 2001 From: ROBOTIS Date: Mon, 7 Mar 2016 20:26:04 +0900 Subject: [PATCH 003/238] - remove unnecessary code - change the code for multi-platform --- .../src/dynamixel_sdk/PortHandler.cpp | 4 +- .../dynamixel_sdk/Protocol2PacketHandler.cpp | 50 ++----------------- .../dynamixel_sdk_linux/PortHandlerLinux.cpp | 8 --- 3 files changed, 6 insertions(+), 56 deletions(-) diff --git a/dynamixel_sdk/src/dynamixel_sdk/PortHandler.cpp b/dynamixel_sdk/src/dynamixel_sdk/PortHandler.cpp index 2f1b5b0..3bce1bf 100644 --- a/dynamixel_sdk/src/dynamixel_sdk/PortHandler.cpp +++ b/dynamixel_sdk/src/dynamixel_sdk/PortHandler.cpp @@ -11,7 +11,7 @@ #include "dynamixel_sdk_linux/PortHandlerLinux.h" #endif -#ifdef _WIN32 || _WIN64 +#if defined(_WIN32) || defined(_WIN64) #include "dynamixel_sdk_windows/PortHandlerWindows.h" #endif @@ -23,7 +23,7 @@ PortHandler *PortHandler::GetPortHandler(const char *port_name) return (PortHandler *)(new PortHandlerLinux(port_name)); #endif -#ifdef _WIN32 || _WIN64 +#if defined(_WIN32) || defined(_WIN64) return (PortHandler *)(new PortHandlerWindows(port_name)); #endif } diff --git a/dynamixel_sdk/src/dynamixel_sdk/Protocol2PacketHandler.cpp b/dynamixel_sdk/src/dynamixel_sdk/Protocol2PacketHandler.cpp index 3cd3200..ecdc260 100644 --- a/dynamixel_sdk/src/dynamixel_sdk/Protocol2PacketHandler.cpp +++ b/dynamixel_sdk/src/dynamixel_sdk/Protocol2PacketHandler.cpp @@ -213,11 +213,6 @@ int Protocol2PacketHandler::RxPacket(PortHandler *port, UINT8_T *rxpacket) UINT16_T _idx = 0; // find packet header -// for(_idx = 0; _idx < (_rx_length - 2); _idx++) -// { -// if(rxpacket[_idx] == 0xFF && rxpacket[_idx+1] == 0xFF && rxpacket[_idx+2] == 0xFD) -// break; -// } for(_idx = 0; _idx < (_rx_length - 3); _idx++) { if((rxpacket[_idx] == 0xFF) && (rxpacket[_idx+1] == 0xFF) && (rxpacket[_idx+2] == 0xFD) && (rxpacket[_idx+3] != 0xFD)) @@ -228,9 +223,6 @@ int Protocol2PacketHandler::RxPacket(PortHandler *port, UINT8_T *rxpacket) { // re-calculate the exact length of the rx packet _wait_length = DXL_MAKEWORD(rxpacket[PKT_LENGTH_L], rxpacket[PKT_LENGTH_H]) + PKT_LENGTH_H + 1; - if(_wait_length > 100) - fprintf(stderr, "id : %d , _wait_length : %d \n", rxpacket[PKT_ID], _wait_length); - if(_wait_length > RXPACKET_MAX_LEN - 10) { _result = COMM_RX_CORRUPT; @@ -244,18 +236,10 @@ int Protocol2PacketHandler::RxPacket(PortHandler *port, UINT8_T *rxpacket) { if(_rx_length == 0) _result = COMM_RX_TIMEOUT; - else { + else _result = COMM_RX_CORRUPT; -// fprintf(stderr, "Fail Rx Packet(short_length1) : "); -// for(int _i = 0; _i < _rx_length + 11; _i++) -// { -// fprintf(stderr, " %x", rxpacket[_i]); -// } -// fprintf(stderr, "\n"); - } break; } - else continue; } @@ -264,15 +248,8 @@ int Protocol2PacketHandler::RxPacket(PortHandler *port, UINT8_T *rxpacket) UINT16_T crc = DXL_MAKEWORD(rxpacket[_wait_length-2], rxpacket[_wait_length-1]); if(UpdateCRC(0, rxpacket, _wait_length - 2) == crc) _result = COMM_SUCCESS; - else { + else _result = COMM_RX_CORRUPT; -// fprintf(stderr, "Rx Packet(crc_error) : "); -// for(int _i = 0; _i < _rx_length + 11; _i++) -// { -// fprintf(stderr, " %x", rxpacket[_i]); -// } -// fprintf(stderr, "\n"); - } break; } else @@ -291,15 +268,8 @@ int Protocol2PacketHandler::RxPacket(PortHandler *port, UINT8_T *rxpacket) { if(_rx_length == 0) _result = COMM_RX_TIMEOUT; - else { + else _result = COMM_RX_CORRUPT; -// fprintf(stderr, "Fail Rx Packet(short_length2): "); -// for(int _i = 0; _i < _rx_length + 11; _i++) -// { -// fprintf(stderr, " %x", rxpacket[_i]); -// } -// fprintf(stderr, "\n"); - } break; } } @@ -474,8 +444,6 @@ int Protocol2PacketHandler::BroadcastPing(PortHandler *port, std::vector Date: Tue, 8 Mar 2016 10:54:47 +0900 Subject: [PATCH 004/238] - sync with Dynamixel SDK --- dynamixel_sdk/include/DynamixelSDK.h | 29 +++++++++++++++++++ .../include/dynamixel_sdk/GroupBulkRead.h | 6 ++-- .../include/dynamixel_sdk/GroupBulkWrite.h | 6 ++-- .../include/dynamixel_sdk/GroupSyncRead.h | 6 ++-- .../include/dynamixel_sdk/GroupSyncWrite.h | 6 ++-- .../include/dynamixel_sdk/PacketHandler.h | 6 ++-- .../include/dynamixel_sdk/PortHandler.h | 6 ++-- .../dynamixel_sdk/Protocol1PacketHandler.h | 6 ++-- .../dynamixel_sdk/Protocol2PacketHandler.h | 6 ++-- .../include/dynamixel_sdk/RobotisDef.h | 6 ++-- .../dynamixel_sdk_linux/PortHandlerLinux.h | 6 ++-- .../src/dynamixel_sdk/GroupBulkRead.cpp | 4 +-- .../src/dynamixel_sdk/GroupBulkWrite.cpp | 6 ++-- .../src/dynamixel_sdk/GroupSyncRead.cpp | 4 +-- .../src/dynamixel_sdk/GroupSyncWrite.cpp | 4 +-- 15 files changed, 68 insertions(+), 39 deletions(-) create mode 100644 dynamixel_sdk/include/DynamixelSDK.h diff --git a/dynamixel_sdk/include/DynamixelSDK.h b/dynamixel_sdk/include/DynamixelSDK.h new file mode 100644 index 0000000..e64ae0d --- /dev/null +++ b/dynamixel_sdk/include/DynamixelSDK.h @@ -0,0 +1,29 @@ +/* + * DynamixelSDK.h + * + * Created on: 2016. 3. 8. + * Author: zerom + */ + +#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_DYNAMIXELSDK_H_ +#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_DYNAMIXELSDK_H_ + + +#include "dynamixel_sdk/RobotisDef.h" +#include "dynamixel_sdk/GroupBulkRead.h" +#include "dynamixel_sdk/GroupBulkWrite.h" +#include "dynamixel_sdk/GroupSyncRead.h" +#include "dynamixel_sdk/GroupSyncWrite.h" +#include "dynamixel_sdk/Protocol1PacketHandler.h" +#include "dynamixel_sdk/Protocol2PacketHandler.h" + +#ifdef __linux__ + #include "dynamixel_sdk_linux/PortHandlerLinux.h" +#endif + +#if defined(_WIN32) || defined(_WIN64) + #include "dynamixel_sdk_windows/PortHandlerWindows.h" +#endif + + +#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_DYNAMIXELSDK_H_ */ diff --git a/dynamixel_sdk/include/dynamixel_sdk/GroupBulkRead.h b/dynamixel_sdk/include/dynamixel_sdk/GroupBulkRead.h index 2447f87..e98b19f 100644 --- a/dynamixel_sdk/include/dynamixel_sdk/GroupBulkRead.h +++ b/dynamixel_sdk/include/dynamixel_sdk/GroupBulkRead.h @@ -5,8 +5,8 @@ * Author: zerom */ -#ifndef ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPBULKREAD_H_ -#define ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPBULKREAD_H_ +#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPBULKREAD_H_ +#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPBULKREAD_H_ #include @@ -56,4 +56,4 @@ public: } -#endif /* ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPBULKREAD_H_ */ +#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPBULKREAD_H_ */ diff --git a/dynamixel_sdk/include/dynamixel_sdk/GroupBulkWrite.h b/dynamixel_sdk/include/dynamixel_sdk/GroupBulkWrite.h index 510af24..348803e 100644 --- a/dynamixel_sdk/include/dynamixel_sdk/GroupBulkWrite.h +++ b/dynamixel_sdk/include/dynamixel_sdk/GroupBulkWrite.h @@ -5,8 +5,8 @@ * Author: zerom */ -#ifndef ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPBULKWRITE_H_ -#define ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPBULKWRITE_H_ +#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPBULKWRITE_H_ +#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPBULKWRITE_H_ #include @@ -52,4 +52,4 @@ public: } -#endif /* ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPBULKWRITE_H_ */ +#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPBULKWRITE_H_ */ diff --git a/dynamixel_sdk/include/dynamixel_sdk/GroupSyncRead.h b/dynamixel_sdk/include/dynamixel_sdk/GroupSyncRead.h index 900cd6f..5fb7b5e 100644 --- a/dynamixel_sdk/include/dynamixel_sdk/GroupSyncRead.h +++ b/dynamixel_sdk/include/dynamixel_sdk/GroupSyncRead.h @@ -5,8 +5,8 @@ * Author: zerom */ -#ifndef ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPSYNCREAD_H_ -#define ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPSYNCREAD_H_ +#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPSYNCREAD_H_ +#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPSYNCREAD_H_ #include @@ -56,4 +56,4 @@ public: } -#endif /* ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPSYNCREAD_H_ */ +#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPSYNCREAD_H_ */ diff --git a/dynamixel_sdk/include/dynamixel_sdk/GroupSyncWrite.h b/dynamixel_sdk/include/dynamixel_sdk/GroupSyncWrite.h index 720e60a..62a8951 100644 --- a/dynamixel_sdk/include/dynamixel_sdk/GroupSyncWrite.h +++ b/dynamixel_sdk/include/dynamixel_sdk/GroupSyncWrite.h @@ -5,8 +5,8 @@ * Author: zerom */ -#ifndef ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPSYNCWRITE_H_ -#define ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPSYNCWRITE_H_ +#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPSYNCWRITE_H_ +#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPSYNCWRITE_H_ #include @@ -51,4 +51,4 @@ public: } -#endif /* ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPSYNCWRITE_H_ */ +#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPSYNCWRITE_H_ */ diff --git a/dynamixel_sdk/include/dynamixel_sdk/PacketHandler.h b/dynamixel_sdk/include/dynamixel_sdk/PacketHandler.h index 3746831..5a048a5 100644 --- a/dynamixel_sdk/include/dynamixel_sdk/PacketHandler.h +++ b/dynamixel_sdk/include/dynamixel_sdk/PacketHandler.h @@ -5,8 +5,8 @@ * Author: zerom */ -#ifndef ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PACKETHANDLER_H_ -#define ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PACKETHANDLER_H_ +#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PACKETHANDLER_H_ +#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PACKETHANDLER_H_ #include @@ -127,4 +127,4 @@ public: } -#endif /* ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PACKETHANDLER_H_ */ +#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PACKETHANDLER_H_ */ diff --git a/dynamixel_sdk/include/dynamixel_sdk/PortHandler.h b/dynamixel_sdk/include/dynamixel_sdk/PortHandler.h index 4406f50..6920f8e 100644 --- a/dynamixel_sdk/include/dynamixel_sdk/PortHandler.h +++ b/dynamixel_sdk/include/dynamixel_sdk/PortHandler.h @@ -5,8 +5,8 @@ * Author: zerom */ -#ifndef ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PORTHANDLER_H_ -#define ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PORTHANDLER_H_ +#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PORTHANDLER_H_ +#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PORTHANDLER_H_ #include "RobotisDef.h" @@ -48,4 +48,4 @@ public: } -#endif /* ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PORTHANDLER_H_ */ +#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PORTHANDLER_H_ */ diff --git a/dynamixel_sdk/include/dynamixel_sdk/Protocol1PacketHandler.h b/dynamixel_sdk/include/dynamixel_sdk/Protocol1PacketHandler.h index a1a9634..5f1b2a4 100644 --- a/dynamixel_sdk/include/dynamixel_sdk/Protocol1PacketHandler.h +++ b/dynamixel_sdk/include/dynamixel_sdk/Protocol1PacketHandler.h @@ -5,8 +5,8 @@ * Author: zerom */ -#ifndef ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL1PACKETHANDLER_H_ -#define ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL1PACKETHANDLER_H_ +#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL1PACKETHANDLER_H_ +#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL1PACKETHANDLER_H_ #include "PacketHandler.h" @@ -92,4 +92,4 @@ public: } -#endif /* ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL1PACKETHANDLER_H_ */ +#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL1PACKETHANDLER_H_ */ diff --git a/dynamixel_sdk/include/dynamixel_sdk/Protocol2PacketHandler.h b/dynamixel_sdk/include/dynamixel_sdk/Protocol2PacketHandler.h index f37e9a9..c0ad605 100644 --- a/dynamixel_sdk/include/dynamixel_sdk/Protocol2PacketHandler.h +++ b/dynamixel_sdk/include/dynamixel_sdk/Protocol2PacketHandler.h @@ -5,8 +5,8 @@ * Author: zerom */ -#ifndef ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL2PACKETHANDLER_H_ -#define ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL2PACKETHANDLER_H_ +#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL2PACKETHANDLER_H_ +#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL2PACKETHANDLER_H_ #include "PacketHandler.h" @@ -97,4 +97,4 @@ public: } -#endif /* ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL2PACKETHANDLER_H_ */ +#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL2PACKETHANDLER_H_ */ diff --git a/dynamixel_sdk/include/dynamixel_sdk/RobotisDef.h b/dynamixel_sdk/include/dynamixel_sdk/RobotisDef.h index a860e9d..1fe2c61 100644 --- a/dynamixel_sdk/include/dynamixel_sdk/RobotisDef.h +++ b/dynamixel_sdk/include/dynamixel_sdk/RobotisDef.h @@ -5,8 +5,8 @@ * Author: zerom */ -#ifndef ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_ROBOTISDEF_H_ -#define ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_ROBOTISDEF_H_ +#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_ROBOTISDEF_H_ +#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_ROBOTISDEF_H_ typedef char INT8_T; @@ -18,4 +18,4 @@ typedef unsigned short int UINT16_T; typedef unsigned int UINT32_T; -#endif /* ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_ROBOTISDEF_H_ */ +#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_ROBOTISDEF_H_ */ diff --git a/dynamixel_sdk/include/dynamixel_sdk_linux/PortHandlerLinux.h b/dynamixel_sdk/include/dynamixel_sdk_linux/PortHandlerLinux.h index 2f3d487..02e4f63 100644 --- a/dynamixel_sdk/include/dynamixel_sdk_linux/PortHandlerLinux.h +++ b/dynamixel_sdk/include/dynamixel_sdk_linux/PortHandlerLinux.h @@ -5,8 +5,8 @@ * Author: zerom */ -#ifndef ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_LINUX_PORTHANDLERLINUX_H_ -#define ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_LINUX_PORTHANDLERLINUX_H_ +#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_LINUX_PORTHANDLERLINUX_H_ +#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_LINUX_PORTHANDLERLINUX_H_ #include "dynamixel_sdk/PortHandler.h" @@ -59,4 +59,4 @@ public: } -#endif /* ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_LINUX_PORTHANDLERLINUX_H_ */ +#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_LINUX_PORTHANDLERLINUX_H_ */ diff --git a/dynamixel_sdk/src/dynamixel_sdk/GroupBulkRead.cpp b/dynamixel_sdk/src/dynamixel_sdk/GroupBulkRead.cpp index a97c4cc..d779039 100644 --- a/dynamixel_sdk/src/dynamixel_sdk/GroupBulkRead.cpp +++ b/dynamixel_sdk/src/dynamixel_sdk/GroupBulkRead.cpp @@ -34,7 +34,7 @@ void GroupBulkRead::MakeParam() param_ = new UINT8_T[id_list_.size() * 5]; // ID(1) + ADDR(2) + LENGTH(2) int _idx = 0; - for(int _i = 0; _i < id_list_.size(); _i++) + for(unsigned int _i = 0; _i < id_list_.size(); _i++) { UINT8_T _id = id_list_[_i]; if(ph_->GetProtocolVersion() == 1.0) @@ -87,7 +87,7 @@ void GroupBulkRead::ClearParam() { if(id_list_.size() != 0) { - for(int _i = 0; _i < id_list_.size(); _i++) + for(unsigned int _i = 0; _i < id_list_.size(); _i++) delete[] data_list_[id_list_[_i]]; } diff --git a/dynamixel_sdk/src/dynamixel_sdk/GroupBulkWrite.cpp b/dynamixel_sdk/src/dynamixel_sdk/GroupBulkWrite.cpp index 7096463..0083db8 100644 --- a/dynamixel_sdk/src/dynamixel_sdk/GroupBulkWrite.cpp +++ b/dynamixel_sdk/src/dynamixel_sdk/GroupBulkWrite.cpp @@ -29,13 +29,13 @@ void GroupBulkWrite::MakeParam() param_ = 0; param_length_ = 0; - for(int _i = 0; _i < id_list_.size(); _i++) + for(unsigned int _i = 0; _i < id_list_.size(); _i++) param_length_ += 1 + 2 + 2 + length_list_[id_list_[_i]]; param_ = new UINT8_T[param_length_]; int _idx = 0; - for(int _i = 0; _i < id_list_.size(); _i++) + for(unsigned int _i = 0; _i < id_list_.size(); _i++) { UINT8_T _id = id_list_[_i]; if(data_list_[_id] == 0) @@ -112,7 +112,7 @@ void GroupBulkWrite::ClearParam() if(id_list_.size() != 0) { - for(int _i = 0; _i < id_list_.size(); _i++) + for(unsigned int _i = 0; _i < id_list_.size(); _i++) delete[] data_list_[id_list_[_i]]; } diff --git a/dynamixel_sdk/src/dynamixel_sdk/GroupSyncRead.cpp b/dynamixel_sdk/src/dynamixel_sdk/GroupSyncRead.cpp index 5006a49..7f12393 100644 --- a/dynamixel_sdk/src/dynamixel_sdk/GroupSyncRead.cpp +++ b/dynamixel_sdk/src/dynamixel_sdk/GroupSyncRead.cpp @@ -32,7 +32,7 @@ void GroupSyncRead::MakeParam() param_ = new UINT8_T[id_list_.size() * 1]; // ID(1) int _idx = 0; - for(int _i = 0; _i < id_list_.size(); _i++) + for(unsigned int _i = 0; _i < id_list_.size(); _i++) param_[_idx++] = id_list_[_i]; } @@ -72,7 +72,7 @@ void GroupSyncRead::ClearParam() if(id_list_.size() != 0) { - for(int _i = 0; _i < id_list_.size(); _i++) + for(unsigned int _i = 0; _i < id_list_.size(); _i++) delete[] data_list_[id_list_[_i]]; } diff --git a/dynamixel_sdk/src/dynamixel_sdk/GroupSyncWrite.cpp b/dynamixel_sdk/src/dynamixel_sdk/GroupSyncWrite.cpp index ac17423..3c12caa 100644 --- a/dynamixel_sdk/src/dynamixel_sdk/GroupSyncWrite.cpp +++ b/dynamixel_sdk/src/dynamixel_sdk/GroupSyncWrite.cpp @@ -32,7 +32,7 @@ void GroupSyncWrite::MakeParam() param_ = new UINT8_T[id_list_.size() * (1 + data_length_)]; // ID(1) + DATA(data_length) int _idx = 0; - for(int _i = 0; _i < id_list_.size(); _i++) + for(unsigned int _i = 0; _i < id_list_.size(); _i++) { UINT8_T _id = id_list_[_i]; if(data_list_[_id] == 0) @@ -90,7 +90,7 @@ void GroupSyncWrite::ClearParam() { if(id_list_.size() != 0) { - for(int _i = 0; _i < id_list_.size(); _i++) + for(unsigned int _i = 0; _i < id_list_.size(); _i++) delete[] data_list_[id_list_[_i]]; } From a0f412e4ddaebb7ba7bbfd4b60398e20ef7e5448 Mon Sep 17 00:00:00 2001 From: ROBOTIS Date: Wed, 9 Mar 2016 17:38:46 +0900 Subject: [PATCH 005/238] - sync with dynamixel sdk (print comm result & rx packet error function added) --- .../include/dynamixel_sdk/PacketHandler.h | 4 + .../dynamixel_sdk/Protocol1PacketHandler.h | 3 + .../dynamixel_sdk/Protocol2PacketHandler.h | 3 + .../src/dynamixel_sdk/GroupBulkRead.cpp | 1 + .../dynamixel_sdk/Protocol1PacketHandler.cpp | 69 ++++++++++++++ .../dynamixel_sdk/Protocol2PacketHandler.cpp | 91 +++++++++++++++++++ 6 files changed, 171 insertions(+) diff --git a/dynamixel_sdk/include/dynamixel_sdk/PacketHandler.h b/dynamixel_sdk/include/dynamixel_sdk/PacketHandler.h index 5a048a5..6edb44e 100644 --- a/dynamixel_sdk/include/dynamixel_sdk/PacketHandler.h +++ b/dynamixel_sdk/include/dynamixel_sdk/PacketHandler.h @@ -9,6 +9,7 @@ #define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PACKETHANDLER_H_ +#include #include #include "RobotisDef.h" #include "PortHandler.h" @@ -65,6 +66,9 @@ public: virtual float GetProtocolVersion() = 0; + virtual void PrintTxRxResult(int result) = 0; + virtual void PrintRxPacketError(UINT8_T error) = 0; + virtual int TxPacket (PortHandler *port, UINT8_T *txpacket) = 0; virtual int RxPacket (PortHandler *port, UINT8_T *rxpacket) = 0; virtual int TxRxPacket (PortHandler *port, UINT8_T *txpacket, UINT8_T *rxpacket, UINT8_T *error = 0) = 0; diff --git a/dynamixel_sdk/include/dynamixel_sdk/Protocol1PacketHandler.h b/dynamixel_sdk/include/dynamixel_sdk/Protocol1PacketHandler.h index 5f1b2a4..55182f4 100644 --- a/dynamixel_sdk/include/dynamixel_sdk/Protocol1PacketHandler.h +++ b/dynamixel_sdk/include/dynamixel_sdk/Protocol1PacketHandler.h @@ -28,6 +28,9 @@ public: float GetProtocolVersion() { return 1.0; } + void PrintTxRxResult(int result); + void PrintRxPacketError(UINT8_T error); + int TxPacket (PortHandler *port, UINT8_T *txpacket); int RxPacket (PortHandler *port, UINT8_T *rxpacket); int TxRxPacket (PortHandler *port, UINT8_T *txpacket, UINT8_T *rxpacket, UINT8_T *error = 0); diff --git a/dynamixel_sdk/include/dynamixel_sdk/Protocol2PacketHandler.h b/dynamixel_sdk/include/dynamixel_sdk/Protocol2PacketHandler.h index c0ad605..823c226 100644 --- a/dynamixel_sdk/include/dynamixel_sdk/Protocol2PacketHandler.h +++ b/dynamixel_sdk/include/dynamixel_sdk/Protocol2PacketHandler.h @@ -32,6 +32,9 @@ public: float GetProtocolVersion() { return 2.0; } + void PrintTxRxResult(int result); + void PrintRxPacketError(UINT8_T error); + int TxPacket (PortHandler *port, UINT8_T *txpacket); int RxPacket (PortHandler *port, UINT8_T *rxpacket); int TxRxPacket (PortHandler *port, UINT8_T *txpacket, UINT8_T *rxpacket, UINT8_T *error = 0); diff --git a/dynamixel_sdk/src/dynamixel_sdk/GroupBulkRead.cpp b/dynamixel_sdk/src/dynamixel_sdk/GroupBulkRead.cpp index d779039..cab6adc 100644 --- a/dynamixel_sdk/src/dynamixel_sdk/GroupBulkRead.cpp +++ b/dynamixel_sdk/src/dynamixel_sdk/GroupBulkRead.cpp @@ -123,6 +123,7 @@ int GroupBulkRead::RxPacket() { UINT8_T _id = id_list_[_i]; + port_->SetPacketTimeout((UINT16_T)(length_list_[_id] + 11)); _result = ph_->ReadRx(port_, length_list_[_id], data_list_[_id]); if(_result != COMM_SUCCESS) { diff --git a/dynamixel_sdk/src/dynamixel_sdk/Protocol1PacketHandler.cpp b/dynamixel_sdk/src/dynamixel_sdk/Protocol1PacketHandler.cpp index a1a52b6..2b5ccc4 100644 --- a/dynamixel_sdk/src/dynamixel_sdk/Protocol1PacketHandler.cpp +++ b/dynamixel_sdk/src/dynamixel_sdk/Protocol1PacketHandler.cpp @@ -36,6 +36,75 @@ Protocol1PacketHandler *Protocol1PacketHandler::unique_instance_ = new Protocol1 Protocol1PacketHandler::Protocol1PacketHandler() { } +void Protocol1PacketHandler::PrintTxRxResult(int result) +{ + switch(result) + { + case COMM_SUCCESS: + printf("[TxRxResult] Communication success.\n"); + break; + + case COMM_PORT_BUSY: + printf("[TxRxResult] Port is in use!\n"); + break; + + case COMM_TX_FAIL: + printf("[TxRxResult] Failed transmit instruction packet!\n"); + break; + + case COMM_RX_FAIL: + printf("[TxRxResult] Failed get status packet from device!\n"); + break; + + case COMM_TX_ERROR: + printf("[TxRxResult] Incorrect instruction packet!\n"); + break; + + case COMM_RX_WAITING: + printf("[TxRxResult] Now recieving status packet!\n"); + break; + + case COMM_RX_TIMEOUT: + printf("[TxRxResult] There is no status packet!\n"); + break; + + case COMM_RX_CORRUPT: + printf("[TxRxResult] Incorrect status packet!\n"); + break; + + case COMM_NOT_AVAILABLE: + printf("[TxRxResult] Protocol does not support This function!\n"); + break; + + default: + break; + } +} + +void Protocol1PacketHandler::PrintRxPacketError(UINT8_T error) +{ + if(error & ERRBIT_VOLTAGE) + printf("[RxPacketError] Input voltage error!\n"); + + if(error & ERRBIT_ANGLE) + printf("[RxPacketError] Angle limit error!\n"); + + if(error & ERRBIT_OVERHEAT) + printf("[RxPacketError] Overheat error!\n"); + + if(error & ERRBIT_RANGE) + printf("[RxPacketError] Out of range error!\n"); + + if(error & ERRBIT_CHECKSUM) + printf("[RxPacketError] Checksum error!\n"); + + if(error & ERRBIT_OVERLOAD) + printf("[RxPacketError] Overload error!\n"); + + if(error & ERRBIT_INSTRUCTION) + printf("[RxPacketError] Instruction code error!\n"); +} + int Protocol1PacketHandler::TxPacket(PortHandler *port, UINT8_T *txpacket) { UINT8_T _checksum = 0; diff --git a/dynamixel_sdk/src/dynamixel_sdk/Protocol2PacketHandler.cpp b/dynamixel_sdk/src/dynamixel_sdk/Protocol2PacketHandler.cpp index ecdc260..c178f76 100644 --- a/dynamixel_sdk/src/dynamixel_sdk/Protocol2PacketHandler.cpp +++ b/dynamixel_sdk/src/dynamixel_sdk/Protocol2PacketHandler.cpp @@ -42,6 +42,97 @@ Protocol2PacketHandler *Protocol2PacketHandler::unique_instance_ = new Protocol2 Protocol2PacketHandler::Protocol2PacketHandler() { } +void Protocol2PacketHandler::PrintTxRxResult(int result) +{ + switch(result) + { + case COMM_SUCCESS: + printf("[TxRxResult] Communication success.\n"); + break; + + case COMM_PORT_BUSY: + printf("[TxRxResult] Port is in use!\n"); + break; + + case COMM_TX_FAIL: + printf("[TxRxResult] Failed transmit instruction packet!\n"); + break; + + case COMM_RX_FAIL: + printf("[TxRxResult] Failed get status packet from device!\n"); + break; + + case COMM_TX_ERROR: + printf("[TxRxResult] Incorrect instruction packet!\n"); + break; + + case COMM_RX_WAITING: + printf("[TxRxResult] Now recieving status packet!\n"); + break; + + case COMM_RX_TIMEOUT: + printf("[TxRxResult] There is no status packet!\n"); + break; + + case COMM_RX_CORRUPT: + printf("[TxRxResult] Incorrect status packet!\n"); + break; + + case COMM_NOT_AVAILABLE: + printf("[TxRxResult] Protocol does not support This function!\n"); + break; + + default: + break; + } +} + +void Protocol2PacketHandler::PrintRxPacketError(UINT8_T error) +{ + if(error & ERRBIT_ALERT) + printf("[RxPacketError] Hardware error occurred. Check the error at Control Table (Hardware Error Status)!\n"); + + int _error = error & ~ERRBIT_ALERT; + + switch(_error) + { + case 0: + break; + + case ERRNUM_RESULT_FAIL: + printf("[RxPacketError] Failed to process the instruction packet!\n"); + break; + + case ERRNUM_INSTRUCTION: + printf("[RxPacketError] Undefined instruction or incorrect instruction!\n"); + break; + + case ERRNUM_CRC: + printf("[RxPacketError] CRC doesn't match!\n"); + break; + + case ERRNUM_DATA_RANGE: + printf("[RxPacketError] The data value is out of range!\n"); + break; + + case ERRNUM_DATA_LENGTH: + printf("[RxPacketError] The data length does not match as expected!\n"); + break; + + case ERRNUM_DATA_LIMIT: + printf("[RxPacketError] The data value exceeds the limit value!\n"); + break; + + case ERRNUM_ACCESS: + printf("[RxPacketError] Writing or Reading is not available to target address!\n"); + break; + + default: + printf("[RxPacketError] Unknown error code!\n"); + break; + } +} + unsigned short Protocol2PacketHandler::UpdateCRC(UINT16_T crc_accum, UINT8_T *data_blk_ptr, UINT16_T data_blk_size) { UINT16_T i, j; From 4e319441dde319bb4fdabfcfacb08abbec842a93 Mon Sep 17 00:00:00 2001 From: ROBOTIS Date: Wed, 9 Mar 2016 17:54:35 +0900 Subject: [PATCH 006/238] - added first bulk read failure protection code --- .../src/robotis_controller/RobotisController.cpp | 15 +++++++++++++-- 1 file changed, 13 insertions(+), 2 deletions(-) diff --git a/robotis_controller/src/robotis_controller/RobotisController.cpp b/robotis_controller/src/robotis_controller/RobotisController.cpp index edd2269..16351d8 100644 --- a/robotis_controller/src/robotis_controller/RobotisController.cpp +++ b/robotis_controller/src/robotis_controller/RobotisController.cpp @@ -52,9 +52,20 @@ void RobotisController::InitSyncWrite() // bulkread twice for(std::map::iterator _it = port_to_bulk_read.begin(); _it != port_to_bulk_read.end(); _it++) _it->second->TxRxPacket(); - usleep(50*1000); for(std::map::iterator _it = port_to_bulk_read.begin(); _it != port_to_bulk_read.end(); _it++) - _it->second->TxRxPacket(); + { + int _error_cnt = 0; + int _result = COMM_SUCCESS; + do { + if(++_error_cnt > 10) + { + ROS_ERROR("[RobotisController] bulk read fail!!"); + exit(-1); + } + usleep(10*1000); + _result = _it->second->TxRxPacket(); + } while (_result != COMM_SUCCESS); + } ROS_INFO("FIRST BULKREAD END"); // clear syncwrite param setting From 33462ebfb78b1eb3b649a5362fd549b646a8a378 Mon Sep 17 00:00:00 2001 From: ROBOTIS Date: Thu, 10 Mar 2016 13:52:14 +0900 Subject: [PATCH 007/238] - change line endings ( \r\n -> \n ) --- dynamixel_sdk/src/dynamixel_sdk/GroupBulkRead.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/dynamixel_sdk/src/dynamixel_sdk/GroupBulkRead.cpp b/dynamixel_sdk/src/dynamixel_sdk/GroupBulkRead.cpp index cab6adc..d779039 100644 --- a/dynamixel_sdk/src/dynamixel_sdk/GroupBulkRead.cpp +++ b/dynamixel_sdk/src/dynamixel_sdk/GroupBulkRead.cpp @@ -123,7 +123,6 @@ int GroupBulkRead::RxPacket() { UINT8_T _id = id_list_[_i]; - port_->SetPacketTimeout((UINT16_T)(length_list_[_id] + 11)); _result = ph_->ReadRx(port_, length_list_[_id], data_list_[_id]); if(_result != COMM_SUCCESS) { From 16489939220e47753e4b270389172cd108e00705 Mon Sep 17 00:00:00 2001 From: ROBOTIS Date: Thu, 10 Mar 2016 19:57:33 +0900 Subject: [PATCH 008/238] - added code to support the gazebo simulator --- .../robotis_controller/RobotisController.h | 13 +- .../robotis_controller/RobotisController.cpp | 1354 +++++++++-------- robotis_device/src/robotis_device/Robot.cpp | 4 +- 3 files changed, 738 insertions(+), 633 deletions(-) diff --git a/robotis_controller/include/robotis_controller/RobotisController.h b/robotis_controller/include/robotis_controller/RobotisController.h index f855a38..07b6716 100644 --- a/robotis_controller/include/robotis_controller/RobotisController.h +++ b/robotis_controller/include/robotis_controller/RobotisController.h @@ -13,6 +13,7 @@ #include #include #include +#include #include #include "robotis_device/Robot.h" @@ -41,8 +42,10 @@ private: static RobotisController *unique_instance_; boost::thread queue_thread_; - boost::mutex queue_mutex_; + boost::thread gazebo_thread_; + boost::mutex queue_mutex_; + bool init_pose_loaded_; bool is_timer_running_; bool stop_timer_; pthread_t timer_thread_; @@ -54,6 +57,7 @@ private: RobotisController(); void QueueThread(); + void GazeboThread(); bool CheckTimerStop(); void InitSyncWrite(); @@ -64,6 +68,9 @@ public: bool DEBUG_PRINT; Robot *robot; + bool gazebo_mode; + std::string gazebo_robot_name; + // TODO: TEMPORARY CODE !! std::map port_to_bulk_read; std::map port_to_sync_write; @@ -73,6 +80,8 @@ public: ros::Publisher present_joint_state_pub; ros::Publisher current_module_pub; + std::map gazebo_joint_pub; + static void *ThreadProc(void *param); static RobotisController *GetInstance() { return unique_instance_; } @@ -94,6 +103,8 @@ public: void SetCtrlModuleCallback(const robotis_controller_msgs::JointCtrlModule::ConstPtr &msg); bool GetCtrlModuleCallback(robotis_controller_msgs::GetJointModule::Request &req, robotis_controller_msgs::GetJointModule::Response &res); + void GazeboJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg); + int Ping (const std::string joint_name, UINT8_T *error = 0); int Ping (const std::string joint_name, UINT16_T* model_number, UINT8_T *error = 0); diff --git a/robotis_controller/src/robotis_controller/RobotisController.cpp b/robotis_controller/src/robotis_controller/RobotisController.cpp index 16351d8..7f93f94 100644 --- a/robotis_controller/src/robotis_controller/RobotisController.cpp +++ b/robotis_controller/src/robotis_controller/RobotisController.cpp @@ -38,24 +38,30 @@ const UINT16_T EXT_PORT_DATA4_ADDR = 653; RobotisController::RobotisController() : is_timer_running_(false), stop_timer_(false), + init_pose_loaded_(false), timer_thread_(0), controller_mode_(MOTION_MODULE_MODE), DEBUG_PRINT(false), - robot(0) + robot(0), + gazebo_mode(false), + gazebo_robot_name("robotis") { - direct_sync_write_.clear(); + direct_sync_write_.clear(); } void RobotisController::InitSyncWrite() { - ROS_INFO("FIRST BULKREAD"); - // bulkread twice - for(std::map::iterator _it = port_to_bulk_read.begin(); _it != port_to_bulk_read.end(); _it++) - _it->second->TxRxPacket(); - for(std::map::iterator _it = port_to_bulk_read.begin(); _it != port_to_bulk_read.end(); _it++) - { - int _error_cnt = 0; - int _result = COMM_SUCCESS; + if(gazebo_mode == true) + return; + + ROS_INFO("FIRST BULKREAD"); + // bulkread twice + for(std::map::iterator _it = port_to_bulk_read.begin(); _it != port_to_bulk_read.end(); _it++) + _it->second->TxRxPacket(); + for(std::map::iterator _it = port_to_bulk_read.begin(); _it != port_to_bulk_read.end(); _it++) + { + int _error_cnt = 0; + int _result = COMM_SUCCESS; do { if(++_error_cnt > 10) { @@ -65,58 +71,68 @@ void RobotisController::InitSyncWrite() usleep(10*1000); _result = _it->second->TxRxPacket(); } while (_result != COMM_SUCCESS); - } - ROS_INFO("FIRST BULKREAD END"); + } + init_pose_loaded_ = true; + ROS_INFO("FIRST BULKREAD END"); - // clear syncwrite param setting - for(std::map::iterator _it = port_to_sync_write.begin(); _it != port_to_sync_write.end(); _it++) - _it->second->ClearParam(); + // clear syncwrite param setting + for(std::map::iterator _it = port_to_sync_write.begin(); _it != port_to_sync_write.end(); _it++) + _it->second->ClearParam(); - // set init syncwrite param(from data of bulkread) - for(std::map::iterator _it = robot->dxls.begin(); _it != robot->dxls.end(); _it++) - { - INT32_T _pos = 0; + // set init syncwrite param(from data of bulkread) + for(std::map::iterator _it = robot->dxls.begin(); _it != robot->dxls.end(); _it++) + { + INT32_T _pos = 0; - std::string _joint_name = _it->first; - Dynamixel *_dxl = _it->second; + std::string _joint_name = _it->first; + Dynamixel *_dxl = _it->second; - bool _res = false; - _res = port_to_bulk_read[_dxl->port_name]->GetData(_dxl->id, PRESENT_POSITION_ADDR, (UINT32_T*)&_pos); - if(_res == true) - { - _dxl->dxl_state->present_position = _dxl->ConvertValue2Radian(_pos) - _dxl->dxl_state->position_offset; // remove offset; - _dxl->dxl_state->goal_position = _dxl->dxl_state->present_position; - } + bool _res = false; + _res = port_to_bulk_read[_dxl->port_name]->GetData(_dxl->id, PRESENT_POSITION_ADDR, (UINT32_T*)&_pos); + if(_res == true) + { + _dxl->dxl_state->present_position = _dxl->ConvertValue2Radian(_pos) - _dxl->dxl_state->position_offset; // remove offset; + _dxl->dxl_state->goal_position = _dxl->dxl_state->present_position; + } - UINT8_T _sync_write_data[4]; - _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_pos)); - _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_pos)); - _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_pos)); - _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_pos)); + UINT8_T _sync_write_data[4]; + _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_pos)); + _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_pos)); + _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_pos)); + _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_pos)); - port_to_sync_write[_dxl->port_name]->AddParam(_dxl->id, _sync_write_data); - } + port_to_sync_write[_dxl->port_name]->AddParam(_dxl->id, _sync_write_data); + } } bool RobotisController::Initialize(const std::string robot_file_path, const std::string init_file_path) { - std::string _dev_desc_dir_path = ros::package::getPath("robotis_device") + "/devices"; + std::string _dev_desc_dir_path = ros::package::getPath("robotis_device") + "/devices"; - // load robot info : port , device - robot = new Robot(robot_file_path, _dev_desc_dir_path); - // TODO: fill joint_ctrl_module_ + // load robot info : port , device + robot = new Robot(robot_file_path, _dev_desc_dir_path); + if(gazebo_mode == true) + { + queue_thread_ = boost::thread(boost::bind(&RobotisController::QueueThread, this)); + return true; + } - // TODO: TEMPORARY CODE !! - /* temporary code start */ - PacketHandler *_protocol2_handler = PacketHandler::GetPacketHandler(2.0); + // TODO: TEMPORARY CODE !! + /* temporary code start */ + PacketHandler *_protocol2_handler = PacketHandler::GetPacketHandler(2.0); - for(std::map::iterator _it = robot->ports.begin(); _it != robot->ports.end(); _it++) - { - port_to_bulk_read[_it->first] = new GroupBulkRead(_it->second, _protocol2_handler); - port_to_sync_write[_it->first] = new GroupSyncWrite(_it->second, _protocol2_handler, GOAL_POSITION_ADDR, 4); - } + for(std::map::iterator _it = robot->ports.begin(); _it != robot->ports.end(); _it++) + { + if(_it->second->SetBaudRate(_it->second->GetBaudRate()) == false) + { + ROS_ERROR("PORT [%s] SETUP ERROR! (baudrate: %d)", _it->first.c_str(), _it->second->GetBaudRate()); + exit(-1); + } + port_to_bulk_read[_it->first] = new GroupBulkRead(_it->second, _protocol2_handler); + port_to_sync_write[_it->first] = new GroupSyncWrite(_it->second, _protocol2_handler, GOAL_POSITION_ADDR, 4); + } // TODO: // for loop ping --> no response : return false @@ -133,782 +149,862 @@ bool RobotisController::Initialize(const std::string robot_file_path, const std: } // joint(dynamixel) initialize - if(DEBUG_PRINT) ROS_WARN("INIT FILE LOAD"); + if(DEBUG_PRINT) ROS_WARN("INIT FILE LOAD"); - YAML::Node _doc; - try{ - _doc = YAML::LoadFile(init_file_path.c_str()); + YAML::Node _doc; + try{ + _doc = YAML::LoadFile(init_file_path.c_str()); - for(YAML::const_iterator _it_doc = _doc.begin(); _it_doc != _doc.end(); _it_doc++) - { - std::string _joint_name = _it_doc->first.as(); + for(YAML::const_iterator _it_doc = _doc.begin(); _it_doc != _doc.end(); _it_doc++) + { + std::string _joint_name = _it_doc->first.as(); - YAML::Node _joint_node = _doc[_joint_name]; - if(_joint_node.size() == 0) - continue; + YAML::Node _joint_node = _doc[_joint_name]; + if(_joint_node.size() == 0) + continue; Dynamixel *_dxl = NULL; - std::map::iterator _dxl_it = robot->dxls.find(_joint_name); - if(_dxl_it != robot->dxls.end()) - _dxl = _dxl_it->second; + std::map::iterator _dxl_it = robot->dxls.find(_joint_name); + if(_dxl_it != robot->dxls.end()) + _dxl = _dxl_it->second; - if(_dxl == NULL) - { - ROS_WARN("Joint [%s] does not found.", _joint_name.c_str()); - continue; - } - if(DEBUG_PRINT) ROS_INFO("JOINT_NAME: %s", _joint_name.c_str()); + if(_dxl == NULL) + { + ROS_WARN("Joint [%s] does not found.", _joint_name.c_str()); + continue; + } + if(DEBUG_PRINT) ROS_INFO("JOINT_NAME: %s", _joint_name.c_str()); - for(YAML::const_iterator _it_joint = _joint_node.begin(); _it_joint != _joint_node.end(); _it_joint++) - { - std::string _item_name = _it_joint->first.as(); + for(YAML::const_iterator _it_joint = _joint_node.begin(); _it_joint != _joint_node.end(); _it_joint++) + { + std::string _item_name = _it_joint->first.as(); - if(DEBUG_PRINT) ROS_INFO(" ITEM_NAME: %s", _item_name.c_str()); - // indirect address setting - if(_item_name == INDIRECT_ADDRESS) - { - YAML::Node _indirect_node = _joint_node[_item_name]; - if(_indirect_node.size() == 0) - continue; + if(DEBUG_PRINT) ROS_INFO(" ITEM_NAME: %s", _item_name.c_str()); + // indirect address setting + if(_item_name == INDIRECT_ADDRESS) + { + YAML::Node _indirect_node = _joint_node[_item_name]; + if(_indirect_node.size() == 0) + continue; - for(YAML::const_iterator _it_idx = _indirect_node.begin(); _it_idx != _indirect_node.end(); _it_idx++) - { - int _start_idx = _it_idx->first.as(); - if(_start_idx < 1 || 256 < _start_idx) - ROS_WARN("[%s] INDIRECT ADDRESS start index is out of range. (%d)", _joint_name.c_str(), _start_idx); - if(DEBUG_PRINT) ROS_INFO(" START_IDX: %d", _start_idx); + for(YAML::const_iterator _it_idx = _indirect_node.begin(); _it_idx != _indirect_node.end(); _it_idx++) + { + int _start_idx = _it_idx->first.as(); + if(_start_idx < 1 || 256 < _start_idx) + ROS_WARN("[%s] INDIRECT ADDRESS start index is out of range. (%d)", _joint_name.c_str(), _start_idx); + if(DEBUG_PRINT) ROS_INFO(" START_IDX: %d", _start_idx); - YAML::Node _indirect_item_node = _indirect_node[_start_idx]; - if(_indirect_item_node.size() == 0) - continue; + YAML::Node _indirect_item_node = _indirect_node[_start_idx]; + if(_indirect_item_node.size() == 0) + continue; - int _start_address = _dxl->ctrl_table[INDIRECT_ADDRESS]->address + _start_idx - 1; - for(unsigned int _i = 0; _i < _indirect_item_node.size(); _i++) - { - std::string _indir_item_name = _indirect_item_node[_i].as().c_str(); - int _addr_leng = _dxl->ctrl_table[_indir_item_name]->data_length; - for(int _l = 0; _l < _addr_leng; _l++) - { - if(DEBUG_PRINT) ROS_INFO(" - INDIR_ADDR[%d] : %d", _start_address, _dxl->ctrl_table[_indir_item_name]->address + _l); - Write2Byte(_joint_name, _start_address, _dxl->ctrl_table[_indir_item_name]->address + _l); - _start_address += 2; - } - } - } - } - // other items setting - else - { - UINT32_T _value = _it_joint->second.as(); + int _start_address = _dxl->ctrl_table[INDIRECT_ADDRESS]->address + _start_idx - 1; + for(unsigned int _i = 0; _i < _indirect_item_node.size(); _i++) + { + std::string _indir_item_name = _indirect_item_node[_i].as().c_str(); + int _addr_leng = _dxl->ctrl_table[_indir_item_name]->data_length; + for(int _l = 0; _l < _addr_leng; _l++) + { + if(DEBUG_PRINT) ROS_INFO(" - INDIR_ADDR[%d] : %d", _start_address, _dxl->ctrl_table[_indir_item_name]->address + _l); + Write2Byte(_joint_name, _start_address, _dxl->ctrl_table[_indir_item_name]->address + _l); + _start_address += 2; + } + } + } + } + else // other items setting + { + UINT32_T _value = _it_joint->second.as(); - ControlTableItem *_item = _dxl->ctrl_table[_item_name]; - if(_item == NULL) - { - ROS_WARN("Control Item [%s] does not found.", _item_name.c_str()); - continue; - } + ControlTableItem *_item = _dxl->ctrl_table[_item_name]; + if(_item == NULL) + { + ROS_WARN("Control Item [%s] does not found.", _item_name.c_str()); + continue; + } - switch(_item->data_length) - { - case 1: - Write1Byte(_joint_name, _item->address, (UINT8_T)_value); - break; - case 2: - Write2Byte(_joint_name, _item->address, (UINT16_T)_value); - break; - case 4: - Write4Byte(_joint_name, _item->address, _value); - break; - default: - break; - } - } - } - } - } catch(const std::exception& e) { - ROS_INFO("Dynamixel Init file not found."); - } + switch(_item->data_length) + { + case 1: + Write1Byte(_joint_name, _item->address, (UINT8_T)_value); + break; + case 2: + Write2Byte(_joint_name, _item->address, (UINT16_T)_value); + break; + case 4: + Write4Byte(_joint_name, _item->address, _value); + break; + default: + break; + } + } + } + } + } catch(const std::exception& e) { + ROS_INFO("Dynamixel Init file not found."); + } - // [ BulkRead ] StartAddress : Present Position , Length : 10 ( Position/Velocity/Current ) - for(std::map::iterator _it = robot->dxls.begin(); _it != robot->dxls.end(); _it++) - { - std::string _joint_name = _it->first; - Dynamixel *_dxl = _it->second; + // [ BulkRead ] StartAddress : Present Position , Length : 10 ( Position/Velocity/Current ) + for(std::map::iterator _it = robot->dxls.begin(); _it != robot->dxls.end(); _it++) + { + std::string _joint_name = _it->first; + Dynamixel *_dxl = _it->second; if(_dxl == NULL) continue; - UINT16_T _data_length = 0; - if(_dxl->id == 11 || _dxl->id == 12 || _dxl->id == 23 || _dxl->id == 24) - _data_length = 17; - else if(_dxl->id == 13 || _dxl->id == 14 || _dxl->id == 25 || _dxl->id == 26) - _data_length = 21; - else - _data_length = 13; - port_to_bulk_read[_dxl->port_name]->AddParam(robot->dxls[_joint_name]->id, PRESENT_POSITION_ADDR, _data_length); + UINT16_T _data_length = 0; + if(_dxl->id == 11 || _dxl->id == 12 || _dxl->id == 23 || _dxl->id == 24) + _data_length = 17; + else if(_dxl->id == 13 || _dxl->id == 14 || _dxl->id == 25 || _dxl->id == 26) + _data_length = 21; + else + _data_length = 13; + port_to_bulk_read[_dxl->port_name]->AddParam(robot->dxls[_joint_name]->id, PRESENT_POSITION_ADDR, _data_length); - // Torque ON - _protocol2_handler->Write1ByteTxRx(robot->ports[_dxl->port_name], robot->dxls[_joint_name]->id, TORQUE_ENABLE_ADDR, 1); - } + // Torque ON + _protocol2_handler->Write1ByteTxRx(robot->ports[_dxl->port_name], robot->dxls[_joint_name]->id, TORQUE_ENABLE_ADDR, 1); + } - //InitSyncWrite(); + //InitSyncWrite(); - for(std::map::iterator _it = robot->ports.begin(); _it != robot->ports.end(); _it++) - { - // set goal velocity = 0 - _protocol2_handler->Write4ByteTxOnly(_it->second, BROADCAST_ID, GOAL_VELOCITY_ADDR, 0); - // set goal acceleration = 0 - _protocol2_handler->Write4ByteTxOnly(_it->second, BROADCAST_ID, GOAL_ACCELERATION_ADDR, 0); - } + for(std::map::iterator _it = robot->ports.begin(); _it != robot->ports.end(); _it++) + { + // set goal velocity = 0 + _protocol2_handler->Write4ByteTxOnly(_it->second, BROADCAST_ID, GOAL_VELOCITY_ADDR, 0); + // set goal acceleration = 0 + _protocol2_handler->Write4ByteTxOnly(_it->second, BROADCAST_ID, GOAL_ACCELERATION_ADDR, 0); + } - /* temporary code end */ + /* temporary code end */ - queue_thread_ = boost::thread(boost::bind(&RobotisController::QueueThread, this)); - return true; + queue_thread_ = boost::thread(boost::bind(&RobotisController::QueueThread, this)); + return true; +} + +void RobotisController::GazeboThread() +{ + ros::Rate gazebo_rate(1000 / CONTROL_CYCLE_MSEC); + + while(!stop_timer_) + { + if(init_pose_loaded_ == true) + Process(); + gazebo_rate.sleep(); + } } void RobotisController::QueueThread() { - ros::NodeHandle _ros_node; - ros::CallbackQueue _callback_queue; + ros::NodeHandle _ros_node; + ros::CallbackQueue _callback_queue; - _ros_node.setCallbackQueue(&_callback_queue); + _ros_node.setCallbackQueue(&_callback_queue); - /* subscriber */ - ros::Subscriber _sync_write_item_sub= _ros_node.subscribe("/robotis/sync_write_item", 10, &RobotisController::SyncWriteItemCallback, this); - ros::Subscriber _ctrl_module_sub = _ros_node.subscribe("/robotis/set_ctrl_module", 10, &RobotisController::SetCtrlModuleCallback, this); - ros::Subscriber _controller_mode_sub= _ros_node.subscribe("/robotis/set_controller_mode", 10, &RobotisController::SetControllerModeCallback, this); - ros::Subscriber _direct_control_sub = _ros_node.subscribe("/robotis/set_joint_states", 10, &RobotisController::SetJointStatesCallback, this); + /* subscriber */ + ros::Subscriber _sync_write_item_sub= _ros_node.subscribe("/robotis/sync_write_item", 10, &RobotisController::SyncWriteItemCallback, this); + ros::Subscriber _ctrl_module_sub = _ros_node.subscribe("/robotis/set_ctrl_module", 10, &RobotisController::SetCtrlModuleCallback, this); + ros::Subscriber _controller_mode_sub= _ros_node.subscribe("/robotis/set_controller_mode", 10, &RobotisController::SetControllerModeCallback, this); + ros::Subscriber _direct_control_sub = _ros_node.subscribe("/robotis/set_joint_states", 10, &RobotisController::SetJointStatesCallback, this); - /* publisher */ - goal_joint_state_pub = _ros_node.advertise("/robotis/goal_joint_states", 10); - present_joint_state_pub = _ros_node.advertise("/robotis/present_joint_states", 10); - current_module_pub = _ros_node.advertise("/robotis/current_ctrl_module", 10); + /* publisher */ + goal_joint_state_pub = _ros_node.advertise("/robotis/goal_joint_states", 10); + present_joint_state_pub = _ros_node.advertise("/robotis/present_joint_states", 10); + current_module_pub = _ros_node.advertise("/robotis/current_ctrl_module", 10); - /* service */ - ros::ServiceServer _joint_module_server = _ros_node.advertiseService("/robotis/get_ctrl_module", &RobotisController::GetCtrlModuleCallback, this); + ros::Subscriber _gazebo_joint_states_sub; + if(gazebo_mode == true) + { + _gazebo_joint_states_sub = _ros_node.subscribe("/" + gazebo_robot_name + "/joint_states", 10, &RobotisController::GazeboJointStatesCallback, this); - while(_ros_node.ok()) - { - _callback_queue.callAvailable(); - } + for(std::map::iterator _it = robot->dxls.begin(); _it != robot->dxls.end(); _it++) + gazebo_joint_pub[_it->first] = _ros_node.advertise("/" + gazebo_robot_name + "/" + _it->first + "_pos/command", 1); + } + + /* service */ + ros::ServiceServer _joint_module_server = _ros_node.advertiseService("/robotis/get_ctrl_module", &RobotisController::GetCtrlModuleCallback, this); + + while(_ros_node.ok()) + { + _callback_queue.callAvailable(); + } } void *RobotisController::ThreadProc(void *param) { - RobotisController *controller = (RobotisController *)param; - static struct timespec next_time; - static struct timespec curr_time; + RobotisController *controller = (RobotisController *)param; + static struct timespec next_time; + static struct timespec curr_time; - ROS_INFO("controller::thread_proc"); + ROS_INFO("controller::thread_proc"); - clock_gettime(CLOCK_MONOTONIC, &next_time); + clock_gettime(CLOCK_MONOTONIC, &next_time); - while(!controller->stop_timer_) - { - next_time.tv_sec += (next_time.tv_nsec + CONTROL_CYCLE_MSEC * 1000000) / 1000000000; - next_time.tv_nsec = (next_time.tv_nsec + CONTROL_CYCLE_MSEC * 1000000) % 1000000000; + while(!controller->stop_timer_) + { + next_time.tv_sec += (next_time.tv_nsec + CONTROL_CYCLE_MSEC * 1000000) / 1000000000; + next_time.tv_nsec = (next_time.tv_nsec + CONTROL_CYCLE_MSEC * 1000000) % 1000000000; - controller->Process(); + controller->Process(); - clock_gettime(CLOCK_MONOTONIC, &curr_time); - long delta_nsec = (next_time.tv_sec - curr_time.tv_sec) * 1000000000 + (next_time.tv_nsec - curr_time.tv_nsec); - if(delta_nsec < -100000 ) - { - if(controller->DEBUG_PRINT == true) - ROS_WARN("[RobotisController::ThreadProc] NEXT TIME < CURR TIME.. (%f)[%ld.%09ld / %ld.%09ld]", delta_nsec/1000000.0, (long)next_time.tv_sec, (long)next_time.tv_nsec, (long)curr_time.tv_sec, (long)curr_time.tv_nsec); - // next_time = curr_time + 3 msec - next_time.tv_sec = curr_time.tv_sec + (curr_time.tv_nsec + 3000000) / 1000000000; - next_time.tv_nsec = (curr_time.tv_nsec + 3000000) % 1000000000; - } + clock_gettime(CLOCK_MONOTONIC, &curr_time); + long delta_nsec = (next_time.tv_sec - curr_time.tv_sec) * 1000000000 + (next_time.tv_nsec - curr_time.tv_nsec); + if(delta_nsec < -100000 ) + { + if(controller->DEBUG_PRINT == true) + ROS_WARN("[RobotisController::ThreadProc] NEXT TIME < CURR TIME.. (%f)[%ld.%09ld / %ld.%09ld]", delta_nsec/1000000.0, (long)next_time.tv_sec, (long)next_time.tv_nsec, (long)curr_time.tv_sec, (long)curr_time.tv_nsec); + // next_time = curr_time + 3 msec + next_time.tv_sec = curr_time.tv_sec + (curr_time.tv_nsec + 3000000) / 1000000000; + next_time.tv_nsec = (curr_time.tv_nsec + 3000000) % 1000000000; + } - clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &next_time, NULL); - } - return 0; + clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &next_time, NULL); + } + return 0; } void RobotisController::StartTimer() { - if(this->is_timer_running_ == true) - return; + if(this->is_timer_running_ == true) + return; - InitSyncWrite(); + if(this->gazebo_mode == true) + { + // create and start the thread + gazebo_thread_ = boost::thread(boost::bind(&RobotisController::GazeboThread, this)); + } + else + { + InitSyncWrite(); - for(std::map::iterator _it = port_to_bulk_read.begin(); _it != port_to_bulk_read.end(); _it++) - _it->second->TxPacket(); + for(std::map::iterator _it = port_to_bulk_read.begin(); _it != port_to_bulk_read.end(); _it++) + _it->second->TxPacket(); - int error; - struct sched_param param; - pthread_attr_t attr; + int error; + struct sched_param param; + pthread_attr_t attr; - pthread_attr_init(&attr); + pthread_attr_init(&attr); - error = pthread_attr_setschedpolicy(&attr, SCHED_RR); - if(error != 0) - ROS_ERROR("pthread_attr_setschedpolicy error = %d\n",error); - error = pthread_attr_setinheritsched(&attr,PTHREAD_EXPLICIT_SCHED); - if(error != 0) - ROS_ERROR("pthread_attr_setinheritsched error = %d\n",error); + error = pthread_attr_setschedpolicy(&attr, SCHED_RR); + if(error != 0) + ROS_ERROR("pthread_attr_setschedpolicy error = %d\n",error); + error = pthread_attr_setinheritsched(&attr,PTHREAD_EXPLICIT_SCHED); + if(error != 0) + ROS_ERROR("pthread_attr_setinheritsched error = %d\n",error); - memset(¶m, 0, sizeof(param)); - param.sched_priority = 31;// RT - error = pthread_attr_setschedparam(&attr, ¶m); - if(error != 0) - ROS_ERROR("pthread_attr_setschedparam error = %d\n",error); + memset(¶m, 0, sizeof(param)); + param.sched_priority = 31;// RT + error = pthread_attr_setschedparam(&attr, ¶m); + if(error != 0) + ROS_ERROR("pthread_attr_setschedparam error = %d\n",error); - // create and start the thread - if((error = pthread_create(&this->timer_thread_, &attr, this->ThreadProc, this))!= 0) { - ROS_ERROR("timer thread create fail!!"); - exit(-1); - } + // create and start the thread + if((error = pthread_create(&this->timer_thread_, &attr, this->ThreadProc, this))!= 0) { + ROS_ERROR("timer thread create fail!!"); + exit(-1); + } + } - this->is_timer_running_ = true; + this->is_timer_running_ = true; } void RobotisController::StopTimer() { - int error = 0; + int error = 0; - // set the flag to stop the thread - if(this->is_timer_running_) - { - this->stop_timer_ = true; + // set the flag to stop the thread + if(this->is_timer_running_) + { + this->stop_timer_ = true; - // wait until the thread is stopped. - if((error = pthread_join(this->timer_thread_, NULL)) != 0) - exit(-1); + if(this->gazebo_mode == false) + { + // wait until the thread is stopped. + if((error = pthread_join(this->timer_thread_, NULL)) != 0) + exit(-1); - for(std::map::iterator _it = port_to_bulk_read.begin(); _it != port_to_bulk_read.end(); _it++) - _it->second->RxPacket(); + for(std::map::iterator _it = port_to_bulk_read.begin(); _it != port_to_bulk_read.end(); _it++) + _it->second->RxPacket(); - for(std::map::iterator _it = port_to_sync_write.begin(); _it != port_to_sync_write.end(); _it++) - _it->second->ClearParam(); + for(std::map::iterator _it = port_to_sync_write.begin(); _it != port_to_sync_write.end(); _it++) + _it->second->ClearParam(); + } + else + { + // wait until the thread is stopped. + gazebo_thread_.join(); + } - this->stop_timer_ = false; - this->is_timer_running_ = false; - } + this->stop_timer_ = false; + this->is_timer_running_ = false; + } } bool RobotisController::IsTimerRunning() { - return this->is_timer_running_; + return this->is_timer_running_; } void RobotisController::LoadOffset(const std::string path) { - YAML::Node _doc; - try{ - _doc = YAML::LoadFile(path.c_str()); - } catch(const std::exception& e) { - ROS_ERROR("Fail to load offset yaml."); - return; - } + YAML::Node _doc; + try{ + _doc = YAML::LoadFile(path.c_str()); + } catch(const std::exception& e) { + ROS_ERROR("Fail to load offset yaml."); + return; + } - YAML::Node _offset_node = _doc["offset"]; - if(_offset_node.size() == 0) - return; + YAML::Node _offset_node = _doc["offset"]; + if(_offset_node.size() == 0) + return; - ROS_INFO("Load offsets..."); - for(YAML::const_iterator _it = _offset_node.begin(); _it != _offset_node.end(); _it++) - { - std::string _joint_name = _it->first.as(); - double _offset = _it->second.as(); + ROS_INFO("Load offsets..."); + for(YAML::const_iterator _it = _offset_node.begin(); _it != _offset_node.end(); _it++) + { + std::string _joint_name = _it->first.as(); + double _offset = _it->second.as(); - std::map::iterator _dxl_it = robot->dxls.find(_joint_name); - if(_dxl_it != robot->dxls.end()) - _dxl_it->second->dxl_state->position_offset = _offset; - } + std::map::iterator _dxl_it = robot->dxls.find(_joint_name); + if(_dxl_it != robot->dxls.end()) + _dxl_it->second->dxl_state->position_offset = _offset; + } } void RobotisController::Process() { - static bool _is_process_running = false; + static bool _is_process_running = false; - if(_is_process_running == true) - return; - _is_process_running = true; + if(_is_process_running == true) + return; + _is_process_running = true; - // ROS_INFO("Controller::Process()"); + // ROS_INFO("Controller::Process()"); - sensor_msgs::JointState _present_state; - sensor_msgs::JointState _goal_state; + sensor_msgs::JointState _present_state; + sensor_msgs::JointState _goal_state; - ros::Time _now = ros::Time::now(); + ros::Time _now = ros::Time::now(); - // TODO: BulkRead Rx - bool _do_sync_write = false; + // TODO: BulkRead Rx + bool _do_sync_write = false; - for(std::map::iterator _it = port_to_bulk_read.begin(); _it != port_to_bulk_read.end(); _it++) - _it->second->RxPacket(); + if(gazebo_mode == false) + { + for(std::map::iterator _it = port_to_bulk_read.begin(); _it != port_to_bulk_read.end(); _it++) + _it->second->RxPacket(); + } ros::Duration _dur = ros::Time::now() - _now; double _msec = _dur.nsec * 0.000001; if(_msec > 8) std::cout << "Process duration : " << _msec << std::endl; - // -> save to Robot->dxls[]->dynamixel_state.present_position - // -> update time stamp to Robot->dxls[]->dynamixel_state.update_time_stamp - for(std::map::iterator dxl_it = robot->dxls.begin(); dxl_it != robot->dxls.end(); dxl_it++) - { - UINT32_T _pos = 0; + // -> save to Robot->dxls[]->dynamixel_state.present_position + // -> update time stamp to Robot->dxls[]->dynamixel_state.update_time_stamp + for(std::map::iterator dxl_it = robot->dxls.begin(); dxl_it != robot->dxls.end(); dxl_it++) + { + UINT32_T _pos = 0; - std::string _joint_name = dxl_it->first; - Dynamixel *_dxl = dxl_it->second; + std::string _joint_name = dxl_it->first; + Dynamixel *_dxl = dxl_it->second; - if(port_to_bulk_read[_dxl->port_name]->GetData(_dxl->id, PRESENT_POSITION_ADDR, &_pos) == true) - _dxl->dxl_state->present_position = _dxl->ConvertValue2Radian(_pos) - _dxl->dxl_state->position_offset; // remove offset + if(gazebo_mode == false) + { + if(port_to_bulk_read[_dxl->port_name]->GetData(_dxl->id, PRESENT_POSITION_ADDR, &_pos) == true) + _dxl->dxl_state->present_position = _dxl->ConvertValue2Radian(_pos) - _dxl->dxl_state->position_offset; // remove offset - UINT16_T _ext_data = 0; - if(_dxl->id == 11 || _dxl->id == 12 || _dxl->id == 23 || _dxl->id == 24) - { - if(port_to_bulk_read[_dxl->port_name]->GetData(_dxl->id, EXT_PORT_DATA1_ADDR, &_ext_data) == true) - _dxl->dxl_state->ext_port_data[0] = _ext_data; - if(port_to_bulk_read[_dxl->port_name]->GetData(_dxl->id, EXT_PORT_DATA2_ADDR, &_ext_data) == true) - _dxl->dxl_state->ext_port_data[1] = _ext_data; - } - else if(_dxl->id == 13 || _dxl->id == 14 || _dxl->id == 25 || _dxl->id == 26) - { - if(port_to_bulk_read[_dxl->port_name]->GetData(_dxl->id, EXT_PORT_DATA1_ADDR, &_ext_data) == true) - _dxl->dxl_state->ext_port_data[0] = _ext_data; - if(port_to_bulk_read[_dxl->port_name]->GetData(_dxl->id, EXT_PORT_DATA2_ADDR, &_ext_data) == true) - _dxl->dxl_state->ext_port_data[1] = _ext_data; - if(port_to_bulk_read[_dxl->port_name]->GetData(_dxl->id, EXT_PORT_DATA3_ADDR, &_ext_data) == true) - _dxl->dxl_state->ext_port_data[2] = _ext_data; - if(port_to_bulk_read[_dxl->port_name]->GetData(_dxl->id, EXT_PORT_DATA4_ADDR, &_ext_data) == true) - _dxl->dxl_state->ext_port_data[3] = _ext_data; - } + UINT16_T _ext_data = 0; + if(_dxl->id == 11 || _dxl->id == 12 || _dxl->id == 23 || _dxl->id == 24) + { + if(port_to_bulk_read[_dxl->port_name]->GetData(_dxl->id, EXT_PORT_DATA1_ADDR, &_ext_data) == true) + _dxl->dxl_state->ext_port_data[0] = _ext_data; + if(port_to_bulk_read[_dxl->port_name]->GetData(_dxl->id, EXT_PORT_DATA2_ADDR, &_ext_data) == true) + _dxl->dxl_state->ext_port_data[1] = _ext_data; + } + else if(_dxl->id == 13 || _dxl->id == 14 || _dxl->id == 25 || _dxl->id == 26) + { + if(port_to_bulk_read[_dxl->port_name]->GetData(_dxl->id, EXT_PORT_DATA1_ADDR, &_ext_data) == true) + _dxl->dxl_state->ext_port_data[0] = _ext_data; + if(port_to_bulk_read[_dxl->port_name]->GetData(_dxl->id, EXT_PORT_DATA2_ADDR, &_ext_data) == true) + _dxl->dxl_state->ext_port_data[1] = _ext_data; + if(port_to_bulk_read[_dxl->port_name]->GetData(_dxl->id, EXT_PORT_DATA3_ADDR, &_ext_data) == true) + _dxl->dxl_state->ext_port_data[2] = _ext_data; + if(port_to_bulk_read[_dxl->port_name]->GetData(_dxl->id, EXT_PORT_DATA4_ADDR, &_ext_data) == true) + _dxl->dxl_state->ext_port_data[3] = _ext_data; + } + } - _present_state.name.push_back(_joint_name); - // TODO: should be converted to rad, rad/s, Nm - _present_state.position.push_back(_dxl->dxl_state->present_position); - _present_state.velocity.push_back(_dxl->dxl_state->present_velocity); - _present_state.effort.push_back(_dxl->dxl_state->present_load); + _present_state.name.push_back(_joint_name); + // TODO: should be converted to rad, rad/s, Nm + _present_state.position.push_back(_dxl->dxl_state->present_position); + _present_state.velocity.push_back(_dxl->dxl_state->present_velocity); + _present_state.effort.push_back(_dxl->dxl_state->present_load); _goal_state.name.push_back(_joint_name); _goal_state.position.push_back(_dxl->dxl_state->goal_position); _goal_state.velocity.push_back(_dxl->dxl_state->goal_velocity); _goal_state.effort.push_back(_dxl->dxl_state->goal_torque); - } - // -> publish present joint_states topic - _present_state.header.stamp = ros::Time::now(); - present_joint_state_pub.publish(_present_state); + } + // -> publish present joint_states topic + _present_state.header.stamp = ros::Time::now(); + present_joint_state_pub.publish(_present_state); // -> publish goal joint_states topic _goal_state.header.stamp = _present_state.header.stamp; goal_joint_state_pub.publish(_goal_state); - if(controller_mode_ == MOTION_MODULE_MODE) - { - // TODO: Call MotionModule Process() - // -> for loop : call MotionModule list -> Process() - if(modules_.size() > 0) - { - queue_mutex_.lock(); + if(controller_mode_ == MOTION_MODULE_MODE) + { + // TODO: Call MotionModule Process() + // -> for loop : call MotionModule list -> Process() + if(modules_.size() > 0) + { + queue_mutex_.lock(); - for(std::list::iterator module_it = modules_.begin(); module_it != modules_.end(); module_it++) - { - // ros::Time _before = ros::Time::now(); + for(std::list::iterator module_it = modules_.begin(); module_it != modules_.end(); module_it++) + { + // ros::Time _before = ros::Time::now(); - (*module_it)->Process(robot->dxls); + (*module_it)->Process(robot->dxls); - // ros::Duration _dur = ros::Time::now() - _before; - // double _msec = _dur.nsec * 0.000001; + // ros::Duration _dur = ros::Time::now() - _before; + // double _msec = _dur.nsec * 0.000001; - // std::cout << "Process duration ["<< (*module_it)->module_name << "] : " << _msec << std::endl; + // std::cout << "Process duration ["<< (*module_it)->module_name << "] : " << _msec << std::endl; - // for loop : joint list - for(std::map::iterator dxl_it = robot->dxls.begin(); dxl_it != robot->dxls.end(); dxl_it++) - { - std::string _joint_name = dxl_it->first; - Dynamixel *_dxl = dxl_it->second; + // for loop : joint list + for(std::map::iterator dxl_it = robot->dxls.begin(); dxl_it != robot->dxls.end(); dxl_it++) + { + std::string _joint_name = dxl_it->first; + Dynamixel *_dxl = dxl_it->second; - DynamixelState *_dxl_state = _dxl->dxl_state; - if(_dxl->ctrl_module_name == (*module_it)->module_name) - { - _do_sync_write = true; - // ROS_INFO("Set goal value"); - DynamixelState *_result_state = (*module_it)->result[_joint_name]; + DynamixelState *_dxl_state = _dxl->dxl_state; + if(_dxl->ctrl_module_name == (*module_it)->module_name) + { + _do_sync_write = true; + // ROS_INFO("Set goal value"); + DynamixelState *_result_state = (*module_it)->result[_joint_name]; - if(_result_state == NULL) { - ROS_INFO("[%s] %s", (*module_it)->module_name.c_str(), _joint_name.c_str()); - continue; - } + if(_result_state == NULL) { + ROS_INFO("[%s] %s", (*module_it)->module_name.c_str(), _joint_name.c_str()); + continue; + } - // TODO: check update time stamp ? + // TODO: check update time stamp ? - if((*module_it)->control_mode == POSITION_CONTROL) - { -// if(_result_state->goal_position == 0 && _dxl->id == 3) -// ROS_INFO("[MODULE:%s][ID:3] goal position = %f", (*module_it)->module_name.c_str(), _dxl_state->goal_position); - _dxl_state->goal_position = _result_state->goal_position; + if((*module_it)->control_mode == POSITION_CONTROL) + { +// if(_result_state->goal_position == 0 && _dxl->id == 3) +// ROS_INFO("[MODULE:%s][ID:3] goal position = %f", (*module_it)->module_name.c_str(), _dxl_state->goal_position); + _dxl_state->goal_position = _result_state->goal_position; - // add offset - UINT32_T _pos_data = _dxl->ConvertRadian2Value(_dxl_state->goal_position + _dxl_state->position_offset); - UINT8_T _sync_write_data[4]; - _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_pos_data)); - _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_pos_data)); - _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_pos_data)); - _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_pos_data)); + if(gazebo_mode == false) + { + // add offset + UINT32_T _pos_data = _dxl->ConvertRadian2Value(_dxl_state->goal_position + _dxl_state->position_offset); + UINT8_T _sync_write_data[4]; + _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_pos_data)); + _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_pos_data)); + _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_pos_data)); + _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_pos_data)); - if(abs(_pos_data) > 151800) - printf("goal_pos : %f | position_offset : %f | pos_data : %d\n", _dxl_state->goal_position , _dxl_state->position_offset, _pos_data); + if(abs(_pos_data) > 151800) + printf("goal_pos : %f | position_offset : %f | pos_data : %d\n", _dxl_state->goal_position , _dxl_state->position_offset, _pos_data); - port_to_sync_write[_dxl->port_name]->ChangeParam(_dxl->id, _sync_write_data); - } - else if((*module_it)->control_mode == VELOCITY_CONTROL) - { - _dxl_state->goal_velocity = _result_state->goal_velocity; - } - else if((*module_it)->control_mode == TORQUE_CONTROL) - { - _dxl_state->goal_torque = _result_state->goal_torque; - } - } - } - } + port_to_sync_write[_dxl->port_name]->ChangeParam(_dxl->id, _sync_write_data); + } + } + else if((*module_it)->control_mode == VELOCITY_CONTROL) + { + _dxl_state->goal_velocity = _result_state->goal_velocity; + } + else if((*module_it)->control_mode == TORQUE_CONTROL) + { + _dxl_state->goal_torque = _result_state->goal_torque; + } + } + } + } - // std::cout << " ------------------------------------------- " << std::endl; - queue_mutex_.unlock(); - } + // std::cout << " ------------------------------------------- " << std::endl; + queue_mutex_.unlock(); + } - // TODO: Combine the result && SyncWrite - // -> SyncWrite - if(_do_sync_write) { - for(std::map::iterator _it = port_to_sync_write.begin(); _it != port_to_sync_write.end(); _it++) - _it->second->TxPacket(); - } - } - else if(controller_mode_ == DIRECT_CONTROL_MODE) - { - queue_mutex_.lock(); + // TODO: Combine the result && SyncWrite + // -> SyncWrite + if(gazebo_mode == false && _do_sync_write) + { + for(std::map::iterator _it = port_to_sync_write.begin(); _it != port_to_sync_write.end(); _it++) + _it->second->TxPacket(); + } + else if(gazebo_mode == true) + { + std_msgs::Float64 _joint_msg; - for(std::map::iterator _it = port_to_sync_write.begin(); _it != port_to_sync_write.end(); _it++) - { - _it->second->TxPacket(); - _it->second->ClearParam(); - } + for(std::map::iterator dxl_it = robot->dxls.begin(); dxl_it != robot->dxls.end(); dxl_it++) + { + _joint_msg.data = dxl_it->second->dxl_state->goal_position; + gazebo_joint_pub[dxl_it->first].publish(_joint_msg); + } + } + } + else if(controller_mode_ == DIRECT_CONTROL_MODE) + { + queue_mutex_.lock(); - if(direct_sync_write_.size() > 0) - { - for(int _i = 0; _i < direct_sync_write_.size(); _i++) - { - direct_sync_write_[_i]->TxPacket(); - direct_sync_write_[_i]->ClearParam(); - } - direct_sync_write_.clear(); - } + for(std::map::iterator _it = port_to_sync_write.begin(); _it != port_to_sync_write.end(); _it++) + { + _it->second->TxPacket(); + _it->second->ClearParam(); + } - queue_mutex_.unlock(); - } + if(direct_sync_write_.size() > 0) + { + for(int _i = 0; _i < direct_sync_write_.size(); _i++) + { + direct_sync_write_[_i]->TxPacket(); + direct_sync_write_[_i]->ClearParam(); + } + direct_sync_write_.clear(); + } - // TODO: User Read/Write + queue_mutex_.unlock(); + } - // TODO: BulkRead Tx - for(std::map::iterator _it = port_to_bulk_read.begin(); _it != port_to_bulk_read.end(); _it++) - _it->second->TxPacket(); + // TODO: User Read/Write - // for test : goal to present - // for(std::map::iterator dxl_it = robot_->dxls.begin(); dxl_it != robot_->dxls.end(); dxl_it++) - // { - // dxl_it->second->dxl_state->present_position = dxl_it->second->dxl_state->goal_position; - // dxl_it->second->dxl_state->present_velocity = dxl_it->second->dxl_state->goal_velocity; - // } + // TODO: BulkRead Tx + if(gazebo_mode == false) + { + for(std::map::iterator _it = port_to_bulk_read.begin(); _it != port_to_bulk_read.end(); _it++) + _it->second->TxPacket(); + } + + // for test : goal to present + // for(std::map::iterator dxl_it = robot_->dxls.begin(); dxl_it != robot_->dxls.end(); dxl_it++) + // { + // dxl_it->second->dxl_state->present_position = dxl_it->second->dxl_state->goal_position; + // dxl_it->second->dxl_state->present_velocity = dxl_it->second->dxl_state->goal_velocity; + // } -// ros::Duration _dur = ros::Time::now() - _now; -// double _msec = _dur.nsec * 0.000001; +// ros::Duration _dur = ros::Time::now() - _now; +// double _msec = _dur.nsec * 0.000001; // -// if(_msec > 8) std::cout << "Process duration : " << _msec << std::endl; +// if(_msec > 8) std::cout << "Process duration : " << _msec << std::endl; - _is_process_running = false; + _is_process_running = false; } void RobotisController::AddModule(MotionModule *module) { - module->Initialize(CONTROL_CYCLE_MSEC); - modules_.push_back(module); - modules_.unique(); + module->Initialize(CONTROL_CYCLE_MSEC); + modules_.push_back(module); + modules_.unique(); } void RobotisController::RemoveModule(MotionModule *module) { - modules_.remove(module); + modules_.remove(module); } void RobotisController::SyncWriteItemCallback(const robotis_controller_msgs::SyncWriteItem::ConstPtr &msg) { - for(int _i = 0; _i < msg->joint_name.size(); _i++) - { - Dynamixel *_dxl = robot->dxls[msg->joint_name[_i]]; - ControlTableItem *_item = _dxl->ctrl_table[msg->item_name]; + for(int _i = 0; _i < msg->joint_name.size(); _i++) + { + Dynamixel *_dxl = robot->dxls[msg->joint_name[_i]]; + ControlTableItem *_item = _dxl->ctrl_table[msg->item_name]; - PortHandler *_port = robot->ports[_dxl->port_name]; - PacketHandler *_packet_handler= PacketHandler::GetPacketHandler(_dxl->protocol_version); + PortHandler *_port = robot->ports[_dxl->port_name]; + PacketHandler *_packet_handler= PacketHandler::GetPacketHandler(_dxl->protocol_version); - if(_item->access_type == READ) - continue; + if(_item->access_type == READ) + continue; - int _idx = 0; - if(direct_sync_write_.size() == 0) - { - direct_sync_write_.push_back(new GroupSyncWrite(_port, _packet_handler, _item->address, _item->data_length)); - _idx = 0; - } - else - { - for(_idx = 0; _idx < direct_sync_write_.size(); _idx++) - { - if(direct_sync_write_[_idx]->GetPortHandler() == _port && - direct_sync_write_[_idx]->GetPacketHandler() == _packet_handler) - break; - } + int _idx = 0; + if(direct_sync_write_.size() == 0) + { + direct_sync_write_.push_back(new GroupSyncWrite(_port, _packet_handler, _item->address, _item->data_length)); + _idx = 0; + } + else + { + for(_idx = 0; _idx < direct_sync_write_.size(); _idx++) + { + if(direct_sync_write_[_idx]->GetPortHandler() == _port && + direct_sync_write_[_idx]->GetPacketHandler() == _packet_handler) + break; + } - if(_idx == direct_sync_write_.size()) - direct_sync_write_.push_back(new GroupSyncWrite(_port, _packet_handler, _item->address, _item->data_length)); - } + if(_idx == direct_sync_write_.size()) + direct_sync_write_.push_back(new GroupSyncWrite(_port, _packet_handler, _item->address, _item->data_length)); + } - UINT8_T *_data = new UINT8_T[_item->data_length]; - if(_item->data_length == 1) - _data[0] = (UINT8_T)msg->value[_i]; - else if(_item->data_length == 2) - { - _data[0] = DXL_LOBYTE((UINT16_T)msg->value[_i]); - _data[1] = DXL_HIBYTE((UINT16_T)msg->value[_i]); - } - else if(_item->data_length == 4) - { - _data[0] = DXL_LOBYTE(DXL_LOWORD((UINT32_T)msg->value[_i])); - _data[1] = DXL_HIBYTE(DXL_LOWORD((UINT32_T)msg->value[_i])); - _data[2] = DXL_LOBYTE(DXL_HIWORD((UINT32_T)msg->value[_i])); - _data[3] = DXL_HIBYTE(DXL_HIWORD((UINT32_T)msg->value[_i])); - } - direct_sync_write_[_idx]->AddParam(_dxl->id, _data); - delete[] _data; - } + UINT8_T *_data = new UINT8_T[_item->data_length]; + if(_item->data_length == 1) + _data[0] = (UINT8_T)msg->value[_i]; + else if(_item->data_length == 2) + { + _data[0] = DXL_LOBYTE((UINT16_T)msg->value[_i]); + _data[1] = DXL_HIBYTE((UINT16_T)msg->value[_i]); + } + else if(_item->data_length == 4) + { + _data[0] = DXL_LOBYTE(DXL_LOWORD((UINT32_T)msg->value[_i])); + _data[1] = DXL_HIBYTE(DXL_LOWORD((UINT32_T)msg->value[_i])); + _data[2] = DXL_LOBYTE(DXL_HIWORD((UINT32_T)msg->value[_i])); + _data[3] = DXL_HIBYTE(DXL_HIWORD((UINT32_T)msg->value[_i])); + } + direct_sync_write_[_idx]->AddParam(_dxl->id, _data); + delete[] _data; + } } void RobotisController::SetControllerModeCallback(const std_msgs::String::ConstPtr &msg) { - if(msg->data == "DIRECT_CONTROL_MODE") - controller_mode_ = DIRECT_CONTROL_MODE; - else if(msg->data == "MOTION_MODULE_MODE") - controller_mode_ = MOTION_MODULE_MODE; + if(msg->data == "DIRECT_CONTROL_MODE") + controller_mode_ = DIRECT_CONTROL_MODE; + else if(msg->data == "MOTION_MODULE_MODE") + controller_mode_ = MOTION_MODULE_MODE; } void RobotisController::SetJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg) { - if(controller_mode_ != DIRECT_CONTROL_MODE) - return; + if(controller_mode_ != DIRECT_CONTROL_MODE) + return; - queue_mutex_.lock(); + queue_mutex_.lock(); - for(int _i = 0; _i < msg->name.size(); _i++) - { - INT32_T _pos = 0; + for(int _i = 0; _i < msg->name.size(); _i++) + { + INT32_T _pos = 0; - Dynamixel *_dxl = robot->dxls[msg->name[_i]]; - if(_dxl == NULL) - continue; + Dynamixel *_dxl = robot->dxls[msg->name[_i]]; + if(_dxl == NULL) + continue; - _dxl->dxl_state->goal_position = msg->position[_i]; - _pos = _dxl->ConvertRadian2Value((double)msg->position[_i]); + _dxl->dxl_state->goal_position = msg->position[_i]; + _pos = _dxl->ConvertRadian2Value((double)msg->position[_i]); - UINT8_T _sync_write_data[4]; - _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_pos)); - _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_pos)); - _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_pos)); - _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_pos)); + UINT8_T _sync_write_data[4]; + _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_pos)); + _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_pos)); + _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_pos)); + _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_pos)); - port_to_sync_write[_dxl->port_name]->AddParam(_dxl->id, _sync_write_data); - } + port_to_sync_write[_dxl->port_name]->AddParam(_dxl->id, _sync_write_data); + } - queue_mutex_.unlock(); + queue_mutex_.unlock(); } void RobotisController::SetCtrlModuleCallback(const robotis_controller_msgs::JointCtrlModule::ConstPtr &msg) { - if(msg->joint_name.size() != msg->module_name.size()) - return; + if(msg->joint_name.size() != msg->module_name.size()) + return; - queue_mutex_.lock(); + queue_mutex_.lock(); - for(unsigned int idx = 0; idx < msg->joint_name.size(); idx++) - { - Dynamixel *_dxl = NULL; + for(unsigned int idx = 0; idx < msg->joint_name.size(); idx++) + { + Dynamixel *_dxl = NULL; std::map::iterator _dxl_it = robot->dxls.find((std::string)(msg->joint_name[idx])); if(_dxl_it != robot->dxls.end()) _dxl = _dxl_it->second; else continue; - // none - if(msg->module_name[idx] == "" || msg->module_name[idx] == "none") - { - _dxl->ctrl_module_name = msg->module_name[idx]; - continue; - } + // none + if(msg->module_name[idx] == "" || msg->module_name[idx] == "none") + { + _dxl->ctrl_module_name = msg->module_name[idx]; + continue; + } - // check whether the module is using this joint - for(std::list::iterator _m_it = modules_.begin(); _m_it != modules_.end(); _m_it++) - { - if((*_m_it)->module_name == msg->module_name[idx]) - { - if((*_m_it)->result.find(msg->joint_name[idx]) != (*_m_it)->result.end()) - { - _dxl->ctrl_module_name = msg->module_name[idx]; - break; - } - } - } - } + // check whether the module is using this joint + for(std::list::iterator _m_it = modules_.begin(); _m_it != modules_.end(); _m_it++) + { + if((*_m_it)->module_name == msg->module_name[idx]) + { + if((*_m_it)->result.find(msg->joint_name[idx]) != (*_m_it)->result.end()) + { + _dxl->ctrl_module_name = msg->module_name[idx]; + break; + } + } + } + } - for(std::list::iterator _m_it = modules_.begin(); _m_it != modules_.end(); _m_it++) - { - // set all modules -> disable - (*_m_it)->enable = false; + for(std::list::iterator _m_it = modules_.begin(); _m_it != modules_.end(); _m_it++) + { + // set all modules -> disable + (*_m_it)->enable = false; - // set all used modules -> enable - for(std::map::iterator _d_it = robot->dxls.begin(); _d_it != robot->dxls.end(); _d_it++) - { - if(_d_it->second->ctrl_module_name == (*_m_it)->module_name) - { - (*_m_it)->enable = true; - break; - } - } - } + // set all used modules -> enable + for(std::map::iterator _d_it = robot->dxls.begin(); _d_it != robot->dxls.end(); _d_it++) + { + if(_d_it->second->ctrl_module_name == (*_m_it)->module_name) + { + (*_m_it)->enable = true; + break; + } + } + } - // TODO: set indirect address - // -> check module's control_mode + // TODO: set indirect address + // -> check module's control_mode - queue_mutex_.unlock(); + queue_mutex_.unlock(); - robotis_controller_msgs::JointCtrlModule _current_module_msg; - for(std::map::iterator _dxl_iter = robot->dxls.begin(); _dxl_iter != robot->dxls.end(); ++_dxl_iter) - { - _current_module_msg.joint_name.push_back(_dxl_iter->first); - _current_module_msg.module_name.push_back(_dxl_iter->second->ctrl_module_name); - } + robotis_controller_msgs::JointCtrlModule _current_module_msg; + for(std::map::iterator _dxl_iter = robot->dxls.begin(); _dxl_iter != robot->dxls.end(); ++_dxl_iter) + { + _current_module_msg.joint_name.push_back(_dxl_iter->first); + _current_module_msg.module_name.push_back(_dxl_iter->second->ctrl_module_name); + } - if(_current_module_msg.joint_name.size() == _current_module_msg.module_name.size()) - current_module_pub.publish(_current_module_msg); + if(_current_module_msg.joint_name.size() == _current_module_msg.module_name.size()) + current_module_pub.publish(_current_module_msg); } bool RobotisController::GetCtrlModuleCallback(robotis_controller_msgs::GetJointModule::Request &req, robotis_controller_msgs::GetJointModule::Response &res) { - for(unsigned int idx = 0; idx < req.joint_name.size(); idx++) - { - std::map::iterator _d_it = robot->dxls.find((std::string)(req.joint_name[idx])); - if(_d_it != robot->dxls.end()) - { - res.joint_name.push_back(req.joint_name[idx]); - res.module_name.push_back(_d_it->second->ctrl_module_name); - } - } + for(unsigned int idx = 0; idx < req.joint_name.size(); idx++) + { + std::map::iterator _d_it = robot->dxls.find((std::string)(req.joint_name[idx])); + if(_d_it != robot->dxls.end()) + { + res.joint_name.push_back(req.joint_name[idx]); + res.module_name.push_back(_d_it->second->ctrl_module_name); + } + } - if(res.joint_name.size() == 0) return false; + if(res.joint_name.size() == 0) return false; - return true; + return true; +} + +void RobotisController::GazeboJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg) +{ + for(unsigned int _i = 0; _i < msg->name.size(); _i++) + { + std::map::iterator _d_it = robot->dxls.find((std::string)msg->name[_i]); + if(_d_it != robot->dxls.end()) + { + _d_it->second->dxl_state->present_position = msg->position[_i]; + _d_it->second->dxl_state->present_velocity = msg->velocity[_i]; + _d_it->second->dxl_state->present_load = msg->effort[_i]; + } + } + + if(init_pose_loaded_ == false) + { + for(std::map::iterator _it = robot->dxls.begin(); _it != robot->dxls.end(); _it++) + _it->second->dxl_state->goal_position = _it->second->dxl_state->present_position; + init_pose_loaded_ = true; + } } bool RobotisController::CheckTimerStop() { - if(this->is_timer_running_) - { - if(DEBUG_PRINT == true) - ROS_WARN("Process Timer is running.. STOP the timer first."); - return false; - } - return true; + if(this->is_timer_running_) + { + if(DEBUG_PRINT == true) + ROS_WARN("Process Timer is running.. STOP the timer first."); + return false; + } + return true; } int RobotisController::Ping(const std::string joint_name, UINT8_T *error) { - if(CheckTimerStop() == false) - return COMM_PORT_BUSY; + if(CheckTimerStop() == false) + return COMM_PORT_BUSY; - Dynamixel *_dxl = robot->dxls[joint_name]; + Dynamixel *_dxl = robot->dxls[joint_name]; if(_dxl == NULL) return COMM_NOT_AVAILABLE; - PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); - PortHandler *_port_handler = robot->ports[_dxl->port_name]; + PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); + PortHandler *_port_handler = robot->ports[_dxl->port_name]; - return _pkt_handler->Ping(_port_handler, _dxl->id, error); + return _pkt_handler->Ping(_port_handler, _dxl->id, error); } int RobotisController::Ping(const std::string joint_name, UINT16_T* model_number, UINT8_T *error) { - if(CheckTimerStop() == false) - return COMM_PORT_BUSY; + if(CheckTimerStop() == false) + return COMM_PORT_BUSY; - Dynamixel *_dxl = robot->dxls[joint_name]; + Dynamixel *_dxl = robot->dxls[joint_name]; if(_dxl == NULL) return COMM_NOT_AVAILABLE; - PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); - PortHandler *_port_handler = robot->ports[_dxl->port_name]; + PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); + PortHandler *_port_handler = robot->ports[_dxl->port_name]; - return _pkt_handler->Ping(_port_handler, _dxl->id, model_number, error); + return _pkt_handler->Ping(_port_handler, _dxl->id, model_number, error); } int RobotisController::Action(const std::string joint_name) { - if(CheckTimerStop() == false) - return COMM_PORT_BUSY; + if(CheckTimerStop() == false) + return COMM_PORT_BUSY; - Dynamixel *_dxl = robot->dxls[joint_name]; + Dynamixel *_dxl = robot->dxls[joint_name]; if(_dxl == NULL) return COMM_NOT_AVAILABLE; - PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); - PortHandler *_port_handler = robot->ports[_dxl->port_name]; + PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); + PortHandler *_port_handler = robot->ports[_dxl->port_name]; - return _pkt_handler->Action(_port_handler, _dxl->id); + return _pkt_handler->Action(_port_handler, _dxl->id); } int RobotisController::Reboot(const std::string joint_name, UINT8_T *error) { - if(CheckTimerStop() == false) - return COMM_PORT_BUSY; + if(CheckTimerStop() == false) + return COMM_PORT_BUSY; - Dynamixel *_dxl = robot->dxls[joint_name]; + Dynamixel *_dxl = robot->dxls[joint_name]; if(_dxl == NULL) return COMM_NOT_AVAILABLE; - PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); - PortHandler *_port_handler = robot->ports[_dxl->port_name]; + PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); + PortHandler *_port_handler = robot->ports[_dxl->port_name]; - return _pkt_handler->Reboot(_port_handler, _dxl->id, error); + return _pkt_handler->Reboot(_port_handler, _dxl->id, error); } int RobotisController::FactoryReset(const std::string joint_name, UINT8_T option, UINT8_T *error) { - if(CheckTimerStop() == false) - return COMM_PORT_BUSY; + if(CheckTimerStop() == false) + return COMM_PORT_BUSY; - Dynamixel *_dxl = robot->dxls[joint_name]; + Dynamixel *_dxl = robot->dxls[joint_name]; if(_dxl == NULL) return COMM_NOT_AVAILABLE; - PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); - PortHandler *_port_handler = robot->ports[_dxl->port_name]; + PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); + PortHandler *_port_handler = robot->ports[_dxl->port_name]; - return _pkt_handler->FactoryReset(_port_handler, _dxl->id, option, error); + return _pkt_handler->FactoryReset(_port_handler, _dxl->id, option, error); } int RobotisController::Read(const std::string joint_name, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error) { - if(CheckTimerStop() == false) - return COMM_PORT_BUSY; + if(CheckTimerStop() == false) + return COMM_PORT_BUSY; - Dynamixel *_dxl = robot->dxls[joint_name]; + Dynamixel *_dxl = robot->dxls[joint_name]; if(_dxl == NULL) return COMM_NOT_AVAILABLE; - PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); - PortHandler *_port_handler = robot->ports[_dxl->port_name]; + PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); + PortHandler *_port_handler = robot->ports[_dxl->port_name]; - return _pkt_handler->ReadTxRx(_port_handler, _dxl->id, address, length, data, error); + return _pkt_handler->ReadTxRx(_port_handler, _dxl->id, address, length, data, error); } int RobotisController::ReadCtrlItem(const std::string joint_name, const std::string item_name, UINT32_T *data, UINT8_T *error) @@ -962,62 +1058,62 @@ int RobotisController::ReadCtrlItem(const std::string joint_name, const std::str int RobotisController::Read1Byte(const std::string joint_name, UINT16_T address, UINT8_T *data, UINT8_T *error) { - if(CheckTimerStop() == false) - return COMM_PORT_BUSY; + if(CheckTimerStop() == false) + return COMM_PORT_BUSY; - Dynamixel *_dxl = robot->dxls[joint_name]; + Dynamixel *_dxl = robot->dxls[joint_name]; if(_dxl == NULL) return COMM_NOT_AVAILABLE; - PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); - PortHandler *_port_handler = robot->ports[_dxl->port_name]; + PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); + PortHandler *_port_handler = robot->ports[_dxl->port_name]; - return _pkt_handler->Read1ByteTxRx(_port_handler, _dxl->id, address, data, error); + return _pkt_handler->Read1ByteTxRx(_port_handler, _dxl->id, address, data, error); } int RobotisController::Read2Byte(const std::string joint_name, UINT16_T address, UINT16_T *data, UINT8_T *error) { - if(CheckTimerStop() == false) - return COMM_PORT_BUSY; + if(CheckTimerStop() == false) + return COMM_PORT_BUSY; - Dynamixel *_dxl = robot->dxls[joint_name]; + Dynamixel *_dxl = robot->dxls[joint_name]; if(_dxl == NULL) return COMM_NOT_AVAILABLE; - PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); - PortHandler *_port_handler = robot->ports[_dxl->port_name]; + PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); + PortHandler *_port_handler = robot->ports[_dxl->port_name]; - return _pkt_handler->Read2ByteTxRx(_port_handler, _dxl->id, address, data, error); + return _pkt_handler->Read2ByteTxRx(_port_handler, _dxl->id, address, data, error); } int RobotisController::Read4Byte(const std::string joint_name, UINT16_T address, UINT32_T *data, UINT8_T *error) { - if(CheckTimerStop() == false) - return COMM_PORT_BUSY; + if(CheckTimerStop() == false) + return COMM_PORT_BUSY; - Dynamixel *_dxl = robot->dxls[joint_name]; + Dynamixel *_dxl = robot->dxls[joint_name]; if(_dxl == NULL) return COMM_NOT_AVAILABLE; - PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); - PortHandler *_port_handler = robot->ports[_dxl->port_name]; + PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); + PortHandler *_port_handler = robot->ports[_dxl->port_name]; - return _pkt_handler->Read4ByteTxRx(_port_handler, _dxl->id, address, data, error); + return _pkt_handler->Read4ByteTxRx(_port_handler, _dxl->id, address, data, error); } int RobotisController::Write(const std::string joint_name, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error) { - if(CheckTimerStop() == false) - return COMM_PORT_BUSY; + if(CheckTimerStop() == false) + return COMM_PORT_BUSY; - Dynamixel *_dxl = robot->dxls[joint_name]; + Dynamixel *_dxl = robot->dxls[joint_name]; if(_dxl == NULL) return COMM_NOT_AVAILABLE; - PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); - PortHandler *_port_handler = robot->ports[_dxl->port_name]; + PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); + PortHandler *_port_handler = robot->ports[_dxl->port_name]; - return _pkt_handler->WriteTxRx(_port_handler, _dxl->id, address, length, data, error); + return _pkt_handler->WriteTxRx(_port_handler, _dxl->id, address, length, data, error); } int RobotisController::WriteCtrlItem(const std::string joint_name, const std::string item_name, UINT32_T data, UINT8_T *error) @@ -1063,61 +1159,61 @@ int RobotisController::WriteCtrlItem(const std::string joint_name, const std::st int RobotisController::Write1Byte(const std::string joint_name, UINT16_T address, UINT8_T data, UINT8_T *error) { - if(CheckTimerStop() == false) - return COMM_PORT_BUSY; + if(CheckTimerStop() == false) + return COMM_PORT_BUSY; - Dynamixel *_dxl = robot->dxls[joint_name]; + Dynamixel *_dxl = robot->dxls[joint_name]; if(_dxl == NULL) return COMM_NOT_AVAILABLE; - PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); - PortHandler *_port_handler = robot->ports[_dxl->port_name]; + PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); + PortHandler *_port_handler = robot->ports[_dxl->port_name]; - return _pkt_handler->Write1ByteTxRx(_port_handler, _dxl->id, address, data, error); + return _pkt_handler->Write1ByteTxRx(_port_handler, _dxl->id, address, data, error); } int RobotisController::Write2Byte(const std::string joint_name, UINT16_T address, UINT16_T data, UINT8_T *error) { - if(CheckTimerStop() == false) - return COMM_PORT_BUSY; + if(CheckTimerStop() == false) + return COMM_PORT_BUSY; - Dynamixel *_dxl = robot->dxls[joint_name]; + Dynamixel *_dxl = robot->dxls[joint_name]; if(_dxl == NULL) return COMM_NOT_AVAILABLE; - PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); - PortHandler *_port_handler = robot->ports[_dxl->port_name]; + PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); + PortHandler *_port_handler = robot->ports[_dxl->port_name]; - return _pkt_handler->Write2ByteTxRx(_port_handler, _dxl->id, address, data, error); + return _pkt_handler->Write2ByteTxRx(_port_handler, _dxl->id, address, data, error); } int RobotisController::Write4Byte(const std::string joint_name, UINT16_T address, UINT32_T data, UINT8_T *error) { - if(CheckTimerStop() == false) - return COMM_PORT_BUSY; + if(CheckTimerStop() == false) + return COMM_PORT_BUSY; - Dynamixel *_dxl = robot->dxls[joint_name]; + Dynamixel *_dxl = robot->dxls[joint_name]; if(_dxl == NULL) return COMM_NOT_AVAILABLE; - PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); - PortHandler *_port_handler = robot->ports[_dxl->port_name]; + PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); + PortHandler *_port_handler = robot->ports[_dxl->port_name]; - return _pkt_handler->Write4ByteTxRx(_port_handler, _dxl->id, address, data, error); + return _pkt_handler->Write4ByteTxRx(_port_handler, _dxl->id, address, data, error); } int RobotisController::RegWrite(const std::string joint_name, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error) { - if(CheckTimerStop() == false) - return COMM_PORT_BUSY; + if(CheckTimerStop() == false) + return COMM_PORT_BUSY; - Dynamixel *_dxl = robot->dxls[joint_name]; + Dynamixel *_dxl = robot->dxls[joint_name]; if(_dxl == NULL) return COMM_NOT_AVAILABLE; - PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); - PortHandler *_port_handler = robot->ports[_dxl->port_name]; + PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); + PortHandler *_port_handler = robot->ports[_dxl->port_name]; - return _pkt_handler->RegWriteTxRx(_port_handler, _dxl->id, address, length, data, error); + return _pkt_handler->RegWriteTxRx(_port_handler, _dxl->id, address, length, data, error); } diff --git a/robotis_device/src/robotis_device/Robot.cpp b/robotis_device/src/robotis_device/Robot.cpp index 43d2cb5..a6504fb 100644 --- a/robotis_device/src/robotis_device/Robot.cpp +++ b/robotis_device/src/robotis_device/Robot.cpp @@ -77,9 +77,7 @@ Robot::Robot(std::string robot_file_path, std::string dev_desc_dir_path) std::cout << tokens[0] << " added. (baudrate: " << tokens[1] << ")" << std::endl; ports[tokens[0]] = (PortHandler*)PortHandler::GetPortHandler(tokens[0].c_str()); - bool _port_result = ports[tokens[0]]->SetBaudRate(std::atoi(tokens[1].c_str())); - if(_port_result == false) - exit(-1); + ports[tokens[0]]->SetBaudRate(std::atoi(tokens[1].c_str())); } else if(session == "device info") { From c14f0b2c3459235c920542d8929a67a968e4caf3 Mon Sep 17 00:00:00 2001 From: ROBOTIS Date: Fri, 11 Mar 2016 14:42:13 +0900 Subject: [PATCH 009/238] - if the last bulk_read / sync_read result is failure -> GetData return false --- dynamixel_sdk/include/dynamixel_sdk/GroupBulkRead.h | 8 +++++--- dynamixel_sdk/include/dynamixel_sdk/GroupSyncRead.h | 2 ++ dynamixel_sdk/src/dynamixel_sdk/GroupBulkRead.cpp | 12 +++++++++--- dynamixel_sdk/src/dynamixel_sdk/GroupSyncRead.cpp | 12 +++++++++--- 4 files changed, 25 insertions(+), 9 deletions(-) diff --git a/dynamixel_sdk/include/dynamixel_sdk/GroupBulkRead.h b/dynamixel_sdk/include/dynamixel_sdk/GroupBulkRead.h index e98b19f..45706c9 100644 --- a/dynamixel_sdk/include/dynamixel_sdk/GroupBulkRead.h +++ b/dynamixel_sdk/include/dynamixel_sdk/GroupBulkRead.h @@ -21,15 +21,17 @@ namespace ROBOTIS class GroupBulkRead { private: - PortHandler *port_; - PacketHandler *ph_; + PortHandler *port_; + PacketHandler *ph_; std::vector id_list_; std::map address_list_; // std::map length_list_; // std::map data_list_; // - UINT8_T *param_; + bool last_result_; + + UINT8_T *param_; void MakeParam(); diff --git a/dynamixel_sdk/include/dynamixel_sdk/GroupSyncRead.h b/dynamixel_sdk/include/dynamixel_sdk/GroupSyncRead.h index 5fb7b5e..56f7cff 100644 --- a/dynamixel_sdk/include/dynamixel_sdk/GroupSyncRead.h +++ b/dynamixel_sdk/include/dynamixel_sdk/GroupSyncRead.h @@ -27,6 +27,8 @@ private: std::vector id_list_; std::map data_list_; // + bool last_result_; + UINT8_T *param_; UINT16_T start_address_; UINT16_T data_length_; diff --git a/dynamixel_sdk/src/dynamixel_sdk/GroupBulkRead.cpp b/dynamixel_sdk/src/dynamixel_sdk/GroupBulkRead.cpp index d779039..aa08587 100644 --- a/dynamixel_sdk/src/dynamixel_sdk/GroupBulkRead.cpp +++ b/dynamixel_sdk/src/dynamixel_sdk/GroupBulkRead.cpp @@ -14,6 +14,7 @@ using namespace ROBOTIS; GroupBulkRead::GroupBulkRead(PortHandler *port, PacketHandler *ph) : port_(port), ph_(ph), + last_result_(false), param_(0) { ClearParam(); @@ -116,6 +117,8 @@ int GroupBulkRead::RxPacket() int _cnt = id_list_.size(); int _result = COMM_RX_FAIL; + last_result_ = false; + if(_cnt == 0) return COMM_NOT_AVAILABLE; @@ -131,6 +134,9 @@ int GroupBulkRead::RxPacket() } } + if(_result == COMM_SUCCESS) + last_result_ = true; + return _result; } @@ -149,7 +155,7 @@ bool GroupBulkRead::GetData(UINT8_T id, UINT16_T address, UINT8_T *data) { UINT16_T _start_addr, _data_length; - if(data_list_.find(id) == data_list_.end()) + if(last_result_ == false || data_list_.find(id) == data_list_.end()) return false; _start_addr = address_list_[id]; @@ -167,7 +173,7 @@ bool GroupBulkRead::GetData(UINT8_T id, UINT16_T address, UINT16_T *data) { UINT16_T _start_addr, _data_length; - if(data_list_.find(id) == data_list_.end()) + if(last_result_ == false || data_list_.find(id) == data_list_.end()) return false; _start_addr = address_list_[id]; @@ -185,7 +191,7 @@ bool GroupBulkRead::GetData(UINT8_T id, UINT16_T address, UINT32_T *data) { UINT16_T _start_addr, _data_length; - if(data_list_.find(id) == data_list_.end()) + if(last_result_ == false || data_list_.find(id) == data_list_.end()) return false; _start_addr = address_list_[id]; diff --git a/dynamixel_sdk/src/dynamixel_sdk/GroupSyncRead.cpp b/dynamixel_sdk/src/dynamixel_sdk/GroupSyncRead.cpp index 7f12393..cf399e0 100644 --- a/dynamixel_sdk/src/dynamixel_sdk/GroupSyncRead.cpp +++ b/dynamixel_sdk/src/dynamixel_sdk/GroupSyncRead.cpp @@ -13,6 +13,7 @@ using namespace ROBOTIS; GroupSyncRead::GroupSyncRead(PortHandler *port, PacketHandler *ph, UINT16_T start_address, UINT16_T data_length) : port_(port), ph_(ph), + last_result_(false), param_(0), start_address_(start_address), data_length_(data_length) @@ -93,6 +94,8 @@ int GroupSyncRead::TxPacket() int GroupSyncRead::RxPacket() { + last_result_ = false; + if(ph_->GetProtocolVersion() == 1.0) return COMM_NOT_AVAILABLE; @@ -111,6 +114,9 @@ int GroupSyncRead::RxPacket() return _result; } + if(_result == COMM_SUCCESS) + last_result_ = true; + return _result; } @@ -130,7 +136,7 @@ int GroupSyncRead::TxRxPacket() bool GroupSyncRead::GetData(UINT8_T id, UINT16_T address, UINT8_T *data) { - if(ph_->GetProtocolVersion() == 1.0) + if(last_result_ == false || ph_->GetProtocolVersion() == 1.0) return false; if(data_list_.find(id) == data_list_.end()) @@ -145,7 +151,7 @@ bool GroupSyncRead::GetData(UINT8_T id, UINT16_T address, UINT8_T *data) } bool GroupSyncRead::GetData(UINT8_T id, UINT16_T address, UINT16_T *data) { - if(ph_->GetProtocolVersion() == 1.0) + if(last_result_ == false || ph_->GetProtocolVersion() == 1.0) return false; if(data_list_.find(id) == data_list_.end()) @@ -160,7 +166,7 @@ bool GroupSyncRead::GetData(UINT8_T id, UINT16_T address, UINT16_T *data) } bool GroupSyncRead::GetData(UINT8_T id, UINT16_T address, UINT32_T *data) { - if(ph_->GetProtocolVersion() == 1.0) + if(last_result_ == false || ph_->GetProtocolVersion() == 1.0) return false; if(data_list_.find(id) == data_list_.end()) From ba234e8e065cd60dea7c210df335fd64b9efde3d Mon Sep 17 00:00:00 2001 From: ROBOTIS Date: Mon, 14 Mar 2016 15:23:01 +0900 Subject: [PATCH 010/238] - variable name changed. - ConvertRadian2Value / ConvertValue2Radian function bug fixed. --- .../devices/dynamixel/GRIPPER.device | 32 +++++----- .../devices/dynamixel/H42-20-S300-R.device | 32 +++++----- .../devices/dynamixel/H54-100-S500-R.device | 32 +++++----- .../devices/dynamixel/H54-200-B500-R.device | 32 +++++----- .../devices/dynamixel/H54-200-S500-R.device | 32 +++++----- .../devices/dynamixel/L42-10-S300-R.device | 32 +++++----- .../devices/dynamixel/L54-30-S400-R.device | 32 +++++----- .../devices/dynamixel/L54-30-S500-R.device | 32 +++++----- .../devices/dynamixel/L54-50-S290-R.device | 32 +++++----- .../devices/dynamixel/L54-50-S500-R.device | 32 +++++----- .../devices/dynamixel/M42-10-S260-R.device | 32 +++++----- .../devices/dynamixel/M54-40-S250-R.device | 32 +++++----- .../devices/dynamixel/M54-60-S250-R.device | 32 +++++----- robotis_device/devices/dynamixel/MX-28.device | 32 +++++----- .../include/robotis_device/Dynamixel.h | 10 ++-- .../src/robotis_device/Dynamixel.cpp | 58 ++++++++++++++++++- robotis_device/src/robotis_device/Robot.cpp | 12 ++-- 17 files changed, 290 insertions(+), 238 deletions(-) diff --git a/robotis_device/devices/dynamixel/GRIPPER.device b/robotis_device/devices/dynamixel/GRIPPER.device index 42de920..ebbea55 100644 --- a/robotis_device/devices/dynamixel/GRIPPER.device +++ b/robotis_device/devices/dynamixel/GRIPPER.device @@ -3,22 +3,22 @@ model_name = GRIPPER device_type = Dynamixel [type info] -0_radian_position_value = 0 -min_radian_position_value = 0 -max_radian_position_value = 750 -min_radian = 0 -max_radian = 1.150767 -torque_enable_address = 562 -position_d_gain_address = -position_i_gain_address = -position_p_gain_address = 594 -goal_position_address = 596 -goal_velocity_address = 600 -goal_torque_address = 604 -present_position_address = 611 -present_velocity_address = 615 -present_load_address = 621 -is_moving_address = 610 +value_of_0_radian_position = 0 +value_of_min_radian_position = 0 +value_of_max_radian_position = 750 +min_radian = 0 +max_radian = 1.150767 +torque_enable_address = 562 +position_d_gain_address = +position_i_gain_address = +position_p_gain_address = 594 +goal_position_address = 596 +goal_velocity_address = 600 +goal_torque_address = 604 +present_position_address = 611 +present_velocity_address = 615 +present_load_address = 621 +is_moving_address = 610 [control table] # addr | item name | length | access | memory | min value | max value | signed diff --git a/robotis_device/devices/dynamixel/H42-20-S300-R.device b/robotis_device/devices/dynamixel/H42-20-S300-R.device index f5ae090..10884d8 100644 --- a/robotis_device/devices/dynamixel/H42-20-S300-R.device +++ b/robotis_device/devices/dynamixel/H42-20-S300-R.device @@ -3,22 +3,22 @@ model_name = H42-20-S300-R device_type = Dynamixel [type info] -0_radian_position_value = 0 -min_radian_position_value = -151900 -max_radian_position_value = 151900 -min_radian = -3.14159265 -max_radian = 3.14159265 -torque_enable_address = 562 -position_d_gain_address = -position_i_gain_address = -position_p_gain_address = 594 -goal_position_address = 596 -goal_velocity_address = 600 -goal_torque_address = 604 -present_position_address = 611 -present_velocity_address = 615 -present_load_address = 621 -is_moving_address = 610 +value_of_0_radian_position = 0 +value_of_min_radian_position = -151900 +value_of_max_radian_position = 151900 +min_radian = -3.14159265 +max_radian = 3.14159265 +torque_enable_address = 562 +position_d_gain_address = +position_i_gain_address = +position_p_gain_address = 594 +goal_position_address = 596 +goal_velocity_address = 600 +goal_torque_address = 604 +present_position_address = 611 +present_velocity_address = 615 +present_load_address = 621 +is_moving_address = 610 [control table] # addr | item name | length | access | memory | min value | max value | signed diff --git a/robotis_device/devices/dynamixel/H54-100-S500-R.device b/robotis_device/devices/dynamixel/H54-100-S500-R.device index 4043cad..64e0915 100644 --- a/robotis_device/devices/dynamixel/H54-100-S500-R.device +++ b/robotis_device/devices/dynamixel/H54-100-S500-R.device @@ -3,22 +3,22 @@ model_name = H54-100-S500-R device_type = Dynamixel [type info] -0_radian_position_value = 0 -min_radian_position_value = -250950 -max_radian_position_value = 250950 -min_radian = -3.14159265 -max_radian = 3.14159265 -torque_enable_address = 562 -position_d_gain_address = -position_i_gain_address = -position_p_gain_address = 594 -goal_position_address = 596 -goal_velocity_address = 600 -goal_torque_address = 604 -present_position_address = 611 -present_velocity_address = 615 -present_load_address = 621 -is_moving_address = 610 +value_of_0_radian_position = 0 +value_of_min_radian_position = -250950 +value_of_max_radian_position = 250950 +min_radian = -3.14159265 +max_radian = 3.14159265 +torque_enable_address = 562 +position_d_gain_address = +position_i_gain_address = +position_p_gain_address = 594 +goal_position_address = 596 +goal_velocity_address = 600 +goal_torque_address = 604 +present_position_address = 611 +present_velocity_address = 615 +present_load_address = 621 +is_moving_address = 610 [control table] # addr | item name | length | access | memory | min value | max value | signed diff --git a/robotis_device/devices/dynamixel/H54-200-B500-R.device b/robotis_device/devices/dynamixel/H54-200-B500-R.device index c3f82c5..d6ad870 100644 --- a/robotis_device/devices/dynamixel/H54-200-B500-R.device +++ b/robotis_device/devices/dynamixel/H54-200-B500-R.device @@ -3,22 +3,22 @@ model_name = H54-200-B500-R device_type = Dynamixel [type info] -0_radian_position_value = 0 -min_radian_position_value = -250950 -max_radian_position_value = 250950 -min_radian = -3.14159265 -max_radian = 3.14159265 -torque_enable_address = 562 -position_d_gain_address = -position_i_gain_address = -position_p_gain_address = 594 -goal_position_address = 596 -goal_velocity_address = 600 -goal_torque_address = 604 -present_position_address = 611 -present_velocity_address = 615 -present_load_address = 621 -is_moving_address = 610 +value_of_0_radian_position = 0 +value_of_min_radian_position = -250950 +value_of_max_radian_position = 250950 +min_radian = -3.14159265 +max_radian = 3.14159265 +torque_enable_address = 562 +position_d_gain_address = +position_i_gain_address = +position_p_gain_address = 594 +goal_position_address = 596 +goal_velocity_address = 600 +goal_torque_address = 604 +present_position_address = 611 +present_velocity_address = 615 +present_load_address = 621 +is_moving_address = 610 [control table] # addr | item name | length | access | memory | min value | max value | signed diff --git a/robotis_device/devices/dynamixel/H54-200-S500-R.device b/robotis_device/devices/dynamixel/H54-200-S500-R.device index e29ba3e..cae259f 100644 --- a/robotis_device/devices/dynamixel/H54-200-S500-R.device +++ b/robotis_device/devices/dynamixel/H54-200-S500-R.device @@ -3,22 +3,22 @@ model_name = H54-200-S500-R device_type = Dynamixel [type info] -0_radian_position_value = 0 -min_radian_position_value = -250950 -max_radian_position_value = 250950 -min_radian = -3.14159265 -max_radian = 3.14159265 -torque_enable_address = 562 -position_d_gain_address = -position_i_gain_address = -position_p_gain_address = 594 -goal_position_address = 596 -goal_velocity_address = 600 -goal_torque_address = 604 -present_position_address = 611 -present_velocity_address = 615 -present_load_address = 621 -is_moving_address = 610 +value_of_0_radian_position = 0 +value_of_min_radian_position = -250950 +value_of_max_radian_position = 250950 +min_radian = -3.14159265 +max_radian = 3.14159265 +torque_enable_address = 562 +position_d_gain_address = +position_i_gain_address = +position_p_gain_address = 594 +goal_position_address = 596 +goal_velocity_address = 600 +goal_torque_address = 604 +present_position_address = 611 +present_velocity_address = 615 +present_load_address = 621 +is_moving_address = 610 [control table] # addr | item name | length | access | memory | min value | max value | signed diff --git a/robotis_device/devices/dynamixel/L42-10-S300-R.device b/robotis_device/devices/dynamixel/L42-10-S300-R.device index 057d9a6..497761c 100644 --- a/robotis_device/devices/dynamixel/L42-10-S300-R.device +++ b/robotis_device/devices/dynamixel/L42-10-S300-R.device @@ -3,22 +3,22 @@ model_name = L42-10-S300-R device_type = Dynamixel [type info] -0_radian_position_value = 0 -min_radian_position_value = -2047 -max_radian_position_value = 2048 -min_radian = -3.14159265 -max_radian = 3.14159265 -torque_enable_address = 562 -position_d_gain_address = -position_i_gain_address = -position_p_gain_address = 594 -goal_position_address = 596 -goal_velocity_address = 600 -goal_torque_address = 604 -present_position_address = 611 -present_velocity_address = 615 -present_load_address = 621 -is_moving_address = 610 +value_of_0_radian_position = 0 +value_of_min_radian_position = -2047 +value_of_max_radian_position = 2048 +min_radian = -3.14159265 +max_radian = 3.14159265 +torque_enable_address = 562 +position_d_gain_address = +position_i_gain_address = +position_p_gain_address = 594 +goal_position_address = 596 +goal_velocity_address = 600 +goal_torque_address = 604 +present_position_address = 611 +present_velocity_address = 615 +present_load_address = 621 +is_moving_address = 610 [control table] # addr | item name | length | access | memory | min value | max value | signed diff --git a/robotis_device/devices/dynamixel/L54-30-S400-R.device b/robotis_device/devices/dynamixel/L54-30-S400-R.device index 13e8d55..b360faa 100644 --- a/robotis_device/devices/dynamixel/L54-30-S400-R.device +++ b/robotis_device/devices/dynamixel/L54-30-S400-R.device @@ -3,22 +3,22 @@ model_name = L54-30-S400-R device_type = Dynamixel [type info] -0_radian_position_value = 0 -min_radian_position_value = -144198 -max_radian_position_value = 144198 -min_radian = -3.14159265 -max_radian = 3.14159265 -torque_enable_address = 562 -position_d_gain_address = -position_i_gain_address = -position_p_gain_address = 594 -goal_position_address = 596 -goal_velocity_address = 600 -goal_torque_address = 604 -present_position_address = 611 -present_velocity_address = 615 -present_load_address = 621 -is_moving_address = 610 +value_of_0_radian_position = 0 +value_of_min_radian_position = -144198 +value_of_max_radian_position = 144198 +min_radian = -3.14159265 +max_radian = 3.14159265 +torque_enable_address = 562 +position_d_gain_address = +position_i_gain_address = +position_p_gain_address = 594 +goal_position_address = 596 +goal_velocity_address = 600 +goal_torque_address = 604 +present_position_address = 611 +present_velocity_address = 615 +present_load_address = 621 +is_moving_address = 610 [control table] # addr | item name | length | access | memory | min value | max value | signed diff --git a/robotis_device/devices/dynamixel/L54-30-S500-R.device b/robotis_device/devices/dynamixel/L54-30-S500-R.device index 000fdb9..1a2f29c 100644 --- a/robotis_device/devices/dynamixel/L54-30-S500-R.device +++ b/robotis_device/devices/dynamixel/L54-30-S500-R.device @@ -3,22 +3,22 @@ model_name = L54-30-S500-R device_type = Dynamixel [type info] -0_radian_position_value = 0 -min_radian_position_value = -180684 -max_radian_position_value = 180684 -min_radian = -3.14159265 -max_radian = 3.14159265 -torque_enable_address = 562 -position_d_gain_address = -position_i_gain_address = -position_p_gain_address = 594 -goal_position_address = 596 -goal_velocity_address = 600 -goal_torque_address = 604 -present_position_address = 611 -present_velocity_address = 615 -present_load_address = 621 -is_moving_address = 610 +value_of_0_radian_position = 0 +value_of_min_radian_position = -180684 +value_of_max_radian_position = 180684 +min_radian = -3.14159265 +max_radian = 3.14159265 +torque_enable_address = 562 +position_d_gain_address = +position_i_gain_address = +position_p_gain_address = 594 +goal_position_address = 596 +goal_velocity_address = 600 +goal_torque_address = 604 +present_position_address = 611 +present_velocity_address = 615 +present_load_address = 621 +is_moving_address = 610 [control table] # addr | item name | length | access | memory | min value | max value | signed diff --git a/robotis_device/devices/dynamixel/L54-50-S290-R.device b/robotis_device/devices/dynamixel/L54-50-S290-R.device index 1bf2115..64577dd 100644 --- a/robotis_device/devices/dynamixel/L54-50-S290-R.device +++ b/robotis_device/devices/dynamixel/L54-50-S290-R.device @@ -3,22 +3,22 @@ model_name = L54-50-S290-R device_type = Dynamixel [type info] -0_radian_position_value = 0 -min_radian_position_value = -103860 -max_radian_position_value = 103860 -min_radian = -3.14159265 -max_radian = 3.14159265 -torque_enable_address = 562 -position_d_gain_address = -position_i_gain_address = -position_p_gain_address = 594 -goal_position_address = 596 -goal_velocity_address = 600 -goal_torque_address = 604 -present_position_address = 611 -present_velocity_address = 615 -present_load_address = 621 -is_moving_address = 610 +value_of_0_radian_position = 0 +value_of_min_radian_position = -103860 +value_of_max_radian_position = 103860 +min_radian = -3.14159265 +max_radian = 3.14159265 +torque_enable_address = 562 +position_d_gain_address = +position_i_gain_address = +position_p_gain_address = 594 +goal_position_address = 596 +goal_velocity_address = 600 +goal_torque_address = 604 +present_position_address = 611 +present_velocity_address = 615 +present_load_address = 621 +is_moving_address = 610 [control table] # addr | item name | length | access | memory | min value | max value | signed diff --git a/robotis_device/devices/dynamixel/L54-50-S500-R.device b/robotis_device/devices/dynamixel/L54-50-S500-R.device index 8fcd40c..1de23fd 100644 --- a/robotis_device/devices/dynamixel/L54-50-S500-R.device +++ b/robotis_device/devices/dynamixel/L54-50-S500-R.device @@ -3,22 +3,22 @@ model_name = L54-50-S500-R device_type = Dynamixel [type info] -0_radian_position_value = 0 -min_radian_position_value = -180684 -max_radian_position_value = 180684 -min_radian = -3.14159265 -max_radian = 3.14159265 -torque_enable_address = 562 -position_d_gain_address = -position_i_gain_address = -position_p_gain_address = 594 -goal_position_address = 596 -goal_velocity_address = 600 -goal_torque_address = 604 -present_position_address = 611 -present_velocity_address = 615 -present_load_address = 621 -is_moving_address = 610 +value_of_0_radian_position = 0 +value_of_min_radian_position = -180684 +value_of_max_radian_position = 180684 +min_radian = -3.14159265 +max_radian = 3.14159265 +torque_enable_address = 562 +position_d_gain_address = +position_i_gain_address = +position_p_gain_address = 594 +goal_position_address = 596 +goal_velocity_address = 600 +goal_torque_address = 604 +present_position_address = 611 +present_velocity_address = 615 +present_load_address = 621 +is_moving_address = 610 [control table] # addr | item name | length | access | memory | min value | max value | signed diff --git a/robotis_device/devices/dynamixel/M42-10-S260-R.device b/robotis_device/devices/dynamixel/M42-10-S260-R.device index d9836d8..b4511af 100644 --- a/robotis_device/devices/dynamixel/M42-10-S260-R.device +++ b/robotis_device/devices/dynamixel/M42-10-S260-R.device @@ -3,22 +3,22 @@ model_name = M42-10-S260-R device_type = Dynamixel [type info] -0_radian_position_value = 0 -min_radian_position_value = -131584 -max_radian_position_value = 131584 -min_radian = -3.14159265 -max_radian = 3.14159265 -torque_enable_address = 562 -position_d_gain_address = -position_i_gain_address = -position_p_gain_address = 594 -goal_position_address = 596 -goal_velocity_address = 600 -goal_torque_address = 604 -present_position_address = 611 -present_velocity_address = 615 -present_load_address = 621 -is_moving_address = 610 +value_of_0_radian_position = 0 +value_of_min_radian_position = -131584 +value_of_max_radian_position = 131584 +min_radian = -3.14159265 +max_radian = 3.14159265 +torque_enable_address = 562 +position_d_gain_address = +position_i_gain_address = +position_p_gain_address = 594 +goal_position_address = 596 +goal_velocity_address = 600 +goal_torque_address = 604 +present_position_address = 611 +present_velocity_address = 615 +present_load_address = 621 +is_moving_address = 610 [control table] # addr | item name | length | access | memory | min value | max value | signed diff --git a/robotis_device/devices/dynamixel/M54-40-S250-R.device b/robotis_device/devices/dynamixel/M54-40-S250-R.device index ba3d1ac..345dc1e 100644 --- a/robotis_device/devices/dynamixel/M54-40-S250-R.device +++ b/robotis_device/devices/dynamixel/M54-40-S250-R.device @@ -3,22 +3,22 @@ model_name = M54-40-S250-R device_type = Dynamixel [type info] -0_radian_position_value = 0 -min_radian_position_value = -125700 -max_radian_position_value = 125700 -min_radian = -3.14159265 -max_radian = 3.14159265 -torque_enable_address = 562 -position_d_gain_address = -position_i_gain_address = -position_p_gain_address = 594 -goal_position_address = 596 -goal_velocity_address = 600 -goal_torque_address = 604 -present_position_address = 611 -present_velocity_address = 615 -present_load_address = 621 -is_moving_address = 610 +value_of_0_radian_position = 0 +value_of_min_radian_position = -125700 +value_of_max_radian_position = 125700 +min_radian = -3.14159265 +max_radian = 3.14159265 +torque_enable_address = 562 +position_d_gain_address = +position_i_gain_address = +position_p_gain_address = 594 +goal_position_address = 596 +goal_velocity_address = 600 +goal_torque_address = 604 +present_position_address = 611 +present_velocity_address = 615 +present_load_address = 621 +is_moving_address = 610 [control table] # addr | item name | length | access | memory | min value | max value | signed diff --git a/robotis_device/devices/dynamixel/M54-60-S250-R.device b/robotis_device/devices/dynamixel/M54-60-S250-R.device index ae9cbed..bfcc9ed 100644 --- a/robotis_device/devices/dynamixel/M54-60-S250-R.device +++ b/robotis_device/devices/dynamixel/M54-60-S250-R.device @@ -3,22 +3,22 @@ model_name = M54-60-S250-R device_type = Dynamixel [type info] -0_radian_position_value = 0 -min_radian_position_value = -125700 -max_radian_position_value = 125700 -min_radian = -3.14159265 -max_radian = 3.14159265 -torque_enable_address = 562 -position_d_gain_address = -position_i_gain_address = -position_p_gain_address = 594 -goal_position_address = 596 -goal_velocity_address = 600 -goal_torque_address = 604 -present_position_address = 611 -present_velocity_address = 615 -present_load_address = 621 -is_moving_address = 610 +value_of_0_radian_position = 0 +value_of_min_radian_position = -125700 +value_of_max_radian_position = 125700 +min_radian = -3.14159265 +max_radian = 3.14159265 +torque_enable_address = 562 +position_d_gain_address = +position_i_gain_address = +position_p_gain_address = 594 +goal_position_address = 596 +goal_velocity_address = 600 +goal_torque_address = 604 +present_position_address = 611 +present_velocity_address = 615 +present_load_address = 621 +is_moving_address = 610 [control table] # addr | item name | length | access | memory | min value | max value | signed diff --git a/robotis_device/devices/dynamixel/MX-28.device b/robotis_device/devices/dynamixel/MX-28.device index ef59e83..beb0af8 100644 --- a/robotis_device/devices/dynamixel/MX-28.device +++ b/robotis_device/devices/dynamixel/MX-28.device @@ -3,22 +3,22 @@ model_name = MX-28 device_type = Dynamixel [type info] -0_radian_position_value = 2048 -min_radian_position_value = 0 -max_radian_position_value = 4095 -min_radian = -3.14159265 -max_radian = 3.14159265 -torque_enable_address = 24 -position_d_gain_address = 26 -position_i_gain_address = 27 -position_p_gain_address = 28 -goal_position_address = 30 -goal_velocity_address = 32 -goal_torque_address = 34 -present_position_address = 36 -present_velocity_address = 38 -present_load_address = 40 -is_moving_address = 46 +value_of_0_radian_position = 2048 +value_of_min_radian_position = 0 +value_of_max_radian_position = 4095 +min_radian = -3.14159265 +max_radian = 3.14159265 +torque_enable_address = 24 +position_d_gain_address = 26 +position_i_gain_address = 27 +position_p_gain_address = 28 +goal_position_address = 30 +goal_velocity_address = 32 +goal_torque_address = 34 +present_position_address = 36 +present_velocity_address = 38 +present_load_address = 40 +is_moving_address = 46 [control table] # addr | item name | length | access | memory | min value | max value | signed diff --git a/robotis_device/include/robotis_device/Dynamixel.h b/robotis_device/include/robotis_device/Dynamixel.h index 4649044..ef4e826 100644 --- a/robotis_device/include/robotis_device/Dynamixel.h +++ b/robotis_device/include/robotis_device/Dynamixel.h @@ -30,9 +30,9 @@ public: std::string ctrl_module_name; DynamixelState *dxl_state; - INT32_T zero_radian_position_value; - INT32_T min_radian_position_value; - INT32_T max_radian_position_value; + INT32_T value_of_0_radian_position; + INT32_T value_of_min_radian_position; + INT32_T value_of_max_radian_position; double min_radian; double max_radian; @@ -50,8 +50,8 @@ public: Dynamixel(int id, std::string model_name, float protocol_version); - double ConvertValue2Radian(int32_t value) { return value * max_radian / max_radian_position_value; } - INT32_T ConvertRadian2Value(double radian) { return radian * max_radian_position_value / max_radian; } + double ConvertValue2Radian(INT32_T value); + INT32_T ConvertRadian2Value(double radian); }; } diff --git a/robotis_device/src/robotis_device/Dynamixel.cpp b/robotis_device/src/robotis_device/Dynamixel.cpp index baafc2e..f2a78a7 100644 --- a/robotis_device/src/robotis_device/Dynamixel.cpp +++ b/robotis_device/src/robotis_device/Dynamixel.cpp @@ -15,9 +15,9 @@ Dynamixel::Dynamixel(int id, std::string model_name, float protocol_version) port_name(""), ctrl_module_name("none"), protocol_version(protocol_version), - zero_radian_position_value(0), - min_radian_position_value(0), - max_radian_position_value(0), + value_of_0_radian_position(0), + value_of_min_radian_position(0), + value_of_max_radian_position(0), min_radian(-3.14), max_radian(3.14), torque_enable_address(0), @@ -35,3 +35,55 @@ Dynamixel::Dynamixel(int id, std::string model_name, float protocol_version) ctrl_table.clear(); dxl_state = new DynamixelState(); } + +double Dynamixel::ConvertValue2Radian(INT32_T value) +{ + double _radian = 0.0; + if(value > value_of_0_radian_position) + { + if(max_radian <= 0) + return max_radian; + _radian = (double)(value - value_of_0_radian_position) * max_radian / (double)(value_of_max_radian_position - value_of_0_radian_position); + } + else if(value < value_of_0_radian_position) + { + if(min_radian >= 0) + return min_radian; + _radian = (double)(value - value_of_0_radian_position) * min_radian / (double)(value_of_min_radian_position - value_of_0_radian_position); + } + + if(_radian > max_radian) + return max_radian; + else if(_radian < min_radian) + return min_radian; + + return _radian; +} + +INT32_T Dynamixel::ConvertRadian2Value(double radian) +{ + //return radian * value_of_max_radian_position / max_radian; + + INT32_T _value = 0; + if(radian > 0) + { + if(value_of_max_radian_position <= value_of_0_radian_position) + return value_of_max_radian_position; + _value = (radian * (value_of_max_radian_position - value_of_0_radian_position) / max_radian) + value_of_0_radian_position; + } + else if(radian < 0) + { + if(value_of_min_radian_position >= value_of_0_radian_position) + return value_of_min_radian_position; + _value = (radian * (value_of_min_radian_position - value_of_0_radian_position) / min_radian) + value_of_0_radian_position; + } + else + _value = value_of_0_radian_position; + + if(_value > value_of_max_radian_position) + return value_of_max_radian_position; + else if(_value < value_of_min_radian_position) + return value_of_min_radian_position; + + return _value; +} diff --git a/robotis_device/src/robotis_device/Robot.cpp b/robotis_device/src/robotis_device/Robot.cpp index a6504fb..64c5594 100644 --- a/robotis_device/src/robotis_device/Robot.cpp +++ b/robotis_device/src/robotis_device/Robot.cpp @@ -152,12 +152,12 @@ Dynamixel *Robot::getDynamixel(std::string path, int id, std::string port, float if(tokens.size() != 2) continue; - if(tokens[0] == "0_radian_position_value") - dxl->zero_radian_position_value = std::atoi(tokens[1].c_str()); - else if(tokens[0] == "min_radian_position_value") - dxl->min_radian_position_value = std::atoi(tokens[1].c_str()); - else if(tokens[0] == "max_radian_position_value") - dxl->max_radian_position_value = std::atoi(tokens[1].c_str()); + if(tokens[0] == "value_of_0_radian_position") + dxl->value_of_0_radian_position = std::atoi(tokens[1].c_str()); + else if(tokens[0] == "value_of_min_radian_position") + dxl->value_of_min_radian_position = std::atoi(tokens[1].c_str()); + else if(tokens[0] == "value_of_max_radian_position") + dxl->value_of_max_radian_position = std::atoi(tokens[1].c_str()); else if(tokens[0] == "min_radian") dxl->min_radian = std::atof(tokens[1].c_str()); else if(tokens[0] == "max_radian") From f81f17c5596a718e2003d59072f1807383c6f8f4 Mon Sep 17 00:00:00 2001 From: sadtale Date: Tue, 29 Mar 2016 15:08:10 +0900 Subject: [PATCH 011/238] - added rxpacket error check - modified Read TxRx function --- .../dynamixel_sdk/Protocol1PacketHandler.cpp | 74 ++++++++++---- .../dynamixel_sdk/Protocol2PacketHandler.cpp | 99 ++++++++++++------- 2 files changed, 117 insertions(+), 56 deletions(-) diff --git a/dynamixel_sdk/src/dynamixel_sdk/Protocol1PacketHandler.cpp b/dynamixel_sdk/src/dynamixel_sdk/Protocol1PacketHandler.cpp index 2b5ccc4..630afe0 100644 --- a/dynamixel_sdk/src/dynamixel_sdk/Protocol1PacketHandler.cpp +++ b/dynamixel_sdk/src/dynamixel_sdk/Protocol1PacketHandler.cpp @@ -167,6 +167,18 @@ int Protocol1PacketHandler::RxPacket(PortHandler *port, UINT8_T *rxpacket) if(_idx == 0) // found at the beginning of the packet { + if(rxpacket[PKT_ID] > 0xFD || // unavailable ID + rxpacket[PKT_LENGTH] > RXPACKET_MAX_LEN || // unavailable Length + rxpacket[PKT_ERROR] >= 0x64) // unavailable Error + { + // remove the first byte in the packet + for(UINT8_T _s = 0; _s < _rx_length - 1; _s++) + rxpacket[_s] = rxpacket[1 + _s]; + //memcpy(&rxpacket[0], &rxpacket[_idx], _rx_length - _idx); + _rx_length -= 1; + continue; + } + // re-calculate the exact length of the rx packet _wait_length = rxpacket[PKT_LENGTH] + PKT_LENGTH + 1; if(_rx_length < _wait_length) @@ -250,7 +262,9 @@ int Protocol1PacketHandler::TxRxPacket(PortHandler *port, UINT8_T *txpacket, UIN // rx packet _result = RxPacket(port, rxpacket); - // TODO: check txpacket ID == rxpacket ID + // check txpacket ID == rxpacket ID + if(txpacket[PKT_ID] != rxpacket[PKT_ID]) + _result = RxPacket(port, rxpacket); if(_result == COMM_SUCCESS && txpacket[PKT_ID] != BROADCAST_ID) { @@ -374,11 +388,31 @@ int Protocol1PacketHandler::ReadTxRx(PortHandler *port, UINT8_T id, UINT16_T add { int _result = COMM_TX_FAIL; - _result = ReadTx(port, id, address, length); - if(_result != COMM_SUCCESS) - return _result; + UINT8_T txpacket[8] = {0}; + UINT8_T *rxpacket = (UINT8_T *)malloc(RXPACKET_MAX_LEN);//(length+6); - return ReadRx(port, length, data, error); + if(id >= BROADCAST_ID) + return COMM_NOT_AVAILABLE; + + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH] = 4; + txpacket[PKT_INSTRUCTION] = INST_READ; + txpacket[PKT_PARAMETER0+0] = (UINT8_T)address; + txpacket[PKT_PARAMETER0+1] = (UINT8_T)length; + + _result = TxRxPacket(port, txpacket, rxpacket, error); + if(_result == COMM_SUCCESS) + { + if(error != 0) + *error = (UINT8_T)rxpacket[PKT_ERROR]; + for(UINT8_T _s = 0; _s < length; _s++) + data[_s] = rxpacket[PKT_PARAMETER0 + _s]; + //memcpy(data, &rxpacket[PKT_PARAMETER0], length); + } + + free(rxpacket); + //delete[] rxpacket; + return _result; } int Protocol1PacketHandler::Read1ByteTx(PortHandler *port, UINT8_T id, UINT16_T address) @@ -389,18 +423,17 @@ int Protocol1PacketHandler::Read1ByteRx(PortHandler *port, UINT8_T *data, UINT8_ { UINT8_T _data[1] = {0}; int _result = ReadRx(port, 1, _data, error); - *data = _data[0]; + if(_result == COMM_SUCCESS) + *data = _data[0]; return _result; } int Protocol1PacketHandler::Read1ByteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T *data, UINT8_T *error) { - int _result = COMM_TX_FAIL; - - _result = Read1ByteTx(port, id, address); - if(_result != COMM_SUCCESS) - return _result; - - return Read1ByteRx(port, data, error); + UINT8_T _data[1] = {0}; + int _result = ReadTxRx(port, id, address, 1, _data, error); + if(_result == COMM_SUCCESS) + *data = _data[0]; + return _result; } int Protocol1PacketHandler::Read2ByteTx(PortHandler *port, UINT8_T id, UINT16_T address) @@ -411,18 +444,17 @@ int Protocol1PacketHandler::Read2ByteRx(PortHandler *port, UINT16_T *data, UINT8 { UINT8_T _data[2] = {0}; int _result = ReadRx(port, 2, _data, error); - *data = DXL_MAKEWORD(_data[0], _data[1]); + if(_result == COMM_SUCCESS) + *data = DXL_MAKEWORD(_data[0], _data[1]); return _result; } int Protocol1PacketHandler::Read2ByteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T *data, UINT8_T *error) { - int _result = COMM_TX_FAIL; - - _result = Read2ByteTx(port, id, address); - if(_result != COMM_SUCCESS) - return _result; - - return Read2ByteRx(port, data, error); + UINT8_T _data[2] = {0}; + int _result = ReadTxRx(port, id, address, 2, _data, error); + if(_result == COMM_SUCCESS) + *data = DXL_MAKEWORD(_data[0], _data[1]); + return _result; } int Protocol1PacketHandler::Read4ByteTx(PortHandler *port, UINT8_T id, UINT16_T address) diff --git a/dynamixel_sdk/src/dynamixel_sdk/Protocol2PacketHandler.cpp b/dynamixel_sdk/src/dynamixel_sdk/Protocol2PacketHandler.cpp index c178f76..39cba41 100644 --- a/dynamixel_sdk/src/dynamixel_sdk/Protocol2PacketHandler.cpp +++ b/dynamixel_sdk/src/dynamixel_sdk/Protocol2PacketHandler.cpp @@ -312,14 +312,21 @@ int Protocol2PacketHandler::RxPacket(PortHandler *port, UINT8_T *rxpacket) if(_idx == 0) // found at the beginning of the packet { + if(rxpacket[PKT_RESERVED] != 0x00 || + rxpacket[PKT_ID] > 0xFC || + DXL_MAKEWORD(rxpacket[PKT_LENGTH_L], rxpacket[PKT_LENGTH_H]) > RXPACKET_MAX_LEN || + rxpacket[PKT_INSTRUCTION] != 0x55) + { + // remove the first byte in the packet + for(UINT8_T _s = 0; _s < _rx_length - 1; _s++) + rxpacket[_s] = rxpacket[1 + _s]; + //memcpy(&rxpacket[0], &rxpacket[_idx], _rx_length - _idx); + _rx_length -= 1; + continue; + } + // re-calculate the exact length of the rx packet _wait_length = DXL_MAKEWORD(rxpacket[PKT_LENGTH_L], rxpacket[PKT_LENGTH_H]) + PKT_LENGTH_H + 1; - if(_wait_length > RXPACKET_MAX_LEN - 10) - { - _result = COMM_RX_CORRUPT; - break; - } - if(_rx_length < _wait_length) { // check timeout @@ -401,7 +408,9 @@ int Protocol2PacketHandler::TxRxPacket(PortHandler *port, UINT8_T *txpacket, UIN // rx packet _result = RxPacket(port, rxpacket); - // TODO: check txpacket ID == rxpacket ID + // check txpacket ID == rxpacket ID + if(txpacket[PKT_ID] != rxpacket[PKT_ID]) + _result = RxPacket(port, rxpacket); if(_result == COMM_SUCCESS && txpacket[PKT_ID] != BROADCAST_ID) { @@ -624,11 +633,34 @@ int Protocol2PacketHandler::ReadTxRx(PortHandler *port, UINT8_T id, UINT16_T add { int _result = COMM_TX_FAIL; - _result = ReadTx(port, id, address, length); - if(_result != COMM_SUCCESS) - return _result; + UINT8_T txpacket[14] = {0}; + UINT8_T *rxpacket = (UINT8_T *)malloc(RXPACKET_MAX_LEN);//(length + 11 + (length/3)); // (length/3): consider stuffing - return ReadRx(port, length, data, error); + if(id >= BROADCAST_ID) + return COMM_NOT_AVAILABLE; + + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH_L] = 7; + txpacket[PKT_LENGTH_H] = 0; + txpacket[PKT_INSTRUCTION] = INST_READ; + txpacket[PKT_PARAMETER0+0] = (UINT8_T)DXL_LOBYTE(address); + txpacket[PKT_PARAMETER0+1] = (UINT8_T)DXL_HIBYTE(address); + txpacket[PKT_PARAMETER0+2] = (UINT8_T)DXL_LOBYTE(length); + txpacket[PKT_PARAMETER0+3] = (UINT8_T)DXL_HIBYTE(length); + + _result = TxRxPacket(port, txpacket, rxpacket, error); + if(_result == COMM_SUCCESS) + { + if(error != 0) + *error = (UINT8_T)rxpacket[PKT_ERROR]; + for(UINT8_T _s = 0; _s < length; _s++) + data[_s] = rxpacket[PKT_PARAMETER0 + 1 + _s]; + //memcpy(data, &rxpacket[PKT_PARAMETER0+1], length); + } + + free(rxpacket); + //delete[] rxpacket; + return _result; } int Protocol2PacketHandler::Read1ByteTx(PortHandler *port, UINT8_T id, UINT16_T address) @@ -639,18 +671,17 @@ int Protocol2PacketHandler::Read1ByteRx(PortHandler *port, UINT8_T *data, UINT8_ { UINT8_T _data[1] = {0}; int _result = ReadRx(port, 1, _data, error); - *data = _data[0]; + if(_result == COMM_SUCCESS) + *data = _data[0]; return _result; } int Protocol2PacketHandler::Read1ByteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T *data, UINT8_T *error) { - int _result = COMM_TX_FAIL; - - _result = Read1ByteTx(port, id, address); - if(_result != COMM_SUCCESS) - return _result; - - return Read1ByteRx(port, data, error); + UINT8_T _data[1] = {0}; + int _result = ReadTxRx(port, id, address, 1, _data, error); + if(_result == COMM_SUCCESS) + *data = _data[0]; + return _result; } int Protocol2PacketHandler::Read2ByteTx(PortHandler *port, UINT8_T id, UINT16_T address) @@ -661,18 +692,17 @@ int Protocol2PacketHandler::Read2ByteRx(PortHandler *port, UINT16_T *data, UINT8 { UINT8_T _data[2] = {0}; int _result = ReadRx(port, 2, _data, error); - *data = DXL_MAKEWORD(_data[0], _data[1]); + if(_result == COMM_SUCCESS) + *data = DXL_MAKEWORD(_data[0], _data[1]); return _result; } int Protocol2PacketHandler::Read2ByteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T *data, UINT8_T *error) { - int _result = COMM_TX_FAIL; - - _result = Read2ByteTx(port, id, address); - if(_result != COMM_SUCCESS) - return _result; - - return Read2ByteRx(port, data, error); + UINT8_T _data[2] = {0}; + int _result = ReadTxRx(port, id, address, 2, _data, error); + if(_result == COMM_SUCCESS) + *data = DXL_MAKEWORD(_data[0], _data[1]); + return _result; } int Protocol2PacketHandler::Read4ByteTx(PortHandler *port, UINT8_T id, UINT16_T address) @@ -683,18 +713,17 @@ int Protocol2PacketHandler::Read4ByteRx(PortHandler *port, UINT32_T *data, UINT8 { UINT8_T _data[4] = {0}; int _result = ReadRx(port, 4, _data, error); - *data = DXL_MAKEDWORD(DXL_MAKEWORD(_data[0], _data[1]), DXL_MAKEWORD(_data[2], _data[3])); + if(_result == COMM_SUCCESS) + *data = DXL_MAKEDWORD(DXL_MAKEWORD(_data[0], _data[1]), DXL_MAKEWORD(_data[2], _data[3])); return _result; } int Protocol2PacketHandler::Read4ByteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T *data, UINT8_T *error) { - int _result = COMM_TX_FAIL; - - _result = Read4ByteTx(port, id, address); - if(_result != COMM_SUCCESS) - return _result; - - return Read4ByteRx(port, data, error); + UINT8_T _data[4] = {0}; + int _result = ReadTxRx(port, id, address, 4, _data, error); + if(_result == COMM_SUCCESS) + *data = DXL_MAKEDWORD(DXL_MAKEWORD(_data[0], _data[1]), DXL_MAKEWORD(_data[2], _data[3])); + return _result; } From 738b68b6e4974d0fdae31515965e0d73a5b6acdb Mon Sep 17 00:00:00 2001 From: sadtale Date: Thu, 31 Mar 2016 21:58:43 +0900 Subject: [PATCH 012/238] - reducing the count of calling MakeParam function --- dynamixel_sdk/include/dynamixel_sdk/GroupBulkRead.h | 1 + dynamixel_sdk/include/dynamixel_sdk/GroupBulkWrite.h | 2 ++ dynamixel_sdk/include/dynamixel_sdk/GroupSyncRead.h | 1 + dynamixel_sdk/include/dynamixel_sdk/GroupSyncWrite.h | 2 ++ dynamixel_sdk/src/dynamixel_sdk/GroupBulkRead.cpp | 8 ++++++-- dynamixel_sdk/src/dynamixel_sdk/GroupBulkWrite.cpp | 10 +++++++--- dynamixel_sdk/src/dynamixel_sdk/GroupSyncRead.cpp | 8 ++++++-- dynamixel_sdk/src/dynamixel_sdk/GroupSyncWrite.cpp | 10 +++++++--- 8 files changed, 32 insertions(+), 10 deletions(-) diff --git a/dynamixel_sdk/include/dynamixel_sdk/GroupBulkRead.h b/dynamixel_sdk/include/dynamixel_sdk/GroupBulkRead.h index 45706c9..af8f99b 100644 --- a/dynamixel_sdk/include/dynamixel_sdk/GroupBulkRead.h +++ b/dynamixel_sdk/include/dynamixel_sdk/GroupBulkRead.h @@ -30,6 +30,7 @@ private: std::map data_list_; // bool last_result_; + bool is_param_changed_; UINT8_T *param_; diff --git a/dynamixel_sdk/include/dynamixel_sdk/GroupBulkWrite.h b/dynamixel_sdk/include/dynamixel_sdk/GroupBulkWrite.h index 348803e..650bc25 100644 --- a/dynamixel_sdk/include/dynamixel_sdk/GroupBulkWrite.h +++ b/dynamixel_sdk/include/dynamixel_sdk/GroupBulkWrite.h @@ -29,6 +29,8 @@ private: std::map length_list_; // std::map data_list_; // + bool is_param_changed_; + UINT8_T *param_; UINT16_T param_length_; diff --git a/dynamixel_sdk/include/dynamixel_sdk/GroupSyncRead.h b/dynamixel_sdk/include/dynamixel_sdk/GroupSyncRead.h index 56f7cff..40dac81 100644 --- a/dynamixel_sdk/include/dynamixel_sdk/GroupSyncRead.h +++ b/dynamixel_sdk/include/dynamixel_sdk/GroupSyncRead.h @@ -28,6 +28,7 @@ private: std::map data_list_; // bool last_result_; + bool is_param_changed_; UINT8_T *param_; UINT16_T start_address_; diff --git a/dynamixel_sdk/include/dynamixel_sdk/GroupSyncWrite.h b/dynamixel_sdk/include/dynamixel_sdk/GroupSyncWrite.h index 62a8951..28e1458 100644 --- a/dynamixel_sdk/include/dynamixel_sdk/GroupSyncWrite.h +++ b/dynamixel_sdk/include/dynamixel_sdk/GroupSyncWrite.h @@ -27,6 +27,8 @@ private: std::vector id_list_; std::map data_list_; // + bool is_param_changed_; + UINT8_T *param_; UINT16_T start_address_; UINT16_T data_length_; diff --git a/dynamixel_sdk/src/dynamixel_sdk/GroupBulkRead.cpp b/dynamixel_sdk/src/dynamixel_sdk/GroupBulkRead.cpp index aa08587..2496fa0 100644 --- a/dynamixel_sdk/src/dynamixel_sdk/GroupBulkRead.cpp +++ b/dynamixel_sdk/src/dynamixel_sdk/GroupBulkRead.cpp @@ -15,6 +15,7 @@ GroupBulkRead::GroupBulkRead(PortHandler *port, PacketHandler *ph) : port_(port), ph_(ph), last_result_(false), + is_param_changed_(false), param_(0) { ClearParam(); @@ -65,7 +66,7 @@ bool GroupBulkRead::AddParam(UINT8_T id, UINT16_T start_address, UINT16_T data_l address_list_[id] = start_address; data_list_[id] = new UINT8_T[data_length]; - MakeParam(); + is_param_changed_ = true; return true; } @@ -81,7 +82,7 @@ void GroupBulkRead::RemoveParam(UINT8_T id) delete[] data_list_[id]; data_list_.erase(id); - MakeParam(); + is_param_changed_ = true; } void GroupBulkRead::ClearParam() @@ -106,6 +107,9 @@ int GroupBulkRead::TxPacket() if(id_list_.size() == 0) return COMM_NOT_AVAILABLE; + if(is_param_changed_ == true) + MakeParam(); + if(ph_->GetProtocolVersion() == 1.0) return ph_->BulkReadTx(port_, param_, id_list_.size() * 3); else // 2.0 diff --git a/dynamixel_sdk/src/dynamixel_sdk/GroupBulkWrite.cpp b/dynamixel_sdk/src/dynamixel_sdk/GroupBulkWrite.cpp index 0083db8..39e8eb4 100644 --- a/dynamixel_sdk/src/dynamixel_sdk/GroupBulkWrite.cpp +++ b/dynamixel_sdk/src/dynamixel_sdk/GroupBulkWrite.cpp @@ -13,6 +13,7 @@ using namespace ROBOTIS; GroupBulkWrite::GroupBulkWrite(PortHandler *port, PacketHandler *ph) : port_(port), ph_(ph), + is_param_changed_(false), param_(0), param_length_(0) { @@ -66,7 +67,7 @@ bool GroupBulkWrite::AddParam(UINT8_T id, UINT16_T start_address, UINT16_T data_ for(int _c = 0; _c < data_length; _c++) data_list_[id][_c] = data[_c]; - MakeParam(); + is_param_changed_ = true; return true; } void GroupBulkWrite::RemoveParam(UINT8_T id) @@ -84,7 +85,7 @@ void GroupBulkWrite::RemoveParam(UINT8_T id) delete[] data_list_[id]; data_list_.erase(id); - MakeParam(); + is_param_changed_ = true; } bool GroupBulkWrite::ChangeParam(UINT8_T id, UINT16_T start_address, UINT16_T data_length, UINT8_T *data) { @@ -102,7 +103,7 @@ bool GroupBulkWrite::ChangeParam(UINT8_T id, UINT16_T start_address, UINT16_T da for(int _c = 0; _c < data_length; _c++) data_list_[id][_c] = data[_c]; - MakeParam(); + is_param_changed_ = true; return true; } void GroupBulkWrite::ClearParam() @@ -129,5 +130,8 @@ int GroupBulkWrite::TxPacket() if(ph_->GetProtocolVersion() == 1.0 || id_list_.size() == 0) return COMM_NOT_AVAILABLE; + if(is_param_changed_ == true) + MakeParam(); + return ph_->BulkWriteTxOnly(port_, param_, param_length_); } diff --git a/dynamixel_sdk/src/dynamixel_sdk/GroupSyncRead.cpp b/dynamixel_sdk/src/dynamixel_sdk/GroupSyncRead.cpp index cf399e0..1a89544 100644 --- a/dynamixel_sdk/src/dynamixel_sdk/GroupSyncRead.cpp +++ b/dynamixel_sdk/src/dynamixel_sdk/GroupSyncRead.cpp @@ -14,6 +14,7 @@ GroupSyncRead::GroupSyncRead(PortHandler *port, PacketHandler *ph, UINT16_T star : port_(port), ph_(ph), last_result_(false), + is_param_changed_(false), param_(0), start_address_(start_address), data_length_(data_length) @@ -48,7 +49,7 @@ bool GroupSyncRead::AddParam(UINT8_T id) id_list_.push_back(id); data_list_[id] = new UINT8_T[data_length_]; - MakeParam(); + is_param_changed_ = true; return true; } void GroupSyncRead::RemoveParam(UINT8_T id) @@ -64,7 +65,7 @@ void GroupSyncRead::RemoveParam(UINT8_T id) delete[] data_list_[id]; data_list_.erase(id); - MakeParam(); + is_param_changed_ = true; } void GroupSyncRead::ClearParam() { @@ -89,6 +90,9 @@ int GroupSyncRead::TxPacket() if(ph_->GetProtocolVersion() == 1.0 || id_list_.size() == 0) return COMM_NOT_AVAILABLE; + if(is_param_changed_ == true) + MakeParam(); + return ph_->SyncReadTx(port_, start_address_, data_length_, param_, (UINT16_T)id_list_.size() * 1); } diff --git a/dynamixel_sdk/src/dynamixel_sdk/GroupSyncWrite.cpp b/dynamixel_sdk/src/dynamixel_sdk/GroupSyncWrite.cpp index 3c12caa..38d381b 100644 --- a/dynamixel_sdk/src/dynamixel_sdk/GroupSyncWrite.cpp +++ b/dynamixel_sdk/src/dynamixel_sdk/GroupSyncWrite.cpp @@ -13,6 +13,7 @@ using namespace ROBOTIS; GroupSyncWrite::GroupSyncWrite(PortHandler *port, PacketHandler *ph, UINT16_T start_address, UINT16_T data_length) : port_(port), ph_(ph), + is_param_changed_(false), param_(0), start_address_(start_address), data_length_(data_length) @@ -54,7 +55,7 @@ bool GroupSyncWrite::AddParam(UINT8_T id, UINT8_T *data) for(int _c = 0; _c < data_length_; _c++) data_list_[id][_c] = data[_c]; - MakeParam(); + is_param_changed_ = true; return true; } @@ -68,7 +69,7 @@ void GroupSyncWrite::RemoveParam(UINT8_T id) delete[] data_list_[id]; data_list_.erase(id); - MakeParam(); + is_param_changed_ = true; } bool GroupSyncWrite::ChangeParam(UINT8_T id, UINT8_T *data) @@ -82,7 +83,7 @@ bool GroupSyncWrite::ChangeParam(UINT8_T id, UINT8_T *data) for(int _c = 0; _c < data_length_; _c++) data_list_[id][_c] = data[_c]; - MakeParam(); + is_param_changed_ = true; return true; } @@ -106,5 +107,8 @@ int GroupSyncWrite::TxPacket() if(id_list_.size() == 0) return COMM_NOT_AVAILABLE; + if(is_param_changed_ == true) + MakeParam(); + return ph_->SyncWriteTxOnly(port_, start_address_, data_length_, param_, id_list_.size() * (1 + data_length_)); } From c72bab42ba1f467c57aa64b28f15c42c0c3b53b9 Mon Sep 17 00:00:00 2001 From: ROBOTIS-zerom Date: Fri, 29 Apr 2016 16:18:25 +0900 Subject: [PATCH 013/238] modified. --- dynamixel_sdk/CMakeLists.txt | 2 +- dynamixel_sdk/include/DynamixelSDK.h | 2 +- .../include/dynamixel_sdk/GroupBulkRead.h | 9 +- .../include/dynamixel_sdk/GroupBulkWrite.h | 4 +- .../include/dynamixel_sdk/GroupSyncRead.h | 9 +- .../include/dynamixel_sdk/GroupSyncWrite.h | 4 +- .../include/dynamixel_sdk/PacketHandler.h | 4 +- .../include/dynamixel_sdk/PortHandler.h | 13 +- .../dynamixel_sdk/Protocol1PacketHandler.h | 4 +- .../dynamixel_sdk/Protocol2PacketHandler.h | 4 +- .../include/dynamixel_sdk/RobotisDef.h | 2 +- .../dynamixel_sdk_linux/PortHandlerLinux.h | 2 +- .../src/dynamixel_sdk/GroupBulkRead.cpp | 57 +- .../src/dynamixel_sdk/GroupBulkWrite.cpp | 5 +- .../src/dynamixel_sdk/GroupSyncRead.cpp | 68 +- .../src/dynamixel_sdk/GroupSyncWrite.cpp | 5 +- .../src/dynamixel_sdk/PacketHandler.cpp | 5 +- .../src/dynamixel_sdk/PortHandler.cpp | 7 +- .../dynamixel_sdk/Protocol1PacketHandler.cpp | 13 +- .../dynamixel_sdk/Protocol2PacketHandler.cpp | 13 +- .../dynamixel_sdk_linux/PortHandlerLinux.cpp | 2 +- robotis_controller/CMakeLists.txt | 4 +- .../robotis_controller/RobotisController.h | 22 +- .../robotis_controller/RobotisController.cpp | 724 ++++++++++++------ robotis_controller_msgs/CMakeLists.txt | 1 + robotis_controller_msgs/msg/StatusMsg.msg | 10 + robotis_device/CMakeLists.txt | 3 +- .../devices/dynamixel/GRIPPER.device | 30 +- .../devices/dynamixel/H42-20-S300-R.device | 32 +- .../devices/dynamixel/H54-100-S500-R.device | 32 +- .../devices/dynamixel/H54-200-B500-R.device | 32 +- .../devices/dynamixel/H54-200-S500-R.device | 32 +- .../devices/dynamixel/L42-10-S300-R.device | 30 +- .../devices/dynamixel/L54-30-S400-R.device | 32 +- .../devices/dynamixel/L54-30-S500-R.device | 32 +- .../devices/dynamixel/L54-50-S290-R.device | 32 +- .../devices/dynamixel/L54-50-S500-R.device | 32 +- .../devices/dynamixel/M42-10-S260-R.device | 32 +- .../devices/dynamixel/M54-40-S250-R.device | 32 +- .../devices/dynamixel/M54-60-S250-R.device | 32 +- robotis_device/devices/dynamixel/MX-28.device | 30 +- .../include/robotis_device/ControlTableItem.h | 4 +- .../include/robotis_device/Dynamixel.h | 31 +- .../include/robotis_device/DynamixelState.h | 42 +- robotis_device/include/robotis_device/Robot.h | 2 + .../src/robotis_device/Dynamixel.cpp | 47 +- robotis_device/src/robotis_device/Robot.cpp | 143 ++-- robotis_framework_common/CMakeLists.txt | 130 ---- .../robotis_framework_common/MotionModule.h | 21 +- .../robotis_framework_common/SensorModule.h | 37 + robotis_math/CMakeLists.txt | 65 ++ .../robotis_math/RobotisLinearAlgebra.h | 52 ++ .../include/robotis_math/RobotisMath.h | 13 + .../include/robotis_math/RobotisMathBase.h | 33 + .../RobotisTrajectoryCalculator.h | 37 + robotis_math/package.xml | 37 + robotis_math/src/RobotisLinearAlgebra.cpp | 291 +++++++ robotis_math/src/RobotisMathBase.cpp | 26 + .../src/RobotisTrajectoryCalculator.cpp | 325 ++++++++ 59 files changed, 1987 insertions(+), 789 deletions(-) create mode 100644 robotis_controller_msgs/msg/StatusMsg.msg create mode 100644 robotis_framework_common/include/robotis_framework_common/SensorModule.h create mode 100644 robotis_math/CMakeLists.txt create mode 100644 robotis_math/include/robotis_math/RobotisLinearAlgebra.h create mode 100644 robotis_math/include/robotis_math/RobotisMath.h create mode 100644 robotis_math/include/robotis_math/RobotisMathBase.h create mode 100644 robotis_math/include/robotis_math/RobotisTrajectoryCalculator.h create mode 100644 robotis_math/package.xml create mode 100644 robotis_math/src/RobotisLinearAlgebra.cpp create mode 100644 robotis_math/src/RobotisMathBase.cpp create mode 100644 robotis_math/src/RobotisTrajectoryCalculator.cpp diff --git a/dynamixel_sdk/CMakeLists.txt b/dynamixel_sdk/CMakeLists.txt index 4f16e17..b3b5c84 100644 --- a/dynamixel_sdk/CMakeLists.txt +++ b/dynamixel_sdk/CMakeLists.txt @@ -7,7 +7,7 @@ find_package(catkin REQUIRED COMPONENTS catkin_package( INCLUDE_DIRS include -# LIBRARIES dynamixel_sdk + LIBRARIES dynamixel_sdk # CATKIN_DEPENDS roscpp # DEPENDS system_lib ) diff --git a/dynamixel_sdk/include/DynamixelSDK.h b/dynamixel_sdk/include/DynamixelSDK.h index e64ae0d..0d34655 100644 --- a/dynamixel_sdk/include/DynamixelSDK.h +++ b/dynamixel_sdk/include/DynamixelSDK.h @@ -2,7 +2,7 @@ * DynamixelSDK.h * * Created on: 2016. 3. 8. - * Author: zerom + * Author: zerom, leon */ #ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_DYNAMIXELSDK_H_ diff --git a/dynamixel_sdk/include/dynamixel_sdk/GroupBulkRead.h b/dynamixel_sdk/include/dynamixel_sdk/GroupBulkRead.h index af8f99b..bd3374b 100644 --- a/dynamixel_sdk/include/dynamixel_sdk/GroupBulkRead.h +++ b/dynamixel_sdk/include/dynamixel_sdk/GroupBulkRead.h @@ -2,7 +2,7 @@ * GroupBulkRead.h * * Created on: 2016. 1. 28. - * Author: zerom + * Author: zerom, leon */ #ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPBULKREAD_H_ @@ -18,7 +18,7 @@ namespace ROBOTIS { -class GroupBulkRead +class WINDECLSPEC GroupBulkRead { private: PortHandler *port_; @@ -51,9 +51,8 @@ public: int RxPacket(); int TxRxPacket(); - bool GetData(UINT8_T id, UINT16_T address, UINT8_T *data); - bool GetData(UINT8_T id, UINT16_T address, UINT16_T *data); - bool GetData(UINT8_T id, UINT16_T address, UINT32_T *data); + bool IsAvailable (UINT8_T id, UINT16_T address, UINT16_T data_length); + UINT32_T GetData (UINT8_T id, UINT16_T address, UINT16_T data_length); }; } diff --git a/dynamixel_sdk/include/dynamixel_sdk/GroupBulkWrite.h b/dynamixel_sdk/include/dynamixel_sdk/GroupBulkWrite.h index 650bc25..3efbb80 100644 --- a/dynamixel_sdk/include/dynamixel_sdk/GroupBulkWrite.h +++ b/dynamixel_sdk/include/dynamixel_sdk/GroupBulkWrite.h @@ -2,7 +2,7 @@ * GroupBulkWrite.h * * Created on: 2016. 2. 2. - * Author: zerom + * Author: zerom, leon */ #ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPBULKWRITE_H_ @@ -18,7 +18,7 @@ namespace ROBOTIS { -class GroupBulkWrite +class WINDECLSPEC GroupBulkWrite { private: PortHandler *port_; diff --git a/dynamixel_sdk/include/dynamixel_sdk/GroupSyncRead.h b/dynamixel_sdk/include/dynamixel_sdk/GroupSyncRead.h index 40dac81..b6599de 100644 --- a/dynamixel_sdk/include/dynamixel_sdk/GroupSyncRead.h +++ b/dynamixel_sdk/include/dynamixel_sdk/GroupSyncRead.h @@ -2,7 +2,7 @@ * GroupSyncRead.h * * Created on: 2016. 2. 2. - * Author: zerom + * Author: zerom, leon */ #ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPSYNCREAD_H_ @@ -18,7 +18,7 @@ namespace ROBOTIS { -class GroupSyncRead +class WINDECLSPEC GroupSyncRead { private: PortHandler *port_; @@ -51,9 +51,8 @@ public: int RxPacket(); int TxRxPacket(); - bool GetData(UINT8_T id, UINT16_T address, UINT8_T *data); - bool GetData(UINT8_T id, UINT16_T address, UINT16_T *data); - bool GetData(UINT8_T id, UINT16_T address, UINT32_T *data); + bool IsAvailable (UINT8_T id, UINT16_T address, UINT16_T data_length); + UINT32_T GetData (UINT8_T id, UINT16_T address, UINT16_T data_length); }; } diff --git a/dynamixel_sdk/include/dynamixel_sdk/GroupSyncWrite.h b/dynamixel_sdk/include/dynamixel_sdk/GroupSyncWrite.h index 28e1458..1ff03b2 100644 --- a/dynamixel_sdk/include/dynamixel_sdk/GroupSyncWrite.h +++ b/dynamixel_sdk/include/dynamixel_sdk/GroupSyncWrite.h @@ -2,7 +2,7 @@ * GroupSyncWrite.h * * Created on: 2016. 1. 28. - * Author: zerom + * Author: zerom, leon */ #ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPSYNCWRITE_H_ @@ -18,7 +18,7 @@ namespace ROBOTIS { -class GroupSyncWrite +class WINDECLSPEC GroupSyncWrite { private: PortHandler *port_; diff --git a/dynamixel_sdk/include/dynamixel_sdk/PacketHandler.h b/dynamixel_sdk/include/dynamixel_sdk/PacketHandler.h index 6edb44e..921d380 100644 --- a/dynamixel_sdk/include/dynamixel_sdk/PacketHandler.h +++ b/dynamixel_sdk/include/dynamixel_sdk/PacketHandler.h @@ -2,7 +2,7 @@ * PacketHandler.h * * Created on: 2016. 1. 26. - * Author: zerom + * Author: zerom, leon */ #ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PACKETHANDLER_H_ @@ -54,7 +54,7 @@ namespace ROBOTIS { -class PacketHandler +class WINDECLSPEC PacketHandler { protected: PacketHandler() { } diff --git a/dynamixel_sdk/include/dynamixel_sdk/PortHandler.h b/dynamixel_sdk/include/dynamixel_sdk/PortHandler.h index 6920f8e..6d3e6dd 100644 --- a/dynamixel_sdk/include/dynamixel_sdk/PortHandler.h +++ b/dynamixel_sdk/include/dynamixel_sdk/PortHandler.h @@ -2,19 +2,28 @@ * PortHandler.h * * Created on: 2016. 1. 26. - * Author: zerom + * Author: zerom, leon */ #ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PORTHANDLER_H_ #define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PORTHANDLER_H_ +#ifdef __linux__ +#define WINDECLSPEC +#elif defined(_WIN32) || defined(_WIN64) +#ifdef WINDLLEXPORT +#define WINDECLSPEC __declspec(dllexport) +#else +#define WINDECLSPEC __declspec(dllimport) +#endif +#endif #include "RobotisDef.h" namespace ROBOTIS { -class PortHandler +class WINDECLSPEC PortHandler { public: static const int DEFAULT_BAUDRATE = 1000000; diff --git a/dynamixel_sdk/include/dynamixel_sdk/Protocol1PacketHandler.h b/dynamixel_sdk/include/dynamixel_sdk/Protocol1PacketHandler.h index 55182f4..1932d27 100644 --- a/dynamixel_sdk/include/dynamixel_sdk/Protocol1PacketHandler.h +++ b/dynamixel_sdk/include/dynamixel_sdk/Protocol1PacketHandler.h @@ -2,7 +2,7 @@ * Protocol1PacketHandler.h * * Created on: 2016. 1. 26. - * Author: zerom + * Author: zerom, leon */ #ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL1PACKETHANDLER_H_ @@ -14,7 +14,7 @@ namespace ROBOTIS { -class Protocol1PacketHandler : public PacketHandler +class WINDECLSPEC Protocol1PacketHandler : public PacketHandler { private: static Protocol1PacketHandler *unique_instance_; diff --git a/dynamixel_sdk/include/dynamixel_sdk/Protocol2PacketHandler.h b/dynamixel_sdk/include/dynamixel_sdk/Protocol2PacketHandler.h index 823c226..101413c 100644 --- a/dynamixel_sdk/include/dynamixel_sdk/Protocol2PacketHandler.h +++ b/dynamixel_sdk/include/dynamixel_sdk/Protocol2PacketHandler.h @@ -2,7 +2,7 @@ * Protocol2PacketHandler.h * * Created on: 2016. 1. 26. - * Author: zerom + * Author: zerom, leon */ #ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL2PACKETHANDLER_H_ @@ -14,7 +14,7 @@ namespace ROBOTIS { -class Protocol2PacketHandler : public PacketHandler +class WINDECLSPEC Protocol2PacketHandler : public PacketHandler { private: static Protocol2PacketHandler *unique_instance_; diff --git a/dynamixel_sdk/include/dynamixel_sdk/RobotisDef.h b/dynamixel_sdk/include/dynamixel_sdk/RobotisDef.h index 1fe2c61..95ab804 100644 --- a/dynamixel_sdk/include/dynamixel_sdk/RobotisDef.h +++ b/dynamixel_sdk/include/dynamixel_sdk/RobotisDef.h @@ -2,7 +2,7 @@ * RobotisDef.h * * Created on: 2016. 1. 27. - * Author: zerom + * Author: zerom, leon */ #ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_ROBOTISDEF_H_ diff --git a/dynamixel_sdk/include/dynamixel_sdk_linux/PortHandlerLinux.h b/dynamixel_sdk/include/dynamixel_sdk_linux/PortHandlerLinux.h index 02e4f63..6ff516e 100644 --- a/dynamixel_sdk/include/dynamixel_sdk_linux/PortHandlerLinux.h +++ b/dynamixel_sdk/include/dynamixel_sdk_linux/PortHandlerLinux.h @@ -2,7 +2,7 @@ * PortHandlerLinux.h * * Created on: 2016. 1. 26. - * Author: zerom + * Author: zerom, leon */ #ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_LINUX_PORTHANDLERLINUX_H_ diff --git a/dynamixel_sdk/src/dynamixel_sdk/GroupBulkRead.cpp b/dynamixel_sdk/src/dynamixel_sdk/GroupBulkRead.cpp index 2496fa0..d8f2b6d 100644 --- a/dynamixel_sdk/src/dynamixel_sdk/GroupBulkRead.cpp +++ b/dynamixel_sdk/src/dynamixel_sdk/GroupBulkRead.cpp @@ -2,8 +2,11 @@ * GroupBulkRead.cpp * * Created on: 2016. 1. 28. - * Author: zerom + * Author: zerom, leon */ +#if defined(_WIN32) || defined(_WIN64) +#define WINDLLEXPORT +#endif #include #include @@ -155,7 +158,7 @@ int GroupBulkRead::TxRxPacket() return RxPacket(); } -bool GroupBulkRead::GetData(UINT8_T id, UINT16_T address, UINT8_T *data) +bool GroupBulkRead::IsAvailable(UINT8_T id, UINT16_T address, UINT16_T data_length) { UINT16_T _start_addr, _data_length; @@ -165,48 +168,32 @@ bool GroupBulkRead::GetData(UINT8_T id, UINT16_T address, UINT8_T *data) _start_addr = address_list_[id]; _data_length = length_list_[id]; - if(address < _start_addr || _start_addr + _data_length - 1 < address) + if(address < _start_addr || _start_addr + _data_length - data_length < address) return false; - *data = data_list_[id][address - _start_addr]; - return true; } -bool GroupBulkRead::GetData(UINT8_T id, UINT16_T address, UINT16_T *data) +UINT32_T GroupBulkRead::GetData(UINT8_T id, UINT16_T address, UINT16_T data_length) { - UINT16_T _start_addr, _data_length; + if(IsAvailable(id, address, data_length) == false) + return 0; - if(last_result_ == false || data_list_.find(id) == data_list_.end()) - return false; + UINT16_T _start_addr = address_list_[id]; - _start_addr = address_list_[id]; - _data_length = length_list_[id]; + switch(data_length) + { + case 1: + return data_list_[id][address - _start_addr]; - if(address < _start_addr || _start_addr + _data_length - 2 < address) - return false; + case 2: + return DXL_MAKEWORD(data_list_[id][address - _start_addr], data_list_[id][address - _start_addr + 1]); - *data = DXL_MAKEWORD(data_list_[id][address - _start_addr], data_list_[id][address - _start_addr + 1]); + case 4: + return DXL_MAKEDWORD(DXL_MAKEWORD(data_list_[id][address - _start_addr + 0], data_list_[id][address - _start_addr + 1]), + DXL_MAKEWORD(data_list_[id][address - _start_addr + 2], data_list_[id][address - _start_addr + 3])); - return true; + default: + return 0; + } } - -bool GroupBulkRead::GetData(UINT8_T id, UINT16_T address, UINT32_T *data) -{ - UINT16_T _start_addr, _data_length; - - if(last_result_ == false || data_list_.find(id) == data_list_.end()) - return false; - - _start_addr = address_list_[id]; - _data_length = length_list_[id]; - - if(address < _start_addr || _start_addr + _data_length - 4 < address) - return false; - - *data = DXL_MAKEDWORD(DXL_MAKEWORD(data_list_[id][address - _start_addr + 0], data_list_[id][address - _start_addr + 1]), - DXL_MAKEWORD(data_list_[id][address - _start_addr + 2], data_list_[id][address - _start_addr + 3])); - - return true; -} - diff --git a/dynamixel_sdk/src/dynamixel_sdk/GroupBulkWrite.cpp b/dynamixel_sdk/src/dynamixel_sdk/GroupBulkWrite.cpp index 39e8eb4..399b32e 100644 --- a/dynamixel_sdk/src/dynamixel_sdk/GroupBulkWrite.cpp +++ b/dynamixel_sdk/src/dynamixel_sdk/GroupBulkWrite.cpp @@ -2,8 +2,11 @@ * GroupBulkWrite.cpp * * Created on: 2016. 2. 2. - * Author: zerom + * Author: zerom, leon */ +#if defined(_WIN32) || defined(_WIN64) +#define WINDLLEXPORT +#endif #include #include "dynamixel_sdk/GroupBulkWrite.h" diff --git a/dynamixel_sdk/src/dynamixel_sdk/GroupSyncRead.cpp b/dynamixel_sdk/src/dynamixel_sdk/GroupSyncRead.cpp index 1a89544..8e602e3 100644 --- a/dynamixel_sdk/src/dynamixel_sdk/GroupSyncRead.cpp +++ b/dynamixel_sdk/src/dynamixel_sdk/GroupSyncRead.cpp @@ -2,8 +2,11 @@ * GroupSyncRead.cpp * * Created on: 2016. 2. 2. - * Author: zerom + * Author: zerom, leon */ +#if defined(_WIN32) || defined(_WIN64) +#define WINDLLEXPORT +#endif #include #include "dynamixel_sdk/GroupSyncRead.h" @@ -138,50 +141,35 @@ int GroupSyncRead::TxRxPacket() return RxPacket(); } -bool GroupSyncRead::GetData(UINT8_T id, UINT16_T address, UINT8_T *data) +bool GroupSyncRead::IsAvailable(UINT8_T id, UINT16_T address, UINT16_T data_length) { - if(last_result_ == false || ph_->GetProtocolVersion() == 1.0) + if(ph_->GetProtocolVersion() == 1.0 || last_result_ == false || data_list_.find(id) == data_list_.end()) return false; - if(data_list_.find(id) == data_list_.end()) + if(address < start_address_ || start_address_ + data_length_ - data_length < address) return false; - if(address < start_address_ || start_address_ + data_length_ - 1 < address) - return false; - - *data = data_list_[id][address - start_address_]; - - return true; -} -bool GroupSyncRead::GetData(UINT8_T id, UINT16_T address, UINT16_T *data) -{ - if(last_result_ == false || ph_->GetProtocolVersion() == 1.0) - return false; - - if(data_list_.find(id) == data_list_.end()) - return false; - - if(address < start_address_ || start_address_ + data_length_ - 2 < address) - return false; - - *data = DXL_MAKEWORD(data_list_[id][address - start_address_], data_list_[id][address - start_address_ + 1]); - - return true; -} -bool GroupSyncRead::GetData(UINT8_T id, UINT16_T address, UINT32_T *data) -{ - if(last_result_ == false || ph_->GetProtocolVersion() == 1.0) - return false; - - if(data_list_.find(id) == data_list_.end()) - return false; - - if(address < start_address_ || start_address_ + data_length_ - 4 < address) - return false; - - *data = DXL_MAKEDWORD(DXL_MAKEWORD(data_list_[id][address - start_address_ + 0], data_list_[id][address - start_address_ + 1]), - DXL_MAKEWORD(data_list_[id][address - start_address_ + 2], data_list_[id][address - start_address_ + 3])); - return true; } +UINT32_T GroupSyncRead::GetData(UINT8_T id, UINT16_T address, UINT16_T data_length) +{ + if(IsAvailable(id, address, data_length) == false) + return 0; + + switch(data_length) + { + case 1: + return data_list_[id][address - start_address_]; + + case 2: + return DXL_MAKEWORD(data_list_[id][address - start_address_], data_list_[id][address - start_address_ + 1]); + + case 4: + return DXL_MAKEDWORD(DXL_MAKEWORD(data_list_[id][address - start_address_ + 0], data_list_[id][address - start_address_ + 1]), + DXL_MAKEWORD(data_list_[id][address - start_address_ + 2], data_list_[id][address - start_address_ + 3])); + + default: + return 0; + } +} diff --git a/dynamixel_sdk/src/dynamixel_sdk/GroupSyncWrite.cpp b/dynamixel_sdk/src/dynamixel_sdk/GroupSyncWrite.cpp index 38d381b..45d90f8 100644 --- a/dynamixel_sdk/src/dynamixel_sdk/GroupSyncWrite.cpp +++ b/dynamixel_sdk/src/dynamixel_sdk/GroupSyncWrite.cpp @@ -2,8 +2,11 @@ * GroupSyncWrite.cpp * * Created on: 2016. 1. 28. - * Author: zerom + * Author: zerom, leon */ +#if defined(_WIN32) || defined(_WIN64) +#define WINDLLEXPORT +#endif #include #include "dynamixel_sdk/GroupSyncWrite.h" diff --git a/dynamixel_sdk/src/dynamixel_sdk/PacketHandler.cpp b/dynamixel_sdk/src/dynamixel_sdk/PacketHandler.cpp index 57579b0..8ad1c48 100644 --- a/dynamixel_sdk/src/dynamixel_sdk/PacketHandler.cpp +++ b/dynamixel_sdk/src/dynamixel_sdk/PacketHandler.cpp @@ -2,8 +2,11 @@ * PacketHandler.cpp * * Created on: 2016. 1. 26. - * Author: zerom + * Author: zerom, leon */ +#if defined(_WIN32) || defined(_WIN64) +#define WINDLLEXPORT +#endif #include "dynamixel_sdk/PacketHandler.h" #include "dynamixel_sdk/Protocol1PacketHandler.h" diff --git a/dynamixel_sdk/src/dynamixel_sdk/PortHandler.cpp b/dynamixel_sdk/src/dynamixel_sdk/PortHandler.cpp index 3bce1bf..ff4355c 100644 --- a/dynamixel_sdk/src/dynamixel_sdk/PortHandler.cpp +++ b/dynamixel_sdk/src/dynamixel_sdk/PortHandler.cpp @@ -2,8 +2,11 @@ * PortHandler.cpp * * Created on: 2016. 2. 5. - * Author: zerom + * Author: zerom, leon */ +#if defined(_WIN32) || defined(_WIN64) +#define WINDLLEXPORT +#endif #include "dynamixel_sdk/PortHandler.h" @@ -27,5 +30,3 @@ PortHandler *PortHandler::GetPortHandler(const char *port_name) return (PortHandler *)(new PortHandlerWindows(port_name)); #endif } - - diff --git a/dynamixel_sdk/src/dynamixel_sdk/Protocol1PacketHandler.cpp b/dynamixel_sdk/src/dynamixel_sdk/Protocol1PacketHandler.cpp index 630afe0..66877e7 100644 --- a/dynamixel_sdk/src/dynamixel_sdk/Protocol1PacketHandler.cpp +++ b/dynamixel_sdk/src/dynamixel_sdk/Protocol1PacketHandler.cpp @@ -2,8 +2,11 @@ * Protocol1PacketHandler.cpp * * Created on: 2016. 1. 26. - * Author: zerom + * Author: zerom, leon */ +#if defined(_WIN32) || defined(_WIN64) +#define WINDLLEXPORT +#endif #include #include @@ -180,7 +183,12 @@ int Protocol1PacketHandler::RxPacket(PortHandler *port, UINT8_T *rxpacket) } // re-calculate the exact length of the rx packet - _wait_length = rxpacket[PKT_LENGTH] + PKT_LENGTH + 1; + if(_wait_length != rxpacket[PKT_LENGTH] + PKT_LENGTH + 1) + { + _wait_length = rxpacket[PKT_LENGTH] + PKT_LENGTH + 1; + continue; + } + if(_rx_length < _wait_length) { // check timeout @@ -660,4 +668,3 @@ int Protocol1PacketHandler::BulkWriteTxOnly(PortHandler *port, UINT8_T *param, U { return COMM_NOT_AVAILABLE; } - diff --git a/dynamixel_sdk/src/dynamixel_sdk/Protocol2PacketHandler.cpp b/dynamixel_sdk/src/dynamixel_sdk/Protocol2PacketHandler.cpp index 39cba41..771fee7 100644 --- a/dynamixel_sdk/src/dynamixel_sdk/Protocol2PacketHandler.cpp +++ b/dynamixel_sdk/src/dynamixel_sdk/Protocol2PacketHandler.cpp @@ -2,9 +2,13 @@ * Protocol2PacketHandler.cpp * * Created on: 2016. 1. 26. - * Author: zerom + * Author: zerom, leon */ +#if defined(_WIN32) || defined(_WIN64) +#define WINDLLEXPORT +#endif + #include #include #include @@ -326,7 +330,12 @@ int Protocol2PacketHandler::RxPacket(PortHandler *port, UINT8_T *rxpacket) } // re-calculate the exact length of the rx packet - _wait_length = DXL_MAKEWORD(rxpacket[PKT_LENGTH_L], rxpacket[PKT_LENGTH_H]) + PKT_LENGTH_H + 1; + if(_wait_length != DXL_MAKEWORD(rxpacket[PKT_LENGTH_L], rxpacket[PKT_LENGTH_H]) + PKT_LENGTH_H + 1) + { + _wait_length = DXL_MAKEWORD(rxpacket[PKT_LENGTH_L], rxpacket[PKT_LENGTH_H]) + PKT_LENGTH_H + 1; + continue; + } + if(_rx_length < _wait_length) { // check timeout diff --git a/dynamixel_sdk/src/dynamixel_sdk_linux/PortHandlerLinux.cpp b/dynamixel_sdk/src/dynamixel_sdk_linux/PortHandlerLinux.cpp index 8fb8b02..96c3f75 100644 --- a/dynamixel_sdk/src/dynamixel_sdk_linux/PortHandlerLinux.cpp +++ b/dynamixel_sdk/src/dynamixel_sdk_linux/PortHandlerLinux.cpp @@ -2,7 +2,7 @@ * PortHandlerLinux.cpp * * Created on: 2016. 1. 26. - * Author: zerom + * Author: zerom, leon */ #include diff --git a/robotis_controller/CMakeLists.txt b/robotis_controller/CMakeLists.txt index a41b0d8..5865116 100644 --- a/robotis_controller/CMakeLists.txt +++ b/robotis_controller/CMakeLists.txt @@ -24,7 +24,7 @@ find_package(catkin REQUIRED COMPONENTS ## DEPENDS: system dependencies of this project that dependent projects also need catkin_package( INCLUDE_DIRS include -# LIBRARIES robotis_controller + LIBRARIES robotis_controller CATKIN_DEPENDS roscpp roslib sensor_msgs std_msgs # DEPENDS system_lib ) @@ -49,7 +49,5 @@ add_library(robotis_controller ## Specify libraries to link a library or executable target against target_link_libraries(robotis_controller yaml-cpp - robotis_device - dynamixel_sdk ${catkin_LIBRARIES} ) diff --git a/robotis_controller/include/robotis_controller/RobotisController.h b/robotis_controller/include/robotis_controller/RobotisController.h index 07b6716..6db923b 100644 --- a/robotis_controller/include/robotis_controller/RobotisController.h +++ b/robotis_controller/include/robotis_controller/RobotisController.h @@ -18,6 +18,7 @@ #include "robotis_device/Robot.h" #include "robotis_framework_common/MotionModule.h" +#include "robotis_framework_common/SensorModule.h" #include "robotis_controller_msgs/SyncWriteItem.h" #include "robotis_controller_msgs/JointCtrlModule.h" @@ -43,6 +44,7 @@ private: boost::thread queue_thread_; boost::thread gazebo_thread_; + boost::thread set_module_thread_; boost::mutex queue_mutex_; bool init_pose_loaded_; @@ -51,13 +53,17 @@ private: pthread_t timer_thread_; CONTROLLER_MODE controller_mode_; - std::list modules_; + std::list motion_modules_; + std::list sensor_modules_; std::vector direct_sync_write_; + std::map sensor_result_; + RobotisController(); void QueueThread(); void GazeboThread(); + void SetCtrlModuleThread(std::string ctrl_module); bool CheckTimerStop(); void InitSyncWrite(); @@ -73,7 +79,9 @@ public: // TODO: TEMPORARY CODE !! std::map port_to_bulk_read; - std::map port_to_sync_write; + std::map port_to_sync_write_position; + std::map port_to_sync_write_velocity; + std::map port_to_sync_write_torque; /* publisher */ ros::Publisher goal_joint_state_pub; @@ -87,8 +95,11 @@ public: bool Initialize(const std::string robot_file_path, const std::string init_file_path); void Process(); - void AddModule(MotionModule *module); - void RemoveModule(MotionModule *module); + + void AddMotionModule(MotionModule *module); + void RemoveMotionModule(MotionModule *module); + void AddSensorModule(SensorModule *module); + void RemoveSensorModule(SensorModule *module); void StartTimer(); void StopTimer(); @@ -100,7 +111,8 @@ public: void SyncWriteItemCallback(const robotis_controller_msgs::SyncWriteItem::ConstPtr &msg); void SetControllerModeCallback(const std_msgs::String::ConstPtr &msg); void SetJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg); - void SetCtrlModuleCallback(const robotis_controller_msgs::JointCtrlModule::ConstPtr &msg); + void SetJointCtrlModuleCallback(const robotis_controller_msgs::JointCtrlModule::ConstPtr &msg); + void SetCtrlModuleCallback(const std_msgs::String::ConstPtr &msg); bool GetCtrlModuleCallback(robotis_controller_msgs::GetJointModule::Request &req, robotis_controller_msgs::GetJointModule::Response &res); void GazeboJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg); diff --git a/robotis_controller/src/robotis_controller/RobotisController.cpp b/robotis_controller/src/robotis_controller/RobotisController.cpp index 7f93f94..20855f6 100644 --- a/robotis_controller/src/robotis_controller/RobotisController.cpp +++ b/robotis_controller/src/robotis_controller/RobotisController.cpp @@ -10,31 +10,10 @@ #include "robotis_controller/RobotisController.h" -#define INDIRECT_ADDRESS "indirect_address_1" - using namespace ROBOTIS; RobotisController *RobotisController::unique_instance_ = new RobotisController(); -// INDIRECT ADDR : 634 -> Present Position (4 Byte) -// INDIRECT ADDR : 638 -> Present Velocity (4 Byte) -// INDIRECT ADDR : 642 -> Present Current (2 Byte) -// INDIRECT ADDR : 644 -> Input Voltage (2 Byte) -// INDIRECT ADDR : 646 -> Temperature (1 byte) -// INDIRECT ADDR : 647 -> External Port Data 1 (2 byte) -// INDIRECT ADDR : 649 -> External Port Data 2 (2 byte) -// INDIRECT ADDR : 651 -> External Port Data 3 (2 byte) -// INDIRECT ADDR : 653 -> External Port Data 4 (2 byte) -const UINT16_T PRESENT_POSITION_ADDR = 634; -const UINT16_T TORQUE_ENABLE_ADDR = 562; -const UINT16_T GOAL_POSITION_ADDR = 596; -const UINT16_T GOAL_VELOCITY_ADDR = 600; -const UINT16_T GOAL_ACCELERATION_ADDR = 606; -const UINT16_T EXT_PORT_DATA1_ADDR = 647; -const UINT16_T EXT_PORT_DATA2_ADDR = 649; -const UINT16_T EXT_PORT_DATA3_ADDR = 651; -const UINT16_T EXT_PORT_DATA4_ADDR = 653; - RobotisController::RobotisController() : is_timer_running_(false), stop_timer_(false), @@ -76,32 +55,74 @@ void RobotisController::InitSyncWrite() ROS_INFO("FIRST BULKREAD END"); // clear syncwrite param setting - for(std::map::iterator _it = port_to_sync_write.begin(); _it != port_to_sync_write.end(); _it++) + for(std::map::iterator _it = port_to_sync_write_position.begin(); _it != port_to_sync_write_position.end(); _it++) + _it->second->ClearParam(); + for(std::map::iterator _it = port_to_sync_write_velocity.begin(); _it != port_to_sync_write_velocity.end(); _it++) + _it->second->ClearParam(); + for(std::map::iterator _it = port_to_sync_write_torque.begin(); _it != port_to_sync_write_torque.end(); _it++) _it->second->ClearParam(); // set init syncwrite param(from data of bulkread) for(std::map::iterator _it = robot->dxls.begin(); _it != robot->dxls.end(); _it++) { - INT32_T _pos = 0; - std::string _joint_name = _it->first; Dynamixel *_dxl = _it->second; - bool _res = false; - _res = port_to_bulk_read[_dxl->port_name]->GetData(_dxl->id, PRESENT_POSITION_ADDR, (UINT32_T*)&_pos); - if(_res == true) + CONTROL_MODE _ctrl_mode = POSITION_CONTROL; + + for(std::list::iterator _m_it = motion_modules_.begin(); _m_it != motion_modules_.end(); _m_it++) { - _dxl->dxl_state->present_position = _dxl->ConvertValue2Radian(_pos) - _dxl->dxl_state->position_offset; // remove offset; - _dxl->dxl_state->goal_position = _dxl->dxl_state->present_position; + if((*_m_it)->module_name == _dxl->ctrl_module_name) + { + _ctrl_mode = (*_m_it)->control_mode; + break; + } } - UINT8_T _sync_write_data[4]; - _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_pos)); - _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_pos)); - _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_pos)); - _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_pos)); + for(int _i = 0; _i < _dxl->bulk_read_items.size(); _i++) + { + UINT32_T _read_data = 0; + UINT8_T _sync_write_data[4]; - port_to_sync_write[_dxl->port_name]->AddParam(_dxl->id, _sync_write_data); + if(port_to_bulk_read[_dxl->port_name]->IsAvailable(_dxl->id, + _dxl->bulk_read_items[_i]->address, + _dxl->bulk_read_items[_i]->data_length) == true) + { + _read_data = port_to_bulk_read[_dxl->port_name]->GetData(_dxl->id, + _dxl->bulk_read_items[_i]->address, + _dxl->bulk_read_items[_i]->data_length); + + _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_read_data)); + _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_read_data)); + _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_read_data)); + _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_read_data)); + + if(_dxl->present_position_item != 0 && _dxl->bulk_read_items[_i]->item_name == _dxl->present_position_item->item_name) + { + _dxl->dxl_state->present_position = _dxl->ConvertValue2Radian(_read_data) - _dxl->dxl_state->position_offset; // remove offset + _dxl->dxl_state->goal_position = _dxl->dxl_state->present_position; + + if(_ctrl_mode == POSITION_CONTROL) + port_to_sync_write_position[_dxl->port_name]->AddParam(_dxl->id, _sync_write_data); + } + else if(_dxl->present_velocity_item != 0 && _dxl->bulk_read_items[_i]->item_name == _dxl->present_velocity_item->item_name) + { + _dxl->dxl_state->present_velocity = _dxl->ConvertValue2Velocity(_read_data); + _dxl->dxl_state->goal_velocity = _dxl->dxl_state->present_velocity; + + if(_ctrl_mode == VELOCITY_CONTROL) + port_to_sync_write_velocity[_dxl->port_name]->AddParam(_dxl->id, _sync_write_data); + } + else if(_dxl->present_current_item != 0 && _dxl->bulk_read_items[_i]->item_name == _dxl->present_current_item->item_name) + { + _dxl->dxl_state->present_current = _dxl->ConvertValue2Current(_read_data); + _dxl->dxl_state->goal_current = _dxl->dxl_state->present_current; + + if(_ctrl_mode == CURRENT_CONTROL) + port_to_sync_write_torque[_dxl->port_name]->AddParam(_dxl->id, _sync_write_data); + } + } + } } } @@ -120,18 +141,40 @@ bool RobotisController::Initialize(const std::string robot_file_path, const std: // TODO: TEMPORARY CODE !! /* temporary code start */ - PacketHandler *_protocol2_handler = PacketHandler::GetPacketHandler(2.0); - for(std::map::iterator _it = robot->ports.begin(); _it != robot->ports.end(); _it++) { - if(_it->second->SetBaudRate(_it->second->GetBaudRate()) == false) + std::string _port_name = _it->first; + PortHandler *_port = _it->second; + + PacketHandler *_default_pkt_handler = PacketHandler::GetPacketHandler(2.0); + + if(_port->SetBaudRate(_port->GetBaudRate()) == false) { - ROS_ERROR("PORT [%s] SETUP ERROR! (baudrate: %d)", _it->first.c_str(), _it->second->GetBaudRate()); + ROS_ERROR("PORT [%s] SETUP ERROR! (baudrate: %d)", _port_name.c_str(), _port->GetBaudRate()); exit(-1); } - port_to_bulk_read[_it->first] = new GroupBulkRead(_it->second, _protocol2_handler); - port_to_sync_write[_it->first] = new GroupSyncWrite(_it->second, _protocol2_handler, GOAL_POSITION_ADDR, 4); + Dynamixel *_port_default_dxl = robot->dxls[robot->port_default_joint[_port_name]]; + if(_port_default_dxl != NULL) + _default_pkt_handler = PacketHandler::GetPacketHandler(_port_default_dxl->protocol_version); + + port_to_bulk_read[_port_name] = new GroupBulkRead(_port, _default_pkt_handler); + + port_to_sync_write_position[_port_name] = new GroupSyncWrite(_port, + _default_pkt_handler, + _port_default_dxl->goal_position_item->address, + _port_default_dxl->goal_position_item->data_length); + + port_to_sync_write_velocity[_port_name] = new GroupSyncWrite(_port, + _default_pkt_handler, + _port_default_dxl->goal_velocity_item->address, + _port_default_dxl->goal_velocity_item->data_length); + + port_to_sync_write_torque[_port_name] = new GroupSyncWrite(_port, + _default_pkt_handler, + _port_default_dxl->goal_current_item->address, + _port_default_dxl->goal_current_item->data_length); + } // TODO: @@ -140,6 +183,7 @@ bool RobotisController::Initialize(const std::string robot_file_path, const std: { std::string _joint_name = _it->first; Dynamixel *_dxl = _it->second; + if(Ping(_joint_name) != 0) { usleep(10*1000); @@ -180,64 +224,64 @@ bool RobotisController::Initialize(const std::string robot_file_path, const std: std::string _item_name = _it_joint->first.as(); if(DEBUG_PRINT) ROS_INFO(" ITEM_NAME: %s", _item_name.c_str()); - // indirect address setting - if(_item_name == INDIRECT_ADDRESS) + + UINT32_T _value = _it_joint->second.as(); + + ControlTableItem *_item = _dxl->ctrl_table[_item_name]; + if(_item == NULL) { - YAML::Node _indirect_node = _joint_node[_item_name]; - if(_indirect_node.size() == 0) - continue; - - for(YAML::const_iterator _it_idx = _indirect_node.begin(); _it_idx != _indirect_node.end(); _it_idx++) - { - int _start_idx = _it_idx->first.as(); - if(_start_idx < 1 || 256 < _start_idx) - ROS_WARN("[%s] INDIRECT ADDRESS start index is out of range. (%d)", _joint_name.c_str(), _start_idx); - if(DEBUG_PRINT) ROS_INFO(" START_IDX: %d", _start_idx); - - YAML::Node _indirect_item_node = _indirect_node[_start_idx]; - if(_indirect_item_node.size() == 0) - continue; - - int _start_address = _dxl->ctrl_table[INDIRECT_ADDRESS]->address + _start_idx - 1; - for(unsigned int _i = 0; _i < _indirect_item_node.size(); _i++) - { - std::string _indir_item_name = _indirect_item_node[_i].as().c_str(); - int _addr_leng = _dxl->ctrl_table[_indir_item_name]->data_length; - for(int _l = 0; _l < _addr_leng; _l++) - { - if(DEBUG_PRINT) ROS_INFO(" - INDIR_ADDR[%d] : %d", _start_address, _dxl->ctrl_table[_indir_item_name]->address + _l); - Write2Byte(_joint_name, _start_address, _dxl->ctrl_table[_indir_item_name]->address + _l); - _start_address += 2; - } - } - } + ROS_WARN("Control Item [%s] does not found.", _item_name.c_str()); + continue; } - else // other items setting - { - UINT32_T _value = _it_joint->second.as(); - ControlTableItem *_item = _dxl->ctrl_table[_item_name]; - if(_item == NULL) - { - ROS_WARN("Control Item [%s] does not found.", _item_name.c_str()); - continue; - } + if(_item->memory_type == EEPROM) + { + UINT8_T _data8 = 0; + UINT16_T _data16 = 0; + UINT32_T _data32 = 0; switch(_item->data_length) { case 1: - Write1Byte(_joint_name, _item->address, (UINT8_T)_value); + Read1Byte(_joint_name, _item->address, &_data8); + if(_data8 == _value) + continue; break; case 2: - Write2Byte(_joint_name, _item->address, (UINT16_T)_value); + Read2Byte(_joint_name, _item->address, &_data16); + if(_data16 == _value) + continue; break; case 4: - Write4Byte(_joint_name, _item->address, _value); + Read4Byte(_joint_name, _item->address, &_data32); + if(_data32 == _value) + continue; break; default: break; } } + + switch(_item->data_length) + { + case 1: + Write1Byte(_joint_name, _item->address, (UINT8_T)_value); + break; + case 2: + Write2Byte(_joint_name, _item->address, (UINT16_T)_value); + break; + case 4: + Write4Byte(_joint_name, _item->address, _value); + break; + default: + break; + } + + if(_item->memory_type == EEPROM) + { + // Write to EEPROM -> delay is required (max delay: 55 msec per byte) + usleep(_item->data_length * 55 * 1000); + } } } } catch(const std::exception& e) { @@ -254,27 +298,69 @@ bool RobotisController::Initialize(const std::string robot_file_path, const std: if(_dxl == NULL) continue; - UINT16_T _data_length = 0; - if(_dxl->id == 11 || _dxl->id == 12 || _dxl->id == 23 || _dxl->id == 24) - _data_length = 17; - else if(_dxl->id == 13 || _dxl->id == 14 || _dxl->id == 25 || _dxl->id == 26) - _data_length = 21; - else - _data_length = 13; - port_to_bulk_read[_dxl->port_name]->AddParam(robot->dxls[_joint_name]->id, PRESENT_POSITION_ADDR, _data_length); + int _bulkread_start_addr = 0; + int _bulkread_data_length = 0; + + // bulk read default : present position + if(_dxl->present_position_item != 0) + { + _bulkread_start_addr = _dxl->present_position_item->address; + _bulkread_data_length = _dxl->present_position_item->data_length; + } + + // TODO: modifing + std::map::iterator _indirect_addr_it = _dxl->ctrl_table.find(INDIRECT_ADDRESS_1); + if(_indirect_addr_it != _dxl->ctrl_table.end()) // INDIRECT_ADDRESS_1 exist + { + if(_dxl->bulk_read_items.size() != 0) + { + _bulkread_start_addr = _dxl->bulk_read_items[0]->address; + _bulkread_data_length = 0; + + // set indirect address + int _indirect_addr = _indirect_addr_it->second->address; + for(int _i = 0; _i < _dxl->bulk_read_items.size(); _i++) + { + int _addr_leng = _dxl->bulk_read_items[_i]->data_length; + + _bulkread_data_length += _addr_leng; + for(int _l = 0; _l < _addr_leng; _l++) + { +// ROS_WARN("[%12s] INDIR_ADDR: %d, ITEM_ADDR: %d", _joint_name.c_str(), _indirect_addr, _dxl->ctrl_table[_dxl->bulk_read_items[_i]->item_name]->address + _l); + Write2Byte(_joint_name, _indirect_addr, _dxl->ctrl_table[_dxl->bulk_read_items[_i]->item_name]->address + _l); + _indirect_addr += 2; + } + } + } + } + else // INDIRECT_ADDRESS_1 NOT exist + { + if(_dxl->bulk_read_items.size() != 0) + { + _bulkread_start_addr = _dxl->bulk_read_items[0]->address; + _bulkread_data_length = 0; + + ControlTableItem *_last_item = _dxl->bulk_read_items[0]; + + for(int _i = 0; _i < _dxl->bulk_read_items.size(); _i++) + { + int _addr = _dxl->bulk_read_items[_i]->address; + if(_addr < _bulkread_start_addr) + _bulkread_start_addr = _addr; + else if(_last_item->address < _addr) + _last_item = _dxl->bulk_read_items[_i]; + } + + _bulkread_data_length = _last_item->address - _bulkread_start_addr + _last_item->data_length; + } + } + +// ROS_WARN("[%12s] start_addr: %d, data_length: %d", _joint_name.c_str(), _bulkread_start_addr, _bulkread_data_length); + port_to_bulk_read[_dxl->port_name]->AddParam(_dxl->id, _bulkread_start_addr, _bulkread_data_length); // Torque ON - _protocol2_handler->Write1ByteTxRx(robot->ports[_dxl->port_name], robot->dxls[_joint_name]->id, TORQUE_ENABLE_ADDR, 1); - } - - //InitSyncWrite(); - - for(std::map::iterator _it = robot->ports.begin(); _it != robot->ports.end(); _it++) - { - // set goal velocity = 0 - _protocol2_handler->Write4ByteTxOnly(_it->second, BROADCAST_ID, GOAL_VELOCITY_ADDR, 0); - // set goal acceleration = 0 - _protocol2_handler->Write4ByteTxOnly(_it->second, BROADCAST_ID, GOAL_ACCELERATION_ADDR, 0); + if(WriteCtrlItem(_joint_name, _dxl->torque_enable_item->item_name, 1) != COMM_SUCCESS) + WriteCtrlItem(_joint_name, _dxl->torque_enable_item->item_name, 1); } /* temporary code end */ @@ -303,15 +389,16 @@ void RobotisController::QueueThread() _ros_node.setCallbackQueue(&_callback_queue); /* subscriber */ - ros::Subscriber _sync_write_item_sub= _ros_node.subscribe("/robotis/sync_write_item", 10, &RobotisController::SyncWriteItemCallback, this); - ros::Subscriber _ctrl_module_sub = _ros_node.subscribe("/robotis/set_ctrl_module", 10, &RobotisController::SetCtrlModuleCallback, this); - ros::Subscriber _controller_mode_sub= _ros_node.subscribe("/robotis/set_controller_mode", 10, &RobotisController::SetControllerModeCallback, this); - ros::Subscriber _direct_control_sub = _ros_node.subscribe("/robotis/set_joint_states", 10, &RobotisController::SetJointStatesCallback, this); + ros::Subscriber _sync_write_item_sub = _ros_node.subscribe("/robotis/sync_write_item", 10, &RobotisController::SyncWriteItemCallback, this); + ros::Subscriber _joint_ctrl_modules_sub = _ros_node.subscribe("/robotis/set_joint_ctrl_modules", 10, &RobotisController::SetJointCtrlModuleCallback, this); + ros::Subscriber _enable_ctrl_module_sub = _ros_node.subscribe("/robotis/enable_ctrl_module", 10, &RobotisController::SetCtrlModuleCallback, this); + ros::Subscriber _control_mode_sub = _ros_node.subscribe("/robotis/set_control_mode", 10, &RobotisController::SetControllerModeCallback, this); + ros::Subscriber _joint_states_sub = _ros_node.subscribe("/robotis/set_joint_states", 10, &RobotisController::SetJointStatesCallback, this); /* publisher */ - goal_joint_state_pub = _ros_node.advertise("/robotis/goal_joint_states", 10); - present_joint_state_pub = _ros_node.advertise("/robotis/present_joint_states", 10); - current_module_pub = _ros_node.advertise("/robotis/current_ctrl_module", 10); + goal_joint_state_pub = _ros_node.advertise("/robotis/goal_joint_states", 10); + present_joint_state_pub = _ros_node.advertise("/robotis/present_joint_states", 10); + current_module_pub = _ros_node.advertise("/robotis/present_joint_ctrl_modules", 10); ros::Subscriber _gazebo_joint_states_sub; if(gazebo_mode == true) @@ -323,7 +410,7 @@ void RobotisController::QueueThread() } /* service */ - ros::ServiceServer _joint_module_server = _ros_node.advertiseService("/robotis/get_ctrl_module", &RobotisController::GetCtrlModuleCallback, this); + ros::ServiceServer _joint_module_server = _ros_node.advertiseService("/robotis/get_present_joint_ctrl_modules", &RobotisController::GetCtrlModuleCallback, this); while(_ros_node.ok()) { @@ -428,7 +515,11 @@ void RobotisController::StopTimer() for(std::map::iterator _it = port_to_bulk_read.begin(); _it != port_to_bulk_read.end(); _it++) _it->second->RxPacket(); - for(std::map::iterator _it = port_to_sync_write.begin(); _it != port_to_sync_write.end(); _it++) + for(std::map::iterator _it = port_to_sync_write_position.begin(); _it != port_to_sync_write_position.end(); _it++) + _it->second->ClearParam(); + for(std::map::iterator _it = port_to_sync_write_velocity.begin(); _it != port_to_sync_write_velocity.end(); _it++) + _it->second->ClearParam(); + for(std::map::iterator _it = port_to_sync_write_torque.begin(); _it != port_to_sync_write_torque.end(); _it++) _it->second->ClearParam(); } else @@ -475,106 +566,129 @@ void RobotisController::LoadOffset(const std::string path) void RobotisController::Process() { + // avoid duplicated function call static bool _is_process_running = false; - if(_is_process_running == true) return; _is_process_running = true; // ROS_INFO("Controller::Process()"); - - sensor_msgs::JointState _present_state; - sensor_msgs::JointState _goal_state; - - ros::Time _now = ros::Time::now(); - - - // TODO: BulkRead Rx bool _do_sync_write = false; + sensor_msgs::JointState _goal_state; + sensor_msgs::JointState _present_state; + + +// ros::Time _now = ros::Time::now(); + + + // BulkRead Rx if(gazebo_mode == false) { for(std::map::iterator _it = port_to_bulk_read.begin(); _it != port_to_bulk_read.end(); _it++) _it->second->RxPacket(); } - ros::Duration _dur = ros::Time::now() - _now; - double _msec = _dur.nsec * 0.000001; - - if(_msec > 8) std::cout << "Process duration : " << _msec << std::endl; +// ros::Duration _dur = ros::Time::now() - _now; +// double _msec = _dur.nsec * 0.000001; +// +// if(_msec > 8) std::cout << "Process duration : " << _msec << std::endl; // -> save to Robot->dxls[]->dynamixel_state.present_position - // -> update time stamp to Robot->dxls[]->dynamixel_state.update_time_stamp for(std::map::iterator dxl_it = robot->dxls.begin(); dxl_it != robot->dxls.end(); dxl_it++) { - UINT32_T _pos = 0; + UINT32_T _data = 0; + std::string _port_name = dxl_it->second->port_name; std::string _joint_name = dxl_it->first; - Dynamixel *_dxl = dxl_it->second; + Dynamixel *_dxl = dxl_it->second; + + _present_state.header.stamp = ros::Time::now(); + _goal_state.header.stamp = _present_state.header.stamp; if(gazebo_mode == false) { - if(port_to_bulk_read[_dxl->port_name]->GetData(_dxl->id, PRESENT_POSITION_ADDR, &_pos) == true) - _dxl->dxl_state->present_position = _dxl->ConvertValue2Radian(_pos) - _dxl->dxl_state->position_offset; // remove offset + for(int _i = 0; _i < _dxl->bulk_read_items.size(); _i++) + { + ControlTableItem *_item = _dxl->bulk_read_items[_i]; + if(port_to_bulk_read[_port_name]->IsAvailable(_dxl->id, _item->address, _item->data_length) == true) + { + _data = port_to_bulk_read[_port_name]->GetData(_dxl->id, _item->address, _item->data_length); - UINT16_T _ext_data = 0; - if(_dxl->id == 11 || _dxl->id == 12 || _dxl->id == 23 || _dxl->id == 24) - { - if(port_to_bulk_read[_dxl->port_name]->GetData(_dxl->id, EXT_PORT_DATA1_ADDR, &_ext_data) == true) - _dxl->dxl_state->ext_port_data[0] = _ext_data; - if(port_to_bulk_read[_dxl->port_name]->GetData(_dxl->id, EXT_PORT_DATA2_ADDR, &_ext_data) == true) - _dxl->dxl_state->ext_port_data[1] = _ext_data; - } - else if(_dxl->id == 13 || _dxl->id == 14 || _dxl->id == 25 || _dxl->id == 26) - { - if(port_to_bulk_read[_dxl->port_name]->GetData(_dxl->id, EXT_PORT_DATA1_ADDR, &_ext_data) == true) - _dxl->dxl_state->ext_port_data[0] = _ext_data; - if(port_to_bulk_read[_dxl->port_name]->GetData(_dxl->id, EXT_PORT_DATA2_ADDR, &_ext_data) == true) - _dxl->dxl_state->ext_port_data[1] = _ext_data; - if(port_to_bulk_read[_dxl->port_name]->GetData(_dxl->id, EXT_PORT_DATA3_ADDR, &_ext_data) == true) - _dxl->dxl_state->ext_port_data[2] = _ext_data; - if(port_to_bulk_read[_dxl->port_name]->GetData(_dxl->id, EXT_PORT_DATA4_ADDR, &_ext_data) == true) - _dxl->dxl_state->ext_port_data[3] = _ext_data; + // TODO: change dxl_state + if(_dxl->present_position_item != 0 && _item->item_name == _dxl->present_position_item->item_name) + _dxl->dxl_state->present_position = _dxl->ConvertValue2Radian(_data) - _dxl->dxl_state->position_offset; // remove offset + else if(_dxl->present_velocity_item != 0 && _item->item_name == _dxl->present_velocity_item->item_name) + _dxl->dxl_state->present_velocity = _dxl->ConvertValue2Velocity(_data); + else if(_dxl->present_current_item != 0 && _item->item_name == _dxl->present_current_item->item_name) + _dxl->dxl_state->present_current = _dxl->ConvertValue2Current(_data); + else if(_dxl->goal_position_item != 0 && _item->item_name == _dxl->goal_position_item->item_name) + _dxl->dxl_state->goal_position = _dxl->ConvertValue2Radian(_data) - _dxl->dxl_state->position_offset; // remove offset + else if(_dxl->goal_velocity_item != 0 && _item->item_name == _dxl->goal_velocity_item->item_name) + _dxl->dxl_state->goal_velocity = _dxl->ConvertValue2Velocity(_data); + else if(_dxl->goal_current_item != 0 && _item->item_name == _dxl->goal_current_item->item_name) + _dxl->dxl_state->goal_current = _dxl->ConvertValue2Current(_data); + else + _dxl->dxl_state->bulk_read_table[_item->item_name] = _data; + } } + + // -> update time stamp to Robot->dxls[]->dynamixel_state.update_time_stamp + _dxl->dxl_state->update_time_stamp = TimeStamp(_present_state.header.stamp.sec, _present_state.header.stamp.nsec); } _present_state.name.push_back(_joint_name); // TODO: should be converted to rad, rad/s, Nm _present_state.position.push_back(_dxl->dxl_state->present_position); _present_state.velocity.push_back(_dxl->dxl_state->present_velocity); - _present_state.effort.push_back(_dxl->dxl_state->present_load); + _present_state.effort.push_back(_dxl->dxl_state->present_current); _goal_state.name.push_back(_joint_name); _goal_state.position.push_back(_dxl->dxl_state->goal_position); _goal_state.velocity.push_back(_dxl->dxl_state->goal_velocity); - _goal_state.effort.push_back(_dxl->dxl_state->goal_torque); + _goal_state.effort.push_back(_dxl->dxl_state->goal_current); } - // -> publish present joint_states topic - _present_state.header.stamp = ros::Time::now(); + // -> publish present joint_states & goal joint states topic present_joint_state_pub.publish(_present_state); - - // -> publish goal joint_states topic - _goal_state.header.stamp = _present_state.header.stamp; goal_joint_state_pub.publish(_goal_state); + // Call SensorModule Process() + // -> for loop : call SensorModule list -> Process() + if(sensor_modules_.size() > 0) + { + for(std::list::iterator _module_it = sensor_modules_.begin(); _module_it != sensor_modules_.end(); _module_it++) + { + (*_module_it)->Process(robot->dxls); + + for(std::map::iterator _it = (*_module_it)->result.begin(); _it != (*_module_it)->result.end(); _it++) + sensor_result_[_it->first] = _it->second; + } + } + if(controller_mode_ == MOTION_MODULE_MODE) { - // TODO: Call MotionModule Process() + // Call MotionModule Process() // -> for loop : call MotionModule list -> Process() - if(modules_.size() > 0) + if(motion_modules_.size() > 0) { queue_mutex_.lock(); - for(std::list::iterator module_it = modules_.begin(); module_it != modules_.end(); module_it++) + for(std::list::iterator module_it = motion_modules_.begin(); module_it != motion_modules_.end(); module_it++) { - // ros::Time _before = ros::Time::now(); +// ros::Time _before = ros::Time::now(); - (*module_it)->Process(robot->dxls); + if((*module_it)->enable == false) + continue; - // ros::Duration _dur = ros::Time::now() - _before; - // double _msec = _dur.nsec * 0.000001; + (*module_it)->Process(robot->dxls, sensor_result_); - // std::cout << "Process duration ["<< (*module_it)->module_name << "] : " << _msec << std::endl; +// ros::Duration _dur = ros::Time::now() - _before; +// double _msec = _dur.sec * 1e+3 + _dur.nsec * 0.000001; + +// std::cout << "Process duration ["<< (*module_it)->module_name << "] : " << _msec << std::endl; + + + ros::Time _before = ros::Time::now(); // for loop : joint list for(std::map::iterator dxl_it = robot->dxls.begin(); dxl_it != robot->dxls.end(); dxl_it++) @@ -615,19 +729,47 @@ void RobotisController::Process() if(abs(_pos_data) > 151800) printf("goal_pos : %f | position_offset : %f | pos_data : %d\n", _dxl_state->goal_position , _dxl_state->position_offset, _pos_data); - port_to_sync_write[_dxl->port_name]->ChangeParam(_dxl->id, _sync_write_data); + port_to_sync_write_position[_dxl->port_name]->ChangeParam(_dxl->id, _sync_write_data); } } else if((*module_it)->control_mode == VELOCITY_CONTROL) { _dxl_state->goal_velocity = _result_state->goal_velocity; + + if(gazebo_mode == false) + { + UINT32_T _vel_data = _dxl->ConvertVelocity2Value(_dxl_state->goal_velocity); + UINT8_T _sync_write_data[4]; + _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_vel_data)); + _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_vel_data)); + _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_vel_data)); + _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_vel_data)); + + port_to_sync_write_velocity[_dxl->port_name]->ChangeParam(_dxl->id, _sync_write_data); + } } - else if((*module_it)->control_mode == TORQUE_CONTROL) + else if((*module_it)->control_mode == CURRENT_CONTROL) { - _dxl_state->goal_torque = _result_state->goal_torque; + _dxl_state->goal_current = _result_state->goal_current; + + if(gazebo_mode == false) + { + UINT32_T _torq_data = _dxl->ConvertCurrent2Value(_dxl_state->goal_current); + UINT8_T _sync_write_data[2]; + _sync_write_data[0] = DXL_LOBYTE(_torq_data); + _sync_write_data[1] = DXL_HIBYTE(_torq_data); + + port_to_sync_write_torque[_dxl->port_name]->ChangeParam(_dxl->id, _sync_write_data); + } } } } + + ros::Duration _dur = ros::Time::now() - _before; + double _msec = _dur.sec * 1e+3 + _dur.nsec * 0.000001; + + //std::cout << "Process duration ["<< (*module_it)->module_name << "] : " << _msec << std::endl; + } // std::cout << " ------------------------------------------- " << std::endl; @@ -638,7 +780,11 @@ void RobotisController::Process() // -> SyncWrite if(gazebo_mode == false && _do_sync_write) { - for(std::map::iterator _it = port_to_sync_write.begin(); _it != port_to_sync_write.end(); _it++) + for(std::map::iterator _it = port_to_sync_write_position.begin(); _it != port_to_sync_write_position.end(); _it++) + _it->second->TxPacket(); + for(std::map::iterator _it = port_to_sync_write_velocity.begin(); _it != port_to_sync_write_velocity.end(); _it++) + _it->second->TxPacket(); + for(std::map::iterator _it = port_to_sync_write_torque.begin(); _it != port_to_sync_write_torque.end(); _it++) _it->second->TxPacket(); } else if(gazebo_mode == true) @@ -656,7 +802,7 @@ void RobotisController::Process() { queue_mutex_.lock(); - for(std::map::iterator _it = port_to_sync_write.begin(); _it != port_to_sync_write.end(); _it++) + for(std::map::iterator _it = port_to_sync_write_position.begin(); _it != port_to_sync_write_position.end(); _it++) { _it->second->TxPacket(); _it->second->ClearParam(); @@ -677,39 +823,43 @@ void RobotisController::Process() // TODO: User Read/Write - // TODO: BulkRead Tx + // BulkRead Tx if(gazebo_mode == false) { for(std::map::iterator _it = port_to_bulk_read.begin(); _it != port_to_bulk_read.end(); _it++) _it->second->TxPacket(); } - // for test : goal to present - // for(std::map::iterator dxl_it = robot_->dxls.begin(); dxl_it != robot_->dxls.end(); dxl_it++) - // { - // dxl_it->second->dxl_state->present_position = dxl_it->second->dxl_state->goal_position; - // dxl_it->second->dxl_state->present_velocity = dxl_it->second->dxl_state->goal_velocity; - // } - - -// ros::Duration _dur = ros::Time::now() - _now; -// double _msec = _dur.nsec * 0.000001; -// -// if(_msec > 8) std::cout << "Process duration : " << _msec << std::endl; + // ros::Duration _dur = ros::Time::now() - _now; + // double _msec = _dur.nsec * 0.000001; + // + // if(_msec > 8) std::cout << "Process duration : " << _msec << std::endl; _is_process_running = false; } -void RobotisController::AddModule(MotionModule *module) +void RobotisController::AddMotionModule(MotionModule *module) { - module->Initialize(CONTROL_CYCLE_MSEC); - modules_.push_back(module); - modules_.unique(); + module->Initialize(CONTROL_CYCLE_MSEC, robot); + motion_modules_.push_back(module); + motion_modules_.unique(); } -void RobotisController::RemoveModule(MotionModule *module) +void RobotisController::RemoveMotionModule(MotionModule *module) { - modules_.remove(module); + motion_modules_.remove(module); +} + +void RobotisController::AddSensorModule(SensorModule *module) +{ + module->Initialize(CONTROL_CYCLE_MSEC, robot); + sensor_modules_.push_back(module); + sensor_modules_.unique(); +} + +void RobotisController::RemoveSensorModule(SensorModule *module) +{ + sensor_modules_.remove(module); } void RobotisController::SyncWriteItemCallback(const robotis_controller_msgs::SyncWriteItem::ConstPtr &msg) @@ -796,13 +946,20 @@ void RobotisController::SetJointStatesCallback(const sensor_msgs::JointState::Co _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_pos)); _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_pos)); - port_to_sync_write[_dxl->port_name]->AddParam(_dxl->id, _sync_write_data); + port_to_sync_write_position[_dxl->port_name]->AddParam(_dxl->id, _sync_write_data); } queue_mutex_.unlock(); } -void RobotisController::SetCtrlModuleCallback(const robotis_controller_msgs::JointCtrlModule::ConstPtr &msg) +void RobotisController::SetCtrlModuleCallback(const std_msgs::String::ConstPtr &msg) +{ + std::string _module_name_to_set = msg->data; + + set_module_thread_ = boost::thread(boost::bind(&RobotisController::SetCtrlModuleThread, this, _module_name_to_set)); +} + +void RobotisController::SetJointCtrlModuleCallback(const robotis_controller_msgs::JointCtrlModule::ConstPtr &msg) { if(msg->joint_name.size() != msg->module_name.size()) return; @@ -826,7 +983,7 @@ void RobotisController::SetCtrlModuleCallback(const robotis_controller_msgs::Joi } // check whether the module is using this joint - for(std::list::iterator _m_it = modules_.begin(); _m_it != modules_.end(); _m_it++) + for(std::list::iterator _m_it = motion_modules_.begin(); _m_it != motion_modules_.end(); _m_it++) { if((*_m_it)->module_name == msg->module_name[idx]) { @@ -839,7 +996,7 @@ void RobotisController::SetCtrlModuleCallback(const robotis_controller_msgs::Joi } } - for(std::list::iterator _m_it = modules_.begin(); _m_it != modules_.end(); _m_it++) + for(std::list::iterator _m_it = motion_modules_.begin(); _m_it != motion_modules_.end(); _m_it++) { // set all modules -> disable (*_m_it)->enable = false; @@ -888,6 +1045,139 @@ bool RobotisController::GetCtrlModuleCallback(robotis_controller_msgs::GetJointM return true; } +void RobotisController::SetCtrlModuleThread(std::string ctrl_module) +{ + // stop module + std::list _stop_modules; + + if(ctrl_module == "" || ctrl_module == "none") + { + // enqueue all modules in order to stop + for(std::list::iterator _m_it = motion_modules_.begin(); _m_it != motion_modules_.end(); _m_it++) + { + if((*_m_it)->enable == true) _stop_modules.push_back(*_m_it); + } + } + else + { + for(std::list::iterator _m_it = motion_modules_.begin(); _m_it != motion_modules_.end(); _m_it++) + { + // if it exist + if((*_m_it)->module_name == ctrl_module) + { + // enqueue the module which lost control of joint in order to stop + for(std::map::iterator _result_it = (*_m_it)->result.begin(); _result_it != (*_m_it)->result.end(); _result_it++) + { + std::map::iterator _d_it = robot->dxls.find(_result_it->first); + + if(_d_it != robot->dxls.end()) + { + // enqueue + if(_d_it->second->ctrl_module_name != ctrl_module) + { + for(std::list::iterator _stop_m_it = motion_modules_.begin(); _stop_m_it != motion_modules_.end(); _stop_m_it++) + { + if((*_stop_m_it)->module_name == _d_it->second->ctrl_module_name && (*_stop_m_it)->enable == true) + _stop_modules.push_back(*_stop_m_it); + } + } + } + } + + break; + } + } + } + + // stop the module + _stop_modules.unique(); + for(std::list::iterator _stop_m_it = _stop_modules.begin(); _stop_m_it != _stop_modules.end(); _stop_m_it++) + { + (*_stop_m_it)->Stop(); + } + + // wait to stop + for(std::list::iterator _stop_m_it = _stop_modules.begin(); _stop_m_it != _stop_modules.end(); _stop_m_it++) + { + while((*_stop_m_it)->IsRunning()) + usleep(CONTROL_CYCLE_MSEC * 1000); + } + + // set ctrl module + queue_mutex_.lock(); + + if(DEBUG_PRINT) ROS_INFO_STREAM("set module : " << ctrl_module); + + // none + if(ctrl_module == "" || ctrl_module == "none") + { + // set all modules -> disable + for(std::list::iterator _m_it = motion_modules_.begin(); _m_it != motion_modules_.end(); _m_it++) + { + (*_m_it)->enable = false; + } + + // set dxl's control module to "none" + for(std::map::iterator _d_it = robot->dxls.begin(); _d_it != robot->dxls.end(); _d_it++) + { + _d_it->second->ctrl_module_name = "none"; + } + } + else + { + // check whether the module exist + for(std::list::iterator _m_it = motion_modules_.begin(); _m_it != motion_modules_.end(); _m_it++) + { + // if it exist + if((*_m_it)->module_name == ctrl_module) + { + for(std::map::iterator _result_it = (*_m_it)->result.begin(); _result_it != (*_m_it)->result.end(); _result_it++) + { + std::map::iterator _d_it = robot->dxls.find(_result_it->first); + if(_d_it != robot->dxls.end()) + { + _d_it->second->ctrl_module_name = ctrl_module; + } + } + + break; + } + } + } + + for(std::list::iterator _m_it = motion_modules_.begin(); _m_it != motion_modules_.end(); _m_it++) + { + // set all modules -> disable + (*_m_it)->enable = false; + + // set all used modules -> enable + for(std::map::iterator _d_it = robot->dxls.begin(); _d_it != robot->dxls.end(); _d_it++) + { + if(_d_it->second->ctrl_module_name == (*_m_it)->module_name) + { + (*_m_it)->enable = true; + break; + } + } + } + + // TODO: set indirect address + // -> check module's control_mode + + queue_mutex_.unlock(); + + // publish current module + robotis_controller_msgs::JointCtrlModule _current_module_msg; + for(std::map::iterator _dxl_iter = robot->dxls.begin(); _dxl_iter != robot->dxls.end(); ++_dxl_iter) + { + _current_module_msg.joint_name.push_back(_dxl_iter->first); + _current_module_msg.module_name.push_back(_dxl_iter->second->ctrl_module_name); + } + + if(_current_module_msg.joint_name.size() == _current_module_msg.module_name.size()) + current_module_pub.publish(_current_module_msg); +} + void RobotisController::GazeboJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg) { for(unsigned int _i = 0; _i < msg->name.size(); _i++) @@ -897,7 +1187,7 @@ void RobotisController::GazeboJointStatesCallback(const sensor_msgs::JointState: { _d_it->second->dxl_state->present_position = msg->position[_i]; _d_it->second->dxl_state->present_velocity = msg->velocity[_i]; - _d_it->second->dxl_state->present_load = msg->effort[_i]; + _d_it->second->dxl_state->present_current = msg->effort[_i]; } } @@ -1026,32 +1316,32 @@ int RobotisController::ReadCtrlItem(const std::string joint_name, const std::str int _result = COMM_NOT_AVAILABLE; switch(_item->data_length) { - case 1: - { - UINT8_T _data = 0; - _result = _pkt_handler->Read1ByteTxRx(_port_handler, _dxl->id, _item->address, &_data, error); - if(_result == COMM_SUCCESS) - *data = _data; - break; - } - case 2: - { - UINT16_T _data = 0; - _result = _pkt_handler->Read2ByteTxRx(_port_handler, _dxl->id, _item->address, &_data, error); - if(_result == COMM_SUCCESS) - *data = _data; - break; - } - case 4: - { - UINT32_T _data = 0; - _result = _pkt_handler->Read4ByteTxRx(_port_handler, _dxl->id, _item->address, &_data, error); - if(_result == COMM_SUCCESS) - *data = _data; - break; - } - default: - break; + case 1: + { + UINT8_T _data = 0; + _result = _pkt_handler->Read1ByteTxRx(_port_handler, _dxl->id, _item->address, &_data, error); + if(_result == COMM_SUCCESS) + *data = _data; + break; + } + case 2: + { + UINT16_T _data = 0; + _result = _pkt_handler->Read2ByteTxRx(_port_handler, _dxl->id, _item->address, &_data, error); + if(_result == COMM_SUCCESS) + *data = _data; + break; + } + case 4: + { + UINT32_T _data = 0; + _result = _pkt_handler->Read4ByteTxRx(_port_handler, _dxl->id, _item->address, &_data, error); + if(_result == COMM_SUCCESS) + *data = _data; + break; + } + default: + break; } return _result; } diff --git a/robotis_controller_msgs/CMakeLists.txt b/robotis_controller_msgs/CMakeLists.txt index 221d97d..d9f015c 100644 --- a/robotis_controller_msgs/CMakeLists.txt +++ b/robotis_controller_msgs/CMakeLists.txt @@ -15,6 +15,7 @@ add_message_files( FILES SyncWriteItem.msg JointCtrlModule.msg + StatusMsg.msg ) ## Generate services in the 'srv' folder diff --git a/robotis_controller_msgs/msg/StatusMsg.msg b/robotis_controller_msgs/msg/StatusMsg.msg new file mode 100644 index 0000000..47b706c --- /dev/null +++ b/robotis_controller_msgs/msg/StatusMsg.msg @@ -0,0 +1,10 @@ +# Status Constants +uint8 STATUS_UNKNOWN = 0 +uint8 STATUS_INFO = 1 +uint8 STATUS_WARN = 2 +uint8 STATUS_ERROR = 3 + +std_msgs/Header header +uint8 type +string module_name +string status_msg \ No newline at end of file diff --git a/robotis_device/CMakeLists.txt b/robotis_device/CMakeLists.txt index 6580f81..efe9e20 100644 --- a/robotis_device/CMakeLists.txt +++ b/robotis_device/CMakeLists.txt @@ -19,7 +19,7 @@ find_package(catkin REQUIRED COMPONENTS ## DEPENDS: system dependencies of this project that dependent projects also need catkin_package( INCLUDE_DIRS include -# LIBRARIES robotis_device + LIBRARIES robotis_device CATKIN_DEPENDS roscpp rospy # DEPENDS system_lib ) @@ -56,6 +56,5 @@ add_dependencies(robotis_device ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXP ## Specify libraries to link a library or executable target against target_link_libraries(robotis_device - dynamixel_sdk ${catkin_LIBRARIES} ) diff --git a/robotis_device/devices/dynamixel/GRIPPER.device b/robotis_device/devices/dynamixel/GRIPPER.device index ebbea55..e95ff17 100644 --- a/robotis_device/devices/dynamixel/GRIPPER.device +++ b/robotis_device/devices/dynamixel/GRIPPER.device @@ -1,6 +1,6 @@ [device info] model_name = GRIPPER -device_type = Dynamixel +device_type = dynamixel [type info] value_of_0_radian_position = 0 @@ -8,17 +8,17 @@ value_of_min_radian_position = 0 value_of_max_radian_position = 750 min_radian = 0 max_radian = 1.150767 -torque_enable_address = 562 -position_d_gain_address = -position_i_gain_address = -position_p_gain_address = 594 -goal_position_address = 596 -goal_velocity_address = 600 -goal_torque_address = 604 -present_position_address = 611 -present_velocity_address = 615 -present_load_address = 621 -is_moving_address = 610 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_torque +position_d_gain_item_name = +position_i_gain_item_name = +position_p_gain_item_name = position_p_gain [control table] # addr | item name | length | access | memory | min value | max value | signed @@ -47,9 +47,9 @@ is_moving_address = 610 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N - 586 | velocity_I_gain | 2 | RW | RAM | 0 | 2047 | N - 588 | velocity_P_gain | 2 | RW | RAM | 0 | 2047 | N - 594 | position_P_gain | 2 | RW | RAM | 0 | 2047 | N + 586 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N + 588 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N + 594 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y diff --git a/robotis_device/devices/dynamixel/H42-20-S300-R.device b/robotis_device/devices/dynamixel/H42-20-S300-R.device index 10884d8..1ed385f 100644 --- a/robotis_device/devices/dynamixel/H42-20-S300-R.device +++ b/robotis_device/devices/dynamixel/H42-20-S300-R.device @@ -1,24 +1,26 @@ [device info] model_name = H42-20-S300-R -device_type = Dynamixel +device_type = dynamixel [type info] +current_ratio = 4.0283203125 + value_of_0_radian_position = 0 value_of_min_radian_position = -151900 value_of_max_radian_position = 151900 min_radian = -3.14159265 max_radian = 3.14159265 -torque_enable_address = 562 -position_d_gain_address = -position_i_gain_address = -position_p_gain_address = 594 -goal_position_address = 596 -goal_velocity_address = 600 -goal_torque_address = 604 -present_position_address = 611 -present_velocity_address = 615 -present_load_address = 621 -is_moving_address = 610 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_torque +position_d_gain_item_name = +position_i_gain_item_name = +position_p_gain_item_name = position_p_gain [control table] # addr | item name | length | access | memory | min value | max value | signed @@ -48,9 +50,9 @@ is_moving_address = 610 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N - 586 | velocity_I_gain | 2 | RW | RAM | 0 | 2047 | N - 588 | velocity_P_gain | 2 | RW | RAM | 0 | 2047 | N - 594 | position_P_gain | 2 | RW | RAM | 0 | 2047 | N + 586 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N + 588 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N + 594 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y diff --git a/robotis_device/devices/dynamixel/H54-100-S500-R.device b/robotis_device/devices/dynamixel/H54-100-S500-R.device index 64e0915..09ef828 100644 --- a/robotis_device/devices/dynamixel/H54-100-S500-R.device +++ b/robotis_device/devices/dynamixel/H54-100-S500-R.device @@ -1,24 +1,26 @@ [device info] model_name = H54-100-S500-R -device_type = Dynamixel +device_type = dynamixel [type info] +current_ratio = 16.11328125 + value_of_0_radian_position = 0 value_of_min_radian_position = -250950 value_of_max_radian_position = 250950 min_radian = -3.14159265 max_radian = 3.14159265 -torque_enable_address = 562 -position_d_gain_address = -position_i_gain_address = -position_p_gain_address = 594 -goal_position_address = 596 -goal_velocity_address = 600 -goal_torque_address = 604 -present_position_address = 611 -present_velocity_address = 615 -present_load_address = 621 -is_moving_address = 610 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_torque +position_d_gain_item_name = +position_i_gain_item_name = +position_p_gain_item_name = position_p_gain [control table] # addr | item name | length | access | memory | min value | max value | signed @@ -48,9 +50,9 @@ is_moving_address = 610 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N - 586 | velocity_I_gain | 2 | RW | RAM | 0 | 2047 | N - 588 | velocity_P_gain | 2 | RW | RAM | 0 | 2047 | N - 594 | position_P_gain | 2 | RW | RAM | 0 | 2047 | N + 586 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N + 588 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N + 594 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y diff --git a/robotis_device/devices/dynamixel/H54-200-B500-R.device b/robotis_device/devices/dynamixel/H54-200-B500-R.device index d6ad870..8791d7c 100644 --- a/robotis_device/devices/dynamixel/H54-200-B500-R.device +++ b/robotis_device/devices/dynamixel/H54-200-B500-R.device @@ -1,24 +1,26 @@ [device info] model_name = H54-200-B500-R -device_type = Dynamixel +device_type = dynamixel [type info] +current_ratio = 16.11328125 + value_of_0_radian_position = 0 value_of_min_radian_position = -250950 value_of_max_radian_position = 250950 min_radian = -3.14159265 max_radian = 3.14159265 -torque_enable_address = 562 -position_d_gain_address = -position_i_gain_address = -position_p_gain_address = 594 -goal_position_address = 596 -goal_velocity_address = 600 -goal_torque_address = 604 -present_position_address = 611 -present_velocity_address = 615 -present_load_address = 621 -is_moving_address = 610 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_torque +position_d_gain_item_name = +position_i_gain_item_name = +position_p_gain_item_name = position_p_gain [control table] # addr | item name | length | access | memory | min value | max value | signed @@ -48,9 +50,9 @@ is_moving_address = 610 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N - 586 | velocity_I_gain | 2 | RW | RAM | 0 | 2047 | N - 588 | velocity_P_gain | 2 | RW | RAM | 0 | 2047 | N - 594 | position_P_gain | 2 | RW | RAM | 0 | 2047 | N + 586 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N + 588 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N + 594 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y diff --git a/robotis_device/devices/dynamixel/H54-200-S500-R.device b/robotis_device/devices/dynamixel/H54-200-S500-R.device index cae259f..dba268c 100644 --- a/robotis_device/devices/dynamixel/H54-200-S500-R.device +++ b/robotis_device/devices/dynamixel/H54-200-S500-R.device @@ -1,24 +1,26 @@ [device info] model_name = H54-200-S500-R -device_type = Dynamixel +device_type = dynamixel [type info] +current_ratio = 16.11328125 + value_of_0_radian_position = 0 value_of_min_radian_position = -250950 value_of_max_radian_position = 250950 min_radian = -3.14159265 max_radian = 3.14159265 -torque_enable_address = 562 -position_d_gain_address = -position_i_gain_address = -position_p_gain_address = 594 -goal_position_address = 596 -goal_velocity_address = 600 -goal_torque_address = 604 -present_position_address = 611 -present_velocity_address = 615 -present_load_address = 621 -is_moving_address = 610 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_torque +position_d_gain_item_name = +position_i_gain_item_name = +position_p_gain_item_name = position_p_gain [control table] # addr | item name | length | access | memory | min value | max value | signed @@ -48,9 +50,9 @@ is_moving_address = 610 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N - 586 | velocity_I_gain | 2 | RW | RAM | 0 | 2047 | N - 588 | velocity_P_gain | 2 | RW | RAM | 0 | 2047 | N - 594 | position_P_gain | 2 | RW | RAM | 0 | 2047 | N + 586 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N + 588 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N + 594 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y diff --git a/robotis_device/devices/dynamixel/L42-10-S300-R.device b/robotis_device/devices/dynamixel/L42-10-S300-R.device index 497761c..8a4ca5a 100644 --- a/robotis_device/devices/dynamixel/L42-10-S300-R.device +++ b/robotis_device/devices/dynamixel/L42-10-S300-R.device @@ -1,6 +1,6 @@ [device info] model_name = L42-10-S300-R -device_type = Dynamixel +device_type = dynamixel [type info] value_of_0_radian_position = 0 @@ -8,17 +8,17 @@ value_of_min_radian_position = -2047 value_of_max_radian_position = 2048 min_radian = -3.14159265 max_radian = 3.14159265 -torque_enable_address = 562 -position_d_gain_address = -position_i_gain_address = -position_p_gain_address = 594 -goal_position_address = 596 -goal_velocity_address = 600 -goal_torque_address = 604 -present_position_address = 611 -present_velocity_address = 615 -present_load_address = 621 -is_moving_address = 610 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_torque +position_d_gain_item_name = +position_i_gain_item_name = +position_p_gain_item_name = position_p_gain [control table] # addr | item name | length | access | memory | min value | max value | signed @@ -47,9 +47,9 @@ is_moving_address = 610 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N - 586 | velocity_I_gain | 2 | RW | RAM | 0 | 2047 | N - 588 | velocity_P_gain | 2 | RW | RAM | 0 | 2047 | N - 594 | position_P_gain | 2 | RW | RAM | 0 | 2047 | N + 586 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N + 588 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N + 594 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y diff --git a/robotis_device/devices/dynamixel/L54-30-S400-R.device b/robotis_device/devices/dynamixel/L54-30-S400-R.device index b360faa..8cdcff3 100644 --- a/robotis_device/devices/dynamixel/L54-30-S400-R.device +++ b/robotis_device/devices/dynamixel/L54-30-S400-R.device @@ -1,24 +1,26 @@ [device info] model_name = L54-30-S400-R -device_type = Dynamixel +device_type = dynamixel [type info] +current_ratio = 16.11328125 + value_of_0_radian_position = 0 value_of_min_radian_position = -144198 value_of_max_radian_position = 144198 min_radian = -3.14159265 max_radian = 3.14159265 -torque_enable_address = 562 -position_d_gain_address = -position_i_gain_address = -position_p_gain_address = 594 -goal_position_address = 596 -goal_velocity_address = 600 -goal_torque_address = 604 -present_position_address = 611 -present_velocity_address = 615 -present_load_address = 621 -is_moving_address = 610 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_torque +position_d_gain_item_name = +position_i_gain_item_name = +position_p_gain_item_name = position_p_gain [control table] # addr | item name | length | access | memory | min value | max value | signed @@ -48,9 +50,9 @@ is_moving_address = 610 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N - 586 | velocity_I_gain | 2 | RW | RAM | 0 | 2047 | N - 588 | velocity_P_gain | 2 | RW | RAM | 0 | 2047 | N - 594 | position_P_gain | 2 | RW | RAM | 0 | 2047 | N + 586 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N + 588 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N + 594 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y diff --git a/robotis_device/devices/dynamixel/L54-30-S500-R.device b/robotis_device/devices/dynamixel/L54-30-S500-R.device index 1a2f29c..db5a0b4 100644 --- a/robotis_device/devices/dynamixel/L54-30-S500-R.device +++ b/robotis_device/devices/dynamixel/L54-30-S500-R.device @@ -1,24 +1,26 @@ [device info] model_name = L54-30-S500-R -device_type = Dynamixel +device_type = dynamixel [type info] +current_ratio = 16.11328125 + value_of_0_radian_position = 0 value_of_min_radian_position = -180684 value_of_max_radian_position = 180684 min_radian = -3.14159265 max_radian = 3.14159265 -torque_enable_address = 562 -position_d_gain_address = -position_i_gain_address = -position_p_gain_address = 594 -goal_position_address = 596 -goal_velocity_address = 600 -goal_torque_address = 604 -present_position_address = 611 -present_velocity_address = 615 -present_load_address = 621 -is_moving_address = 610 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_torque +position_d_gain_item_name = +position_i_gain_item_name = +position_p_gain_item_name = position_p_gain [control table] # addr | item name | length | access | memory | min value | max value | signed @@ -48,9 +50,9 @@ is_moving_address = 610 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N - 586 | velocity_I_gain | 2 | RW | RAM | 0 | 2047 | N - 588 | velocity_P_gain | 2 | RW | RAM | 0 | 2047 | N - 594 | position_P_gain | 2 | RW | RAM | 0 | 2047 | N + 586 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N + 588 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N + 594 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y diff --git a/robotis_device/devices/dynamixel/L54-50-S290-R.device b/robotis_device/devices/dynamixel/L54-50-S290-R.device index 64577dd..cbd0453 100644 --- a/robotis_device/devices/dynamixel/L54-50-S290-R.device +++ b/robotis_device/devices/dynamixel/L54-50-S290-R.device @@ -1,24 +1,26 @@ [device info] model_name = L54-50-S290-R -device_type = Dynamixel +device_type = dynamixel [type info] +current_ratio = 16.11328125 + value_of_0_radian_position = 0 value_of_min_radian_position = -103860 value_of_max_radian_position = 103860 min_radian = -3.14159265 max_radian = 3.14159265 -torque_enable_address = 562 -position_d_gain_address = -position_i_gain_address = -position_p_gain_address = 594 -goal_position_address = 596 -goal_velocity_address = 600 -goal_torque_address = 604 -present_position_address = 611 -present_velocity_address = 615 -present_load_address = 621 -is_moving_address = 610 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_torque +position_d_gain_item_name = +position_i_gain_item_name = +position_p_gain_item_name = position_p_gain [control table] # addr | item name | length | access | memory | min value | max value | signed @@ -48,9 +50,9 @@ is_moving_address = 610 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N - 586 | velocity_I_gain | 2 | RW | RAM | 0 | 2047 | N - 588 | velocity_P_gain | 2 | RW | RAM | 0 | 2047 | N - 594 | position_P_gain | 2 | RW | RAM | 0 | 2047 | N + 586 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N + 588 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N + 594 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y diff --git a/robotis_device/devices/dynamixel/L54-50-S500-R.device b/robotis_device/devices/dynamixel/L54-50-S500-R.device index 1de23fd..d72f2ef 100644 --- a/robotis_device/devices/dynamixel/L54-50-S500-R.device +++ b/robotis_device/devices/dynamixel/L54-50-S500-R.device @@ -1,24 +1,26 @@ [device info] model_name = L54-50-S500-R -device_type = Dynamixel +device_type = dynamixel [type info] +current_ratio = 16.11328125 + value_of_0_radian_position = 0 value_of_min_radian_position = -180684 value_of_max_radian_position = 180684 min_radian = -3.14159265 max_radian = 3.14159265 -torque_enable_address = 562 -position_d_gain_address = -position_i_gain_address = -position_p_gain_address = 594 -goal_position_address = 596 -goal_velocity_address = 600 -goal_torque_address = 604 -present_position_address = 611 -present_velocity_address = 615 -present_load_address = 621 -is_moving_address = 610 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_torque +position_d_gain_item_name = +position_i_gain_item_name = +position_p_gain_item_name = position_p_gain [control table] # addr | item name | length | access | memory | min value | max value | signed @@ -48,9 +50,9 @@ is_moving_address = 610 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N - 586 | velocity_I_gain | 2 | RW | RAM | 0 | 2047 | N - 588 | velocity_P_gain | 2 | RW | RAM | 0 | 2047 | N - 594 | position_P_gain | 2 | RW | RAM | 0 | 2047 | N + 586 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N + 588 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N + 594 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y diff --git a/robotis_device/devices/dynamixel/M42-10-S260-R.device b/robotis_device/devices/dynamixel/M42-10-S260-R.device index b4511af..8c96988 100644 --- a/robotis_device/devices/dynamixel/M42-10-S260-R.device +++ b/robotis_device/devices/dynamixel/M42-10-S260-R.device @@ -1,24 +1,26 @@ [device info] model_name = M42-10-S260-R -device_type = Dynamixel +device_type = dynamixel [type info] +current_ratio = 4.0283203125 + value_of_0_radian_position = 0 value_of_min_radian_position = -131584 value_of_max_radian_position = 131584 min_radian = -3.14159265 max_radian = 3.14159265 -torque_enable_address = 562 -position_d_gain_address = -position_i_gain_address = -position_p_gain_address = 594 -goal_position_address = 596 -goal_velocity_address = 600 -goal_torque_address = 604 -present_position_address = 611 -present_velocity_address = 615 -present_load_address = 621 -is_moving_address = 610 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_torque +position_d_gain_item_name = +position_i_gain_item_name = +position_p_gain_item_name = position_p_gain [control table] # addr | item name | length | access | memory | min value | max value | signed @@ -48,9 +50,9 @@ is_moving_address = 610 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N - 586 | velocity_I_gain | 2 | RW | RAM | 0 | 2047 | N - 588 | velocity_P_gain | 2 | RW | RAM | 0 | 2047 | N - 594 | position_P_gain | 2 | RW | RAM | 0 | 2047 | N + 586 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N + 588 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N + 594 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y diff --git a/robotis_device/devices/dynamixel/M54-40-S250-R.device b/robotis_device/devices/dynamixel/M54-40-S250-R.device index 345dc1e..2bc7c1c 100644 --- a/robotis_device/devices/dynamixel/M54-40-S250-R.device +++ b/robotis_device/devices/dynamixel/M54-40-S250-R.device @@ -1,24 +1,26 @@ [device info] model_name = M54-40-S250-R -device_type = Dynamixel +device_type = dynamixel [type info] +current_ratio = 16.11328125 + value_of_0_radian_position = 0 value_of_min_radian_position = -125700 value_of_max_radian_position = 125700 min_radian = -3.14159265 max_radian = 3.14159265 -torque_enable_address = 562 -position_d_gain_address = -position_i_gain_address = -position_p_gain_address = 594 -goal_position_address = 596 -goal_velocity_address = 600 -goal_torque_address = 604 -present_position_address = 611 -present_velocity_address = 615 -present_load_address = 621 -is_moving_address = 610 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_torque +position_d_gain_item_name = +position_i_gain_item_name = +position_p_gain_item_name = position_p_gain [control table] # addr | item name | length | access | memory | min value | max value | signed @@ -48,9 +50,9 @@ is_moving_address = 610 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N - 586 | velocity_I_gain | 2 | RW | RAM | 0 | 2047 | N - 588 | velocity_P_gain | 2 | RW | RAM | 0 | 2047 | N - 594 | position_P_gain | 2 | RW | RAM | 0 | 2047 | N + 586 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N + 588 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N + 594 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y diff --git a/robotis_device/devices/dynamixel/M54-60-S250-R.device b/robotis_device/devices/dynamixel/M54-60-S250-R.device index bfcc9ed..72382de 100644 --- a/robotis_device/devices/dynamixel/M54-60-S250-R.device +++ b/robotis_device/devices/dynamixel/M54-60-S250-R.device @@ -1,24 +1,26 @@ [device info] model_name = M54-60-S250-R -device_type = Dynamixel +device_type = dynamixel [type info] +current_ratio = 16.11328125 + value_of_0_radian_position = 0 value_of_min_radian_position = -125700 value_of_max_radian_position = 125700 min_radian = -3.14159265 max_radian = 3.14159265 -torque_enable_address = 562 -position_d_gain_address = -position_i_gain_address = -position_p_gain_address = 594 -goal_position_address = 596 -goal_velocity_address = 600 -goal_torque_address = 604 -present_position_address = 611 -present_velocity_address = 615 -present_load_address = 621 -is_moving_address = 610 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_torque +position_d_gain_item_name = +position_i_gain_item_name = +position_p_gain_item_name = position_p_gain [control table] # addr | item name | length | access | memory | min value | max value | signed @@ -48,9 +50,9 @@ is_moving_address = 610 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N - 586 | velocity_I_gain | 2 | RW | RAM | 0 | 2047 | N - 588 | velocity_P_gain | 2 | RW | RAM | 0 | 2047 | N - 594 | position_P_gain | 2 | RW | RAM | 0 | 2047 | N + 586 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N + 588 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N + 594 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y diff --git a/robotis_device/devices/dynamixel/MX-28.device b/robotis_device/devices/dynamixel/MX-28.device index beb0af8..f1cfc21 100644 --- a/robotis_device/devices/dynamixel/MX-28.device +++ b/robotis_device/devices/dynamixel/MX-28.device @@ -1,6 +1,6 @@ [device info] model_name = MX-28 -device_type = Dynamixel +device_type = dynamixel [type info] value_of_0_radian_position = 2048 @@ -8,17 +8,17 @@ value_of_min_radian_position = 0 value_of_max_radian_position = 4095 min_radian = -3.14159265 max_radian = 3.14159265 -torque_enable_address = 24 -position_d_gain_address = 26 -position_i_gain_address = 27 -position_p_gain_address = 28 -goal_position_address = 30 -goal_velocity_address = 32 -goal_torque_address = 34 -present_position_address = 36 -present_velocity_address = 38 -present_load_address = 40 -is_moving_address = 46 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = +position_d_gain_item_name = position_d_gain +position_i_gain_item_name = position_i_gain +position_p_gain_item_name = position_p_gain [control table] # addr | item name | length | access | memory | min value | max value | signed @@ -41,9 +41,9 @@ is_moving_address = 46 22 | resolution_dividor | 1 | RW | EEPROM | 1 | 255 | N 24 | torque_enable | 1 | RW | RAM | 0 | 1 | N 25 | LED | 1 | RW | RAM | 0 | 1 | N - 26 | position_D_gain | 1 | RW | RAM | 0 | 254 | N - 27 | position_I_gain | 1 | RW | RAM | 0 | 254 | N - 28 | position_P_gain | 1 | RW | RAM | 0 | 254 | N + 26 | position_d_gain | 1 | RW | RAM | 0 | 254 | N + 27 | position_i_gain | 1 | RW | RAM | 0 | 254 | N + 28 | position_p_gain | 1 | RW | RAM | 0 | 254 | N 30 | goal_position | 2 | RW | RAM | -28672 | 28672 | Y 32 | goal_velocity | 2 | RW | RAM | 0 | 1023 | N 34 | goal_torque | 2 | RW | RAM | 0 | 1023 | N diff --git a/robotis_device/include/robotis_device/ControlTableItem.h b/robotis_device/include/robotis_device/ControlTableItem.h index 5a48bab..e7b75fd 100644 --- a/robotis_device/include/robotis_device/ControlTableItem.h +++ b/robotis_device/include/robotis_device/ControlTableItem.h @@ -27,6 +27,7 @@ enum MEMORY_TYPE { class ControlTableItem { public: + std::string item_name; UINT16_T address; ACCESS_TYPE access_type; MEMORY_TYPE memory_type; @@ -36,7 +37,8 @@ public: bool is_signed; ControlTableItem() - : address(0), + : item_name(""), + address(0), access_type(READ), memory_type(RAM), data_length(0), diff --git a/robotis_device/include/robotis_device/Dynamixel.h b/robotis_device/include/robotis_device/Dynamixel.h index ef4e826..1a65f72 100644 --- a/robotis_device/include/robotis_device/Dynamixel.h +++ b/robotis_device/include/robotis_device/Dynamixel.h @@ -10,6 +10,7 @@ #include +#include #include #include "DynamixelState.h" #include "ControlTableItem.h" @@ -30,28 +31,36 @@ public: std::string ctrl_module_name; DynamixelState *dxl_state; + double current_ratio; + double velocity_ratio; + INT32_T value_of_0_radian_position; INT32_T value_of_min_radian_position; INT32_T value_of_max_radian_position; double min_radian; double max_radian; - UINT16_T torque_enable_address; - UINT16_T position_d_gain_address; - UINT16_T position_i_gain_address; - UINT16_T position_p_gain_address; - UINT16_T goal_position_address; - UINT16_T goal_velocity_address; - UINT16_T goal_torque_address; - UINT16_T present_position_address; - UINT16_T present_velocity_address; - UINT16_T present_load_address; - UINT16_T is_moving_address; + ControlTableItem *torque_enable_item; + ControlTableItem *present_position_item; + ControlTableItem *present_velocity_item; + ControlTableItem *present_current_item; + ControlTableItem *goal_position_item; + ControlTableItem *goal_velocity_item; + ControlTableItem *goal_current_item; + + std::vector bulk_read_items; + std::map indirect_address_table; Dynamixel(int id, std::string model_name, float protocol_version); double ConvertValue2Radian(INT32_T value); INT32_T ConvertRadian2Value(double radian); + + double ConvertValue2Velocity(INT32_T value); + INT32_T ConvertVelocity2Value(double velocity); + + double ConvertValue2Current(INT16_T value); + INT16_T ConvertCurrent2Value(double torque); }; } diff --git a/robotis_device/include/robotis_device/DynamixelState.h b/robotis_device/include/robotis_device/DynamixelState.h index 7917aed..f5df513 100644 --- a/robotis_device/include/robotis_device/DynamixelState.h +++ b/robotis_device/include/robotis_device/DynamixelState.h @@ -11,6 +11,9 @@ #include #include +#define INDIRECT_DATA_1 "indirect_data_1" +#define INDIRECT_ADDRESS_1 "indirect_address_1" + namespace ROBOTIS { @@ -26,47 +29,28 @@ class DynamixelState public: TimeStamp update_time_stamp; - bool torque_enable; - UINT16_T position_d_gain; - UINT16_T position_i_gain; - UINT16_T position_p_gain; - UINT16_T velocity_d_gain; - UINT16_T velocity_i_gain; - UINT16_T velocity_p_gain; - double goal_position; - double goal_velocity; - double goal_torque; double present_position; double present_velocity; - double present_load; - bool is_moving; + double present_current; + double goal_position; + double goal_velocity; + double goal_current; - UINT16_T ext_port_data[4]; + std::map bulk_read_table; double position_offset; DynamixelState() : update_time_stamp(0, 0), - torque_enable(false), - position_d_gain(0), - position_i_gain(0), - position_p_gain(0), - velocity_d_gain(0), - velocity_i_gain(0), - velocity_p_gain(0), - goal_position(0.0), - goal_velocity(0.0), - goal_torque(0.0), present_position(0.0), present_velocity(0.0), - present_load(0.0), - is_moving(false), + present_current(0.0), + goal_position(0.0), + goal_velocity(0.0), + goal_current(0.0), position_offset(0.0) { - ext_port_data[0] = 2048; - ext_port_data[1] = 2048; - ext_port_data[2] = 2048; - ext_port_data[3] = 2048; + bulk_read_table.clear(); } }; diff --git a/robotis_device/include/robotis_device/Robot.h b/robotis_device/include/robotis_device/Robot.h index 121d385..d169d35 100644 --- a/robotis_device/include/robotis_device/Robot.h +++ b/robotis_device/include/robotis_device/Robot.h @@ -21,6 +21,8 @@ class Robot { public: std::map ports; // string: port name + std::map port_default_joint; // port name, default joint name + std::map dxls; // string: joint name std::map sensors; // string: sensor name diff --git a/robotis_device/src/robotis_device/Dynamixel.cpp b/robotis_device/src/robotis_device/Dynamixel.cpp index f2a78a7..2ccb3a0 100644 --- a/robotis_device/src/robotis_device/Dynamixel.cpp +++ b/robotis_device/src/robotis_device/Dynamixel.cpp @@ -15,25 +15,26 @@ Dynamixel::Dynamixel(int id, std::string model_name, float protocol_version) port_name(""), ctrl_module_name("none"), protocol_version(protocol_version), + current_ratio(1.0), + velocity_ratio(1.0), value_of_0_radian_position(0), value_of_min_radian_position(0), value_of_max_radian_position(0), - min_radian(-3.14), - max_radian(3.14), - torque_enable_address(0), - position_d_gain_address(0), - position_i_gain_address(0), - position_p_gain_address(0), - goal_position_address(0), - goal_velocity_address(0), - goal_torque_address(0), - present_position_address(0), - present_velocity_address(0), - present_load_address(0), - is_moving_address(0) + min_radian(-3.14159265), + max_radian(3.14159265), + torque_enable_item(0), + present_position_item(0), + present_velocity_item(0), + present_current_item(0), + goal_position_item(0), + goal_velocity_item(0), + goal_current_item(0) { ctrl_table.clear(); dxl_state = new DynamixelState(); + + bulk_read_items.clear(); + indirect_address_table.clear(); } double Dynamixel::ConvertValue2Radian(INT32_T value) @@ -87,3 +88,23 @@ INT32_T Dynamixel::ConvertRadian2Value(double radian) return _value; } + +double Dynamixel::ConvertValue2Velocity(INT32_T value) +{ + return (double)value * velocity_ratio; +} + +INT32_T Dynamixel::ConvertVelocity2Value(double velocity) +{ + return (INT32_T)(velocity / velocity_ratio);; +} + +double Dynamixel::ConvertValue2Current(INT16_T value) +{ + return (double)value * current_ratio; +} + +INT16_T Dynamixel::ConvertCurrent2Value(double torque) +{ + return (INT16_T)(torque / current_ratio); +} diff --git a/robotis_device/src/robotis_device/Robot.cpp b/robotis_device/src/robotis_device/Robot.cpp index 64c5594..f397c3c 100644 --- a/robotis_device/src/robotis_device/Robot.cpp +++ b/robotis_device/src/robotis_device/Robot.cpp @@ -71,18 +71,19 @@ Robot::Robot(std::string robot_file_path, std::string dev_desc_dir_path) if(session == "port info") { std::vector tokens = split(input_str, '|'); - if(tokens.size() != 2) + if(tokens.size() != 3) continue; std::cout << tokens[0] << " added. (baudrate: " << tokens[1] << ")" << std::endl; ports[tokens[0]] = (PortHandler*)PortHandler::GetPortHandler(tokens[0].c_str()); ports[tokens[0]]->SetBaudRate(std::atoi(tokens[1].c_str())); + port_default_joint[tokens[0]] = tokens[2]; } else if(session == "device info") { std::vector tokens = split(input_str, '|'); - if(tokens.size() != 6) + if(tokens.size() != 7) continue; if(tokens[0] == "dynamixel") @@ -94,6 +95,39 @@ Robot::Robot(std::string robot_file_path, std::string dev_desc_dir_path) std::string _dev_name = tokens[5]; dxls[_dev_name] = getDynamixel(_file_path, _id, _port, _protocol); + + Dynamixel *_dxl = dxls[_dev_name]; + std::vector sub_tokens = split(tokens[6], ','); + if(sub_tokens.size() > 0) + { + std::map::iterator _indirect_it = _dxl->ctrl_table.find(INDIRECT_ADDRESS_1); + if(_indirect_it != _dxl->ctrl_table.end()) // INDIRECT_ADDRESS_1 exist + { + UINT16_T _indirect_data_addr = _dxl->ctrl_table[INDIRECT_DATA_1]->address; + for(int _i = 0; _i < sub_tokens.size(); _i++) + { + _dxl->bulk_read_items.push_back(new ControlTableItem()); + ControlTableItem *_dest_item = _dxl->bulk_read_items[_i]; + ControlTableItem *_src_item = _dxl->ctrl_table[sub_tokens[_i]]; + + _dest_item->item_name = _src_item->item_name; + _dest_item->address = _indirect_data_addr; + _dest_item->access_type = _src_item->access_type; + _dest_item->memory_type = _src_item->memory_type; + _dest_item->data_length = _src_item->data_length; + _dest_item->data_min_value = _src_item->data_min_value; + _dest_item->data_max_value = _src_item->data_max_value; + _dest_item->is_signed = _src_item->is_signed; + + _indirect_data_addr += _dest_item->data_length; + } + } + else // INDIRECT_ADDRESS_1 exist + { + for(int _i = 0; _i < sub_tokens.size(); _i++) + _dxl->bulk_read_items.push_back(_dxl->ctrl_table[sub_tokens[_i]]); + } + } } } } @@ -112,47 +146,61 @@ Dynamixel *Robot::getDynamixel(std::string path, int id, std::string port, float std::ifstream file(path.c_str()); if(file.is_open()) { - std::string session = ""; - std::string input_str; + std::string _session = ""; + std::string _input_str; + + std::string _torque_enable_item_name = ""; + std::string _present_position_item_name = ""; + std::string _present_velocity_item_name = ""; + std::string _present_current_item_name = ""; + std::string _goal_position_item_name = ""; + std::string _goal_velocity_item_name = ""; + std::string _goal_current_item_name = ""; + while(!file.eof()) { - std::getline(file, input_str); + std::getline(file, _input_str); // remove comment ( # ) - std::size_t pos = input_str.find("#"); + std::size_t pos = _input_str.find("#"); if(pos != std::string::npos) - input_str = input_str.substr(0, pos); + _input_str = _input_str.substr(0, pos); // trim - input_str = trim(input_str); - if(input_str == "") + _input_str = trim(_input_str); + if(_input_str == "") continue; - // find session - if(!input_str.compare(0, 1, "[") && !input_str.compare(input_str.size()-1, 1, "]")) + // find _session + if(!_input_str.compare(0, 1, "[") && !_input_str.compare(_input_str.size()-1, 1, "]")) { - input_str = input_str.substr(1, input_str.size()-2); - std::transform(input_str.begin(), input_str.end(), input_str.begin(), ::tolower); - session = trim(input_str); + _input_str = _input_str.substr(1, _input_str.size()-2); + std::transform(_input_str.begin(), _input_str.end(), _input_str.begin(), ::tolower); + _session = trim(_input_str); continue; } - if(session == "device info") + if(_session == "device info") { - std::vector tokens = split(input_str, '='); + std::vector tokens = split(_input_str, '='); if(tokens.size() != 2) continue; if(tokens[0] == "model_name") dxl = new Dynamixel(id, tokens[1], protocol_version); } - else if(session == "type info") + else if(_session == "type info") { - std::vector tokens = split(input_str, '='); + std::vector tokens = split(_input_str, '='); if(tokens.size() != 2) continue; - if(tokens[0] == "value_of_0_radian_position") + if(tokens[0] == "current_ratio") + dxl->current_ratio = std::atof(tokens[1].c_str()); + else if(tokens[0] == "velocity_ratio") + dxl->velocity_ratio = std::atof(tokens[1].c_str()); + + else if(tokens[0] == "value_of_0_radian_position") dxl->value_of_0_radian_position = std::atoi(tokens[1].c_str()); else if(tokens[0] == "value_of_min_radian_position") dxl->value_of_min_radian_position = std::atoi(tokens[1].c_str()); @@ -162,36 +210,30 @@ Dynamixel *Robot::getDynamixel(std::string path, int id, std::string port, float dxl->min_radian = std::atof(tokens[1].c_str()); else if(tokens[0] == "max_radian") dxl->max_radian = std::atof(tokens[1].c_str()); - else if(tokens[0] == "torque_enable_address") - dxl->torque_enable_address = std::atoi(tokens[1].c_str()); - else if(tokens[0] == "position_d_gain_address") - dxl->position_d_gain_address = std::atoi(tokens[1].c_str()); - else if(tokens[0] == "position_i_gain_address") - dxl->position_i_gain_address = std::atoi(tokens[1].c_str()); - else if(tokens[0] == "position_p_gain_address") - dxl->position_p_gain_address = std::atoi(tokens[1].c_str()); - else if(tokens[0] == "goal_position_address") - dxl->goal_position_address = std::atoi(tokens[1].c_str()); - else if(tokens[0] == "goal_velocity_address") - dxl->goal_velocity_address = std::atoi(tokens[1].c_str()); - else if(tokens[0] == "goal_torque_address") - dxl->goal_torque_address = std::atoi(tokens[1].c_str()); - else if(tokens[0] == "present_position_address") - dxl->present_position_address = std::atoi(tokens[1].c_str()); - else if(tokens[0] == "present_velocity_address") - dxl->present_velocity_address = std::atoi(tokens[1].c_str()); - else if(tokens[0] == "present_load_address") - dxl->present_load_address = std::atoi(tokens[1].c_str()); - else if(tokens[0] == "is_moving_address") - dxl->is_moving_address = std::atoi(tokens[1].c_str()); + + else if(tokens[0] == "torque_enable_item_name") + _torque_enable_item_name = tokens[1]; + else if(tokens[0] == "present_position_item_name") + _present_position_item_name = tokens[1]; + else if(tokens[0] == "present_velocity_item_name") + _present_velocity_item_name = tokens[1]; + else if(tokens[0] == "present_current_item_name") + _present_current_item_name = tokens[1]; + else if(tokens[0] == "goal_position_item_name") + _goal_position_item_name = tokens[1]; + else if(tokens[0] == "goal_velocity_item_name") + _goal_velocity_item_name = tokens[1]; + else if(tokens[0] == "goal_current_item_name") + _goal_current_item_name = tokens[1]; } - else if(session == "control table") + else if(_session == "control table") { - std::vector tokens = split(input_str, '|'); + std::vector tokens = split(_input_str, '|'); if(tokens.size() != 8) continue; ControlTableItem *item = new ControlTableItem(); + item->item_name = tokens[1]; item->address = std::atoi(tokens[0].c_str()); item->data_length = std::atoi(tokens[2].c_str()); if(tokens[3] == "R") @@ -213,6 +255,21 @@ Dynamixel *Robot::getDynamixel(std::string path, int id, std::string port, float } dxl->port_name = port; + if(dxl->ctrl_table[_torque_enable_item_name] != NULL) + dxl->torque_enable_item = dxl->ctrl_table[_torque_enable_item_name]; + if(dxl->ctrl_table[_present_position_item_name] != NULL) + dxl->present_position_item = dxl->ctrl_table[_present_position_item_name]; + if(dxl->ctrl_table[_present_velocity_item_name] != NULL) + dxl->present_velocity_item = dxl->ctrl_table[_present_velocity_item_name]; + if(dxl->ctrl_table[_present_current_item_name] != NULL) + dxl->present_current_item = dxl->ctrl_table[_present_current_item_name]; + if(dxl->ctrl_table[_goal_position_item_name] != NULL) + dxl->goal_position_item = dxl->ctrl_table[_goal_position_item_name]; + if(dxl->ctrl_table[_goal_velocity_item_name] != NULL) + dxl->goal_velocity_item = dxl->ctrl_table[_goal_velocity_item_name]; + if(dxl->ctrl_table[_goal_current_item_name] != NULL) + dxl->goal_current_item = dxl->ctrl_table[_goal_current_item_name]; + fprintf(stderr, "(%s) [ID:%3d] %14s added. \n", port.c_str(), dxl->id, dxl->model_name.c_str()); //std::cout << "[ID:" << (int)(dxl->id) << "] " << dxl->model_name << " added. (" << port << ")" << std::endl; file.close(); diff --git a/robotis_framework_common/CMakeLists.txt b/robotis_framework_common/CMakeLists.txt index 2a38558..17cd2cc 100644 --- a/robotis_framework_common/CMakeLists.txt +++ b/robotis_framework_common/CMakeLists.txt @@ -1,93 +1,10 @@ cmake_minimum_required(VERSION 2.8.3) project(robotis_framework_common) -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages find_package(catkin REQUIRED COMPONENTS roscpp ) -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) - - -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() - -################################################ -## Declare ROS messages, services and actions ## -################################################ - -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend tag for "message_generation" -## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependency has been pulled in -## but can be declared for certainty nonetheless: -## * add a run_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) - -## Generate messages in the 'msg' folder -# add_message_files( -# FILES -# Message1.msg -# Message2.msg -# ) - -## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - -## Generate added messages and services with any dependencies listed here -# generate_messages( -# DEPENDENCIES -# std_msgs # Or other packages containing msgs -# ) - -################################################ -## Declare ROS dynamic reconfigure parameters ## -################################################ - -## To declare and build dynamic reconfigure parameters within this -## package, follow these steps: -## * In the file package.xml: -## * add a build_depend and a run_depend tag for "dynamic_reconfigure" -## * In this file (CMakeLists.txt): -## * add "dynamic_reconfigure" to -## find_package(catkin REQUIRED COMPONENTS ...) -## * uncomment the "generate_dynamic_reconfigure_options" section below -## and list every .cfg file to be processed - -## Generate dynamic reconfigure parameters in the 'cfg' folder -# generate_dynamic_reconfigure_options( -# cfg/DynReconf1.cfg -# cfg/DynReconf2.cfg -# ) - ################################### ## catkin specific configuration ## ################################### @@ -137,50 +54,3 @@ include_directories( # ${catkin_LIBRARIES} # ) -############# -## Install ## -############# - -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# install(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables and/or libraries for installation -# install(TARGETS robotis_framework_common robotis_framework_common_node -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE -# ) - -## Mark other files for installation (e.g. launch and bag files, etc.) -# install(FILES -# # myfile1 -# # myfile2 -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -# ) - -############# -## Testing ## -############# - -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_robotis_framework_common.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() - -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) diff --git a/robotis_framework_common/include/robotis_framework_common/MotionModule.h b/robotis_framework_common/include/robotis_framework_common/MotionModule.h index 8237032..18d962e 100644 --- a/robotis_framework_common/include/robotis_framework_common/MotionModule.h +++ b/robotis_framework_common/include/robotis_framework_common/MotionModule.h @@ -5,15 +5,15 @@ * Author: zerom */ -#ifndef ROBOTIS_FRAMEWORK_ROBOTIS_CONTROLLER_MSGS_INCLUDE_MOTIONMODULE_H_ -#define ROBOTIS_FRAMEWORK_ROBOTIS_CONTROLLER_MSGS_INCLUDE_MOTIONMODULE_H_ +#ifndef ROBOTIS_FRAMEWORK_COMMON_INCLUDE_ROBOTIS_FRAMEWORK_COMMON_MOTIONMODULE_H_ +#define ROBOTIS_FRAMEWORK_COMMON_INCLUDE_ROBOTIS_FRAMEWORK_COMMON_MOTIONMODULE_H_ #include #include +#include "robotis_device/Robot.h" #include "robotis_device/Dynamixel.h" -#include "robotis_device/DynamixelState.h" namespace ROBOTIS { @@ -22,7 +22,7 @@ enum CONTROL_MODE { POSITION_CONTROL, VELOCITY_CONTROL, - TORQUE_CONTROL + CURRENT_CONTROL }; class MotionModule @@ -36,18 +36,15 @@ public: virtual ~MotionModule() { } - virtual void Initialize(const int control_cycle_msec) = 0; - virtual void Process(std::map dxls) = 0; + virtual void Initialize(const int control_cycle_msec, Robot *robot) = 0; + virtual void Process(std::map dxls, std::map sensors) = 0; - inline double powDI(double a, int b); + virtual void Stop() = 0; + virtual bool IsRunning() = 0; }; -inline double MotionModule::powDI(double a, int b) -{ - return (b == 0 ? 1 : (b > 0 ? a * powDI(a, b - 1) : 1 / powDI(a, -b))); -} } -#endif /* ROBOTIS_FRAMEWORK_ROBOTIS_CONTROLLER_MSGS_INCLUDE_MOTIONMODULE_H_ */ +#endif /* ROBOTIS_FRAMEWORK_COMMON_INCLUDE_ROBOTIS_FRAMEWORK_COMMON_MOTIONMODULE_H_ */ diff --git a/robotis_framework_common/include/robotis_framework_common/SensorModule.h b/robotis_framework_common/include/robotis_framework_common/SensorModule.h new file mode 100644 index 0000000..d34bfbb --- /dev/null +++ b/robotis_framework_common/include/robotis_framework_common/SensorModule.h @@ -0,0 +1,37 @@ +/* + * SensorModule.h + * + * Created on: 2016. 3. 30. + * Author: zerom + */ + +#ifndef ROBOTIS_FRAMEWORK_COMMON_INCLUDE_ROBOTIS_FRAMEWORK_COMMON_SENSORMODULE_H_ +#define ROBOTIS_FRAMEWORK_COMMON_INCLUDE_ROBOTIS_FRAMEWORK_COMMON_SENSORMODULE_H_ + + +#include +#include + +#include "robotis_device/Robot.h" +#include "robotis_device/Dynamixel.h" + +namespace ROBOTIS +{ + +class SensorModule +{ +public: + std::string module_name; + + std::map result; + + virtual ~SensorModule() { } + + virtual void Initialize(const int control_cycle_msec, Robot *robot) = 0; + virtual void Process(std::map dxls) = 0; +}; + +} + + +#endif /* ROBOTIS_FRAMEWORK_COMMON_INCLUDE_ROBOTIS_FRAMEWORK_COMMON_SENSORMODULE_H_ */ diff --git a/robotis_math/CMakeLists.txt b/robotis_math/CMakeLists.txt new file mode 100644 index 0000000..147809f --- /dev/null +++ b/robotis_math/CMakeLists.txt @@ -0,0 +1,65 @@ +cmake_minimum_required(VERSION 2.8.3) +project(robotis_math) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + roscpp + cmake_modules +) + +find_package(Eigen REQUIRED) + + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if you package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( + INCLUDE_DIRS include + LIBRARIES robotis_math + CATKIN_DEPENDS roscpp +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${Eigen_INCLUDE_DIRS} +) + +## Declare a C++ library +add_library(robotis_math + src/RobotisMathBase.cpp + src/RobotisTrajectoryCalculator.cpp + src/RobotisLinearAlgebra.cpp +) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +add_dependencies(robotis_math ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +# add_executable(robotis_math_node src/robotis_math_node.cpp) + +## Add cmake target dependencies of the executable +## same as for the library above +#add_dependencies(robotis_math_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +target_link_libraries(robotis_math + ${catkin_LIBRARIES} +) diff --git a/robotis_math/include/robotis_math/RobotisLinearAlgebra.h b/robotis_math/include/robotis_math/RobotisLinearAlgebra.h new file mode 100644 index 0000000..6188c50 --- /dev/null +++ b/robotis_math/include/robotis_math/RobotisLinearAlgebra.h @@ -0,0 +1,52 @@ +/* + * RobotisLinearAlgebra.h + * + * Created on: Mar 18, 2016 + * Author: jay + */ + +#ifndef ROBOTIS_LINEAR_ALGEBRA_H_ +#define ROBOTIS_LINEAR_ALGEBRA_H_ + +#include + +#define EIGEN_NO_DEBUG +#define EIGEN_NO_STATIC_ASSERT + +#include + +namespace ROBOTIS +{ + +Eigen::MatrixXd transitionXYZ ( double position_x, double position_y, double position_z ); +Eigen::MatrixXd transformationXYZRPY ( double position_x, double position_y, double position_z , double roll , double pitch , double yaw ); +Eigen::MatrixXd InverseTransformation(Eigen::MatrixXd transform); + +Eigen::MatrixXd inertiaXYZ( double ixx, double ixy, double ixz , double iyy , double iyz, double izz ); + +Eigen::MatrixXd rotationX( double angle ); +Eigen::MatrixXd rotationY( double angle ); +Eigen::MatrixXd rotationZ( double angle ); + +Eigen::MatrixXd rotation2rpy( Eigen::MatrixXd rotation ); +Eigen::MatrixXd rpy2rotation( double roll, double pitch, double yaw ); + +Eigen::Quaterniond rpy2quaternion( double roll, double pitch, double yaw ); +Eigen::Quaterniond rotation2quaternion( Eigen::MatrixXd rotation ); + +Eigen::MatrixXd quaternion2rpy( Eigen::Quaterniond quaternion ); +Eigen::MatrixXd quaternion2rotation( Eigen::Quaterniond quaternion ); + +Eigen::MatrixXd rotation4d( double roll, double pitch, double yaw ); + +Eigen::MatrixXd hatto( Eigen::MatrixXd matrix3d ); +Eigen::MatrixXd Rodrigues( Eigen::MatrixXd hat_matrix , double angle ); +Eigen::MatrixXd rot2omega(Eigen::MatrixXd rotation ); +Eigen::MatrixXd cross(Eigen::MatrixXd vector3d_a, Eigen::MatrixXd vector3d_b ); +double dot(Eigen::MatrixXd vector3d_a, Eigen::MatrixXd vector3d_b ); + +} + + + +#endif /* ROBOTIS_LINEAR_ALGEBRA_H_ */ diff --git a/robotis_math/include/robotis_math/RobotisMath.h b/robotis_math/include/robotis_math/RobotisMath.h new file mode 100644 index 0000000..752038d --- /dev/null +++ b/robotis_math/include/robotis_math/RobotisMath.h @@ -0,0 +1,13 @@ +/* + * robotis_math.h + * + * Created on: Mar 18, 2016 + * Author: jay + */ + +#ifndef ROBOTIS_MATH_H_ +#define ROBOTIS_MATH_H_ + +#include "RobotisTrajectoryCalculator.h" + +#endif /* ROBOTIS_MATH_H_ */ diff --git a/robotis_math/include/robotis_math/RobotisMathBase.h b/robotis_math/include/robotis_math/RobotisMathBase.h new file mode 100644 index 0000000..7b7c713 --- /dev/null +++ b/robotis_math/include/robotis_math/RobotisMathBase.h @@ -0,0 +1,33 @@ +/* + * RobotisMathBase.h + * + * Created on: 2016. 3. 28. + * Author: JaySong + */ + +#ifndef ROBOTIS_MATH_BASE_H_ +#define ROBOTIS_MATH_BASE_H_ + +#include + +namespace ROBOTIS +{ + +#define PRINT_VAR(X) std::cout << #X << " : " << X << std::endl +#define PRINT_MAT(X) std::cout << #X << ":\n" << X << std::endl << std::endl + +#define deg2rad (M_PI / 180.0) +#define rad2deg (180.0 / M_PI) + +inline double powDI(double a, int b) +{ + return (b == 0 ? 1 : (b > 0 ? a * powDI(a, b - 1) : 1 / powDI(a, -b))); +} + +double sign( double x ); + +} + + + +#endif /* ROBOTIS_MATH_BASE_H_ */ diff --git a/robotis_math/include/robotis_math/RobotisTrajectoryCalculator.h b/robotis_math/include/robotis_math/RobotisTrajectoryCalculator.h new file mode 100644 index 0000000..3d1ff70 --- /dev/null +++ b/robotis_math/include/robotis_math/RobotisTrajectoryCalculator.h @@ -0,0 +1,37 @@ +/* + * RobotisTrajectoryCalculator.h + * + * Created on: Mar 18, 2016 + * Author: jay + */ + +#ifndef ROBOTIS_TRAJECTORY_CALCULATOR_H_ +#define ROBOTIS_TRAJECTORY_CALCULATOR_H_ + + +#include "RobotisMathBase.h" +#include "RobotisLinearAlgebra.h" + +// minimum jerk trajectory + +namespace ROBOTIS +{ + +Eigen::MatrixXd minimum_jerk_tra( double pos_start , double vel_start , double accel_start, + double pos_end , double vel_end , double accel_end, + double smp_time , double mov_time ); + +Eigen::MatrixXd minimum_jerk_tra_via_n_qdqddq( int via_num, + double pos_start , double vel_start , double accel_start , + Eigen::MatrixXd pos_via, Eigen::MatrixXd vel_via, Eigen::MatrixXd accel_via, + double pos_end, double vel_end, double accel_end, + double smp_time, Eigen::MatrixXd via_time, double mov_time ); + +Eigen::MatrixXd arc3d_tra( double smp_time, double mov_time, + Eigen::MatrixXd center_point, Eigen::MatrixXd normal_vector, Eigen::MatrixXd start_point, + double rotation_angle, double cross_ratio ); + +} + + +#endif /* ROBOTIS_TRAJECTORY_CALCULATOR_H_ */ diff --git a/robotis_math/package.xml b/robotis_math/package.xml new file mode 100644 index 0000000..895ab75 --- /dev/null +++ b/robotis_math/package.xml @@ -0,0 +1,37 @@ + + + robotis_math + 0.0.0 + The robotis_math package + + + + + jay + + + + + + TODO + + + + + + + + + + + + + + catkin + roscpp + cmake_modules + + roscpp + cmake_modules + + \ No newline at end of file diff --git a/robotis_math/src/RobotisLinearAlgebra.cpp b/robotis_math/src/RobotisLinearAlgebra.cpp new file mode 100644 index 0000000..30bb5ce --- /dev/null +++ b/robotis_math/src/RobotisLinearAlgebra.cpp @@ -0,0 +1,291 @@ +/* + * RobotisLinearAlgebra.cpp + * + * Created on: Mar 18, 2016 + * Author: jay + */ + + +#include "robotis_math/RobotisLinearAlgebra.h" + +namespace ROBOTIS +{ + +Eigen::MatrixXd transitionXYZ ( double position_x, double position_y, double position_z ) +{ + Eigen::MatrixXd _position(3,1); + + _position << position_x, + position_y, + position_z; + + return _position; +} + +Eigen::MatrixXd transformationXYZRPY ( double position_x, double position_y, double position_z , double roll , double pitch , double yaw ) +{ +// Eigen::MatrixXd _position(3,1); +// +// _position << position_x, +// position_y, +// position_z; +// +// Eigen::MatrixXd _rotation = rpy2rotation( roll , pitch , yaw ); +// +// Eigen::MatrixXd _transformation(4,4); +// +// _transformation << _rotation , _position, +// 0 , 0 , 0 , 1; + + Eigen::MatrixXd _transformation = rotation4d(roll, pitch, yaw); + _transformation.coeffRef(0,3) = position_x; + _transformation.coeffRef(1,3) = position_y; + _transformation.coeffRef(2,3) = position_z; + + return _transformation; +} + +Eigen::MatrixXd InverseTransformation(Eigen::MatrixXd transform) +{ + Eigen::Vector3d vecBOA; //If T is Transform Matrix A from B, the BOA is translation component coordi. B to coordi. A + Eigen::Vector3d vec_x, vec_y, vec_z; + Eigen::MatrixXd _invT(4,4); + + vecBOA.coeffRef(0) = -transform(0,3); vecBOA(1) = -transform(1,3); vecBOA(2) = -transform(2,3); + vec_x(0) = transform(0,0); vec_x(1) = transform(1,0); vec_x(2) = transform(2,0); + vec_y(0) = transform(0,1); vec_y(1) = transform(1,1); vec_y(2) = transform(2,1); + vec_z(0) = transform(0,2); vec_z(1) = transform(1,2); vec_z(2) = transform(2,2); +// +// +// // inv = [ x' | -AtoB??x ] +// // [ y' | -AtoB??y ] +// // [ z' | -AtoB??z ] +// // [ 0 0 0 | 1 ] +// + _invT << vec_x(0), vec_x(1), vec_x(2), vecBOA.dot(vec_x), + vec_y(0), vec_y(1), vec_y(2), vecBOA.dot(vec_y), + vec_z(0), vec_z(1), vec_z(2), vecBOA.dot(vec_z), + 0, 0, 0, 1; + + return _invT; +} + +Eigen::MatrixXd inertiaXYZ( double ixx, double ixy, double ixz , double iyy , double iyz, double izz ) +{ + Eigen::MatrixXd _inertia(3,3); + + _inertia << ixx , ixy , ixz, + ixy , iyy , iyz, + ixz , iyz , izz ; + + return _inertia; +} + +Eigen::MatrixXd rotationX( double angle ) +{ + Eigen::MatrixXd _rotation( 3 , 3 ); + + _rotation << 1.0, 0.0, 0.0, + 0.0, cos( angle ), -sin( angle ), + 0.0, sin( angle ), cos( angle ); + + return _rotation; +} + +Eigen::MatrixXd rotationY( double angle ) +{ + Eigen::MatrixXd _rotation( 3 , 3 ); + + _rotation << cos( angle ), 0.0, sin( angle ), + 0.0, 1.0, 0.0, + -sin( angle ), 0.0, cos( angle ); + + return _rotation; +} + +Eigen::MatrixXd rotationZ( double angle ) +{ + Eigen::MatrixXd _rotation(3,3); + + _rotation << cos( angle ), -sin( angle ), 0.0, + sin( angle ), cos( angle ), 0.0, + 0.0, 0.0, 1.0; + + return _rotation; +} + +Eigen::MatrixXd rotation2rpy( Eigen::MatrixXd rotation ) +{ + Eigen::MatrixXd _rpy = Eigen::MatrixXd::Zero( 3 , 1 ); + + _rpy.coeffRef( 0 , 0 ) = atan2( rotation.coeff( 2 , 1 ), rotation.coeff( 2 , 2 ) ); + _rpy.coeffRef( 1 , 0 ) = atan2( -rotation.coeff( 2 , 0 ), sqrt( pow( rotation.coeff( 2 , 1 ) , 2 ) + pow( rotation.coeff( 2 , 2 ) , 2 ) ) ); + _rpy.coeffRef( 2 , 0 ) = atan2 ( rotation.coeff( 1 , 0 ) , rotation.coeff( 0 , 0 ) ); + + return _rpy; +} + +Eigen::MatrixXd rpy2rotation( double roll, double pitch, double yaw ) +{ + Eigen::MatrixXd _rotation = rotationZ( yaw ) * rotationY( pitch ) * rotationX( roll ); + + return _rotation; +} + +Eigen::Quaterniond rpy2quaternion( double roll, double pitch, double yaw ) +{ + Eigen::MatrixXd _rotation = rpy2rotation( roll, pitch, yaw ); + + Eigen::Matrix3d _rotation3d; + _rotation3d = _rotation.block<3,3>( 0 , 0); + + Eigen::Quaterniond _quaternion; + + _quaternion = _rotation3d; + + return _quaternion; +} + +Eigen::Quaterniond rotation2quaternion( Eigen::MatrixXd rotation ) +{ + Eigen::Matrix3d _rotation3d; + + _rotation3d = rotation.block<3,3>( 0 , 0); + + Eigen::Quaterniond _quaternion; + _quaternion = _rotation3d; + + return _quaternion; +} + +Eigen::MatrixXd quaternion2rpy( Eigen::Quaterniond quaternion ) +{ + Eigen::MatrixXd _rpy = rotation2rpy( quaternion.toRotationMatrix() ); + + return _rpy; +} + +Eigen::MatrixXd quaternion2rotation( Eigen::Quaterniond quaternion ) +{ + Eigen::MatrixXd _rotation = quaternion.toRotationMatrix(); + + return _rotation; +} + +Eigen::MatrixXd rotation4d( double roll, double pitch, double yaw ) +{ +// Eigen::MatrixXd _rotation4d = Eigen::MatrixXd::Zero( 4 , 4 ); +// _rotation4d.coeffRef( 3 , 3 ) = 1.0; +// +// Eigen::MatrixXd _rotation = rotationZ( yaw ) * rotationY( pitch ) * rotationX( roll ); +// +//// _rotation4d.block<3,3>(0,0) = _rotation; +// _rotation4d.coeffRef(0,0) = _rotation.coeff(0,0); +// _rotation4d.coeffRef(0,1) = _rotation.coeff(0,1); +// _rotation4d.coeffRef(0,2) = _rotation.coeff(0,2); +// _rotation4d.coeffRef(1,0) = _rotation.coeff(1,0); +// _rotation4d.coeffRef(1,1) = _rotation.coeff(1,1); +// _rotation4d.coeffRef(1,2) = _rotation.coeff(1,2); +// _rotation4d.coeffRef(2,0) = _rotation.coeff(2,0); +// _rotation4d.coeffRef(2,1) = _rotation.coeff(2,1); +// _rotation4d.coeffRef(2,2) = _rotation.coeff(2,2); + +// return _rotation4d; + double sr = sin(roll), cr = cos(roll); + double sp = sin(pitch), cp = cos(pitch); + double sy = sin(yaw), cy = cos(yaw); + + Eigen::MatrixXd matRoll(4,4); + Eigen::MatrixXd matPitch(4,4); + Eigen::MatrixXd matYaw(4,4); + + matRoll << 1, 0, 0, 0, + 0, cr, -sr, 0, + 0, sr, cr, 0, + 0, 0, 0, 1; + + matPitch << cp, 0, sp, 0, + 0, 1, 0, 0, + -sp, 0, cp, 0, + 0, 0, 0, 1; + + matYaw << cy, -sy, 0, 0, + sy, cy, 0, 0, + 0, 0, 1, 0, + 0, 0, 0, 1; + + + return (matYaw*matPitch)*matRoll; +} + + + +Eigen::MatrixXd hatto( Eigen::MatrixXd matrix3d ) +{ + Eigen::MatrixXd _hatto(3,3); + + _hatto << 0.0, -matrix3d.coeff(2,0), matrix3d.coeff(1,0), + matrix3d.coeff(2,0), 0.0, -matrix3d.coeff(0,0), + -matrix3d.coeff(1,0), matrix3d.coeff(0,0), 0.0; + + return _hatto; +} + +Eigen::MatrixXd Rodrigues( Eigen::MatrixXd hat_matrix , double angle ) +{ + Eigen::MatrixXd _E = Eigen::MatrixXd::Identity( 3 , 3 ); + Eigen::MatrixXd _Rodrigues = _E + hat_matrix * sin( angle ) + hat_matrix * hat_matrix * ( 1 - cos( angle ) ); + + return _Rodrigues; +} + +Eigen::MatrixXd rot2omega( Eigen::MatrixXd rotation ) +{ + double _eps = 1e-12; + +// Eigen::MatrixXd _E = Eigen::MatrixXd::Identity( 3 , 3 ); + + double _alpha = ( rotation.coeff( 0 , 0 ) + rotation.coeff( 1 , 1 ) + rotation.coeff( 2 , 2 ) - 1.0 ) / 2.0 ; + + double _alpha_plus = fabs( _alpha - 1.0 ); + + Eigen::MatrixXd _rot2omega( 3 , 1 ); + + if( _alpha_plus < _eps ) + { + _rot2omega << 0.0, + 0.0, + 0.0; + } + else + { + double _theta = acos( _alpha ); + + _rot2omega << rotation.coeff( 2 , 1 ) - rotation.coeff( 1 , 2 ), + rotation.coeff( 0 , 2 ) - rotation.coeff( 2 , 0 ), + rotation.coeff( 1 , 0 ) - rotation.coeff( 0 , 1 ); + + _rot2omega = 0.5 * ( _theta / sin( _theta ) ) * _rot2omega; + } + + return _rot2omega; +} + +Eigen::MatrixXd cross(Eigen::MatrixXd vector3d_a, Eigen::MatrixXd vector3d_b ) +{ + Eigen::MatrixXd _cross( 3 , 1 ); + + _cross << vector3d_a.coeff( 1 , 0 ) * vector3d_b.coeff( 2 , 0 ) - vector3d_a.coeff( 2 , 0 ) * vector3d_b.coeff( 1 , 0 ), + vector3d_a.coeff( 2 , 0 ) * vector3d_b.coeff( 0 , 0 ) - vector3d_a.coeff( 0 , 0 ) * vector3d_b.coeff( 2 , 0 ), + vector3d_a.coeff( 0 , 0 ) * vector3d_b.coeff( 1 , 0 ) - vector3d_a.coeff( 1 , 0 ) * vector3d_b.coeff( 0 , 0 ); + + return _cross; +} + +double dot(Eigen::MatrixXd vector3d_a, Eigen::MatrixXd vector3d_b ) +{ + return vector3d_a.dot(vector3d_b); + //return vector3d_a.coeff(0,0)*vector3d_b.coeff(0,0) + vector3d_a.coeff(1,0)*vector3d_b.coeff(1,0) + vector3d_a.coeff(2,0)*vector3d_b.coeff(2,0); +} + +} diff --git a/robotis_math/src/RobotisMathBase.cpp b/robotis_math/src/RobotisMathBase.cpp new file mode 100644 index 0000000..0afb845 --- /dev/null +++ b/robotis_math/src/RobotisMathBase.cpp @@ -0,0 +1,26 @@ +/* + * RobotisMathBase.cpp + * + * Created on: Mar 18, 2016 + * Author: jay + */ + +#include "robotis_math/RobotisMathBase.h" + + + + +namespace ROBOTIS +{ + +double sign( double x ) +{ + if ( x < 0.0 ) + return -1.0; + else if ( x > 0.0) + return 1.0; + else + return 0.0; +} + +} diff --git a/robotis_math/src/RobotisTrajectoryCalculator.cpp b/robotis_math/src/RobotisTrajectoryCalculator.cpp new file mode 100644 index 0000000..665015b --- /dev/null +++ b/robotis_math/src/RobotisTrajectoryCalculator.cpp @@ -0,0 +1,325 @@ +/* + * RobotisTrajectoryCalculator.cpp + * + * Created on: Mar 18, 2016 + * Author: jay + */ + + + +#include "robotis_math/RobotisTrajectoryCalculator.h" + + +/* + * trajectory.cpp + * + * Created on: Jul 13, 2015 + * Author: sch + */ + + + +namespace ROBOTIS +{ + +Eigen::MatrixXd minimum_jerk_tra( double pos_start , double vel_start , double accel_start, + double pos_end , double vel_end , double accel_end, + double smp_time , double mov_time ) +/* + simple minimum jerk trajectory + + pos_start : position at initial state + vel_start : velocity at initial state + accel_start : acceleration at initial state + + pos_end : position at final state + vel_end : velocity at final state + accel_end : acceleration at final state + + smp_time : sampling time + + mov_time : movement time +*/ + +{ + Eigen::MatrixXd ___C( 3 , 3 ); + Eigen::MatrixXd __C( 3 , 1 ); + + ___C << pow( mov_time , 3 ) , pow( mov_time , 4 ) , pow( mov_time , 5 ), + 3 * pow( mov_time , 2 ) , 4 * pow( mov_time , 3 ) , 5 * pow( mov_time , 4 ), + 6 * mov_time , 12 * pow( mov_time , 2 ) , 20 * pow( mov_time , 3 ); + + __C << pos_end - pos_start - vel_start * mov_time - accel_start * pow( mov_time , 2 ) / 2, + vel_end - vel_start - accel_start * mov_time, + accel_end - accel_start ; + + Eigen::Matrix _A = ___C.inverse() * __C; + + double _time_steps; + + _time_steps = mov_time / smp_time; + int __time_steps = round( _time_steps + 1 ); + + Eigen::MatrixXd _time = Eigen::MatrixXd::Zero( __time_steps , 1 ); + Eigen::MatrixXd _tra = Eigen::MatrixXd::Zero( __time_steps , 1 ); + + for ( int step = 0; step < __time_steps; step++ ) + _time.coeffRef( step , 0 ) = step * smp_time; + + for ( int step = 0; step < __time_steps; step++ ) + { + _tra.coeffRef( step , 0 ) = + pos_start + + vel_start * _time.coeff( step , 0 ) + + 0.5 * accel_start * pow( _time.coeff( step , 0 ) , 2 ) + + _A.coeff( 0 , 0 ) * pow( _time.coeff( step , 0 ) , 3 ) + + _A.coeff( 1 , 0 ) * pow( _time.coeff( step , 0 ) , 4 ) + + _A.coeff( 2 , 0 ) * pow( _time.coeff( step , 0 ) , 5 ); + } + + return _tra; +} + +Eigen::MatrixXd minimum_jerk_tra_via_n_qdqddq( int via_num, + double pos_start , double vel_start , double accel_start , + Eigen::MatrixXd pos_via, Eigen::MatrixXd vel_via, Eigen::MatrixXd accel_via, + double pos_end, double vel_end, double accel_end, + double smp_time, Eigen::MatrixXd via_time, double mov_time ) +/* + minimum jerk trajectory with via-points + (via-point constraints: position and velocity at each point) + + n : the number of via-points + + x0 : position at initial state + v0 : velocity at initial state + a0 : acceleration at initial state + + x : position matrix at via-points state ( size : n x 1 ) + dx : velocity matrix at via-points state ( size : n x 1 ) + ddx : acceleration matrix at via-points state ( size : n x 1 ) + + xf : position at final state + vf : velocity at final state + af : acceleration at final state + + smp : sampling time + + t : time matrix passing through via-points state ( size : n x 1 ) + tf : movement time +*/ + +{ + int i,j,k ; + + /* Calculation Matrix B */ + + Eigen::MatrixXd B = Eigen::MatrixXd::Zero(3*via_num+3,1); + + for (i=1; i<=via_num; i++){ + B.coeffRef(3*i-3,0) = + pos_via.coeff(i-1,0) - + pos_start - + vel_start*via_time.coeff(i-1,0) - + (accel_start/2)*pow(via_time.coeff(i-1,0),2) ; + + B.coeffRef(3*i-2,0) = + vel_via.coeff(i-1,0) - + vel_start - + accel_start*via_time.coeff(i-1,0) ; + + B.coeffRef(3*i-1,0) = + accel_via.coeff(i-1,0) - + accel_start ; + } + + B.coeffRef(3*via_num,0) = + pos_end - + pos_start - + vel_start*mov_time - + (accel_start/2)*pow(mov_time,2) ; + + B.coeffRef(3*via_num+1,0) = + vel_end - + vel_start - + accel_start*mov_time ; + + B.coeffRef(3*via_num+2,0) = + accel_end - + accel_start ; + + + /* Calculation Matrix A */ + + Eigen::MatrixXd A1 = Eigen::MatrixXd::Zero(3*via_num,3); + + for (i=1; i<=via_num; i++){ + A1.coeffRef(3*i-3,0) = pow(via_time.coeff(i-1,0),3) ; + A1.coeffRef(3*i-3,1) = pow(via_time.coeff(i-1,0),4) ; + A1.coeffRef(3*i-3,2) = pow(via_time.coeff(i-1,0),5) ; + + A1.coeffRef(3*i-2,0) = 3*pow(via_time.coeff(i-1,0),2) ; + A1.coeffRef(3*i-2,1) = 4*pow(via_time.coeff(i-1,0),3) ; + A1.coeffRef(3*i-2,2) = 5*pow(via_time.coeff(i-1,0),4) ; + + A1.coeffRef(3*i-1,0) = 6*via_time.coeff(i-1,0) ; + A1.coeffRef(3*i-1,1) = 12*pow(via_time.coeff(i-1,0),2) ; + A1.coeffRef(3*i-1,2) = 20*pow(via_time.coeff(i-1,0),3) ; + } + + + Eigen::MatrixXd A2 = Eigen::MatrixXd::Zero(3*via_num,3*via_num); + + for (i=1; i<=via_num; i++){ + for (j=1; j<=via_num; j++){ + if (i > j){ + k = i ; + }else{ + k = j ; + } + A2.coeffRef(3*j-3,3*i-3) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),3)/6 ; + A2.coeffRef(3*j-3,3*i-2) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),4)/24 ; + A2.coeffRef(3*j-3,3*i-1) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),5)/120 ; + + A2.coeffRef(3*j-2,3*i-3) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),2)/2 ; + A2.coeffRef(3*j-2,3*i-2) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),3)/6 ; + A2.coeffRef(3*j-2,3*i-1) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),4)/24 ; + + A2.coeffRef(3*j-1,3*i-3) = via_time.coeff(k-1,0)-via_time.coeff(i-1,0) ; + A2.coeffRef(3*j-1,3*i-2) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),2)/2 ; + A2.coeffRef(3*j-1,3*i-1) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),3)/6 ; + } + } + + + Eigen::MatrixXd A3 = Eigen::MatrixXd::Zero(3,3*via_num+3); + + A3.coeffRef(0,0) = pow(mov_time,3); + A3.coeffRef(0,1) = pow(mov_time,4); + A3.coeffRef(0,2) = pow(mov_time,5); + + A3.coeffRef(1,0) = 3*pow(mov_time,2); + A3.coeffRef(1,1) = 4*pow(mov_time,3); + A3.coeffRef(1,2) = 5*pow(mov_time,4); + + A3.coeffRef(2,0) = 6*mov_time; + A3.coeffRef(2,1) = 12*pow(mov_time,2); + A3.coeffRef(2,2) = 20*pow(mov_time,3); + + for (i=1; i<=via_num; i++){ + A3.coeffRef(0,3*i) = pow(mov_time-via_time.coeff(i-1,0),3)/6 ; + A3.coeffRef(1,3*i) = pow(mov_time-via_time.coeff(i-1,0),2)/2 ; + A3.coeffRef(2,3*i) = mov_time-via_time.coeff(i-1,0) ; + + A3.coeffRef(0,3*i+1) = pow(mov_time-via_time.coeff(i-1,0),4)/24 ; + A3.coeffRef(1,3*i+1) = pow(mov_time-via_time.coeff(i-1,0),3)/6 ; + A3.coeffRef(2,3*i+1) = pow(mov_time-via_time.coeff(i-1,0),2)/2 ; + + A3.coeffRef(0,3*i+2) = pow(mov_time-via_time.coeff(i-1,0),5)/120 ; + A3.coeffRef(1,3*i+2) = pow(mov_time-via_time.coeff(i-1,0),4)/24 ; + A3.coeffRef(2,3*i+2) = pow(mov_time-via_time.coeff(i-1,0),3)/6 ; + } + + Eigen::MatrixXd A = Eigen::MatrixXd::Zero(3*via_num+3,3*via_num+3); + + A.block(0,0,3*via_num,3) = A1 ; + A.block(0,3,3*via_num,3*via_num) = A2 ; + A.block(3*via_num,0,3,3*via_num+3) = A3 ; + + /* Calculation Matrix C (coefficient of polynomial function) */ + + Eigen::MatrixXd C(3*via_num+3,1); + //C = A.inverse()*B; + C = A.colPivHouseholderQr().solve(B); + + /* Time */ + + int NN; + double N; + + N = mov_time/smp_time ; + NN = round(N) ; + + Eigen::MatrixXd Time = Eigen::MatrixXd::Zero(NN+1,1); + + for (i=1; i<=NN+1; i++){ + Time.coeffRef(i-1,0) = (i-1)*smp_time; + } + + /* Time_via */ + + Eigen::MatrixXd Time_via = Eigen::MatrixXd::Zero(via_num,1); + + for (i=1; i<=via_num; i++){ + Time_via.coeffRef(i-1,0) = round(via_time.coeff(i-1,0)/smp_time)+2; + } + + /* Minimum Jerk Trajectory with Via-points */ + + Eigen::MatrixXd _tra_jerk_via = Eigen::MatrixXd::Zero(NN+1,1); + + for (i=1; i<=NN+1; i++){ + _tra_jerk_via.coeffRef(i-1,0) = + pos_start + + vel_start*Time.coeff(i-1,0) + + 0.5*accel_start*pow(Time.coeff(i-1,0),2) + + C.coeff(0,0)*pow(Time.coeff(i-1,0),3) + + C.coeff(1,0)*pow(Time.coeff(i-1,0),4) + + C.coeff(2,0)*pow(Time.coeff(i-1,0),5) ; + } + + for (i=1; i<=via_num; i++){ + for (j=Time_via.coeff(i-1,0); j<=NN+1; j++){ + _tra_jerk_via.coeffRef(j-1,0) = + _tra_jerk_via.coeff(j-1,0) + + C.coeff(3*i,0)*pow((Time.coeff(j-1,0)-via_time.coeff(i-1,0)),3)/6 + + C.coeff(3*i+1,0)*pow((Time.coeff(j-1,0)-via_time.coeff(i-1,0)),4)/24 + + C.coeff(3*i+2,0)*pow((Time.coeff(j-1,0)-via_time.coeff(i-1,0)),5)/120 ; + + } + } + + return _tra_jerk_via; + +} + +Eigen::MatrixXd arc3d_tra( double smp_time, double mov_time, + Eigen::MatrixXd center_point, Eigen::MatrixXd normal_vector, Eigen::MatrixXd start_point, + double rotation_angle, double cross_ratio ) +{ + int _all_time_steps = int ( round( mov_time / smp_time ) ) + 1 ; + + Eigen::MatrixXd _angle_tra = minimum_jerk_tra( 0.0 , 0.0 , 0.0 , + rotation_angle , 0.0 , 0.0 , + smp_time , mov_time ); + + Eigen::MatrixXd _pt = Eigen::MatrixXd::Zero( 3 , _all_time_steps ); + + for (int i = 0; i < _all_time_steps; i++ ) + { + double _t = ( ( double ) i ) * smp_time ; + + double _th = _angle_tra.coeff( i , 0 );//( _t / mov_time ) * rotation_angle; + + Eigen::MatrixXd _w_wedge( 3 , 3 ); + + _w_wedge << 0.0 , -normal_vector.coeff(2,0), normal_vector.coeff(1,0), + normal_vector.coeff(2,0), 0.0 , -normal_vector.coeff(0,0), + -normal_vector.coeff(1,0), normal_vector.coeff(0,0), 0.0 ; + + Eigen::MatrixXd _E = Eigen::MatrixXd::Identity( 3 , 3 ); + + Eigen::MatrixXd _R = _E + _w_wedge * sin( _th ) + _w_wedge * _w_wedge * ( 1 - cos( _th ) ); + + double _cross = cross_ratio * ( 1.0 - 2.0 * ( abs ( 0.5 - _t/mov_time ) ) ); + + _pt.block( 0 , i , 3 , 1 ) = ( 1 + _cross ) * ( _R * ( start_point - center_point ) ) + center_point; + } + + Eigen::MatrixXd _pt_trans = _pt.transpose(); + + return _pt_trans; +} + + +} From 49f1fd9586e196e9838d6895f6b7e035cfa5124b Mon Sep 17 00:00:00 2001 From: ROBOTIS-zerom Date: Wed, 4 May 2016 17:23:11 +0900 Subject: [PATCH 014/238] Update README.md --- README.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/README.md b/README.md index 73be6ba..a1ba62c 100644 --- a/README.md +++ b/README.md @@ -1,2 +1,4 @@ # ROBOTIS-Framework ROBOTIS Framework GIT REP MAIN + +Documents : [ROBOTIS Framework WIKI](https://github.com/ROBOTIS-GIT/ROBOTIS-Documents/wiki/ROBOTIS-ROS-Framework-Documents) From 9e1300eedafd278fd7ba86c1d70d4dbcea335d46 Mon Sep 17 00:00:00 2001 From: ROBOTIS-zerom Date: Mon, 16 May 2016 16:32:26 +0900 Subject: [PATCH 015/238] - XM-430 / CM-740 device file added. - Sensor device added. --- .../robotis_controller/RobotisController.h | 1 + .../robotis_controller/RobotisController.cpp | 301 ++++++++++-------- robotis_device/CMakeLists.txt | 1 + .../devices/dynamixel/XM-430.device | 77 +++++ robotis_device/devices/sensor/CM-740.device | 25 ++ .../include/robotis_device/Device.h | 37 +++ .../include/robotis_device/Dynamixel.h | 19 +- robotis_device/include/robotis_device/Robot.h | 5 +- .../include/robotis_device/Sensor.h | 16 +- .../src/robotis_device/Dynamixel.cpp | 14 +- robotis_device/src/robotis_device/Robot.cpp | 184 +++++++++-- robotis_device/src/robotis_device/Sensor.cpp | 21 ++ 12 files changed, 513 insertions(+), 188 deletions(-) create mode 100644 robotis_device/devices/dynamixel/XM-430.device create mode 100644 robotis_device/devices/sensor/CM-740.device create mode 100644 robotis_device/include/robotis_device/Device.h create mode 100644 robotis_device/src/robotis_device/Sensor.cpp diff --git a/robotis_controller/include/robotis_controller/RobotisController.h b/robotis_controller/include/robotis_controller/RobotisController.h index 6db923b..c28256c 100644 --- a/robotis_controller/include/robotis_controller/RobotisController.h +++ b/robotis_controller/include/robotis_controller/RobotisController.h @@ -94,6 +94,7 @@ public: static RobotisController *GetInstance() { return unique_instance_; } bool Initialize(const std::string robot_file_path, const std::string init_file_path); + void DeviceInit(const std::string init_file_path); void Process(); void AddMotionModule(MotionModule *module); diff --git a/robotis_controller/src/robotis_controller/RobotisController.cpp b/robotis_controller/src/robotis_controller/RobotisController.cpp index 20855f6..60cdf91 100644 --- a/robotis_controller/src/robotis_controller/RobotisController.cpp +++ b/robotis_controller/src/robotis_controller/RobotisController.cpp @@ -68,17 +68,6 @@ void RobotisController::InitSyncWrite() std::string _joint_name = _it->first; Dynamixel *_dxl = _it->second; - CONTROL_MODE _ctrl_mode = POSITION_CONTROL; - - for(std::list::iterator _m_it = motion_modules_.begin(); _m_it != motion_modules_.end(); _m_it++) - { - if((*_m_it)->module_name == _dxl->ctrl_module_name) - { - _ctrl_mode = (*_m_it)->control_mode; - break; - } - } - for(int _i = 0; _i < _dxl->bulk_read_items.size(); _i++) { UINT32_T _read_data = 0; @@ -102,24 +91,17 @@ void RobotisController::InitSyncWrite() _dxl->dxl_state->present_position = _dxl->ConvertValue2Radian(_read_data) - _dxl->dxl_state->position_offset; // remove offset _dxl->dxl_state->goal_position = _dxl->dxl_state->present_position; - if(_ctrl_mode == POSITION_CONTROL) - port_to_sync_write_position[_dxl->port_name]->AddParam(_dxl->id, _sync_write_data); + port_to_sync_write_position[_dxl->port_name]->AddParam(_dxl->id, _sync_write_data); } else if(_dxl->present_velocity_item != 0 && _dxl->bulk_read_items[_i]->item_name == _dxl->present_velocity_item->item_name) { _dxl->dxl_state->present_velocity = _dxl->ConvertValue2Velocity(_read_data); _dxl->dxl_state->goal_velocity = _dxl->dxl_state->present_velocity; - - if(_ctrl_mode == VELOCITY_CONTROL) - port_to_sync_write_velocity[_dxl->port_name]->AddParam(_dxl->id, _sync_write_data); } else if(_dxl->present_current_item != 0 && _dxl->bulk_read_items[_i]->item_name == _dxl->present_current_item->item_name) { _dxl->dxl_state->present_current = _dxl->ConvertValue2Current(_read_data); _dxl->dxl_state->goal_current = _dxl->dxl_state->present_current; - - if(_ctrl_mode == CURRENT_CONTROL) - port_to_sync_write_torque[_dxl->port_name]->AddParam(_dxl->id, _sync_write_data); } } } @@ -139,8 +121,6 @@ bool RobotisController::Initialize(const std::string robot_file_path, const std: return true; } - // TODO: TEMPORARY CODE !! - /* temporary code start */ for(std::map::iterator _it = robot->ports.begin(); _it != robot->ports.end(); _it++) { std::string _port_name = _it->first; @@ -154,31 +134,40 @@ bool RobotisController::Initialize(const std::string robot_file_path, const std: exit(-1); } - Dynamixel *_port_default_dxl = robot->dxls[robot->port_default_joint[_port_name]]; - if(_port_default_dxl != NULL) - _default_pkt_handler = PacketHandler::GetPacketHandler(_port_default_dxl->protocol_version); + // get the default device info of the port + std::string _default_device_name = robot->port_default_device[_port_name]; + std::map::iterator _dxl_it = robot->dxls.find(_default_device_name); + std::map::iterator _sensor_it = robot->sensors.find(_default_device_name); + if(_dxl_it != robot->dxls.end()) + { + Dynamixel *_default_device = _dxl_it->second; + _default_pkt_handler = PacketHandler::GetPacketHandler(_default_device->protocol_version); + + port_to_sync_write_position[_port_name] = new GroupSyncWrite(_port, + _default_pkt_handler, + _default_device->goal_position_item->address, + _default_device->goal_position_item->data_length); + + port_to_sync_write_velocity[_port_name] = new GroupSyncWrite(_port, + _default_pkt_handler, + _default_device->goal_velocity_item->address, + _default_device->goal_velocity_item->data_length); + + port_to_sync_write_torque[_port_name] = new GroupSyncWrite(_port, + _default_pkt_handler, + _default_device->goal_current_item->address, + _default_device->goal_current_item->data_length); + } + else if(_sensor_it != robot->sensors.end()) + { + Sensor *_default_device = _sensor_it->second; + _default_pkt_handler = PacketHandler::GetPacketHandler(_default_device->protocol_version); + } port_to_bulk_read[_port_name] = new GroupBulkRead(_port, _default_pkt_handler); - - port_to_sync_write_position[_port_name] = new GroupSyncWrite(_port, - _default_pkt_handler, - _port_default_dxl->goal_position_item->address, - _port_default_dxl->goal_position_item->data_length); - - port_to_sync_write_velocity[_port_name] = new GroupSyncWrite(_port, - _default_pkt_handler, - _port_default_dxl->goal_velocity_item->address, - _port_default_dxl->goal_velocity_item->data_length); - - port_to_sync_write_torque[_port_name] = new GroupSyncWrite(_port, - _default_pkt_handler, - _port_default_dxl->goal_current_item->address, - _port_default_dxl->goal_current_item->data_length); - } - // TODO: - // for loop ping --> no response : return false + // (for loop) check all dxls are connected. for(std::map::iterator _it = robot->dxls.begin(); _it != robot->dxls.end(); _it++) { std::string _joint_name = _it->first; @@ -188,11 +177,93 @@ bool RobotisController::Initialize(const std::string robot_file_path, const std: { usleep(10*1000); if(Ping(_joint_name) != 0) - ROS_ERROR("JOINT[%s] does NOT response!!", _joint_name.c_str()); + ROS_ERROR("JOINT[%s] does NOT respond!!", _joint_name.c_str()); } } - // joint(dynamixel) initialize + DeviceInit(init_file_path); + + // [ BulkRead ] StartAddress : Present Position , Length : 10 ( Position/Velocity/Current ) + for(std::map::iterator _it = robot->dxls.begin(); _it != robot->dxls.end(); _it++) + { + std::string _joint_name = _it->first; + Dynamixel *_dxl = _it->second; + + if(_dxl == NULL) + continue; + + int _bulkread_start_addr = 0; + int _bulkread_data_length = 0; + + // bulk read default : present position + if(_dxl->present_position_item != 0) + { + _bulkread_start_addr = _dxl->present_position_item->address; + _bulkread_data_length = _dxl->present_position_item->data_length; + } + + // TODO: modifing + std::map::iterator _indirect_addr_it = _dxl->ctrl_table.find(INDIRECT_ADDRESS_1); + if(_indirect_addr_it != _dxl->ctrl_table.end()) // INDIRECT_ADDRESS_1 exist + { + if(_dxl->bulk_read_items.size() != 0) + { + _bulkread_start_addr = _dxl->bulk_read_items[0]->address; + _bulkread_data_length = 0; + + // set indirect address + int _indirect_addr = _indirect_addr_it->second->address; + for(int _i = 0; _i < _dxl->bulk_read_items.size(); _i++) + { + int _addr_leng = _dxl->bulk_read_items[_i]->data_length; + + _bulkread_data_length += _addr_leng; + for(int _l = 0; _l < _addr_leng; _l++) + { +// ROS_WARN("[%12s] INDIR_ADDR: %d, ITEM_ADDR: %d", _joint_name.c_str(), _indirect_addr, _dxl->ctrl_table[_dxl->bulk_read_items[_i]->item_name]->address + _l); + Write2Byte(_joint_name, _indirect_addr, _dxl->ctrl_table[_dxl->bulk_read_items[_i]->item_name]->address + _l); + _indirect_addr += 2; + } + } + } + } + else // INDIRECT_ADDRESS_1 NOT exist + { + if(_dxl->bulk_read_items.size() != 0) + { + _bulkread_start_addr = _dxl->bulk_read_items[0]->address; + _bulkread_data_length = 0; + + ControlTableItem *_last_item = _dxl->bulk_read_items[0]; + + for(int _i = 0; _i < _dxl->bulk_read_items.size(); _i++) + { + int _addr = _dxl->bulk_read_items[_i]->address; + if(_addr < _bulkread_start_addr) + _bulkread_start_addr = _addr; + else if(_last_item->address < _addr) + _last_item = _dxl->bulk_read_items[_i]; + } + + _bulkread_data_length = _last_item->address - _bulkread_start_addr + _last_item->data_length; + } + } + +// ROS_WARN("[%12s] start_addr: %d, data_length: %d", _joint_name.c_str(), _bulkread_start_addr, _bulkread_data_length); + port_to_bulk_read[_dxl->port_name]->AddParam(_dxl->id, _bulkread_start_addr, _bulkread_data_length); + + // Torque ON + if(WriteCtrlItem(_joint_name, _dxl->torque_enable_item->item_name, 1) != COMM_SUCCESS) + WriteCtrlItem(_joint_name, _dxl->torque_enable_item->item_name, 1); + } + + queue_thread_ = boost::thread(boost::bind(&RobotisController::QueueThread, this)); + return true; +} + +void RobotisController::DeviceInit(const std::string init_file_path) +{ + // device initialize if(DEBUG_PRINT) ROS_WARN("INIT FILE LOAD"); YAML::Node _doc; @@ -287,86 +358,6 @@ bool RobotisController::Initialize(const std::string robot_file_path, const std: } catch(const std::exception& e) { ROS_INFO("Dynamixel Init file not found."); } - - - // [ BulkRead ] StartAddress : Present Position , Length : 10 ( Position/Velocity/Current ) - for(std::map::iterator _it = robot->dxls.begin(); _it != robot->dxls.end(); _it++) - { - std::string _joint_name = _it->first; - Dynamixel *_dxl = _it->second; - - if(_dxl == NULL) - continue; - - int _bulkread_start_addr = 0; - int _bulkread_data_length = 0; - - // bulk read default : present position - if(_dxl->present_position_item != 0) - { - _bulkread_start_addr = _dxl->present_position_item->address; - _bulkread_data_length = _dxl->present_position_item->data_length; - } - - // TODO: modifing - std::map::iterator _indirect_addr_it = _dxl->ctrl_table.find(INDIRECT_ADDRESS_1); - if(_indirect_addr_it != _dxl->ctrl_table.end()) // INDIRECT_ADDRESS_1 exist - { - if(_dxl->bulk_read_items.size() != 0) - { - _bulkread_start_addr = _dxl->bulk_read_items[0]->address; - _bulkread_data_length = 0; - - // set indirect address - int _indirect_addr = _indirect_addr_it->second->address; - for(int _i = 0; _i < _dxl->bulk_read_items.size(); _i++) - { - int _addr_leng = _dxl->bulk_read_items[_i]->data_length; - - _bulkread_data_length += _addr_leng; - for(int _l = 0; _l < _addr_leng; _l++) - { -// ROS_WARN("[%12s] INDIR_ADDR: %d, ITEM_ADDR: %d", _joint_name.c_str(), _indirect_addr, _dxl->ctrl_table[_dxl->bulk_read_items[_i]->item_name]->address + _l); - Write2Byte(_joint_name, _indirect_addr, _dxl->ctrl_table[_dxl->bulk_read_items[_i]->item_name]->address + _l); - _indirect_addr += 2; - } - } - } - } - else // INDIRECT_ADDRESS_1 NOT exist - { - if(_dxl->bulk_read_items.size() != 0) - { - _bulkread_start_addr = _dxl->bulk_read_items[0]->address; - _bulkread_data_length = 0; - - ControlTableItem *_last_item = _dxl->bulk_read_items[0]; - - for(int _i = 0; _i < _dxl->bulk_read_items.size(); _i++) - { - int _addr = _dxl->bulk_read_items[_i]->address; - if(_addr < _bulkread_start_addr) - _bulkread_start_addr = _addr; - else if(_last_item->address < _addr) - _last_item = _dxl->bulk_read_items[_i]; - } - - _bulkread_data_length = _last_item->address - _bulkread_start_addr + _last_item->data_length; - } - } - -// ROS_WARN("[%12s] start_addr: %d, data_length: %d", _joint_name.c_str(), _bulkread_start_addr, _bulkread_data_length); - port_to_bulk_read[_dxl->port_name]->AddParam(_dxl->id, _bulkread_start_addr, _bulkread_data_length); - - // Torque ON - if(WriteCtrlItem(_joint_name, _dxl->torque_enable_item->item_name, 1) != COMM_SUCCESS) - WriteCtrlItem(_joint_name, _dxl->torque_enable_item->item_name, 1); - } - - /* temporary code end */ - - queue_thread_ = boost::thread(boost::bind(&RobotisController::QueueThread, this)); - return true; } void RobotisController::GazeboThread() @@ -1120,7 +1111,20 @@ void RobotisController::SetCtrlModuleThread(std::string ctrl_module) // set dxl's control module to "none" for(std::map::iterator _d_it = robot->dxls.begin(); _d_it != robot->dxls.end(); _d_it++) { - _d_it->second->ctrl_module_name = "none"; + Dynamixel *_dxl = _d_it->second; + _dxl->ctrl_module_name = "none"; + + UINT32_T _pos_data = _dxl->ConvertRadian2Value(_dxl->dxl_state->goal_position + _dxl->dxl_state->position_offset); + UINT8_T _sync_write_data[4]; + _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_pos_data)); + _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_pos_data)); + _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_pos_data)); + _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_pos_data)); + + port_to_sync_write_position[_dxl->port_name]->AddParam(_dxl->id, _sync_write_data); + + port_to_sync_write_torque[_dxl->port_name]->RemoveParam(_dxl->id); + port_to_sync_write_velocity[_dxl->port_name]->RemoveParam(_dxl->id); } } else @@ -1131,12 +1135,57 @@ void RobotisController::SetCtrlModuleThread(std::string ctrl_module) // if it exist if((*_m_it)->module_name == ctrl_module) { + CONTROL_MODE _mode = (*_m_it)->control_mode; for(std::map::iterator _result_it = (*_m_it)->result.begin(); _result_it != (*_m_it)->result.end(); _result_it++) { std::map::iterator _d_it = robot->dxls.find(_result_it->first); if(_d_it != robot->dxls.end()) { - _d_it->second->ctrl_module_name = ctrl_module; + Dynamixel *_dxl = _d_it->second; + _dxl->ctrl_module_name = ctrl_module; + + if(_mode == POSITION_CONTROL) + { + UINT32_T _pos_data = _dxl->ConvertRadian2Value(_dxl->dxl_state->goal_position + _dxl->dxl_state->position_offset); + UINT8_T _sync_write_data[4]; + _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_pos_data)); + _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_pos_data)); + _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_pos_data)); + _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_pos_data)); + + port_to_sync_write_position[_dxl->port_name]->AddParam(_dxl->id, _sync_write_data); + + port_to_sync_write_torque[_dxl->port_name]->RemoveParam(_dxl->id); + port_to_sync_write_velocity[_dxl->port_name]->RemoveParam(_dxl->id); + } + else if(_mode == VELOCITY_CONTROL) + { + UINT32_T _vel_data = _dxl->ConvertVelocity2Value(_dxl->dxl_state->goal_velocity); + UINT8_T _sync_write_data[4]; + _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_vel_data)); + _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_vel_data)); + _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_vel_data)); + _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_vel_data)); + + port_to_sync_write_velocity[_dxl->port_name]->AddParam(_dxl->id, _sync_write_data); + + port_to_sync_write_torque[_dxl->port_name]->RemoveParam(_dxl->id); + port_to_sync_write_position[_dxl->port_name]->RemoveParam(_dxl->id); + } + else if(_mode == CURRENT_CONTROL) + { + UINT32_T _curr_data = _dxl->ConvertCurrent2Value(_dxl->dxl_state->goal_current); + UINT8_T _sync_write_data[4]; + _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_curr_data)); + _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_curr_data)); + _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_curr_data)); + _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_curr_data)); + + port_to_sync_write_torque[_dxl->port_name]->AddParam(_dxl->id, _sync_write_data); + + port_to_sync_write_velocity[_dxl->port_name]->RemoveParam(_dxl->id); + port_to_sync_write_position[_dxl->port_name]->RemoveParam(_dxl->id); + } } } diff --git a/robotis_device/CMakeLists.txt b/robotis_device/CMakeLists.txt index efe9e20..8bd7be5 100644 --- a/robotis_device/CMakeLists.txt +++ b/robotis_device/CMakeLists.txt @@ -39,6 +39,7 @@ include_directories( ## Declare a C++ library add_library(robotis_device src/robotis_device/Robot.cpp + src/robotis_device/Sensor.cpp src/robotis_device/Dynamixel.cpp ) diff --git a/robotis_device/devices/dynamixel/XM-430.device b/robotis_device/devices/dynamixel/XM-430.device new file mode 100644 index 0000000..9f17d37 --- /dev/null +++ b/robotis_device/devices/dynamixel/XM-430.device @@ -0,0 +1,77 @@ +[device info] +model_name = XM-430 +device_type = dynamixel + +[type info] +current_ratio = 2.69 + +value_of_0_radian_position = 2048 +value_of_min_radian_position = 0 +value_of_max_radian_position = 4095 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_current +position_d_gain_item_name = position_d_gain +position_i_gain_item_name = position_i_gain +position_p_gain_item_name = position_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 7 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 16 | N + 20 | homing_offset | 4 | RW | EEPROM | -1044479 | 1044479 | Y + 24 | moving_threshold | 4 | RW | EEPROM | 0 | 1023 | N + 31 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 32 | max_voltage_limit | 2 | RW | EEPROM | 95 | 160 | N + 34 | min_voltage_limit | 2 | RW | EEPROM | 95 | 160 | N + 36 | pwm_limit | 2 | RW | EEPROM | 0 | 885 | N + 38 | current_limit | 2 | RW | EEPROM | 0 | 1193 | N + 40 | acceleration_limit | 4 | RW | EEPROM | 0 | 32767 | N + 44 | velocity_limit | 4 | RW | EEPROM | 0 | 1023 | N + 48 | max_position_limit | 4 | RW | EEPROM | 0 | 4095 | N + 52 | min_position_limit | 4 | RW | EEPROM | 0 | 4095 | N + 63 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 64 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 65 | LED | 1 | RW | RAM | 0 | 1 | N + 68 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 69 | registered_instruction | 1 | R | RAM | 0 | 1 | N + 70 | hardware_error_status | 1 | R | RAM | 0 | 255 | N + 76 | velocity_i_gain | 2 | RW | RAM | 0 | 32767 | N + 78 | velocity_p_gain | 2 | RW | RAM | 0 | 32767 | N + 80 | position_d_gain | 2 | RW | RAM | 0 | 32767 | N + 82 | position_i_gain | 2 | RW | RAM | 0 | 32767 | N + 84 | position_p_gain | 2 | RW | RAM | 0 | 32767 | N + 88 | feedforward_2nd_gain | 2 | RW | RAM | 0 | 32767 | N + 90 | feedforward_1st_gain | 2 | RW | RAM | 0 | 32767 | N + 100 | goal_pwm | 2 | RW | RAM | 0 | 885 | N + 102 | goal_current | 2 | RW | RAM | 0 | 1193 | N + 104 | goal_velocity | 4 | RW | RAM | 0 | 1023 | N + 108 | profile_acceleration | 4 | RW | RAM | 0 | 32767 | N + 112 | profile_velocity | 4 | RW | RAM | 0 | 1023 | N + 116 | goal_position | 4 | RW | RAM | 0 | 4095 | N + 120 | realtime_tick | 2 | R | RAM | 0 | 32767 | N + 122 | moving | 1 | R | RAM | 0 | 1 | N + 123 | moving_status | 1 | R | RAM | 0 | 255 | N + 124 | present_pwm | 2 | R | RAM | 0 | 885 | N + 126 | present_current | 2 | R | RAM | 0 | 1193 | N + 128 | present_velocity | 4 | R | RAM | 0 | 1023 | N + 132 | present_position | 4 | R | RAM | 0 | 4095 | N + 136 | velocity_trajectory | 4 | R | RAM | 0 | 1023 | N + 140 | position_trajectory | 4 | R | RAM | 0 | 4095 | N + 144 | present_input_voltage | 2 | R | RAM | 95 | 160 | N + 146 | present_temperature | 1 | R | RAM | 0 | 100 | N + 168 | indirect_address_1 | 2 | RW | RAM | 0 | 65535 | N + 224 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 578 | indirect_address_29 | 2 | RW | RAM | 0 | 65535 | N + 634 | indirect_data_29 | 1 | RW | RAM | 0 | 255 | N diff --git a/robotis_device/devices/sensor/CM-740.device b/robotis_device/devices/sensor/CM-740.device new file mode 100644 index 0000000..700d43a --- /dev/null +++ b/robotis_device/devices/sensor/CM-740.device @@ -0,0 +1,25 @@ +[device info] +model_name = CM-740 +device_type = sensor + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 2 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 3 | ID | 1 | RW | EEPROM | 0 | 252 | N + 4 | baudrate | 1 | RW | EEPROM | 0 | 252 | N + 5 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 16 | status_return_level | 1 | RW | EEPROM | 0 | 2 | N + 24 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 25 | LED | 1 | RW | RAM | 0 | 7 | N + 26 | LED_5 | 2 | RW | RAM | 0 | 32767 | N + 28 | LED_6 | 2 | RW | RAM | 0 | 32767 | N + 30 | button | 1 | RW | RAM | 0 | 3 | N + 38 | gyro_z | 2 | R | RAM | 0 | 1023 | N + 40 | gyro_y | 2 | R | RAM | 0 | 1023 | N + 42 | gyro_x | 2 | R | RAM | 0 | 1023 | N + 44 | acc_x | 2 | R | RAM | 0 | 1023 | N + 46 | acc_y | 2 | R | RAM | 0 | 1023 | N + 48 | acc_z | 2 | R | RAM | 0 | 1023 | N + 50 | present_voltage | 1 | R | RAM | 50 | 250 | N + \ No newline at end of file diff --git a/robotis_device/include/robotis_device/Device.h b/robotis_device/include/robotis_device/Device.h new file mode 100644 index 0000000..d9fd0bf --- /dev/null +++ b/robotis_device/include/robotis_device/Device.h @@ -0,0 +1,37 @@ +/* + * Device.h + * + * Created on: 2016. 5. 12. + * Author: zerom + */ + +#ifndef ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_DEVICE_H_ +#define ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_DEVICE_H_ + + +#include +#include +#include +#include "ControlTableItem.h" + +namespace ROBOTIS +{ + +class Device +{ +public: + UINT8_T id; + float protocol_version; + std::string model_name; + std::string port_name; + + std::map ctrl_table; + std::vector bulk_read_items; + + virtual ~Device() { } +}; + +} + + +#endif /* ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_DEVICE_H_ */ diff --git a/robotis_device/include/robotis_device/Dynamixel.h b/robotis_device/include/robotis_device/Dynamixel.h index 1a65f72..64d4a4a 100644 --- a/robotis_device/include/robotis_device/Dynamixel.h +++ b/robotis_device/include/robotis_device/Dynamixel.h @@ -5,29 +5,23 @@ * Author: zerom */ -#ifndef ROBOTIS_FRAMEWORK_ROBOTIS_CONTROLLER_INCLUDE_DEVICE_DYNAMIXEL_H_ -#define ROBOTIS_FRAMEWORK_ROBOTIS_CONTROLLER_INCLUDE_DEVICE_DYNAMIXEL_H_ +#ifndef ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_DYNAMIXEL_H_ +#define ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_DYNAMIXEL_H_ #include #include #include +#include "Device.h" #include "DynamixelState.h" #include "ControlTableItem.h" namespace ROBOTIS { -class Dynamixel +class Dynamixel : public Device { public: - UINT8_T id; - std::string model_name; - float protocol_version; - - std::map ctrl_table; // string: item name - - std::string port_name; std::string ctrl_module_name; DynamixelState *dxl_state; @@ -48,9 +42,6 @@ public: ControlTableItem *goal_velocity_item; ControlTableItem *goal_current_item; - std::vector bulk_read_items; - std::map indirect_address_table; - Dynamixel(int id, std::string model_name, float protocol_version); double ConvertValue2Radian(INT32_T value); @@ -66,4 +57,4 @@ public: } -#endif /* ROBOTIS_FRAMEWORK_ROBOTIS_CONTROLLER_INCLUDE_DEVICE_DYNAMIXEL_H_ */ +#endif /* ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_DYNAMIXEL_H_ */ diff --git a/robotis_device/include/robotis_device/Robot.h b/robotis_device/include/robotis_device/Robot.h index d169d35..2473e7c 100644 --- a/robotis_device/include/robotis_device/Robot.h +++ b/robotis_device/include/robotis_device/Robot.h @@ -21,14 +21,15 @@ class Robot { public: std::map ports; // string: port name - std::map port_default_joint; // port name, default joint name + std::map port_default_device; // port name, default device name std::map dxls; // string: joint name std::map sensors; // string: sensor name Robot(std::string robot_file_path, std::string dev_desc_dir_path); - Dynamixel *getDynamixel(std::string path, int id, std::string port, float protocol_version); + Sensor *getSensor(std::string path, int id, std::string port, float protocol_version); + Dynamixel *getDynamixel(std::string path, int id, std::string port, float protocol_version); }; } diff --git a/robotis_device/include/robotis_device/Sensor.h b/robotis_device/include/robotis_device/Sensor.h index ca3e28a..78de619 100644 --- a/robotis_device/include/robotis_device/Sensor.h +++ b/robotis_device/include/robotis_device/Sensor.h @@ -5,30 +5,26 @@ * Author: zerom */ -#ifndef ROBOTIS_FRAMEWORK_ROBOTIS_DEVICE_INCLUDE_DEVICE_SENSOR_H_ -#define ROBOTIS_FRAMEWORK_ROBOTIS_DEVICE_INCLUDE_DEVICE_SENSOR_H_ +#ifndef ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_SENSOR_H_ +#define ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_SENSOR_H_ #include #include #include +#include "Device.h" #include "ControlTableItem.h" namespace ROBOTIS { -class Sensor +class Sensor : public Device { + std::map sensed_value; public: - UINT8_T id; - std::string model_name; - float protocol_version; - - std::map ctrl_table; - Sensor(int id, std::string model_name, float protocol_version); }; } -#endif /* ROBOTIS_FRAMEWORK_ROBOTIS_DEVICE_INCLUDE_DEVICE_SENSOR_H_ */ +#endif /* ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_SENSOR_H_ */ diff --git a/robotis_device/src/robotis_device/Dynamixel.cpp b/robotis_device/src/robotis_device/Dynamixel.cpp index 2ccb3a0..daf454d 100644 --- a/robotis_device/src/robotis_device/Dynamixel.cpp +++ b/robotis_device/src/robotis_device/Dynamixel.cpp @@ -5,16 +5,12 @@ * Author: zerom */ -#include "../include/robotis_device/Dynamixel.h" +#include "robotis_device/Dynamixel.h" using namespace ROBOTIS; Dynamixel::Dynamixel(int id, std::string model_name, float protocol_version) - : id(id), - model_name(model_name), - port_name(""), - ctrl_module_name("none"), - protocol_version(protocol_version), + : ctrl_module_name("none"), current_ratio(1.0), velocity_ratio(1.0), value_of_0_radian_position(0), @@ -30,11 +26,15 @@ Dynamixel::Dynamixel(int id, std::string model_name, float protocol_version) goal_velocity_item(0), goal_current_item(0) { + this->id = id; + this->model_name = model_name; + this->port_name = ""; + this->protocol_version = protocol_version; + ctrl_table.clear(); dxl_state = new DynamixelState(); bulk_read_items.clear(); - indirect_address_table.clear(); } double Dynamixel::ConvertValue2Radian(INT32_T value) diff --git a/robotis_device/src/robotis_device/Robot.cpp b/robotis_device/src/robotis_device/Robot.cpp index f397c3c..dc1b50e 100644 --- a/robotis_device/src/robotis_device/Robot.cpp +++ b/robotis_device/src/robotis_device/Robot.cpp @@ -78,7 +78,7 @@ Robot::Robot(std::string robot_file_path, std::string dev_desc_dir_path) ports[tokens[0]] = (PortHandler*)PortHandler::GetPortHandler(tokens[0].c_str()); ports[tokens[0]]->SetBaudRate(std::atoi(tokens[1].c_str())); - port_default_joint[tokens[0]] = tokens[2]; + port_default_device[tokens[0]] = tokens[2]; } else if(session == "device info") { @@ -129,6 +129,49 @@ Robot::Robot(std::string robot_file_path, std::string dev_desc_dir_path) } } } + else if(tokens[0] == "sensor") + { + std::string _file_path = dev_desc_dir_path + tokens[0] + "/" + tokens[3] + ".device"; + int _id = std::atoi(tokens[2].c_str()); + std::string _port = tokens[1]; + float _protocol = std::atof(tokens[4].c_str()); + std::string _dev_name = tokens[5]; + + sensors[_dev_name] = getSensor(_file_path, _id, _port, _protocol); + + Sensor *_sensor = sensors[_dev_name]; + std::vector sub_tokens = split(tokens[6], ','); + if(sub_tokens.size() > 0) + { + std::map::iterator _indirect_it = _sensor->ctrl_table.find(INDIRECT_ADDRESS_1); + if(_indirect_it != _sensor->ctrl_table.end()) // INDIRECT_ADDRESS_1 exist + { + UINT16_T _indirect_data_addr = _sensor->ctrl_table[INDIRECT_DATA_1]->address; + for(int _i = 0; _i < sub_tokens.size(); _i++) + { + _sensor->bulk_read_items.push_back(new ControlTableItem()); + ControlTableItem *_dest_item = _sensor->bulk_read_items[_i]; + ControlTableItem *_src_item = _sensor->ctrl_table[sub_tokens[_i]]; + + _dest_item->item_name = _src_item->item_name; + _dest_item->address = _indirect_data_addr; + _dest_item->access_type = _src_item->access_type; + _dest_item->memory_type = _src_item->memory_type; + _dest_item->data_length = _src_item->data_length; + _dest_item->data_min_value = _src_item->data_min_value; + _dest_item->data_max_value = _src_item->data_max_value; + _dest_item->is_signed = _src_item->is_signed; + + _indirect_data_addr += _dest_item->data_length; + } + } + else // INDIRECT_ADDRESS_1 exist + { + for(int _i = 0; _i < sub_tokens.size(); _i++) + _sensor->bulk_read_items.push_back(_sensor->ctrl_table[sub_tokens[_i]]); + } + } + } } } file.close(); @@ -139,9 +182,92 @@ Robot::Robot(std::string robot_file_path, std::string dev_desc_dir_path) } } +Sensor *Robot::getSensor(std::string path, int id, std::string port, float protocol_version) +{ + Sensor *_sensor; + + // load file model_name.device + std::ifstream file(path.c_str()); + if(file.is_open()) + { + std::string _session = ""; + std::string _input_str; + + while(!file.eof()) + { + std::getline(file, _input_str); + + // remove comment ( # ) + std::size_t pos = _input_str.find("#"); + if(pos != std::string::npos) + _input_str = _input_str.substr(0, pos); + + // trim + _input_str = trim(_input_str); + if(_input_str == "") + continue; + + // find _session + if(!_input_str.compare(0, 1, "[") && !_input_str.compare(_input_str.size()-1, 1, "]")) + { + _input_str = _input_str.substr(1, _input_str.size()-2); + std::transform(_input_str.begin(), _input_str.end(), _input_str.begin(), ::tolower); + _session = trim(_input_str); + continue; + } + + if(_session == "device info") + { + std::vector tokens = split(_input_str, '='); + if(tokens.size() != 2) + continue; + + if(tokens[0] == "model_name") + _sensor = new Sensor(id, tokens[1], protocol_version); + } + else if(_session == "control table") + { + std::vector tokens = split(_input_str, '|'); + if(tokens.size() != 8) + continue; + + ControlTableItem *item = new ControlTableItem(); + item->item_name = tokens[1]; + item->address = std::atoi(tokens[0].c_str()); + item->data_length = std::atoi(tokens[2].c_str()); + if(tokens[3] == "R") + item->access_type = READ; + else if(tokens[3] == "RW") + item->access_type = READ_WRITE; + if(tokens[4] == "EEPROM") + item->memory_type = EEPROM; + else if(tokens[4] == "RAM") + item->memory_type = RAM; + item->data_min_value = std::atol(tokens[5].c_str()); + item->data_max_value = std::atol(tokens[6].c_str()); + if(tokens[7] == "Y") + item->is_signed = true; + else if(tokens[7] == "N") + item->is_signed = false; + _sensor->ctrl_table[tokens[1]] = item; + } + } + _sensor->port_name = port; + + fprintf(stderr, "(%s) [ID:%3d] %14s added. \n", port.c_str(), _sensor->id, _sensor->model_name.c_str()); + //std::cout << "[ID:" << (int)(_sensor->id) << "] " << _sensor->model_name << " added. (" << port << ")" << std::endl; + file.close(); + } + else + std::cout << "Unable to open file : " + path << std::endl; + + return _sensor; +} + Dynamixel *Robot::getDynamixel(std::string path, int id, std::string port, float protocol_version) { - Dynamixel *dxl; + Dynamixel *_dxl; + // load file model_name.device std::ifstream file(path.c_str()); if(file.is_open()) @@ -187,7 +313,7 @@ Dynamixel *Robot::getDynamixel(std::string path, int id, std::string port, float continue; if(tokens[0] == "model_name") - dxl = new Dynamixel(id, tokens[1], protocol_version); + _dxl = new Dynamixel(id, tokens[1], protocol_version); } else if(_session == "type info") { @@ -196,20 +322,20 @@ Dynamixel *Robot::getDynamixel(std::string path, int id, std::string port, float continue; if(tokens[0] == "current_ratio") - dxl->current_ratio = std::atof(tokens[1].c_str()); + _dxl->current_ratio = std::atof(tokens[1].c_str()); else if(tokens[0] == "velocity_ratio") - dxl->velocity_ratio = std::atof(tokens[1].c_str()); + _dxl->velocity_ratio = std::atof(tokens[1].c_str()); else if(tokens[0] == "value_of_0_radian_position") - dxl->value_of_0_radian_position = std::atoi(tokens[1].c_str()); + _dxl->value_of_0_radian_position = std::atoi(tokens[1].c_str()); else if(tokens[0] == "value_of_min_radian_position") - dxl->value_of_min_radian_position = std::atoi(tokens[1].c_str()); + _dxl->value_of_min_radian_position = std::atoi(tokens[1].c_str()); else if(tokens[0] == "value_of_max_radian_position") - dxl->value_of_max_radian_position = std::atoi(tokens[1].c_str()); + _dxl->value_of_max_radian_position = std::atoi(tokens[1].c_str()); else if(tokens[0] == "min_radian") - dxl->min_radian = std::atof(tokens[1].c_str()); + _dxl->min_radian = std::atof(tokens[1].c_str()); else if(tokens[0] == "max_radian") - dxl->max_radian = std::atof(tokens[1].c_str()); + _dxl->max_radian = std::atof(tokens[1].c_str()); else if(tokens[0] == "torque_enable_item_name") _torque_enable_item_name = tokens[1]; @@ -250,33 +376,33 @@ Dynamixel *Robot::getDynamixel(std::string path, int id, std::string port, float item->is_signed = true; else if(tokens[7] == "N") item->is_signed = false; - dxl->ctrl_table[tokens[1]] = item; + _dxl->ctrl_table[tokens[1]] = item; } } - dxl->port_name = port; + _dxl->port_name = port; - if(dxl->ctrl_table[_torque_enable_item_name] != NULL) - dxl->torque_enable_item = dxl->ctrl_table[_torque_enable_item_name]; - if(dxl->ctrl_table[_present_position_item_name] != NULL) - dxl->present_position_item = dxl->ctrl_table[_present_position_item_name]; - if(dxl->ctrl_table[_present_velocity_item_name] != NULL) - dxl->present_velocity_item = dxl->ctrl_table[_present_velocity_item_name]; - if(dxl->ctrl_table[_present_current_item_name] != NULL) - dxl->present_current_item = dxl->ctrl_table[_present_current_item_name]; - if(dxl->ctrl_table[_goal_position_item_name] != NULL) - dxl->goal_position_item = dxl->ctrl_table[_goal_position_item_name]; - if(dxl->ctrl_table[_goal_velocity_item_name] != NULL) - dxl->goal_velocity_item = dxl->ctrl_table[_goal_velocity_item_name]; - if(dxl->ctrl_table[_goal_current_item_name] != NULL) - dxl->goal_current_item = dxl->ctrl_table[_goal_current_item_name]; + if(_dxl->ctrl_table[_torque_enable_item_name] != NULL) + _dxl->torque_enable_item = _dxl->ctrl_table[_torque_enable_item_name]; + if(_dxl->ctrl_table[_present_position_item_name] != NULL) + _dxl->present_position_item = _dxl->ctrl_table[_present_position_item_name]; + if(_dxl->ctrl_table[_present_velocity_item_name] != NULL) + _dxl->present_velocity_item = _dxl->ctrl_table[_present_velocity_item_name]; + if(_dxl->ctrl_table[_present_current_item_name] != NULL) + _dxl->present_current_item = _dxl->ctrl_table[_present_current_item_name]; + if(_dxl->ctrl_table[_goal_position_item_name] != NULL) + _dxl->goal_position_item = _dxl->ctrl_table[_goal_position_item_name]; + if(_dxl->ctrl_table[_goal_velocity_item_name] != NULL) + _dxl->goal_velocity_item = _dxl->ctrl_table[_goal_velocity_item_name]; + if(_dxl->ctrl_table[_goal_current_item_name] != NULL) + _dxl->goal_current_item = _dxl->ctrl_table[_goal_current_item_name]; - fprintf(stderr, "(%s) [ID:%3d] %14s added. \n", port.c_str(), dxl->id, dxl->model_name.c_str()); - //std::cout << "[ID:" << (int)(dxl->id) << "] " << dxl->model_name << " added. (" << port << ")" << std::endl; + fprintf(stderr, "(%s) [ID:%3d] %14s added. \n", port.c_str(), _dxl->id, _dxl->model_name.c_str()); + //std::cout << "[ID:" << (int)(_dxl->id) << "] " << _dxl->model_name << " added. (" << port << ")" << std::endl; file.close(); } else std::cout << "Unable to open file : " + path << std::endl; - return dxl; + return _dxl; } diff --git a/robotis_device/src/robotis_device/Sensor.cpp b/robotis_device/src/robotis_device/Sensor.cpp new file mode 100644 index 0000000..afe4f9f --- /dev/null +++ b/robotis_device/src/robotis_device/Sensor.cpp @@ -0,0 +1,21 @@ +/* + * Sensor.cpp + * + * Created on: 2016. 5. 11. + * Author: zerom + */ + +#include "robotis_device/Sensor.h" + +using namespace ROBOTIS; + +Sensor::Sensor(int id, std::string model_name, float protocol_version) +{ + this->id = id; + this->model_name = model_name; + this->port_name = ""; + this->protocol_version = protocol_version; + ctrl_table.clear(); + + bulk_read_items.clear(); +} From 2f18b587ded62c0e1fd6c2a5d08ec6758bd31095 Mon Sep 17 00:00:00 2001 From: ROBOTIS-zerom Date: Tue, 17 May 2016 20:41:14 +0900 Subject: [PATCH 016/238] - add SensorState - add Singleton template --- .../robotis_controller/RobotisController.h | 9 +- .../robotis_controller/RobotisController.cpp | 129 +++++++++++------- .../include/robotis_device/DynamixelState.h | 17 +-- .../include/robotis_device/Sensor.h | 4 +- .../include/robotis_device/SensorState.h | 35 +++++ .../include/robotis_device/TimeStamp.h | 25 ++++ robotis_device/src/robotis_device/Sensor.cpp | 2 + .../robotis_framework_common/MotionModule.h | 1 + .../robotis_framework_common/SensorModule.h | 3 +- .../robotis_framework_common/Singleton.h | 49 +++++++ 10 files changed, 207 insertions(+), 67 deletions(-) create mode 100644 robotis_device/include/robotis_device/SensorState.h create mode 100644 robotis_device/include/robotis_device/TimeStamp.h create mode 100644 robotis_framework_common/include/robotis_framework_common/Singleton.h diff --git a/robotis_controller/include/robotis_controller/RobotisController.h b/robotis_controller/include/robotis_controller/RobotisController.h index c28256c..2fa4b48 100644 --- a/robotis_controller/include/robotis_controller/RobotisController.h +++ b/robotis_controller/include/robotis_controller/RobotisController.h @@ -37,11 +37,9 @@ enum CONTROLLER_MODE DIRECT_CONTROL_MODE }; -class RobotisController +class RobotisController : public Singleton { private: - static RobotisController *unique_instance_; - boost::thread queue_thread_; boost::thread gazebo_thread_; boost::thread set_module_thread_; @@ -59,8 +57,6 @@ private: std::map sensor_result_; - RobotisController(); - void QueueThread(); void GazeboThread(); void SetCtrlModuleThread(std::string ctrl_module); @@ -91,7 +87,8 @@ public: std::map gazebo_joint_pub; static void *ThreadProc(void *param); - static RobotisController *GetInstance() { return unique_instance_; } + + RobotisController(); bool Initialize(const std::string robot_file_path, const std::string init_file_path); void DeviceInit(const std::string init_file_path); diff --git a/robotis_controller/src/robotis_controller/RobotisController.cpp b/robotis_controller/src/robotis_controller/RobotisController.cpp index 60cdf91..a2d5251 100644 --- a/robotis_controller/src/robotis_controller/RobotisController.cpp +++ b/robotis_controller/src/robotis_controller/RobotisController.cpp @@ -12,8 +12,6 @@ using namespace ROBOTIS; -RobotisController *RobotisController::unique_instance_ = new RobotisController(); - RobotisController::RobotisController() : is_timer_running_(false), stop_timer_(false), @@ -586,70 +584,105 @@ void RobotisController::Process() // if(_msec > 8) std::cout << "Process duration : " << _msec << std::endl; // -> save to Robot->dxls[]->dynamixel_state.present_position - for(std::map::iterator dxl_it = robot->dxls.begin(); dxl_it != robot->dxls.end(); dxl_it++) + if(robot->dxls.size() > 0) { - UINT32_T _data = 0; - - std::string _port_name = dxl_it->second->port_name; - std::string _joint_name = dxl_it->first; - Dynamixel *_dxl = dxl_it->second; - - _present_state.header.stamp = ros::Time::now(); - _goal_state.header.stamp = _present_state.header.stamp; - - if(gazebo_mode == false) + for(std::map::iterator dxl_it = robot->dxls.begin(); dxl_it != robot->dxls.end(); dxl_it++) { - for(int _i = 0; _i < _dxl->bulk_read_items.size(); _i++) - { - ControlTableItem *_item = _dxl->bulk_read_items[_i]; - if(port_to_bulk_read[_port_name]->IsAvailable(_dxl->id, _item->address, _item->data_length) == true) - { - _data = port_to_bulk_read[_port_name]->GetData(_dxl->id, _item->address, _item->data_length); + UINT32_T _data = 0; - // TODO: change dxl_state - if(_dxl->present_position_item != 0 && _item->item_name == _dxl->present_position_item->item_name) - _dxl->dxl_state->present_position = _dxl->ConvertValue2Radian(_data) - _dxl->dxl_state->position_offset; // remove offset - else if(_dxl->present_velocity_item != 0 && _item->item_name == _dxl->present_velocity_item->item_name) - _dxl->dxl_state->present_velocity = _dxl->ConvertValue2Velocity(_data); - else if(_dxl->present_current_item != 0 && _item->item_name == _dxl->present_current_item->item_name) - _dxl->dxl_state->present_current = _dxl->ConvertValue2Current(_data); - else if(_dxl->goal_position_item != 0 && _item->item_name == _dxl->goal_position_item->item_name) - _dxl->dxl_state->goal_position = _dxl->ConvertValue2Radian(_data) - _dxl->dxl_state->position_offset; // remove offset - else if(_dxl->goal_velocity_item != 0 && _item->item_name == _dxl->goal_velocity_item->item_name) - _dxl->dxl_state->goal_velocity = _dxl->ConvertValue2Velocity(_data); - else if(_dxl->goal_current_item != 0 && _item->item_name == _dxl->goal_current_item->item_name) - _dxl->dxl_state->goal_current = _dxl->ConvertValue2Current(_data); - else - _dxl->dxl_state->bulk_read_table[_item->item_name] = _data; + std::string _port_name = dxl_it->second->port_name; + std::string _joint_name = dxl_it->first; + Dynamixel *_dxl = dxl_it->second; + + _present_state.header.stamp = ros::Time::now(); + _goal_state.header.stamp = _present_state.header.stamp; + + if(gazebo_mode == false && _dxl->bulk_read_items.size() > 0) + { + for(int _i = 0; _i < _dxl->bulk_read_items.size(); _i++) + { + ControlTableItem *_item = _dxl->bulk_read_items[_i]; + if(port_to_bulk_read[_port_name]->IsAvailable(_dxl->id, _item->address, _item->data_length) == true) + { + _data = port_to_bulk_read[_port_name]->GetData(_dxl->id, _item->address, _item->data_length); + + // TODO: change dxl_state + if(_dxl->present_position_item != 0 && _item->item_name == _dxl->present_position_item->item_name) + _dxl->dxl_state->present_position = _dxl->ConvertValue2Radian(_data) - _dxl->dxl_state->position_offset; // remove offset + else if(_dxl->present_velocity_item != 0 && _item->item_name == _dxl->present_velocity_item->item_name) + _dxl->dxl_state->present_velocity = _dxl->ConvertValue2Velocity(_data); + else if(_dxl->present_current_item != 0 && _item->item_name == _dxl->present_current_item->item_name) + _dxl->dxl_state->present_current = _dxl->ConvertValue2Current(_data); + else if(_dxl->goal_position_item != 0 && _item->item_name == _dxl->goal_position_item->item_name) + _dxl->dxl_state->goal_position = _dxl->ConvertValue2Radian(_data) - _dxl->dxl_state->position_offset; // remove offset + else if(_dxl->goal_velocity_item != 0 && _item->item_name == _dxl->goal_velocity_item->item_name) + _dxl->dxl_state->goal_velocity = _dxl->ConvertValue2Velocity(_data); + else if(_dxl->goal_current_item != 0 && _item->item_name == _dxl->goal_current_item->item_name) + _dxl->dxl_state->goal_current = _dxl->ConvertValue2Current(_data); + else + _dxl->dxl_state->bulk_read_table[_item->item_name] = _data; + } } + + // -> update time stamp to Robot->dxls[]->dynamixel_state.update_time_stamp + _dxl->dxl_state->update_time_stamp = TimeStamp(_present_state.header.stamp.sec, _present_state.header.stamp.nsec); } - // -> update time stamp to Robot->dxls[]->dynamixel_state.update_time_stamp - _dxl->dxl_state->update_time_stamp = TimeStamp(_present_state.header.stamp.sec, _present_state.header.stamp.nsec); + _present_state.name.push_back(_joint_name); + // TODO: should be converted to rad, rad/s, Nm + _present_state.position.push_back(_dxl->dxl_state->present_position); + _present_state.velocity.push_back(_dxl->dxl_state->present_velocity); + _present_state.effort.push_back(_dxl->dxl_state->present_current); + + _goal_state.name.push_back(_joint_name); + _goal_state.position.push_back(_dxl->dxl_state->goal_position); + _goal_state.velocity.push_back(_dxl->dxl_state->goal_velocity); + _goal_state.effort.push_back(_dxl->dxl_state->goal_current); } - - _present_state.name.push_back(_joint_name); - // TODO: should be converted to rad, rad/s, Nm - _present_state.position.push_back(_dxl->dxl_state->present_position); - _present_state.velocity.push_back(_dxl->dxl_state->present_velocity); - _present_state.effort.push_back(_dxl->dxl_state->present_current); - - _goal_state.name.push_back(_joint_name); - _goal_state.position.push_back(_dxl->dxl_state->goal_position); - _goal_state.velocity.push_back(_dxl->dxl_state->goal_velocity); - _goal_state.effort.push_back(_dxl->dxl_state->goal_current); } + // -> publish present joint_states & goal joint states topic present_joint_state_pub.publish(_present_state); goal_joint_state_pub.publish(_goal_state); + if(robot->sensors.size() > 0) + { + for(std::map::iterator sensor_it = robot->sensors.begin(); sensor_it != robot->sensors.end(); sensor_it++) + { + UINT32_T _data = 0; + + std::string _port_name = sensor_it->second->port_name; + std::string _sensor_name = sensor_it->first; + Sensor *_sensor = sensor_it->second; + + if(gazebo_mode == false && _sensor->bulk_read_items.size() > 0) + { + for(int _i = 0; _i < _sensor->bulk_read_items.size(); _i++) + { + ControlTableItem *_item = _sensor->bulk_read_items[_i]; + if(port_to_bulk_read[_port_name]->IsAvailable(_sensor->id, _item->address, _item->data_length) == true) + { + _data = port_to_bulk_read[_port_name]->GetData(_sensor->id, _item->address, _item->data_length); + + // TODO: change sensor_state + _sensor->sensor_state->bulk_read_table[_item->item_name] = _data; + } + } + + // -> update time stamp to Robot->dxls[]->dynamixel_state.update_time_stamp + ros::Time _now = ros::Time::now(); + _sensor->sensor_state->update_time_stamp = TimeStamp(_now.sec, _now.nsec); + } + } + } + // Call SensorModule Process() // -> for loop : call SensorModule list -> Process() if(sensor_modules_.size() > 0) { for(std::list::iterator _module_it = sensor_modules_.begin(); _module_it != sensor_modules_.end(); _module_it++) { - (*_module_it)->Process(robot->dxls); + (*_module_it)->Process(robot->dxls, robot->sensors); for(std::map::iterator _it = (*_module_it)->result.begin(); _it != (*_module_it)->result.end(); _it++) sensor_result_[_it->first] = _it->second; diff --git a/robotis_device/include/robotis_device/DynamixelState.h b/robotis_device/include/robotis_device/DynamixelState.h index f5df513..29212c4 100644 --- a/robotis_device/include/robotis_device/DynamixelState.h +++ b/robotis_device/include/robotis_device/DynamixelState.h @@ -5,11 +5,13 @@ * Author: zerom */ -#ifndef ROBOTIS_FRAMEWORK_ROBOTIS_CONTROLLER_INCLUDE_DEVICE_DYNAMIXELSTATE_H_ -#define ROBOTIS_FRAMEWORK_ROBOTIS_CONTROLLER_INCLUDE_DEVICE_DYNAMIXELSTATE_H_ +#ifndef ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_DYNAMIXELSTATE_H_ +#define ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_DYNAMIXELSTATE_H_ #include -#include + +#include "robotis_device/TimeStamp.h" +#include "robotis_framework_common/RobotisDef.h" #define INDIRECT_DATA_1 "indirect_data_1" #define INDIRECT_ADDRESS_1 "indirect_address_1" @@ -17,13 +19,6 @@ namespace ROBOTIS { -class TimeStamp { -public: - long sec; - long nsec; - TimeStamp(long sec, long nsec) : sec(sec), nsec(nsec) { } -}; - class DynamixelState { public: @@ -57,4 +52,4 @@ public: } -#endif /* ROBOTIS_FRAMEWORK_ROBOTIS_CONTROLLER_INCLUDE_DEVICE_DYNAMIXELSTATE_H_ */ +#endif /* ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_DYNAMIXELSTATE_H_ */ diff --git a/robotis_device/include/robotis_device/Sensor.h b/robotis_device/include/robotis_device/Sensor.h index 78de619..54df44f 100644 --- a/robotis_device/include/robotis_device/Sensor.h +++ b/robotis_device/include/robotis_device/Sensor.h @@ -12,6 +12,7 @@ #include #include #include "Device.h" +#include "SensorState.h" #include "ControlTableItem.h" namespace ROBOTIS @@ -19,8 +20,9 @@ namespace ROBOTIS class Sensor : public Device { - std::map sensed_value; public: + SensorState *sensor_state; + Sensor(int id, std::string model_name, float protocol_version); }; diff --git a/robotis_device/include/robotis_device/SensorState.h b/robotis_device/include/robotis_device/SensorState.h new file mode 100644 index 0000000..4137b14 --- /dev/null +++ b/robotis_device/include/robotis_device/SensorState.h @@ -0,0 +1,35 @@ +/* + * SensorState.h + * + * Created on: 2016. 5. 16. + * Author: zerom + */ + +#ifndef ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_SENSORSTATE_H_ +#define ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_SENSORSTATE_H_ + + +#include "robotis_device/TimeStamp.h" +#include "robotis_framework_common/RobotisDef.h" + +namespace ROBOTIS +{ + +class SensorState +{ +public: + TimeStamp update_time_stamp; + + std::map bulk_read_table; + + SensorState() + : update_time_stamp(0, 0) + { + bulk_read_table.clear(); + } +}; + +} + + +#endif /* ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_SENSORSTATE_H_ */ diff --git a/robotis_device/include/robotis_device/TimeStamp.h b/robotis_device/include/robotis_device/TimeStamp.h new file mode 100644 index 0000000..17b749a --- /dev/null +++ b/robotis_device/include/robotis_device/TimeStamp.h @@ -0,0 +1,25 @@ +/* + * TimeStamp.h + * + * Created on: 2016. 5. 16. + * Author: zerom + */ + +#ifndef ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_TIMESTAMP_H_ +#define ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_TIMESTAMP_H_ + + +namespace ROBOTIS +{ + +class TimeStamp { +public: + long sec; + long nsec; + TimeStamp(long sec, long nsec) : sec(sec), nsec(nsec) { } +}; + +} + + +#endif /* ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_TIMESTAMP_H_ */ diff --git a/robotis_device/src/robotis_device/Sensor.cpp b/robotis_device/src/robotis_device/Sensor.cpp index afe4f9f..db95494 100644 --- a/robotis_device/src/robotis_device/Sensor.cpp +++ b/robotis_device/src/robotis_device/Sensor.cpp @@ -17,5 +17,7 @@ Sensor::Sensor(int id, std::string model_name, float protocol_version) this->protocol_version = protocol_version; ctrl_table.clear(); + sensor_state = new SensorState(); + bulk_read_items.clear(); } diff --git a/robotis_framework_common/include/robotis_framework_common/MotionModule.h b/robotis_framework_common/include/robotis_framework_common/MotionModule.h index 18d962e..a305a6a 100644 --- a/robotis_framework_common/include/robotis_framework_common/MotionModule.h +++ b/robotis_framework_common/include/robotis_framework_common/MotionModule.h @@ -14,6 +14,7 @@ #include "robotis_device/Robot.h" #include "robotis_device/Dynamixel.h" +#include "robotis_framework_common/Singleton.h" namespace ROBOTIS { diff --git a/robotis_framework_common/include/robotis_framework_common/SensorModule.h b/robotis_framework_common/include/robotis_framework_common/SensorModule.h index d34bfbb..6111391 100644 --- a/robotis_framework_common/include/robotis_framework_common/SensorModule.h +++ b/robotis_framework_common/include/robotis_framework_common/SensorModule.h @@ -14,6 +14,7 @@ #include "robotis_device/Robot.h" #include "robotis_device/Dynamixel.h" +#include "robotis_framework_common/Singleton.h" namespace ROBOTIS { @@ -28,7 +29,7 @@ public: virtual ~SensorModule() { } virtual void Initialize(const int control_cycle_msec, Robot *robot) = 0; - virtual void Process(std::map dxls) = 0; + virtual void Process(std::map dxls, std::map sensors) = 0; }; } diff --git a/robotis_framework_common/include/robotis_framework_common/Singleton.h b/robotis_framework_common/include/robotis_framework_common/Singleton.h new file mode 100644 index 0000000..2b4c7d8 --- /dev/null +++ b/robotis_framework_common/include/robotis_framework_common/Singleton.h @@ -0,0 +1,49 @@ +/* + * Singleton.h + * + * Created on: 2016. 5. 17. + * Author: zerom + */ + +#ifndef ROBOTIS_FRAMEWORK_COMMON_INCLUDE_ROBOTIS_FRAMEWORK_COMMON_SINGLETON_H_ +#define ROBOTIS_FRAMEWORK_COMMON_INCLUDE_ROBOTIS_FRAMEWORK_COMMON_SINGLETON_H_ + + +namespace ROBOTIS +{ + +template +class Singleton +{ +private: + static T *unique_instance_; + +protected: + Singleton() { } + Singleton(Singleton const&) { } + Singleton& operator=(Singleton const&) { return *this; } + +public: + static T* GetInstance() + { + if(unique_instance_ == NULL) + unique_instance_ = new T; + return unique_instance_; + } + + static void DestroyInstance() + { + if(unique_instance_) + { + delete unique_instance_; + unique_instance_ = NULL; + } + } +}; + +template T* Singleton::unique_instance_ = NULL; + +} + + +#endif /* ROBOTIS_FRAMEWORK_COMMON_INCLUDE_ROBOTIS_FRAMEWORK_COMMON_SINGLETON_H_ */ From e85ef20d314103a1fc5f03efa09ee8dbcf0877b8 Mon Sep 17 00:00:00 2001 From: Alexander Stumpf Date: Wed, 18 May 2016 12:20:18 +0200 Subject: [PATCH 017/238] Fixed high CPU consumption due to busy waits --- robotis_controller/src/robotis_controller/RobotisController.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/robotis_controller/src/robotis_controller/RobotisController.cpp b/robotis_controller/src/robotis_controller/RobotisController.cpp index 20855f6..c5c7f40 100644 --- a/robotis_controller/src/robotis_controller/RobotisController.cpp +++ b/robotis_controller/src/robotis_controller/RobotisController.cpp @@ -415,6 +415,7 @@ void RobotisController::QueueThread() while(_ros_node.ok()) { _callback_queue.callAvailable(); + usleep(100); } } From 0f98400116160ed0df71400e2a38d00200742465 Mon Sep 17 00:00:00 2001 From: ROBOTIS-zerom Date: Fri, 20 May 2016 14:52:55 +0900 Subject: [PATCH 018/238] - function name changed : DeviceInit() -> InitDevice() --- .../include/robotis_controller/RobotisController.h | 2 +- .../src/robotis_controller/RobotisController.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/robotis_controller/include/robotis_controller/RobotisController.h b/robotis_controller/include/robotis_controller/RobotisController.h index 2fa4b48..46eb104 100644 --- a/robotis_controller/include/robotis_controller/RobotisController.h +++ b/robotis_controller/include/robotis_controller/RobotisController.h @@ -91,7 +91,7 @@ public: RobotisController(); bool Initialize(const std::string robot_file_path, const std::string init_file_path); - void DeviceInit(const std::string init_file_path); + void InitDevice(const std::string init_file_path); void Process(); void AddMotionModule(MotionModule *module); diff --git a/robotis_controller/src/robotis_controller/RobotisController.cpp b/robotis_controller/src/robotis_controller/RobotisController.cpp index a2d5251..76382ae 100644 --- a/robotis_controller/src/robotis_controller/RobotisController.cpp +++ b/robotis_controller/src/robotis_controller/RobotisController.cpp @@ -179,7 +179,7 @@ bool RobotisController::Initialize(const std::string robot_file_path, const std: } } - DeviceInit(init_file_path); + InitDevice(init_file_path); // [ BulkRead ] StartAddress : Present Position , Length : 10 ( Position/Velocity/Current ) for(std::map::iterator _it = robot->dxls.begin(); _it != robot->dxls.end(); _it++) @@ -259,7 +259,7 @@ bool RobotisController::Initialize(const std::string robot_file_path, const std: return true; } -void RobotisController::DeviceInit(const std::string init_file_path) +void RobotisController::InitDevice(const std::string init_file_path) { // device initialize if(DEBUG_PRINT) ROS_WARN("INIT FILE LOAD"); From 243b4dfbc91fe481f4b4913c2af3b471bc38b8ff Mon Sep 17 00:00:00 2001 From: ROBOTIS-zerom Date: Fri, 20 May 2016 15:41:22 +0900 Subject: [PATCH 019/238] - Group Bulk/Sync class ClearParam() function changed. --- dynamixel_sdk/src/dynamixel_sdk/GroupBulkRead.cpp | 10 +++++----- dynamixel_sdk/src/dynamixel_sdk/GroupBulkWrite.cpp | 9 +++------ dynamixel_sdk/src/dynamixel_sdk/GroupSyncRead.cpp | 9 +++------ dynamixel_sdk/src/dynamixel_sdk/GroupSyncWrite.cpp | 10 +++++----- 4 files changed, 16 insertions(+), 22 deletions(-) diff --git a/dynamixel_sdk/src/dynamixel_sdk/GroupBulkRead.cpp b/dynamixel_sdk/src/dynamixel_sdk/GroupBulkRead.cpp index d8f2b6d..f248ee8 100644 --- a/dynamixel_sdk/src/dynamixel_sdk/GroupBulkRead.cpp +++ b/dynamixel_sdk/src/dynamixel_sdk/GroupBulkRead.cpp @@ -90,11 +90,11 @@ void GroupBulkRead::RemoveParam(UINT8_T id) void GroupBulkRead::ClearParam() { - if(id_list_.size() != 0) - { - for(unsigned int _i = 0; _i < id_list_.size(); _i++) - delete[] data_list_[id_list_[_i]]; - } + if(id_list_.size() == 0) + return; + + for(unsigned int _i = 0; _i < id_list_.size(); _i++) + delete[] data_list_[id_list_[_i]]; id_list_.clear(); address_list_.clear(); diff --git a/dynamixel_sdk/src/dynamixel_sdk/GroupBulkWrite.cpp b/dynamixel_sdk/src/dynamixel_sdk/GroupBulkWrite.cpp index 399b32e..973793b 100644 --- a/dynamixel_sdk/src/dynamixel_sdk/GroupBulkWrite.cpp +++ b/dynamixel_sdk/src/dynamixel_sdk/GroupBulkWrite.cpp @@ -111,14 +111,11 @@ bool GroupBulkWrite::ChangeParam(UINT8_T id, UINT16_T start_address, UINT16_T da } void GroupBulkWrite::ClearParam() { - if(ph_->GetProtocolVersion() == 1.0) + if(ph_->GetProtocolVersion() == 1.0 || id_list_.size() == 0) return; - if(id_list_.size() != 0) - { - for(unsigned int _i = 0; _i < id_list_.size(); _i++) - delete[] data_list_[id_list_[_i]]; - } + for(unsigned int _i = 0; _i < id_list_.size(); _i++) + delete[] data_list_[id_list_[_i]]; id_list_.clear(); address_list_.clear(); diff --git a/dynamixel_sdk/src/dynamixel_sdk/GroupSyncRead.cpp b/dynamixel_sdk/src/dynamixel_sdk/GroupSyncRead.cpp index 8e602e3..fede283 100644 --- a/dynamixel_sdk/src/dynamixel_sdk/GroupSyncRead.cpp +++ b/dynamixel_sdk/src/dynamixel_sdk/GroupSyncRead.cpp @@ -72,14 +72,11 @@ void GroupSyncRead::RemoveParam(UINT8_T id) } void GroupSyncRead::ClearParam() { - if(ph_->GetProtocolVersion() == 1.0) + if(ph_->GetProtocolVersion() == 1.0 || id_list_.size() == 0) return; - if(id_list_.size() != 0) - { - for(unsigned int _i = 0; _i < id_list_.size(); _i++) - delete[] data_list_[id_list_[_i]]; - } + for(unsigned int _i = 0; _i < id_list_.size(); _i++) + delete[] data_list_[id_list_[_i]]; id_list_.clear(); data_list_.clear(); diff --git a/dynamixel_sdk/src/dynamixel_sdk/GroupSyncWrite.cpp b/dynamixel_sdk/src/dynamixel_sdk/GroupSyncWrite.cpp index 45d90f8..8a61435 100644 --- a/dynamixel_sdk/src/dynamixel_sdk/GroupSyncWrite.cpp +++ b/dynamixel_sdk/src/dynamixel_sdk/GroupSyncWrite.cpp @@ -92,11 +92,11 @@ bool GroupSyncWrite::ChangeParam(UINT8_T id, UINT8_T *data) void GroupSyncWrite::ClearParam() { - if(id_list_.size() != 0) - { - for(unsigned int _i = 0; _i < id_list_.size(); _i++) - delete[] data_list_[id_list_[_i]]; - } + if(id_list_.size() == 0) + return; + + for(unsigned int _i = 0; _i < id_list_.size(); _i++) + delete[] data_list_[id_list_[_i]]; id_list_.clear(); data_list_.clear(); From b7f84182acf7c6e1ce1605715887644ae16f3d97 Mon Sep 17 00:00:00 2001 From: Alexander Stumpf Date: Tue, 24 May 2016 16:33:40 +0200 Subject: [PATCH 020/238] added .gitignore --- .gitignore | 11 +++++++++++ 1 file changed, 11 insertions(+) create mode 100644 .gitignore diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..36a5c0a --- /dev/null +++ b/.gitignore @@ -0,0 +1,11 @@ +build +devel +bin +lib +msg_gen +srv_gen +qtcreator-build +*~ +*.backup +*.user +*.autosave From 30586ed499852d60d7948ffb7bae2890262834c4 Mon Sep 17 00:00:00 2001 From: ROBOTIS-zerom Date: Wed, 25 May 2016 11:37:07 +0900 Subject: [PATCH 021/238] - added position_p_gain sync write - MotionModule/SensorModule member variable access changed (public -> protected). - some bug fixed. --- .../robotis_controller/RobotisController.h | 6 +- .../robotis_controller/RobotisController.cpp | 332 +++++++++++++----- .../include/robotis_device/Dynamixel.h | 1 + .../include/robotis_device/DynamixelState.h | 2 + .../src/robotis_device/Dynamixel.cpp | 3 +- robotis_device/src/robotis_device/Robot.cpp | 10 +- .../robotis_framework_common/MotionModule.h | 22 +- .../robotis_framework_common/SensorModule.h | 5 +- 8 files changed, 293 insertions(+), 88 deletions(-) diff --git a/robotis_controller/include/robotis_controller/RobotisController.h b/robotis_controller/include/robotis_controller/RobotisController.h index 46eb104..d974a3e 100644 --- a/robotis_controller/include/robotis_controller/RobotisController.h +++ b/robotis_controller/include/robotis_controller/RobotisController.h @@ -24,7 +24,6 @@ #include "robotis_controller_msgs/JointCtrlModule.h" #include "robotis_controller_msgs/GetJointModule.h" -// TODO: TEMPORARY CODE !! #include "dynamixel_sdk/GroupBulkRead.h" #include "dynamixel_sdk/GroupSyncWrite.h" @@ -73,11 +72,14 @@ public: bool gazebo_mode; std::string gazebo_robot_name; - // TODO: TEMPORARY CODE !! + /* bulk read */ std::map port_to_bulk_read; + + /* sync write */ std::map port_to_sync_write_position; std::map port_to_sync_write_velocity; std::map port_to_sync_write_torque; + std::map port_to_sync_write_position_p_gain; /* publisher */ ros::Publisher goal_joint_state_pub; diff --git a/robotis_controller/src/robotis_controller/RobotisController.cpp b/robotis_controller/src/robotis_controller/RobotisController.cpp index e93e58c..d6dfda1 100644 --- a/robotis_controller/src/robotis_controller/RobotisController.cpp +++ b/robotis_controller/src/robotis_controller/RobotisController.cpp @@ -13,15 +13,15 @@ using namespace ROBOTIS; RobotisController::RobotisController() -: is_timer_running_(false), - stop_timer_(false), - init_pose_loaded_(false), - timer_thread_(0), - controller_mode_(MOTION_MODULE_MODE), - DEBUG_PRINT(false), - robot(0), - gazebo_mode(false), - gazebo_robot_name("robotis") + : is_timer_running_(false), + stop_timer_(false), + init_pose_loaded_(false), + timer_thread_(0), + controller_mode_(MOTION_MODULE_MODE), + DEBUG_PRINT(false), + robot(0), + gazebo_mode(false), + gazebo_robot_name("robotis") { direct_sync_write_.clear(); } @@ -54,11 +54,25 @@ void RobotisController::InitSyncWrite() // clear syncwrite param setting for(std::map::iterator _it = port_to_sync_write_position.begin(); _it != port_to_sync_write_position.end(); _it++) - _it->second->ClearParam(); + { + if(_it->second != NULL) + _it->second->ClearParam(); + } + for(std::map::iterator _it = port_to_sync_write_position_p_gain.begin(); _it != port_to_sync_write_position_p_gain.end(); _it++) + { + if(_it->second != NULL) + _it->second->ClearParam(); + } for(std::map::iterator _it = port_to_sync_write_velocity.begin(); _it != port_to_sync_write_velocity.end(); _it++) - _it->second->ClearParam(); + { + if(_it->second != NULL) + _it->second->ClearParam(); + } for(std::map::iterator _it = port_to_sync_write_torque.begin(); _it != port_to_sync_write_torque.end(); _it++) - _it->second->ClearParam(); + { + if(_it->second != NULL) + _it->second->ClearParam(); + } // set init syncwrite param(from data of bulkread) for(std::map::iterator _it = robot->dxls.begin(); _it != robot->dxls.end(); _it++) @@ -91,6 +105,10 @@ void RobotisController::InitSyncWrite() port_to_sync_write_position[_dxl->port_name]->AddParam(_dxl->id, _sync_write_data); } + else if(_dxl->position_p_gain_item != 0 && _dxl->bulk_read_items[_i]->item_name == _dxl->position_p_gain_item->item_name) + { + _dxl->dxl_state->position_p_gain = _read_data; + } else if(_dxl->present_velocity_item != 0 && _dxl->bulk_read_items[_i]->item_name == _dxl->present_velocity_item->item_name) { _dxl->dxl_state->present_velocity = _dxl->ConvertValue2Velocity(_read_data); @@ -141,20 +159,37 @@ bool RobotisController::Initialize(const std::string robot_file_path, const std: Dynamixel *_default_device = _dxl_it->second; _default_pkt_handler = PacketHandler::GetPacketHandler(_default_device->protocol_version); - port_to_sync_write_position[_port_name] = new GroupSyncWrite(_port, - _default_pkt_handler, - _default_device->goal_position_item->address, - _default_device->goal_position_item->data_length); + if(_default_device->goal_position_item != 0) + { + port_to_sync_write_position[_port_name] = new GroupSyncWrite(_port, + _default_pkt_handler, + _default_device->goal_position_item->address, + _default_device->goal_position_item->data_length); + } - port_to_sync_write_velocity[_port_name] = new GroupSyncWrite(_port, - _default_pkt_handler, - _default_device->goal_velocity_item->address, - _default_device->goal_velocity_item->data_length); + if(_default_device->position_p_gain_item != 0) + { + port_to_sync_write_position_p_gain[_port_name] = new GroupSyncWrite(_port, + _default_pkt_handler, + _default_device->position_p_gain_item->address, + _default_device->position_p_gain_item->data_length); + } - port_to_sync_write_torque[_port_name] = new GroupSyncWrite(_port, - _default_pkt_handler, - _default_device->goal_current_item->address, - _default_device->goal_current_item->data_length); + if(_default_device->goal_velocity_item != 0) + { + port_to_sync_write_velocity[_port_name] = new GroupSyncWrite(_port, + _default_pkt_handler, + _default_device->goal_velocity_item->address, + _default_device->goal_velocity_item->data_length); + } + + if(_default_device->goal_current_item != 0) + { + port_to_sync_write_torque[_port_name] = new GroupSyncWrite(_port, + _default_pkt_handler, + _default_device->goal_current_item->address, + _default_device->goal_current_item->data_length); + } } else if(_sensor_it != robot->sensors.end()) { @@ -193,14 +228,14 @@ bool RobotisController::Initialize(const std::string robot_file_path, const std: int _bulkread_start_addr = 0; int _bulkread_data_length = 0; - // bulk read default : present position - if(_dxl->present_position_item != 0) - { - _bulkread_start_addr = _dxl->present_position_item->address; - _bulkread_data_length = _dxl->present_position_item->data_length; - } +// // bulk read default : present position +// if(_dxl->present_position_item != 0) +// { +// _bulkread_start_addr = _dxl->present_position_item->address; +// _bulkread_data_length = _dxl->present_position_item->data_length; +// } - // TODO: modifing + // calculate bulk read start address & data length std::map::iterator _indirect_addr_it = _dxl->ctrl_table.find(INDIRECT_ADDRESS_1); if(_indirect_addr_it != _dxl->ctrl_table.end()) // INDIRECT_ADDRESS_1 exist { @@ -248,13 +283,77 @@ bool RobotisController::Initialize(const std::string robot_file_path, const std: } // ROS_WARN("[%12s] start_addr: %d, data_length: %d", _joint_name.c_str(), _bulkread_start_addr, _bulkread_data_length); - port_to_bulk_read[_dxl->port_name]->AddParam(_dxl->id, _bulkread_start_addr, _bulkread_data_length); + if(_bulkread_start_addr != 0) + port_to_bulk_read[_dxl->port_name]->AddParam(_dxl->id, _bulkread_start_addr, _bulkread_data_length); // Torque ON if(WriteCtrlItem(_joint_name, _dxl->torque_enable_item->item_name, 1) != COMM_SUCCESS) WriteCtrlItem(_joint_name, _dxl->torque_enable_item->item_name, 1); } + for(std::map::iterator _it = robot->sensors.begin(); _it != robot->sensors.end(); _it++) + { + std::string _sensor_name = _it->first; + Sensor *_sensor = _it->second; + + if(_sensor == NULL) + continue; + + int _bulkread_start_addr = 0; + int _bulkread_data_length = 0; + + // calculate bulk read start address & data length + std::map::iterator _indirect_addr_it = _sensor->ctrl_table.find(INDIRECT_ADDRESS_1); + if(_indirect_addr_it != _sensor->ctrl_table.end()) // INDIRECT_ADDRESS_1 exist + { + if(_sensor->bulk_read_items.size() != 0) + { + _bulkread_start_addr = _sensor->bulk_read_items[0]->address; + _bulkread_data_length = 0; + + // set indirect address + int _indirect_addr = _indirect_addr_it->second->address; + for(int _i = 0; _i < _sensor->bulk_read_items.size(); _i++) + { + int _addr_leng = _sensor->bulk_read_items[_i]->data_length; + + _bulkread_data_length += _addr_leng; + for(int _l = 0; _l < _addr_leng; _l++) + { +// ROS_WARN("[%12s] INDIR_ADDR: %d, ITEM_ADDR: %d", _sensor_name.c_str(), _indirect_addr, _sensor->ctrl_table[_sensor->bulk_read_items[_i]->item_name]->address + _l); + Write2Byte(_sensor_name, _indirect_addr, _sensor->ctrl_table[_sensor->bulk_read_items[_i]->item_name]->address + _l); + _indirect_addr += 2; + } + } + } + } + else // INDIRECT_ADDRESS_1 NOT exist + { + if(_sensor->bulk_read_items.size() != 0) + { + _bulkread_start_addr = _sensor->bulk_read_items[0]->address; + _bulkread_data_length = 0; + + ControlTableItem *_last_item = _sensor->bulk_read_items[0]; + + for(int _i = 0; _i < _sensor->bulk_read_items.size(); _i++) + { + int _addr = _sensor->bulk_read_items[_i]->address; + if(_addr < _bulkread_start_addr) + _bulkread_start_addr = _addr; + else if(_last_item->address < _addr) + _last_item = _sensor->bulk_read_items[_i]; + } + + _bulkread_data_length = _last_item->address - _bulkread_start_addr + _last_item->data_length; + } + } + + //ROS_WARN("[%12s] start_addr: %d, data_length: %d", _sensor_name.c_str(), _bulkread_start_addr, _bulkread_data_length); + if(_bulkread_start_addr != 0) + port_to_bulk_read[_sensor->port_name]->AddParam(_sensor->id, _bulkread_start_addr, _bulkread_data_length); + } + queue_thread_ = boost::thread(boost::bind(&RobotisController::QueueThread, this)); return true; } @@ -503,14 +602,31 @@ void RobotisController::StopTimer() exit(-1); for(std::map::iterator _it = port_to_bulk_read.begin(); _it != port_to_bulk_read.end(); _it++) - _it->second->RxPacket(); + { + if(_it->second != NULL) + _it->second->RxPacket(); + } for(std::map::iterator _it = port_to_sync_write_position.begin(); _it != port_to_sync_write_position.end(); _it++) - _it->second->ClearParam(); + { + if(_it->second != NULL) + _it->second->ClearParam(); + } + for(std::map::iterator _it = port_to_sync_write_position_p_gain.begin(); _it != port_to_sync_write_position_p_gain.end(); _it++) + { + if(_it->second != NULL) + _it->second->ClearParam(); + } for(std::map::iterator _it = port_to_sync_write_velocity.begin(); _it != port_to_sync_write_velocity.end(); _it++) - _it->second->ClearParam(); + { + if(_it->second != NULL) + _it->second->ClearParam(); + } for(std::map::iterator _it = port_to_sync_write_torque.begin(); _it != port_to_sync_write_torque.end(); _it++) - _it->second->ClearParam(); + { + if(_it->second != NULL) + _it->second->ClearParam(); + } } else { @@ -607,7 +723,7 @@ void RobotisController::Process() { _data = port_to_bulk_read[_port_name]->GetData(_dxl->id, _item->address, _item->data_length); - // TODO: change dxl_state + // change dxl_state if(_dxl->present_position_item != 0 && _item->item_name == _dxl->present_position_item->item_name) _dxl->dxl_state->present_position = _dxl->ConvertValue2Radian(_data) - _dxl->dxl_state->position_offset; // remove offset else if(_dxl->present_velocity_item != 0 && _item->item_name == _dxl->present_velocity_item->item_name) @@ -630,7 +746,6 @@ void RobotisController::Process() } _present_state.name.push_back(_joint_name); - // TODO: should be converted to rad, rad/s, Nm _present_state.position.push_back(_dxl->dxl_state->present_position); _present_state.velocity.push_back(_dxl->dxl_state->present_velocity); _present_state.effort.push_back(_dxl->dxl_state->present_current); @@ -665,7 +780,7 @@ void RobotisController::Process() { _data = port_to_bulk_read[_port_name]->GetData(_sensor->id, _item->address, _item->data_length); - // TODO: change sensor_state + // change sensor_state _sensor->sensor_state->bulk_read_table[_item->item_name] = _data; } } @@ -702,7 +817,7 @@ void RobotisController::Process() { // ros::Time _before = ros::Time::now(); - if((*module_it)->enable == false) + if((*module_it)->GetModuleEnable() == false) continue; (*module_it)->Process(robot->dxls, sensor_result_); @@ -722,20 +837,20 @@ void RobotisController::Process() Dynamixel *_dxl = dxl_it->second; DynamixelState *_dxl_state = _dxl->dxl_state; - if(_dxl->ctrl_module_name == (*module_it)->module_name) + if(_dxl->ctrl_module_name == (*module_it)->GetModuleName()) { _do_sync_write = true; // ROS_INFO("Set goal value"); DynamixelState *_result_state = (*module_it)->result[_joint_name]; if(_result_state == NULL) { - ROS_INFO("[%s] %s", (*module_it)->module_name.c_str(), _joint_name.c_str()); + ROS_INFO("[%s] %s", (*module_it)->GetModuleName().c_str(), _joint_name.c_str()); continue; } // TODO: check update time stamp ? - if((*module_it)->control_mode == POSITION_CONTROL) + if((*module_it)->GetControlMode() == POSITION_CONTROL) { // if(_result_state->goal_position == 0 && _dxl->id == 3) // ROS_INFO("[MODULE:%s][ID:3] goal position = %f", (*module_it)->module_name.c_str(), _dxl_state->goal_position); @@ -745,7 +860,7 @@ void RobotisController::Process() { // add offset UINT32_T _pos_data = _dxl->ConvertRadian2Value(_dxl_state->goal_position + _dxl_state->position_offset); - UINT8_T _sync_write_data[4]; + UINT8_T _sync_write_data[4] = {0}; _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_pos_data)); _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_pos_data)); _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_pos_data)); @@ -754,10 +869,26 @@ void RobotisController::Process() if(abs(_pos_data) > 151800) printf("goal_pos : %f | position_offset : %f | pos_data : %d\n", _dxl_state->goal_position , _dxl_state->position_offset, _pos_data); - port_to_sync_write_position[_dxl->port_name]->ChangeParam(_dxl->id, _sync_write_data); + if(port_to_sync_write_position[_dxl->port_name] != NULL) + port_to_sync_write_position[_dxl->port_name]->ChangeParam(_dxl->id, _sync_write_data); + + // if position p gain value is changed -> sync write + if(_dxl_state->position_p_gain != _result_state->position_p_gain) + { + _dxl_state->position_p_gain = _result_state->position_p_gain; + + UINT8_T _sync_write_p_gain[4] = {0}; + _sync_write_p_gain[0] = DXL_LOBYTE(DXL_LOWORD(_dxl_state->position_p_gain)); + _sync_write_p_gain[1] = DXL_HIBYTE(DXL_LOWORD(_dxl_state->position_p_gain)); + _sync_write_p_gain[2] = DXL_LOBYTE(DXL_HIWORD(_dxl_state->position_p_gain)); + _sync_write_p_gain[3] = DXL_HIBYTE(DXL_HIWORD(_dxl_state->position_p_gain)); + + if(port_to_sync_write_position_p_gain[_dxl->port_name] != NULL) + port_to_sync_write_position_p_gain[_dxl->port_name]->AddParam(_dxl->id, _sync_write_p_gain); + } } } - else if((*module_it)->control_mode == VELOCITY_CONTROL) + else if((*module_it)->GetControlMode() == VELOCITY_CONTROL) { _dxl_state->goal_velocity = _result_state->goal_velocity; @@ -770,10 +901,11 @@ void RobotisController::Process() _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_vel_data)); _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_vel_data)); - port_to_sync_write_velocity[_dxl->port_name]->ChangeParam(_dxl->id, _sync_write_data); + if(port_to_sync_write_velocity[_dxl->port_name] != NULL) + port_to_sync_write_velocity[_dxl->port_name]->ChangeParam(_dxl->id, _sync_write_data); } } - else if((*module_it)->control_mode == CURRENT_CONTROL) + else if((*module_it)->GetControlMode() == CURRENT_CONTROL) { _dxl_state->goal_current = _result_state->goal_current; @@ -784,7 +916,8 @@ void RobotisController::Process() _sync_write_data[0] = DXL_LOBYTE(_torq_data); _sync_write_data[1] = DXL_HIBYTE(_torq_data); - port_to_sync_write_torque[_dxl->port_name]->ChangeParam(_dxl->id, _sync_write_data); + if(port_to_sync_write_torque[_dxl->port_name] != NULL) + port_to_sync_write_torque[_dxl->port_name]->ChangeParam(_dxl->id, _sync_write_data); } } } @@ -801,16 +934,23 @@ void RobotisController::Process() queue_mutex_.unlock(); } - // TODO: Combine the result && SyncWrite - // -> SyncWrite + // SyncWrite if(gazebo_mode == false && _do_sync_write) { + if(port_to_sync_write_position_p_gain.size() > 0) + { + for(std::map::iterator _it = port_to_sync_write_position_p_gain.begin(); _it != port_to_sync_write_position_p_gain.end(); _it++) + { + _it->second->TxPacket(); + _it->second->ClearParam(); + } + } for(std::map::iterator _it = port_to_sync_write_position.begin(); _it != port_to_sync_write_position.end(); _it++) - _it->second->TxPacket(); + if(_it->second != NULL) _it->second->TxPacket(); for(std::map::iterator _it = port_to_sync_write_velocity.begin(); _it != port_to_sync_write_velocity.end(); _it++) - _it->second->TxPacket(); + if(_it->second != NULL) _it->second->TxPacket(); for(std::map::iterator _it = port_to_sync_write_torque.begin(); _it != port_to_sync_write_torque.end(); _it++) - _it->second->TxPacket(); + if(_it->second != NULL) _it->second->TxPacket(); } else if(gazebo_mode == true) { @@ -855,16 +995,26 @@ void RobotisController::Process() _it->second->TxPacket(); } - // ros::Duration _dur = ros::Time::now() - _now; - // double _msec = _dur.nsec * 0.000001; - // - // if(_msec > 8) std::cout << "Process duration : " << _msec << std::endl; +// ros::Duration _dur = ros::Time::now() - _now; +// double _msec = _dur.nsec * 0.000001; +// +// if(_msec > 8) std::cout << "Process duration : " << _msec << std::endl; _is_process_running = false; } void RobotisController::AddMotionModule(MotionModule *module) { + // check whether the module name already exists + for(std::list::iterator _m_it = motion_modules_.begin(); _m_it != motion_modules_.end(); _m_it++) + { + if((*_m_it)->GetModuleName() == module->GetModuleName()) + { + ROS_ERROR("Motion Module Name [%s] already exist !!", module->GetModuleName().c_str()); + return; + } + } + module->Initialize(CONTROL_CYCLE_MSEC, robot); motion_modules_.push_back(module); motion_modules_.unique(); @@ -877,6 +1027,16 @@ void RobotisController::RemoveMotionModule(MotionModule *module) void RobotisController::AddSensorModule(SensorModule *module) { + // check whether the module name already exists + for(std::list::iterator _m_it = sensor_modules_.begin(); _m_it != sensor_modules_.end(); _m_it++) + { + if((*_m_it)->GetModuleName() == module->GetModuleName()) + { + ROS_ERROR("Sensor Module Name [%s] already exist !!", module->GetModuleName().c_str()); + return; + } + } + module->Initialize(CONTROL_CYCLE_MSEC, robot); sensor_modules_.push_back(module); sensor_modules_.unique(); @@ -971,7 +1131,8 @@ void RobotisController::SetJointStatesCallback(const sensor_msgs::JointState::Co _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_pos)); _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_pos)); - port_to_sync_write_position[_dxl->port_name]->AddParam(_dxl->id, _sync_write_data); + if(port_to_sync_write_position[_dxl->port_name] != NULL) + port_to_sync_write_position[_dxl->port_name]->AddParam(_dxl->id, _sync_write_data); } queue_mutex_.unlock(); @@ -1010,7 +1171,7 @@ void RobotisController::SetJointCtrlModuleCallback(const robotis_controller_msgs // check whether the module is using this joint for(std::list::iterator _m_it = motion_modules_.begin(); _m_it != motion_modules_.end(); _m_it++) { - if((*_m_it)->module_name == msg->module_name[idx]) + if((*_m_it)->GetModuleName() == msg->module_name[idx]) { if((*_m_it)->result.find(msg->joint_name[idx]) != (*_m_it)->result.end()) { @@ -1024,14 +1185,14 @@ void RobotisController::SetJointCtrlModuleCallback(const robotis_controller_msgs for(std::list::iterator _m_it = motion_modules_.begin(); _m_it != motion_modules_.end(); _m_it++) { // set all modules -> disable - (*_m_it)->enable = false; + (*_m_it)->SetModuleEnable(false); // set all used modules -> enable for(std::map::iterator _d_it = robot->dxls.begin(); _d_it != robot->dxls.end(); _d_it++) { - if(_d_it->second->ctrl_module_name == (*_m_it)->module_name) + if(_d_it->second->ctrl_module_name == (*_m_it)->GetModuleName()) { - (*_m_it)->enable = true; + (*_m_it)->SetModuleEnable(true); break; } } @@ -1080,7 +1241,7 @@ void RobotisController::SetCtrlModuleThread(std::string ctrl_module) // enqueue all modules in order to stop for(std::list::iterator _m_it = motion_modules_.begin(); _m_it != motion_modules_.end(); _m_it++) { - if((*_m_it)->enable == true) _stop_modules.push_back(*_m_it); + if((*_m_it)->GetModuleEnable() == true) _stop_modules.push_back(*_m_it); } } else @@ -1088,7 +1249,7 @@ void RobotisController::SetCtrlModuleThread(std::string ctrl_module) for(std::list::iterator _m_it = motion_modules_.begin(); _m_it != motion_modules_.end(); _m_it++) { // if it exist - if((*_m_it)->module_name == ctrl_module) + if((*_m_it)->GetModuleName() == ctrl_module) { // enqueue the module which lost control of joint in order to stop for(std::map::iterator _result_it = (*_m_it)->result.begin(); _result_it != (*_m_it)->result.end(); _result_it++) @@ -1102,7 +1263,7 @@ void RobotisController::SetCtrlModuleThread(std::string ctrl_module) { for(std::list::iterator _stop_m_it = motion_modules_.begin(); _stop_m_it != motion_modules_.end(); _stop_m_it++) { - if((*_stop_m_it)->module_name == _d_it->second->ctrl_module_name && (*_stop_m_it)->enable == true) + if((*_stop_m_it)->GetModuleName() == _d_it->second->ctrl_module_name && (*_stop_m_it)->GetModuleEnable() == true) _stop_modules.push_back(*_stop_m_it); } } @@ -1139,7 +1300,7 @@ void RobotisController::SetCtrlModuleThread(std::string ctrl_module) // set all modules -> disable for(std::list::iterator _m_it = motion_modules_.begin(); _m_it != motion_modules_.end(); _m_it++) { - (*_m_it)->enable = false; + (*_m_it)->SetModuleEnable(false); } // set dxl's control module to "none" @@ -1167,9 +1328,9 @@ void RobotisController::SetCtrlModuleThread(std::string ctrl_module) for(std::list::iterator _m_it = motion_modules_.begin(); _m_it != motion_modules_.end(); _m_it++) { // if it exist - if((*_m_it)->module_name == ctrl_module) + if((*_m_it)->GetModuleName() == ctrl_module) { - CONTROL_MODE _mode = (*_m_it)->control_mode; + CONTROL_MODE _mode = (*_m_it)->GetControlMode(); for(std::map::iterator _result_it = (*_m_it)->result.begin(); _result_it != (*_m_it)->result.end(); _result_it++) { std::map::iterator _d_it = robot->dxls.find(_result_it->first); @@ -1187,10 +1348,13 @@ void RobotisController::SetCtrlModuleThread(std::string ctrl_module) _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_pos_data)); _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_pos_data)); - port_to_sync_write_position[_dxl->port_name]->AddParam(_dxl->id, _sync_write_data); + if(port_to_sync_write_position[_dxl->port_name] != NULL) + port_to_sync_write_position[_dxl->port_name]->AddParam(_dxl->id, _sync_write_data); - port_to_sync_write_torque[_dxl->port_name]->RemoveParam(_dxl->id); - port_to_sync_write_velocity[_dxl->port_name]->RemoveParam(_dxl->id); + if(port_to_sync_write_torque[_dxl->port_name] != NULL) + port_to_sync_write_torque[_dxl->port_name]->RemoveParam(_dxl->id); + if(port_to_sync_write_velocity[_dxl->port_name] != NULL) + port_to_sync_write_velocity[_dxl->port_name]->RemoveParam(_dxl->id); } else if(_mode == VELOCITY_CONTROL) { @@ -1201,10 +1365,13 @@ void RobotisController::SetCtrlModuleThread(std::string ctrl_module) _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_vel_data)); _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_vel_data)); - port_to_sync_write_velocity[_dxl->port_name]->AddParam(_dxl->id, _sync_write_data); + if(port_to_sync_write_velocity[_dxl->port_name] != NULL) + port_to_sync_write_velocity[_dxl->port_name]->AddParam(_dxl->id, _sync_write_data); - port_to_sync_write_torque[_dxl->port_name]->RemoveParam(_dxl->id); - port_to_sync_write_position[_dxl->port_name]->RemoveParam(_dxl->id); + if(port_to_sync_write_torque[_dxl->port_name] != NULL) + port_to_sync_write_torque[_dxl->port_name]->RemoveParam(_dxl->id); + if(port_to_sync_write_position[_dxl->port_name] != NULL) + port_to_sync_write_position[_dxl->port_name]->RemoveParam(_dxl->id); } else if(_mode == CURRENT_CONTROL) { @@ -1215,10 +1382,13 @@ void RobotisController::SetCtrlModuleThread(std::string ctrl_module) _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_curr_data)); _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_curr_data)); - port_to_sync_write_torque[_dxl->port_name]->AddParam(_dxl->id, _sync_write_data); + if(port_to_sync_write_torque[_dxl->port_name] != NULL) + port_to_sync_write_torque[_dxl->port_name]->AddParam(_dxl->id, _sync_write_data); - port_to_sync_write_velocity[_dxl->port_name]->RemoveParam(_dxl->id); - port_to_sync_write_position[_dxl->port_name]->RemoveParam(_dxl->id); + if(port_to_sync_write_velocity[_dxl->port_name] != NULL) + port_to_sync_write_velocity[_dxl->port_name]->RemoveParam(_dxl->id); + if(port_to_sync_write_position[_dxl->port_name] != NULL) + port_to_sync_write_position[_dxl->port_name]->RemoveParam(_dxl->id); } } } @@ -1231,14 +1401,14 @@ void RobotisController::SetCtrlModuleThread(std::string ctrl_module) for(std::list::iterator _m_it = motion_modules_.begin(); _m_it != motion_modules_.end(); _m_it++) { // set all modules -> disable - (*_m_it)->enable = false; + (*_m_it)->SetModuleEnable(false); // set all used modules -> enable for(std::map::iterator _d_it = robot->dxls.begin(); _d_it != robot->dxls.end(); _d_it++) { - if(_d_it->second->ctrl_module_name == (*_m_it)->module_name) + if(_d_it->second->ctrl_module_name == (*_m_it)->GetModuleName()) { - (*_m_it)->enable = true; + (*_m_it)->SetModuleEnable(true); break; } } diff --git a/robotis_device/include/robotis_device/Dynamixel.h b/robotis_device/include/robotis_device/Dynamixel.h index 64d4a4a..a0a9beb 100644 --- a/robotis_device/include/robotis_device/Dynamixel.h +++ b/robotis_device/include/robotis_device/Dynamixel.h @@ -41,6 +41,7 @@ public: ControlTableItem *goal_position_item; ControlTableItem *goal_velocity_item; ControlTableItem *goal_current_item; + ControlTableItem *position_p_gain_item; Dynamixel(int id, std::string model_name, float protocol_version); diff --git a/robotis_device/include/robotis_device/DynamixelState.h b/robotis_device/include/robotis_device/DynamixelState.h index 29212c4..e3768d4 100644 --- a/robotis_device/include/robotis_device/DynamixelState.h +++ b/robotis_device/include/robotis_device/DynamixelState.h @@ -30,6 +30,7 @@ public: double goal_position; double goal_velocity; double goal_current; + double position_p_gain; std::map bulk_read_table; @@ -43,6 +44,7 @@ public: goal_position(0.0), goal_velocity(0.0), goal_current(0.0), + position_p_gain(0.0), position_offset(0.0) { bulk_read_table.clear(); diff --git a/robotis_device/src/robotis_device/Dynamixel.cpp b/robotis_device/src/robotis_device/Dynamixel.cpp index daf454d..55108ca 100644 --- a/robotis_device/src/robotis_device/Dynamixel.cpp +++ b/robotis_device/src/robotis_device/Dynamixel.cpp @@ -24,7 +24,8 @@ Dynamixel::Dynamixel(int id, std::string model_name, float protocol_version) present_current_item(0), goal_position_item(0), goal_velocity_item(0), - goal_current_item(0) + goal_current_item(0), + position_p_gain_item(0) { this->id = id; this->model_name = model_name; diff --git a/robotis_device/src/robotis_device/Robot.cpp b/robotis_device/src/robotis_device/Robot.cpp index dc1b50e..caa0463 100644 --- a/robotis_device/src/robotis_device/Robot.cpp +++ b/robotis_device/src/robotis_device/Robot.cpp @@ -106,6 +106,9 @@ Robot::Robot(std::string robot_file_path, std::string dev_desc_dir_path) UINT16_T _indirect_data_addr = _dxl->ctrl_table[INDIRECT_DATA_1]->address; for(int _i = 0; _i < sub_tokens.size(); _i++) { + if(_dxl->bulk_read_items[_i] == NULL) + continue; + _dxl->bulk_read_items.push_back(new ControlTableItem()); ControlTableItem *_dest_item = _dxl->bulk_read_items[_i]; ControlTableItem *_src_item = _dxl->ctrl_table[sub_tokens[_i]]; @@ -122,10 +125,13 @@ Robot::Robot(std::string robot_file_path, std::string dev_desc_dir_path) _indirect_data_addr += _dest_item->data_length; } } - else // INDIRECT_ADDRESS_1 exist + else // INDIRECT_ADDRESS_1 not exist { for(int _i = 0; _i < sub_tokens.size(); _i++) - _dxl->bulk_read_items.push_back(_dxl->ctrl_table[sub_tokens[_i]]); + { + if(_dxl->ctrl_table[sub_tokens[_i]] != NULL) + _dxl->bulk_read_items.push_back(_dxl->ctrl_table[sub_tokens[_i]]); + } } } } diff --git a/robotis_framework_common/include/robotis_framework_common/MotionModule.h b/robotis_framework_common/include/robotis_framework_common/MotionModule.h index a305a6a..0d0fb0f 100644 --- a/robotis_framework_common/include/robotis_framework_common/MotionModule.h +++ b/robotis_framework_common/include/robotis_framework_common/MotionModule.h @@ -28,15 +28,35 @@ enum CONTROL_MODE class MotionModule { -public: +protected: bool enable; std::string module_name; CONTROL_MODE control_mode; +public: std::map result; virtual ~MotionModule() { } + std::string GetModuleName() { return module_name; } + CONTROL_MODE GetControlMode() { return control_mode; } + + void SetModuleEnable(bool enable) + { + if(this->enable == enable) + return; + + this->enable = enable; + if(enable) + OnModuleEnable(); + else + OnModuleDisable(); + } + bool GetModuleEnable() { return enable; } + + virtual void OnModuleEnable() { } + virtual void OnModuleDisable() { } + virtual void Initialize(const int control_cycle_msec, Robot *robot) = 0; virtual void Process(std::map dxls, std::map sensors) = 0; diff --git a/robotis_framework_common/include/robotis_framework_common/SensorModule.h b/robotis_framework_common/include/robotis_framework_common/SensorModule.h index 6111391..1c604e9 100644 --- a/robotis_framework_common/include/robotis_framework_common/SensorModule.h +++ b/robotis_framework_common/include/robotis_framework_common/SensorModule.h @@ -21,13 +21,16 @@ namespace ROBOTIS class SensorModule { -public: +protected: std::string module_name; +public: std::map result; virtual ~SensorModule() { } + std::string GetModuleName() { return module_name; } + virtual void Initialize(const int control_cycle_msec, Robot *robot) = 0; virtual void Process(std::map dxls, std::map sensors) = 0; }; From 0e2805e17ed4e4daa2700f479dc46b317fd35239 Mon Sep 17 00:00:00 2001 From: ROBOTIS-zerom Date: Thu, 26 May 2016 11:11:29 +0900 Subject: [PATCH 022/238] - sync write bug fix. --- .../src/robotis_controller/RobotisController.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/robotis_controller/src/robotis_controller/RobotisController.cpp b/robotis_controller/src/robotis_controller/RobotisController.cpp index d6dfda1..f69624a 100644 --- a/robotis_controller/src/robotis_controller/RobotisController.cpp +++ b/robotis_controller/src/robotis_controller/RobotisController.cpp @@ -1316,10 +1316,13 @@ void RobotisController::SetCtrlModuleThread(std::string ctrl_module) _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_pos_data)); _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_pos_data)); - port_to_sync_write_position[_dxl->port_name]->AddParam(_dxl->id, _sync_write_data); + if(port_to_sync_write_position[_dxl->port_name] != NULL) + port_to_sync_write_position[_dxl->port_name]->AddParam(_dxl->id, _sync_write_data); - port_to_sync_write_torque[_dxl->port_name]->RemoveParam(_dxl->id); - port_to_sync_write_velocity[_dxl->port_name]->RemoveParam(_dxl->id); + if(port_to_sync_write_torque[_dxl->port_name] != NULL) + port_to_sync_write_torque[_dxl->port_name]->RemoveParam(_dxl->id); + if(port_to_sync_write_velocity[_dxl->port_name] != NULL) + port_to_sync_write_velocity[_dxl->port_name]->RemoveParam(_dxl->id); } } else From 9e71d55eecb99bfef1ca6728141037cf3aa7db40 Mon Sep 17 00:00:00 2001 From: ROBOTIS-zerom Date: Thu, 26 May 2016 19:11:01 +0900 Subject: [PATCH 023/238] Update LICENSE License changed (to BSD License) --- LICENSE | 365 ++++---------------------------------------------------- 1 file changed, 26 insertions(+), 339 deletions(-) diff --git a/LICENSE b/LICENSE index 23cb790..1d93559 100644 --- a/LICENSE +++ b/LICENSE @@ -1,339 +1,26 @@ - GNU GENERAL PUBLIC LICENSE - Version 2, June 1991 - - Copyright (C) 1989, 1991 Free Software Foundation, Inc., - 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA - Everyone is permitted to copy and distribute verbatim copies - of this license document, but changing it is not allowed. - - Preamble - - The licenses for most software are designed to take away your -freedom to share and change it. By contrast, the GNU General Public -License is intended to guarantee your freedom to share and change free -software--to make sure the software is free for all its users. This -General Public License applies to most of the Free Software -Foundation's software and to any other program whose authors commit to -using it. (Some other Free Software Foundation software is covered by -the GNU Lesser General Public License instead.) You can apply it to -your programs, too. - - When we speak of free software, we are referring to freedom, not -price. Our General Public Licenses are designed to make sure that you -have the freedom to distribute copies of free software (and charge for -this service if you wish), that you receive source code or can get it -if you want it, that you can change the software or use pieces of it -in new free programs; and that you know you can do these things. - - To protect your rights, we need to make restrictions that forbid -anyone to deny you these rights or to ask you to surrender the rights. -These restrictions translate to certain responsibilities for you if you -distribute copies of the software, or if you modify it. - - For example, if you distribute copies of such a program, whether -gratis or for a fee, you must give the recipients all the rights that -you have. You must make sure that they, too, receive or can get the -source code. And you must show them these terms so they know their -rights. - - We protect your rights with two steps: (1) copyright the software, and -(2) offer you this license which gives you legal permission to copy, -distribute and/or modify the software. - - Also, for each author's protection and ours, we want to make certain -that everyone understands that there is no warranty for this free -software. If the software is modified by someone else and passed on, we -want its recipients to know that what they have is not the original, so -that any problems introduced by others will not reflect on the original -authors' reputations. - - Finally, any free program is threatened constantly by software -patents. We wish to avoid the danger that redistributors of a free -program will individually obtain patent licenses, in effect making the -program proprietary. To prevent this, we have made it clear that any -patent must be licensed for everyone's free use or not licensed at all. - - The precise terms and conditions for copying, distribution and -modification follow. - - GNU GENERAL PUBLIC LICENSE - TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION - - 0. This License applies to any program or other work which contains -a notice placed by the copyright holder saying it may be distributed -under the terms of this General Public License. The "Program", below, -refers to any such program or work, and a "work based on the Program" -means either the Program or any derivative work under copyright law: -that is to say, a work containing the Program or a portion of it, -either verbatim or with modifications and/or translated into another -language. (Hereinafter, translation is included without limitation in -the term "modification".) Each licensee is addressed as "you". - -Activities other than copying, distribution and modification are not -covered by this License; they are outside its scope. The act of -running the Program is not restricted, and the output from the Program -is covered only if its contents constitute a work based on the -Program (independent of having been made by running the Program). -Whether that is true depends on what the Program does. - - 1. You may copy and distribute verbatim copies of the Program's -source code as you receive it, in any medium, provided that you -conspicuously and appropriately publish on each copy an appropriate -copyright notice and disclaimer of warranty; keep intact all the -notices that refer to this License and to the absence of any warranty; -and give any other recipients of the Program a copy of this License -along with the Program. - -You may charge a fee for the physical act of transferring a copy, and -you may at your option offer warranty protection in exchange for a fee. - - 2. You may modify your copy or copies of the Program or any portion -of it, thus forming a work based on the Program, and copy and -distribute such modifications or work under the terms of Section 1 -above, provided that you also meet all of these conditions: - - a) You must cause the modified files to carry prominent notices - stating that you changed the files and the date of any change. - - b) You must cause any work that you distribute or publish, that in - whole or in part contains or is derived from the Program or any - part thereof, to be licensed as a whole at no charge to all third - parties under the terms of this License. - - c) If the modified program normally reads commands interactively - when run, you must cause it, when started running for such - interactive use in the most ordinary way, to print or display an - announcement including an appropriate copyright notice and a - notice that there is no warranty (or else, saying that you provide - a warranty) and that users may redistribute the program under - these conditions, and telling the user how to view a copy of this - License. (Exception: if the Program itself is interactive but - does not normally print such an announcement, your work based on - the Program is not required to print an announcement.) - -These requirements apply to the modified work as a whole. If -identifiable sections of that work are not derived from the Program, -and can be reasonably considered independent and separate works in -themselves, then this License, and its terms, do not apply to those -sections when you distribute them as separate works. But when you -distribute the same sections as part of a whole which is a work based -on the Program, the distribution of the whole must be on the terms of -this License, whose permissions for other licensees extend to the -entire whole, and thus to each and every part regardless of who wrote it. - -Thus, it is not the intent of this section to claim rights or contest -your rights to work written entirely by you; rather, the intent is to -exercise the right to control the distribution of derivative or -collective works based on the Program. - -In addition, mere aggregation of another work not based on the Program -with the Program (or with a work based on the Program) on a volume of -a storage or distribution medium does not bring the other work under -the scope of this License. - - 3. You may copy and distribute the Program (or a work based on it, -under Section 2) in object code or executable form under the terms of -Sections 1 and 2 above provided that you also do one of the following: - - a) Accompany it with the complete corresponding machine-readable - source code, which must be distributed under the terms of Sections - 1 and 2 above on a medium customarily used for software interchange; or, - - b) Accompany it with a written offer, valid for at least three - years, to give any third party, for a charge no more than your - cost of physically performing source distribution, a complete - machine-readable copy of the corresponding source code, to be - distributed under the terms of Sections 1 and 2 above on a medium - customarily used for software interchange; or, - - c) Accompany it with the information you received as to the offer - to distribute corresponding source code. (This alternative is - allowed only for noncommercial distribution and only if you - received the program in object code or executable form with such - an offer, in accord with Subsection b above.) - -The source code for a work means the preferred form of the work for -making modifications to it. For an executable work, complete source -code means all the source code for all modules it contains, plus any -associated interface definition files, plus the scripts used to -control compilation and installation of the executable. However, as a -special exception, the source code distributed need not include -anything that is normally distributed (in either source or binary -form) with the major components (compiler, kernel, and so on) of the -operating system on which the executable runs, unless that component -itself accompanies the executable. - -If distribution of executable or object code is made by offering -access to copy from a designated place, then offering equivalent -access to copy the source code from the same place counts as -distribution of the source code, even though third parties are not -compelled to copy the source along with the object code. - - 4. You may not copy, modify, sublicense, or distribute the Program -except as expressly provided under this License. Any attempt -otherwise to copy, modify, sublicense or distribute the Program is -void, and will automatically terminate your rights under this License. -However, parties who have received copies, or rights, from you under -this License will not have their licenses terminated so long as such -parties remain in full compliance. - - 5. You are not required to accept this License, since you have not -signed it. However, nothing else grants you permission to modify or -distribute the Program or its derivative works. These actions are -prohibited by law if you do not accept this License. Therefore, by -modifying or distributing the Program (or any work based on the -Program), you indicate your acceptance of this License to do so, and -all its terms and conditions for copying, distributing or modifying -the Program or works based on it. - - 6. Each time you redistribute the Program (or any work based on the -Program), the recipient automatically receives a license from the -original licensor to copy, distribute or modify the Program subject to -these terms and conditions. You may not impose any further -restrictions on the recipients' exercise of the rights granted herein. -You are not responsible for enforcing compliance by third parties to -this License. - - 7. If, as a consequence of a court judgment or allegation of patent -infringement or for any other reason (not limited to patent issues), -conditions are imposed on you (whether by court order, agreement or -otherwise) that contradict the conditions of this License, they do not -excuse you from the conditions of this License. If you cannot -distribute so as to satisfy simultaneously your obligations under this -License and any other pertinent obligations, then as a consequence you -may not distribute the Program at all. For example, if a patent -license would not permit royalty-free redistribution of the Program by -all those who receive copies directly or indirectly through you, then -the only way you could satisfy both it and this License would be to -refrain entirely from distribution of the Program. - -If any portion of this section is held invalid or unenforceable under -any particular circumstance, the balance of the section is intended to -apply and the section as a whole is intended to apply in other -circumstances. - -It is not the purpose of this section to induce you to infringe any -patents or other property right claims or to contest validity of any -such claims; this section has the sole purpose of protecting the -integrity of the free software distribution system, which is -implemented by public license practices. Many people have made -generous contributions to the wide range of software distributed -through that system in reliance on consistent application of that -system; it is up to the author/donor to decide if he or she is willing -to distribute software through any other system and a licensee cannot -impose that choice. - -This section is intended to make thoroughly clear what is believed to -be a consequence of the rest of this License. - - 8. If the distribution and/or use of the Program is restricted in -certain countries either by patents or by copyrighted interfaces, the -original copyright holder who places the Program under this License -may add an explicit geographical distribution limitation excluding -those countries, so that distribution is permitted only in or among -countries not thus excluded. In such case, this License incorporates -the limitation as if written in the body of this License. - - 9. The Free Software Foundation may publish revised and/or new versions -of the General Public License from time to time. Such new versions will -be similar in spirit to the present version, but may differ in detail to -address new problems or concerns. - -Each version is given a distinguishing version number. If the Program -specifies a version number of this License which applies to it and "any -later version", you have the option of following the terms and conditions -either of that version or of any later version published by the Free -Software Foundation. If the Program does not specify a version number of -this License, you may choose any version ever published by the Free Software -Foundation. - - 10. If you wish to incorporate parts of the Program into other free -programs whose distribution conditions are different, write to the author -to ask for permission. For software which is copyrighted by the Free -Software Foundation, write to the Free Software Foundation; we sometimes -make exceptions for this. Our decision will be guided by the two goals -of preserving the free status of all derivatives of our free software and -of promoting the sharing and reuse of software generally. - - NO WARRANTY - - 11. BECAUSE THE PROGRAM IS LICENSED FREE OF CHARGE, THERE IS NO WARRANTY -FOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW. EXCEPT WHEN -OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES -PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED -OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF -MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE ENTIRE RISK AS -TO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS WITH YOU. SHOULD THE -PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING, -REPAIR OR CORRECTION. - - 12. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING -WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY AND/OR -REDISTRIBUTE THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, -INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING -OUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED -TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY -YOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER -PROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE -POSSIBILITY OF SUCH DAMAGES. - - END OF TERMS AND CONDITIONS - - How to Apply These Terms to Your New Programs - - If you develop a new program, and you want it to be of the greatest -possible use to the public, the best way to achieve this is to make it -free software which everyone can redistribute and change under these terms. - - To do so, attach the following notices to the program. It is safest -to attach them to the start of each source file to most effectively -convey the exclusion of warranty; and each file should have at least -the "copyright" line and a pointer to where the full notice is found. - - {description} - Copyright (C) {year} {fullname} - - This program is free software; you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation; either version 2 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License along - with this program; if not, write to the Free Software Foundation, Inc., - 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. - -Also add information on how to contact you by electronic and paper mail. - -If the program is interactive, make it output a short notice like this -when it starts in an interactive mode: - - Gnomovision version 69, Copyright (C) year name of author - Gnomovision comes with ABSOLUTELY NO WARRANTY; for details type `show w'. - This is free software, and you are welcome to redistribute it - under certain conditions; type `show c' for details. - -The hypothetical commands `show w' and `show c' should show the appropriate -parts of the General Public License. Of course, the commands you use may -be called something other than `show w' and `show c'; they could even be -mouse-clicks or menu items--whatever suits your program. - -You should also get your employer (if you work as a programmer) or your -school, if any, to sign a "copyright disclaimer" for the program, if -necessary. Here is a sample; alter the names: - - Yoyodyne, Inc., hereby disclaims all copyright interest in the program - `Gnomovision' (which makes passes at compilers) written by James Hacker. - - {signature of Ty Coon}, 1 April 1989 - Ty Coon, President of Vice - -This General Public License does not permit incorporating your program into -proprietary programs. If your program is a subroutine library, you may -consider it more useful to permit linking proprietary applications with the -library. If this is what you want to do, use the GNU Lesser General -Public License instead of this License. +Software License Agreement (BSD License) + +Copyright (c) 2014, ROBOTIS Inc. +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 ROBOTIS 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 ROBOTIS "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 ROBOTIS 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. From 47a7028dd484a99e70d1b1ccf79444684c3bfb86 Mon Sep 17 00:00:00 2001 From: ROBOTIS-zerom Date: Thu, 26 May 2016 19:39:38 +0900 Subject: [PATCH 024/238] Setting the license to BSD. --- dynamixel_sdk/package.xml | 2 +- robotis_controller/package.xml | 2 +- robotis_controller_msgs/package.xml | 2 +- robotis_device/package.xml | 2 +- robotis_framework_common/package.xml | 2 +- robotis_math/package.xml | 2 +- 6 files changed, 6 insertions(+), 6 deletions(-) diff --git a/dynamixel_sdk/package.xml b/dynamixel_sdk/package.xml index f21a3d3..ae8e683 100644 --- a/dynamixel_sdk/package.xml +++ b/dynamixel_sdk/package.xml @@ -5,7 +5,7 @@ The dynamixel_sdk package robotis - GPLv2 + BSD ROBOTIS diff --git a/robotis_controller/package.xml b/robotis_controller/package.xml index c504764..8a25f7e 100644 --- a/robotis_controller/package.xml +++ b/robotis_controller/package.xml @@ -7,7 +7,7 @@ ROBOTIS - GPLv2 + BSD diff --git a/robotis_controller_msgs/package.xml b/robotis_controller_msgs/package.xml index 88a4148..3ce5db7 100644 --- a/robotis_controller_msgs/package.xml +++ b/robotis_controller_msgs/package.xml @@ -5,7 +5,7 @@ The robotis_controller_msgs package robotis - GPLv2 + BSD diff --git a/robotis_device/package.xml b/robotis_device/package.xml index 06de7e5..be72665 100644 --- a/robotis_device/package.xml +++ b/robotis_device/package.xml @@ -7,7 +7,7 @@ robotis - GPLv2 + BSD diff --git a/robotis_framework_common/package.xml b/robotis_framework_common/package.xml index 7863f09..feec7f8 100644 --- a/robotis_framework_common/package.xml +++ b/robotis_framework_common/package.xml @@ -13,7 +13,7 @@ - TODO + BSD diff --git a/robotis_math/package.xml b/robotis_math/package.xml index 895ab75..506f261 100644 --- a/robotis_math/package.xml +++ b/robotis_math/package.xml @@ -13,7 +13,7 @@ - TODO + BSD From 79c945f8a0ff7927646af01f0274d759970e58da Mon Sep 17 00:00:00 2001 From: ROBOTIS-zerom Date: Fri, 27 May 2016 18:32:44 +0900 Subject: [PATCH 025/238] - Robot() bug fixed. --- robotis_device/src/robotis_device/Robot.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/robotis_device/src/robotis_device/Robot.cpp b/robotis_device/src/robotis_device/Robot.cpp index caa0463..ff4d86b 100644 --- a/robotis_device/src/robotis_device/Robot.cpp +++ b/robotis_device/src/robotis_device/Robot.cpp @@ -106,9 +106,6 @@ Robot::Robot(std::string robot_file_path, std::string dev_desc_dir_path) UINT16_T _indirect_data_addr = _dxl->ctrl_table[INDIRECT_DATA_1]->address; for(int _i = 0; _i < sub_tokens.size(); _i++) { - if(_dxl->bulk_read_items[_i] == NULL) - continue; - _dxl->bulk_read_items.push_back(new ControlTableItem()); ControlTableItem *_dest_item = _dxl->bulk_read_items[_i]; ControlTableItem *_src_item = _dxl->ctrl_table[sub_tokens[_i]]; From 11b864aa05576d5e8beadf6bcef5beb1a8d1d251 Mon Sep 17 00:00:00 2001 From: ROBOTIS-zerom Date: Mon, 30 May 2016 21:22:24 +0900 Subject: [PATCH 026/238] - cleanup code. - change the order of processing in the Process() function. - added missing mutex for gazebo - fixed crash when running in gazebo simulation --- .../robotis_controller/RobotisController.cpp | 255 ++++++++++-------- 1 file changed, 139 insertions(+), 116 deletions(-) diff --git a/robotis_controller/src/robotis_controller/RobotisController.cpp b/robotis_controller/src/robotis_controller/RobotisController.cpp index f69624a..97d283a 100644 --- a/robotis_controller/src/robotis_controller/RobotisController.cpp +++ b/robotis_controller/src/robotis_controller/RobotisController.cpp @@ -483,16 +483,17 @@ void RobotisController::QueueThread() ros::Subscriber _control_mode_sub = _ros_node.subscribe("/robotis/set_control_mode", 10, &RobotisController::SetControllerModeCallback, this); ros::Subscriber _joint_states_sub = _ros_node.subscribe("/robotis/set_joint_states", 10, &RobotisController::SetJointStatesCallback, this); + ros::Subscriber _gazebo_joint_states_sub; + if(gazebo_mode == true) + _gazebo_joint_states_sub = _ros_node.subscribe("/" + gazebo_robot_name + "/joint_states", 10, &RobotisController::GazeboJointStatesCallback, this); + /* publisher */ goal_joint_state_pub = _ros_node.advertise("/robotis/goal_joint_states", 10); present_joint_state_pub = _ros_node.advertise("/robotis/present_joint_states", 10); current_module_pub = _ros_node.advertise("/robotis/present_joint_ctrl_modules", 10); - ros::Subscriber _gazebo_joint_states_sub; if(gazebo_mode == true) { - _gazebo_joint_states_sub = _ros_node.subscribe("/" + gazebo_robot_name + "/joint_states", 10, &RobotisController::GazeboJointStatesCallback, this); - for(std::map::iterator _it = robot->dxls.begin(); _it != robot->dxls.end(); _it++) gazebo_joint_pub[_it->first] = _ros_node.advertise("/" + gazebo_robot_name + "/" + _it->first + "_pos/command", 1); } @@ -681,115 +682,105 @@ void RobotisController::Process() // ROS_INFO("Controller::Process()"); bool _do_sync_write = false; + ros::Time _start_time; + ros::Duration _time_duration; + + if(DEBUG_PRINT) + _start_time = ros::Time::now(); + sensor_msgs::JointState _goal_state; sensor_msgs::JointState _present_state; - -// ros::Time _now = ros::Time::now(); - + _present_state.header.stamp = ros::Time::now(); + _goal_state.header.stamp = _present_state.header.stamp; // BulkRead Rx + // -> save to Robot->dxls[]->dynamixel_state.present_position if(gazebo_mode == false) { + // BulkRead Rx for(std::map::iterator _it = port_to_bulk_read.begin(); _it != port_to_bulk_read.end(); _it++) + { + robot->ports[_it->first]->SetPacketTimeout(0.0); _it->second->RxPacket(); - } + } -// ros::Duration _dur = ros::Time::now() - _now; -// double _msec = _dur.nsec * 0.000001; -// -// if(_msec > 8) std::cout << "Process duration : " << _msec << std::endl; - - // -> save to Robot->dxls[]->dynamixel_state.present_position - if(robot->dxls.size() > 0) - { - for(std::map::iterator dxl_it = robot->dxls.begin(); dxl_it != robot->dxls.end(); dxl_it++) + if(robot->dxls.size() > 0) { - UINT32_T _data = 0; - - std::string _port_name = dxl_it->second->port_name; - std::string _joint_name = dxl_it->first; - Dynamixel *_dxl = dxl_it->second; - - _present_state.header.stamp = ros::Time::now(); - _goal_state.header.stamp = _present_state.header.stamp; - - if(gazebo_mode == false && _dxl->bulk_read_items.size() > 0) + for(std::map::iterator dxl_it = robot->dxls.begin(); dxl_it != robot->dxls.end(); dxl_it++) { - for(int _i = 0; _i < _dxl->bulk_read_items.size(); _i++) + Dynamixel *_dxl = dxl_it->second; + std::string _port_name = dxl_it->second->port_name; + std::string _joint_name = dxl_it->first; + + if(_dxl->bulk_read_items.size() > 0) { - ControlTableItem *_item = _dxl->bulk_read_items[_i]; - if(port_to_bulk_read[_port_name]->IsAvailable(_dxl->id, _item->address, _item->data_length) == true) + UINT32_T _data = 0; + for(int _i = 0; _i < _dxl->bulk_read_items.size(); _i++) { - _data = port_to_bulk_read[_port_name]->GetData(_dxl->id, _item->address, _item->data_length); + ControlTableItem *_item = _dxl->bulk_read_items[_i]; + if(port_to_bulk_read[_port_name]->IsAvailable(_dxl->id, _item->address, _item->data_length) == true) + { + _data = port_to_bulk_read[_port_name]->GetData(_dxl->id, _item->address, _item->data_length); - // change dxl_state - if(_dxl->present_position_item != 0 && _item->item_name == _dxl->present_position_item->item_name) - _dxl->dxl_state->present_position = _dxl->ConvertValue2Radian(_data) - _dxl->dxl_state->position_offset; // remove offset - else if(_dxl->present_velocity_item != 0 && _item->item_name == _dxl->present_velocity_item->item_name) - _dxl->dxl_state->present_velocity = _dxl->ConvertValue2Velocity(_data); - else if(_dxl->present_current_item != 0 && _item->item_name == _dxl->present_current_item->item_name) - _dxl->dxl_state->present_current = _dxl->ConvertValue2Current(_data); - else if(_dxl->goal_position_item != 0 && _item->item_name == _dxl->goal_position_item->item_name) - _dxl->dxl_state->goal_position = _dxl->ConvertValue2Radian(_data) - _dxl->dxl_state->position_offset; // remove offset - else if(_dxl->goal_velocity_item != 0 && _item->item_name == _dxl->goal_velocity_item->item_name) - _dxl->dxl_state->goal_velocity = _dxl->ConvertValue2Velocity(_data); - else if(_dxl->goal_current_item != 0 && _item->item_name == _dxl->goal_current_item->item_name) - _dxl->dxl_state->goal_current = _dxl->ConvertValue2Current(_data); - else - _dxl->dxl_state->bulk_read_table[_item->item_name] = _data; + // change dxl_state + if(_dxl->present_position_item != 0 && _item->item_name == _dxl->present_position_item->item_name) + _dxl->dxl_state->present_position = _dxl->ConvertValue2Radian(_data) - _dxl->dxl_state->position_offset; // remove offset + else if(_dxl->present_velocity_item != 0 && _item->item_name == _dxl->present_velocity_item->item_name) + _dxl->dxl_state->present_velocity = _dxl->ConvertValue2Velocity(_data); + else if(_dxl->present_current_item != 0 && _item->item_name == _dxl->present_current_item->item_name) + _dxl->dxl_state->present_current = _dxl->ConvertValue2Current(_data); + else if(_dxl->goal_position_item != 0 && _item->item_name == _dxl->goal_position_item->item_name) + _dxl->dxl_state->goal_position = _dxl->ConvertValue2Radian(_data) - _dxl->dxl_state->position_offset; // remove offset + else if(_dxl->goal_velocity_item != 0 && _item->item_name == _dxl->goal_velocity_item->item_name) + _dxl->dxl_state->goal_velocity = _dxl->ConvertValue2Velocity(_data); + else if(_dxl->goal_current_item != 0 && _item->item_name == _dxl->goal_current_item->item_name) + _dxl->dxl_state->goal_current = _dxl->ConvertValue2Current(_data); + else + _dxl->dxl_state->bulk_read_table[_item->item_name] = _data; + } } + + // -> update time stamp to Robot->dxls[]->dynamixel_state.update_time_stamp + _dxl->dxl_state->update_time_stamp = TimeStamp(_present_state.header.stamp.sec, _present_state.header.stamp.nsec); } - - // -> update time stamp to Robot->dxls[]->dynamixel_state.update_time_stamp - _dxl->dxl_state->update_time_stamp = TimeStamp(_present_state.header.stamp.sec, _present_state.header.stamp.nsec); } + } - _present_state.name.push_back(_joint_name); - _present_state.position.push_back(_dxl->dxl_state->present_position); - _present_state.velocity.push_back(_dxl->dxl_state->present_velocity); - _present_state.effort.push_back(_dxl->dxl_state->present_current); + if(robot->sensors.size() > 0) + { + for(std::map::iterator sensor_it = robot->sensors.begin(); sensor_it != robot->sensors.end(); sensor_it++) + { + Sensor *_sensor = sensor_it->second; + std::string _port_name = sensor_it->second->port_name; + std::string _sensor_name = sensor_it->first; - _goal_state.name.push_back(_joint_name); - _goal_state.position.push_back(_dxl->dxl_state->goal_position); - _goal_state.velocity.push_back(_dxl->dxl_state->goal_velocity); - _goal_state.effort.push_back(_dxl->dxl_state->goal_current); + if(_sensor->bulk_read_items.size() > 0) + { + UINT32_T _data = 0; + for(int _i = 0; _i < _sensor->bulk_read_items.size(); _i++) + { + ControlTableItem *_item = _sensor->bulk_read_items[_i]; + if(port_to_bulk_read[_port_name]->IsAvailable(_sensor->id, _item->address, _item->data_length) == true) + { + _data = port_to_bulk_read[_port_name]->GetData(_sensor->id, _item->address, _item->data_length); + + // change sensor_state + _sensor->sensor_state->bulk_read_table[_item->item_name] = _data; + } + } + + // -> update time stamp to Robot->dxls[]->dynamixel_state.update_time_stamp + _sensor->sensor_state->update_time_stamp = TimeStamp(_present_state.header.stamp.sec, _present_state.header.stamp.nsec); + } + } } } - // -> publish present joint_states & goal joint states topic - present_joint_state_pub.publish(_present_state); - goal_joint_state_pub.publish(_goal_state); - - if(robot->sensors.size() > 0) + if(DEBUG_PRINT) { - for(std::map::iterator sensor_it = robot->sensors.begin(); sensor_it != robot->sensors.end(); sensor_it++) - { - UINT32_T _data = 0; - - std::string _port_name = sensor_it->second->port_name; - std::string _sensor_name = sensor_it->first; - Sensor *_sensor = sensor_it->second; - - if(gazebo_mode == false && _sensor->bulk_read_items.size() > 0) - { - for(int _i = 0; _i < _sensor->bulk_read_items.size(); _i++) - { - ControlTableItem *_item = _sensor->bulk_read_items[_i]; - if(port_to_bulk_read[_port_name]->IsAvailable(_sensor->id, _item->address, _item->data_length) == true) - { - _data = port_to_bulk_read[_port_name]->GetData(_sensor->id, _item->address, _item->data_length); - - // change sensor_state - _sensor->sensor_state->bulk_read_table[_item->item_name] = _data; - } - } - - // -> update time stamp to Robot->dxls[]->dynamixel_state.update_time_stamp - ros::Time _now = ros::Time::now(); - _sensor->sensor_state->update_time_stamp = TimeStamp(_now.sec, _now.nsec); - } - } + _time_duration = ros::Time::now() - _start_time; + ROS_INFO("(%2.6f) BulkRead Rx & update state", _time_duration.nsec * 0.000001); } // Call SensorModule Process() @@ -805,6 +796,12 @@ void RobotisController::Process() } } + if(DEBUG_PRINT) + { + _time_duration = ros::Time::now() - _start_time; + ROS_INFO("(%2.6f) SensorModule Process() & save result", _time_duration.nsec * 0.000001); + } + if(controller_mode_ == MOTION_MODULE_MODE) { // Call MotionModule Process() @@ -815,32 +812,21 @@ void RobotisController::Process() for(std::list::iterator module_it = motion_modules_.begin(); module_it != motion_modules_.end(); module_it++) { -// ros::Time _before = ros::Time::now(); - if((*module_it)->GetModuleEnable() == false) continue; (*module_it)->Process(robot->dxls, sensor_result_); -// ros::Duration _dur = ros::Time::now() - _before; -// double _msec = _dur.sec * 1e+3 + _dur.nsec * 0.000001; - -// std::cout << "Process duration ["<< (*module_it)->module_name << "] : " << _msec << std::endl; - - - ros::Time _before = ros::Time::now(); - // for loop : joint list for(std::map::iterator dxl_it = robot->dxls.begin(); dxl_it != robot->dxls.end(); dxl_it++) { - std::string _joint_name = dxl_it->first; - Dynamixel *_dxl = dxl_it->second; + std::string _joint_name = dxl_it->first; + Dynamixel *_dxl = dxl_it->second; + DynamixelState *_dxl_state = dxl_it->second->dxl_state; - DynamixelState *_dxl_state = _dxl->dxl_state; if(_dxl->ctrl_module_name == (*module_it)->GetModuleName()) { _do_sync_write = true; - // ROS_INFO("Set goal value"); DynamixelState *_result_state = (*module_it)->result[_joint_name]; if(_result_state == NULL) { @@ -853,14 +839,14 @@ void RobotisController::Process() if((*module_it)->GetControlMode() == POSITION_CONTROL) { // if(_result_state->goal_position == 0 && _dxl->id == 3) -// ROS_INFO("[MODULE:%s][ID:3] goal position = %f", (*module_it)->module_name.c_str(), _dxl_state->goal_position); +// ROS_INFO("[MODULE:%s][ID:%2d] goal position = %f", (*module_it)->GetModuleName().c_str(), _dxl->id, _dxl_state->goal_position); _dxl_state->goal_position = _result_state->goal_position; if(gazebo_mode == false) { // add offset UINT32_T _pos_data = _dxl->ConvertRadian2Value(_dxl_state->goal_position + _dxl_state->position_offset); - UINT8_T _sync_write_data[4] = {0}; + UINT8_T _sync_write_data[4] = {0}; _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_pos_data)); _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_pos_data)); _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_pos_data)); @@ -895,7 +881,7 @@ void RobotisController::Process() if(gazebo_mode == false) { UINT32_T _vel_data = _dxl->ConvertVelocity2Value(_dxl_state->goal_velocity); - UINT8_T _sync_write_data[4]; + UINT8_T _sync_write_data[4] = {0}; _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_vel_data)); _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_vel_data)); _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_vel_data)); @@ -922,18 +908,17 @@ void RobotisController::Process() } } } - - ros::Duration _dur = ros::Time::now() - _before; - double _msec = _dur.sec * 1e+3 + _dur.nsec * 0.000001; - - //std::cout << "Process duration ["<< (*module_it)->module_name << "] : " << _msec << std::endl; - } - // std::cout << " ------------------------------------------- " << std::endl; queue_mutex_.unlock(); } + if(DEBUG_PRINT) + { + _time_duration = ros::Time::now() - _start_time; + ROS_INFO("(%2.6f) MotionModule Process() & save result", _time_duration.nsec * 0.000001); + } + // SyncWrite if(gazebo_mode == false && _do_sync_write) { @@ -995,10 +980,38 @@ void RobotisController::Process() _it->second->TxPacket(); } -// ros::Duration _dur = ros::Time::now() - _now; -// double _msec = _dur.nsec * 0.000001; -// -// if(_msec > 8) std::cout << "Process duration : " << _msec << std::endl; + if(DEBUG_PRINT) + { + _time_duration = ros::Time::now() - _start_time; + ROS_INFO("(%2.6f) SyncWrite & BulkRead Tx", _time_duration.nsec * 0.000001); + } + + // publish present & goal position + for(std::map::iterator dxl_it = robot->dxls.begin(); dxl_it != robot->dxls.end(); dxl_it++) + { + std::string _joint_name = dxl_it->first; + Dynamixel *_dxl = dxl_it->second; + + _present_state.name.push_back(_joint_name); + _present_state.position.push_back(_dxl->dxl_state->present_position); + _present_state.velocity.push_back(_dxl->dxl_state->present_velocity); + _present_state.effort.push_back(_dxl->dxl_state->present_current); + + _goal_state.name.push_back(_joint_name); + _goal_state.position.push_back(_dxl->dxl_state->goal_position); + _goal_state.velocity.push_back(_dxl->dxl_state->goal_velocity); + _goal_state.effort.push_back(_dxl->dxl_state->goal_current); + } + + // -> publish present joint_states & goal joint states topic + present_joint_state_pub.publish(_present_state); + goal_joint_state_pub.publish(_goal_state); + + if(DEBUG_PRINT) + { + _time_duration = ros::Time::now() - _start_time; + ROS_WARN("(%2.6f) Process() DONE", _time_duration.nsec * 0.000001); + } _is_process_running = false; } @@ -1309,6 +1322,9 @@ void RobotisController::SetCtrlModuleThread(std::string ctrl_module) Dynamixel *_dxl = _d_it->second; _dxl->ctrl_module_name = "none"; + if(gazebo_mode == true) + continue; + UINT32_T _pos_data = _dxl->ConvertRadian2Value(_dxl->dxl_state->goal_position + _dxl->dxl_state->position_offset); UINT8_T _sync_write_data[4]; _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_pos_data)); @@ -1342,6 +1358,9 @@ void RobotisController::SetCtrlModuleThread(std::string ctrl_module) Dynamixel *_dxl = _d_it->second; _dxl->ctrl_module_name = ctrl_module; + if(gazebo_mode == true) + continue; + if(_mode == POSITION_CONTROL) { UINT32_T _pos_data = _dxl->ConvertRadian2Value(_dxl->dxl_state->goal_position + _dxl->dxl_state->position_offset); @@ -1436,6 +1455,8 @@ void RobotisController::SetCtrlModuleThread(std::string ctrl_module) void RobotisController::GazeboJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg) { + queue_mutex_.lock(); + for(unsigned int _i = 0; _i < msg->name.size(); _i++) { std::map::iterator _d_it = robot->dxls.find((std::string)msg->name[_i]); @@ -1453,6 +1474,8 @@ void RobotisController::GazeboJointStatesCallback(const sensor_msgs::JointState: _it->second->dxl_state->goal_position = _it->second->dxl_state->present_position; init_pose_loaded_ = true; } + + queue_mutex_.unlock(); } bool RobotisController::CheckTimerStop() From 7ad51f94ea713106b4d9c9f0204e738b8804bde3 Mon Sep 17 00:00:00 2001 From: ROBOTIS-zerom Date: Wed, 1 Jun 2016 16:53:03 +0900 Subject: [PATCH 027/238] - License changed (to BSD) --- LICENSE | 365 ++------------------------- dynamixel_sdk/package.xml | 2 +- robotis_controller/package.xml | 2 +- robotis_controller_msgs/package.xml | 2 +- robotis_device/package.xml | 2 +- robotis_framework_common/package.xml | 2 +- robotis_math/package.xml | 2 +- 7 files changed, 32 insertions(+), 345 deletions(-) diff --git a/LICENSE b/LICENSE index 23cb790..1d93559 100644 --- a/LICENSE +++ b/LICENSE @@ -1,339 +1,26 @@ - GNU GENERAL PUBLIC LICENSE - Version 2, June 1991 - - Copyright (C) 1989, 1991 Free Software Foundation, Inc., - 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA - Everyone is permitted to copy and distribute verbatim copies - of this license document, but changing it is not allowed. - - Preamble - - The licenses for most software are designed to take away your -freedom to share and change it. By contrast, the GNU General Public -License is intended to guarantee your freedom to share and change free -software--to make sure the software is free for all its users. This -General Public License applies to most of the Free Software -Foundation's software and to any other program whose authors commit to -using it. (Some other Free Software Foundation software is covered by -the GNU Lesser General Public License instead.) You can apply it to -your programs, too. - - When we speak of free software, we are referring to freedom, not -price. Our General Public Licenses are designed to make sure that you -have the freedom to distribute copies of free software (and charge for -this service if you wish), that you receive source code or can get it -if you want it, that you can change the software or use pieces of it -in new free programs; and that you know you can do these things. - - To protect your rights, we need to make restrictions that forbid -anyone to deny you these rights or to ask you to surrender the rights. -These restrictions translate to certain responsibilities for you if you -distribute copies of the software, or if you modify it. - - For example, if you distribute copies of such a program, whether -gratis or for a fee, you must give the recipients all the rights that -you have. You must make sure that they, too, receive or can get the -source code. And you must show them these terms so they know their -rights. - - We protect your rights with two steps: (1) copyright the software, and -(2) offer you this license which gives you legal permission to copy, -distribute and/or modify the software. - - Also, for each author's protection and ours, we want to make certain -that everyone understands that there is no warranty for this free -software. If the software is modified by someone else and passed on, we -want its recipients to know that what they have is not the original, so -that any problems introduced by others will not reflect on the original -authors' reputations. - - Finally, any free program is threatened constantly by software -patents. We wish to avoid the danger that redistributors of a free -program will individually obtain patent licenses, in effect making the -program proprietary. To prevent this, we have made it clear that any -patent must be licensed for everyone's free use or not licensed at all. - - The precise terms and conditions for copying, distribution and -modification follow. - - GNU GENERAL PUBLIC LICENSE - TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION - - 0. This License applies to any program or other work which contains -a notice placed by the copyright holder saying it may be distributed -under the terms of this General Public License. The "Program", below, -refers to any such program or work, and a "work based on the Program" -means either the Program or any derivative work under copyright law: -that is to say, a work containing the Program or a portion of it, -either verbatim or with modifications and/or translated into another -language. (Hereinafter, translation is included without limitation in -the term "modification".) Each licensee is addressed as "you". - -Activities other than copying, distribution and modification are not -covered by this License; they are outside its scope. The act of -running the Program is not restricted, and the output from the Program -is covered only if its contents constitute a work based on the -Program (independent of having been made by running the Program). -Whether that is true depends on what the Program does. - - 1. You may copy and distribute verbatim copies of the Program's -source code as you receive it, in any medium, provided that you -conspicuously and appropriately publish on each copy an appropriate -copyright notice and disclaimer of warranty; keep intact all the -notices that refer to this License and to the absence of any warranty; -and give any other recipients of the Program a copy of this License -along with the Program. - -You may charge a fee for the physical act of transferring a copy, and -you may at your option offer warranty protection in exchange for a fee. - - 2. You may modify your copy or copies of the Program or any portion -of it, thus forming a work based on the Program, and copy and -distribute such modifications or work under the terms of Section 1 -above, provided that you also meet all of these conditions: - - a) You must cause the modified files to carry prominent notices - stating that you changed the files and the date of any change. - - b) You must cause any work that you distribute or publish, that in - whole or in part contains or is derived from the Program or any - part thereof, to be licensed as a whole at no charge to all third - parties under the terms of this License. - - c) If the modified program normally reads commands interactively - when run, you must cause it, when started running for such - interactive use in the most ordinary way, to print or display an - announcement including an appropriate copyright notice and a - notice that there is no warranty (or else, saying that you provide - a warranty) and that users may redistribute the program under - these conditions, and telling the user how to view a copy of this - License. (Exception: if the Program itself is interactive but - does not normally print such an announcement, your work based on - the Program is not required to print an announcement.) - -These requirements apply to the modified work as a whole. If -identifiable sections of that work are not derived from the Program, -and can be reasonably considered independent and separate works in -themselves, then this License, and its terms, do not apply to those -sections when you distribute them as separate works. But when you -distribute the same sections as part of a whole which is a work based -on the Program, the distribution of the whole must be on the terms of -this License, whose permissions for other licensees extend to the -entire whole, and thus to each and every part regardless of who wrote it. - -Thus, it is not the intent of this section to claim rights or contest -your rights to work written entirely by you; rather, the intent is to -exercise the right to control the distribution of derivative or -collective works based on the Program. - -In addition, mere aggregation of another work not based on the Program -with the Program (or with a work based on the Program) on a volume of -a storage or distribution medium does not bring the other work under -the scope of this License. - - 3. You may copy and distribute the Program (or a work based on it, -under Section 2) in object code or executable form under the terms of -Sections 1 and 2 above provided that you also do one of the following: - - a) Accompany it with the complete corresponding machine-readable - source code, which must be distributed under the terms of Sections - 1 and 2 above on a medium customarily used for software interchange; or, - - b) Accompany it with a written offer, valid for at least three - years, to give any third party, for a charge no more than your - cost of physically performing source distribution, a complete - machine-readable copy of the corresponding source code, to be - distributed under the terms of Sections 1 and 2 above on a medium - customarily used for software interchange; or, - - c) Accompany it with the information you received as to the offer - to distribute corresponding source code. (This alternative is - allowed only for noncommercial distribution and only if you - received the program in object code or executable form with such - an offer, in accord with Subsection b above.) - -The source code for a work means the preferred form of the work for -making modifications to it. For an executable work, complete source -code means all the source code for all modules it contains, plus any -associated interface definition files, plus the scripts used to -control compilation and installation of the executable. However, as a -special exception, the source code distributed need not include -anything that is normally distributed (in either source or binary -form) with the major components (compiler, kernel, and so on) of the -operating system on which the executable runs, unless that component -itself accompanies the executable. - -If distribution of executable or object code is made by offering -access to copy from a designated place, then offering equivalent -access to copy the source code from the same place counts as -distribution of the source code, even though third parties are not -compelled to copy the source along with the object code. - - 4. You may not copy, modify, sublicense, or distribute the Program -except as expressly provided under this License. Any attempt -otherwise to copy, modify, sublicense or distribute the Program is -void, and will automatically terminate your rights under this License. -However, parties who have received copies, or rights, from you under -this License will not have their licenses terminated so long as such -parties remain in full compliance. - - 5. You are not required to accept this License, since you have not -signed it. However, nothing else grants you permission to modify or -distribute the Program or its derivative works. These actions are -prohibited by law if you do not accept this License. Therefore, by -modifying or distributing the Program (or any work based on the -Program), you indicate your acceptance of this License to do so, and -all its terms and conditions for copying, distributing or modifying -the Program or works based on it. - - 6. Each time you redistribute the Program (or any work based on the -Program), the recipient automatically receives a license from the -original licensor to copy, distribute or modify the Program subject to -these terms and conditions. You may not impose any further -restrictions on the recipients' exercise of the rights granted herein. -You are not responsible for enforcing compliance by third parties to -this License. - - 7. If, as a consequence of a court judgment or allegation of patent -infringement or for any other reason (not limited to patent issues), -conditions are imposed on you (whether by court order, agreement or -otherwise) that contradict the conditions of this License, they do not -excuse you from the conditions of this License. If you cannot -distribute so as to satisfy simultaneously your obligations under this -License and any other pertinent obligations, then as a consequence you -may not distribute the Program at all. For example, if a patent -license would not permit royalty-free redistribution of the Program by -all those who receive copies directly or indirectly through you, then -the only way you could satisfy both it and this License would be to -refrain entirely from distribution of the Program. - -If any portion of this section is held invalid or unenforceable under -any particular circumstance, the balance of the section is intended to -apply and the section as a whole is intended to apply in other -circumstances. - -It is not the purpose of this section to induce you to infringe any -patents or other property right claims or to contest validity of any -such claims; this section has the sole purpose of protecting the -integrity of the free software distribution system, which is -implemented by public license practices. Many people have made -generous contributions to the wide range of software distributed -through that system in reliance on consistent application of that -system; it is up to the author/donor to decide if he or she is willing -to distribute software through any other system and a licensee cannot -impose that choice. - -This section is intended to make thoroughly clear what is believed to -be a consequence of the rest of this License. - - 8. If the distribution and/or use of the Program is restricted in -certain countries either by patents or by copyrighted interfaces, the -original copyright holder who places the Program under this License -may add an explicit geographical distribution limitation excluding -those countries, so that distribution is permitted only in or among -countries not thus excluded. In such case, this License incorporates -the limitation as if written in the body of this License. - - 9. The Free Software Foundation may publish revised and/or new versions -of the General Public License from time to time. Such new versions will -be similar in spirit to the present version, but may differ in detail to -address new problems or concerns. - -Each version is given a distinguishing version number. If the Program -specifies a version number of this License which applies to it and "any -later version", you have the option of following the terms and conditions -either of that version or of any later version published by the Free -Software Foundation. If the Program does not specify a version number of -this License, you may choose any version ever published by the Free Software -Foundation. - - 10. If you wish to incorporate parts of the Program into other free -programs whose distribution conditions are different, write to the author -to ask for permission. For software which is copyrighted by the Free -Software Foundation, write to the Free Software Foundation; we sometimes -make exceptions for this. Our decision will be guided by the two goals -of preserving the free status of all derivatives of our free software and -of promoting the sharing and reuse of software generally. - - NO WARRANTY - - 11. BECAUSE THE PROGRAM IS LICENSED FREE OF CHARGE, THERE IS NO WARRANTY -FOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW. EXCEPT WHEN -OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES -PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED -OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF -MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE ENTIRE RISK AS -TO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS WITH YOU. SHOULD THE -PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING, -REPAIR OR CORRECTION. - - 12. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING -WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY AND/OR -REDISTRIBUTE THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, -INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING -OUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED -TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY -YOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER -PROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE -POSSIBILITY OF SUCH DAMAGES. - - END OF TERMS AND CONDITIONS - - How to Apply These Terms to Your New Programs - - If you develop a new program, and you want it to be of the greatest -possible use to the public, the best way to achieve this is to make it -free software which everyone can redistribute and change under these terms. - - To do so, attach the following notices to the program. It is safest -to attach them to the start of each source file to most effectively -convey the exclusion of warranty; and each file should have at least -the "copyright" line and a pointer to where the full notice is found. - - {description} - Copyright (C) {year} {fullname} - - This program is free software; you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation; either version 2 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License along - with this program; if not, write to the Free Software Foundation, Inc., - 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. - -Also add information on how to contact you by electronic and paper mail. - -If the program is interactive, make it output a short notice like this -when it starts in an interactive mode: - - Gnomovision version 69, Copyright (C) year name of author - Gnomovision comes with ABSOLUTELY NO WARRANTY; for details type `show w'. - This is free software, and you are welcome to redistribute it - under certain conditions; type `show c' for details. - -The hypothetical commands `show w' and `show c' should show the appropriate -parts of the General Public License. Of course, the commands you use may -be called something other than `show w' and `show c'; they could even be -mouse-clicks or menu items--whatever suits your program. - -You should also get your employer (if you work as a programmer) or your -school, if any, to sign a "copyright disclaimer" for the program, if -necessary. Here is a sample; alter the names: - - Yoyodyne, Inc., hereby disclaims all copyright interest in the program - `Gnomovision' (which makes passes at compilers) written by James Hacker. - - {signature of Ty Coon}, 1 April 1989 - Ty Coon, President of Vice - -This General Public License does not permit incorporating your program into -proprietary programs. If your program is a subroutine library, you may -consider it more useful to permit linking proprietary applications with the -library. If this is what you want to do, use the GNU Lesser General -Public License instead of this License. +Software License Agreement (BSD License) + +Copyright (c) 2014, ROBOTIS Inc. +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 ROBOTIS 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 ROBOTIS "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 ROBOTIS 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. diff --git a/dynamixel_sdk/package.xml b/dynamixel_sdk/package.xml index f21a3d3..ae8e683 100644 --- a/dynamixel_sdk/package.xml +++ b/dynamixel_sdk/package.xml @@ -5,7 +5,7 @@ The dynamixel_sdk package robotis - GPLv2 + BSD ROBOTIS diff --git a/robotis_controller/package.xml b/robotis_controller/package.xml index c504764..8a25f7e 100644 --- a/robotis_controller/package.xml +++ b/robotis_controller/package.xml @@ -7,7 +7,7 @@ ROBOTIS - GPLv2 + BSD diff --git a/robotis_controller_msgs/package.xml b/robotis_controller_msgs/package.xml index 88a4148..3ce5db7 100644 --- a/robotis_controller_msgs/package.xml +++ b/robotis_controller_msgs/package.xml @@ -5,7 +5,7 @@ The robotis_controller_msgs package robotis - GPLv2 + BSD diff --git a/robotis_device/package.xml b/robotis_device/package.xml index 06de7e5..be72665 100644 --- a/robotis_device/package.xml +++ b/robotis_device/package.xml @@ -7,7 +7,7 @@ robotis - GPLv2 + BSD diff --git a/robotis_framework_common/package.xml b/robotis_framework_common/package.xml index 7863f09..feec7f8 100644 --- a/robotis_framework_common/package.xml +++ b/robotis_framework_common/package.xml @@ -13,7 +13,7 @@ - TODO + BSD diff --git a/robotis_math/package.xml b/robotis_math/package.xml index 895ab75..506f261 100644 --- a/robotis_math/package.xml +++ b/robotis_math/package.xml @@ -13,7 +13,7 @@ - TODO + BSD From bd9bf5dcb2124f687a4579269cff2c20076dbfbe Mon Sep 17 00:00:00 2001 From: ROBOTIS-zerom Date: Fri, 3 Jun 2016 19:54:29 +0900 Subject: [PATCH 028/238] - file name rename (according to the ROS C++ Coding Style) --- ...botisController.h => robotis_controller.h} | 0 ...sController.cpp => robotis_controller.cpp} | 0 ...ontrolTableItem.h => control_table_item.h} | 0 .../robotis_device/{Device.h => device.h} | 0 .../{Dynamixel.h => dynamixel.h} | 0 .../{DynamixelState.h => dynamixel_state.h} | 0 .../robotis_device/{Robot.h => robot.h} | 0 .../robotis_device/{Sensor.h => sensor.h} | 0 .../{SensorState.h => sensor_state.h} | 0 .../{TimeStamp.h => time_stamp.h} | 0 .../{Dynamixel.cpp => dynamixel.cpp} | 0 .../robotis_device/{Robot.cpp => robot.cpp} | 0 .../robotis_device/{Sensor.cpp => sensor.cpp} | 0 .../robotis_framework_common/RobotisDef.h | 21 ------------------- .../{MotionModule.h => motion_module.h} | 0 .../{SensorModule.h => sensor_module.h} | 0 .../{Singleton.h => singleton.h} | 0 ...nearAlgebra.h => robotis_linear_algebra.h} | 0 .../{RobotisMath.h => robotis_math.h} | 0 ...{RobotisMathBase.h => robotis_math_base.h} | 0 ...ator.h => robotis_trajectory_calculator.h} | 0 ...Algebra.cpp => robotis_linear_algebra.cpp} | 0 ...otisMathBase.cpp => robotis_math_base.cpp} | 0 ....cpp => robotis_trajectory_calculator.cpp} | 0 24 files changed, 21 deletions(-) rename robotis_controller/include/robotis_controller/{RobotisController.h => robotis_controller.h} (100%) rename robotis_controller/src/robotis_controller/{RobotisController.cpp => robotis_controller.cpp} (100%) rename robotis_device/include/robotis_device/{ControlTableItem.h => control_table_item.h} (100%) rename robotis_device/include/robotis_device/{Device.h => device.h} (100%) rename robotis_device/include/robotis_device/{Dynamixel.h => dynamixel.h} (100%) rename robotis_device/include/robotis_device/{DynamixelState.h => dynamixel_state.h} (100%) rename robotis_device/include/robotis_device/{Robot.h => robot.h} (100%) rename robotis_device/include/robotis_device/{Sensor.h => sensor.h} (100%) rename robotis_device/include/robotis_device/{SensorState.h => sensor_state.h} (100%) rename robotis_device/include/robotis_device/{TimeStamp.h => time_stamp.h} (100%) rename robotis_device/src/robotis_device/{Dynamixel.cpp => dynamixel.cpp} (100%) rename robotis_device/src/robotis_device/{Robot.cpp => robot.cpp} (100%) rename robotis_device/src/robotis_device/{Sensor.cpp => sensor.cpp} (100%) delete mode 100644 robotis_framework_common/include/robotis_framework_common/RobotisDef.h rename robotis_framework_common/include/robotis_framework_common/{MotionModule.h => motion_module.h} (100%) rename robotis_framework_common/include/robotis_framework_common/{SensorModule.h => sensor_module.h} (100%) rename robotis_framework_common/include/robotis_framework_common/{Singleton.h => singleton.h} (100%) rename robotis_math/include/robotis_math/{RobotisLinearAlgebra.h => robotis_linear_algebra.h} (100%) rename robotis_math/include/robotis_math/{RobotisMath.h => robotis_math.h} (100%) rename robotis_math/include/robotis_math/{RobotisMathBase.h => robotis_math_base.h} (100%) rename robotis_math/include/robotis_math/{RobotisTrajectoryCalculator.h => robotis_trajectory_calculator.h} (100%) rename robotis_math/src/{RobotisLinearAlgebra.cpp => robotis_linear_algebra.cpp} (100%) rename robotis_math/src/{RobotisMathBase.cpp => robotis_math_base.cpp} (100%) rename robotis_math/src/{RobotisTrajectoryCalculator.cpp => robotis_trajectory_calculator.cpp} (100%) diff --git a/robotis_controller/include/robotis_controller/RobotisController.h b/robotis_controller/include/robotis_controller/robotis_controller.h similarity index 100% rename from robotis_controller/include/robotis_controller/RobotisController.h rename to robotis_controller/include/robotis_controller/robotis_controller.h diff --git a/robotis_controller/src/robotis_controller/RobotisController.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp similarity index 100% rename from robotis_controller/src/robotis_controller/RobotisController.cpp rename to robotis_controller/src/robotis_controller/robotis_controller.cpp diff --git a/robotis_device/include/robotis_device/ControlTableItem.h b/robotis_device/include/robotis_device/control_table_item.h similarity index 100% rename from robotis_device/include/robotis_device/ControlTableItem.h rename to robotis_device/include/robotis_device/control_table_item.h diff --git a/robotis_device/include/robotis_device/Device.h b/robotis_device/include/robotis_device/device.h similarity index 100% rename from robotis_device/include/robotis_device/Device.h rename to robotis_device/include/robotis_device/device.h diff --git a/robotis_device/include/robotis_device/Dynamixel.h b/robotis_device/include/robotis_device/dynamixel.h similarity index 100% rename from robotis_device/include/robotis_device/Dynamixel.h rename to robotis_device/include/robotis_device/dynamixel.h diff --git a/robotis_device/include/robotis_device/DynamixelState.h b/robotis_device/include/robotis_device/dynamixel_state.h similarity index 100% rename from robotis_device/include/robotis_device/DynamixelState.h rename to robotis_device/include/robotis_device/dynamixel_state.h diff --git a/robotis_device/include/robotis_device/Robot.h b/robotis_device/include/robotis_device/robot.h similarity index 100% rename from robotis_device/include/robotis_device/Robot.h rename to robotis_device/include/robotis_device/robot.h diff --git a/robotis_device/include/robotis_device/Sensor.h b/robotis_device/include/robotis_device/sensor.h similarity index 100% rename from robotis_device/include/robotis_device/Sensor.h rename to robotis_device/include/robotis_device/sensor.h diff --git a/robotis_device/include/robotis_device/SensorState.h b/robotis_device/include/robotis_device/sensor_state.h similarity index 100% rename from robotis_device/include/robotis_device/SensorState.h rename to robotis_device/include/robotis_device/sensor_state.h diff --git a/robotis_device/include/robotis_device/TimeStamp.h b/robotis_device/include/robotis_device/time_stamp.h similarity index 100% rename from robotis_device/include/robotis_device/TimeStamp.h rename to robotis_device/include/robotis_device/time_stamp.h diff --git a/robotis_device/src/robotis_device/Dynamixel.cpp b/robotis_device/src/robotis_device/dynamixel.cpp similarity index 100% rename from robotis_device/src/robotis_device/Dynamixel.cpp rename to robotis_device/src/robotis_device/dynamixel.cpp diff --git a/robotis_device/src/robotis_device/Robot.cpp b/robotis_device/src/robotis_device/robot.cpp similarity index 100% rename from robotis_device/src/robotis_device/Robot.cpp rename to robotis_device/src/robotis_device/robot.cpp diff --git a/robotis_device/src/robotis_device/Sensor.cpp b/robotis_device/src/robotis_device/sensor.cpp similarity index 100% rename from robotis_device/src/robotis_device/Sensor.cpp rename to robotis_device/src/robotis_device/sensor.cpp diff --git a/robotis_framework_common/include/robotis_framework_common/RobotisDef.h b/robotis_framework_common/include/robotis_framework_common/RobotisDef.h deleted file mode 100644 index a860e9d..0000000 --- a/robotis_framework_common/include/robotis_framework_common/RobotisDef.h +++ /dev/null @@ -1,21 +0,0 @@ -/* - * RobotisDef.h - * - * Created on: 2016. 1. 27. - * Author: zerom - */ - -#ifndef ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_ROBOTISDEF_H_ -#define ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_ROBOTISDEF_H_ - - -typedef char INT8_T; -typedef short int INT16_T; -typedef int INT32_T; - -typedef unsigned char UINT8_T; -typedef unsigned short int UINT16_T; -typedef unsigned int UINT32_T; - - -#endif /* ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_ROBOTISDEF_H_ */ diff --git a/robotis_framework_common/include/robotis_framework_common/MotionModule.h b/robotis_framework_common/include/robotis_framework_common/motion_module.h similarity index 100% rename from robotis_framework_common/include/robotis_framework_common/MotionModule.h rename to robotis_framework_common/include/robotis_framework_common/motion_module.h diff --git a/robotis_framework_common/include/robotis_framework_common/SensorModule.h b/robotis_framework_common/include/robotis_framework_common/sensor_module.h similarity index 100% rename from robotis_framework_common/include/robotis_framework_common/SensorModule.h rename to robotis_framework_common/include/robotis_framework_common/sensor_module.h diff --git a/robotis_framework_common/include/robotis_framework_common/Singleton.h b/robotis_framework_common/include/robotis_framework_common/singleton.h similarity index 100% rename from robotis_framework_common/include/robotis_framework_common/Singleton.h rename to robotis_framework_common/include/robotis_framework_common/singleton.h diff --git a/robotis_math/include/robotis_math/RobotisLinearAlgebra.h b/robotis_math/include/robotis_math/robotis_linear_algebra.h similarity index 100% rename from robotis_math/include/robotis_math/RobotisLinearAlgebra.h rename to robotis_math/include/robotis_math/robotis_linear_algebra.h diff --git a/robotis_math/include/robotis_math/RobotisMath.h b/robotis_math/include/robotis_math/robotis_math.h similarity index 100% rename from robotis_math/include/robotis_math/RobotisMath.h rename to robotis_math/include/robotis_math/robotis_math.h diff --git a/robotis_math/include/robotis_math/RobotisMathBase.h b/robotis_math/include/robotis_math/robotis_math_base.h similarity index 100% rename from robotis_math/include/robotis_math/RobotisMathBase.h rename to robotis_math/include/robotis_math/robotis_math_base.h diff --git a/robotis_math/include/robotis_math/RobotisTrajectoryCalculator.h b/robotis_math/include/robotis_math/robotis_trajectory_calculator.h similarity index 100% rename from robotis_math/include/robotis_math/RobotisTrajectoryCalculator.h rename to robotis_math/include/robotis_math/robotis_trajectory_calculator.h diff --git a/robotis_math/src/RobotisLinearAlgebra.cpp b/robotis_math/src/robotis_linear_algebra.cpp similarity index 100% rename from robotis_math/src/RobotisLinearAlgebra.cpp rename to robotis_math/src/robotis_linear_algebra.cpp diff --git a/robotis_math/src/RobotisMathBase.cpp b/robotis_math/src/robotis_math_base.cpp similarity index 100% rename from robotis_math/src/RobotisMathBase.cpp rename to robotis_math/src/robotis_math_base.cpp diff --git a/robotis_math/src/RobotisTrajectoryCalculator.cpp b/robotis_math/src/robotis_trajectory_calculator.cpp similarity index 100% rename from robotis_math/src/RobotisTrajectoryCalculator.cpp rename to robotis_math/src/robotis_trajectory_calculator.cpp From 4dcaa5900fe8ed89a4be401078115be70a060595 Mon Sep 17 00:00:00 2001 From: ROBOTIS-zerom Date: Fri, 3 Jun 2016 20:03:18 +0900 Subject: [PATCH 029/238] - rename the files (according to ROS C++ Coding Style) --- dynamixel_sdk/CMakeLists.txt | 18 +- .../{DynamixelSDK.h => dynamixel_sdk.h} | 0 .../include/dynamixel_sdk/RobotisDef.h | 21 -- .../{GroupBulkRead.h => group_bulk_read.h} | 0 .../{GroupBulkWrite.h => group_bulk_write.h} | 0 .../{GroupSyncRead.h => group_sync_read.h} | 0 .../{GroupSyncWrite.h => group_sync_write.h} | 0 .../{PacketHandler.h => packet_handler.h} | 0 .../{PortHandler.h => port_handler.h} | 0 ...etHandler.h => protocol1_packet_handler.h} | 0 ...etHandler.h => protocol2_packet_handler.h} | 0 ...ortHandlerLinux.h => port_handler_linux.h} | 0 .../port_handler_windows.h | 93 +++++++ ...{GroupBulkRead.cpp => group_bulk_read.cpp} | 0 ...roupBulkWrite.cpp => group_bulk_write.cpp} | 0 ...{GroupSyncRead.cpp => group_sync_read.cpp} | 0 ...roupSyncWrite.cpp => group_sync_write.cpp} | 0 .../{PacketHandler.cpp => packet_handler.cpp} | 0 .../{PortHandler.cpp => port_handler.cpp} | 0 ...ndler.cpp => protocol1_packet_handler.cpp} | 0 ...ndler.cpp => protocol2_packet_handler.cpp} | 0 ...andlerLinux.cpp => port_handler_linux.cpp} | 0 .../port_handler_windows.cpp | 245 ++++++++++++++++++ 23 files changed, 347 insertions(+), 30 deletions(-) rename dynamixel_sdk/include/{DynamixelSDK.h => dynamixel_sdk.h} (100%) delete mode 100644 dynamixel_sdk/include/dynamixel_sdk/RobotisDef.h rename dynamixel_sdk/include/dynamixel_sdk/{GroupBulkRead.h => group_bulk_read.h} (100%) rename dynamixel_sdk/include/dynamixel_sdk/{GroupBulkWrite.h => group_bulk_write.h} (100%) rename dynamixel_sdk/include/dynamixel_sdk/{GroupSyncRead.h => group_sync_read.h} (100%) rename dynamixel_sdk/include/dynamixel_sdk/{GroupSyncWrite.h => group_sync_write.h} (100%) rename dynamixel_sdk/include/dynamixel_sdk/{PacketHandler.h => packet_handler.h} (100%) rename dynamixel_sdk/include/dynamixel_sdk/{PortHandler.h => port_handler.h} (100%) rename dynamixel_sdk/include/dynamixel_sdk/{Protocol1PacketHandler.h => protocol1_packet_handler.h} (100%) rename dynamixel_sdk/include/dynamixel_sdk/{Protocol2PacketHandler.h => protocol2_packet_handler.h} (100%) rename dynamixel_sdk/include/dynamixel_sdk_linux/{PortHandlerLinux.h => port_handler_linux.h} (100%) create mode 100644 dynamixel_sdk/include/dynamixel_sdk_windows/port_handler_windows.h rename dynamixel_sdk/src/dynamixel_sdk/{GroupBulkRead.cpp => group_bulk_read.cpp} (100%) rename dynamixel_sdk/src/dynamixel_sdk/{GroupBulkWrite.cpp => group_bulk_write.cpp} (100%) rename dynamixel_sdk/src/dynamixel_sdk/{GroupSyncRead.cpp => group_sync_read.cpp} (100%) rename dynamixel_sdk/src/dynamixel_sdk/{GroupSyncWrite.cpp => group_sync_write.cpp} (100%) rename dynamixel_sdk/src/dynamixel_sdk/{PacketHandler.cpp => packet_handler.cpp} (100%) rename dynamixel_sdk/src/dynamixel_sdk/{PortHandler.cpp => port_handler.cpp} (100%) rename dynamixel_sdk/src/dynamixel_sdk/{Protocol1PacketHandler.cpp => protocol1_packet_handler.cpp} (100%) rename dynamixel_sdk/src/dynamixel_sdk/{Protocol2PacketHandler.cpp => protocol2_packet_handler.cpp} (100%) rename dynamixel_sdk/src/dynamixel_sdk_linux/{PortHandlerLinux.cpp => port_handler_linux.cpp} (100%) create mode 100644 dynamixel_sdk/src/dynamixel_sdk_windows/port_handler_windows.cpp diff --git a/dynamixel_sdk/CMakeLists.txt b/dynamixel_sdk/CMakeLists.txt index b3b5c84..658dfd4 100644 --- a/dynamixel_sdk/CMakeLists.txt +++ b/dynamixel_sdk/CMakeLists.txt @@ -20,15 +20,15 @@ include_directories( ## Declare a C++ library add_library(dynamixel_sdk - src/${PROJECT_NAME}/PacketHandler.cpp - src/${PROJECT_NAME}/Protocol1PacketHandler.cpp - src/${PROJECT_NAME}/Protocol2PacketHandler.cpp - src/${PROJECT_NAME}/GroupSyncRead.cpp - src/${PROJECT_NAME}/GroupSyncWrite.cpp - src/${PROJECT_NAME}/GroupBulkRead.cpp - src/${PROJECT_NAME}/GroupBulkWrite.cpp - src/${PROJECT_NAME}/PortHandler.cpp - src/${PROJECT_NAME}_linux/PortHandlerLinux.cpp + src/${PROJECT_NAME}/packet_handler.cpp + src/${PROJECT_NAME}/protocol1_packet_handler.cpp + src/${PROJECT_NAME}/protocol2_packet_handler.cpp + src/${PROJECT_NAME}/group_sync_read.cpp + src/${PROJECT_NAME}/group_sync_write.cpp + src/${PROJECT_NAME}/group_bulk_read.cpp + src/${PROJECT_NAME}/group_bulk_write.cpp + src/${PROJECT_NAME}/port_handler.cpp + src/${PROJECT_NAME}_linux/port_handler_linux.cpp ) ## Specify libraries to link a library or executable target against diff --git a/dynamixel_sdk/include/DynamixelSDK.h b/dynamixel_sdk/include/dynamixel_sdk.h similarity index 100% rename from dynamixel_sdk/include/DynamixelSDK.h rename to dynamixel_sdk/include/dynamixel_sdk.h diff --git a/dynamixel_sdk/include/dynamixel_sdk/RobotisDef.h b/dynamixel_sdk/include/dynamixel_sdk/RobotisDef.h deleted file mode 100644 index 95ab804..0000000 --- a/dynamixel_sdk/include/dynamixel_sdk/RobotisDef.h +++ /dev/null @@ -1,21 +0,0 @@ -/* - * RobotisDef.h - * - * Created on: 2016. 1. 27. - * Author: zerom, leon - */ - -#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_ROBOTISDEF_H_ -#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_ROBOTISDEF_H_ - - -typedef char INT8_T; -typedef short int INT16_T; -typedef int INT32_T; - -typedef unsigned char UINT8_T; -typedef unsigned short int UINT16_T; -typedef unsigned int UINT32_T; - - -#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_ROBOTISDEF_H_ */ diff --git a/dynamixel_sdk/include/dynamixel_sdk/GroupBulkRead.h b/dynamixel_sdk/include/dynamixel_sdk/group_bulk_read.h similarity index 100% rename from dynamixel_sdk/include/dynamixel_sdk/GroupBulkRead.h rename to dynamixel_sdk/include/dynamixel_sdk/group_bulk_read.h diff --git a/dynamixel_sdk/include/dynamixel_sdk/GroupBulkWrite.h b/dynamixel_sdk/include/dynamixel_sdk/group_bulk_write.h similarity index 100% rename from dynamixel_sdk/include/dynamixel_sdk/GroupBulkWrite.h rename to dynamixel_sdk/include/dynamixel_sdk/group_bulk_write.h diff --git a/dynamixel_sdk/include/dynamixel_sdk/GroupSyncRead.h b/dynamixel_sdk/include/dynamixel_sdk/group_sync_read.h similarity index 100% rename from dynamixel_sdk/include/dynamixel_sdk/GroupSyncRead.h rename to dynamixel_sdk/include/dynamixel_sdk/group_sync_read.h diff --git a/dynamixel_sdk/include/dynamixel_sdk/GroupSyncWrite.h b/dynamixel_sdk/include/dynamixel_sdk/group_sync_write.h similarity index 100% rename from dynamixel_sdk/include/dynamixel_sdk/GroupSyncWrite.h rename to dynamixel_sdk/include/dynamixel_sdk/group_sync_write.h diff --git a/dynamixel_sdk/include/dynamixel_sdk/PacketHandler.h b/dynamixel_sdk/include/dynamixel_sdk/packet_handler.h similarity index 100% rename from dynamixel_sdk/include/dynamixel_sdk/PacketHandler.h rename to dynamixel_sdk/include/dynamixel_sdk/packet_handler.h diff --git a/dynamixel_sdk/include/dynamixel_sdk/PortHandler.h b/dynamixel_sdk/include/dynamixel_sdk/port_handler.h similarity index 100% rename from dynamixel_sdk/include/dynamixel_sdk/PortHandler.h rename to dynamixel_sdk/include/dynamixel_sdk/port_handler.h diff --git a/dynamixel_sdk/include/dynamixel_sdk/Protocol1PacketHandler.h b/dynamixel_sdk/include/dynamixel_sdk/protocol1_packet_handler.h similarity index 100% rename from dynamixel_sdk/include/dynamixel_sdk/Protocol1PacketHandler.h rename to dynamixel_sdk/include/dynamixel_sdk/protocol1_packet_handler.h diff --git a/dynamixel_sdk/include/dynamixel_sdk/Protocol2PacketHandler.h b/dynamixel_sdk/include/dynamixel_sdk/protocol2_packet_handler.h similarity index 100% rename from dynamixel_sdk/include/dynamixel_sdk/Protocol2PacketHandler.h rename to dynamixel_sdk/include/dynamixel_sdk/protocol2_packet_handler.h diff --git a/dynamixel_sdk/include/dynamixel_sdk_linux/PortHandlerLinux.h b/dynamixel_sdk/include/dynamixel_sdk_linux/port_handler_linux.h similarity index 100% rename from dynamixel_sdk/include/dynamixel_sdk_linux/PortHandlerLinux.h rename to dynamixel_sdk/include/dynamixel_sdk_linux/port_handler_linux.h diff --git a/dynamixel_sdk/include/dynamixel_sdk_windows/port_handler_windows.h b/dynamixel_sdk/include/dynamixel_sdk_windows/port_handler_windows.h new file mode 100644 index 0000000..09583cb --- /dev/null +++ b/dynamixel_sdk/include/dynamixel_sdk_windows/port_handler_windows.h @@ -0,0 +1,93 @@ +/******************************************************************************* +* Copyright (c) 2016, ROBOTIS CO., LTD. +* 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 ROBOTIS 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. +*******************************************************************************/ + +/* Author: Leon Ryu Woon Jung */ + +/* +* port_handler_windows.h +* +* Created on: 2016. 4. 26. +*/ + +#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_WINDOWS_PORTHANDLERWINDOWS_H_ +#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_WINDOWS_PORTHANDLERWINDOWS_H_ + +#include + +#include "dynamixel_sdk/port_handler.h" + +namespace dynamixel +{ +class WINDECLSPEC PortHandlerWindows : public PortHandler +{ + private: + HANDLE serial_handle_; + LARGE_INTEGER freq_, counter_; + + int baudrate_; + char port_name_[30]; + + double packet_start_time_; + double packet_timeout_; + double tx_time_per_byte_; + + bool setupPort(const int baudrate); + + double getCurrentTime(); + double getTimeSinceStart(); + + public: + PortHandlerWindows(const char *port_name); + virtual ~PortHandlerWindows() { closePort(); } + + bool openPort(); + void closePort(); + void clearPort(); + + void setPortName(const char *port_name); + char *getPortName(); + + bool setBaudRate(const int baudrate); + int getBaudRate(); + + int getBytesAvailable(); + + int readPort(uint8_t *packet, int length); + int writePort(uint8_t *packet, int length); + + void setPacketTimeout(uint16_t packet_length); + void setPacketTimeout(double msec); + bool isPacketTimeout(); +}; + +} + + +#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_LINUX_PORTHANDLERWINDOWS_H_ */ diff --git a/dynamixel_sdk/src/dynamixel_sdk/GroupBulkRead.cpp b/dynamixel_sdk/src/dynamixel_sdk/group_bulk_read.cpp similarity index 100% rename from dynamixel_sdk/src/dynamixel_sdk/GroupBulkRead.cpp rename to dynamixel_sdk/src/dynamixel_sdk/group_bulk_read.cpp diff --git a/dynamixel_sdk/src/dynamixel_sdk/GroupBulkWrite.cpp b/dynamixel_sdk/src/dynamixel_sdk/group_bulk_write.cpp similarity index 100% rename from dynamixel_sdk/src/dynamixel_sdk/GroupBulkWrite.cpp rename to dynamixel_sdk/src/dynamixel_sdk/group_bulk_write.cpp diff --git a/dynamixel_sdk/src/dynamixel_sdk/GroupSyncRead.cpp b/dynamixel_sdk/src/dynamixel_sdk/group_sync_read.cpp similarity index 100% rename from dynamixel_sdk/src/dynamixel_sdk/GroupSyncRead.cpp rename to dynamixel_sdk/src/dynamixel_sdk/group_sync_read.cpp diff --git a/dynamixel_sdk/src/dynamixel_sdk/GroupSyncWrite.cpp b/dynamixel_sdk/src/dynamixel_sdk/group_sync_write.cpp similarity index 100% rename from dynamixel_sdk/src/dynamixel_sdk/GroupSyncWrite.cpp rename to dynamixel_sdk/src/dynamixel_sdk/group_sync_write.cpp diff --git a/dynamixel_sdk/src/dynamixel_sdk/PacketHandler.cpp b/dynamixel_sdk/src/dynamixel_sdk/packet_handler.cpp similarity index 100% rename from dynamixel_sdk/src/dynamixel_sdk/PacketHandler.cpp rename to dynamixel_sdk/src/dynamixel_sdk/packet_handler.cpp diff --git a/dynamixel_sdk/src/dynamixel_sdk/PortHandler.cpp b/dynamixel_sdk/src/dynamixel_sdk/port_handler.cpp similarity index 100% rename from dynamixel_sdk/src/dynamixel_sdk/PortHandler.cpp rename to dynamixel_sdk/src/dynamixel_sdk/port_handler.cpp diff --git a/dynamixel_sdk/src/dynamixel_sdk/Protocol1PacketHandler.cpp b/dynamixel_sdk/src/dynamixel_sdk/protocol1_packet_handler.cpp similarity index 100% rename from dynamixel_sdk/src/dynamixel_sdk/Protocol1PacketHandler.cpp rename to dynamixel_sdk/src/dynamixel_sdk/protocol1_packet_handler.cpp diff --git a/dynamixel_sdk/src/dynamixel_sdk/Protocol2PacketHandler.cpp b/dynamixel_sdk/src/dynamixel_sdk/protocol2_packet_handler.cpp similarity index 100% rename from dynamixel_sdk/src/dynamixel_sdk/Protocol2PacketHandler.cpp rename to dynamixel_sdk/src/dynamixel_sdk/protocol2_packet_handler.cpp diff --git a/dynamixel_sdk/src/dynamixel_sdk_linux/PortHandlerLinux.cpp b/dynamixel_sdk/src/dynamixel_sdk_linux/port_handler_linux.cpp similarity index 100% rename from dynamixel_sdk/src/dynamixel_sdk_linux/PortHandlerLinux.cpp rename to dynamixel_sdk/src/dynamixel_sdk_linux/port_handler_linux.cpp diff --git a/dynamixel_sdk/src/dynamixel_sdk_windows/port_handler_windows.cpp b/dynamixel_sdk/src/dynamixel_sdk_windows/port_handler_windows.cpp new file mode 100644 index 0000000..c47e76f --- /dev/null +++ b/dynamixel_sdk/src/dynamixel_sdk_windows/port_handler_windows.cpp @@ -0,0 +1,245 @@ +/******************************************************************************* +* Copyright (c) 2016, ROBOTIS CO., LTD. +* 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 ROBOTIS 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. +*******************************************************************************/ + +/* Author: Leon Ryu Woon Jung */ + +/* +* PortHandlerWindows.cpp +* +* Created on: 2016. 4. 06. +*/ +#if defined(_WIN32) || defined(_WIN64) +#define WINDLLEXPORT +#endif + +#include "dynamixel_sdk_windows/port_handler_windows.h" + +#include +#include +#include + +#define LATENCY_TIMER 16 // msec (USB latency timer) + +using namespace dynamixel; + +PortHandlerWindows::PortHandlerWindows(const char *port_name) + : serial_handle_(INVALID_HANDLE_VALUE), + baudrate_(DEFAULT_BAUDRATE_), + packet_start_time_(0.0), + packet_timeout_(0.0), + tx_time_per_byte_(0.0) +{ + is_using_ = false; + + char buffer[15]; + sprintf_s(buffer, sizeof(buffer), "\\\\.\\", port_name); + setPortName(port_name); +} + +bool PortHandlerWindows::openPort() +{ + return setBaudRate(baudrate_); +} + +void PortHandlerWindows::closePort() +{ + if (serial_handle_ != INVALID_HANDLE_VALUE) + { + CloseHandle(serial_handle_); + serial_handle_ = INVALID_HANDLE_VALUE; + } +} + +void PortHandlerWindows::clearPort() +{ + PurgeComm(serial_handle_, PURGE_RXABORT | PURGE_RXCLEAR); +} + +void PortHandlerWindows::setPortName(const char *port_name) +{ + strcpy_s(port_name_, sizeof(port_name_), port_name); +} + +char *PortHandlerWindows::getPortName() +{ + return port_name_; +} + +bool PortHandlerWindows::setBaudRate(const int baudrate) +{ + closePort(); + + baudrate_ = baudrate; + return setupPort(baudrate); +} + +int PortHandlerWindows::getBaudRate() +{ + return baudrate_; +} + +int PortHandlerWindows::getBytesAvailable() +{ + DWORD retbyte = 2; + BOOL res = DeviceIoControl(serial_handle_, GENERIC_READ | GENERIC_WRITE, NULL, 0, 0, 0, &retbyte, (LPOVERLAPPED)NULL); + + printf("%d", (int)res); + return (int)retbyte; +} + +int PortHandlerWindows::readPort(uint8_t *packet, int length) +{ + DWORD dwRead = 0; + + if (ReadFile(serial_handle_, packet, (DWORD)length, &dwRead, NULL) == FALSE) + return -1; + + return (int)dwRead; +} + +int PortHandlerWindows::writePort(uint8_t *packet, int length) +{ + DWORD dwWrite = 0; + + if (WriteFile(serial_handle_, packet, (DWORD)length, &dwWrite, NULL) == FALSE) + return -1; + + return (int)dwWrite; +} + +void PortHandlerWindows::setPacketTimeout(uint16_t packet_length) +{ + packet_start_time_ = getCurrentTime(); + packet_timeout_ = (tx_time_per_byte_ * (double)packet_length) + (LATENCY_TIMER * 2.0) + 2.0; +} + +void PortHandlerWindows::setPacketTimeout(double msec) +{ + packet_start_time_ = getCurrentTime(); + packet_timeout_ = msec; +} + +bool PortHandlerWindows::isPacketTimeout() +{ + if (getTimeSinceStart() > packet_timeout_) + { + packet_timeout_ = 0; + return true; + } + return false; +} + +double PortHandlerWindows::getCurrentTime() +{ + QueryPerformanceCounter(&counter_); + QueryPerformanceFrequency(&freq_); + return (double)counter_.QuadPart / (double)freq_.QuadPart * 1000.0; +} + +double PortHandlerWindows::getTimeSinceStart() +{ + double time; + + time = getCurrentTime() - packet_start_time_; + if (time < 0.0) packet_start_time_ = getCurrentTime(); + + return time; +} + +bool PortHandlerWindows::setupPort(int baudrate) +{ + DCB dcb; + COMMTIMEOUTS timeouts; + DWORD dwError; + + closePort(); + + serial_handle_ = CreateFileA(port_name_, GENERIC_READ | GENERIC_WRITE, 0, NULL, OPEN_EXISTING, FILE_ATTRIBUTE_NORMAL, NULL); + if (serial_handle_ == INVALID_HANDLE_VALUE) + { + printf("[PortHandlerWindows::SetupPort] Error opening serial port!\n"); + return false; + } + + dcb.DCBlength = sizeof(DCB); + if (GetCommState(serial_handle_, &dcb) == FALSE) + goto DXL_HAL_OPEN_ERROR; + + // Set baudrate + dcb.BaudRate = (DWORD)baudrate; + dcb.ByteSize = 8; // Data bit = 8bit + dcb.Parity = NOPARITY; // No parity + dcb.StopBits = ONESTOPBIT; // Stop bit = 1 + dcb.fParity = NOPARITY; // No Parity check + dcb.fBinary = 1; // Binary mode + dcb.fNull = 0; // Get Null byte + dcb.fAbortOnError = 0; + dcb.fErrorChar = 0; + // Not using XOn/XOff + dcb.fOutX = 0; + dcb.fInX = 0; + // Not using H/W flow control + dcb.fDtrControl = DTR_CONTROL_DISABLE; + dcb.fRtsControl = RTS_CONTROL_DISABLE; + dcb.fDsrSensitivity = 0; + dcb.fOutxDsrFlow = 0; + dcb.fOutxCtsFlow = 0; + + if (SetCommState(serial_handle_, &dcb) == FALSE) + goto DXL_HAL_OPEN_ERROR; + + if (SetCommMask(serial_handle_, 0) == FALSE) // Not using Comm event + goto DXL_HAL_OPEN_ERROR; + if (SetupComm(serial_handle_, 4096, 4096) == FALSE) // Buffer size (Rx,Tx) + goto DXL_HAL_OPEN_ERROR; + if (PurgeComm(serial_handle_, PURGE_TXABORT | PURGE_TXCLEAR | PURGE_RXABORT | PURGE_RXCLEAR) == FALSE) // Clear buffer + goto DXL_HAL_OPEN_ERROR; + if (ClearCommError(serial_handle_, &dwError, NULL) == FALSE) + goto DXL_HAL_OPEN_ERROR; + + if (GetCommTimeouts(serial_handle_, &timeouts) == FALSE) + goto DXL_HAL_OPEN_ERROR; + // Timeout (Not using timeout) + // Immediatly return + timeouts.ReadIntervalTimeout = 0; + timeouts.ReadTotalTimeoutMultiplier = 0; + timeouts.ReadTotalTimeoutConstant = 1; // must not be zero. + timeouts.WriteTotalTimeoutMultiplier = 0; + timeouts.WriteTotalTimeoutConstant = 0; + if (SetCommTimeouts(serial_handle_, &timeouts) == FALSE) + goto DXL_HAL_OPEN_ERROR; + + tx_time_per_byte_ = (1000.0 / (double)baudrate_) * 10.0; + return true; + +DXL_HAL_OPEN_ERROR: + closePort(); + return false; +} From 1ed888af00c114f359628bf8665cd03bbb571f9c Mon Sep 17 00:00:00 2001 From: ROBOTIS-zerom Date: Fri, 3 Jun 2016 20:05:21 +0900 Subject: [PATCH 030/238] - ROS C++ Coding Style is applied. --- dynamixel_sdk/include/dynamixel_sdk.h | 52 +- .../include/dynamixel_sdk/group_bulk_read.h | 89 +- .../include/dynamixel_sdk/group_bulk_write.h | 84 +- .../include/dynamixel_sdk/group_sync_read.h | 90 +- .../include/dynamixel_sdk/group_sync_write.h | 82 +- .../include/dynamixel_sdk/packet_handler.h | 136 +- .../include/dynamixel_sdk/port_handler.h | 78 +- .../dynamixel_sdk/protocol1_packet_handler.h | 141 +- .../dynamixel_sdk/protocol2_packet_handler.h | 149 +- .../dynamixel_sdk_linux/port_handler_linux.h | 95 +- .../src/dynamixel_sdk/group_bulk_read.cpp | 338 +- .../src/dynamixel_sdk/group_bulk_write.cpp | 213 +- .../src/dynamixel_sdk/group_sync_read.cpp | 233 +- .../src/dynamixel_sdk/group_sync_write.cpp | 174 +- .../src/dynamixel_sdk/packet_handler.cpp | 59 +- .../src/dynamixel_sdk/port_handler.cpp | 49 +- .../protocol1_packet_handler.cpp | 1127 +++--- .../protocol2_packet_handler.cpp | 1525 ++++---- .../port_handler_linux.cpp | 341 +- robotis_controller/CMakeLists.txt | 20 +- .../robotis_controller/robotis_controller.h | 201 +- robotis_controller/package.xml | 4 - .../robotis_controller/robotis_controller.cpp | 3252 +++++++++-------- robotis_controller_msgs/package.xml | 8 +- robotis_device/CMakeLists.txt | 34 +- .../robotis_device/control_table_item.h | 90 +- .../include/robotis_device/device.h | 57 +- .../include/robotis_device/dynamixel.h | 95 +- .../include/robotis_device/dynamixel_state.h | 89 +- robotis_device/include/robotis_device/robot.h | 70 +- .../include/robotis_device/sensor.h | 51 +- .../include/robotis_device/sensor_state.h | 57 +- .../include/robotis_device/time_stamp.h | 50 +- robotis_device/package.xml | 9 - .../src/robotis_device/dynamixel.cpp | 186 +- robotis_device/src/robotis_device/robot.cpp | 728 ++-- robotis_device/src/robotis_device/sensor.cpp | 50 +- robotis_framework_common/CMakeLists.txt | 38 - .../robotis_framework_common/motion_module.h | 104 +- .../robotis_framework_common/sensor_module.h | 58 +- .../robotis_framework_common/singleton.h | 72 +- robotis_framework_common/package.xml | 45 +- robotis_math/CMakeLists.txt | 6 +- .../robotis_math/robotis_linear_algebra.h | 2 +- .../include/robotis_math/robotis_math.h | 2 +- .../include/robotis_math/robotis_math_base.h | 2 +- .../robotis_trajectory_calculator.h | 6 +- robotis_math/src/robotis_linear_algebra.cpp | 4 +- robotis_math/src/robotis_math_base.cpp | 4 +- .../src/robotis_trajectory_calculator.cpp | 4 +- 50 files changed, 5772 insertions(+), 4681 deletions(-) diff --git a/dynamixel_sdk/include/dynamixel_sdk.h b/dynamixel_sdk/include/dynamixel_sdk.h index 0d34655..a0fdf2a 100644 --- a/dynamixel_sdk/include/dynamixel_sdk.h +++ b/dynamixel_sdk/include/dynamixel_sdk.h @@ -1,28 +1,58 @@ +/******************************************************************************* +* Copyright (c) 2016, ROBOTIS CO., LTD. +* 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 ROBOTIS 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. +*******************************************************************************/ + +/* Author: zerom, Leon Ryu Woon Jung */ + /* - * DynamixelSDK.h + * dynamixel_sdk.h * * Created on: 2016. 3. 8. - * Author: zerom, leon */ #ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_DYNAMIXELSDK_H_ #define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_DYNAMIXELSDK_H_ -#include "dynamixel_sdk/RobotisDef.h" -#include "dynamixel_sdk/GroupBulkRead.h" -#include "dynamixel_sdk/GroupBulkWrite.h" -#include "dynamixel_sdk/GroupSyncRead.h" -#include "dynamixel_sdk/GroupSyncWrite.h" -#include "dynamixel_sdk/Protocol1PacketHandler.h" -#include "dynamixel_sdk/Protocol2PacketHandler.h" +#include "dynamixel_sdk/group_bulk_read.h" +#include "dynamixel_sdk/group_bulk_write.h" +#include "dynamixel_sdk/group_sync_read.h" +#include "dynamixel_sdk/group_sync_write.h" +#include "dynamixel_sdk/protocol1_packet_handler.h" +#include "dynamixel_sdk/protocol2_packet_handler.h" #ifdef __linux__ - #include "dynamixel_sdk_linux/PortHandlerLinux.h" + #include "dynamixel_sdk_linux/port_handler_linux.h" #endif #if defined(_WIN32) || defined(_WIN64) - #include "dynamixel_sdk_windows/PortHandlerWindows.h" + #include "dynamixel_sdk_windows/port_handler_windows.h" #endif diff --git a/dynamixel_sdk/include/dynamixel_sdk/group_bulk_read.h b/dynamixel_sdk/include/dynamixel_sdk/group_bulk_read.h index bd3374b..29b0dba 100644 --- a/dynamixel_sdk/include/dynamixel_sdk/group_bulk_read.h +++ b/dynamixel_sdk/include/dynamixel_sdk/group_bulk_read.h @@ -1,5 +1,37 @@ +/******************************************************************************* +* Copyright (c) 2016, ROBOTIS CO., LTD. +* 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 ROBOTIS 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. +*******************************************************************************/ + +/* Author: zerom, Leon Ryu Woon Jung */ + /* - * GroupBulkRead.h + * group_bulk_read.h * * Created on: 2016. 1. 28. * Author: zerom, leon @@ -11,48 +43,47 @@ #include #include -#include "RobotisDef.h" -#include "PortHandler.h" -#include "PacketHandler.h" +#include "port_handler.h" +#include "packet_handler.h" -namespace ROBOTIS +namespace dynamixel { class WINDECLSPEC GroupBulkRead { -private: - PortHandler *port_; - PacketHandler *ph_; + private: + PortHandler *port_; + PacketHandler *ph_; - std::vector id_list_; - std::map address_list_; // - std::map length_list_; // - std::map data_list_; // + std::vector id_list_; + std::map address_list_; // + std::map length_list_; // + std::map data_list_; // - bool last_result_; - bool is_param_changed_; + bool last_result_; + bool is_param_changed_; - UINT8_T *param_; + uint8_t *param_; - void MakeParam(); + void makeParam(); -public: - GroupBulkRead(PortHandler *port, PacketHandler *ph); - ~GroupBulkRead() { ClearParam(); } + public: + GroupBulkRead(PortHandler *port, PacketHandler *ph); + ~GroupBulkRead() { clearParam(); } - PortHandler *GetPortHandler() { return port_; } - PacketHandler *GetPacketHandler() { return ph_; } + PortHandler *getPortHandler() { return port_; } + PacketHandler *getPacketHandler() { return ph_; } - bool AddParam (UINT8_T id, UINT16_T start_address, UINT16_T data_length); - void RemoveParam (UINT8_T id); - void ClearParam (); + bool addParam (uint8_t id, uint16_t start_address, uint16_t data_length); + void removeParam (uint8_t id); + void clearParam (); - int TxPacket(); - int RxPacket(); - int TxRxPacket(); + int txPacket(); + int rxPacket(); + int txRxPacket(); - bool IsAvailable (UINT8_T id, UINT16_T address, UINT16_T data_length); - UINT32_T GetData (UINT8_T id, UINT16_T address, UINT16_T data_length); + bool isAvailable (uint8_t id, uint16_t address, uint16_t data_length); + uint32_t getData (uint8_t id, uint16_t address, uint16_t data_length); }; } diff --git a/dynamixel_sdk/include/dynamixel_sdk/group_bulk_write.h b/dynamixel_sdk/include/dynamixel_sdk/group_bulk_write.h index 3efbb80..71bd9ec 100644 --- a/dynamixel_sdk/include/dynamixel_sdk/group_bulk_write.h +++ b/dynamixel_sdk/include/dynamixel_sdk/group_bulk_write.h @@ -1,8 +1,39 @@ +/******************************************************************************* +* Copyright (c) 2016, ROBOTIS CO., LTD. +* 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 ROBOTIS 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. +*******************************************************************************/ + +/* Author: zerom, Leon Ryu Woon Jung */ + /* - * GroupBulkWrite.h + * group_bulk_write.h * * Created on: 2016. 2. 2. - * Author: zerom, leon */ #ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPBULKWRITE_H_ @@ -11,44 +42,43 @@ #include #include -#include "RobotisDef.h" -#include "PortHandler.h" -#include "PacketHandler.h" +#include "port_handler.h" +#include "packet_handler.h" -namespace ROBOTIS +namespace dynamixel { class WINDECLSPEC GroupBulkWrite { -private: - PortHandler *port_; - PacketHandler *ph_; + private: + PortHandler *port_; + PacketHandler *ph_; - std::vector id_list_; - std::map address_list_; // - std::map length_list_; // - std::map data_list_; // + std::vector id_list_; + std::map address_list_; // + std::map length_list_; // + std::map data_list_; // - bool is_param_changed_; + bool is_param_changed_; - UINT8_T *param_; - UINT16_T param_length_; + uint8_t *param_; + uint16_t param_length_; - void MakeParam(); + void makeParam(); -public: - GroupBulkWrite(PortHandler *port, PacketHandler *ph); - ~GroupBulkWrite() { ClearParam(); } + public: + GroupBulkWrite(PortHandler *port, PacketHandler *ph); + ~GroupBulkWrite() { clearParam(); } - PortHandler *GetPortHandler() { return port_; } - PacketHandler *GetPacketHandler() { return ph_; } + PortHandler *getPortHandler() { return port_; } + PacketHandler *getPacketHandler() { return ph_; } - bool AddParam (UINT8_T id, UINT16_T start_address, UINT16_T data_length, UINT8_T *data); - void RemoveParam (UINT8_T id); - bool ChangeParam (UINT8_T id, UINT16_T start_address, UINT16_T data_length, UINT8_T *data); - void ClearParam (); + bool addParam (uint8_t id, uint16_t start_address, uint16_t data_length, uint8_t *data); + void removeParam (uint8_t id); + bool changeParam (uint8_t id, uint16_t start_address, uint16_t data_length, uint8_t *data); + void clearParam (); - int TxPacket(); + int txPacket(); }; } diff --git a/dynamixel_sdk/include/dynamixel_sdk/group_sync_read.h b/dynamixel_sdk/include/dynamixel_sdk/group_sync_read.h index b6599de..ec2a208 100644 --- a/dynamixel_sdk/include/dynamixel_sdk/group_sync_read.h +++ b/dynamixel_sdk/include/dynamixel_sdk/group_sync_read.h @@ -1,8 +1,39 @@ +/******************************************************************************* +* Copyright (c) 2016, ROBOTIS CO., LTD. +* 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 ROBOTIS 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. +*******************************************************************************/ + +/* Author: zerom, Leon Ryu Woon Jung */ + /* - * GroupSyncRead.h + * group_sync_read.h * * Created on: 2016. 2. 2. - * Author: zerom, leon */ #ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPSYNCREAD_H_ @@ -11,48 +42,47 @@ #include #include -#include "RobotisDef.h" -#include "PortHandler.h" -#include "PacketHandler.h" +#include "port_handler.h" +#include "packet_handler.h" -namespace ROBOTIS +namespace dynamixel { class WINDECLSPEC GroupSyncRead { -private: - PortHandler *port_; - PacketHandler *ph_; + private: + PortHandler *port_; + PacketHandler *ph_; - std::vector id_list_; - std::map data_list_; // + std::vector id_list_; + std::map data_list_; // - bool last_result_; - bool is_param_changed_; + bool last_result_; + bool is_param_changed_; - UINT8_T *param_; - UINT16_T start_address_; - UINT16_T data_length_; + uint8_t *param_; + uint16_t start_address_; + uint16_t data_length_; - void MakeParam(); + void makeParam(); -public: - GroupSyncRead(PortHandler *port, PacketHandler *ph, UINT16_T start_address, UINT16_T data_length); - ~GroupSyncRead() { ClearParam(); } + public: + GroupSyncRead(PortHandler *port, PacketHandler *ph, uint16_t start_address, uint16_t data_length); + ~GroupSyncRead() { clearParam(); } - PortHandler *GetPortHandler() { return port_; } - PacketHandler *GetPacketHandler() { return ph_; } + PortHandler *getPortHandler() { return port_; } + PacketHandler *getPacketHandler() { return ph_; } - bool AddParam (UINT8_T id); - void RemoveParam (UINT8_T id); - void ClearParam (); + bool addParam (uint8_t id); + void removeParam (uint8_t id); + void clearParam (); - int TxPacket(); - int RxPacket(); - int TxRxPacket(); + int txPacket(); + int rxPacket(); + int txRxPacket(); - bool IsAvailable (UINT8_T id, UINT16_T address, UINT16_T data_length); - UINT32_T GetData (UINT8_T id, UINT16_T address, UINT16_T data_length); + bool isAvailable (uint8_t id, uint16_t address, uint16_t data_length); + uint32_t getData (uint8_t id, uint16_t address, uint16_t data_length); }; } diff --git a/dynamixel_sdk/include/dynamixel_sdk/group_sync_write.h b/dynamixel_sdk/include/dynamixel_sdk/group_sync_write.h index 1ff03b2..1c85666 100644 --- a/dynamixel_sdk/include/dynamixel_sdk/group_sync_write.h +++ b/dynamixel_sdk/include/dynamixel_sdk/group_sync_write.h @@ -1,8 +1,39 @@ +/******************************************************************************* +* Copyright (c) 2016, ROBOTIS CO., LTD. +* 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 ROBOTIS 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. +*******************************************************************************/ + +/* Author: zerom, Leon Ryu Woon Jung */ + /* - * GroupSyncWrite.h + * group_sync_write.h * * Created on: 2016. 1. 28. - * Author: zerom, leon */ #ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPSYNCWRITE_H_ @@ -11,43 +42,42 @@ #include #include -#include "RobotisDef.h" -#include "PortHandler.h" -#include "PacketHandler.h" +#include "port_handler.h" +#include "packet_handler.h" -namespace ROBOTIS +namespace dynamixel { class WINDECLSPEC GroupSyncWrite { -private: - PortHandler *port_; - PacketHandler *ph_; + private: + PortHandler *port_; + PacketHandler *ph_; - std::vector id_list_; - std::map data_list_; // + std::vector id_list_; + std::map data_list_; // - bool is_param_changed_; + bool is_param_changed_; - UINT8_T *param_; - UINT16_T start_address_; - UINT16_T data_length_; + uint8_t *param_; + uint16_t start_address_; + uint16_t data_length_; - void MakeParam(); + void makeParam(); -public: - GroupSyncWrite(PortHandler *port, PacketHandler *ph, UINT16_T start_address, UINT16_T data_length); - ~GroupSyncWrite() { ClearParam(); } + public: + GroupSyncWrite(PortHandler *port, PacketHandler *ph, uint16_t start_address, uint16_t data_length); + ~GroupSyncWrite() { clearParam(); } - PortHandler *GetPortHandler() { return port_; } - PacketHandler *GetPacketHandler() { return ph_; } + PortHandler *getPortHandler() { return port_; } + PacketHandler *getPacketHandler() { return ph_; } - bool AddParam (UINT8_T id, UINT8_T *data); - void RemoveParam (UINT8_T id); - bool ChangeParam (UINT8_T id, UINT8_T *data); - void ClearParam (); + bool addParam (uint8_t id, uint8_t *data); + void removeParam (uint8_t id); + bool changeParam (uint8_t id, uint8_t *data); + void clearParam (); - int TxPacket(); + int txPacket(); }; } diff --git a/dynamixel_sdk/include/dynamixel_sdk/packet_handler.h b/dynamixel_sdk/include/dynamixel_sdk/packet_handler.h index 921d380..3816cbb 100644 --- a/dynamixel_sdk/include/dynamixel_sdk/packet_handler.h +++ b/dynamixel_sdk/include/dynamixel_sdk/packet_handler.h @@ -1,8 +1,39 @@ +/******************************************************************************* +* Copyright (c) 2016, ROBOTIS CO., LTD. +* 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 ROBOTIS 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. +*******************************************************************************/ + +/* Author: zerom, Leon Ryu Woon Jung */ + /* - * PacketHandler.h + * packet_handler.h * * Created on: 2016. 1. 26. - * Author: zerom, leon */ #ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PACKETHANDLER_H_ @@ -11,8 +42,7 @@ #include #include -#include "RobotisDef.h" -#include "PortHandler.h" +#include "port_handler.h" #define BROADCAST_ID 0xFE // 254 #define MAX_ID 0xFC // 252 @@ -51,81 +81,81 @@ #define COMM_RX_CORRUPT -3002 // Incorrect status packet #define COMM_NOT_AVAILABLE -9000 // -namespace ROBOTIS +namespace dynamixel { class WINDECLSPEC PacketHandler { -protected: - PacketHandler() { } + protected: + PacketHandler() { } -public: - static PacketHandler *GetPacketHandler(float protocol_version = 2.0); + public: + static PacketHandler *getPacketHandler(float protocol_version = 2.0); - virtual ~PacketHandler() { } + virtual ~PacketHandler() { } - virtual float GetProtocolVersion() = 0; + virtual float getProtocolVersion() = 0; - virtual void PrintTxRxResult(int result) = 0; - virtual void PrintRxPacketError(UINT8_T error) = 0; + virtual void printTxRxResult(int result) = 0; + virtual void printRxPacketError(uint8_t error) = 0; - virtual int TxPacket (PortHandler *port, UINT8_T *txpacket) = 0; - virtual int RxPacket (PortHandler *port, UINT8_T *rxpacket) = 0; - virtual int TxRxPacket (PortHandler *port, UINT8_T *txpacket, UINT8_T *rxpacket, UINT8_T *error = 0) = 0; + virtual int txPacket (PortHandler *port, uint8_t *txpacket) = 0; + virtual int rxPacket (PortHandler *port, uint8_t *rxpacket) = 0; + virtual int txRxPacket (PortHandler *port, uint8_t *txpacket, uint8_t *rxpacket, uint8_t *error = 0) = 0; - virtual int Ping (PortHandler *port, UINT8_T id, UINT8_T *error = 0) = 0; - virtual int Ping (PortHandler *port, UINT8_T id, UINT16_T *model_number, UINT8_T *error = 0) = 0; + virtual int ping (PortHandler *port, uint8_t id, uint8_t *error = 0) = 0; + virtual int ping (PortHandler *port, uint8_t id, uint16_t *model_number, uint8_t *error = 0) = 0; - // BroadcastPing - virtual int BroadcastPing (PortHandler *port, std::vector &id_list) = 0; + // broadcastPing + virtual int broadcastPing (PortHandler *port, std::vector &id_list) = 0; - virtual int Action (PortHandler *port, UINT8_T id) = 0; - virtual int Reboot (PortHandler *port, UINT8_T id, UINT8_T *error = 0) = 0; - virtual int FactoryReset (PortHandler *port, UINT8_T id, UINT8_T option = 0, UINT8_T *error = 0) = 0; + virtual int action (PortHandler *port, uint8_t id) = 0; + virtual int reboot (PortHandler *port, uint8_t id, uint8_t *error = 0) = 0; + virtual int factoryReset (PortHandler *port, uint8_t id, uint8_t option = 0, uint8_t *error = 0) = 0; - virtual int ReadTx (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length) = 0; - virtual int ReadRx (PortHandler *port, UINT16_T length, UINT8_T *data, UINT8_T *error = 0) = 0; - virtual int ReadTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error = 0) = 0; + virtual int readTx (PortHandler *port, uint8_t id, uint16_t address, uint16_t length) = 0; + virtual int readRx (PortHandler *port, uint16_t length, uint8_t *data, uint8_t *error = 0) = 0; + virtual int readTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0) = 0; - virtual int Read1ByteTx (PortHandler *port, UINT8_T id, UINT16_T address) = 0; - virtual int Read1ByteRx (PortHandler *port, UINT8_T *data, UINT8_T *error = 0) = 0; - virtual int Read1ByteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T *data, UINT8_T *error = 0) = 0; + virtual int read1ByteTx (PortHandler *port, uint8_t id, uint16_t address) = 0; + virtual int read1ByteRx (PortHandler *port, uint8_t *data, uint8_t *error = 0) = 0; + virtual int read1ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint8_t *data, uint8_t *error = 0) = 0; - virtual int Read2ByteTx (PortHandler *port, UINT8_T id, UINT16_T address) = 0; - virtual int Read2ByteRx (PortHandler *port, UINT16_T *data, UINT8_T *error = 0) = 0; - virtual int Read2ByteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T *data, UINT8_T *error = 0) = 0; + virtual int read2ByteTx (PortHandler *port, uint8_t id, uint16_t address) = 0; + virtual int read2ByteRx (PortHandler *port, uint16_t *data, uint8_t *error = 0) = 0; + virtual int read2ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t *data, uint8_t *error = 0) = 0; - virtual int Read4ByteTx (PortHandler *port, UINT8_T id, UINT16_T address) = 0; - virtual int Read4ByteRx (PortHandler *port, UINT32_T *data, UINT8_T *error = 0) = 0; - virtual int Read4ByteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T *data, UINT8_T *error = 0) = 0; + virtual int read4ByteTx (PortHandler *port, uint8_t id, uint16_t address) = 0; + virtual int read4ByteRx (PortHandler *port, uint32_t *data, uint8_t *error = 0) = 0; + virtual int read4ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint32_t *data, uint8_t *error = 0) = 0; - virtual int WriteTxOnly (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data) = 0; - virtual int WriteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error = 0) = 0; + virtual int writeTxOnly (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data) = 0; + virtual int writeTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0) = 0; - virtual int Write1ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T data) = 0; - virtual int Write1ByteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T data, UINT8_T *error = 0) = 0; + virtual int write1ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint8_t data) = 0; + virtual int write1ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint8_t data, uint8_t *error = 0) = 0; - virtual int Write2ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T data) = 0; - virtual int Write2ByteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T data, UINT8_T *error = 0) = 0; + virtual int write2ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint16_t data) = 0; + virtual int write2ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t data, uint8_t *error = 0) = 0; - virtual int Write4ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T data) = 0; - virtual int Write4ByteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T data, UINT8_T *error = 0) = 0; + virtual int write4ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint32_t data) = 0; + virtual int write4ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint32_t data, uint8_t *error = 0) = 0; - virtual int RegWriteTxOnly (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data) = 0; - virtual int RegWriteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error = 0) = 0; + virtual int regWriteTxOnly (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data) = 0; + virtual int regWriteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0) = 0; - virtual int SyncReadTx (PortHandler *port, UINT16_T start_address, UINT16_T data_length, UINT8_T *param, UINT16_T param_length) = 0; - // SyncReadRx -> GroupSyncRead class - // SyncReadTxRx -> GroupSyncRead class + virtual int syncReadTx (PortHandler *port, uint16_t start_address, uint16_t data_length, uint8_t *param, uint16_t param_length) = 0; + // SyncReadRx -> GroupSyncRead class + // SyncReadTxRx -> GroupSyncRead class - virtual int SyncWriteTxOnly (PortHandler *port, UINT16_T start_address, UINT16_T data_length, UINT8_T *param, UINT16_T param_length) = 0; + virtual int syncWriteTxOnly (PortHandler *port, uint16_t start_address, uint16_t data_length, uint8_t *param, uint16_t param_length) = 0; - virtual int BulkReadTx (PortHandler *port, UINT8_T *param, UINT16_T param_length) = 0; - // BulkReadRx -> GroupBulkRead class - // BulkReadTxRx -> GroupBulkRead class + virtual int bulkReadTx (PortHandler *port, uint8_t *param, uint16_t param_length) = 0; + // BulkReadRx -> GroupBulkRead class + // BulkReadTxRx -> GroupBulkRead class - virtual int BulkWriteTxOnly (PortHandler *port, UINT8_T *param, UINT16_T param_length) = 0; + virtual int bulkWriteTxOnly (PortHandler *port, uint8_t *param, uint16_t param_length) = 0; }; } diff --git a/dynamixel_sdk/include/dynamixel_sdk/port_handler.h b/dynamixel_sdk/include/dynamixel_sdk/port_handler.h index 6d3e6dd..dfc83fd 100644 --- a/dynamixel_sdk/include/dynamixel_sdk/port_handler.h +++ b/dynamixel_sdk/include/dynamixel_sdk/port_handler.h @@ -1,13 +1,47 @@ +/******************************************************************************* +* Copyright (c) 2016, ROBOTIS CO., LTD. +* 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 ROBOTIS 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. +*******************************************************************************/ + +/* Author: zerom, Leon Ryu Woon Jung */ + /* - * PortHandler.h + * port_handler.h * * Created on: 2016. 1. 26. - * Author: zerom, leon */ #ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PORTHANDLER_H_ #define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PORTHANDLER_H_ +#undef __linux__ +#define __linux__ + #ifdef __linux__ #define WINDECLSPEC #elif defined(_WIN32) || defined(_WIN64) @@ -18,40 +52,40 @@ #endif #endif -#include "RobotisDef.h" +#include -namespace ROBOTIS +namespace dynamixel { class WINDECLSPEC PortHandler { -public: - static const int DEFAULT_BAUDRATE = 1000000; + public: + static const int DEFAULT_BAUDRATE_ = 1000000; - static PortHandler *GetPortHandler(const char *port_name); + static PortHandler *getPortHandler(const char *port_name); - bool is_using; + bool is_using_; - virtual ~PortHandler() { } + virtual ~PortHandler() { } - virtual bool OpenPort() = 0; - virtual void ClosePort() = 0; - virtual void ClearPort() = 0; + virtual bool openPort() = 0; + virtual void closePort() = 0; + virtual void clearPort() = 0; - virtual void SetPortName(const char* port_name) = 0; - virtual char *GetPortName() = 0; + virtual void setPortName(const char* port_name) = 0; + virtual char *getPortName() = 0; - virtual bool SetBaudRate(const int baudrate) = 0; - virtual int GetBaudRate() = 0; + virtual bool setBaudRate(const int baudrate) = 0; + virtual int getBaudRate() = 0; - virtual int GetBytesAvailable() = 0; + virtual int getBytesAvailable() = 0; - virtual int ReadPort(UINT8_T *packet, int length) = 0; - virtual int WritePort(UINT8_T *packet, int length) = 0; + virtual int readPort(uint8_t *packet, int length) = 0; + virtual int writePort(uint8_t *packet, int length) = 0; - virtual void SetPacketTimeout(UINT16_T packet_length) = 0; - virtual void SetPacketTimeout(double msec) = 0; - virtual bool IsPacketTimeout() = 0; + virtual void setPacketTimeout(uint16_t packet_length) = 0; + virtual void setPacketTimeout(double msec) = 0; + virtual bool isPacketTimeout() = 0; }; } diff --git a/dynamixel_sdk/include/dynamixel_sdk/protocol1_packet_handler.h b/dynamixel_sdk/include/dynamixel_sdk/protocol1_packet_handler.h index 1932d27..758556f 100644 --- a/dynamixel_sdk/include/dynamixel_sdk/protocol1_packet_handler.h +++ b/dynamixel_sdk/include/dynamixel_sdk/protocol1_packet_handler.h @@ -1,95 +1,126 @@ +/******************************************************************************* +* Copyright (c) 2016, ROBOTIS CO., LTD. +* 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 ROBOTIS 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. +*******************************************************************************/ + +/* Author: zerom, Leon Ryu Woon Jung */ + /* - * Protocol1PacketHandler.h + * protocol1_packet_handler.h * * Created on: 2016. 1. 26. - * Author: zerom, leon */ #ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL1PACKETHANDLER_H_ #define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL1PACKETHANDLER_H_ -#include "PacketHandler.h" +#include "packet_handler.h" -namespace ROBOTIS +namespace dynamixel { class WINDECLSPEC Protocol1PacketHandler : public PacketHandler { -private: - static Protocol1PacketHandler *unique_instance_; + private: + static Protocol1PacketHandler *unique_instance_; - Protocol1PacketHandler(); + Protocol1PacketHandler(); -public: - static Protocol1PacketHandler *GetInstance() { return unique_instance_; } + public: + static Protocol1PacketHandler *getInstance() { return unique_instance_; } - virtual ~Protocol1PacketHandler() { } + virtual ~Protocol1PacketHandler() { } - float GetProtocolVersion() { return 1.0; } + float getProtocolVersion() { return 1.0; } - void PrintTxRxResult(int result); - void PrintRxPacketError(UINT8_T error); + void printTxRxResult(int result); + void printRxPacketError(uint8_t error); - int TxPacket (PortHandler *port, UINT8_T *txpacket); - int RxPacket (PortHandler *port, UINT8_T *rxpacket); - int TxRxPacket (PortHandler *port, UINT8_T *txpacket, UINT8_T *rxpacket, UINT8_T *error = 0); + int txPacket (PortHandler *port, uint8_t *txpacket); + int rxPacket (PortHandler *port, uint8_t *rxpacket); + int txRxPacket (PortHandler *port, uint8_t *txpacket, uint8_t *rxpacket, uint8_t *error = 0); - int Ping (PortHandler *port, UINT8_T id, UINT8_T *error = 0); - int Ping (PortHandler *port, UINT8_T id, UINT16_T *model_number, UINT8_T *error = 0); + int ping (PortHandler *port, uint8_t id, uint8_t *error = 0); + int ping (PortHandler *port, uint8_t id, uint16_t *model_number, uint8_t *error = 0); - // BroadcastPing - int BroadcastPing (PortHandler *port, std::vector &id_list); + // broadcastPing + int broadcastPing (PortHandler *port, std::vector &id_list); - int Action (PortHandler *port, UINT8_T id); - int Reboot (PortHandler *port, UINT8_T id, UINT8_T *error = 0); - int FactoryReset (PortHandler *port, UINT8_T id, UINT8_T option, UINT8_T *error = 0); + int action (PortHandler *port, uint8_t id); + int reboot (PortHandler *port, uint8_t id, uint8_t *error = 0); + int factoryReset (PortHandler *port, uint8_t id, uint8_t option, uint8_t *error = 0); - int ReadTx (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length); - int ReadRx (PortHandler *port, UINT16_T length, UINT8_T *data, UINT8_T *error = 0); - int ReadTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error = 0); + int readTx (PortHandler *port, uint8_t id, uint16_t address, uint16_t length); + int readRx (PortHandler *port, uint16_t length, uint8_t *data, uint8_t *error = 0); + int readTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0); - int Read1ByteTx (PortHandler *port, UINT8_T id, UINT16_T address); - int Read1ByteRx (PortHandler *port, UINT8_T *data, UINT8_T *error = 0); - int Read1ByteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T *data, UINT8_T *error = 0); + int read1ByteTx (PortHandler *port, uint8_t id, uint16_t address); + int read1ByteRx (PortHandler *port, uint8_t *data, uint8_t *error = 0); + int read1ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint8_t *data, uint8_t *error = 0); - int Read2ByteTx (PortHandler *port, UINT8_T id, UINT16_T address); - int Read2ByteRx (PortHandler *port, UINT16_T *data, UINT8_T *error = 0); - int Read2ByteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T *data, UINT8_T *error = 0); + int read2ByteTx (PortHandler *port, uint8_t id, uint16_t address); + int read2ByteRx (PortHandler *port, uint16_t *data, uint8_t *error = 0); + int read2ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t *data, uint8_t *error = 0); - int Read4ByteTx (PortHandler *port, UINT8_T id, UINT16_T address); - int Read4ByteRx (PortHandler *port, UINT32_T *data, UINT8_T *error = 0); - int Read4ByteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T *data, UINT8_T *error = 0); + int read4ByteTx (PortHandler *port, uint8_t id, uint16_t address); + int read4ByteRx (PortHandler *port, uint32_t *data, uint8_t *error = 0); + int read4ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint32_t *data, uint8_t *error = 0); - int WriteTxOnly (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data); - int WriteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error = 0); + int writeTxOnly (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data); + int writeTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0); - int Write1ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T data); - int Write1ByteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T data, UINT8_T *error = 0); + int write1ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint8_t data); + int write1ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint8_t data, uint8_t *error = 0); - int Write2ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T data); - int Write2ByteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T data, UINT8_T *error = 0); + int write2ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint16_t data); + int write2ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t data, uint8_t *error = 0); - int Write4ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T data); - int Write4ByteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T data, UINT8_T *error = 0); + int write4ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint32_t data); + int write4ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint32_t data, uint8_t *error = 0); - int RegWriteTxOnly (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data); - int RegWriteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error = 0); + int regWriteTxOnly (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data); + int regWriteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0); - int SyncReadTx (PortHandler *port, UINT16_T start_address, UINT16_T data_length, UINT8_T *param, UINT16_T param_length); - // SyncReadRx -> GroupSyncRead class - // SyncReadTxRx -> GroupSyncRead class + int syncReadTx (PortHandler *port, uint16_t start_address, uint16_t data_length, uint8_t *param, uint16_t param_length); + // SyncReadRx -> GroupSyncRead class + // SyncReadTxRx -> GroupSyncRead class - // param : ID1 DATA0 DATA1 ... DATAn ID2 DATA0 DATA1 ... DATAn ID3 DATA0 DATA1 ... DATAn - int SyncWriteTxOnly (PortHandler *port, UINT16_T start_address, UINT16_T data_length, UINT8_T *param, UINT16_T param_length); + // param : ID1 DATA0 DATA1 ... DATAn ID2 DATA0 DATA1 ... DATAn ID3 DATA0 DATA1 ... DATAn + int syncWriteTxOnly (PortHandler *port, uint16_t start_address, uint16_t data_length, uint8_t *param, uint16_t param_length); - // param : LEN1 ID1 ADDR1 LEN2 ID2 ADDR2 ... - int BulkReadTx (PortHandler *port, UINT8_T *param, UINT16_T param_length); - // BulkReadRx -> GroupBulkRead class - // BulkReadTxRx -> GroupBulkRead class + // param : LEN1 ID1 ADDR1 LEN2 ID2 ADDR2 ... + int bulkReadTx (PortHandler *port, uint8_t *param, uint16_t param_length); + // BulkReadRx -> GroupBulkRead class + // BulkReadTxRx -> GroupBulkRead class - int BulkWriteTxOnly (PortHandler *port, UINT8_T *param, UINT16_T param_length); + int bulkWriteTxOnly (PortHandler *port, uint8_t *param, uint16_t param_length); }; } diff --git a/dynamixel_sdk/include/dynamixel_sdk/protocol2_packet_handler.h b/dynamixel_sdk/include/dynamixel_sdk/protocol2_packet_handler.h index 101413c..ff7e2d1 100644 --- a/dynamixel_sdk/include/dynamixel_sdk/protocol2_packet_handler.h +++ b/dynamixel_sdk/include/dynamixel_sdk/protocol2_packet_handler.h @@ -1,100 +1,131 @@ +/******************************************************************************* +* Copyright (c) 2016, ROBOTIS CO., LTD. +* 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 ROBOTIS 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. +*******************************************************************************/ + +/* Author: zerom, Leon Ryu Woon Jung */ + /* - * Protocol2PacketHandler.h + * protocol2_packet_handler.h * * Created on: 2016. 1. 26. - * Author: zerom, leon */ #ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL2PACKETHANDLER_H_ #define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL2PACKETHANDLER_H_ -#include "PacketHandler.h" +#include "packet_handler.h" -namespace ROBOTIS +namespace dynamixel { class WINDECLSPEC Protocol2PacketHandler : public PacketHandler { -private: - static Protocol2PacketHandler *unique_instance_; + private: + static Protocol2PacketHandler *unique_instance_; - Protocol2PacketHandler(); + Protocol2PacketHandler(); - UINT16_T UpdateCRC(UINT16_T crc_accum, UINT8_T *data_blk_ptr, UINT16_T data_blk_size); - void AddStuffing(UINT8_T *packet); - void RemoveStuffing(UINT8_T *packet); + uint16_t updateCRC(uint16_t crc_accum, uint8_t *data_blk_ptr, uint16_t data_blk_size); + void addStuffing(uint8_t *packet); + void removeStuffing(uint8_t *packet); -public: - static Protocol2PacketHandler *GetInstance() { return unique_instance_; } + public: + static Protocol2PacketHandler *getInstance() { return unique_instance_; } - virtual ~Protocol2PacketHandler() { } + virtual ~Protocol2PacketHandler() { } - float GetProtocolVersion() { return 2.0; } + float getProtocolVersion() { return 2.0; } - void PrintTxRxResult(int result); - void PrintRxPacketError(UINT8_T error); + void printTxRxResult(int result); + void printRxPacketError(uint8_t error); - int TxPacket (PortHandler *port, UINT8_T *txpacket); - int RxPacket (PortHandler *port, UINT8_T *rxpacket); - int TxRxPacket (PortHandler *port, UINT8_T *txpacket, UINT8_T *rxpacket, UINT8_T *error = 0); + int txPacket (PortHandler *port, uint8_t *txpacket); + int rxPacket (PortHandler *port, uint8_t *rxpacket); + int txRxPacket (PortHandler *port, uint8_t *txpacket, uint8_t *rxpacket, uint8_t *error = 0); - int Ping (PortHandler *port, UINT8_T id, UINT8_T *error = 0); - int Ping (PortHandler *port, UINT8_T id, UINT16_T *model_number, UINT8_T *error = 0); + int ping (PortHandler *port, uint8_t id, uint8_t *error = 0); + int ping (PortHandler *port, uint8_t id, uint16_t *model_number, uint8_t *error = 0); - // BroadcastPing - int BroadcastPing (PortHandler *port, std::vector &id_list); + // broadcastPing + int broadcastPing (PortHandler *port, std::vector &id_list); - int Action (PortHandler *port, UINT8_T id); - int Reboot (PortHandler *port, UINT8_T id, UINT8_T *error = 0); - int FactoryReset (PortHandler *port, UINT8_T id, UINT8_T option, UINT8_T *error = 0); + int action (PortHandler *port, uint8_t id); + int reboot (PortHandler *port, uint8_t id, uint8_t *error = 0); + int factoryReset (PortHandler *port, uint8_t id, uint8_t option, uint8_t *error = 0); - int ReadTx (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length); - int ReadRx (PortHandler *port, UINT16_T length, UINT8_T *data, UINT8_T *error = 0); - int ReadTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error = 0); + int readTx (PortHandler *port, uint8_t id, uint16_t address, uint16_t length); + int readRx (PortHandler *port, uint16_t length, uint8_t *data, uint8_t *error = 0); + int readTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0); - int Read1ByteTx (PortHandler *port, UINT8_T id, UINT16_T address); - int Read1ByteRx (PortHandler *port, UINT8_T *data, UINT8_T *error = 0); - int Read1ByteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T *data, UINT8_T *error = 0); + int read1ByteTx (PortHandler *port, uint8_t id, uint16_t address); + int read1ByteRx (PortHandler *port, uint8_t *data, uint8_t *error = 0); + int read1ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint8_t *data, uint8_t *error = 0); - int Read2ByteTx (PortHandler *port, UINT8_T id, UINT16_T address); - int Read2ByteRx (PortHandler *port, UINT16_T *data, UINT8_T *error = 0); - int Read2ByteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T *data, UINT8_T *error = 0); + int read2ByteTx (PortHandler *port, uint8_t id, uint16_t address); + int read2ByteRx (PortHandler *port, uint16_t *data, uint8_t *error = 0); + int read2ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t *data, uint8_t *error = 0); - int Read4ByteTx (PortHandler *port, UINT8_T id, UINT16_T address); - int Read4ByteRx (PortHandler *port, UINT32_T *data, UINT8_T *error = 0); - int Read4ByteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T *data, UINT8_T *error = 0); + int read4ByteTx (PortHandler *port, uint8_t id, uint16_t address); + int read4ByteRx (PortHandler *port, uint32_t *data, uint8_t *error = 0); + int read4ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint32_t *data, uint8_t *error = 0); - int WriteTxOnly (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data); - int WriteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error = 0); + int writeTxOnly (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data); + int writeTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0); - int Write1ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T data); - int Write1ByteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T data, UINT8_T *error = 0); + int write1ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint8_t data); + int write1ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint8_t data, uint8_t *error = 0); - int Write2ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T data); - int Write2ByteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T data, UINT8_T *error = 0); + int write2ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint16_t data); + int write2ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t data, uint8_t *error = 0); - int Write4ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T data); - int Write4ByteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T data, UINT8_T *error = 0); + int write4ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint32_t data); + int write4ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint32_t data, uint8_t *error = 0); - int RegWriteTxOnly (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data); - int RegWriteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error = 0); + int regWriteTxOnly (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data); + int regWriteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0); - int SyncReadTx (PortHandler *port, UINT16_T start_address, UINT16_T data_length, UINT8_T *param, UINT16_T param_length); - // SyncReadRx -> GroupSyncRead class - // SyncReadTxRx -> GroupSyncRead class + int syncReadTx (PortHandler *port, uint16_t start_address, uint16_t data_length, uint8_t *param, uint16_t param_length); + // SyncReadRx -> GroupSyncRead class + // SyncReadTxRx -> GroupSyncRead class - // param : ID1 DATA0 DATA1 ... DATAn ID2 DATA0 DATA1 ... DATAn ID3 DATA0 DATA1 ... DATAn - int SyncWriteTxOnly (PortHandler *port, UINT16_T start_address, UINT16_T data_length, UINT8_T *param, UINT16_T param_length); + // param : ID1 DATA0 DATA1 ... DATAn ID2 DATA0 DATA1 ... DATAn ID3 DATA0 DATA1 ... DATAn + int syncWriteTxOnly (PortHandler *port, uint16_t start_address, uint16_t data_length, uint8_t *param, uint16_t param_length); - // param : ID1 ADDR_L1 ADDR_H1 LEN_L1 LEN_H1 ID2 ADDR_L2 ADDR_H2 LEN_L2 LEN_H2 ... - int BulkReadTx (PortHandler *port, UINT8_T *param, UINT16_T param_length); - // BulkReadRx -> GroupBulkRead class - // BulkReadTxRx -> GroupBulkRead class + // param : ID1 ADDR_L1 ADDR_H1 LEN_L1 LEN_H1 ID2 ADDR_L2 ADDR_H2 LEN_L2 LEN_H2 ... + int bulkReadTx (PortHandler *port, uint8_t *param, uint16_t param_length); + // BulkReadRx -> GroupBulkRead class + // BulkReadTxRx -> GroupBulkRead class - // param : ID1 START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H DATA0 DATA1 ... DATAn ID2 START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H DATA0 DATA1 ... DATAn - int BulkWriteTxOnly (PortHandler *port, UINT8_T *param, UINT16_T param_length); + // param : ID1 START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H DATA0 DATA1 ... DATAn ID2 START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H DATA0 DATA1 ... DATAn + int bulkWriteTxOnly (PortHandler *port, uint8_t *param, uint16_t param_length); }; } diff --git a/dynamixel_sdk/include/dynamixel_sdk_linux/port_handler_linux.h b/dynamixel_sdk/include/dynamixel_sdk_linux/port_handler_linux.h index 6ff516e..3a9ed24 100644 --- a/dynamixel_sdk/include/dynamixel_sdk_linux/port_handler_linux.h +++ b/dynamixel_sdk/include/dynamixel_sdk_linux/port_handler_linux.h @@ -1,59 +1,90 @@ +/******************************************************************************* +* Copyright (c) 2016, ROBOTIS CO., LTD. +* 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 ROBOTIS 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. +*******************************************************************************/ + +/* Author: zerom, Leon Ryu Woon Jung */ + /* - * PortHandlerLinux.h + * port_handler_linux.h * * Created on: 2016. 1. 26. - * Author: zerom, leon */ #ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_LINUX_PORTHANDLERLINUX_H_ #define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_LINUX_PORTHANDLERLINUX_H_ -#include "dynamixel_sdk/PortHandler.h" +#include "dynamixel_sdk/port_handler.h" -namespace ROBOTIS +namespace dynamixel { class PortHandlerLinux : public PortHandler { -private: - int socket_fd_; - int baudrate_; - char port_name_[30]; + private: + int socket_fd_; + int baudrate_; + char port_name_[30]; - double packet_start_time_; - double packet_timeout_; - double tx_time_per_byte; + double packet_start_time_; + double packet_timeout_; + double tx_time_per_byte; - bool SetupPort(const int cflag_baud); - bool SetCustomBaudrate(int speed); - int GetCFlagBaud(const int baudrate); + bool setupPort(const int cflag_baud); + bool setCustomBaudrate(int speed); + int getCFlagBaud(const int baudrate); - double GetCurrentTime(); - double GetTimeSinceStart(); + double getCurrentTime(); + double getTimeSinceStart(); -public: - PortHandlerLinux(const char *port_name); - virtual ~PortHandlerLinux() { ClosePort(); } + public: + PortHandlerLinux(const char *port_name); + virtual ~PortHandlerLinux() { closePort(); } - bool OpenPort(); - void ClosePort(); - void ClearPort(); + bool openPort(); + void closePort(); + void clearPort(); - void SetPortName(const char *port_name); - char *GetPortName(); + void setPortName(const char *port_name); + char *getPortName(); - bool SetBaudRate(const int baudrate); - int GetBaudRate(); + bool setBaudRate(const int baudrate); + int getBaudRate(); - int GetBytesAvailable(); + int getBytesAvailable(); - int ReadPort(UINT8_T *packet, int length); - int WritePort(UINT8_T *packet, int length); + int readPort(uint8_t *packet, int length); + int writePort(uint8_t *packet, int length); - void SetPacketTimeout(UINT16_T packet_length); - void SetPacketTimeout(double msec); - bool IsPacketTimeout(); + void setPacketTimeout(uint16_t packet_length); + void setPacketTimeout(double msec); + bool isPacketTimeout(); }; } diff --git a/dynamixel_sdk/src/dynamixel_sdk/group_bulk_read.cpp b/dynamixel_sdk/src/dynamixel_sdk/group_bulk_read.cpp index f248ee8..7ae113b 100644 --- a/dynamixel_sdk/src/dynamixel_sdk/group_bulk_read.cpp +++ b/dynamixel_sdk/src/dynamixel_sdk/group_bulk_read.cpp @@ -1,8 +1,39 @@ +/******************************************************************************* +* Copyright (c) 2016, ROBOTIS CO., LTD. +* 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 ROBOTIS 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. +*******************************************************************************/ + +/* Author: zerom, Leon Ryu Woon Jung */ + /* - * GroupBulkRead.cpp + * group_bulk_read.cpp * * Created on: 2016. 1. 28. - * Author: zerom, leon */ #if defined(_WIN32) || defined(_WIN64) #define WINDLLEXPORT @@ -10,190 +41,197 @@ #include #include -#include "dynamixel_sdk/GroupBulkRead.h" +#include "dynamixel_sdk/group_bulk_read.h" -using namespace ROBOTIS; +using namespace dynamixel; GroupBulkRead::GroupBulkRead(PortHandler *port, PacketHandler *ph) - : port_(port), - ph_(ph), - last_result_(false), - is_param_changed_(false), - param_(0) + : port_(port), + ph_(ph), + last_result_(false), + is_param_changed_(false), + param_(0) { - ClearParam(); + clearParam(); } -void GroupBulkRead::MakeParam() +void GroupBulkRead::makeParam() { - if(id_list_.size() == 0) - return; + if (id_list_.size() == 0) + return; - if(param_ != 0) - delete[] param_; - param_ = 0; + if (param_ != 0) + delete[] param_; + param_ = 0; - if(ph_->GetProtocolVersion() == 1.0) - param_ = new UINT8_T[id_list_.size() * 3]; // ID(1) + ADDR(1) + LENGTH(1) - else // 2.0 - param_ = new UINT8_T[id_list_.size() * 5]; // ID(1) + ADDR(2) + LENGTH(2) + if (ph_->getProtocolVersion() == 1.0) + { + param_ = new uint8_t[id_list_.size() * 3]; // ID(1) + ADDR(1) + LENGTH(1) + } + else // 2.0 + { + param_ = new uint8_t[id_list_.size() * 5]; // ID(1) + ADDR(2) + LENGTH(2) + } - int _idx = 0; - for(unsigned int _i = 0; _i < id_list_.size(); _i++) + int idx = 0; + for (unsigned int i = 0; i < id_list_.size(); i++) + { + uint8_t id = id_list_[i]; + if (ph_->getProtocolVersion() == 1.0) { - UINT8_T _id = id_list_[_i]; - if(ph_->GetProtocolVersion() == 1.0) - { - param_[_idx++] = (UINT8_T)length_list_[_id]; // LEN - param_[_idx++] = _id; // ID - param_[_idx++] = (UINT8_T)address_list_[_id]; // ADDR - } - else // 2.0 - { - param_[_idx++] = _id; // ID - param_[_idx++] = DXL_LOBYTE(address_list_[_id]); // ADDR_L - param_[_idx++] = DXL_HIBYTE(address_list_[_id]); // ADDR_H - param_[_idx++] = DXL_LOBYTE(length_list_[_id]); // LEN_L - param_[_idx++] = DXL_HIBYTE(length_list_[_id]); // LEN_H - } + param_[idx++] = (uint8_t)length_list_[id]; // LEN + param_[idx++] = id; // ID + param_[idx++] = (uint8_t)address_list_[id]; // ADDR } -} - -bool GroupBulkRead::AddParam(UINT8_T id, UINT16_T start_address, UINT16_T data_length) -{ - if(std::find(id_list_.begin(), id_list_.end(), id) != id_list_.end()) // id already exist - return false; - - id_list_.push_back(id); - length_list_[id] = data_length; - address_list_[id] = start_address; - data_list_[id] = new UINT8_T[data_length]; - - is_param_changed_ = true; - return true; -} - -void GroupBulkRead::RemoveParam(UINT8_T id) -{ - std::vector::iterator it = std::find(id_list_.begin(), id_list_.end(), id); - if(it == id_list_.end()) // NOT exist - return; - - id_list_.erase(it); - address_list_.erase(id); - length_list_.erase(id); - delete[] data_list_[id]; - data_list_.erase(id); - - is_param_changed_ = true; -} - -void GroupBulkRead::ClearParam() -{ - if(id_list_.size() == 0) - return; - - for(unsigned int _i = 0; _i < id_list_.size(); _i++) - delete[] data_list_[id_list_[_i]]; - - id_list_.clear(); - address_list_.clear(); - length_list_.clear(); - data_list_.clear(); - if(param_ != 0) - delete[] param_; - param_ = 0; -} - -int GroupBulkRead::TxPacket() -{ - if(id_list_.size() == 0) - return COMM_NOT_AVAILABLE; - - if(is_param_changed_ == true) - MakeParam(); - - if(ph_->GetProtocolVersion() == 1.0) - return ph_->BulkReadTx(port_, param_, id_list_.size() * 3); else // 2.0 - return ph_->BulkReadTx(port_, param_, id_list_.size() * 5); -} - -int GroupBulkRead::RxPacket() -{ - int _cnt = id_list_.size(); - int _result = COMM_RX_FAIL; - - last_result_ = false; - - if(_cnt == 0) - return COMM_NOT_AVAILABLE; - - for(int _i = 0; _i < _cnt; _i++) { - UINT8_T _id = id_list_[_i]; - - _result = ph_->ReadRx(port_, length_list_[_id], data_list_[_id]); - if(_result != COMM_SUCCESS) - { - fprintf(stderr, "[GroupBulkRead::RxPacket] ID %d result : %d !!!!!!!!!!\n", _id, _result); - return _result; - } + param_[idx++] = id; // ID + param_[idx++] = DXL_LOBYTE(address_list_[id]); // ADDR_L + param_[idx++] = DXL_HIBYTE(address_list_[id]); // ADDR_H + param_[idx++] = DXL_LOBYTE(length_list_[id]); // LEN_L + param_[idx++] = DXL_HIBYTE(length_list_[id]); // LEN_H } - - if(_result == COMM_SUCCESS) - last_result_ = true; - - return _result; + } } -int GroupBulkRead::TxRxPacket() +bool GroupBulkRead::addParam(uint8_t id, uint16_t start_address, uint16_t data_length) { - int _result = COMM_TX_FAIL; + if (std::find(id_list_.begin(), id_list_.end(), id) != id_list_.end()) // id already exist + return false; - _result = TxPacket(); - if(_result != COMM_SUCCESS) - return _result; + id_list_.push_back(id); + length_list_[id] = data_length; + address_list_[id] = start_address; + data_list_[id] = new uint8_t[data_length]; - return RxPacket(); + is_param_changed_ = true; + return true; } -bool GroupBulkRead::IsAvailable(UINT8_T id, UINT16_T address, UINT16_T data_length) +void GroupBulkRead::removeParam(uint8_t id) { - UINT16_T _start_addr, _data_length; + std::vector::iterator it = std::find(id_list_.begin(), id_list_.end(), id); + if (it == id_list_.end()) // NOT exist + return; - if(last_result_ == false || data_list_.find(id) == data_list_.end()) - return false; + id_list_.erase(it); + address_list_.erase(id); + length_list_.erase(id); + delete[] data_list_[id]; + data_list_.erase(id); - _start_addr = address_list_[id]; - _data_length = length_list_[id]; - - if(address < _start_addr || _start_addr + _data_length - data_length < address) - return false; - - return true; + is_param_changed_ = true; } -UINT32_T GroupBulkRead::GetData(UINT8_T id, UINT16_T address, UINT16_T data_length) +void GroupBulkRead::clearParam() { - if(IsAvailable(id, address, data_length) == false) - return 0; + if (id_list_.size() == 0) + return; - UINT16_T _start_addr = address_list_[id]; + for (unsigned int i = 0; i < id_list_.size(); i++) + delete[] data_list_[id_list_[i]]; - switch(data_length) + id_list_.clear(); + address_list_.clear(); + length_list_.clear(); + data_list_.clear(); + if (param_ != 0) + delete[] param_; + param_ = 0; +} + +int GroupBulkRead::txPacket() +{ + if (id_list_.size() == 0) + return COMM_NOT_AVAILABLE; + + if (is_param_changed_ == true) + makeParam(); + + if (ph_->getProtocolVersion() == 1.0) + { + return ph_->bulkReadTx(port_, param_, id_list_.size() * 3); + } + else // 2.0 + { + return ph_->bulkReadTx(port_, param_, id_list_.size() * 5); + } +} + +int GroupBulkRead::rxPacket() +{ + int cnt = id_list_.size(); + int result = COMM_RX_FAIL; + + last_result_ = false; + + if (cnt == 0) + return COMM_NOT_AVAILABLE; + + for (int i = 0; i < cnt; i++) + { + uint8_t id = id_list_[i]; + + result = ph_->readRx(port_, length_list_[id], data_list_[id]); + if (result != COMM_SUCCESS) { + fprintf(stderr, "[GroupBulkRead::rxPacket] ID %d result : %d !!!!!!!!!!\n", id, result); + return result; + } + } + + if (result == COMM_SUCCESS) + last_result_ = true; + + return result; +} + +int GroupBulkRead::txRxPacket() +{ + int result = COMM_TX_FAIL; + + result = txPacket(); + if (result != COMM_SUCCESS) + return result; + + return rxPacket(); +} + +bool GroupBulkRead::isAvailable(uint8_t id, uint16_t address, uint16_t data_length) +{ + uint16_t start_addr; + + if (last_result_ == false || data_list_.find(id) == data_list_.end()) + return false; + + start_addr = address_list_[id]; + + if (address < start_addr || start_addr + length_list_[id] - data_length < address) + return false; + + return true; +} + +uint32_t GroupBulkRead::getData(uint8_t id, uint16_t address, uint16_t data_length) +{ + if (isAvailable(id, address, data_length) == false) + return 0; + + uint16_t start_addr = address_list_[id]; + + switch(data_length) + { case 1: - return data_list_[id][address - _start_addr]; + return data_list_[id][address - start_addr]; case 2: - return DXL_MAKEWORD(data_list_[id][address - _start_addr], data_list_[id][address - _start_addr + 1]); + return DXL_MAKEWORD(data_list_[id][address - start_addr], data_list_[id][address - start_addr + 1]); case 4: - return DXL_MAKEDWORD(DXL_MAKEWORD(data_list_[id][address - _start_addr + 0], data_list_[id][address - _start_addr + 1]), - DXL_MAKEWORD(data_list_[id][address - _start_addr + 2], data_list_[id][address - _start_addr + 3])); + return DXL_MAKEDWORD(DXL_MAKEWORD(data_list_[id][address - start_addr + 0], data_list_[id][address - start_addr + 1]), + DXL_MAKEWORD(data_list_[id][address - start_addr + 2], data_list_[id][address - start_addr + 3])); default: - return 0; - } + return 0; + } } diff --git a/dynamixel_sdk/src/dynamixel_sdk/group_bulk_write.cpp b/dynamixel_sdk/src/dynamixel_sdk/group_bulk_write.cpp index 973793b..7b84151 100644 --- a/dynamixel_sdk/src/dynamixel_sdk/group_bulk_write.cpp +++ b/dynamixel_sdk/src/dynamixel_sdk/group_bulk_write.cpp @@ -1,137 +1,168 @@ +/******************************************************************************* +* Copyright (c) 2016, ROBOTIS CO., LTD. +* 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 ROBOTIS 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. +*******************************************************************************/ + +/* Author: zerom, Leon Ryu Woon Jung */ + /* - * GroupBulkWrite.cpp + * group_bulk_write.cpp * * Created on: 2016. 2. 2. - * Author: zerom, leon */ #if defined(_WIN32) || defined(_WIN64) #define WINDLLEXPORT #endif #include -#include "dynamixel_sdk/GroupBulkWrite.h" +#include "dynamixel_sdk/group_bulk_write.h" -using namespace ROBOTIS; +using namespace dynamixel; GroupBulkWrite::GroupBulkWrite(PortHandler *port, PacketHandler *ph) - : port_(port), - ph_(ph), - is_param_changed_(false), - param_(0), - param_length_(0) + : port_(port), + ph_(ph), + is_param_changed_(false), + param_(0), + param_length_(0) { - ClearParam(); + clearParam(); } -void GroupBulkWrite::MakeParam() +void GroupBulkWrite::makeParam() { - if(ph_->GetProtocolVersion() == 1.0 || id_list_.size() == 0) - return; + if (ph_->getProtocolVersion() == 1.0 || id_list_.size() == 0) + return; - if(param_ != 0) - delete[] param_; - param_ = 0; + if (param_ != 0) + delete[] param_; + param_ = 0; - param_length_ = 0; - for(unsigned int _i = 0; _i < id_list_.size(); _i++) - param_length_ += 1 + 2 + 2 + length_list_[id_list_[_i]]; + param_length_ = 0; + for (unsigned int i = 0; i < id_list_.size(); i++) + param_length_ += 1 + 2 + 2 + length_list_[id_list_[i]]; - param_ = new UINT8_T[param_length_]; + param_ = new uint8_t[param_length_]; - int _idx = 0; - for(unsigned int _i = 0; _i < id_list_.size(); _i++) - { - UINT8_T _id = id_list_[_i]; - if(data_list_[_id] == 0) - return; + int idx = 0; + for (unsigned int i = 0; i < id_list_.size(); i++) + { + uint8_t id = id_list_[i]; + if (data_list_[id] == 0) + return; - param_[_idx++] = _id; - param_[_idx++] = DXL_LOBYTE(address_list_[_id]); - param_[_idx++] = DXL_HIBYTE(address_list_[_id]); - param_[_idx++] = DXL_LOBYTE(length_list_[_id]); - param_[_idx++] = DXL_HIBYTE(length_list_[_id]); - for(int _c = 0; _c < length_list_[_id]; _c++) - param_[_idx++] = (data_list_[_id])[_c]; - } + param_[idx++] = id; + param_[idx++] = DXL_LOBYTE(address_list_[id]); + param_[idx++] = DXL_HIBYTE(address_list_[id]); + param_[idx++] = DXL_LOBYTE(length_list_[id]); + param_[idx++] = DXL_HIBYTE(length_list_[id]); + for (int c = 0; c < length_list_[id]; c++) + param_[idx++] = (data_list_[id])[c]; + } } -bool GroupBulkWrite::AddParam(UINT8_T id, UINT16_T start_address, UINT16_T data_length, UINT8_T *data) +bool GroupBulkWrite::addParam(uint8_t id, uint16_t start_address, uint16_t data_length, uint8_t *data) { - if(ph_->GetProtocolVersion() == 1.0) - return false; + if (ph_->getProtocolVersion() == 1.0) + return false; - if(std::find(id_list_.begin(), id_list_.end(), id) != id_list_.end()) // id already exist - return false; + if (std::find(id_list_.begin(), id_list_.end(), id) != id_list_.end()) // id already exist + return false; - id_list_.push_back(id); - address_list_[id] = start_address; - length_list_[id] = data_length; - data_list_[id] = new UINT8_T[data_length]; - for(int _c = 0; _c < data_length; _c++) - data_list_[id][_c] = data[_c]; + id_list_.push_back(id); + address_list_[id] = start_address; + length_list_[id] = data_length; + data_list_[id] = new uint8_t[data_length]; + for (int c = 0; c < data_length; c++) + data_list_[id][c] = data[c]; - is_param_changed_ = true; - return true; + is_param_changed_ = true; + return true; } -void GroupBulkWrite::RemoveParam(UINT8_T id) +void GroupBulkWrite::removeParam(uint8_t id) { - if(ph_->GetProtocolVersion() == 1.0) - return; + if (ph_->getProtocolVersion() == 1.0) + return; - std::vector::iterator it = std::find(id_list_.begin(), id_list_.end(), id); - if(it == id_list_.end()) // NOT exist - return; + std::vector::iterator it = std::find(id_list_.begin(), id_list_.end(), id); + if (it == id_list_.end()) // NOT exist + return; - id_list_.erase(it); - address_list_.erase(id); - length_list_.erase(id); - delete[] data_list_[id]; - data_list_.erase(id); + id_list_.erase(it); + address_list_.erase(id); + length_list_.erase(id); + delete[] data_list_[id]; + data_list_.erase(id); - is_param_changed_ = true; + is_param_changed_ = true; } -bool GroupBulkWrite::ChangeParam(UINT8_T id, UINT16_T start_address, UINT16_T data_length, UINT8_T *data) +bool GroupBulkWrite::changeParam(uint8_t id, uint16_t start_address, uint16_t data_length, uint8_t *data) { - if(ph_->GetProtocolVersion() == 1.0) - return false; + if (ph_->getProtocolVersion() == 1.0) + return false; - std::vector::iterator it = std::find(id_list_.begin(), id_list_.end(), id); - if(it == id_list_.end()) // NOT exist - return false; + std::vector::iterator it = std::find(id_list_.begin(), id_list_.end(), id); + if (it == id_list_.end()) // NOT exist + return false; - address_list_[id] = start_address; - length_list_[id] = data_length; - delete[] data_list_[id]; - data_list_[id] = new UINT8_T[data_length]; - for(int _c = 0; _c < data_length; _c++) - data_list_[id][_c] = data[_c]; + address_list_[id] = start_address; + length_list_[id] = data_length; + delete[] data_list_[id]; + data_list_[id] = new uint8_t[data_length]; + for (int c = 0; c < data_length; c++) + data_list_[id][c] = data[c]; - is_param_changed_ = true; - return true; + is_param_changed_ = true; + return true; } -void GroupBulkWrite::ClearParam() +void GroupBulkWrite::clearParam() { - if(ph_->GetProtocolVersion() == 1.0 || id_list_.size() == 0) - return; + if (ph_->getProtocolVersion() == 1.0 || id_list_.size() == 0) + return; - for(unsigned int _i = 0; _i < id_list_.size(); _i++) - delete[] data_list_[id_list_[_i]]; + for (unsigned int i = 0; i < id_list_.size(); i++) + delete[] data_list_[id_list_[i]]; - id_list_.clear(); - address_list_.clear(); - length_list_.clear(); - data_list_.clear(); - if(param_ != 0) - delete[] param_; - param_ = 0; + id_list_.clear(); + address_list_.clear(); + length_list_.clear(); + data_list_.clear(); + if (param_ != 0) + delete[] param_; + param_ = 0; } -int GroupBulkWrite::TxPacket() +int GroupBulkWrite::txPacket() { - if(ph_->GetProtocolVersion() == 1.0 || id_list_.size() == 0) - return COMM_NOT_AVAILABLE; + if (ph_->getProtocolVersion() == 1.0 || id_list_.size() == 0) + return COMM_NOT_AVAILABLE; - if(is_param_changed_ == true) - MakeParam(); + if (is_param_changed_ == true) + makeParam(); - return ph_->BulkWriteTxOnly(port_, param_, param_length_); + return ph_->bulkWriteTxOnly(port_, param_, param_length_); } diff --git a/dynamixel_sdk/src/dynamixel_sdk/group_sync_read.cpp b/dynamixel_sdk/src/dynamixel_sdk/group_sync_read.cpp index fede283..13818cc 100644 --- a/dynamixel_sdk/src/dynamixel_sdk/group_sync_read.cpp +++ b/dynamixel_sdk/src/dynamixel_sdk/group_sync_read.cpp @@ -1,172 +1,203 @@ +/******************************************************************************* +* Copyright (c) 2016, ROBOTIS CO., LTD. +* 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 ROBOTIS 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. +*******************************************************************************/ + +/* Author: zerom, Leon Ryu Woon Jung */ + /* - * GroupSyncRead.cpp + * group_sync_read.cpp * * Created on: 2016. 2. 2. - * Author: zerom, leon */ #if defined(_WIN32) || defined(_WIN64) #define WINDLLEXPORT #endif #include -#include "dynamixel_sdk/GroupSyncRead.h" +#include "dynamixel_sdk/group_sync_read.h" -using namespace ROBOTIS; +using namespace dynamixel; -GroupSyncRead::GroupSyncRead(PortHandler *port, PacketHandler *ph, UINT16_T start_address, UINT16_T data_length) - : port_(port), - ph_(ph), - last_result_(false), - is_param_changed_(false), - param_(0), - start_address_(start_address), - data_length_(data_length) +GroupSyncRead::GroupSyncRead(PortHandler *port, PacketHandler *ph, uint16_t start_address, uint16_t data_length) + : port_(port), + ph_(ph), + last_result_(false), + is_param_changed_(false), + param_(0), + start_address_(start_address), + data_length_(data_length) { - ClearParam(); + clearParam(); } -void GroupSyncRead::MakeParam() +void GroupSyncRead::makeParam() { - if(ph_->GetProtocolVersion() == 1.0 || id_list_.size() == 0) - return; + if (ph_->getProtocolVersion() == 1.0 || id_list_.size() == 0) + return; - if(param_ != 0) - delete[] param_; - param_ = 0; + if (param_ != 0) + delete[] param_; + param_ = 0; - param_ = new UINT8_T[id_list_.size() * 1]; // ID(1) + param_ = new uint8_t[id_list_.size() * 1]; // ID(1) - int _idx = 0; - for(unsigned int _i = 0; _i < id_list_.size(); _i++) - param_[_idx++] = id_list_[_i]; + int idx = 0; + for (unsigned int i = 0; i < id_list_.size(); i++) + param_[idx++] = id_list_[i]; } -bool GroupSyncRead::AddParam(UINT8_T id) +bool GroupSyncRead::addParam(uint8_t id) { - if(ph_->GetProtocolVersion() == 1.0) - return false; + if (ph_->getProtocolVersion() == 1.0) + return false; - if(std::find(id_list_.begin(), id_list_.end(), id) != id_list_.end()) // id already exist - return false; + if (std::find(id_list_.begin(), id_list_.end(), id) != id_list_.end()) // id already exist + return false; - id_list_.push_back(id); - data_list_[id] = new UINT8_T[data_length_]; + id_list_.push_back(id); + data_list_[id] = new uint8_t[data_length_]; - is_param_changed_ = true; - return true; + is_param_changed_ = true; + return true; } -void GroupSyncRead::RemoveParam(UINT8_T id) +void GroupSyncRead::removeParam(uint8_t id) { - if(ph_->GetProtocolVersion() == 1.0) - return; + if (ph_->getProtocolVersion() == 1.0) + return; - std::vector::iterator it = std::find(id_list_.begin(), id_list_.end(), id); - if(it == id_list_.end()) // NOT exist - return; + std::vector::iterator it = std::find(id_list_.begin(), id_list_.end(), id); + if (it == id_list_.end()) // NOT exist + return; - id_list_.erase(it); - delete[] data_list_[id]; - data_list_.erase(id); + id_list_.erase(it); + delete[] data_list_[id]; + data_list_.erase(id); - is_param_changed_ = true; + is_param_changed_ = true; } -void GroupSyncRead::ClearParam() +void GroupSyncRead::clearParam() { - if(ph_->GetProtocolVersion() == 1.0 || id_list_.size() == 0) - return; + if (ph_->getProtocolVersion() == 1.0 || id_list_.size() == 0) + return; - for(unsigned int _i = 0; _i < id_list_.size(); _i++) - delete[] data_list_[id_list_[_i]]; + for (unsigned int i = 0; i < id_list_.size(); i++) + delete[] data_list_[id_list_[i]]; - id_list_.clear(); - data_list_.clear(); - if(param_ != 0) - delete[] param_; - param_ = 0; + id_list_.clear(); + data_list_.clear(); + if (param_ != 0) + delete[] param_; + param_ = 0; } -int GroupSyncRead::TxPacket() +int GroupSyncRead::txPacket() { - if(ph_->GetProtocolVersion() == 1.0 || id_list_.size() == 0) - return COMM_NOT_AVAILABLE; + if (ph_->getProtocolVersion() == 1.0 || id_list_.size() == 0) + return COMM_NOT_AVAILABLE; - if(is_param_changed_ == true) - MakeParam(); + if (is_param_changed_ == true) + makeParam(); - return ph_->SyncReadTx(port_, start_address_, data_length_, param_, (UINT16_T)id_list_.size() * 1); + return ph_->syncReadTx(port_, start_address_, data_length_, param_, (uint16_t)id_list_.size() * 1); } -int GroupSyncRead::RxPacket() +int GroupSyncRead::rxPacket() { - last_result_ = false; + last_result_ = false; - if(ph_->GetProtocolVersion() == 1.0) - return COMM_NOT_AVAILABLE; + if (ph_->getProtocolVersion() == 1.0) + return COMM_NOT_AVAILABLE; - int _cnt = id_list_.size(); - int _result = COMM_RX_FAIL; + int cnt = id_list_.size(); + int result = COMM_RX_FAIL; - if(_cnt == 0) - return COMM_NOT_AVAILABLE; + if (cnt == 0) + return COMM_NOT_AVAILABLE; - for(int _i = 0; _i < _cnt; _i++) - { - UINT8_T _id = id_list_[_i]; + for (int i = 0; i < cnt; i++) + { + uint8_t id = id_list_[i]; - _result = ph_->ReadRx(port_, data_length_, data_list_[_id]); - if(_result != COMM_SUCCESS) - return _result; - } + result = ph_->readRx(port_, data_length_, data_list_[id]); + if (result != COMM_SUCCESS) + return result; + } - if(_result == COMM_SUCCESS) - last_result_ = true; + if (result == COMM_SUCCESS) + last_result_ = true; - return _result; + return result; } -int GroupSyncRead::TxRxPacket() +int GroupSyncRead::txRxPacket() { - if(ph_->GetProtocolVersion() == 1.0) - return COMM_NOT_AVAILABLE; + if (ph_->getProtocolVersion() == 1.0) + return COMM_NOT_AVAILABLE; - int _result = COMM_TX_FAIL; + int result = COMM_TX_FAIL; - _result = TxPacket(); - if(_result != COMM_SUCCESS) - return _result; + result = txPacket(); + if (result != COMM_SUCCESS) + return result; - return RxPacket(); + return rxPacket(); } -bool GroupSyncRead::IsAvailable(UINT8_T id, UINT16_T address, UINT16_T data_length) +bool GroupSyncRead::isAvailable(uint8_t id, uint16_t address, uint16_t data_length) { - if(ph_->GetProtocolVersion() == 1.0 || last_result_ == false || data_list_.find(id) == data_list_.end()) - return false; + if (ph_->getProtocolVersion() == 1.0 || last_result_ == false || data_list_.find(id) == data_list_.end()) + return false; - if(address < start_address_ || start_address_ + data_length_ - data_length < address) - return false; + if (address < start_address_ || start_address_ + data_length_ - data_length < address) + return false; - return true; + return true; } -UINT32_T GroupSyncRead::GetData(UINT8_T id, UINT16_T address, UINT16_T data_length) +uint32_t GroupSyncRead::getData(uint8_t id, uint16_t address, uint16_t data_length) { - if(IsAvailable(id, address, data_length) == false) - return 0; + if (isAvailable(id, address, data_length) == false) + return 0; - switch(data_length) - { + switch(data_length) + { case 1: - return data_list_[id][address - start_address_]; + return data_list_[id][address - start_address_]; case 2: - return DXL_MAKEWORD(data_list_[id][address - start_address_], data_list_[id][address - start_address_ + 1]); + return DXL_MAKEWORD(data_list_[id][address - start_address_], data_list_[id][address - start_address_ + 1]); case 4: - return DXL_MAKEDWORD(DXL_MAKEWORD(data_list_[id][address - start_address_ + 0], data_list_[id][address - start_address_ + 1]), - DXL_MAKEWORD(data_list_[id][address - start_address_ + 2], data_list_[id][address - start_address_ + 3])); + return DXL_MAKEDWORD(DXL_MAKEWORD(data_list_[id][address - start_address_ + 0], data_list_[id][address - start_address_ + 1]), + DXL_MAKEWORD(data_list_[id][address - start_address_ + 2], data_list_[id][address - start_address_ + 3])); default: - return 0; - } + return 0; + } } diff --git a/dynamixel_sdk/src/dynamixel_sdk/group_sync_write.cpp b/dynamixel_sdk/src/dynamixel_sdk/group_sync_write.cpp index 8a61435..94dea6f 100644 --- a/dynamixel_sdk/src/dynamixel_sdk/group_sync_write.cpp +++ b/dynamixel_sdk/src/dynamixel_sdk/group_sync_write.cpp @@ -1,117 +1,147 @@ +/******************************************************************************* +* Copyright (c) 2016, ROBOTIS CO., LTD. +* 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 ROBOTIS 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. +*******************************************************************************/ + +/* Author: zerom, Leon Ryu Woon Jung */ + /* - * GroupSyncWrite.cpp + * group_sync_write.cpp * * Created on: 2016. 1. 28. - * Author: zerom, leon */ #if defined(_WIN32) || defined(_WIN64) #define WINDLLEXPORT #endif #include -#include "dynamixel_sdk/GroupSyncWrite.h" +#include "dynamixel_sdk/group_sync_write.h" -using namespace ROBOTIS; +using namespace dynamixel; -GroupSyncWrite::GroupSyncWrite(PortHandler *port, PacketHandler *ph, UINT16_T start_address, UINT16_T data_length) - : port_(port), - ph_(ph), - is_param_changed_(false), - param_(0), - start_address_(start_address), - data_length_(data_length) +GroupSyncWrite::GroupSyncWrite(PortHandler *port, PacketHandler *ph, uint16_t start_address, uint16_t data_length) + : port_(port), + ph_(ph), + is_param_changed_(false), + param_(0), + start_address_(start_address), + data_length_(data_length) { - ClearParam(); + clearParam(); } -void GroupSyncWrite::MakeParam() +void GroupSyncWrite::makeParam() { - if(id_list_.size() == 0) - return; + if (id_list_.size() == 0) return; - if(param_ != 0) - delete[] param_; - param_ = 0; + if (param_ != 0) + delete[] param_; + param_ = 0; - param_ = new UINT8_T[id_list_.size() * (1 + data_length_)]; // ID(1) + DATA(data_length) + param_ = new uint8_t[id_list_.size() * (1 + data_length_)]; // ID(1) + DATA(data_length) - int _idx = 0; - for(unsigned int _i = 0; _i < id_list_.size(); _i++) - { - UINT8_T _id = id_list_[_i]; - if(data_list_[_id] == 0) - return; + int idx = 0; + for (unsigned int i = 0; i < id_list_.size(); i++) + { + uint8_t id = id_list_[i]; + if (data_list_[id] == 0) + return; - param_[_idx++] = _id; - for(int _c = 0; _c < data_length_; _c++) - param_[_idx++] = (data_list_[_id])[_c]; - } + param_[idx++] = id; + for (int c = 0; c < data_length_; c++) + param_[idx++] = (data_list_[id])[c]; + } } -bool GroupSyncWrite::AddParam(UINT8_T id, UINT8_T *data) +bool GroupSyncWrite::addParam(uint8_t id, uint8_t *data) { - if(std::find(id_list_.begin(), id_list_.end(), id) != id_list_.end()) // id already exist - return false; + if (std::find(id_list_.begin(), id_list_.end(), id) != id_list_.end()) // id already exist + return false; - id_list_.push_back(id); - data_list_[id] = new UINT8_T[data_length_]; - for(int _c = 0; _c < data_length_; _c++) - data_list_[id][_c] = data[_c]; + id_list_.push_back(id); + data_list_[id] = new uint8_t[data_length_]; + for (int c = 0; c < data_length_; c++) + data_list_[id][c] = data[c]; - is_param_changed_ = true; - return true; + is_param_changed_ = true; + return true; } -void GroupSyncWrite::RemoveParam(UINT8_T id) +void GroupSyncWrite::removeParam(uint8_t id) { - std::vector::iterator it = std::find(id_list_.begin(), id_list_.end(), id); - if(it == id_list_.end()) // NOT exist - return; + std::vector::iterator it = std::find(id_list_.begin(), id_list_.end(), id); + if (it == id_list_.end()) // NOT exist + return; - id_list_.erase(it); - delete[] data_list_[id]; - data_list_.erase(id); + id_list_.erase(it); + delete[] data_list_[id]; + data_list_.erase(id); - is_param_changed_ = true; + is_param_changed_ = true; } -bool GroupSyncWrite::ChangeParam(UINT8_T id, UINT8_T *data) +bool GroupSyncWrite::changeParam(uint8_t id, uint8_t *data) { - std::vector::iterator it = std::find(id_list_.begin(), id_list_.end(), id); - if(it == id_list_.end()) // NOT exist - return false; + std::vector::iterator it = std::find(id_list_.begin(), id_list_.end(), id); + if (it == id_list_.end()) // NOT exist + return false; - delete[] data_list_[id]; - data_list_[id] = new UINT8_T[data_length_]; - for(int _c = 0; _c < data_length_; _c++) - data_list_[id][_c] = data[_c]; + delete[] data_list_[id]; + data_list_[id] = new uint8_t[data_length_]; + for (int c = 0; c < data_length_; c++) + data_list_[id][c] = data[c]; - is_param_changed_ = true; - return true; + is_param_changed_ = true; + return true; } -void GroupSyncWrite::ClearParam() +void GroupSyncWrite::clearParam() { - if(id_list_.size() == 0) - return; + if (id_list_.size() == 0) + return; - for(unsigned int _i = 0; _i < id_list_.size(); _i++) - delete[] data_list_[id_list_[_i]]; + for (unsigned int i = 0; i < id_list_.size(); i++) + delete[] data_list_[id_list_[i]]; - id_list_.clear(); - data_list_.clear(); - if(param_ != 0) - delete[] param_; - param_ = 0; + id_list_.clear(); + data_list_.clear(); + if (param_ != 0) + delete[] param_; + param_ = 0; } -int GroupSyncWrite::TxPacket() +int GroupSyncWrite::txPacket() { - if(id_list_.size() == 0) - return COMM_NOT_AVAILABLE; + if (id_list_.size() == 0) + return COMM_NOT_AVAILABLE; - if(is_param_changed_ == true) - MakeParam(); + if (is_param_changed_ == true) + makeParam(); - return ph_->SyncWriteTxOnly(port_, start_address_, data_length_, param_, id_list_.size() * (1 + data_length_)); + return ph_->syncWriteTxOnly(port_, start_address_, data_length_, param_, id_list_.size() * (1 + data_length_)); } diff --git a/dynamixel_sdk/src/dynamixel_sdk/packet_handler.cpp b/dynamixel_sdk/src/dynamixel_sdk/packet_handler.cpp index 8ad1c48..04fdc20 100644 --- a/dynamixel_sdk/src/dynamixel_sdk/packet_handler.cpp +++ b/dynamixel_sdk/src/dynamixel_sdk/packet_handler.cpp @@ -1,25 +1,60 @@ +/******************************************************************************* +* Copyright (c) 2016, ROBOTIS CO., LTD. +* 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 ROBOTIS 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. +*******************************************************************************/ + +/* Author: zerom, Leon Ryu Woon Jung */ + /* - * PacketHandler.cpp + * packet_handler.cpp * * Created on: 2016. 1. 26. - * Author: zerom, leon */ #if defined(_WIN32) || defined(_WIN64) #define WINDLLEXPORT #endif -#include "dynamixel_sdk/PacketHandler.h" -#include "dynamixel_sdk/Protocol1PacketHandler.h" -#include "dynamixel_sdk/Protocol2PacketHandler.h" +#include "dynamixel_sdk/packet_handler.h" +#include "dynamixel_sdk/protocol1_packet_handler.h" +#include "dynamixel_sdk/protocol2_packet_handler.h" -using namespace ROBOTIS; +using namespace dynamixel; -PacketHandler *PacketHandler::GetPacketHandler(float protocol_version) +PacketHandler *PacketHandler::getPacketHandler(float protocol_version) { - if(protocol_version == 1.0) - return (PacketHandler *)(Protocol1PacketHandler::GetInstance()); - else if(protocol_version == 2.0) - return (PacketHandler *)(Protocol2PacketHandler::GetInstance()); + if (protocol_version == 1.0) + { + return (PacketHandler *)(Protocol1PacketHandler::getInstance()); + } + else if (protocol_version == 2.0) + { + return (PacketHandler *)(Protocol2PacketHandler::getInstance()); + } - return (PacketHandler *)(Protocol2PacketHandler::GetInstance()); + return (PacketHandler *)(Protocol2PacketHandler::getInstance()); } diff --git a/dynamixel_sdk/src/dynamixel_sdk/port_handler.cpp b/dynamixel_sdk/src/dynamixel_sdk/port_handler.cpp index ff4355c..14c0c28 100644 --- a/dynamixel_sdk/src/dynamixel_sdk/port_handler.cpp +++ b/dynamixel_sdk/src/dynamixel_sdk/port_handler.cpp @@ -1,32 +1,63 @@ +/******************************************************************************* +* Copyright (c) 2016, ROBOTIS CO., LTD. +* 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 ROBOTIS 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. +*******************************************************************************/ + +/* Author: zerom, Leon Ryu Woon Jung */ + /* - * PortHandler.cpp + * port_handler.cpp * * Created on: 2016. 2. 5. - * Author: zerom, leon */ #if defined(_WIN32) || defined(_WIN64) #define WINDLLEXPORT #endif -#include "dynamixel_sdk/PortHandler.h" +#include "dynamixel_sdk/port_handler.h" #ifdef __linux__ - #include "dynamixel_sdk_linux/PortHandlerLinux.h" + #include "dynamixel_sdk_linux/port_handler_linux.h" #endif #if defined(_WIN32) || defined(_WIN64) - #include "dynamixel_sdk_windows/PortHandlerWindows.h" + #include "dynamixel_sdk_windows/port_handler_windows.h" #endif -using namespace ROBOTIS; +using namespace dynamixel; -PortHandler *PortHandler::GetPortHandler(const char *port_name) +PortHandler *PortHandler::getPortHandler(const char *port_name) { #ifdef __linux__ - return (PortHandler *)(new PortHandlerLinux(port_name)); + return (PortHandler *)(new PortHandlerLinux(port_name)); #endif #if defined(_WIN32) || defined(_WIN64) - return (PortHandler *)(new PortHandlerWindows(port_name)); + return (PortHandler *)(new PortHandlerWindows(port_name)); #endif } diff --git a/dynamixel_sdk/src/dynamixel_sdk/protocol1_packet_handler.cpp b/dynamixel_sdk/src/dynamixel_sdk/protocol1_packet_handler.cpp index 66877e7..5129164 100644 --- a/dynamixel_sdk/src/dynamixel_sdk/protocol1_packet_handler.cpp +++ b/dynamixel_sdk/src/dynamixel_sdk/protocol1_packet_handler.cpp @@ -1,8 +1,39 @@ +/******************************************************************************* +* Copyright (c) 2016, ROBOTIS CO., LTD. +* 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 ROBOTIS 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. +*******************************************************************************/ + +/* Author: zerom, Leon Ryu Woon Jung */ + /* - * Protocol1PacketHandler.cpp + * protocol1_packet_handler.cpp * * Created on: 2016. 1. 26. - * Author: zerom, leon */ #if defined(_WIN32) || defined(_WIN64) #define WINDLLEXPORT @@ -10,7 +41,7 @@ #include #include -#include "dynamixel_sdk/Protocol1PacketHandler.h" +#include "dynamixel_sdk/protocol1_packet_handler.h" #define TXPACKET_MAX_LEN (250) #define RXPACKET_MAX_LEN (250) @@ -33,638 +64,664 @@ #define ERRBIT_OVERLOAD 32 // The current load cannot be controlled by the set torque. #define ERRBIT_INSTRUCTION 64 // Undefined instruction or delivering the action command without the reg_write command. -using namespace ROBOTIS; +using namespace dynamixel; Protocol1PacketHandler *Protocol1PacketHandler::unique_instance_ = new Protocol1PacketHandler(); Protocol1PacketHandler::Protocol1PacketHandler() { } -void Protocol1PacketHandler::PrintTxRxResult(int result) +void Protocol1PacketHandler::printTxRxResult(int result) { - switch(result) - { - case COMM_SUCCESS: - printf("[TxRxResult] Communication success.\n"); - break; + switch(result) + { + case COMM_SUCCESS: + printf("[TxRxResult] Communication success.\n"); + break; - case COMM_PORT_BUSY: - printf("[TxRxResult] Port is in use!\n"); - break; + case COMM_PORT_BUSY: + printf("[TxRxResult] Port is in use!\n"); + break; - case COMM_TX_FAIL: - printf("[TxRxResult] Failed transmit instruction packet!\n"); - break; + case COMM_TX_FAIL: + printf("[TxRxResult] Failed transmit instruction packet!\n"); + break; - case COMM_RX_FAIL: - printf("[TxRxResult] Failed get status packet from device!\n"); - break; + case COMM_RX_FAIL: + printf("[TxRxResult] Failed get status packet from device!\n"); + break; - case COMM_TX_ERROR: - printf("[TxRxResult] Incorrect instruction packet!\n"); - break; + case COMM_TX_ERROR: + printf("[TxRxResult] Incorrect instruction packet!\n"); + break; - case COMM_RX_WAITING: - printf("[TxRxResult] Now recieving status packet!\n"); - break; + case COMM_RX_WAITING: + printf("[TxRxResult] Now recieving status packet!\n"); + break; - case COMM_RX_TIMEOUT: - printf("[TxRxResult] There is no status packet!\n"); - break; + case COMM_RX_TIMEOUT: + printf("[TxRxResult] There is no status packet!\n"); + break; - case COMM_RX_CORRUPT: - printf("[TxRxResult] Incorrect status packet!\n"); - break; + case COMM_RX_CORRUPT: + printf("[TxRxResult] Incorrect status packet!\n"); + break; - case COMM_NOT_AVAILABLE: - printf("[TxRxResult] Protocol does not support This function!\n"); - break; + case COMM_NOT_AVAILABLE: + printf("[TxRxResult] Protocol does not support This function!\n"); + break; - default: - break; - } + default: + break; + } } -void Protocol1PacketHandler::PrintRxPacketError(UINT8_T error) +void Protocol1PacketHandler::printRxPacketError(uint8_t error) { - if(error & ERRBIT_VOLTAGE) - printf("[RxPacketError] Input voltage error!\n"); + if (error & ERRBIT_VOLTAGE) + printf("[RxPacketError] Input voltage error!\n"); - if(error & ERRBIT_ANGLE) - printf("[RxPacketError] Angle limit error!\n"); + if (error & ERRBIT_ANGLE) + printf("[RxPacketError] Angle limit error!\n"); - if(error & ERRBIT_OVERHEAT) - printf("[RxPacketError] Overheat error!\n"); + if (error & ERRBIT_OVERHEAT) + printf("[RxPacketError] Overheat error!\n"); - if(error & ERRBIT_RANGE) - printf("[RxPacketError] Out of range error!\n"); + if (error & ERRBIT_RANGE) + printf("[RxPacketError] Out of range error!\n"); - if(error & ERRBIT_CHECKSUM) - printf("[RxPacketError] Checksum error!\n"); + if (error & ERRBIT_CHECKSUM) + printf("[RxPacketError] Checksum error!\n"); - if(error & ERRBIT_OVERLOAD) - printf("[RxPacketError] Overload error!\n"); + if (error & ERRBIT_OVERLOAD) + printf("[RxPacketError] Overload error!\n"); - if(error & ERRBIT_INSTRUCTION) - printf("[RxPacketError] Instruction code error!\n"); + if (error & ERRBIT_INSTRUCTION) + printf("[RxPacketError] Instruction code error!\n"); } -int Protocol1PacketHandler::TxPacket(PortHandler *port, UINT8_T *txpacket) +int Protocol1PacketHandler::txPacket(PortHandler *port, uint8_t *txpacket) { - UINT8_T _checksum = 0; - UINT8_T _total_packet_length = txpacket[PKT_LENGTH] + 4; // 4: HEADER0 HEADER1 ID LENGTH - UINT8_T _written_packet_length = 0; + uint8_t checksum = 0; + uint8_t total_packet_length = txpacket[PKT_LENGTH] + 4; // 4: HEADER0 HEADER1 ID LENGTH + uint8_t written_packet_length = 0; - if(port->is_using) - return COMM_PORT_BUSY; - port->is_using = true; + if (port->is_using_) + return COMM_PORT_BUSY; + port->is_using_ = true; - // check max packet length - if(_total_packet_length > TXPACKET_MAX_LEN) - { - port->is_using = false; - return COMM_TX_ERROR; - } + // check max packet length + if (total_packet_length > TXPACKET_MAX_LEN) + { + port->is_using_ = false; + return COMM_TX_ERROR; + } - // make packet header - txpacket[PKT_HEADER0] = 0xFF; - txpacket[PKT_HEADER1] = 0xFF; + // make packet header + txpacket[PKT_HEADER0] = 0xFF; + txpacket[PKT_HEADER1] = 0xFF; - // add a checksum to the packet - for(int _idx = 2; _idx < _total_packet_length - 1; _idx++) // except header, checksum - _checksum += txpacket[_idx]; - txpacket[_total_packet_length - 1] = ~_checksum; + // add a checksum to the packet + for (int idx = 2; idx < total_packet_length - 1; idx++) // except header, checksum + checksum += txpacket[idx]; + txpacket[total_packet_length - 1] = ~checksum; - // tx packet - port->ClearPort(); - _written_packet_length = port->WritePort(txpacket, _total_packet_length); - if(_total_packet_length != _written_packet_length) - { - port->is_using = false; - return COMM_TX_FAIL; - } + // tx packet + port->clearPort(); + written_packet_length = port->writePort(txpacket, total_packet_length); + if (total_packet_length != written_packet_length) + { + port->is_using_ = false; + return COMM_TX_FAIL; + } - return COMM_SUCCESS; + return COMM_SUCCESS; } -int Protocol1PacketHandler::RxPacket(PortHandler *port, UINT8_T *rxpacket) +int Protocol1PacketHandler::rxPacket(PortHandler *port, uint8_t *rxpacket) { - int _result = COMM_TX_FAIL; + int result = COMM_TX_FAIL; - UINT8_T _checksum = 0; - UINT8_T _rx_length = 0; - UINT8_T _wait_length = 6; // minimum length ( HEADER0 HEADER1 ID LENGTH ERROR CHKSUM ) + uint8_t checksum = 0; + uint8_t rx_length = 0; + uint8_t wait_length = 6; // minimum length (HEADER0 HEADER1 ID LENGTH ERROR CHKSUM) - while(true) + while(true) + { + rx_length += port->readPort(&rxpacket[rx_length], wait_length - rx_length); + if (rx_length >= wait_length) { - _rx_length += port->ReadPort(&rxpacket[_rx_length], _wait_length - _rx_length); - if(_rx_length >= _wait_length) + uint8_t idx = 0; + + // find packet header + for (idx = 0; idx < (rx_length - 1); idx++) + { + if (rxpacket[idx] == 0xFF && rxpacket[idx+1] == 0xFF) + break; + } + + if (idx == 0) // found at the beginning of the packet + { + if (rxpacket[PKT_ID] > 0xFD || // unavailable ID + rxpacket[PKT_LENGTH] > RXPACKET_MAX_LEN || // unavailable Length + rxpacket[PKT_ERROR] >= 0x64) // unavailable Error { - UINT8_T _idx = 0; + // remove the first byte in the packet + for (uint8_t s = 0; s < rx_length - 1; s++) + rxpacket[s] = rxpacket[1 + s]; + //memcpy(&rxpacket[0], &rxpacket[idx], rx_length - idx); + rx_length -= 1; + continue; + } - // find packet header - for(_idx = 0; _idx < (_rx_length - 1); _idx++) + // re-calculate the exact length of the rx packet + if (wait_length != rxpacket[PKT_LENGTH] + PKT_LENGTH + 1) + { + wait_length = rxpacket[PKT_LENGTH] + PKT_LENGTH + 1; + continue; + } + + if (rx_length < wait_length) + { + // check timeout + if (port->isPacketTimeout() == true) + { + if (rx_length == 0) { - if(rxpacket[_idx] == 0xFF && rxpacket[_idx+1] == 0xFF) - break; - } - - if(_idx == 0) // found at the beginning of the packet - { - if(rxpacket[PKT_ID] > 0xFD || // unavailable ID - rxpacket[PKT_LENGTH] > RXPACKET_MAX_LEN || // unavailable Length - rxpacket[PKT_ERROR] >= 0x64) // unavailable Error - { - // remove the first byte in the packet - for(UINT8_T _s = 0; _s < _rx_length - 1; _s++) - rxpacket[_s] = rxpacket[1 + _s]; - //memcpy(&rxpacket[0], &rxpacket[_idx], _rx_length - _idx); - _rx_length -= 1; - continue; - } - - // re-calculate the exact length of the rx packet - if(_wait_length != rxpacket[PKT_LENGTH] + PKT_LENGTH + 1) - { - _wait_length = rxpacket[PKT_LENGTH] + PKT_LENGTH + 1; - continue; - } - - if(_rx_length < _wait_length) - { - // check timeout - if(port->IsPacketTimeout() == true) - { - if(_rx_length == 0) - _result = COMM_RX_TIMEOUT; - else - _result = COMM_RX_CORRUPT; - break; - } - else - continue; - } - - // calculate checksum - for(int _i = 2; _i < _wait_length - 1; _i++) // except header, checksum - _checksum += rxpacket[_i]; - _checksum = ~_checksum; - - // verify checksum - if(rxpacket[_wait_length - 1] == _checksum) - _result = COMM_SUCCESS; - else - _result = COMM_RX_CORRUPT; - break; + result = COMM_RX_TIMEOUT; } else { - // remove unnecessary packets - for(UINT8_T _s = 0; _s < _rx_length - _idx; _s++) - rxpacket[_s] = rxpacket[_idx + _s]; - //memcpy(&rxpacket[0], &rxpacket[_idx], _rx_length - _idx); - _rx_length -= _idx; + result = COMM_RX_CORRUPT; } + break; + } + else + { + continue; + } + } + + // calculate checksum + for (int i = 2; i < wait_length - 1; i++) // except header, checksum + checksum += rxpacket[i]; + checksum = ~checksum; + + // verify checksum + if (rxpacket[wait_length - 1] == checksum) + { + result = COMM_SUCCESS; } else { - // check timeout - if(port->IsPacketTimeout() == true) - { - if(_rx_length == 0) - _result = COMM_RX_TIMEOUT; - else - _result = COMM_RX_CORRUPT; - break; - } + result = COMM_RX_CORRUPT; } + break; + } + else + { + // remove unnecessary packets + for (uint8_t s = 0; s < rx_length - idx; s++) + rxpacket[s] = rxpacket[idx + s]; + //memcpy(&rxpacket[0], &rxpacket[idx], rx_length - idx); + rx_length -= idx; + } } - port->is_using = false; + else + { + // check timeout + if (port->isPacketTimeout() == true) + { + if (rx_length == 0) + { + result = COMM_RX_TIMEOUT; + } + else + { + result = COMM_RX_CORRUPT; + } + break; + } + } + } + port->is_using_ = false; - return _result; + return result; } // NOT for BulkRead instruction -int Protocol1PacketHandler::TxRxPacket(PortHandler *port, UINT8_T *txpacket, UINT8_T *rxpacket, UINT8_T *error) +int Protocol1PacketHandler::txRxPacket(PortHandler *port, uint8_t *txpacket, uint8_t *rxpacket, uint8_t *error) { - int _result = COMM_TX_FAIL; + int result = COMM_TX_FAIL; // tx packet - _result = TxPacket(port, txpacket); - if(_result != COMM_SUCCESS) - return _result; + result = txPacket(port, txpacket); + if (result != COMM_SUCCESS) + return result; // (ID == Broadcast ID && NOT BulkRead) == no need to wait for status packet - // (Instruction == Action) == no need to wait for status packet - if((txpacket[PKT_ID] == BROADCAST_ID && txpacket[PKT_INSTRUCTION] != INST_BULK_READ) || + // (Instruction == action) == no need to wait for status packet + if ((txpacket[PKT_ID] == BROADCAST_ID && txpacket[PKT_INSTRUCTION] != INST_BULK_READ) || (txpacket[PKT_INSTRUCTION] == INST_ACTION)) { - port->is_using = false; - return _result; + port->is_using_ = false; + return result; } // set packet timeout - if(txpacket[PKT_INSTRUCTION] == INST_READ) - port->SetPacketTimeout((UINT16_T)(txpacket[PKT_PARAMETER0+1] + 6)); + if (txpacket[PKT_INSTRUCTION] == INST_READ) + { + port->setPacketTimeout((uint16_t)(txpacket[PKT_PARAMETER0+1] + 6)); + } else - port->SetPacketTimeout((UINT16_T)6); + { + port->setPacketTimeout((uint16_t)6); + } // rx packet - _result = RxPacket(port, rxpacket); + result = rxPacket(port, rxpacket); // check txpacket ID == rxpacket ID - if(txpacket[PKT_ID] != rxpacket[PKT_ID]) - _result = RxPacket(port, rxpacket); + if (txpacket[PKT_ID] != rxpacket[PKT_ID]) + result = rxPacket(port, rxpacket); - if(_result == COMM_SUCCESS && txpacket[PKT_ID] != BROADCAST_ID) + if (result == COMM_SUCCESS && txpacket[PKT_ID] != BROADCAST_ID) { - if(error != 0) - *error = (UINT8_T)rxpacket[PKT_ERROR]; + if (error != 0) + *error = (uint8_t)rxpacket[PKT_ERROR]; } - - return _result; + return result; } -int Protocol1PacketHandler::Ping(PortHandler *port, UINT8_T id, UINT8_T *error) +int Protocol1PacketHandler::ping(PortHandler *port, uint8_t id, uint8_t *error) { - return Ping(port, id, 0, error); + return ping(port, id, 0, error); } -int Protocol1PacketHandler::Ping(PortHandler *port, UINT8_T id, UINT16_T *model_number, UINT8_T *error) +int Protocol1PacketHandler::ping(PortHandler *port, uint8_t id, uint16_t *model_number, uint8_t *error) { - int _result = COMM_TX_FAIL; + int result = COMM_TX_FAIL; - UINT8_T txpacket[6] = {0}; - UINT8_T rxpacket[6] = {0}; + uint8_t txpacket[6] = {0}; + uint8_t rxpacket[6] = {0}; - if(id >= BROADCAST_ID) - return COMM_NOT_AVAILABLE; + if (id >= BROADCAST_ID) + return COMM_NOT_AVAILABLE; - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH] = 2; - txpacket[PKT_INSTRUCTION] = INST_PING; + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH] = 2; + txpacket[PKT_INSTRUCTION] = INST_PING; - _result = TxRxPacket(port, txpacket, rxpacket, error); - if(_result == COMM_SUCCESS && model_number != 0) + result = txRxPacket(port, txpacket, rxpacket, error); + if (result == COMM_SUCCESS && model_number != 0) + { + uint8_t data_read[2] = {0}; + result = readTxRx(port, id, 0, 2, data_read); // Address 0 : Model Number + if (result == COMM_SUCCESS) *model_number = DXL_MAKEWORD(data_read[0], data_read[1]); + } + + return result; +} + +int Protocol1PacketHandler::broadcastPing(PortHandler *port, std::vector &id_list) +{ + return COMM_NOT_AVAILABLE; +} + +int Protocol1PacketHandler::action(PortHandler *port, uint8_t id) +{ + uint8_t txpacket[6] = {0}; + + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH] = 2; + txpacket[PKT_INSTRUCTION] = INST_ACTION; + + return txRxPacket(port, txpacket, 0); +} + +int Protocol1PacketHandler::reboot(PortHandler *port, uint8_t id, uint8_t *error) +{ + return COMM_NOT_AVAILABLE; +} + +int Protocol1PacketHandler::factoryReset(PortHandler *port, uint8_t id, uint8_t option, uint8_t *error) +{ + uint8_t txpacket[6] = {0}; + uint8_t rxpacket[6] = {0}; + + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH] = 2; + txpacket[PKT_INSTRUCTION] = INST_FACTORY_RESET; + + return txRxPacket(port, txpacket, rxpacket, error); +} + +int Protocol1PacketHandler::readTx(PortHandler *port, uint8_t id, uint16_t address, uint16_t length) +{ + int result = COMM_TX_FAIL; + + uint8_t txpacket[8] = {0}; + + if (id >= BROADCAST_ID) + return COMM_NOT_AVAILABLE; + + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH] = 4; + txpacket[PKT_INSTRUCTION] = INST_READ; + txpacket[PKT_PARAMETER0+0] = (uint8_t)address; + txpacket[PKT_PARAMETER0+1] = (uint8_t)length; + + result = txPacket(port, txpacket); + + // set packet timeout + if (result == COMM_SUCCESS) + port->setPacketTimeout((uint16_t)(length+6)); + + return result; +} + +int Protocol1PacketHandler::readRx(PortHandler *port, uint16_t length, uint8_t *data, uint8_t *error) +{ + int result = COMM_TX_FAIL; + uint8_t *rxpacket = (uint8_t *)malloc(RXPACKET_MAX_LEN);//(length+6); + //uint8_t *rxpacket = new uint8_t[length+6]; + + result = rxPacket(port, rxpacket); + if (result == COMM_SUCCESS) + { + if (error != 0) { - UINT8_T _data[2] = {0}; - _result = ReadTxRx(port, id, 0, 2, _data); // Address 0 : Model Number - if(_result == COMM_SUCCESS) - *model_number = DXL_MAKEWORD(_data[0], _data[1]); + *error = (uint8_t)rxpacket[PKT_ERROR]; } - - return _result; -} - -int Protocol1PacketHandler::BroadcastPing(PortHandler *port, std::vector &id_list) -{ - return COMM_NOT_AVAILABLE; -} - -int Protocol1PacketHandler::Action(PortHandler *port, UINT8_T id) -{ - UINT8_T txpacket[6] = {0}; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH] = 2; - txpacket[PKT_INSTRUCTION] = INST_ACTION; - - return TxRxPacket(port, txpacket, 0); -} - -int Protocol1PacketHandler::Reboot(PortHandler *port, UINT8_T id, UINT8_T *error) -{ - return COMM_NOT_AVAILABLE; -} - -int Protocol1PacketHandler::FactoryReset(PortHandler *port, UINT8_T id, UINT8_T option, UINT8_T *error) -{ - UINT8_T txpacket[6] = {0}; - UINT8_T rxpacket[6] = {0}; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH] = 2; - txpacket[PKT_INSTRUCTION] = INST_FACTORY_RESET; - - return TxRxPacket(port, txpacket, rxpacket, error); -} - -int Protocol1PacketHandler::ReadTx(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length) -{ - int _result = COMM_TX_FAIL; - - UINT8_T txpacket[8] = {0}; - - if(id >= BROADCAST_ID) - return COMM_NOT_AVAILABLE; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH] = 4; - txpacket[PKT_INSTRUCTION] = INST_READ; - txpacket[PKT_PARAMETER0+0] = (UINT8_T)address; - txpacket[PKT_PARAMETER0+1] = (UINT8_T)length; - - _result = TxPacket(port, txpacket); - - // set packet timeout - if(_result == COMM_SUCCESS) - port->SetPacketTimeout((UINT16_T)(length+6)); - - return _result; -} - -int Protocol1PacketHandler::ReadRx(PortHandler *port, UINT16_T length, UINT8_T *data, UINT8_T *error) -{ - int _result = COMM_TX_FAIL; - UINT8_T *rxpacket = (UINT8_T *)malloc(RXPACKET_MAX_LEN);//(length+6); - //UINT8_T *rxpacket = new UINT8_T[length+6]; - - _result = RxPacket(port, rxpacket); - if(_result == COMM_SUCCESS) + for (uint8_t s = 0; s < length; s++) { - if(error != 0) - *error = (UINT8_T)rxpacket[PKT_ERROR]; - for(UINT8_T _s = 0; _s < length; _s++) - data[_s] = rxpacket[PKT_PARAMETER0 + _s]; - //memcpy(data, &rxpacket[PKT_PARAMETER0], length); + data[s] = rxpacket[PKT_PARAMETER0 + s]; } + //memcpy(data, &rxpacket[PKT_PARAMETER0], length); + } - free(rxpacket); - //delete[] rxpacket; - return _result; + free(rxpacket); + //delete[] rxpacket; + return result; } -int Protocol1PacketHandler::ReadTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error) +int Protocol1PacketHandler::readTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error) { - int _result = COMM_TX_FAIL; + int result = COMM_TX_FAIL; - UINT8_T txpacket[8] = {0}; - UINT8_T *rxpacket = (UINT8_T *)malloc(RXPACKET_MAX_LEN);//(length+6); + uint8_t txpacket[8] = {0}; + uint8_t *rxpacket = (uint8_t *)malloc(RXPACKET_MAX_LEN);//(length+6); - if(id >= BROADCAST_ID) - return COMM_NOT_AVAILABLE; + if (id >= BROADCAST_ID) + return COMM_NOT_AVAILABLE; - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH] = 4; - txpacket[PKT_INSTRUCTION] = INST_READ; - txpacket[PKT_PARAMETER0+0] = (UINT8_T)address; - txpacket[PKT_PARAMETER0+1] = (UINT8_T)length; + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH] = 4; + txpacket[PKT_INSTRUCTION] = INST_READ; + txpacket[PKT_PARAMETER0+0] = (uint8_t)address; + txpacket[PKT_PARAMETER0+1] = (uint8_t)length; - _result = TxRxPacket(port, txpacket, rxpacket, error); - if(_result == COMM_SUCCESS) + result = txRxPacket(port, txpacket, rxpacket, error); + if (result == COMM_SUCCESS) + { + if (error != 0) { - if(error != 0) - *error = (UINT8_T)rxpacket[PKT_ERROR]; - for(UINT8_T _s = 0; _s < length; _s++) - data[_s] = rxpacket[PKT_PARAMETER0 + _s]; - //memcpy(data, &rxpacket[PKT_PARAMETER0], length); + *error = (uint8_t)rxpacket[PKT_ERROR]; } - - free(rxpacket); - //delete[] rxpacket; - return _result; -} - -int Protocol1PacketHandler::Read1ByteTx(PortHandler *port, UINT8_T id, UINT16_T address) -{ - return ReadTx(port, id, address, 1); -} -int Protocol1PacketHandler::Read1ByteRx(PortHandler *port, UINT8_T *data, UINT8_T *error) -{ - UINT8_T _data[1] = {0}; - int _result = ReadRx(port, 1, _data, error); - if(_result == COMM_SUCCESS) - *data = _data[0]; - return _result; -} -int Protocol1PacketHandler::Read1ByteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T *data, UINT8_T *error) -{ - UINT8_T _data[1] = {0}; - int _result = ReadTxRx(port, id, address, 1, _data, error); - if(_result == COMM_SUCCESS) - *data = _data[0]; - return _result; -} - -int Protocol1PacketHandler::Read2ByteTx(PortHandler *port, UINT8_T id, UINT16_T address) -{ - return ReadTx(port, id, address, 2); -} -int Protocol1PacketHandler::Read2ByteRx(PortHandler *port, UINT16_T *data, UINT8_T *error) -{ - UINT8_T _data[2] = {0}; - int _result = ReadRx(port, 2, _data, error); - if(_result == COMM_SUCCESS) - *data = DXL_MAKEWORD(_data[0], _data[1]); - return _result; -} -int Protocol1PacketHandler::Read2ByteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T *data, UINT8_T *error) -{ - UINT8_T _data[2] = {0}; - int _result = ReadTxRx(port, id, address, 2, _data, error); - if(_result == COMM_SUCCESS) - *data = DXL_MAKEWORD(_data[0], _data[1]); - return _result; -} - -int Protocol1PacketHandler::Read4ByteTx(PortHandler *port, UINT8_T id, UINT16_T address) -{ - return COMM_NOT_AVAILABLE; -} -int Protocol1PacketHandler::Read4ByteRx(PortHandler *port, UINT32_T *data, UINT8_T *error) -{ - return COMM_NOT_AVAILABLE; -} -int Protocol1PacketHandler::Read4ByteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T *data, UINT8_T *error) -{ - return COMM_NOT_AVAILABLE; -} - -int Protocol1PacketHandler::WriteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data) -{ - int _result = COMM_TX_FAIL; - - UINT8_T *txpacket = (UINT8_T *)malloc(length+7); - //UINT8_T *txpacket = new UINT8_T[length+7]; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH] = length+3; - txpacket[PKT_INSTRUCTION] = INST_WRITE; - txpacket[PKT_PARAMETER0] = (UINT8_T)address; - - for(UINT8_T _s = 0; _s < length; _s++) - txpacket[PKT_PARAMETER0+1+_s] = data[_s]; - //memcpy(&txpacket[PKT_PARAMETER0+1], data, length); - - _result = TxPacket(port, txpacket); - port->is_using = false; - - free(txpacket); - //delete[] txpacket; - return _result; -} - -int Protocol1PacketHandler::WriteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error) -{ - int _result = COMM_TX_FAIL; - - UINT8_T *txpacket = (UINT8_T *)malloc(length+6); - //UINT8_T *txpacket = new UINT8_T[length+6]; - UINT8_T rxpacket[6] = {0}; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH] = length+3; - txpacket[PKT_INSTRUCTION] = INST_WRITE; - txpacket[PKT_PARAMETER0] = (UINT8_T)address; - - for(UINT8_T _s = 0; _s < length; _s++) - txpacket[PKT_PARAMETER0+1+_s] = data[_s]; - //memcpy(&txpacket[PKT_PARAMETER0+1], data, length); - - _result = TxRxPacket(port, txpacket, rxpacket, error); - - free(txpacket); - //delete[] txpacket; - return _result; -} - -int Protocol1PacketHandler::Write1ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T data) -{ - UINT8_T _data[1] = { data }; - return WriteTxOnly(port, id, address, 1, _data); -} -int Protocol1PacketHandler::Write1ByteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T data, UINT8_T *error) -{ - UINT8_T _data[1] = { data }; - return WriteTxRx(port, id, address, 1, _data, error); -} - -int Protocol1PacketHandler::Write2ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T data) -{ - UINT8_T _data[2] = { DXL_LOBYTE(data), DXL_HIBYTE(data) }; - return WriteTxOnly(port, id, address, 2, _data); -} -int Protocol1PacketHandler::Write2ByteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T data, UINT8_T *error) -{ - UINT8_T _data[2] = { DXL_LOBYTE(data), DXL_HIBYTE(data) }; - return WriteTxRx(port, id, address, 2, _data, error); -} - -int Protocol1PacketHandler::Write4ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T data) -{ - return COMM_NOT_AVAILABLE; -} -int Protocol1PacketHandler::Write4ByteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T data, UINT8_T *error) -{ - return COMM_NOT_AVAILABLE; -} - -int Protocol1PacketHandler::RegWriteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data) -{ - int _result = COMM_TX_FAIL; - - UINT8_T *txpacket = (UINT8_T *)malloc(length+6); - //UINT8_T *txpacket = new UINT8_T[length+6]; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH] = length+3; - txpacket[PKT_INSTRUCTION] = INST_REG_WRITE; - txpacket[PKT_PARAMETER0] = (UINT8_T)address; - - for(UINT8_T _s = 0; _s < length; _s++) - txpacket[PKT_PARAMETER0+1+_s] = data[_s]; - //memcpy(&txpacket[PKT_PARAMETER0+1], data, length); - - _result = TxPacket(port, txpacket); - port->is_using = false; - - free(txpacket); - //delete[] txpacket; - return _result; -} - -int Protocol1PacketHandler::RegWriteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error) -{ - int _result = COMM_TX_FAIL; - - UINT8_T *txpacket = (UINT8_T *)malloc(length+6); - //UINT8_T *txpacket = new UINT8_T[length+6]; - UINT8_T rxpacket[6] = {0}; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH] = length+3; - txpacket[PKT_INSTRUCTION] = INST_REG_WRITE; - txpacket[PKT_PARAMETER0] = (UINT8_T)address; - - for(UINT8_T _s = 0; _s < length; _s++) - txpacket[PKT_PARAMETER0+1+_s] = data[_s]; - //memcpy(&txpacket[PKT_PARAMETER0+1], data, length); - - _result = TxRxPacket(port, txpacket, rxpacket, error); - - free(txpacket); - //delete[] txpacket; - return _result; -} - -int Protocol1PacketHandler::SyncReadTx(PortHandler *port, UINT16_T start_address, UINT16_T data_length, UINT8_T *param, UINT16_T param_length) -{ - return COMM_NOT_AVAILABLE; -} - -int Protocol1PacketHandler::SyncWriteTxOnly(PortHandler *port, UINT16_T start_address, UINT16_T data_length, UINT8_T *param, UINT16_T param_length) -{ - int _result = COMM_TX_FAIL; - - UINT8_T *txpacket = (UINT8_T *)malloc(param_length+8); // 8: HEADER0 HEADER1 ID LEN INST START_ADDR DATA_LEN ... CHKSUM - //UINT8_T *txpacket = new UINT8_T[param_length + 8]; // 8: HEADER0 HEADER1 ID LEN INST START_ADDR DATA_LEN ... CHKSUM - - txpacket[PKT_ID] = BROADCAST_ID; - txpacket[PKT_LENGTH] = param_length + 4; // 4: INST START_ADDR DATA_LEN ... CHKSUM - txpacket[PKT_INSTRUCTION] = INST_SYNC_WRITE; - txpacket[PKT_PARAMETER0+0] = start_address; - txpacket[PKT_PARAMETER0+1] = data_length; - - for(UINT8_T _s = 0; _s < param_length; _s++) - txpacket[PKT_PARAMETER0+2+_s] = param[_s]; - //memcpy(&txpacket[PKT_PARAMETER0+2], param, param_length); - - _result = TxRxPacket(port, txpacket, 0, 0); - - free(txpacket); - //delete[] txpacket; - return _result; -} - -int Protocol1PacketHandler::BulkReadTx(PortHandler *port, UINT8_T *param, UINT16_T param_length) -{ - int _result = COMM_TX_FAIL; - - UINT8_T *txpacket = (UINT8_T *)malloc(param_length+7); // 7: HEADER0 HEADER1 ID LEN INST 0x00 ... CHKSUM - //UINT8_T *txpacket = new UINT8_T[param_length + 7]; // 7: HEADER0 HEADER1 ID LEN INST 0x00 ... CHKSUM - - txpacket[PKT_ID] = BROADCAST_ID; - txpacket[PKT_LENGTH] = param_length + 3; // 3: INST 0x00 ... CHKSUM - txpacket[PKT_INSTRUCTION] = INST_BULK_READ; - txpacket[PKT_PARAMETER0+0] = 0x00; - - for(UINT8_T _s = 0; _s < param_length; _s++) - txpacket[PKT_PARAMETER0+1+_s] = param[_s]; - //memcpy(&txpacket[PKT_PARAMETER0+1], param, param_length); - - _result = TxPacket(port, txpacket); - if(_result == COMM_SUCCESS) + for (uint8_t s = 0; s < length; s++) { - int _wait_length = 0; - for(int _i = 0; _i < param_length; _i += 3) - _wait_length += param[_i] + 7; - port->SetPacketTimeout((UINT16_T)_wait_length); + data[s] = rxpacket[PKT_PARAMETER0 + s]; } + //memcpy(data, &rxpacket[PKT_PARAMETER0], length); + } - free(txpacket); - //delete[] txpacket; - return _result; + free(rxpacket); + //delete[] rxpacket; + return result; } -int Protocol1PacketHandler::BulkWriteTxOnly(PortHandler *port, UINT8_T *param, UINT16_T param_length) +int Protocol1PacketHandler::read1ByteTx(PortHandler *port, uint8_t id, uint16_t address) { - return COMM_NOT_AVAILABLE; + return readTx(port, id, address, 1); +} +int Protocol1PacketHandler::read1ByteRx(PortHandler *port, uint8_t *data, uint8_t *error) +{ + uint8_t data_read[1] = {0}; + int result = readRx(port, 1, data_read, error); + if (result == COMM_SUCCESS) + *data = data_read[0]; + return result; +} +int Protocol1PacketHandler::read1ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint8_t *data, uint8_t *error) +{ + uint8_t data_read[1] = {0}; + int result = readTxRx(port, id, address, 1, data_read, error); + if (result == COMM_SUCCESS) + *data = data_read[0]; + return result; +} + +int Protocol1PacketHandler::read2ByteTx(PortHandler *port, uint8_t id, uint16_t address) +{ + return readTx(port, id, address, 2); +} +int Protocol1PacketHandler::read2ByteRx(PortHandler *port, uint16_t *data, uint8_t *error) +{ + uint8_t data_read[2] = {0}; + int result = readRx(port, 2, data_read, error); + if (result == COMM_SUCCESS) + *data = DXL_MAKEWORD(data_read[0], data_read[1]); + return result; +} +int Protocol1PacketHandler::read2ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t *data, uint8_t *error) +{ + uint8_t data_read[2] = {0}; + int result = readTxRx(port, id, address, 2, data_read, error); + if (result == COMM_SUCCESS) + *data = DXL_MAKEWORD(data_read[0], data_read[1]); + return result; +} + +int Protocol1PacketHandler::read4ByteTx(PortHandler *port, uint8_t id, uint16_t address) +{ + return COMM_NOT_AVAILABLE; +} +int Protocol1PacketHandler::read4ByteRx(PortHandler *port, uint32_t *data, uint8_t *error) +{ + return COMM_NOT_AVAILABLE; +} +int Protocol1PacketHandler::read4ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint32_t *data, uint8_t *error) +{ + return COMM_NOT_AVAILABLE; +} + +int Protocol1PacketHandler::writeTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data) +{ + int result = COMM_TX_FAIL; + + uint8_t *txpacket = (uint8_t *)malloc(length+7); + //uint8_t *txpacket = new uint8_t[length+7]; + + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH] = length+3; + txpacket[PKT_INSTRUCTION] = INST_WRITE; + txpacket[PKT_PARAMETER0] = (uint8_t)address; + + for (uint8_t s = 0; s < length; s++) + txpacket[PKT_PARAMETER0+1+s] = data[s]; + //memcpy(&txpacket[PKT_PARAMETER0+1], data, length); + + result = txPacket(port, txpacket); + port->is_using_ = false; + + free(txpacket); + //delete[] txpacket; + return result; +} + +int Protocol1PacketHandler::writeTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error) +{ + int result = COMM_TX_FAIL; + + uint8_t *txpacket = (uint8_t *)malloc(length+7); //#6->7 + //uint8_t *txpacket = new uint8_t[length+7]; + uint8_t rxpacket[6] = {0}; + + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH] = length+3; + txpacket[PKT_INSTRUCTION] = INST_WRITE; + txpacket[PKT_PARAMETER0] = (uint8_t)address; + + for (uint8_t s = 0; s < length; s++) + txpacket[PKT_PARAMETER0+1+s] = data[s]; + //memcpy(&txpacket[PKT_PARAMETER0+1], data, length); + + result = txRxPacket(port, txpacket, rxpacket, error); + + free(txpacket); + //delete[] txpacket; + return result; +} + +int Protocol1PacketHandler::write1ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint8_t data) +{ + uint8_t data_write[1] = { data }; + return writeTxOnly(port, id, address, 1, data_write); +} +int Protocol1PacketHandler::write1ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint8_t data, uint8_t *error) +{ + uint8_t data_write[1] = { data }; + return writeTxRx(port, id, address, 1, data_write, error); +} + +int Protocol1PacketHandler::write2ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint16_t data) +{ + uint8_t data_write[2] = { DXL_LOBYTE(data), DXL_HIBYTE(data) }; + return writeTxOnly(port, id, address, 2, data_write); +} +int Protocol1PacketHandler::write2ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t data, uint8_t *error) +{ + uint8_t data_write[2] = { DXL_LOBYTE(data), DXL_HIBYTE(data) }; + return writeTxRx(port, id, address, 2, data_write, error); +} + +int Protocol1PacketHandler::write4ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint32_t data) +{ + return COMM_NOT_AVAILABLE; +} +int Protocol1PacketHandler::write4ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint32_t data, uint8_t *error) +{ + return COMM_NOT_AVAILABLE; +} + +int Protocol1PacketHandler::regWriteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data) +{ + int result = COMM_TX_FAIL; + + uint8_t *txpacket = (uint8_t *)malloc(length+6); + //uint8_t *txpacket = new uint8_t[length+6]; + + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH] = length+3; + txpacket[PKT_INSTRUCTION] = INST_REG_WRITE; + txpacket[PKT_PARAMETER0] = (uint8_t)address; + + for (uint8_t s = 0; s < length; s++) + txpacket[PKT_PARAMETER0+1+s] = data[s]; + //memcpy(&txpacket[PKT_PARAMETER0+1], data, length); + + result = txPacket(port, txpacket); + port->is_using_ = false; + + free(txpacket); + //delete[] txpacket; + return result; +} + +int Protocol1PacketHandler::regWriteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error) +{ + int result = COMM_TX_FAIL; + + uint8_t *txpacket = (uint8_t *)malloc(length+6); + //uint8_t *txpacket = new uint8_t[length+6]; + uint8_t rxpacket[6] = {0}; + + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH] = length+3; + txpacket[PKT_INSTRUCTION] = INST_REG_WRITE; + txpacket[PKT_PARAMETER0] = (uint8_t)address; + + for (uint8_t s = 0; s < length; s++) + txpacket[PKT_PARAMETER0+1+s] = data[s]; + //memcpy(&txpacket[PKT_PARAMETER0+1], data, length); + + result = txRxPacket(port, txpacket, rxpacket, error); + + free(txpacket); + //delete[] txpacket; + return result; +} + +int Protocol1PacketHandler::syncReadTx(PortHandler *port, uint16_t start_address, uint16_t data_length, uint8_t *param, uint16_t param_length) +{ + return COMM_NOT_AVAILABLE; +} + +int Protocol1PacketHandler::syncWriteTxOnly(PortHandler *port, uint16_t start_address, uint16_t data_length, uint8_t *param, uint16_t param_length) +{ + int result = COMM_TX_FAIL; + + uint8_t *txpacket = (uint8_t *)malloc(param_length+8); + // 8: HEADER0 HEADER1 ID LEN INST START_ADDR DATA_LEN ... CHKSUM + //uint8_t *txpacket = new uint8_t[param_length + 8]; + + txpacket[PKT_ID] = BROADCAST_ID; + txpacket[PKT_LENGTH] = param_length + 4; // 4: INST START_ADDR DATA_LEN ... CHKSUM + txpacket[PKT_INSTRUCTION] = INST_SYNC_WRITE; + txpacket[PKT_PARAMETER0+0] = start_address; + txpacket[PKT_PARAMETER0+1] = data_length; + + for (uint8_t s = 0; s < param_length; s++) + txpacket[PKT_PARAMETER0+2+s] = param[s]; + //memcpy(&txpacket[PKT_PARAMETER0+2], param, param_length); + + result = txRxPacket(port, txpacket, 0, 0); + + free(txpacket); + //delete[] txpacket; + return result; +} + +int Protocol1PacketHandler::bulkReadTx(PortHandler *port, uint8_t *param, uint16_t param_length) +{ + int result = COMM_TX_FAIL; + + uint8_t *txpacket = (uint8_t *)malloc(param_length+7); + // 7: HEADER0 HEADER1 ID LEN INST 0x00 ... CHKSUM + //uint8_t *txpacket = new uint8_t[param_length + 7]; + + txpacket[PKT_ID] = BROADCAST_ID; + txpacket[PKT_LENGTH] = param_length + 3; // 3: INST 0x00 ... CHKSUM + txpacket[PKT_INSTRUCTION] = INST_BULK_READ; + txpacket[PKT_PARAMETER0+0] = 0x00; + + for (uint8_t s = 0; s < param_length; s++) + txpacket[PKT_PARAMETER0+1+s] = param[s]; + //memcpy(&txpacket[PKT_PARAMETER0+1], param, param_length); + + result = txPacket(port, txpacket); + if (result == COMM_SUCCESS) + { + int wait_length = 0; + for (int i = 0; i < param_length; i += 3) + wait_length += param[i] + 7; + port->setPacketTimeout((uint16_t)wait_length); + } + + free(txpacket); + //delete[] txpacket; + return result; +} + +int Protocol1PacketHandler::bulkWriteTxOnly(PortHandler *port, uint8_t *param, uint16_t param_length) +{ + return COMM_NOT_AVAILABLE; } diff --git a/dynamixel_sdk/src/dynamixel_sdk/protocol2_packet_handler.cpp b/dynamixel_sdk/src/dynamixel_sdk/protocol2_packet_handler.cpp index 771fee7..f814229 100644 --- a/dynamixel_sdk/src/dynamixel_sdk/protocol2_packet_handler.cpp +++ b/dynamixel_sdk/src/dynamixel_sdk/protocol2_packet_handler.cpp @@ -1,8 +1,39 @@ +/******************************************************************************* +* Copyright (c) 2016, ROBOTIS CO., LTD. +* 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 ROBOTIS 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. +*******************************************************************************/ + +/* Author: zerom, Leon Ryu Woon Jung */ + /* - * Protocol2PacketHandler.cpp + * protocol2_packet_handler.cpp * * Created on: 2016. 1. 26. - * Author: zerom, leon */ #if defined(_WIN32) || defined(_WIN64) @@ -12,7 +43,7 @@ #include #include #include -#include "dynamixel_sdk/Protocol2PacketHandler.h" +#include "dynamixel_sdk/protocol2_packet_handler.h" #define TXPACKET_MAX_LEN (4*1024) #define RXPACKET_MAX_LEN (4*1024) @@ -40,946 +71,966 @@ #define ERRBIT_ALERT 128 //When the device has a problem, this bit is set to 1. Check "Device Status Check" value. -using namespace ROBOTIS; +using namespace dynamixel; Protocol2PacketHandler *Protocol2PacketHandler::unique_instance_ = new Protocol2PacketHandler(); Protocol2PacketHandler::Protocol2PacketHandler() { } -void Protocol2PacketHandler::PrintTxRxResult(int result) +void Protocol2PacketHandler::printTxRxResult(int result) { - switch(result) - { + switch(result) + { case COMM_SUCCESS: - printf("[TxRxResult] Communication success.\n"); - break; + printf("[TxRxResult] Communication success.\n"); + break; case COMM_PORT_BUSY: - printf("[TxRxResult] Port is in use!\n"); - break; + printf("[TxRxResult] Port is in use!\n"); + break; case COMM_TX_FAIL: - printf("[TxRxResult] Failed transmit instruction packet!\n"); - break; + printf("[TxRxResult] Failed transmit instruction packet!\n"); + break; case COMM_RX_FAIL: - printf("[TxRxResult] Failed get status packet from device!\n"); - break; + printf("[TxRxResult] Failed get status packet from device!\n"); + break; case COMM_TX_ERROR: - printf("[TxRxResult] Incorrect instruction packet!\n"); - break; + printf("[TxRxResult] Incorrect instruction packet!\n"); + break; case COMM_RX_WAITING: - printf("[TxRxResult] Now recieving status packet!\n"); - break; + printf("[TxRxResult] Now recieving status packet!\n"); + break; case COMM_RX_TIMEOUT: - printf("[TxRxResult] There is no status packet!\n"); - break; + printf("[TxRxResult] There is no status packet!\n"); + break; case COMM_RX_CORRUPT: - printf("[TxRxResult] Incorrect status packet!\n"); - break; + printf("[TxRxResult] Incorrect status packet!\n"); + break; case COMM_NOT_AVAILABLE: - printf("[TxRxResult] Protocol does not support This function!\n"); - break; + printf("[TxRxResult] Protocol does not support This function!\n"); + break; default: - break; - } + break; + } } -void Protocol2PacketHandler::PrintRxPacketError(UINT8_T error) +void Protocol2PacketHandler::printRxPacketError(uint8_t error) { - if(error & ERRBIT_ALERT) - printf("[RxPacketError] Hardware error occurred. Check the error at Control Table (Hardware Error Status)!\n"); + if (error & ERRBIT_ALERT) + printf("[RxPacketError] Hardware error occurred. Check the error at Control Table (Hardware Error Status)!\n"); - int _error = error & ~ERRBIT_ALERT; + int not_alert_error = error & ~ERRBIT_ALERT; - switch(_error) - { + switch(not_alert_error) + { case 0: - break; + break; case ERRNUM_RESULT_FAIL: - printf("[RxPacketError] Failed to process the instruction packet!\n"); - break; + printf("[RxPacketError] Failed to process the instruction packet!\n"); + break; case ERRNUM_INSTRUCTION: - printf("[RxPacketError] Undefined instruction or incorrect instruction!\n"); - break; + printf("[RxPacketError] Undefined instruction or incorrect instruction!\n"); + break; case ERRNUM_CRC: - printf("[RxPacketError] CRC doesn't match!\n"); - break; + printf("[RxPacketError] CRC doesn't match!\n"); + break; case ERRNUM_DATA_RANGE: - printf("[RxPacketError] The data value is out of range!\n"); - break; + printf("[RxPacketError] The data value is out of range!\n"); + break; case ERRNUM_DATA_LENGTH: - printf("[RxPacketError] The data length does not match as expected!\n"); - break; + printf("[RxPacketError] The data length does not match as expected!\n"); + break; case ERRNUM_DATA_LIMIT: - printf("[RxPacketError] The data value exceeds the limit value!\n"); - break; + printf("[RxPacketError] The data value exceeds the limit value!\n"); + break; case ERRNUM_ACCESS: - printf("[RxPacketError] Writing or Reading is not available to target address!\n"); - break; + printf("[RxPacketError] Writing or Reading is not available to target address!\n"); + break; default: - printf("[RxPacketError] Unknown error code!\n"); - break; - } + printf("[RxPacketError] Unknown error code!\n"); + break; + } } -unsigned short Protocol2PacketHandler::UpdateCRC(UINT16_T crc_accum, UINT8_T *data_blk_ptr, UINT16_T data_blk_size) +unsigned short Protocol2PacketHandler::updateCRC(uint16_t crc_accum, uint8_t *data_blk_ptr, uint16_t data_blk_size) { - UINT16_T i, j; - UINT16_T crc_table[256] = {0x0000, - 0x8005, 0x800F, 0x000A, 0x801B, 0x001E, 0x0014, 0x8011, - 0x8033, 0x0036, 0x003C, 0x8039, 0x0028, 0x802D, 0x8027, - 0x0022, 0x8063, 0x0066, 0x006C, 0x8069, 0x0078, 0x807D, - 0x8077, 0x0072, 0x0050, 0x8055, 0x805F, 0x005A, 0x804B, - 0x004E, 0x0044, 0x8041, 0x80C3, 0x00C6, 0x00CC, 0x80C9, - 0x00D8, 0x80DD, 0x80D7, 0x00D2, 0x00F0, 0x80F5, 0x80FF, - 0x00FA, 0x80EB, 0x00EE, 0x00E4, 0x80E1, 0x00A0, 0x80A5, - 0x80AF, 0x00AA, 0x80BB, 0x00BE, 0x00B4, 0x80B1, 0x8093, - 0x0096, 0x009C, 0x8099, 0x0088, 0x808D, 0x8087, 0x0082, - 0x8183, 0x0186, 0x018C, 0x8189, 0x0198, 0x819D, 0x8197, - 0x0192, 0x01B0, 0x81B5, 0x81BF, 0x01BA, 0x81AB, 0x01AE, - 0x01A4, 0x81A1, 0x01E0, 0x81E5, 0x81EF, 0x01EA, 0x81FB, - 0x01FE, 0x01F4, 0x81F1, 0x81D3, 0x01D6, 0x01DC, 0x81D9, - 0x01C8, 0x81CD, 0x81C7, 0x01C2, 0x0140, 0x8145, 0x814F, - 0x014A, 0x815B, 0x015E, 0x0154, 0x8151, 0x8173, 0x0176, - 0x017C, 0x8179, 0x0168, 0x816D, 0x8167, 0x0162, 0x8123, - 0x0126, 0x012C, 0x8129, 0x0138, 0x813D, 0x8137, 0x0132, - 0x0110, 0x8115, 0x811F, 0x011A, 0x810B, 0x010E, 0x0104, - 0x8101, 0x8303, 0x0306, 0x030C, 0x8309, 0x0318, 0x831D, - 0x8317, 0x0312, 0x0330, 0x8335, 0x833F, 0x033A, 0x832B, - 0x032E, 0x0324, 0x8321, 0x0360, 0x8365, 0x836F, 0x036A, - 0x837B, 0x037E, 0x0374, 0x8371, 0x8353, 0x0356, 0x035C, - 0x8359, 0x0348, 0x834D, 0x8347, 0x0342, 0x03C0, 0x83C5, - 0x83CF, 0x03CA, 0x83DB, 0x03DE, 0x03D4, 0x83D1, 0x83F3, - 0x03F6, 0x03FC, 0x83F9, 0x03E8, 0x83ED, 0x83E7, 0x03E2, - 0x83A3, 0x03A6, 0x03AC, 0x83A9, 0x03B8, 0x83BD, 0x83B7, - 0x03B2, 0x0390, 0x8395, 0x839F, 0x039A, 0x838B, 0x038E, - 0x0384, 0x8381, 0x0280, 0x8285, 0x828F, 0x028A, 0x829B, - 0x029E, 0x0294, 0x8291, 0x82B3, 0x02B6, 0x02BC, 0x82B9, - 0x02A8, 0x82AD, 0x82A7, 0x02A2, 0x82E3, 0x02E6, 0x02EC, - 0x82E9, 0x02F8, 0x82FD, 0x82F7, 0x02F2, 0x02D0, 0x82D5, - 0x82DF, 0x02DA, 0x82CB, 0x02CE, 0x02C4, 0x82C1, 0x8243, - 0x0246, 0x024C, 0x8249, 0x0258, 0x825D, 0x8257, 0x0252, - 0x0270, 0x8275, 0x827F, 0x027A, 0x826B, 0x026E, 0x0264, - 0x8261, 0x0220, 0x8225, 0x822F, 0x022A, 0x823B, 0x023E, - 0x0234, 0x8231, 0x8213, 0x0216, 0x021C, 0x8219, 0x0208, - 0x820D, 0x8207, 0x0202 }; + uint16_t i; + uint16_t crc_table[256] = {0x0000, + 0x8005, 0x800F, 0x000A, 0x801B, 0x001E, 0x0014, 0x8011, + 0x8033, 0x0036, 0x003C, 0x8039, 0x0028, 0x802D, 0x8027, + 0x0022, 0x8063, 0x0066, 0x006C, 0x8069, 0x0078, 0x807D, + 0x8077, 0x0072, 0x0050, 0x8055, 0x805F, 0x005A, 0x804B, + 0x004E, 0x0044, 0x8041, 0x80C3, 0x00C6, 0x00CC, 0x80C9, + 0x00D8, 0x80DD, 0x80D7, 0x00D2, 0x00F0, 0x80F5, 0x80FF, + 0x00FA, 0x80EB, 0x00EE, 0x00E4, 0x80E1, 0x00A0, 0x80A5, + 0x80AF, 0x00AA, 0x80BB, 0x00BE, 0x00B4, 0x80B1, 0x8093, + 0x0096, 0x009C, 0x8099, 0x0088, 0x808D, 0x8087, 0x0082, + 0x8183, 0x0186, 0x018C, 0x8189, 0x0198, 0x819D, 0x8197, + 0x0192, 0x01B0, 0x81B5, 0x81BF, 0x01BA, 0x81AB, 0x01AE, + 0x01A4, 0x81A1, 0x01E0, 0x81E5, 0x81EF, 0x01EA, 0x81FB, + 0x01FE, 0x01F4, 0x81F1, 0x81D3, 0x01D6, 0x01DC, 0x81D9, + 0x01C8, 0x81CD, 0x81C7, 0x01C2, 0x0140, 0x8145, 0x814F, + 0x014A, 0x815B, 0x015E, 0x0154, 0x8151, 0x8173, 0x0176, + 0x017C, 0x8179, 0x0168, 0x816D, 0x8167, 0x0162, 0x8123, + 0x0126, 0x012C, 0x8129, 0x0138, 0x813D, 0x8137, 0x0132, + 0x0110, 0x8115, 0x811F, 0x011A, 0x810B, 0x010E, 0x0104, + 0x8101, 0x8303, 0x0306, 0x030C, 0x8309, 0x0318, 0x831D, + 0x8317, 0x0312, 0x0330, 0x8335, 0x833F, 0x033A, 0x832B, + 0x032E, 0x0324, 0x8321, 0x0360, 0x8365, 0x836F, 0x036A, + 0x837B, 0x037E, 0x0374, 0x8371, 0x8353, 0x0356, 0x035C, + 0x8359, 0x0348, 0x834D, 0x8347, 0x0342, 0x03C0, 0x83C5, + 0x83CF, 0x03CA, 0x83DB, 0x03DE, 0x03D4, 0x83D1, 0x83F3, + 0x03F6, 0x03FC, 0x83F9, 0x03E8, 0x83ED, 0x83E7, 0x03E2, + 0x83A3, 0x03A6, 0x03AC, 0x83A9, 0x03B8, 0x83BD, 0x83B7, + 0x03B2, 0x0390, 0x8395, 0x839F, 0x039A, 0x838B, 0x038E, + 0x0384, 0x8381, 0x0280, 0x8285, 0x828F, 0x028A, 0x829B, + 0x029E, 0x0294, 0x8291, 0x82B3, 0x02B6, 0x02BC, 0x82B9, + 0x02A8, 0x82AD, 0x82A7, 0x02A2, 0x82E3, 0x02E6, 0x02EC, + 0x82E9, 0x02F8, 0x82FD, 0x82F7, 0x02F2, 0x02D0, 0x82D5, + 0x82DF, 0x02DA, 0x82CB, 0x02CE, 0x02C4, 0x82C1, 0x8243, + 0x0246, 0x024C, 0x8249, 0x0258, 0x825D, 0x8257, 0x0252, + 0x0270, 0x8275, 0x827F, 0x027A, 0x826B, 0x026E, 0x0264, + 0x8261, 0x0220, 0x8225, 0x822F, 0x022A, 0x823B, 0x023E, + 0x0234, 0x8231, 0x8213, 0x0216, 0x021C, 0x8219, 0x0208, + 0x820D, 0x8207, 0x0202 }; - for(j = 0; j < data_blk_size; j++) - { - i = ((UINT16_T)(crc_accum >> 8) ^ *data_blk_ptr++) & 0xFF; - crc_accum = (crc_accum << 8) ^ crc_table[i]; - } + for (uint16_t j = 0; j < data_blk_size; j++) + { + i = ((uint16_t)(crc_accum >> 8) ^ *data_blk_ptr++) & 0xFF; + crc_accum = (crc_accum << 8) ^ crc_table[i]; + } - return crc_accum; + return crc_accum; } -void Protocol2PacketHandler::AddStuffing(UINT8_T *packet) +void Protocol2PacketHandler::addStuffing(uint8_t *packet) { - int i = 0, index = 0; - int packet_length_in = DXL_MAKEWORD(packet[PKT_LENGTH_L], packet[PKT_LENGTH_H]); - int packet_length_out = packet_length_in; - UINT8_T temp[TXPACKET_MAX_LEN] = {0}; + int i = 0, index = 0; + int packet_length_in = DXL_MAKEWORD(packet[PKT_LENGTH_L], packet[PKT_LENGTH_H]); + int packet_length_out = packet_length_in; + uint8_t temp[TXPACKET_MAX_LEN] = {0}; - for(UINT8_T _s = PKT_HEADER0; _s <= PKT_LENGTH_H; _s++) - temp[_s] = packet[_s]; // FF FF FD XX ID LEN_L LEN_H - //memcpy(temp, packet, PKT_LENGTH_H+1); - index = PKT_INSTRUCTION; - for( i = 0; i < packet_length_in - 2; i++) // except CRC - { - temp[index++] = packet[i+PKT_INSTRUCTION]; - if(packet[i+PKT_INSTRUCTION] == 0xFD && packet[i+PKT_INSTRUCTION-1] == 0xFF && packet[i+PKT_INSTRUCTION-2] == 0xFF) - { // FF FF FD - temp[index++] = 0xFD; - packet_length_out++; - } + for (uint8_t s = PKT_HEADER0; s <= PKT_LENGTH_H; s++) + temp[s] = packet[s]; // FF FF FD XX ID LEN_L LEN_H + //memcpy(temp, packet, PKT_LENGTH_H+1); + index = PKT_INSTRUCTION; + for (i = 0; i < packet_length_in - 2; i++) // except CRC + { + temp[index++] = packet[i+PKT_INSTRUCTION]; + if (packet[i+PKT_INSTRUCTION] == 0xFD && packet[i+PKT_INSTRUCTION-1] == 0xFF && packet[i+PKT_INSTRUCTION-2] == 0xFF) + { // FF FF FD + temp[index++] = 0xFD; + packet_length_out++; } - temp[index++] = packet[PKT_INSTRUCTION+packet_length_in-2]; - temp[index++] = packet[PKT_INSTRUCTION+packet_length_in-1]; + } + temp[index++] = packet[PKT_INSTRUCTION+packet_length_in-2]; + temp[index++] = packet[PKT_INSTRUCTION+packet_length_in-1]; - ////////////////////////// - if(packet_length_in != packet_length_out) - packet = (UINT8_T *)realloc(packet, index * sizeof(UINT8_T)); + ////////////////////////// + if (packet_length_in != packet_length_out) + packet = (uint8_t *)realloc(packet, index * sizeof(uint8_t)); - /////////////////////////// + /////////////////////////// - for(UINT8_T _s = 0; _s < index; _s++) - packet[_s] = temp[_s]; - //memcpy(packet, temp, index); - packet[PKT_LENGTH_L] = DXL_LOBYTE(packet_length_out); - packet[PKT_LENGTH_H] = DXL_HIBYTE(packet_length_out); + for (uint8_t s = 0; s < index; s++) + packet[s] = temp[s]; + //memcpy(packet, temp, index); + packet[PKT_LENGTH_L] = DXL_LOBYTE(packet_length_out); + packet[PKT_LENGTH_H] = DXL_HIBYTE(packet_length_out); } -void Protocol2PacketHandler::RemoveStuffing(UINT8_T *packet) +void Protocol2PacketHandler::removeStuffing(uint8_t *packet) { - int i = 0, index = 0; - int packet_length_in = DXL_MAKEWORD(packet[PKT_LENGTH_L], packet[PKT_LENGTH_H]); - int packet_length_out = packet_length_in; + int i = 0, index = 0; + int packet_length_in = DXL_MAKEWORD(packet[PKT_LENGTH_L], packet[PKT_LENGTH_H]); + int packet_length_out = packet_length_in; - index = PKT_INSTRUCTION; - for( i = 0; i < packet_length_in - 2; i++) // except CRC - { - if(packet[i+PKT_INSTRUCTION] == 0xFD && packet[i+PKT_INSTRUCTION+1] == 0xFD && packet[i+PKT_INSTRUCTION-1] == 0xFF && packet[i+PKT_INSTRUCTION-2] == 0xFF) - { // FF FF FD FD - packet_length_out--; - i++; - } - packet[index++] = packet[i+PKT_INSTRUCTION]; + index = PKT_INSTRUCTION; + for (i = 0; i < packet_length_in - 2; i++) // except CRC + { + if (packet[i+PKT_INSTRUCTION] == 0xFD && packet[i+PKT_INSTRUCTION+1] == 0xFD && packet[i+PKT_INSTRUCTION-1] == 0xFF && packet[i+PKT_INSTRUCTION-2] == 0xFF) + { // FF FF FD FD + packet_length_out--; + i++; } - packet[index++] = packet[PKT_INSTRUCTION+packet_length_in-2]; - packet[index++] = packet[PKT_INSTRUCTION+packet_length_in-1]; + packet[index++] = packet[i+PKT_INSTRUCTION]; + } + packet[index++] = packet[PKT_INSTRUCTION+packet_length_in-2]; + packet[index++] = packet[PKT_INSTRUCTION+packet_length_in-1]; - packet[PKT_LENGTH_L] = DXL_LOBYTE(packet_length_out); - packet[PKT_LENGTH_H] = DXL_HIBYTE(packet_length_out); + packet[PKT_LENGTH_L] = DXL_LOBYTE(packet_length_out); + packet[PKT_LENGTH_H] = DXL_HIBYTE(packet_length_out); } -int Protocol2PacketHandler::TxPacket(PortHandler *port, UINT8_T *txpacket) +int Protocol2PacketHandler::txPacket(PortHandler *port, uint8_t *txpacket) { - UINT16_T _total_packet_length = 0; - UINT16_T _written_packet_length = 0; + uint16_t total_packet_length = 0; + uint16_t written_packet_length = 0; - if(port->is_using) - return COMM_PORT_BUSY; - port->is_using = true; + if (port->is_using_) + return COMM_PORT_BUSY; + port->is_using_ = true; - // byte stuffing for header - AddStuffing(txpacket); + // byte stuffing for header + addStuffing(txpacket); - // check max packet length - _total_packet_length = DXL_MAKEWORD(txpacket[PKT_LENGTH_L], txpacket[PKT_LENGTH_H]) + 7; - // 7: HEADER0 HEADER1 HEADER2 RESERVED ID LENGTH_L LENGTH_H - if(_total_packet_length > TXPACKET_MAX_LEN) - { - port->is_using = false; - return COMM_TX_ERROR; - } + // check max packet length + total_packet_length = DXL_MAKEWORD(txpacket[PKT_LENGTH_L], txpacket[PKT_LENGTH_H]) + 7; + // 7: HEADER0 HEADER1 HEADER2 RESERVED ID LENGTH_L LENGTH_H + if (total_packet_length > TXPACKET_MAX_LEN) + { + port->is_using_ = false; + return COMM_TX_ERROR; + } - // make packet header - txpacket[PKT_HEADER0] = 0xFF; - txpacket[PKT_HEADER1] = 0xFF; - txpacket[PKT_HEADER2] = 0xFD; - txpacket[PKT_RESERVED] = 0x00; + // make packet header + txpacket[PKT_HEADER0] = 0xFF; + txpacket[PKT_HEADER1] = 0xFF; + txpacket[PKT_HEADER2] = 0xFD; + txpacket[PKT_RESERVED] = 0x00; - // add CRC16 - UINT16_T crc = UpdateCRC(0, txpacket, _total_packet_length - 2); // 2: CRC16 - txpacket[_total_packet_length - 2] = DXL_LOBYTE(crc); - txpacket[_total_packet_length - 1] = DXL_HIBYTE(crc); + // add CRC16 + uint16_t crc = updateCRC(0, txpacket, total_packet_length - 2); // 2: CRC16 + txpacket[total_packet_length - 2] = DXL_LOBYTE(crc); + txpacket[total_packet_length - 1] = DXL_HIBYTE(crc); - // tx packet - port->ClearPort(); - _written_packet_length = port->WritePort(txpacket, _total_packet_length); - if(_total_packet_length != _written_packet_length) - { - port->is_using = false; - return COMM_TX_FAIL; - } + // tx packet + port->clearPort(); + written_packet_length = port->writePort(txpacket, total_packet_length); + if (total_packet_length != written_packet_length) + { + port->is_using_ = false; + return COMM_TX_FAIL; + } - return COMM_SUCCESS; + return COMM_SUCCESS; } -int Protocol2PacketHandler::RxPacket(PortHandler *port, UINT8_T *rxpacket) +int Protocol2PacketHandler::rxPacket(PortHandler *port, uint8_t *rxpacket) { - int _result = COMM_TX_FAIL; + int result = COMM_TX_FAIL; - UINT16_T _rx_length = 0; - UINT16_T _wait_length = 11; - // minimum length ( HEADER0 HEADER1 HEADER2 RESERVED ID LENGTH_L LENGTH_H INST ERROR CRC16_L CRC16_H ) + uint16_t rx_length = 0; + uint16_t wait_length = 11; // minimum length (HEADER0 HEADER1 HEADER2 RESERVED ID LENGTH_L LENGTH_H INST ERROR CRC16_L CRC16_H) - while(true) + while(true) + { + rx_length += port->readPort(&rxpacket[rx_length], wait_length - rx_length); + if (rx_length >= wait_length) { - _rx_length += port->ReadPort(&rxpacket[_rx_length], _wait_length - _rx_length); - if(_rx_length >= _wait_length) + uint16_t idx = 0; + + // find packet header + for (idx = 0; idx < (rx_length - 3); idx++) + { + if ((rxpacket[idx] == 0xFF) && (rxpacket[idx+1] == 0xFF) && (rxpacket[idx+2] == 0xFD) && (rxpacket[idx+3] != 0xFD)) + break; + } + + if (idx == 0) // found at the beginning of the packet + { + if (rxpacket[PKT_RESERVED] != 0x00 || + rxpacket[PKT_ID] > 0xFC || + DXL_MAKEWORD(rxpacket[PKT_LENGTH_L], rxpacket[PKT_LENGTH_H]) > RXPACKET_MAX_LEN || + rxpacket[PKT_INSTRUCTION] != 0x55) { - UINT16_T _idx = 0; + // remove the first byte in the packet + for (uint8_t s = 0; s < rx_length - 1; s++) + rxpacket[s] = rxpacket[1 + s]; + //memcpy(&rxpacket[0], &rxpacket[idx], rx_length - idx); + rx_length -= 1; + continue; + } - // find packet header - for(_idx = 0; _idx < (_rx_length - 3); _idx++) + // re-calculate the exact length of the rx packet + if (wait_length != DXL_MAKEWORD(rxpacket[PKT_LENGTH_L], rxpacket[PKT_LENGTH_H]) + PKT_LENGTH_H + 1) + { + wait_length = DXL_MAKEWORD(rxpacket[PKT_LENGTH_L], rxpacket[PKT_LENGTH_H]) + PKT_LENGTH_H + 1; + continue; + } + + if (rx_length < wait_length) + { + // check timeout + if (port->isPacketTimeout() == true) + { + if (rx_length == 0) { - if((rxpacket[_idx] == 0xFF) && (rxpacket[_idx+1] == 0xFF) && (rxpacket[_idx+2] == 0xFD) && (rxpacket[_idx+3] != 0xFD)) - break; - } - - if(_idx == 0) // found at the beginning of the packet - { - if(rxpacket[PKT_RESERVED] != 0x00 || - rxpacket[PKT_ID] > 0xFC || - DXL_MAKEWORD(rxpacket[PKT_LENGTH_L], rxpacket[PKT_LENGTH_H]) > RXPACKET_MAX_LEN || - rxpacket[PKT_INSTRUCTION] != 0x55) - { - // remove the first byte in the packet - for(UINT8_T _s = 0; _s < _rx_length - 1; _s++) - rxpacket[_s] = rxpacket[1 + _s]; - //memcpy(&rxpacket[0], &rxpacket[_idx], _rx_length - _idx); - _rx_length -= 1; - continue; - } - - // re-calculate the exact length of the rx packet - if(_wait_length != DXL_MAKEWORD(rxpacket[PKT_LENGTH_L], rxpacket[PKT_LENGTH_H]) + PKT_LENGTH_H + 1) - { - _wait_length = DXL_MAKEWORD(rxpacket[PKT_LENGTH_L], rxpacket[PKT_LENGTH_H]) + PKT_LENGTH_H + 1; - continue; - } - - if(_rx_length < _wait_length) - { - // check timeout - if(port->IsPacketTimeout() == true) - { - if(_rx_length == 0) - _result = COMM_RX_TIMEOUT; - else - _result = COMM_RX_CORRUPT; - break; - } - else - continue; - } - - // verify CRC16 - UINT16_T crc = DXL_MAKEWORD(rxpacket[_wait_length-2], rxpacket[_wait_length-1]); - if(UpdateCRC(0, rxpacket, _wait_length - 2) == crc) - _result = COMM_SUCCESS; - else - _result = COMM_RX_CORRUPT; - break; + result = COMM_RX_TIMEOUT; } else { - // remove unnecessary packets - for(UINT8_T _s = 0; _s < _rx_length - _idx; _s++) - rxpacket[_s] = rxpacket[_idx + _s]; - //memcpy(&rxpacket[0], &rxpacket[_idx], _rx_length - _idx); - _rx_length -= _idx; + result = COMM_RX_CORRUPT; } + break; + } + else + { + continue; + } + } + + // verify CRC16 + uint16_t crc = DXL_MAKEWORD(rxpacket[wait_length-2], rxpacket[wait_length-1]); + if (updateCRC(0, rxpacket, wait_length - 2) == crc) + { + result = COMM_SUCCESS; } else { - // check timeout - if(port->IsPacketTimeout() == true) - { - if(_rx_length == 0) - _result = COMM_RX_TIMEOUT; - else - _result = COMM_RX_CORRUPT; - break; - } + result = COMM_RX_CORRUPT; } + break; + } + else + { + // remove unnecessary packets + for (uint8_t s = 0; s < rx_length - idx; s++) + rxpacket[s] = rxpacket[idx + s]; + //memcpy(&rxpacket[0], &rxpacket[idx], rx_length - idx); + rx_length -= idx; + } } - port->is_using = false; + else + { + // check timeout + if (port->isPacketTimeout() == true) + { + if (rx_length == 0) + { + result = COMM_RX_TIMEOUT; + } + else + { + result = COMM_RX_CORRUPT; + } + break; + } + } + } + port->is_using_ = false; - if(_result == COMM_SUCCESS) - RemoveStuffing(rxpacket); + if (result == COMM_SUCCESS) + removeStuffing(rxpacket); - return _result; + return result; } // NOT for BulkRead / SyncRead instruction -int Protocol2PacketHandler::TxRxPacket(PortHandler *port, UINT8_T *txpacket, UINT8_T *rxpacket, UINT8_T *error) +int Protocol2PacketHandler::txRxPacket(PortHandler *port, uint8_t *txpacket, uint8_t *rxpacket, uint8_t *error) { - int _result = COMM_TX_FAIL; + int result = COMM_TX_FAIL; - // tx packet - _result = TxPacket(port, txpacket); - if(_result != COMM_SUCCESS) - return _result; + // tx packet + result = txPacket(port, txpacket); + if (result != COMM_SUCCESS) + return result; - // (ID == Broadcast ID && NOT BulkRead) == no need to wait for status packet - // (Instruction == Action) == no need to wait for status packet - if((txpacket[PKT_ID] == BROADCAST_ID && txpacket[PKT_INSTRUCTION] != INST_BULK_READ) || - (txpacket[PKT_ID] == BROADCAST_ID && txpacket[PKT_INSTRUCTION] != INST_SYNC_READ) || - (txpacket[PKT_INSTRUCTION] == INST_ACTION)) + // (ID == Broadcast ID && NOT BulkRead) == no need to wait for status packet + // (Instruction == action) == no need to wait for status packet + if ((txpacket[PKT_ID] == BROADCAST_ID && txpacket[PKT_INSTRUCTION] != INST_BULK_READ) || + (txpacket[PKT_ID] == BROADCAST_ID && txpacket[PKT_INSTRUCTION] != INST_SYNC_READ) || + (txpacket[PKT_INSTRUCTION] == INST_ACTION)) + { + port->is_using_ = false; + return result; + } + + // set packet timeout + if (txpacket[PKT_INSTRUCTION] == INST_READ) + { + port->setPacketTimeout((uint16_t)(DXL_MAKEWORD(txpacket[PKT_PARAMETER0+2], txpacket[PKT_PARAMETER0+3]) + 11)); + } + else + { + port->setPacketTimeout((uint16_t)11); + // HEADER0 HEADER1 HEADER2 RESERVED ID LENGTH_L LENGTH_H INST ERROR CRC16_L CRC16_H + } + + // rx packet + result = rxPacket(port, rxpacket); + // check txpacket ID == rxpacket ID + if (txpacket[PKT_ID] != rxpacket[PKT_ID]) + result = rxPacket(port, rxpacket); + + if (result == COMM_SUCCESS && txpacket[PKT_ID] != BROADCAST_ID) + { + if (error != 0) + *error = (uint8_t)rxpacket[PKT_ERROR]; + } + + return result; +} + +int Protocol2PacketHandler::ping(PortHandler *port, uint8_t id, uint8_t *error) +{ + return ping(port, id, 0, error); +} + +int Protocol2PacketHandler::ping(PortHandler *port, uint8_t id, uint16_t *model_number, uint8_t *error) +{ + int result = COMM_TX_FAIL; + + uint8_t txpacket[10] = {0}; + uint8_t rxpacket[14] = {0}; + + if (id >= BROADCAST_ID) + return COMM_NOT_AVAILABLE; + + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH_L] = 3; + txpacket[PKT_LENGTH_H] = 0; + txpacket[PKT_INSTRUCTION] = INST_PING; + + result = txRxPacket(port, txpacket, rxpacket, error); + if (result == COMM_SUCCESS && model_number != 0) + *model_number = DXL_MAKEWORD(rxpacket[PKT_PARAMETER0+1], rxpacket[PKT_PARAMETER0+2]); + + return result; +} + +int Protocol2PacketHandler::broadcastPing(PortHandler *port, std::vector &id_list) +{ + const int STATUS_LENGTH = 14; + int result = COMM_TX_FAIL; + + id_list.clear(); + + uint16_t rx_length = 0; + uint16_t wait_length = STATUS_LENGTH * MAX_ID; + + uint8_t txpacket[10] = {0}; + uint8_t rxpacket[STATUS_LENGTH * MAX_ID] = {0}; + + txpacket[PKT_ID] = BROADCAST_ID; + txpacket[PKT_LENGTH_L] = 3; + txpacket[PKT_LENGTH_H] = 0; + txpacket[PKT_INSTRUCTION] = INST_PING; + + result = txPacket(port, txpacket); + if (result != COMM_SUCCESS) + { + port->is_using_ = false; + return result; + } + + // set rx timeout + port->setPacketTimeout((uint16_t)(wait_length * 30)); + + while(1) + { + rx_length += port->readPort(&rxpacket[rx_length], wait_length - rx_length); + if (port->isPacketTimeout() == true)// || rx_length >= wait_length) + break; + } + + port->is_using_ = false; + + if (rx_length == 0) + return COMM_RX_TIMEOUT; + + while(1) + { + if (rx_length < STATUS_LENGTH) + return COMM_RX_CORRUPT; + + uint16_t idx = 0; + + // find packet header + for (idx = 0; idx < (rx_length - 2); idx++) { - port->is_using = false; - return _result; + if (rxpacket[idx] == 0xFF && rxpacket[idx+1] == 0xFF && rxpacket[idx+2] == 0xFD) + break; } - // set packet timeout - if(txpacket[PKT_INSTRUCTION] == INST_READ) - port->SetPacketTimeout((UINT16_T)(DXL_MAKEWORD(txpacket[PKT_PARAMETER0+2], txpacket[PKT_PARAMETER0+3]) + 11)); + if (idx == 0) // found at the beginning of the packet + { + // verify CRC16 + uint16_t crc = DXL_MAKEWORD(rxpacket[STATUS_LENGTH-2], rxpacket[STATUS_LENGTH-1]); + + if (updateCRC(0, rxpacket, STATUS_LENGTH - 2) == crc) + { + result = COMM_SUCCESS; + + id_list.push_back(rxpacket[PKT_ID]); + + for (uint8_t s = 0; s < rx_length - STATUS_LENGTH; s++) + rxpacket[s] = rxpacket[STATUS_LENGTH + s]; + rx_length -= STATUS_LENGTH; + + if (rx_length == 0) + return result; + } + else + { + result = COMM_RX_CORRUPT; + + // remove header (0xFF 0xFF 0xFD) + for (uint8_t s = 0; s < rx_length - 3; s++) + rxpacket[s] = rxpacket[3 + s]; + rx_length -= 3; + } + } else - port->SetPacketTimeout((UINT16_T)11); // HEADER0 HEADER1 HEADER2 RESERVED ID LENGTH_L LENGTH_H INST ERROR CRC16_L CRC16_H - - // rx packet - _result = RxPacket(port, rxpacket); - // check txpacket ID == rxpacket ID - if(txpacket[PKT_ID] != rxpacket[PKT_ID]) - _result = RxPacket(port, rxpacket); - - if(_result == COMM_SUCCESS && txpacket[PKT_ID] != BROADCAST_ID) { - if(error != 0) - *error = (UINT8_T)rxpacket[PKT_ERROR]; + // remove unnecessary packets + for (uint8_t s = 0; s < rx_length - idx; s++) + rxpacket[s] = rxpacket[idx + s]; + rx_length -= idx; } + } - return _result; + return result; } -int Protocol2PacketHandler::Ping(PortHandler *port, UINT8_T id, UINT8_T *error) +int Protocol2PacketHandler::action(PortHandler *port, uint8_t id) { - return Ping(port, id, 0, error); + uint8_t txpacket[10] = {0}; + + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH_L] = 3; + txpacket[PKT_LENGTH_H] = 0; + txpacket[PKT_INSTRUCTION] = INST_ACTION; + + return txRxPacket(port, txpacket, 0); } -int Protocol2PacketHandler::Ping(PortHandler *port, UINT8_T id, UINT16_T *model_number, UINT8_T *error) +int Protocol2PacketHandler::reboot(PortHandler *port, uint8_t id, uint8_t *error) { - int _result = COMM_TX_FAIL; + uint8_t txpacket[10] = {0}; + uint8_t rxpacket[11] = {0}; - UINT8_T txpacket[10] = {0}; - UINT8_T rxpacket[14] = {0}; + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH_L] = 3; + txpacket[PKT_LENGTH_H] = 0; + txpacket[PKT_INSTRUCTION] = INST_REBOOT; - if(id >= BROADCAST_ID) - return COMM_NOT_AVAILABLE; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH_L] = 3; - txpacket[PKT_LENGTH_H] = 0; - txpacket[PKT_INSTRUCTION] = INST_PING; - - _result = TxRxPacket(port, txpacket, rxpacket, error); - if(_result == COMM_SUCCESS && model_number != 0) - *model_number = DXL_MAKEWORD(rxpacket[PKT_PARAMETER0+1], rxpacket[PKT_PARAMETER0+2]); - - return _result; + return txRxPacket(port, txpacket, rxpacket, error); } -int Protocol2PacketHandler::BroadcastPing(PortHandler *port, std::vector &id_list) +int Protocol2PacketHandler::factoryReset(PortHandler *port, uint8_t id, uint8_t option, uint8_t *error) { - const int STATUS_LENGTH = 14; - int _result = COMM_TX_FAIL; + uint8_t txpacket[11] = {0}; + uint8_t rxpacket[11] = {0}; - id_list.clear(); + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH_L] = 4; + txpacket[PKT_LENGTH_H] = 0; + txpacket[PKT_INSTRUCTION] = INST_FACTORY_RESET; + txpacket[PKT_PARAMETER0] = option; - UINT16_T _rx_length = 0; - UINT16_T _wait_length = STATUS_LENGTH * MAX_ID; - - UINT8_T txpacket[10] = {0}; - UINT8_T rxpacket[STATUS_LENGTH * MAX_ID] = {0}; - - txpacket[PKT_ID] = BROADCAST_ID; - txpacket[PKT_LENGTH_L] = 3; - txpacket[PKT_LENGTH_H] = 0; - txpacket[PKT_INSTRUCTION] = INST_PING; - - _result = TxPacket(port, txpacket); - if(_result != COMM_SUCCESS) - { - port->is_using = false; - return _result; - } - - // set rx timeout - port->SetPacketTimeout((UINT16_T)(_wait_length * 30)); - - while(1) - { - _rx_length += port->ReadPort(&rxpacket[_rx_length], _wait_length - _rx_length); - if(port->IsPacketTimeout() == true)// || _rx_length >= _wait_length) - break; - } - - port->is_using = false; - - if(_rx_length == 0) - return COMM_RX_TIMEOUT; - - while(1) - { - if(_rx_length < STATUS_LENGTH) - return COMM_RX_CORRUPT; - - UINT16_T _idx = 0; - - // find packet header - for(_idx = 0; _idx < (_rx_length - 2); _idx++) - { - if(rxpacket[_idx] == 0xFF && rxpacket[_idx+1] == 0xFF && rxpacket[_idx+2] == 0xFD) - break; - } - - if(_idx == 0) // found at the beginning of the packet - { - // verify CRC16 - UINT16_T crc = DXL_MAKEWORD(rxpacket[STATUS_LENGTH-2], rxpacket[STATUS_LENGTH-1]); - - if(UpdateCRC(0, rxpacket, STATUS_LENGTH - 2) == crc) - { - _result = COMM_SUCCESS; - - id_list.push_back(rxpacket[PKT_ID]); - - for(UINT8_T _s = 0; _s < _rx_length - STATUS_LENGTH; _s++) - rxpacket[_s] = rxpacket[STATUS_LENGTH + _s]; - _rx_length -= STATUS_LENGTH; - - if(_rx_length == 0) - return _result; - } - else - { - _result = COMM_RX_CORRUPT; - - // remove header (0xFF 0xFF 0xFD) - for(UINT8_T _s = 0; _s < _rx_length - 3; _s++) - rxpacket[_s] = rxpacket[3 + _s]; - _rx_length -= 3; - } - } - else - { - // remove unnecessary packets - for(UINT8_T _s = 0; _s < _rx_length - _idx; _s++) - rxpacket[_s] = rxpacket[_idx + _s]; - _rx_length -= _idx; - } - } - - return _result; + return txRxPacket(port, txpacket, rxpacket, error); } -int Protocol2PacketHandler::Action(PortHandler *port, UINT8_T id) +int Protocol2PacketHandler::readTx(PortHandler *port, uint8_t id, uint16_t address, uint16_t length) { - UINT8_T txpacket[10] = {0}; + int result = COMM_TX_FAIL; - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH_L] = 3; - txpacket[PKT_LENGTH_H] = 0; - txpacket[PKT_INSTRUCTION] = INST_ACTION; + uint8_t txpacket[14] = {0}; - return TxRxPacket(port, txpacket, 0); + if (id >= BROADCAST_ID) + return COMM_NOT_AVAILABLE; + + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH_L] = 7; + txpacket[PKT_LENGTH_H] = 0; + txpacket[PKT_INSTRUCTION] = INST_READ; + txpacket[PKT_PARAMETER0+0] = (uint8_t)DXL_LOBYTE(address); + txpacket[PKT_PARAMETER0+1] = (uint8_t)DXL_HIBYTE(address); + txpacket[PKT_PARAMETER0+2] = (uint8_t)DXL_LOBYTE(length); + txpacket[PKT_PARAMETER0+3] = (uint8_t)DXL_HIBYTE(length); + + result = txPacket(port, txpacket); + + // set packet timeout + if (result == COMM_SUCCESS) + port->setPacketTimeout((uint16_t)(length + 11)); + + return result; } -int Protocol2PacketHandler::Reboot(PortHandler *port, UINT8_T id, UINT8_T *error) +int Protocol2PacketHandler::readRx(PortHandler *port, uint16_t length, uint8_t *data, uint8_t *error) { - UINT8_T txpacket[10] = {0}; - UINT8_T rxpacket[11] = {0}; + int result = COMM_TX_FAIL; + uint8_t *rxpacket = (uint8_t *)malloc(RXPACKET_MAX_LEN); + //(length + 11 + (length/3)); // (length/3): consider stuffing + //uint8_t *rxpacket = new uint8_t[length + 11 + (length/3)]; // (length/3): consider stuffing - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH_L] = 3; - txpacket[PKT_LENGTH_H] = 0; - txpacket[PKT_INSTRUCTION] = INST_REBOOT; + result = rxPacket(port, rxpacket); + if (result == COMM_SUCCESS) + { + if (error != 0) + *error = (uint8_t)rxpacket[PKT_ERROR]; + for (uint8_t s = 0; s < length; s++) + data[s] = rxpacket[PKT_PARAMETER0 + 1 + s]; + //memcpy(data, &rxpacket[PKT_PARAMETER0+1], length); + } - return TxRxPacket(port, txpacket, rxpacket, error); + free(rxpacket); + //delete[] rxpacket; + return result; } -int Protocol2PacketHandler::FactoryReset(PortHandler *port, UINT8_T id, UINT8_T option, UINT8_T *error) +int Protocol2PacketHandler::readTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error) { - UINT8_T txpacket[11] = {0}; - UINT8_T rxpacket[11] = {0}; + int result = COMM_TX_FAIL; - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH_L] = 4; - txpacket[PKT_LENGTH_H] = 0; - txpacket[PKT_INSTRUCTION] = INST_FACTORY_RESET; - txpacket[PKT_PARAMETER0] = option; + uint8_t txpacket[14] = {0}; + uint8_t *rxpacket = (uint8_t *)malloc(RXPACKET_MAX_LEN); + //(length + 11 + (length/3)); // (length/3): consider stuffing - return TxRxPacket(port, txpacket, rxpacket, error); + if (id >= BROADCAST_ID) + return COMM_NOT_AVAILABLE; + + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH_L] = 7; + txpacket[PKT_LENGTH_H] = 0; + txpacket[PKT_INSTRUCTION] = INST_READ; + txpacket[PKT_PARAMETER0+0] = (uint8_t)DXL_LOBYTE(address); + txpacket[PKT_PARAMETER0+1] = (uint8_t)DXL_HIBYTE(address); + txpacket[PKT_PARAMETER0+2] = (uint8_t)DXL_LOBYTE(length); + txpacket[PKT_PARAMETER0+3] = (uint8_t)DXL_HIBYTE(length); + + result = txRxPacket(port, txpacket, rxpacket, error); + if (result == COMM_SUCCESS) + { + if (error != 0) + *error = (uint8_t)rxpacket[PKT_ERROR]; + for (uint8_t s = 0; s < length; s++) + data[s] = rxpacket[PKT_PARAMETER0 + 1 + s]; + //memcpy(data, &rxpacket[PKT_PARAMETER0+1], length); + } + + free(rxpacket); + //delete[] rxpacket; + return result; } -int Protocol2PacketHandler::ReadTx(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length) +int Protocol2PacketHandler::read1ByteTx(PortHandler *port, uint8_t id, uint16_t address) { - int _result = COMM_TX_FAIL; - - UINT8_T txpacket[14] = {0}; - - if(id >= BROADCAST_ID) - return COMM_NOT_AVAILABLE; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH_L] = 7; - txpacket[PKT_LENGTH_H] = 0; - txpacket[PKT_INSTRUCTION] = INST_READ; - txpacket[PKT_PARAMETER0+0] = (UINT8_T)DXL_LOBYTE(address); - txpacket[PKT_PARAMETER0+1] = (UINT8_T)DXL_HIBYTE(address); - txpacket[PKT_PARAMETER0+2] = (UINT8_T)DXL_LOBYTE(length); - txpacket[PKT_PARAMETER0+3] = (UINT8_T)DXL_HIBYTE(length); - - _result = TxPacket(port, txpacket); - - // set packet timeout - if(_result == COMM_SUCCESS) - port->SetPacketTimeout((UINT16_T)(length + 11)); - - return _result; + return readTx(port, id, address, 1); } - -int Protocol2PacketHandler::ReadRx(PortHandler *port, UINT16_T length, UINT8_T *data, UINT8_T *error) +int Protocol2PacketHandler::read1ByteRx(PortHandler *port, uint8_t *data, uint8_t *error) { - int _result = COMM_TX_FAIL; - UINT8_T *rxpacket = (UINT8_T *)malloc(RXPACKET_MAX_LEN);//(length + 11 + (length/3)); // (length/3): consider stuffing - //UINT8_T *rxpacket = new UINT8_T[length + 11 + (length/3)]; // (length/3): consider stuffing - - _result = RxPacket(port, rxpacket); - if(_result == COMM_SUCCESS) - { - if(error != 0) - *error = (UINT8_T)rxpacket[PKT_ERROR]; - for(UINT8_T _s = 0; _s < length; _s++) - data[_s] = rxpacket[PKT_PARAMETER0 + 1 + _s]; - //memcpy(data, &rxpacket[PKT_PARAMETER0+1], length); - } - - free(rxpacket); - //delete[] rxpacket; - return _result; + uint8_t data_read[1] = {0}; + int result = readRx(port, 1, data_read, error); + if (result == COMM_SUCCESS) + *data = data_read[0]; + return result; } - -int Protocol2PacketHandler::ReadTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error) +int Protocol2PacketHandler::read1ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint8_t *data, uint8_t *error) { - int _result = COMM_TX_FAIL; - - UINT8_T txpacket[14] = {0}; - UINT8_T *rxpacket = (UINT8_T *)malloc(RXPACKET_MAX_LEN);//(length + 11 + (length/3)); // (length/3): consider stuffing - - if(id >= BROADCAST_ID) - return COMM_NOT_AVAILABLE; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH_L] = 7; - txpacket[PKT_LENGTH_H] = 0; - txpacket[PKT_INSTRUCTION] = INST_READ; - txpacket[PKT_PARAMETER0+0] = (UINT8_T)DXL_LOBYTE(address); - txpacket[PKT_PARAMETER0+1] = (UINT8_T)DXL_HIBYTE(address); - txpacket[PKT_PARAMETER0+2] = (UINT8_T)DXL_LOBYTE(length); - txpacket[PKT_PARAMETER0+3] = (UINT8_T)DXL_HIBYTE(length); - - _result = TxRxPacket(port, txpacket, rxpacket, error); - if(_result == COMM_SUCCESS) - { - if(error != 0) - *error = (UINT8_T)rxpacket[PKT_ERROR]; - for(UINT8_T _s = 0; _s < length; _s++) - data[_s] = rxpacket[PKT_PARAMETER0 + 1 + _s]; - //memcpy(data, &rxpacket[PKT_PARAMETER0+1], length); - } - - free(rxpacket); - //delete[] rxpacket; - return _result; + uint8_t data_read[1] = {0}; + int result = readTxRx(port, id, address, 1, data_read, error); + if (result == COMM_SUCCESS) + *data = data_read[0]; + return result; } -int Protocol2PacketHandler::Read1ByteTx(PortHandler *port, UINT8_T id, UINT16_T address) +int Protocol2PacketHandler::read2ByteTx(PortHandler *port, uint8_t id, uint16_t address) { - return ReadTx(port, id, address, 1); + return readTx(port, id, address, 2); } -int Protocol2PacketHandler::Read1ByteRx(PortHandler *port, UINT8_T *data, UINT8_T *error) +int Protocol2PacketHandler::read2ByteRx(PortHandler *port, uint16_t *data, uint8_t *error) { - UINT8_T _data[1] = {0}; - int _result = ReadRx(port, 1, _data, error); - if(_result == COMM_SUCCESS) - *data = _data[0]; - return _result; + uint8_t data_read[2] = {0}; + int result = readRx(port, 2, data_read, error); + if (result == COMM_SUCCESS) + *data = DXL_MAKEWORD(data_read[0], data_read[1]); + return result; } -int Protocol2PacketHandler::Read1ByteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T *data, UINT8_T *error) +int Protocol2PacketHandler::read2ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t *data, uint8_t *error) { - UINT8_T _data[1] = {0}; - int _result = ReadTxRx(port, id, address, 1, _data, error); - if(_result == COMM_SUCCESS) - *data = _data[0]; - return _result; + uint8_t data_read[2] = {0}; + int result = readTxRx(port, id, address, 2, data_read, error); + if (result == COMM_SUCCESS) + *data = DXL_MAKEWORD(data_read[0], data_read[1]); + return result; } -int Protocol2PacketHandler::Read2ByteTx(PortHandler *port, UINT8_T id, UINT16_T address) +int Protocol2PacketHandler::read4ByteTx(PortHandler *port, uint8_t id, uint16_t address) { - return ReadTx(port, id, address, 2); + return readTx(port, id, address, 4); } -int Protocol2PacketHandler::Read2ByteRx(PortHandler *port, UINT16_T *data, UINT8_T *error) +int Protocol2PacketHandler::read4ByteRx(PortHandler *port, uint32_t *data, uint8_t *error) { - UINT8_T _data[2] = {0}; - int _result = ReadRx(port, 2, _data, error); - if(_result == COMM_SUCCESS) - *data = DXL_MAKEWORD(_data[0], _data[1]); - return _result; + uint8_t data_read[4] = {0}; + int result = readRx(port, 4, data_read, error); + if (result == COMM_SUCCESS) + *data = DXL_MAKEDWORD(DXL_MAKEWORD(data_read[0], data_read[1]), DXL_MAKEWORD(data_read[2], data_read[3])); + return result; } -int Protocol2PacketHandler::Read2ByteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T *data, UINT8_T *error) +int Protocol2PacketHandler::read4ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint32_t *data, uint8_t *error) { - UINT8_T _data[2] = {0}; - int _result = ReadTxRx(port, id, address, 2, _data, error); - if(_result == COMM_SUCCESS) - *data = DXL_MAKEWORD(_data[0], _data[1]); - return _result; + uint8_t data_read[4] = {0}; + int result = readTxRx(port, id, address, 4, data_read, error); + if (result == COMM_SUCCESS) + *data = DXL_MAKEDWORD(DXL_MAKEWORD(data_read[0], data_read[1]), DXL_MAKEWORD(data_read[2], data_read[3])); + return result; } -int Protocol2PacketHandler::Read4ByteTx(PortHandler *port, UINT8_T id, UINT16_T address) + +int Protocol2PacketHandler::writeTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data) { - return ReadTx(port, id, address, 4); + int result = COMM_TX_FAIL; + + uint8_t *txpacket = (uint8_t *)malloc(length+12); + //uint8_t *txpacket = new uint8_t[length+12]; + + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length+5); + txpacket[PKT_LENGTH_H] = DXL_HIBYTE(length+5); + txpacket[PKT_INSTRUCTION] = INST_WRITE; + txpacket[PKT_PARAMETER0+0] = (uint8_t)DXL_LOBYTE(address); + txpacket[PKT_PARAMETER0+1] = (uint8_t)DXL_HIBYTE(address); + + for (uint8_t s = 0; s < length; s++) + txpacket[PKT_PARAMETER0+2+s] = data[s]; + //memcpy(&txpacket[PKT_PARAMETER0+2], data, length); + + result = txPacket(port, txpacket); + port->is_using_ = false; + + free(txpacket); + //delete[] txpacket; + return result; } -int Protocol2PacketHandler::Read4ByteRx(PortHandler *port, UINT32_T *data, UINT8_T *error) + +int Protocol2PacketHandler::writeTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error) { - UINT8_T _data[4] = {0}; - int _result = ReadRx(port, 4, _data, error); - if(_result == COMM_SUCCESS) - *data = DXL_MAKEDWORD(DXL_MAKEWORD(_data[0], _data[1]), DXL_MAKEWORD(_data[2], _data[3])); - return _result; + int result = COMM_TX_FAIL; + + uint8_t *txpacket = (uint8_t *)malloc(length + 12); + //uint8_t *txpacket = new uint8_t[length+12]; + uint8_t rxpacket[11] = {0}; + + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length+5); + txpacket[PKT_LENGTH_H] = DXL_HIBYTE(length+5); + txpacket[PKT_INSTRUCTION] = INST_WRITE; + txpacket[PKT_PARAMETER0+0] = (uint8_t)DXL_LOBYTE(address); + txpacket[PKT_PARAMETER0+1] = (uint8_t)DXL_HIBYTE(address); + + for (uint8_t s = 0; s < length; s++) + txpacket[PKT_PARAMETER0+2+s] = data[s]; + //memcpy(&txpacket[PKT_PARAMETER0+2], data, length); + + result = txRxPacket(port, txpacket, rxpacket, error); + + free(txpacket); + //delete[] txpacket; + return result; } -int Protocol2PacketHandler::Read4ByteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T *data, UINT8_T *error) + +int Protocol2PacketHandler::write1ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint8_t data) { - UINT8_T _data[4] = {0}; - int _result = ReadTxRx(port, id, address, 4, _data, error); - if(_result == COMM_SUCCESS) - *data = DXL_MAKEDWORD(DXL_MAKEWORD(_data[0], _data[1]), DXL_MAKEWORD(_data[2], _data[3])); - return _result; + uint8_t data_write[1] = { data }; + return writeTxOnly(port, id, address, 1, data_write); } - - -int Protocol2PacketHandler::WriteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data) +int Protocol2PacketHandler::write1ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint8_t data, uint8_t *error) { - int _result = COMM_TX_FAIL; - - UINT8_T *txpacket = (UINT8_T *)malloc(length+12); - //UINT8_T *txpacket = new UINT8_T[length+12]; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length+5); - txpacket[PKT_LENGTH_H] = DXL_HIBYTE(length+5); - txpacket[PKT_INSTRUCTION] = INST_WRITE; - txpacket[PKT_PARAMETER0+0] = (UINT8_T)DXL_LOBYTE(address); - txpacket[PKT_PARAMETER0+1] = (UINT8_T)DXL_HIBYTE(address); - - for(UINT8_T _s = 0; _s < length; _s++) - txpacket[PKT_PARAMETER0+2+_s] = data[_s]; - //memcpy(&txpacket[PKT_PARAMETER0+2], data, length); - - _result = TxPacket(port, txpacket); - port->is_using = false; - - free(txpacket); - //delete[] txpacket; - return _result; + uint8_t data_write[1] = { data }; + return writeTxRx(port, id, address, 1, data_write, error); } -int Protocol2PacketHandler::WriteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error) +int Protocol2PacketHandler::write2ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint16_t data) { - int _result = COMM_TX_FAIL; - - UINT8_T *txpacket = (UINT8_T *)malloc(length + 12); - //UINT8_T *txpacket = new UINT8_T[length+12]; - UINT8_T rxpacket[11] = {0}; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length+5); - txpacket[PKT_LENGTH_H] = DXL_HIBYTE(length+5); - txpacket[PKT_INSTRUCTION] = INST_WRITE; - txpacket[PKT_PARAMETER0+0] = (UINT8_T)DXL_LOBYTE(address); - txpacket[PKT_PARAMETER0+1] = (UINT8_T)DXL_HIBYTE(address); - - for(UINT8_T _s = 0; _s < length; _s++) - txpacket[PKT_PARAMETER0+2+_s] = data[_s]; - //memcpy(&txpacket[PKT_PARAMETER0+2], data, length); - - _result = TxRxPacket(port, txpacket, rxpacket, error); - - free(txpacket); - //delete[] txpacket; - return _result; + uint8_t data_write[2] = { DXL_LOBYTE(data), DXL_HIBYTE(data) }; + return writeTxOnly(port, id, address, 2, data_write); } - -int Protocol2PacketHandler::Write1ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T data) +int Protocol2PacketHandler::write2ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t data, uint8_t *error) { - UINT8_T _data[1] = { data }; - return WriteTxOnly(port, id, address, 1, _data); + uint8_t data_write[2] = { DXL_LOBYTE(data), DXL_HIBYTE(data) }; + return writeTxRx(port, id, address, 2, data_write, error); } -int Protocol2PacketHandler::Write1ByteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T data, UINT8_T *error) + +int Protocol2PacketHandler::write4ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint32_t data) { - UINT8_T _data[1] = { data }; - return WriteTxRx(port, id, address, 1, _data, error); + uint8_t data_write[4] = { DXL_LOBYTE(DXL_LOWORD(data)), DXL_HIBYTE(DXL_LOWORD(data)), DXL_LOBYTE(DXL_HIWORD(data)), DXL_HIBYTE(DXL_HIWORD(data)) }; + return writeTxOnly(port, id, address, 4, data_write); } - -int Protocol2PacketHandler::Write2ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T data) +int Protocol2PacketHandler::write4ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint32_t data, uint8_t *error) { - UINT8_T _data[2] = { DXL_LOBYTE(data), DXL_HIBYTE(data) }; - return WriteTxOnly(port, id, address, 2, _data); + uint8_t data_write[4] = { DXL_LOBYTE(DXL_LOWORD(data)), DXL_HIBYTE(DXL_LOWORD(data)), DXL_LOBYTE(DXL_HIWORD(data)), DXL_HIBYTE(DXL_HIWORD(data)) }; + return writeTxRx(port, id, address, 4, data_write, error); } -int Protocol2PacketHandler::Write2ByteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T data, UINT8_T *error) + +int Protocol2PacketHandler::regWriteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data) { - UINT8_T _data[2] = { DXL_LOBYTE(data), DXL_HIBYTE(data) }; - return WriteTxRx(port, id, address, 2, _data, error); + int result = COMM_TX_FAIL; + + uint8_t *txpacket = (uint8_t *)malloc(length + 12); + //uint8_t *txpacket = new uint8_t[length+12]; + + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length+5); + txpacket[PKT_LENGTH_H] = DXL_HIBYTE(length+5); + txpacket[PKT_INSTRUCTION] = INST_REG_WRITE; + txpacket[PKT_PARAMETER0+0] = (uint8_t)DXL_LOBYTE(address); + txpacket[PKT_PARAMETER0+1] = (uint8_t)DXL_HIBYTE(address); + + for (uint8_t s = 0; s < length; s++) + txpacket[PKT_PARAMETER0+2+s] = data[s]; + //memcpy(&txpacket[PKT_PARAMETER0+2], data, length); + + result = txPacket(port, txpacket); + port->is_using_ = false; + + free(txpacket); + //delete[] txpacket; + return result; } -int Protocol2PacketHandler::Write4ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T data) +int Protocol2PacketHandler::regWriteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error) { - UINT8_T _data[4] = { DXL_LOBYTE(DXL_LOWORD(data)), DXL_HIBYTE(DXL_LOWORD(data)), DXL_LOBYTE(DXL_HIWORD(data)), DXL_HIBYTE(DXL_HIWORD(data)) }; - return WriteTxOnly(port, id, address, 4, _data); + int result = COMM_TX_FAIL; + + uint8_t *txpacket = (uint8_t *)malloc(length + 12); + //uint8_t *txpacket = new uint8_t[length+12]; + uint8_t rxpacket[11] = {0}; + + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length+5); + txpacket[PKT_LENGTH_H] = DXL_HIBYTE(length+5); + txpacket[PKT_INSTRUCTION] = INST_REG_WRITE; + txpacket[PKT_PARAMETER0+0] = (uint8_t)DXL_LOBYTE(address); + txpacket[PKT_PARAMETER0+1] = (uint8_t)DXL_HIBYTE(address); + + for (uint8_t s = 0; s < length; s++) + txpacket[PKT_PARAMETER0+2+s] = data[s]; + //memcpy(&txpacket[PKT_PARAMETER0+2], data, length); + + result = txRxPacket(port, txpacket, rxpacket, error); + + free(txpacket); + //delete[] txpacket; + return result; } -int Protocol2PacketHandler::Write4ByteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T data, UINT8_T *error) + +int Protocol2PacketHandler::syncReadTx(PortHandler *port, uint16_t start_address, uint16_t data_length, uint8_t *param, uint16_t param_length) { - UINT8_T _data[4] = { DXL_LOBYTE(DXL_LOWORD(data)), DXL_HIBYTE(DXL_LOWORD(data)), DXL_LOBYTE(DXL_HIWORD(data)), DXL_HIBYTE(DXL_HIWORD(data)) }; - return WriteTxRx(port, id, address, 4, _data, error); + int result = COMM_TX_FAIL; + + uint8_t *txpacket = (uint8_t *)malloc(param_length + 14); + // 14: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H + + txpacket[PKT_ID] = BROADCAST_ID; + txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H + txpacket[PKT_LENGTH_H] = DXL_HIBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H + txpacket[PKT_INSTRUCTION] = INST_SYNC_READ; + txpacket[PKT_PARAMETER0+0] = DXL_LOBYTE(start_address); + txpacket[PKT_PARAMETER0+1] = DXL_HIBYTE(start_address); + txpacket[PKT_PARAMETER0+2] = DXL_LOBYTE(data_length); + txpacket[PKT_PARAMETER0+3] = DXL_HIBYTE(data_length); + + for (uint8_t s = 0; s < param_length; s++) + txpacket[PKT_PARAMETER0+4+s] = param[s]; + //memcpy(&txpacket[PKT_PARAMETER0+4], param, param_length); + + result = txPacket(port, txpacket); + if (result == COMM_SUCCESS) + port->setPacketTimeout((uint16_t)((11 + data_length) * param_length)); + + free(txpacket); + return result; } -int Protocol2PacketHandler::RegWriteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data) +int Protocol2PacketHandler::syncWriteTxOnly(PortHandler *port, uint16_t start_address, uint16_t data_length, uint8_t *param, uint16_t param_length) { - int _result = COMM_TX_FAIL; + int result = COMM_TX_FAIL; - UINT8_T *txpacket = (UINT8_T *)malloc(length + 12); - //UINT8_T *txpacket = new UINT8_T[length+12]; + uint8_t *txpacket = (uint8_t *)malloc(param_length + 14); + //uint8_t *txpacket = new uint8_t[param_length + 14]; + // 14: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length+5); - txpacket[PKT_LENGTH_H] = DXL_HIBYTE(length+5); - txpacket[PKT_INSTRUCTION] = INST_REG_WRITE; - txpacket[PKT_PARAMETER0+0] = (UINT8_T)DXL_LOBYTE(address); - txpacket[PKT_PARAMETER0+1] = (UINT8_T)DXL_HIBYTE(address); + txpacket[PKT_ID] = BROADCAST_ID; + txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H + txpacket[PKT_LENGTH_H] = DXL_HIBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H + txpacket[PKT_INSTRUCTION] = INST_SYNC_WRITE; + txpacket[PKT_PARAMETER0+0] = DXL_LOBYTE(start_address); + txpacket[PKT_PARAMETER0+1] = DXL_HIBYTE(start_address); + txpacket[PKT_PARAMETER0+2] = DXL_LOBYTE(data_length); + txpacket[PKT_PARAMETER0+3] = DXL_HIBYTE(data_length); - for(UINT8_T _s = 0; _s < length; _s++) - txpacket[PKT_PARAMETER0+2+_s] = data[_s]; - //memcpy(&txpacket[PKT_PARAMETER0+2], data, length); + for (uint8_t s = 0; s < param_length; s++) + txpacket[PKT_PARAMETER0+4+s] = param[s]; + //memcpy(&txpacket[PKT_PARAMETER0+4], param, param_length); - _result = TxPacket(port, txpacket); - port->is_using = false; + result = txRxPacket(port, txpacket, 0, 0); - free(txpacket); - //delete[] txpacket; - return _result; + free(txpacket); + //delete[] txpacket; + return result; } -int Protocol2PacketHandler::RegWriteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error) +int Protocol2PacketHandler::bulkReadTx(PortHandler *port, uint8_t *param, uint16_t param_length) { - int _result = COMM_TX_FAIL; + int result = COMM_TX_FAIL; - UINT8_T *txpacket = (UINT8_T *)malloc(length + 12); - //UINT8_T *txpacket = new UINT8_T[length+12]; - UINT8_T rxpacket[11] = {0}; + uint8_t *txpacket = (uint8_t *)malloc(param_length + 10); + //uint8_t *txpacket = new uint8_t[param_length + 10]; + // 10: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST CRC16_L CRC16_H - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length+5); - txpacket[PKT_LENGTH_H] = DXL_HIBYTE(length+5); - txpacket[PKT_INSTRUCTION] = INST_REG_WRITE; - txpacket[PKT_PARAMETER0+0] = (UINT8_T)DXL_LOBYTE(address); - txpacket[PKT_PARAMETER0+1] = (UINT8_T)DXL_HIBYTE(address); + txpacket[PKT_ID] = BROADCAST_ID; + txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H + txpacket[PKT_LENGTH_H] = DXL_HIBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H + txpacket[PKT_INSTRUCTION] = INST_BULK_READ; - for(UINT8_T _s = 0; _s < length; _s++) - txpacket[PKT_PARAMETER0+2+_s] = data[_s]; - //memcpy(&txpacket[PKT_PARAMETER0+2], data, length); + for (uint8_t s = 0; s < param_length; s++) + txpacket[PKT_PARAMETER0+s] = param[s]; + //memcpy(&txpacket[PKT_PARAMETER0], param, param_length); - _result = TxRxPacket(port, txpacket, rxpacket, error); + result = txPacket(port, txpacket); + if (result == COMM_SUCCESS) + { + int wait_length = 0; + for (int i = 0; i < param_length; i += 5) + wait_length += DXL_MAKEWORD(param[i+3], param[i+4]) + 10; + port->setPacketTimeout((uint16_t)wait_length); + } - free(txpacket); - //delete[] txpacket; - return _result; + free(txpacket); + //delete[] txpacket; + return result; } -int Protocol2PacketHandler::SyncReadTx(PortHandler *port, UINT16_T start_address, UINT16_T data_length, UINT8_T *param, UINT16_T param_length) +int Protocol2PacketHandler::bulkWriteTxOnly(PortHandler *port, uint8_t *param, uint16_t param_length) { - int _result = COMM_TX_FAIL; + int result = COMM_TX_FAIL; - UINT8_T *txpacket = (UINT8_T *)malloc(param_length + 14); - // 14: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H + uint8_t *txpacket = (uint8_t *)malloc(param_length + 10); + //uint8_t *txpacket = new uint8_t[param_length + 10]; + // 10: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST CRC16_L CRC16_H - txpacket[PKT_ID] = BROADCAST_ID; - txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H - txpacket[PKT_LENGTH_H] = DXL_HIBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H - txpacket[PKT_INSTRUCTION] = INST_SYNC_READ; - txpacket[PKT_PARAMETER0+0] = DXL_LOBYTE(start_address); - txpacket[PKT_PARAMETER0+1] = DXL_HIBYTE(start_address); - txpacket[PKT_PARAMETER0+2] = DXL_LOBYTE(data_length); - txpacket[PKT_PARAMETER0+3] = DXL_HIBYTE(data_length); + txpacket[PKT_ID] = BROADCAST_ID; + txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H + txpacket[PKT_LENGTH_H] = DXL_HIBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H + txpacket[PKT_INSTRUCTION] = INST_BULK_WRITE; - for(UINT8_T _s = 0; _s < param_length; _s++) - txpacket[PKT_PARAMETER0+4+_s] = param[_s]; - //memcpy(&txpacket[PKT_PARAMETER0+4], param, param_length); + for (uint8_t s = 0; s < param_length; s++) + txpacket[PKT_PARAMETER0+s] = param[s]; + //memcpy(&txpacket[PKT_PARAMETER0], param, param_length); - _result = TxPacket(port, txpacket); - if(_result == COMM_SUCCESS) - port->SetPacketTimeout((UINT16_T)((11 + data_length) * param_length)); + result = txRxPacket(port, txpacket, 0, 0); - free(txpacket); - return _result; -} - -int Protocol2PacketHandler::SyncWriteTxOnly(PortHandler *port, UINT16_T start_address, UINT16_T data_length, UINT8_T *param, UINT16_T param_length) -{ - int _result = COMM_TX_FAIL; - - UINT8_T *txpacket = (UINT8_T *)malloc(param_length + 14); - //UINT8_T *txpacket = new UINT8_T[param_length + 14]; - // 14: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H - - txpacket[PKT_ID] = BROADCAST_ID; - txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H - txpacket[PKT_LENGTH_H] = DXL_HIBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H - txpacket[PKT_INSTRUCTION] = INST_SYNC_WRITE; - txpacket[PKT_PARAMETER0+0] = DXL_LOBYTE(start_address); - txpacket[PKT_PARAMETER0+1] = DXL_HIBYTE(start_address); - txpacket[PKT_PARAMETER0+2] = DXL_LOBYTE(data_length); - txpacket[PKT_PARAMETER0+3] = DXL_HIBYTE(data_length); - - for(UINT8_T _s = 0; _s < param_length; _s++) - txpacket[PKT_PARAMETER0+4+_s] = param[_s]; - //memcpy(&txpacket[PKT_PARAMETER0+4], param, param_length); - - _result = TxRxPacket(port, txpacket, 0, 0); - - free(txpacket); - //delete[] txpacket; - return _result; -} - -int Protocol2PacketHandler::BulkReadTx(PortHandler *port, UINT8_T *param, UINT16_T param_length) -{ - int _result = COMM_TX_FAIL; - - UINT8_T *txpacket = (UINT8_T *)malloc(param_length + 10); - //UINT8_T *txpacket = new UINT8_T[param_length + 10]; - // 10: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST CRC16_L CRC16_H - - txpacket[PKT_ID] = BROADCAST_ID; - txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H - txpacket[PKT_LENGTH_H] = DXL_HIBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H - txpacket[PKT_INSTRUCTION] = INST_BULK_READ; - - for(UINT8_T _s = 0; _s < param_length; _s++) - txpacket[PKT_PARAMETER0+_s] = param[_s]; - //memcpy(&txpacket[PKT_PARAMETER0], param, param_length); - - _result = TxPacket(port, txpacket); - if(_result == COMM_SUCCESS) - { - int _wait_length = 0; - for(int _i = 0; _i < param_length; _i += 5) - _wait_length += DXL_MAKEWORD(param[_i+3], param[_i+4]) + 10; - port->SetPacketTimeout((UINT16_T)_wait_length); - } - - free(txpacket); - //delete[] txpacket; - return _result; -} - -int Protocol2PacketHandler::BulkWriteTxOnly(PortHandler *port, UINT8_T *param, UINT16_T param_length) -{ - int _result = COMM_TX_FAIL; - - UINT8_T *txpacket = (UINT8_T *)malloc(param_length + 10); - //UINT8_T *txpacket = new UINT8_T[param_length + 10]; - // 10: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST CRC16_L CRC16_H - - txpacket[PKT_ID] = BROADCAST_ID; - txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H - txpacket[PKT_LENGTH_H] = DXL_HIBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H - txpacket[PKT_INSTRUCTION] = INST_BULK_WRITE; - - for(UINT8_T _s = 0; _s < param_length; _s++) - txpacket[PKT_PARAMETER0+_s] = param[_s]; - //memcpy(&txpacket[PKT_PARAMETER0], param, param_length); - - _result = TxRxPacket(port, txpacket, 0, 0); - - free(txpacket); - //delete[] txpacket; - return _result; + free(txpacket); + //delete[] txpacket; + return result; } diff --git a/dynamixel_sdk/src/dynamixel_sdk_linux/port_handler_linux.cpp b/dynamixel_sdk/src/dynamixel_sdk_linux/port_handler_linux.cpp index 96c3f75..0cc11e0 100644 --- a/dynamixel_sdk/src/dynamixel_sdk_linux/port_handler_linux.cpp +++ b/dynamixel_sdk/src/dynamixel_sdk_linux/port_handler_linux.cpp @@ -1,8 +1,39 @@ +/******************************************************************************* +* Copyright (c) 2016, ROBOTIS CO., LTD. +* 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 ROBOTIS 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. +*******************************************************************************/ + +/* Author: zerom, Leon Ryu Woon Jung */ + /* - * PortHandlerLinux.cpp + * port_handler_linux.cpp * * Created on: 2016. 1. 26. - * Author: zerom, leon */ #include @@ -15,231 +46,231 @@ #include #include -#include "dynamixel_sdk_linux/PortHandlerLinux.h" +#include "dynamixel_sdk_linux/port_handler_linux.h" #define LATENCY_TIMER 4 // msec (USB latency timer) -using namespace ROBOTIS; +using namespace dynamixel; PortHandlerLinux::PortHandlerLinux(const char *port_name) - : socket_fd_(-1), - baudrate_(DEFAULT_BAUDRATE), - packet_start_time_(0.0), - packet_timeout_(0.0), - tx_time_per_byte(0.0) + : socket_fd_(-1), + baudrate_(DEFAULT_BAUDRATE_), + packet_start_time_(0.0), + packet_timeout_(0.0), + tx_time_per_byte(0.0) { - is_using = false; - SetPortName(port_name); + is_using_ = false; + setPortName(port_name); } -bool PortHandlerLinux::OpenPort() +bool PortHandlerLinux::openPort() { - return SetBaudRate(baudrate_); + return setBaudRate(baudrate_); } -void PortHandlerLinux::ClosePort() +void PortHandlerLinux::closePort() { - if(socket_fd_ != -1) - close(socket_fd_); - socket_fd_ = -1; + if(socket_fd_ != -1) + close(socket_fd_); + socket_fd_ = -1; } -void PortHandlerLinux::ClearPort() +void PortHandlerLinux::clearPort() { - tcflush(socket_fd_, TCIOFLUSH); + tcflush(socket_fd_, TCIOFLUSH); } -void PortHandlerLinux::SetPortName(const char *port_name) +void PortHandlerLinux::setPortName(const char *port_name) { - strcpy(port_name_, port_name); + strcpy(port_name_, port_name); } -char *PortHandlerLinux::GetPortName() +char *PortHandlerLinux::getPortName() { - return port_name_; + return port_name_; } // TODO: baud number ?? -bool PortHandlerLinux::SetBaudRate(const int baudrate) +bool PortHandlerLinux::setBaudRate(const int baudrate) { - int _baud = GetCFlagBaud(baudrate); + int baud = getCFlagBaud(baudrate); - ClosePort(); + closePort(); - if(_baud <= 0) // custom baudrate - { - SetupPort(B38400); - baudrate_ = baudrate; - return SetCustomBaudrate(baudrate); - } - else - { - baudrate_ = baudrate; - return SetupPort(_baud); - } + if(baud <= 0) // custom baudrate + { + setupPort(B38400); + baudrate_ = baudrate; + return setCustomBaudrate(baudrate); + } + else + { + baudrate_ = baudrate; + return setupPort(baud); + } } -int PortHandlerLinux::GetBaudRate() +int PortHandlerLinux::getBaudRate() { - return baudrate_; + return baudrate_; } -int PortHandlerLinux::GetBytesAvailable() +int PortHandlerLinux::getBytesAvailable() { - int _bytes_available; - ioctl(socket_fd_, FIONREAD, &_bytes_available); - return _bytes_available; + int bytes_available; + ioctl(socket_fd_, FIONREAD, &bytes_available); + return bytes_available; } -int PortHandlerLinux::ReadPort(UINT8_T *packet, int length) +int PortHandlerLinux::readPort(uint8_t *packet, int length) { - return read(socket_fd_, packet, length); + return read(socket_fd_, packet, length); } -int PortHandlerLinux::WritePort(UINT8_T *packet, int length) +int PortHandlerLinux::writePort(uint8_t *packet, int length) { - return write(socket_fd_, packet, length); + return write(socket_fd_, packet, length); } -void PortHandlerLinux::SetPacketTimeout(UINT16_T packet_length) +void PortHandlerLinux::setPacketTimeout(uint16_t packet_length) { - packet_start_time_ = GetCurrentTime(); - packet_timeout_ = (tx_time_per_byte * (double)packet_length) + (LATENCY_TIMER * 2.0) + 2.0; + packet_start_time_ = getCurrentTime(); + packet_timeout_ = (tx_time_per_byte * (double)packet_length) + (LATENCY_TIMER * 2.0) + 2.0; } -void PortHandlerLinux::SetPacketTimeout(double msec) +void PortHandlerLinux::setPacketTimeout(double msec) { - packet_start_time_ = GetCurrentTime(); - packet_timeout_ = msec; + packet_start_time_ = getCurrentTime(); + packet_timeout_ = msec; } -bool PortHandlerLinux::IsPacketTimeout() +bool PortHandlerLinux::isPacketTimeout() { - if(GetTimeSinceStart() > packet_timeout_) - { - packet_timeout_ = 0; - return true; - } + if(getTimeSinceStart() > packet_timeout_) + { + packet_timeout_ = 0; + return true; + } + return false; +} + +double PortHandlerLinux::getCurrentTime() +{ + struct timespec tv; + clock_gettime( CLOCK_REALTIME, &tv); + return ((double)tv.tv_sec*1000.0 + (double)tv.tv_nsec*0.001*0.001); +} + +double PortHandlerLinux::getTimeSinceStart() +{ + double time; + + time = getCurrentTime() - packet_start_time_; + if(time < 0.0) + packet_start_time_ = getCurrentTime(); + + return time; +} + +bool PortHandlerLinux::setupPort(int cflag_baud) +{ + struct termios newtio; + + socket_fd_ = open(port_name_, O_RDWR|O_NOCTTY|O_NONBLOCK); + if(socket_fd_ < 0) + { + printf("[PortHandlerLinux::SetupPort] Error opening serial port!\n"); return false; + } + + bzero(&newtio, sizeof(newtio)); // clear struct for new port settings + + newtio.c_cflag = cflag_baud | CS8 | CLOCAL | CREAD; + newtio.c_iflag = IGNPAR; + newtio.c_oflag = 0; + newtio.c_lflag = 0; + newtio.c_cc[VTIME] = 0; + newtio.c_cc[VMIN] = 0; + + // clean the buffer and activate the settings for the port + tcflush(socket_fd_, TCIFLUSH); + tcsetattr(socket_fd_, TCSANOW, &newtio); + + tx_time_per_byte = (1000.0 / (double)baudrate_) * 10.0; + return true; } -double PortHandlerLinux::GetCurrentTime() +bool PortHandlerLinux::setCustomBaudrate(int speed) { - struct timespec _tv; - clock_gettime( CLOCK_REALTIME, &_tv); - return ((double)_tv.tv_sec*1000.0 + (double)_tv.tv_nsec*0.001*0.001); + // try to set a custom divisor + struct serial_struct ss; + if(ioctl(socket_fd_, TIOCGSERIAL, &ss) != 0) + { + printf("[PortHandlerLinux::SetCustomBaudrate] TIOCGSERIAL failed!\n"); + return false; + } + + ss.flags = (ss.flags & ~ASYNC_SPD_MASK) | ASYNC_SPD_CUST; + ss.custom_divisor = (ss.baud_base + (speed / 2)) / speed; + int closest_speed = ss.baud_base / ss.custom_divisor; + + if(closest_speed < speed * 98 / 100 || closest_speed > speed * 102 / 100) + { + printf("[PortHandlerLinux::SetCustomBaudrate] Cannot set speed to %d, closest is %d \n", speed, closest_speed); + return false; + } + + if(ioctl(socket_fd_, TIOCSSERIAL, &ss) < 0) + { + printf("[PortHandlerLinux::SetCustomBaudrate] TIOCSSERIAL failed!\n"); + return false; + } + + tx_time_per_byte = (1000.0 / (double)speed) * 10.0; + return true; } -double PortHandlerLinux::GetTimeSinceStart() +int PortHandlerLinux::getCFlagBaud(int baudrate) { - double _time; - - _time = GetCurrentTime() - packet_start_time_; - if(_time < 0.0) - packet_start_time_ = GetCurrentTime(); - - return _time; -} - -bool PortHandlerLinux::SetupPort(int cflag_baud) -{ - struct termios newtio; - - socket_fd_ = open(port_name_, O_RDWR|O_NOCTTY|O_NONBLOCK); - if(socket_fd_ < 0) - { - printf("[PortHandlerLinux::SetupPort] Error opening serial port!\n"); - return false; - } - - bzero(&newtio, sizeof(newtio)); // clear struct for new port settings - - newtio.c_cflag = cflag_baud | CS8 | CLOCAL | CREAD; - newtio.c_iflag = IGNPAR; - newtio.c_oflag = 0; - newtio.c_lflag = 0; - newtio.c_cc[VTIME] = 0; - newtio.c_cc[VMIN] = 0; - - // clean the buffer and activate the settings for the port - tcflush(socket_fd_, TCIFLUSH); - tcsetattr(socket_fd_, TCSANOW, &newtio); - - tx_time_per_byte = (1000.0 / (double)baudrate_) * 10.0; - return true; -} - -bool PortHandlerLinux::SetCustomBaudrate(int speed) -{ - // try to set a custom divisor - struct serial_struct ss; - if(ioctl(socket_fd_, TIOCGSERIAL, &ss) != 0) - { - printf("[PortHandlerLinux::SetCustomBaudrate] TIOCGSERIAL failed!\n"); - return false; - } - - ss.flags = (ss.flags & ~ASYNC_SPD_MASK) | ASYNC_SPD_CUST; - ss.custom_divisor = (ss.baud_base + (speed / 2)) / speed; - int closest_speed = ss.baud_base / ss.custom_divisor; - - if(closest_speed < speed * 98 / 100 || closest_speed > speed * 102 / 100) - { - printf("[PortHandlerLinux::SetCustomBaudrate] Cannot set speed to %d, closest is %d \n", speed, closest_speed); - return false; - } - - if(ioctl(socket_fd_, TIOCSSERIAL, &ss) < 0) - { - printf("[PortHandlerLinux::SetCustomBaudrate] TIOCSSERIAL failed!\n"); - return false; - } - - tx_time_per_byte = (1000.0 / (double)speed) * 10.0; - return true; -} - -int PortHandlerLinux::GetCFlagBaud(int baudrate) -{ - switch(baudrate) - { + switch(baudrate) + { case 9600: - return B9600; + return B9600; case 19200: - return B19200; + return B19200; case 38400: - return B38400; + return B38400; case 57600: - return B57600; + return B57600; case 115200: - return B115200; + return B115200; case 230400: - return B230400; + return B230400; case 460800: - return B460800; + return B460800; case 500000: - return B500000; + return B500000; case 576000: - return B576000; + return B576000; case 921600: - return B921600; + return B921600; case 1000000: - return B1000000; + return B1000000; case 1152000: - return B1152000; + return B1152000; case 1500000: - return B1500000; + return B1500000; case 2000000: - return B2000000; + return B2000000; case 2500000: - return B2500000; + return B2500000; case 3000000: - return B3000000; + return B3000000; case 3500000: - return B3500000; + return B3500000; case 4000000: - return B4000000; + return B4000000; default: - return -1; - } + return -1; + } } diff --git a/robotis_controller/CMakeLists.txt b/robotis_controller/CMakeLists.txt index 5865116..42486cc 100644 --- a/robotis_controller/CMakeLists.txt +++ b/robotis_controller/CMakeLists.txt @@ -13,15 +13,6 @@ find_package(catkin REQUIRED COMPONENTS dynamixel_sdk ) -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if you package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need catkin_package( INCLUDE_DIRS include LIBRARIES robotis_controller @@ -29,24 +20,15 @@ catkin_package( # DEPENDS system_lib ) -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -# include_directories(include) include_directories( include ${catkin_INCLUDE_DIRS} ) -## Declare a cpp library add_library(robotis_controller - src/robotis_controller/RobotisController.cpp + src/robotis_controller/robotis_controller.cpp ) -## Specify libraries to link a library or executable target against target_link_libraries(robotis_controller yaml-cpp ${catkin_LIBRARIES} diff --git a/robotis_controller/include/robotis_controller/robotis_controller.h b/robotis_controller/include/robotis_controller/robotis_controller.h index d974a3e..e2c8b64 100644 --- a/robotis_controller/include/robotis_controller/robotis_controller.h +++ b/robotis_controller/include/robotis_controller/robotis_controller.h @@ -1,12 +1,42 @@ +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * 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 ROBOTIS 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. + *******************************************************************************/ + /* - * RobotisController.h + * robotis_controller.h * * Created on: 2016. 1. 15. * Author: zerom */ -#ifndef ROBOTIS_FRAMEWORK_ROBOTIS_CONTROLLER_INCLUDE_ROBOTIS_CONTROLLER_ROBOTISCONTROLLER_H_ -#define ROBOTIS_FRAMEWORK_ROBOTIS_CONTROLLER_INCLUDE_ROBOTIS_CONTROLLER_ROBOTISCONTROLLER_H_ +#ifndef ROBOTIS_CONTROLLER_ROBOTIS_CONTROLLER_H_ +#define ROBOTIS_CONTROLLER_ROBOTIS_CONTROLLER_H_ #include @@ -16,132 +46,131 @@ #include #include -#include "robotis_device/Robot.h" -#include "robotis_framework_common/MotionModule.h" -#include "robotis_framework_common/SensorModule.h" - #include "robotis_controller_msgs/SyncWriteItem.h" #include "robotis_controller_msgs/JointCtrlModule.h" #include "robotis_controller_msgs/GetJointModule.h" -#include "dynamixel_sdk/GroupBulkRead.h" -#include "dynamixel_sdk/GroupSyncWrite.h" +#include "robotis_device/robot.h" +#include "robotis_framework_common/motion_module.h" +#include "robotis_framework_common/sensor_module.h" +#include "dynamixel_sdk/group_bulk_read.h" +#include "dynamixel_sdk/group_sync_write.h" -namespace ROBOTIS +namespace robotis_framework { -enum CONTROLLER_MODE +enum ControllerMode { - MOTION_MODULE_MODE, - DIRECT_CONTROL_MODE + MotionModuleMode, + DirectControlMode }; class RobotisController : public Singleton { private: - boost::thread queue_thread_; - boost::thread gazebo_thread_; - boost::thread set_module_thread_; - boost::mutex queue_mutex_; + boost::thread queue_thread_; + boost::thread gazebo_thread_; + boost::thread set_module_thread_; + boost::mutex queue_mutex_; - bool init_pose_loaded_; - bool is_timer_running_; - bool stop_timer_; - pthread_t timer_thread_; - CONTROLLER_MODE controller_mode_; + bool init_pose_loaded_; + bool is_timer_running_; + bool stop_timer_; + pthread_t timer_thread_; + ControllerMode controller_mode_; - std::list motion_modules_; - std::list sensor_modules_; - std::vector direct_sync_write_; + std::list motion_modules_; + std::list sensor_modules_; + std::vector direct_sync_write_; - std::map sensor_result_; + std::map sensor_result_; - void QueueThread(); - void GazeboThread(); - void SetCtrlModuleThread(std::string ctrl_module); + void gazeboTimerThread(); + void msgQueueThread(); + void setCtrlModuleThread(std::string ctrl_module); - bool CheckTimerStop(); - void InitSyncWrite(); + bool isTimerStopped(); + void initializeSyncWrite(); public: - static const int CONTROL_CYCLE_MSEC = 8; // 8 msec + static const int CONTROL_CYCLE_MSEC = 8; // 8 msec - bool DEBUG_PRINT; - Robot *robot; + bool DEBUG_PRINT; + Robot *robot_; - bool gazebo_mode; - std::string gazebo_robot_name; + bool gazebo_mode_; + std::string gazebo_robot_name_; - /* bulk read */ - std::map port_to_bulk_read; + /* bulk read */ + std::map port_to_bulk_read_; - /* sync write */ - std::map port_to_sync_write_position; - std::map port_to_sync_write_velocity; - std::map port_to_sync_write_torque; - std::map port_to_sync_write_position_p_gain; + /* sync write */ + std::map port_to_sync_write_position_; + std::map port_to_sync_write_velocity_; + std::map port_to_sync_write_torque_; + std::map port_to_sync_write_position_p_gain_; - /* publisher */ - ros::Publisher goal_joint_state_pub; - ros::Publisher present_joint_state_pub; - ros::Publisher current_module_pub; + /* publisher */ + ros::Publisher goal_joint_state_pub_; + ros::Publisher present_joint_state_pub_; + ros::Publisher current_module_pub_; - std::map gazebo_joint_pub; + std::map gazebo_joint_pub_; - static void *ThreadProc(void *param); + static void *timerThread(void *param); - RobotisController(); + RobotisController(); - bool Initialize(const std::string robot_file_path, const std::string init_file_path); - void InitDevice(const std::string init_file_path); - void Process(); + bool initialize(const std::string robot_file_path, const std::string init_file_path); + void initializeDevice(const std::string init_file_path); + void process(); - void AddMotionModule(MotionModule *module); - void RemoveMotionModule(MotionModule *module); - void AddSensorModule(SensorModule *module); - void RemoveSensorModule(SensorModule *module); + void addMotionModule(MotionModule *module); + void removeMotionModule(MotionModule *module); + void addSensorModule(SensorModule *module); + void removeSensorModule(SensorModule *module); - void StartTimer(); - void StopTimer(); - bool IsTimerRunning(); + void startTimer(); + void stopTimer(); + bool isTimerRunning(); - void LoadOffset(const std::string path); + void loadOffset(const std::string path); - /* ROS Topic Callback Functions */ - void SyncWriteItemCallback(const robotis_controller_msgs::SyncWriteItem::ConstPtr &msg); - void SetControllerModeCallback(const std_msgs::String::ConstPtr &msg); - void SetJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg); - void SetJointCtrlModuleCallback(const robotis_controller_msgs::JointCtrlModule::ConstPtr &msg); - void SetCtrlModuleCallback(const std_msgs::String::ConstPtr &msg); - bool GetCtrlModuleCallback(robotis_controller_msgs::GetJointModule::Request &req, robotis_controller_msgs::GetJointModule::Response &res); + /* ROS Topic Callback Functions */ + void syncWriteItemCallback(const robotis_controller_msgs::SyncWriteItem::ConstPtr &msg); + void setControllerModeCallback(const std_msgs::String::ConstPtr &msg); + void setJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg); + void setJointCtrlModuleCallback(const robotis_controller_msgs::JointCtrlModule::ConstPtr &msg); + void setCtrlModuleCallback(const std_msgs::String::ConstPtr &msg); + bool getCtrlModuleCallback(robotis_controller_msgs::GetJointModule::Request &req, robotis_controller_msgs::GetJointModule::Response &res); - void GazeboJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg); + void gazeboJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg); - int Ping (const std::string joint_name, UINT8_T *error = 0); - int Ping (const std::string joint_name, UINT16_T* model_number, UINT8_T *error = 0); + int ping (const std::string joint_name, uint8_t *error = 0); + int ping (const std::string joint_name, uint16_t* model_number, uint8_t *error = 0); - int Action (const std::string joint_name); - int Reboot (const std::string joint_name, UINT8_T *error = 0); - int FactoryReset(const std::string joint_name, UINT8_T option = 0, UINT8_T *error = 0); + int action (const std::string joint_name); + int reboot (const std::string joint_name, uint8_t *error = 0); + int factoryReset(const std::string joint_name, uint8_t option = 0, uint8_t *error = 0); - int Read (const std::string joint_name, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error = 0); - int ReadCtrlItem(const std::string joint_name, const std::string item_name, UINT32_T *data, UINT8_T *error = 0); + int read (const std::string joint_name, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0); + int readCtrlItem(const std::string joint_name, const std::string item_name, uint32_t *data, uint8_t *error = 0); - int Read1Byte (const std::string joint_name, UINT16_T address, UINT8_T *data, UINT8_T *error = 0); - int Read2Byte (const std::string joint_name, UINT16_T address, UINT16_T *data, UINT8_T *error = 0); - int Read4Byte (const std::string joint_name, UINT16_T address, UINT32_T *data, UINT8_T *error = 0); + int read1Byte (const std::string joint_name, uint16_t address, uint8_t *data, uint8_t *error = 0); + int read2Byte (const std::string joint_name, uint16_t address, uint16_t *data, uint8_t *error = 0); + int read4Byte (const std::string joint_name, uint16_t address, uint32_t *data, uint8_t *error = 0); - int Write (const std::string joint_name, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error = 0); - int WriteCtrlItem(const std::string joint_name, const std::string item_name, UINT32_T data, UINT8_T *error = 0); + int write (const std::string joint_name, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0); + int writeCtrlItem(const std::string joint_name, const std::string item_name, uint32_t data, uint8_t *error = 0); - int Write1Byte (const std::string joint_name, UINT16_T address, UINT8_T data, UINT8_T *error = 0); - int Write2Byte (const std::string joint_name, UINT16_T address, UINT16_T data, UINT8_T *error = 0); - int Write4Byte (const std::string joint_name, UINT16_T address, UINT32_T data, UINT8_T *error = 0); + int write1Byte (const std::string joint_name, uint16_t address, uint8_t data, uint8_t *error = 0); + int write2Byte (const std::string joint_name, uint16_t address, uint16_t data, uint8_t *error = 0); + int write4Byte (const std::string joint_name, uint16_t address, uint32_t data, uint8_t *error = 0); - int RegWrite (const std::string joint_name, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error = 0); + int regWrite (const std::string joint_name, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0); }; } -#endif /* ROBOTIS_FRAMEWORK_ROBOTIS_CONTROLLER_INCLUDE_ROBOTIS_CONTROLLER_ROBOTISCONTROLLER_H_ */ +#endif /* ROBOTIS_CONTROLLER_ROBOTIS_CONTROLLER_H_ */ diff --git a/robotis_controller/package.xml b/robotis_controller/package.xml index 8a25f7e..6dcf6d2 100644 --- a/robotis_controller/package.xml +++ b/robotis_controller/package.xml @@ -6,16 +6,12 @@ ROBOTIS - BSD - - ROBOTIS - catkin roscpp diff --git a/robotis_controller/src/robotis_controller/robotis_controller.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp index 97d283a..8d8275d 100644 --- a/robotis_controller/src/robotis_controller/robotis_controller.cpp +++ b/robotis_controller/src/robotis_controller/robotis_controller.cpp @@ -1,5 +1,35 @@ +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * 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 ROBOTIS 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. + *******************************************************************************/ + /* - * RobotisController.cpp + * robotis_controller.cpp * * Created on: 2016. 1. 15. * Author: zerom @@ -8,1781 +38,1853 @@ #include #include -#include "robotis_controller/RobotisController.h" +#include "robotis_controller/robotis_controller.h" -using namespace ROBOTIS; +using namespace robotis_framework; RobotisController::RobotisController() - : is_timer_running_(false), - stop_timer_(false), - init_pose_loaded_(false), - timer_thread_(0), - controller_mode_(MOTION_MODULE_MODE), - DEBUG_PRINT(false), - robot(0), - gazebo_mode(false), - gazebo_robot_name("robotis") + : is_timer_running_(false), + stop_timer_(false), + init_pose_loaded_(false), + timer_thread_(0), + controller_mode_(MotionModuleMode), + DEBUG_PRINT(false), + robot_(0), + gazebo_mode_(false), + gazebo_robot_name_("robotis") { - direct_sync_write_.clear(); + direct_sync_write_.clear(); } -void RobotisController::InitSyncWrite() +void RobotisController::initializeSyncWrite() { - if(gazebo_mode == true) - return; + if (gazebo_mode_ == true) + return; - ROS_INFO("FIRST BULKREAD"); - // bulkread twice - for(std::map::iterator _it = port_to_bulk_read.begin(); _it != port_to_bulk_read.end(); _it++) - _it->second->TxRxPacket(); - for(std::map::iterator _it = port_to_bulk_read.begin(); _it != port_to_bulk_read.end(); _it++) + ROS_INFO("FIRST BULKREAD"); + // bulkread twice + for (std::map::iterator it = port_to_bulk_read_.begin(); + it != port_to_bulk_read_.end(); it++) + { + it->second->txRxPacket(); + } + for (std::map::iterator it = port_to_bulk_read_.begin(); + it != port_to_bulk_read_.end(); it++) + { + int error_count = 0; + int result = COMM_SUCCESS; + do { - int _error_cnt = 0; - int _result = COMM_SUCCESS; - do { - if(++_error_cnt > 10) - { - ROS_ERROR("[RobotisController] bulk read fail!!"); - exit(-1); - } - usleep(10*1000); - _result = _it->second->TxRxPacket(); - } while (_result != COMM_SUCCESS); - } - init_pose_loaded_ = true; - ROS_INFO("FIRST BULKREAD END"); + if (++error_count > 10) + { + ROS_ERROR("[RobotisController] bulk read fail!!"); + exit(-1); + } + usleep(10 * 1000); + result = it->second->txRxPacket(); + } while (result != COMM_SUCCESS); + } + init_pose_loaded_ = true; + ROS_INFO("FIRST BULKREAD END"); - // clear syncwrite param setting - for(std::map::iterator _it = port_to_sync_write_position.begin(); _it != port_to_sync_write_position.end(); _it++) - { - if(_it->second != NULL) - _it->second->ClearParam(); - } - for(std::map::iterator _it = port_to_sync_write_position_p_gain.begin(); _it != port_to_sync_write_position_p_gain.end(); _it++) - { - if(_it->second != NULL) - _it->second->ClearParam(); - } - for(std::map::iterator _it = port_to_sync_write_velocity.begin(); _it != port_to_sync_write_velocity.end(); _it++) - { - if(_it->second != NULL) - _it->second->ClearParam(); - } - for(std::map::iterator _it = port_to_sync_write_torque.begin(); _it != port_to_sync_write_torque.end(); _it++) - { - if(_it->second != NULL) - _it->second->ClearParam(); - } + // clear syncwrite param setting + for (std::map::iterator it = port_to_sync_write_position_.begin(); + it != port_to_sync_write_position_.end(); it++) + { + if (it->second != NULL) + it->second->clearParam(); + } + for (std::map::iterator it = port_to_sync_write_position_p_gain_.begin(); + it != port_to_sync_write_position_p_gain_.end(); it++) + { + if (it->second != NULL) + it->second->clearParam(); + } + for (std::map::iterator it = port_to_sync_write_velocity_.begin(); + it != port_to_sync_write_velocity_.end(); it++) + { + if (it->second != NULL) + it->second->clearParam(); + } + for (std::map::iterator it = port_to_sync_write_torque_.begin(); + it != port_to_sync_write_torque_.end(); it++) + { + if (it->second != NULL) + it->second->clearParam(); + } - // set init syncwrite param(from data of bulkread) - for(std::map::iterator _it = robot->dxls.begin(); _it != robot->dxls.end(); _it++) - { - std::string _joint_name = _it->first; - Dynamixel *_dxl = _it->second; + // set init syncwrite param(from data of bulkread) + for (std::map::iterator it = robot_->dxls_.begin(); it != robot_->dxls_.end(); it++) + { + std::string joint_name = it->first; + Dynamixel *dxl = it->second; - for(int _i = 0; _i < _dxl->bulk_read_items.size(); _i++) + for (int i = 0; i < dxl->bulk_read_items_.size(); i++) + { + uint32_t read_data = 0; + uint8_t sync_write_data[4]; + + if (port_to_bulk_read_[dxl->port_name_]->isAvailable(dxl->id_, + dxl->bulk_read_items_[i]->address_, + dxl->bulk_read_items_[i]->data_length_) == true) + { + read_data = port_to_bulk_read_[dxl->port_name_]->getData(dxl->id_, + dxl->bulk_read_items_[i]->address_, + dxl->bulk_read_items_[i]->data_length_); + + sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(read_data)); + sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(read_data)); + sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(read_data)); + sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(read_data)); + + if ((dxl->present_position_item_ != 0) && + (dxl->bulk_read_items_[i]->item_name_ == dxl->present_position_item_->item_name_)) { - UINT32_T _read_data = 0; - UINT8_T _sync_write_data[4]; + dxl->dxl_state_->present_position_ = dxl->convertValue2Radian(read_data) - dxl->dxl_state_->position_offset_; // remove offset + dxl->dxl_state_->goal_position_ = dxl->dxl_state_->present_position_; - if(port_to_bulk_read[_dxl->port_name]->IsAvailable(_dxl->id, - _dxl->bulk_read_items[_i]->address, - _dxl->bulk_read_items[_i]->data_length) == true) - { - _read_data = port_to_bulk_read[_dxl->port_name]->GetData(_dxl->id, - _dxl->bulk_read_items[_i]->address, - _dxl->bulk_read_items[_i]->data_length); - - _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_read_data)); - _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_read_data)); - _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_read_data)); - _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_read_data)); - - if(_dxl->present_position_item != 0 && _dxl->bulk_read_items[_i]->item_name == _dxl->present_position_item->item_name) - { - _dxl->dxl_state->present_position = _dxl->ConvertValue2Radian(_read_data) - _dxl->dxl_state->position_offset; // remove offset - _dxl->dxl_state->goal_position = _dxl->dxl_state->present_position; - - port_to_sync_write_position[_dxl->port_name]->AddParam(_dxl->id, _sync_write_data); - } - else if(_dxl->position_p_gain_item != 0 && _dxl->bulk_read_items[_i]->item_name == _dxl->position_p_gain_item->item_name) - { - _dxl->dxl_state->position_p_gain = _read_data; - } - else if(_dxl->present_velocity_item != 0 && _dxl->bulk_read_items[_i]->item_name == _dxl->present_velocity_item->item_name) - { - _dxl->dxl_state->present_velocity = _dxl->ConvertValue2Velocity(_read_data); - _dxl->dxl_state->goal_velocity = _dxl->dxl_state->present_velocity; - } - else if(_dxl->present_current_item != 0 && _dxl->bulk_read_items[_i]->item_name == _dxl->present_current_item->item_name) - { - _dxl->dxl_state->present_current = _dxl->ConvertValue2Current(_read_data); - _dxl->dxl_state->goal_current = _dxl->dxl_state->present_current; - } - } + port_to_sync_write_position_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); } + else if ((dxl->position_p_gain_item_ != 0) && + (dxl->bulk_read_items_[i]->item_name_ == dxl->position_p_gain_item_->item_name_)) + { + dxl->dxl_state_->position_p_gain_ = read_data; + } + else if ((dxl->present_velocity_item_ != 0) && + (dxl->bulk_read_items_[i]->item_name_ == dxl->present_velocity_item_->item_name_)) + { + dxl->dxl_state_->present_velocity_ = dxl->convertValue2Velocity(read_data); + dxl->dxl_state_->goal_velocity_ = dxl->dxl_state_->present_velocity_; + } + else if ((dxl->present_current_item_ != 0) && + (dxl->bulk_read_items_[i]->item_name_ == dxl->present_current_item_->item_name_)) + { + dxl->dxl_state_->present_current_ = dxl->convertValue2Current(read_data); + dxl->dxl_state_->goal_current_ = dxl->dxl_state_->present_current_; + } + } } + } } -bool RobotisController::Initialize(const std::string robot_file_path, const std::string init_file_path) +bool RobotisController::initialize(const std::string robot_file_path, const std::string init_file_path) { - std::string _dev_desc_dir_path = ros::package::getPath("robotis_device") + "/devices"; + std::string dev_desc_dir_path = ros::package::getPath("robotis_device") + "/devices"; - // load robot info : port , device - robot = new Robot(robot_file_path, _dev_desc_dir_path); + // load robot info : port , device + robot_ = new Robot(robot_file_path, dev_desc_dir_path); - if(gazebo_mode == true) - { - queue_thread_ = boost::thread(boost::bind(&RobotisController::QueueThread, this)); - return true; - } - - for(std::map::iterator _it = robot->ports.begin(); _it != robot->ports.end(); _it++) - { - std::string _port_name = _it->first; - PortHandler *_port = _it->second; - - PacketHandler *_default_pkt_handler = PacketHandler::GetPacketHandler(2.0); - - if(_port->SetBaudRate(_port->GetBaudRate()) == false) - { - ROS_ERROR("PORT [%s] SETUP ERROR! (baudrate: %d)", _port_name.c_str(), _port->GetBaudRate()); - exit(-1); - } - - // get the default device info of the port - std::string _default_device_name = robot->port_default_device[_port_name]; - std::map::iterator _dxl_it = robot->dxls.find(_default_device_name); - std::map::iterator _sensor_it = robot->sensors.find(_default_device_name); - if(_dxl_it != robot->dxls.end()) - { - Dynamixel *_default_device = _dxl_it->second; - _default_pkt_handler = PacketHandler::GetPacketHandler(_default_device->protocol_version); - - if(_default_device->goal_position_item != 0) - { - port_to_sync_write_position[_port_name] = new GroupSyncWrite(_port, - _default_pkt_handler, - _default_device->goal_position_item->address, - _default_device->goal_position_item->data_length); - } - - if(_default_device->position_p_gain_item != 0) - { - port_to_sync_write_position_p_gain[_port_name] = new GroupSyncWrite(_port, - _default_pkt_handler, - _default_device->position_p_gain_item->address, - _default_device->position_p_gain_item->data_length); - } - - if(_default_device->goal_velocity_item != 0) - { - port_to_sync_write_velocity[_port_name] = new GroupSyncWrite(_port, - _default_pkt_handler, - _default_device->goal_velocity_item->address, - _default_device->goal_velocity_item->data_length); - } - - if(_default_device->goal_current_item != 0) - { - port_to_sync_write_torque[_port_name] = new GroupSyncWrite(_port, - _default_pkt_handler, - _default_device->goal_current_item->address, - _default_device->goal_current_item->data_length); - } - } - else if(_sensor_it != robot->sensors.end()) - { - Sensor *_default_device = _sensor_it->second; - _default_pkt_handler = PacketHandler::GetPacketHandler(_default_device->protocol_version); - } - - port_to_bulk_read[_port_name] = new GroupBulkRead(_port, _default_pkt_handler); - } - - // (for loop) check all dxls are connected. - for(std::map::iterator _it = robot->dxls.begin(); _it != robot->dxls.end(); _it++) - { - std::string _joint_name = _it->first; - Dynamixel *_dxl = _it->second; - - if(Ping(_joint_name) != 0) - { - usleep(10*1000); - if(Ping(_joint_name) != 0) - ROS_ERROR("JOINT[%s] does NOT respond!!", _joint_name.c_str()); - } - } - - InitDevice(init_file_path); - - // [ BulkRead ] StartAddress : Present Position , Length : 10 ( Position/Velocity/Current ) - for(std::map::iterator _it = robot->dxls.begin(); _it != robot->dxls.end(); _it++) - { - std::string _joint_name = _it->first; - Dynamixel *_dxl = _it->second; - - if(_dxl == NULL) - continue; - - int _bulkread_start_addr = 0; - int _bulkread_data_length = 0; - -// // bulk read default : present position -// if(_dxl->present_position_item != 0) -// { -// _bulkread_start_addr = _dxl->present_position_item->address; -// _bulkread_data_length = _dxl->present_position_item->data_length; -// } - - // calculate bulk read start address & data length - std::map::iterator _indirect_addr_it = _dxl->ctrl_table.find(INDIRECT_ADDRESS_1); - if(_indirect_addr_it != _dxl->ctrl_table.end()) // INDIRECT_ADDRESS_1 exist - { - if(_dxl->bulk_read_items.size() != 0) - { - _bulkread_start_addr = _dxl->bulk_read_items[0]->address; - _bulkread_data_length = 0; - - // set indirect address - int _indirect_addr = _indirect_addr_it->second->address; - for(int _i = 0; _i < _dxl->bulk_read_items.size(); _i++) - { - int _addr_leng = _dxl->bulk_read_items[_i]->data_length; - - _bulkread_data_length += _addr_leng; - for(int _l = 0; _l < _addr_leng; _l++) - { -// ROS_WARN("[%12s] INDIR_ADDR: %d, ITEM_ADDR: %d", _joint_name.c_str(), _indirect_addr, _dxl->ctrl_table[_dxl->bulk_read_items[_i]->item_name]->address + _l); - Write2Byte(_joint_name, _indirect_addr, _dxl->ctrl_table[_dxl->bulk_read_items[_i]->item_name]->address + _l); - _indirect_addr += 2; - } - } - } - } - else // INDIRECT_ADDRESS_1 NOT exist - { - if(_dxl->bulk_read_items.size() != 0) - { - _bulkread_start_addr = _dxl->bulk_read_items[0]->address; - _bulkread_data_length = 0; - - ControlTableItem *_last_item = _dxl->bulk_read_items[0]; - - for(int _i = 0; _i < _dxl->bulk_read_items.size(); _i++) - { - int _addr = _dxl->bulk_read_items[_i]->address; - if(_addr < _bulkread_start_addr) - _bulkread_start_addr = _addr; - else if(_last_item->address < _addr) - _last_item = _dxl->bulk_read_items[_i]; - } - - _bulkread_data_length = _last_item->address - _bulkread_start_addr + _last_item->data_length; - } - } - -// ROS_WARN("[%12s] start_addr: %d, data_length: %d", _joint_name.c_str(), _bulkread_start_addr, _bulkread_data_length); - if(_bulkread_start_addr != 0) - port_to_bulk_read[_dxl->port_name]->AddParam(_dxl->id, _bulkread_start_addr, _bulkread_data_length); - - // Torque ON - if(WriteCtrlItem(_joint_name, _dxl->torque_enable_item->item_name, 1) != COMM_SUCCESS) - WriteCtrlItem(_joint_name, _dxl->torque_enable_item->item_name, 1); - } - - for(std::map::iterator _it = robot->sensors.begin(); _it != robot->sensors.end(); _it++) - { - std::string _sensor_name = _it->first; - Sensor *_sensor = _it->second; - - if(_sensor == NULL) - continue; - - int _bulkread_start_addr = 0; - int _bulkread_data_length = 0; - - // calculate bulk read start address & data length - std::map::iterator _indirect_addr_it = _sensor->ctrl_table.find(INDIRECT_ADDRESS_1); - if(_indirect_addr_it != _sensor->ctrl_table.end()) // INDIRECT_ADDRESS_1 exist - { - if(_sensor->bulk_read_items.size() != 0) - { - _bulkread_start_addr = _sensor->bulk_read_items[0]->address; - _bulkread_data_length = 0; - - // set indirect address - int _indirect_addr = _indirect_addr_it->second->address; - for(int _i = 0; _i < _sensor->bulk_read_items.size(); _i++) - { - int _addr_leng = _sensor->bulk_read_items[_i]->data_length; - - _bulkread_data_length += _addr_leng; - for(int _l = 0; _l < _addr_leng; _l++) - { -// ROS_WARN("[%12s] INDIR_ADDR: %d, ITEM_ADDR: %d", _sensor_name.c_str(), _indirect_addr, _sensor->ctrl_table[_sensor->bulk_read_items[_i]->item_name]->address + _l); - Write2Byte(_sensor_name, _indirect_addr, _sensor->ctrl_table[_sensor->bulk_read_items[_i]->item_name]->address + _l); - _indirect_addr += 2; - } - } - } - } - else // INDIRECT_ADDRESS_1 NOT exist - { - if(_sensor->bulk_read_items.size() != 0) - { - _bulkread_start_addr = _sensor->bulk_read_items[0]->address; - _bulkread_data_length = 0; - - ControlTableItem *_last_item = _sensor->bulk_read_items[0]; - - for(int _i = 0; _i < _sensor->bulk_read_items.size(); _i++) - { - int _addr = _sensor->bulk_read_items[_i]->address; - if(_addr < _bulkread_start_addr) - _bulkread_start_addr = _addr; - else if(_last_item->address < _addr) - _last_item = _sensor->bulk_read_items[_i]; - } - - _bulkread_data_length = _last_item->address - _bulkread_start_addr + _last_item->data_length; - } - } - - //ROS_WARN("[%12s] start_addr: %d, data_length: %d", _sensor_name.c_str(), _bulkread_start_addr, _bulkread_data_length); - if(_bulkread_start_addr != 0) - port_to_bulk_read[_sensor->port_name]->AddParam(_sensor->id, _bulkread_start_addr, _bulkread_data_length); - } - - queue_thread_ = boost::thread(boost::bind(&RobotisController::QueueThread, this)); + if (gazebo_mode_ == true) + { + queue_thread_ = boost::thread(boost::bind(&RobotisController::msgQueueThread, this)); return true; -} + } -void RobotisController::InitDevice(const std::string init_file_path) -{ - // device initialize - if(DEBUG_PRINT) ROS_WARN("INIT FILE LOAD"); + for (std::map::iterator it = robot_->ports_.begin(); + it != robot_->ports_.end(); it++) + { + std::string port_name = it->first; + dynamixel::PortHandler *port = it->second; + dynamixel::PacketHandler *default_pkt_handler = dynamixel::PacketHandler::getPacketHandler(2.0); - YAML::Node _doc; - try{ - _doc = YAML::LoadFile(init_file_path.c_str()); + if (port->setBaudRate(port->getBaudRate()) == false) + { + ROS_ERROR("PORT [%s] SETUP ERROR! (baudrate: %d)", port_name.c_str(), port->getBaudRate()); + exit(-1); + } - for(YAML::const_iterator _it_doc = _doc.begin(); _it_doc != _doc.end(); _it_doc++) + // get the default device info of the port + std::string default_device_name = robot_->port_default_device_[port_name]; + std::map::iterator dxl_it = robot_->dxls_.find(default_device_name); + std::map::iterator sensor_it = robot_->sensors_.find(default_device_name); + if (dxl_it != robot_->dxls_.end()) + { + Dynamixel *default_device = dxl_it->second; + default_pkt_handler = dynamixel::PacketHandler::getPacketHandler(default_device->protocol_version_); + + if (default_device->goal_position_item_ != 0) + { + port_to_sync_write_position_[port_name] + = new dynamixel::GroupSyncWrite(port, + default_pkt_handler, + default_device->goal_position_item_->address_, + default_device->goal_position_item_->data_length_); + } + + if (default_device->position_p_gain_item_ != 0) + { + port_to_sync_write_position_p_gain_[port_name] + = new dynamixel::GroupSyncWrite(port, + default_pkt_handler, + default_device->position_p_gain_item_->address_, + default_device->position_p_gain_item_->data_length_); + } + + if (default_device->goal_velocity_item_ != 0) + { + port_to_sync_write_velocity_[port_name] + = new dynamixel::GroupSyncWrite(port, + default_pkt_handler, + default_device->goal_velocity_item_->address_, + default_device->goal_velocity_item_->data_length_); + } + + if (default_device->goal_current_item_ != 0) + { + port_to_sync_write_torque_[port_name] + = new dynamixel::GroupSyncWrite(port, + default_pkt_handler, + default_device->goal_current_item_->address_, + default_device->goal_current_item_->data_length_); + } + } + else if (sensor_it != robot_->sensors_.end()) + { + Sensor *_default_device = sensor_it->second; + default_pkt_handler = dynamixel::PacketHandler::getPacketHandler(_default_device->protocol_version_); + } + + port_to_bulk_read_[port_name] = new dynamixel::GroupBulkRead(port, default_pkt_handler); + } + + // (for loop) check all dxls are connected. + for (std::map::iterator it = robot_->dxls_.begin(); it != robot_->dxls_.end(); it++) + { + std::string joint_name = it->first; + Dynamixel *dxl = it->second; + + if (ping(joint_name) != 0) + { + usleep(10 * 1000); + if (ping(joint_name) != 0) + ROS_ERROR("JOINT[%s] does NOT respond!!", joint_name.c_str()); + } + } + + initializeDevice(init_file_path); + + // [ BulkRead ] StartAddress : Present Position , Length : 10 ( Position/Velocity/Current ) + for (std::map::iterator it = robot_->dxls_.begin(); it != robot_->dxls_.end(); it++) + { + std::string joint_name = it->first; + Dynamixel *dxl = it->second; + + if (dxl == NULL) + continue; + + int bulkread_start_addr = 0; + int bulkread_data_length = 0; + +// // bulk read default : present position +// if(dxl->present_position_item != 0) +// { +// bulkread_start_addr = dxl->present_position_item->address; +// bulkread_data_length = dxl->present_position_item->data_length; +// } + + // calculate bulk read start address & data length + std::map::iterator indirect_addr_it = dxl->ctrl_table_.find(INDIRECT_ADDRESS_1); + if (indirect_addr_it != dxl->ctrl_table_.end()) // INDIRECT_ADDRESS_1 exist + { + if (dxl->bulk_read_items_.size() != 0) + { + bulkread_start_addr = dxl->bulk_read_items_[0]->address_; + bulkread_data_length = 0; + + // set indirect address + int indirect_addr = indirect_addr_it->second->address_; + for (int i = 0; i < dxl->bulk_read_items_.size(); i++) { - std::string _joint_name = _it_doc->first.as(); + int addr_leng = dxl->bulk_read_items_[i]->data_length_; - YAML::Node _joint_node = _doc[_joint_name]; - if(_joint_node.size() == 0) - continue; - - Dynamixel *_dxl = NULL; - std::map::iterator _dxl_it = robot->dxls.find(_joint_name); - if(_dxl_it != robot->dxls.end()) - _dxl = _dxl_it->second; - - if(_dxl == NULL) - { - ROS_WARN("Joint [%s] does not found.", _joint_name.c_str()); - continue; - } - if(DEBUG_PRINT) ROS_INFO("JOINT_NAME: %s", _joint_name.c_str()); - - for(YAML::const_iterator _it_joint = _joint_node.begin(); _it_joint != _joint_node.end(); _it_joint++) - { - std::string _item_name = _it_joint->first.as(); - - if(DEBUG_PRINT) ROS_INFO(" ITEM_NAME: %s", _item_name.c_str()); - - UINT32_T _value = _it_joint->second.as(); - - ControlTableItem *_item = _dxl->ctrl_table[_item_name]; - if(_item == NULL) - { - ROS_WARN("Control Item [%s] does not found.", _item_name.c_str()); - continue; - } - - if(_item->memory_type == EEPROM) - { - UINT8_T _data8 = 0; - UINT16_T _data16 = 0; - UINT32_T _data32 = 0; - - switch(_item->data_length) - { - case 1: - Read1Byte(_joint_name, _item->address, &_data8); - if(_data8 == _value) - continue; - break; - case 2: - Read2Byte(_joint_name, _item->address, &_data16); - if(_data16 == _value) - continue; - break; - case 4: - Read4Byte(_joint_name, _item->address, &_data32); - if(_data32 == _value) - continue; - break; - default: - break; - } - } - - switch(_item->data_length) - { - case 1: - Write1Byte(_joint_name, _item->address, (UINT8_T)_value); - break; - case 2: - Write2Byte(_joint_name, _item->address, (UINT16_T)_value); - break; - case 4: - Write4Byte(_joint_name, _item->address, _value); - break; - default: - break; - } - - if(_item->memory_type == EEPROM) - { - // Write to EEPROM -> delay is required (max delay: 55 msec per byte) - usleep(_item->data_length * 55 * 1000); - } - } + bulkread_data_length += addr_leng; + for (int l = 0; l < addr_leng; l++) + { +// ROS_WARN("[%12s] INDIR_ADDR: %d, ITEM_ADDR: %d", joint_name.c_str(), indirect_addr, dxl->ctrl_table[dxl->bulk_read_items[i]->item_name]->address + _l); + write2Byte(joint_name, indirect_addr, dxl->ctrl_table_[dxl->bulk_read_items_[i]->item_name_]->address_ + l); + indirect_addr += 2; + } } - } catch(const std::exception& e) { - ROS_INFO("Dynamixel Init file not found."); + } } -} - -void RobotisController::GazeboThread() -{ - ros::Rate gazebo_rate(1000 / CONTROL_CYCLE_MSEC); - - while(!stop_timer_) + else // INDIRECT_ADDRESS_1 NOT exist { - if(init_pose_loaded_ == true) - Process(); - gazebo_rate.sleep(); - } -} + if (dxl->bulk_read_items_.size() != 0) + { + bulkread_start_addr = dxl->bulk_read_items_[0]->address_; + bulkread_data_length = 0; -void RobotisController::QueueThread() -{ - ros::NodeHandle _ros_node; - ros::CallbackQueue _callback_queue; + ControlTableItem *last_item = dxl->bulk_read_items_[0]; - _ros_node.setCallbackQueue(&_callback_queue); - - /* subscriber */ - ros::Subscriber _sync_write_item_sub = _ros_node.subscribe("/robotis/sync_write_item", 10, &RobotisController::SyncWriteItemCallback, this); - ros::Subscriber _joint_ctrl_modules_sub = _ros_node.subscribe("/robotis/set_joint_ctrl_modules", 10, &RobotisController::SetJointCtrlModuleCallback, this); - ros::Subscriber _enable_ctrl_module_sub = _ros_node.subscribe("/robotis/enable_ctrl_module", 10, &RobotisController::SetCtrlModuleCallback, this); - ros::Subscriber _control_mode_sub = _ros_node.subscribe("/robotis/set_control_mode", 10, &RobotisController::SetControllerModeCallback, this); - ros::Subscriber _joint_states_sub = _ros_node.subscribe("/robotis/set_joint_states", 10, &RobotisController::SetJointStatesCallback, this); - - ros::Subscriber _gazebo_joint_states_sub; - if(gazebo_mode == true) - _gazebo_joint_states_sub = _ros_node.subscribe("/" + gazebo_robot_name + "/joint_states", 10, &RobotisController::GazeboJointStatesCallback, this); - - /* publisher */ - goal_joint_state_pub = _ros_node.advertise("/robotis/goal_joint_states", 10); - present_joint_state_pub = _ros_node.advertise("/robotis/present_joint_states", 10); - current_module_pub = _ros_node.advertise("/robotis/present_joint_ctrl_modules", 10); - - if(gazebo_mode == true) - { - for(std::map::iterator _it = robot->dxls.begin(); _it != robot->dxls.end(); _it++) - gazebo_joint_pub[_it->first] = _ros_node.advertise("/" + gazebo_robot_name + "/" + _it->first + "_pos/command", 1); - } - - /* service */ - ros::ServiceServer _joint_module_server = _ros_node.advertiseService("/robotis/get_present_joint_ctrl_modules", &RobotisController::GetCtrlModuleCallback, this); - - while(_ros_node.ok()) - { - _callback_queue.callAvailable(); - usleep(100); - } -} - -void *RobotisController::ThreadProc(void *param) -{ - RobotisController *controller = (RobotisController *)param; - static struct timespec next_time; - static struct timespec curr_time; - - ROS_INFO("controller::thread_proc"); - - clock_gettime(CLOCK_MONOTONIC, &next_time); - - while(!controller->stop_timer_) - { - next_time.tv_sec += (next_time.tv_nsec + CONTROL_CYCLE_MSEC * 1000000) / 1000000000; - next_time.tv_nsec = (next_time.tv_nsec + CONTROL_CYCLE_MSEC * 1000000) % 1000000000; - - controller->Process(); - - clock_gettime(CLOCK_MONOTONIC, &curr_time); - long delta_nsec = (next_time.tv_sec - curr_time.tv_sec) * 1000000000 + (next_time.tv_nsec - curr_time.tv_nsec); - if(delta_nsec < -100000 ) + for (int i = 0; i < dxl->bulk_read_items_.size(); i++) { - if(controller->DEBUG_PRINT == true) - ROS_WARN("[RobotisController::ThreadProc] NEXT TIME < CURR TIME.. (%f)[%ld.%09ld / %ld.%09ld]", delta_nsec/1000000.0, (long)next_time.tv_sec, (long)next_time.tv_nsec, (long)curr_time.tv_sec, (long)curr_time.tv_nsec); - // next_time = curr_time + 3 msec - next_time.tv_sec = curr_time.tv_sec + (curr_time.tv_nsec + 3000000) / 1000000000; - next_time.tv_nsec = (curr_time.tv_nsec + 3000000) % 1000000000; + int addr = dxl->bulk_read_items_[i]->address_; + if (addr < bulkread_start_addr) + bulkread_start_addr = addr; + else if (last_item->address_ < addr) + last_item = dxl->bulk_read_items_[i]; } - clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &next_time, NULL); + bulkread_data_length = last_item->address_ - bulkread_start_addr + last_item->data_length_; + } } - return 0; + +// ROS_WARN("[%12s] start_addr: %d, data_length: %d", joint_name.c_str(), bulkread_start_addr, bulkread_data_length); + if (bulkread_start_addr != 0) + port_to_bulk_read_[dxl->port_name_]->addParam(dxl->id_, bulkread_start_addr, bulkread_data_length); + + // Torque ON + if (writeCtrlItem(joint_name, dxl->torque_enable_item_->item_name_, 1) != COMM_SUCCESS) + writeCtrlItem(joint_name, dxl->torque_enable_item_->item_name_, 1); + } + + for (std::map::iterator it = robot_->sensors_.begin(); it != robot_->sensors_.end(); it++) + { + std::string sensor_name = it->first; + Sensor *sensor = it->second; + + if (sensor == NULL) + continue; + + int bulkread_start_addr = 0; + int bulkread_data_length = 0; + + // calculate bulk read start address & data length + std::map::iterator indirect_addr_it = sensor->ctrl_table_.find(INDIRECT_ADDRESS_1); + if (indirect_addr_it != sensor->ctrl_table_.end()) // INDIRECT_ADDRESS_1 exist + { + if (sensor->bulk_read_items_.size() != 0) + { + bulkread_start_addr = sensor->bulk_read_items_[0]->address_; + bulkread_data_length = 0; + + // set indirect address + int indirect_addr = indirect_addr_it->second->address_; + for (int i = 0; i < sensor->bulk_read_items_.size(); i++) + { + int addr_leng = sensor->bulk_read_items_[i]->data_length_; + + bulkread_data_length += addr_leng; + for (int l = 0; l < addr_leng; l++) + { +// ROS_WARN("[%12s] INDIR_ADDR: %d, ITEM_ADDR: %d", sensor_name.c_str(), indirect_addr, sensor->ctrl_table[sensor->bulk_read_items[i]->item_name]->address + _l); + write2Byte(sensor_name, + indirect_addr, + sensor->ctrl_table_[sensor->bulk_read_items_[i]->item_name_]->address_ + l); + indirect_addr += 2; + } + } + } + } + else // INDIRECT_ADDRESS_1 NOT exist + { + if (sensor->bulk_read_items_.size() != 0) + { + bulkread_start_addr = sensor->bulk_read_items_[0]->address_; + bulkread_data_length = 0; + + ControlTableItem *last_item = sensor->bulk_read_items_[0]; + + for (int i = 0; i < sensor->bulk_read_items_.size(); i++) + { + int addr = sensor->bulk_read_items_[i]->address_; + if (addr < bulkread_start_addr) + bulkread_start_addr = addr; + else if (last_item->address_ < addr) + last_item = sensor->bulk_read_items_[i]; + } + + bulkread_data_length = last_item->address_ - bulkread_start_addr + last_item->data_length_; + } + } + + //ROS_WARN("[%12s] start_addr: %d, data_length: %d", sensor_name.c_str(), bulkread_start_addr, bulkread_data_length); + if (bulkread_start_addr != 0) + port_to_bulk_read_[sensor->port_name_]->addParam(sensor->id_, bulkread_start_addr, bulkread_data_length); + } + + queue_thread_ = boost::thread(boost::bind(&RobotisController::msgQueueThread, this)); + return true; } -void RobotisController::StartTimer() +void RobotisController::initializeDevice(const std::string init_file_path) { - if(this->is_timer_running_ == true) - return; + // device initialize + if (DEBUG_PRINT) + ROS_WARN("INIT FILE LOAD"); - if(this->gazebo_mode == true) + YAML::Node doc; + try + { + doc = YAML::LoadFile(init_file_path.c_str()); + + for (YAML::const_iterator it_doc = doc.begin(); it_doc != doc.end(); it_doc++) { - // create and start the thread - gazebo_thread_ = boost::thread(boost::bind(&RobotisController::GazeboThread, this)); + std::string joint_name = it_doc->first.as(); + + YAML::Node joint_node = doc[joint_name]; + if (joint_node.size() == 0) + continue; + + Dynamixel *dxl = NULL; + std::map::iterator dxl_it = robot_->dxls_.find(joint_name); + if (dxl_it != robot_->dxls_.end()) + dxl = dxl_it->second; + + if (dxl == NULL) + { + ROS_WARN("Joint [%s] does not found.", joint_name.c_str()); + continue; + } + if (DEBUG_PRINT) + ROS_INFO("JOINT_NAME: %s", joint_name.c_str()); + + for (YAML::const_iterator it_joint = joint_node.begin(); it_joint != joint_node.end(); it_joint++) + { + std::string item_name = it_joint->first.as(); + + if (DEBUG_PRINT) + ROS_INFO(" ITEM_NAME: %s", item_name.c_str()); + + uint32_t value = it_joint->second.as(); + + ControlTableItem *item = dxl->ctrl_table_[item_name]; + if (item == NULL) + { + ROS_WARN("Control Item [%s] does not found.", item_name.c_str()); + continue; + } + + if (item->memory_type_ == EEPROM) + { + uint8_t data8 = 0; + uint16_t data16 = 0; + uint32_t data32 = 0; + + switch (item->data_length_) + { + case 1: + read1Byte(joint_name, item->address_, &data8); + if (data8 == value) + continue; + break; + case 2: + read2Byte(joint_name, item->address_, &data16); + if (data16 == value) + continue; + break; + case 4: + read4Byte(joint_name, item->address_, &data32); + if (data32 == value) + continue; + break; + default: + break; + } + } + + switch (item->data_length_) + { + case 1: + write1Byte(joint_name, item->address_, (uint8_t) value); + break; + case 2: + write2Byte(joint_name, item->address_, (uint16_t) value); + break; + case 4: + write4Byte(joint_name, item->address_, value); + break; + default: + break; + } + + if (item->memory_type_ == EEPROM) + { + // Write to EEPROM -> delay is required (max delay: 55 msec per byte) + usleep(item->data_length_ * 55 * 1000); + } + } + } + } catch (const std::exception& e) + { + ROS_INFO("Dynamixel Init file not found."); + } +} + +void RobotisController::gazeboTimerThread() +{ + ros::Rate gazebo_rate(1000 / CONTROL_CYCLE_MSEC); + + while (!stop_timer_) + { + if (init_pose_loaded_ == true) + process(); + gazebo_rate.sleep(); + } +} + +void RobotisController::msgQueueThread() +{ + ros::NodeHandle ros_node; + ros::CallbackQueue callback_queue; + + ros_node.setCallbackQueue(&callback_queue); + + /* subscriber */ + ros::Subscriber sync_write_item_sub = ros_node.subscribe("/robotis/sync_write_item", 10, + &RobotisController::syncWriteItemCallback, this); + ros::Subscriber joint_ctrl_modules_sub = ros_node.subscribe("/robotis/set_joint_ctrl_modules", 10, + &RobotisController::setJointCtrlModuleCallback, this); + ros::Subscriber enable_ctrl_module_sub = ros_node.subscribe("/robotis/enable_ctrl_module", 10, + &RobotisController::setCtrlModuleCallback, this); + ros::Subscriber control_mode_sub = ros_node.subscribe("/robotis/set_control_mode", 10, + &RobotisController::setControllerModeCallback, this); + ros::Subscriber joint_states_sub = ros_node.subscribe("/robotis/set_joint_states", 10, + &RobotisController::setJointStatesCallback, this); + + ros::Subscriber gazebo_joint_states_sub; + if (gazebo_mode_ == true) + gazebo_joint_states_sub = ros_node.subscribe("/" + gazebo_robot_name_ + "/joint_states", 10, + &RobotisController::gazeboJointStatesCallback, this); + + /* publisher */ + goal_joint_state_pub_ = ros_node.advertise("/robotis/goal_joint_states", 10); + present_joint_state_pub_ = ros_node.advertise("/robotis/present_joint_states", 10); + current_module_pub_ = ros_node.advertise( + "/robotis/present_joint_ctrl_modules", 10); + + if (gazebo_mode_ == true) + { + for (std::map::iterator it = robot_->dxls_.begin(); it != robot_->dxls_.end(); it++) + { + gazebo_joint_pub_[it->first] = ros_node.advertise( + "/" + gazebo_robot_name_ + "/" + it->first + "_pos/command", 1); + } + } + + /* service */ + ros::ServiceServer joint_module_server = ros_node.advertiseService("/robotis/get_present_joint_ctrl_modules", + &RobotisController::getCtrlModuleCallback, this); + + while (ros_node.ok()) + { + callback_queue.callAvailable(); + usleep(100); + } +} + +void *RobotisController::timerThread(void *param) +{ + RobotisController *controller = (RobotisController *) param; + static struct timespec next_time; + static struct timespec curr_time; + + ROS_INFO("controller::thread_proc"); + + clock_gettime(CLOCK_MONOTONIC, &next_time); + + while (!controller->stop_timer_) + { + next_time.tv_sec += (next_time.tv_nsec + CONTROL_CYCLE_MSEC * 1000000) / 1000000000; + next_time.tv_nsec = (next_time.tv_nsec + CONTROL_CYCLE_MSEC * 1000000) % 1000000000; + + controller->process(); + + clock_gettime(CLOCK_MONOTONIC, &curr_time); + long delta_nsec = (next_time.tv_sec - curr_time.tv_sec) * 1000000000 + (next_time.tv_nsec - curr_time.tv_nsec); + if (delta_nsec < -100000) + { + if (controller->DEBUG_PRINT == true) + { + ROS_WARN("[RobotisController::ThreadProc] NEXT TIME < CURR TIME.. (%f)[%ld.%09ld / %ld.%09ld]", + delta_nsec / 1000000.0, (long )next_time.tv_sec, (long )next_time.tv_nsec, + (long )curr_time.tv_sec, (long )curr_time.tv_nsec); + } + + // next_time = curr_time + 3 msec + next_time.tv_sec = curr_time.tv_sec + (curr_time.tv_nsec + 3000000) / 1000000000; + next_time.tv_nsec = (curr_time.tv_nsec + 3000000) % 1000000000; + } + + clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &next_time, NULL); + } + return 0; +} + +void RobotisController::startTimer() +{ + if (this->is_timer_running_ == true) + return; + + if (this->gazebo_mode_ == true) + { + // create and start the thread + gazebo_thread_ = boost::thread(boost::bind(&RobotisController::gazeboTimerThread, this)); + } + else + { + initializeSyncWrite(); + + for (std::map::iterator it = port_to_bulk_read_.begin(); + it != port_to_bulk_read_.end(); it++) + { + it->second->txPacket(); + } + + int error; + struct sched_param param; + pthread_attr_t attr; + + pthread_attr_init(&attr); + + error = pthread_attr_setschedpolicy(&attr, SCHED_RR); + if (error != 0) + ROS_ERROR("pthread_attr_setschedpolicy error = %d\n", error); + error = pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED); + if (error != 0) + ROS_ERROR("pthread_attr_setinheritsched error = %d\n", error); + + memset(¶m, 0, sizeof(param)); + param.sched_priority = 31; // RT + error = pthread_attr_setschedparam(&attr, ¶m); + if (error != 0) + ROS_ERROR("pthread_attr_setschedparam error = %d\n", error); + + // create and start the thread + if ((error = pthread_create(&this->timer_thread_, &attr, this->timerThread, this)) != 0) + { + ROS_ERROR("timer thread create fail!!"); + exit(-1); + } + } + + this->is_timer_running_ = true; +} + +void RobotisController::stopTimer() +{ + int error = 0; + + // set the flag to stop the thread + if (this->is_timer_running_) + { + this->stop_timer_ = true; + + if (this->gazebo_mode_ == false) + { + // wait until the thread is stopped. + if ((error = pthread_join(this->timer_thread_, NULL)) != 0) + exit(-1); + + for (std::map::iterator it = port_to_bulk_read_.begin(); + it != port_to_bulk_read_.end(); it++) + { + if (it->second != NULL) + it->second->rxPacket(); + } + + for (std::map::iterator it = port_to_sync_write_position_.begin(); + it != port_to_sync_write_position_.end(); it++) + { + if (it->second != NULL) + it->second->clearParam(); + } + for (std::map::iterator it = port_to_sync_write_position_p_gain_.begin(); + it != port_to_sync_write_position_p_gain_.end(); it++) + { + if (it->second != NULL) + it->second->clearParam(); + } + for (std::map::iterator it = port_to_sync_write_velocity_.begin(); + it != port_to_sync_write_velocity_.end(); it++) + { + if (it->second != NULL) + it->second->clearParam(); + } + for (std::map::iterator it = port_to_sync_write_torque_.begin(); + it != port_to_sync_write_torque_.end(); it++) + { + if (it->second != NULL) + it->second->clearParam(); + } } else { - InitSyncWrite(); - - for(std::map::iterator _it = port_to_bulk_read.begin(); _it != port_to_bulk_read.end(); _it++) - _it->second->TxPacket(); - - int error; - struct sched_param param; - pthread_attr_t attr; - - pthread_attr_init(&attr); - - error = pthread_attr_setschedpolicy(&attr, SCHED_RR); - if(error != 0) - ROS_ERROR("pthread_attr_setschedpolicy error = %d\n",error); - error = pthread_attr_setinheritsched(&attr,PTHREAD_EXPLICIT_SCHED); - if(error != 0) - ROS_ERROR("pthread_attr_setinheritsched error = %d\n",error); - - memset(¶m, 0, sizeof(param)); - param.sched_priority = 31;// RT - error = pthread_attr_setschedparam(&attr, ¶m); - if(error != 0) - ROS_ERROR("pthread_attr_setschedparam error = %d\n",error); - - // create and start the thread - if((error = pthread_create(&this->timer_thread_, &attr, this->ThreadProc, this))!= 0) { - ROS_ERROR("timer thread create fail!!"); - exit(-1); - } + // wait until the thread is stopped. + gazebo_thread_.join(); } - this->is_timer_running_ = true; + this->stop_timer_ = false; + this->is_timer_running_ = false; + } } -void RobotisController::StopTimer() +bool RobotisController::isTimerRunning() { - int error = 0; - - // set the flag to stop the thread - if(this->is_timer_running_) - { - this->stop_timer_ = true; - - if(this->gazebo_mode == false) - { - // wait until the thread is stopped. - if((error = pthread_join(this->timer_thread_, NULL)) != 0) - exit(-1); - - for(std::map::iterator _it = port_to_bulk_read.begin(); _it != port_to_bulk_read.end(); _it++) - { - if(_it->second != NULL) - _it->second->RxPacket(); - } - - for(std::map::iterator _it = port_to_sync_write_position.begin(); _it != port_to_sync_write_position.end(); _it++) - { - if(_it->second != NULL) - _it->second->ClearParam(); - } - for(std::map::iterator _it = port_to_sync_write_position_p_gain.begin(); _it != port_to_sync_write_position_p_gain.end(); _it++) - { - if(_it->second != NULL) - _it->second->ClearParam(); - } - for(std::map::iterator _it = port_to_sync_write_velocity.begin(); _it != port_to_sync_write_velocity.end(); _it++) - { - if(_it->second != NULL) - _it->second->ClearParam(); - } - for(std::map::iterator _it = port_to_sync_write_torque.begin(); _it != port_to_sync_write_torque.end(); _it++) - { - if(_it->second != NULL) - _it->second->ClearParam(); - } - } - else - { - // wait until the thread is stopped. - gazebo_thread_.join(); - } - - this->stop_timer_ = false; - this->is_timer_running_ = false; - } + return this->is_timer_running_; } -bool RobotisController::IsTimerRunning() +void RobotisController::loadOffset(const std::string path) { - return this->is_timer_running_; + YAML::Node doc; + try + { + doc = YAML::LoadFile(path.c_str()); + } catch (const std::exception& e) + { + ROS_ERROR("Fail to load offset yaml."); + return; + } + + YAML::Node offset_node = doc["offset"]; + if (offset_node.size() == 0) + return; + + ROS_INFO("Load offsets..."); + for (YAML::const_iterator it = offset_node.begin(); it != offset_node.end(); it++) + { + std::string joint_name = it->first.as(); + double offset = it->second.as(); + + std::map::iterator dxl_it = robot_->dxls_.find(joint_name); + if (dxl_it != robot_->dxls_.end()) + dxl_it->second->dxl_state_->position_offset_ = offset; + } } -void RobotisController::LoadOffset(const std::string path) +void RobotisController::process() { - YAML::Node _doc; - try{ - _doc = YAML::LoadFile(path.c_str()); - } catch(const std::exception& e) { - ROS_ERROR("Fail to load offset yaml."); - return; - } + // avoid duplicated function call + static bool is_process_running = false; + if (is_process_running == true) + return; + is_process_running = true; - YAML::Node _offset_node = _doc["offset"]; - if(_offset_node.size() == 0) - return; + // ROS_INFO("Controller::Process()"); + bool do_sync_write = false; - ROS_INFO("Load offsets..."); - for(YAML::const_iterator _it = _offset_node.begin(); _it != _offset_node.end(); _it++) - { - std::string _joint_name = _it->first.as(); - double _offset = _it->second.as(); + ros::Time start_time; + ros::Duration time_duration; - std::map::iterator _dxl_it = robot->dxls.find(_joint_name); - if(_dxl_it != robot->dxls.end()) - _dxl_it->second->dxl_state->position_offset = _offset; - } -} + if (DEBUG_PRINT) + start_time = ros::Time::now(); -void RobotisController::Process() -{ - // avoid duplicated function call - static bool _is_process_running = false; - if(_is_process_running == true) - return; - _is_process_running = true; + sensor_msgs::JointState goal_state; + sensor_msgs::JointState present_state; - // ROS_INFO("Controller::Process()"); - bool _do_sync_write = false; - - ros::Time _start_time; - ros::Duration _time_duration; - - if(DEBUG_PRINT) - _start_time = ros::Time::now(); - - sensor_msgs::JointState _goal_state; - sensor_msgs::JointState _present_state; - - _present_state.header.stamp = ros::Time::now(); - _goal_state.header.stamp = _present_state.header.stamp; + present_state.header.stamp = ros::Time::now(); + goal_state.header.stamp = present_state.header.stamp; + // BulkRead Rx + // -> save to Robot->dxls[]->dynamixel_state.present_position + if (gazebo_mode_ == false) + { // BulkRead Rx - // -> save to Robot->dxls[]->dynamixel_state.present_position - if(gazebo_mode == false) + for (std::map::iterator it = port_to_bulk_read_.begin(); + it != port_to_bulk_read_.end(); it++) { - // BulkRead Rx - for(std::map::iterator _it = port_to_bulk_read.begin(); _it != port_to_bulk_read.end(); _it++) - { - robot->ports[_it->first]->SetPacketTimeout(0.0); - _it->second->RxPacket(); - } + robot_->ports_[it->first]->setPacketTimeout(0.0); + it->second->rxPacket(); + } - if(robot->dxls.size() > 0) + if (robot_->dxls_.size() > 0) + { + for (std::map::iterator dxl_it = robot_->dxls_.begin(); + dxl_it != robot_->dxls_.end(); dxl_it++) + { + Dynamixel *dxl = dxl_it->second; + std::string port_name = dxl_it->second->port_name_; + std::string joint_name = dxl_it->first; + + if (dxl->bulk_read_items_.size() > 0) { - for(std::map::iterator dxl_it = robot->dxls.begin(); dxl_it != robot->dxls.end(); dxl_it++) + uint32_t data = 0; + for (int i = 0; i < dxl->bulk_read_items_.size(); i++) + { + ControlTableItem *item = dxl->bulk_read_items_[i]; + if (port_to_bulk_read_[port_name]->isAvailable(dxl->id_, item->address_, item->data_length_) == true) { - Dynamixel *_dxl = dxl_it->second; - std::string _port_name = dxl_it->second->port_name; - std::string _joint_name = dxl_it->first; + data = port_to_bulk_read_[port_name]->getData(dxl->id_, item->address_, item->data_length_); - if(_dxl->bulk_read_items.size() > 0) + // change dxl_state + if (dxl->present_position_item_ != 0 && item->item_name_ == dxl->present_position_item_->item_name_) + dxl->dxl_state_->present_position_ = dxl->convertValue2Radian(data) - dxl->dxl_state_->position_offset_; // remove offset + else if (dxl->present_velocity_item_ != 0 && item->item_name_ == dxl->present_velocity_item_->item_name_) + dxl->dxl_state_->present_velocity_ = dxl->convertValue2Velocity(data); + else if (dxl->present_current_item_ != 0 && item->item_name_ == dxl->present_current_item_->item_name_) + dxl->dxl_state_->present_current_ = dxl->convertValue2Current(data); + else if (dxl->goal_position_item_ != 0 && item->item_name_ == dxl->goal_position_item_->item_name_) + dxl->dxl_state_->goal_position_ = dxl->convertValue2Radian(data) - dxl->dxl_state_->position_offset_; // remove offset + else if (dxl->goal_velocity_item_ != 0 && item->item_name_ == dxl->goal_velocity_item_->item_name_) + dxl->dxl_state_->goal_velocity_ = dxl->convertValue2Velocity(data); + else if (dxl->goal_current_item_ != 0 && item->item_name_ == dxl->goal_current_item_->item_name_) + dxl->dxl_state_->goal_current_ = dxl->convertValue2Current(data); + else + dxl->dxl_state_->bulk_read_table_[item->item_name_] = data; + } + } + + // -> update time stamp to Robot->dxls[]->dynamixel_state.update_time_stamp + dxl->dxl_state_->update_time_stamp_ = TimeStamp(present_state.header.stamp.sec, present_state.header.stamp.nsec); + } + } + } + + if (robot_->sensors_.size() > 0) + { + for (std::map::iterator sensor_it = robot_->sensors_.begin(); + sensor_it != robot_->sensors_.end(); sensor_it++) + { + Sensor *sensor = sensor_it->second; + std::string port_name = sensor_it->second->port_name_; + std::string sensor_name = sensor_it->first; + + if (sensor->bulk_read_items_.size() > 0) + { + uint32_t data = 0; + for (int i = 0; i < sensor->bulk_read_items_.size(); i++) + { + ControlTableItem *item = sensor->bulk_read_items_[i]; + if (port_to_bulk_read_[port_name]->isAvailable(sensor->id_, item->address_, item->data_length_) == true) + { + data = port_to_bulk_read_[port_name]->getData(sensor->id_, item->address_, item->data_length_); + + // change sensor_state + sensor->sensor_state_->bulk_read_table_[item->item_name_] = data; + } + } + + // -> update time stamp to Robot->dxls[]->dynamixel_state.update_time_stamp + sensor->sensor_state_->update_time_stamp_ = TimeStamp(present_state.header.stamp.sec, present_state.header.stamp.nsec); + } + } + } + } + + if (DEBUG_PRINT) + { + time_duration = ros::Time::now() - start_time; + ROS_INFO("(%2.6f) BulkRead Rx & update state", time_duration.nsec * 0.000001); + } + + // Call SensorModule Process() + // -> for loop : call SensorModule list -> Process() + if (sensor_modules_.size() > 0) + { + for (std::list::iterator module_it = sensor_modules_.begin(); module_it != sensor_modules_.end(); module_it++) + { + (*module_it)->process(robot_->dxls_, robot_->sensors_); + + for (std::map::iterator it = (*module_it)->result_.begin(); it != (*module_it)->result_.end(); it++) + sensor_result_[it->first] = it->second; + } + } + + if (DEBUG_PRINT) + { + time_duration = ros::Time::now() - start_time; + ROS_INFO("(%2.6f) SensorModule Process() & save result", time_duration.nsec * 0.000001); + } + + if (controller_mode_ == MotionModuleMode) + { + // Call MotionModule Process() + // -> for loop : call MotionModule list -> Process() + if (motion_modules_.size() > 0) + { + queue_mutex_.lock(); + + for (std::list::iterator module_it = motion_modules_.begin(); module_it != motion_modules_.end(); module_it++) + { + if ((*module_it)->getModuleEnable() == false) + continue; + + (*module_it)->process(robot_->dxls_, sensor_result_); + + // for loop : joint list + for (std::map::iterator dxl_it = robot_->dxls_.begin(); dxl_it != robot_->dxls_.end(); dxl_it++) + { + std::string joint_name = dxl_it->first; + Dynamixel *dxl = dxl_it->second; + DynamixelState *dxl_state = dxl_it->second->dxl_state_; + + if (dxl->ctrl_module_name_ == (*module_it)->getModuleName()) + { + do_sync_write = true; + DynamixelState *result_state = (*module_it)->result_[joint_name]; + + if (result_state == NULL) + { + ROS_INFO("[%s] %s", (*module_it)->getModuleName().c_str(), joint_name.c_str()); + continue; + } + + // TODO: check update time stamp ? + + if ((*module_it)->getControlMode() == PositionControl) + { +// if(result_state->goal_position == 0 && dxl->id == 3) +// ROS_INFO("[MODULE:%s][ID:%2d] goal position = %f", (*module_it)->GetModuleName().c_str(), dxl->id, dxl_state->goal_position); + dxl_state->goal_position_ = result_state->goal_position_; + + if (gazebo_mode_ == false) + { + // add offset + uint32_t pos_data = dxl->convertRadian2Value(dxl_state->goal_position_ + dxl_state->position_offset_); + uint8_t sync_write_data[4] = { 0 }; + sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(pos_data)); + sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(pos_data)); + sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(pos_data)); + sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(pos_data)); + + if (abs(pos_data) > 151800) { - UINT32_T _data = 0; - for(int _i = 0; _i < _dxl->bulk_read_items.size(); _i++) - { - ControlTableItem *_item = _dxl->bulk_read_items[_i]; - if(port_to_bulk_read[_port_name]->IsAvailable(_dxl->id, _item->address, _item->data_length) == true) - { - _data = port_to_bulk_read[_port_name]->GetData(_dxl->id, _item->address, _item->data_length); - - // change dxl_state - if(_dxl->present_position_item != 0 && _item->item_name == _dxl->present_position_item->item_name) - _dxl->dxl_state->present_position = _dxl->ConvertValue2Radian(_data) - _dxl->dxl_state->position_offset; // remove offset - else if(_dxl->present_velocity_item != 0 && _item->item_name == _dxl->present_velocity_item->item_name) - _dxl->dxl_state->present_velocity = _dxl->ConvertValue2Velocity(_data); - else if(_dxl->present_current_item != 0 && _item->item_name == _dxl->present_current_item->item_name) - _dxl->dxl_state->present_current = _dxl->ConvertValue2Current(_data); - else if(_dxl->goal_position_item != 0 && _item->item_name == _dxl->goal_position_item->item_name) - _dxl->dxl_state->goal_position = _dxl->ConvertValue2Radian(_data) - _dxl->dxl_state->position_offset; // remove offset - else if(_dxl->goal_velocity_item != 0 && _item->item_name == _dxl->goal_velocity_item->item_name) - _dxl->dxl_state->goal_velocity = _dxl->ConvertValue2Velocity(_data); - else if(_dxl->goal_current_item != 0 && _item->item_name == _dxl->goal_current_item->item_name) - _dxl->dxl_state->goal_current = _dxl->ConvertValue2Current(_data); - else - _dxl->dxl_state->bulk_read_table[_item->item_name] = _data; - } - } - - // -> update time stamp to Robot->dxls[]->dynamixel_state.update_time_stamp - _dxl->dxl_state->update_time_stamp = TimeStamp(_present_state.header.stamp.sec, _present_state.header.stamp.nsec); + printf("goal_pos : %f | position_offset : %f | pos_data : %d\n", + dxl_state->goal_position_, dxl_state->position_offset_, pos_data); } - } - } - if(robot->sensors.size() > 0) - { - for(std::map::iterator sensor_it = robot->sensors.begin(); sensor_it != robot->sensors.end(); sensor_it++) - { - Sensor *_sensor = sensor_it->second; - std::string _port_name = sensor_it->second->port_name; - std::string _sensor_name = sensor_it->first; + if (port_to_sync_write_position_[dxl->port_name_] != NULL) + port_to_sync_write_position_[dxl->port_name_]->changeParam(dxl->id_, sync_write_data); - if(_sensor->bulk_read_items.size() > 0) + // if position p gain value is changed -> sync write + if (dxl_state->position_p_gain_ != result_state->position_p_gain_) { - UINT32_T _data = 0; - for(int _i = 0; _i < _sensor->bulk_read_items.size(); _i++) - { - ControlTableItem *_item = _sensor->bulk_read_items[_i]; - if(port_to_bulk_read[_port_name]->IsAvailable(_sensor->id, _item->address, _item->data_length) == true) - { - _data = port_to_bulk_read[_port_name]->GetData(_sensor->id, _item->address, _item->data_length); + dxl_state->position_p_gain_ = result_state->position_p_gain_; + uint8_t sync_write_p_gain[4] = { 0 }; + sync_write_p_gain[0] = DXL_LOBYTE(DXL_LOWORD(dxl_state->position_p_gain_)); + sync_write_p_gain[1] = DXL_HIBYTE(DXL_LOWORD(dxl_state->position_p_gain_)); + sync_write_p_gain[2] = DXL_LOBYTE(DXL_HIWORD(dxl_state->position_p_gain_)); + sync_write_p_gain[3] = DXL_HIBYTE(DXL_HIWORD(dxl_state->position_p_gain_)); - // change sensor_state - _sensor->sensor_state->bulk_read_table[_item->item_name] = _data; - } - } - - // -> update time stamp to Robot->dxls[]->dynamixel_state.update_time_stamp - _sensor->sensor_state->update_time_stamp = TimeStamp(_present_state.header.stamp.sec, _present_state.header.stamp.nsec); + if (port_to_sync_write_position_p_gain_[dxl->port_name_] != NULL) + port_to_sync_write_position_p_gain_[dxl->port_name_]->addParam(dxl->id_, sync_write_p_gain); } + } } - } - } - - if(DEBUG_PRINT) - { - _time_duration = ros::Time::now() - _start_time; - ROS_INFO("(%2.6f) BulkRead Rx & update state", _time_duration.nsec * 0.000001); - } - - // Call SensorModule Process() - // -> for loop : call SensorModule list -> Process() - if(sensor_modules_.size() > 0) - { - for(std::list::iterator _module_it = sensor_modules_.begin(); _module_it != sensor_modules_.end(); _module_it++) - { - (*_module_it)->Process(robot->dxls, robot->sensors); - - for(std::map::iterator _it = (*_module_it)->result.begin(); _it != (*_module_it)->result.end(); _it++) - sensor_result_[_it->first] = _it->second; - } - } - - if(DEBUG_PRINT) - { - _time_duration = ros::Time::now() - _start_time; - ROS_INFO("(%2.6f) SensorModule Process() & save result", _time_duration.nsec * 0.000001); - } - - if(controller_mode_ == MOTION_MODULE_MODE) - { - // Call MotionModule Process() - // -> for loop : call MotionModule list -> Process() - if(motion_modules_.size() > 0) - { - queue_mutex_.lock(); - - for(std::list::iterator module_it = motion_modules_.begin(); module_it != motion_modules_.end(); module_it++) + else if ((*module_it)->getControlMode() == VelocityControl) { - if((*module_it)->GetModuleEnable() == false) - continue; + dxl_state->goal_velocity_ = result_state->goal_velocity_; - (*module_it)->Process(robot->dxls, sensor_result_); + if (gazebo_mode_ == false) + { + uint32_t vel_data = dxl->convertVelocity2Value(dxl_state->goal_velocity_); + uint8_t sync_write_data[4] = { 0 }; + sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(vel_data)); + sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(vel_data)); + sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(vel_data)); + sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(vel_data)); - // for loop : joint list - for(std::map::iterator dxl_it = robot->dxls.begin(); dxl_it != robot->dxls.end(); dxl_it++) - { - std::string _joint_name = dxl_it->first; - Dynamixel *_dxl = dxl_it->second; - DynamixelState *_dxl_state = dxl_it->second->dxl_state; - - if(_dxl->ctrl_module_name == (*module_it)->GetModuleName()) - { - _do_sync_write = true; - DynamixelState *_result_state = (*module_it)->result[_joint_name]; - - if(_result_state == NULL) { - ROS_INFO("[%s] %s", (*module_it)->GetModuleName().c_str(), _joint_name.c_str()); - continue; - } - - // TODO: check update time stamp ? - - if((*module_it)->GetControlMode() == POSITION_CONTROL) - { -// if(_result_state->goal_position == 0 && _dxl->id == 3) -// ROS_INFO("[MODULE:%s][ID:%2d] goal position = %f", (*module_it)->GetModuleName().c_str(), _dxl->id, _dxl_state->goal_position); - _dxl_state->goal_position = _result_state->goal_position; - - if(gazebo_mode == false) - { - // add offset - UINT32_T _pos_data = _dxl->ConvertRadian2Value(_dxl_state->goal_position + _dxl_state->position_offset); - UINT8_T _sync_write_data[4] = {0}; - _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_pos_data)); - _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_pos_data)); - _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_pos_data)); - _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_pos_data)); - - if(abs(_pos_data) > 151800) - printf("goal_pos : %f | position_offset : %f | pos_data : %d\n", _dxl_state->goal_position , _dxl_state->position_offset, _pos_data); - - if(port_to_sync_write_position[_dxl->port_name] != NULL) - port_to_sync_write_position[_dxl->port_name]->ChangeParam(_dxl->id, _sync_write_data); - - // if position p gain value is changed -> sync write - if(_dxl_state->position_p_gain != _result_state->position_p_gain) - { - _dxl_state->position_p_gain = _result_state->position_p_gain; - - UINT8_T _sync_write_p_gain[4] = {0}; - _sync_write_p_gain[0] = DXL_LOBYTE(DXL_LOWORD(_dxl_state->position_p_gain)); - _sync_write_p_gain[1] = DXL_HIBYTE(DXL_LOWORD(_dxl_state->position_p_gain)); - _sync_write_p_gain[2] = DXL_LOBYTE(DXL_HIWORD(_dxl_state->position_p_gain)); - _sync_write_p_gain[3] = DXL_HIBYTE(DXL_HIWORD(_dxl_state->position_p_gain)); - - if(port_to_sync_write_position_p_gain[_dxl->port_name] != NULL) - port_to_sync_write_position_p_gain[_dxl->port_name]->AddParam(_dxl->id, _sync_write_p_gain); - } - } - } - else if((*module_it)->GetControlMode() == VELOCITY_CONTROL) - { - _dxl_state->goal_velocity = _result_state->goal_velocity; - - if(gazebo_mode == false) - { - UINT32_T _vel_data = _dxl->ConvertVelocity2Value(_dxl_state->goal_velocity); - UINT8_T _sync_write_data[4] = {0}; - _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_vel_data)); - _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_vel_data)); - _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_vel_data)); - _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_vel_data)); - - if(port_to_sync_write_velocity[_dxl->port_name] != NULL) - port_to_sync_write_velocity[_dxl->port_name]->ChangeParam(_dxl->id, _sync_write_data); - } - } - else if((*module_it)->GetControlMode() == CURRENT_CONTROL) - { - _dxl_state->goal_current = _result_state->goal_current; - - if(gazebo_mode == false) - { - UINT32_T _torq_data = _dxl->ConvertCurrent2Value(_dxl_state->goal_current); - UINT8_T _sync_write_data[2]; - _sync_write_data[0] = DXL_LOBYTE(_torq_data); - _sync_write_data[1] = DXL_HIBYTE(_torq_data); - - if(port_to_sync_write_torque[_dxl->port_name] != NULL) - port_to_sync_write_torque[_dxl->port_name]->ChangeParam(_dxl->id, _sync_write_data); - } - } - } - } + if (port_to_sync_write_velocity_[dxl->port_name_] != NULL) + port_to_sync_write_velocity_[dxl->port_name_]->changeParam(dxl->id_, sync_write_data); + } } - - queue_mutex_.unlock(); - } - - if(DEBUG_PRINT) - { - _time_duration = ros::Time::now() - _start_time; - ROS_INFO("(%2.6f) MotionModule Process() & save result", _time_duration.nsec * 0.000001); - } - - // SyncWrite - if(gazebo_mode == false && _do_sync_write) - { - if(port_to_sync_write_position_p_gain.size() > 0) + else if ((*module_it)->getControlMode() == CurrentControl) { - for(std::map::iterator _it = port_to_sync_write_position_p_gain.begin(); _it != port_to_sync_write_position_p_gain.end(); _it++) - { - _it->second->TxPacket(); - _it->second->ClearParam(); - } + dxl_state->goal_current_ = result_state->goal_current_; + + if (gazebo_mode_ == false) + { + uint32_t curr_data = dxl->convertCurrent2Value(dxl_state->goal_current_); + uint8_t sync_write_data[2] = { 0 }; + sync_write_data[0] = DXL_LOBYTE(curr_data); + sync_write_data[1] = DXL_HIBYTE(curr_data); + + if (port_to_sync_write_torque_[dxl->port_name_] != NULL) + port_to_sync_write_torque_[dxl->port_name_]->changeParam(dxl->id_, sync_write_data); + } } - for(std::map::iterator _it = port_to_sync_write_position.begin(); _it != port_to_sync_write_position.end(); _it++) - if(_it->second != NULL) _it->second->TxPacket(); - for(std::map::iterator _it = port_to_sync_write_velocity.begin(); _it != port_to_sync_write_velocity.end(); _it++) - if(_it->second != NULL) _it->second->TxPacket(); - for(std::map::iterator _it = port_to_sync_write_torque.begin(); _it != port_to_sync_write_torque.end(); _it++) - if(_it->second != NULL) _it->second->TxPacket(); + } } - else if(gazebo_mode == true) - { - std_msgs::Float64 _joint_msg; + } - for(std::map::iterator dxl_it = robot->dxls.begin(); dxl_it != robot->dxls.end(); dxl_it++) - { - _joint_msg.data = dxl_it->second->dxl_state->goal_position; - gazebo_joint_pub[dxl_it->first].publish(_joint_msg); - } - } + queue_mutex_.unlock(); } - else if(controller_mode_ == DIRECT_CONTROL_MODE) + + if (DEBUG_PRINT) { - queue_mutex_.lock(); - - for(std::map::iterator _it = port_to_sync_write_position.begin(); _it != port_to_sync_write_position.end(); _it++) - { - _it->second->TxPacket(); - _it->second->ClearParam(); - } - - if(direct_sync_write_.size() > 0) - { - for(int _i = 0; _i < direct_sync_write_.size(); _i++) - { - direct_sync_write_[_i]->TxPacket(); - direct_sync_write_[_i]->ClearParam(); - } - direct_sync_write_.clear(); - } - - queue_mutex_.unlock(); + time_duration = ros::Time::now() - start_time; + ROS_INFO("(%2.6f) MotionModule Process() & save result", time_duration.nsec * 0.000001); } - // TODO: User Read/Write - - // BulkRead Tx - if(gazebo_mode == false) + // SyncWrite + if (gazebo_mode_ == false && do_sync_write) { - for(std::map::iterator _it = port_to_bulk_read.begin(); _it != port_to_bulk_read.end(); _it++) - _it->second->TxPacket(); - } - - if(DEBUG_PRINT) - { - _time_duration = ros::Time::now() - _start_time; - ROS_INFO("(%2.6f) SyncWrite & BulkRead Tx", _time_duration.nsec * 0.000001); - } - - // publish present & goal position - for(std::map::iterator dxl_it = robot->dxls.begin(); dxl_it != robot->dxls.end(); dxl_it++) - { - std::string _joint_name = dxl_it->first; - Dynamixel *_dxl = dxl_it->second; - - _present_state.name.push_back(_joint_name); - _present_state.position.push_back(_dxl->dxl_state->present_position); - _present_state.velocity.push_back(_dxl->dxl_state->present_velocity); - _present_state.effort.push_back(_dxl->dxl_state->present_current); - - _goal_state.name.push_back(_joint_name); - _goal_state.position.push_back(_dxl->dxl_state->goal_position); - _goal_state.velocity.push_back(_dxl->dxl_state->goal_velocity); - _goal_state.effort.push_back(_dxl->dxl_state->goal_current); - } - - // -> publish present joint_states & goal joint states topic - present_joint_state_pub.publish(_present_state); - goal_joint_state_pub.publish(_goal_state); - - if(DEBUG_PRINT) - { - _time_duration = ros::Time::now() - _start_time; - ROS_WARN("(%2.6f) Process() DONE", _time_duration.nsec * 0.000001); - } - - _is_process_running = false; -} - -void RobotisController::AddMotionModule(MotionModule *module) -{ - // check whether the module name already exists - for(std::list::iterator _m_it = motion_modules_.begin(); _m_it != motion_modules_.end(); _m_it++) - { - if((*_m_it)->GetModuleName() == module->GetModuleName()) + if (port_to_sync_write_position_p_gain_.size() > 0) + { + for (std::map::iterator it = port_to_sync_write_position_p_gain_.begin(); + it != port_to_sync_write_position_p_gain_.end(); it++) { - ROS_ERROR("Motion Module Name [%s] already exist !!", module->GetModuleName().c_str()); - return; + it->second->txPacket(); + it->second->clearParam(); } + } + for (std::map::iterator it = port_to_sync_write_position_.begin(); + it != port_to_sync_write_position_.end(); it++) + { + if (it->second != NULL) + it->second->txPacket(); + } + for (std::map::iterator it = port_to_sync_write_velocity_.begin(); + it != port_to_sync_write_velocity_.end(); it++) + { + if (it->second != NULL) + it->second->txPacket(); + } + for (std::map::iterator it = port_to_sync_write_torque_.begin(); + it != port_to_sync_write_torque_.end(); it++) + { + if (it->second != NULL) + it->second->txPacket(); + } } - - module->Initialize(CONTROL_CYCLE_MSEC, robot); - motion_modules_.push_back(module); - motion_modules_.unique(); -} - -void RobotisController::RemoveMotionModule(MotionModule *module) -{ - motion_modules_.remove(module); -} - -void RobotisController::AddSensorModule(SensorModule *module) -{ - // check whether the module name already exists - for(std::list::iterator _m_it = sensor_modules_.begin(); _m_it != sensor_modules_.end(); _m_it++) + else if (gazebo_mode_ == true) { - if((*_m_it)->GetModuleName() == module->GetModuleName()) - { - ROS_ERROR("Sensor Module Name [%s] already exist !!", module->GetModuleName().c_str()); - return; - } + std_msgs::Float64 joint_msg; + + for (std::map::iterator dxl_it = robot_->dxls_.begin(); dxl_it != robot_->dxls_.end(); + dxl_it++) + { + joint_msg.data = dxl_it->second->dxl_state_->goal_position_; + gazebo_joint_pub_[dxl_it->first].publish(joint_msg); + } } - - module->Initialize(CONTROL_CYCLE_MSEC, robot); - sensor_modules_.push_back(module); - sensor_modules_.unique(); -} - -void RobotisController::RemoveSensorModule(SensorModule *module) -{ - sensor_modules_.remove(module); -} - -void RobotisController::SyncWriteItemCallback(const robotis_controller_msgs::SyncWriteItem::ConstPtr &msg) -{ - for(int _i = 0; _i < msg->joint_name.size(); _i++) - { - Dynamixel *_dxl = robot->dxls[msg->joint_name[_i]]; - ControlTableItem *_item = _dxl->ctrl_table[msg->item_name]; - - PortHandler *_port = robot->ports[_dxl->port_name]; - PacketHandler *_packet_handler= PacketHandler::GetPacketHandler(_dxl->protocol_version); - - if(_item->access_type == READ) - continue; - - int _idx = 0; - if(direct_sync_write_.size() == 0) - { - direct_sync_write_.push_back(new GroupSyncWrite(_port, _packet_handler, _item->address, _item->data_length)); - _idx = 0; - } - else - { - for(_idx = 0; _idx < direct_sync_write_.size(); _idx++) - { - if(direct_sync_write_[_idx]->GetPortHandler() == _port && - direct_sync_write_[_idx]->GetPacketHandler() == _packet_handler) - break; - } - - if(_idx == direct_sync_write_.size()) - direct_sync_write_.push_back(new GroupSyncWrite(_port, _packet_handler, _item->address, _item->data_length)); - } - - UINT8_T *_data = new UINT8_T[_item->data_length]; - if(_item->data_length == 1) - _data[0] = (UINT8_T)msg->value[_i]; - else if(_item->data_length == 2) - { - _data[0] = DXL_LOBYTE((UINT16_T)msg->value[_i]); - _data[1] = DXL_HIBYTE((UINT16_T)msg->value[_i]); - } - else if(_item->data_length == 4) - { - _data[0] = DXL_LOBYTE(DXL_LOWORD((UINT32_T)msg->value[_i])); - _data[1] = DXL_HIBYTE(DXL_LOWORD((UINT32_T)msg->value[_i])); - _data[2] = DXL_LOBYTE(DXL_HIWORD((UINT32_T)msg->value[_i])); - _data[3] = DXL_HIBYTE(DXL_HIWORD((UINT32_T)msg->value[_i])); - } - direct_sync_write_[_idx]->AddParam(_dxl->id, _data); - delete[] _data; - } -} - -void RobotisController::SetControllerModeCallback(const std_msgs::String::ConstPtr &msg) -{ - if(msg->data == "DIRECT_CONTROL_MODE") - controller_mode_ = DIRECT_CONTROL_MODE; - else if(msg->data == "MOTION_MODULE_MODE") - controller_mode_ = MOTION_MODULE_MODE; -} - -void RobotisController::SetJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg) -{ - if(controller_mode_ != DIRECT_CONTROL_MODE) - return; - + } + else if (controller_mode_ == DirectControlMode) + { queue_mutex_.lock(); - for(int _i = 0; _i < msg->name.size(); _i++) + for (std::map::iterator it = port_to_sync_write_position_.begin(); + it != port_to_sync_write_position_.end(); it++) { - INT32_T _pos = 0; + it->second->txPacket(); + it->second->clearParam(); + } - Dynamixel *_dxl = robot->dxls[msg->name[_i]]; - if(_dxl == NULL) - continue; - - _dxl->dxl_state->goal_position = msg->position[_i]; - _pos = _dxl->ConvertRadian2Value((double)msg->position[_i]); - - UINT8_T _sync_write_data[4]; - _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_pos)); - _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_pos)); - _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_pos)); - _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_pos)); - - if(port_to_sync_write_position[_dxl->port_name] != NULL) - port_to_sync_write_position[_dxl->port_name]->AddParam(_dxl->id, _sync_write_data); + if (direct_sync_write_.size() > 0) + { + for (int i = 0; i < direct_sync_write_.size(); i++) + { + direct_sync_write_[i]->txPacket(); + direct_sync_write_[i]->clearParam(); + } + direct_sync_write_.clear(); } queue_mutex_.unlock(); + } + + // TODO: User Read/Write + + // BulkRead Tx + if (gazebo_mode_ == false) + { + for (std::map::iterator it = port_to_bulk_read_.begin(); it != port_to_bulk_read_.end(); it++) + it->second->txPacket(); + } + + if (DEBUG_PRINT) + { + time_duration = ros::Time::now() - start_time; + ROS_INFO("(%2.6f) SyncWrite & BulkRead Tx", time_duration.nsec * 0.000001); + } + + // publish present & goal position + for (std::map::iterator dxl_it = robot_->dxls_.begin(); dxl_it != robot_->dxls_.end(); dxl_it++) + { + std::string joint_name = dxl_it->first; + Dynamixel *dxl = dxl_it->second; + + present_state.name.push_back(joint_name); + present_state.position.push_back(dxl->dxl_state_->present_position_); + present_state.velocity.push_back(dxl->dxl_state_->present_velocity_); + present_state.effort.push_back(dxl->dxl_state_->present_current_); + + goal_state.name.push_back(joint_name); + goal_state.position.push_back(dxl->dxl_state_->goal_position_); + goal_state.velocity.push_back(dxl->dxl_state_->goal_velocity_); + goal_state.effort.push_back(dxl->dxl_state_->goal_current_); + } + + // -> publish present joint_states & goal joint states topic + present_joint_state_pub_.publish(present_state); + goal_joint_state_pub_.publish(goal_state); + + if (DEBUG_PRINT) + { + time_duration = ros::Time::now() - start_time; + ROS_WARN("(%2.6f) Process() DONE", time_duration.nsec * 0.000001); + } + + is_process_running = false; } -void RobotisController::SetCtrlModuleCallback(const std_msgs::String::ConstPtr &msg) +void RobotisController::addMotionModule(MotionModule *module) { - std::string _module_name_to_set = msg->data; + // check whether the module name already exists + for (std::list::iterator m_it = motion_modules_.begin(); m_it != motion_modules_.end(); m_it++) + { + if ((*m_it)->getModuleName() == module->getModuleName()) + { + ROS_ERROR("Motion Module Name [%s] already exist !!", module->getModuleName().c_str()); + return; + } + } - set_module_thread_ = boost::thread(boost::bind(&RobotisController::SetCtrlModuleThread, this, _module_name_to_set)); + module->initialize(CONTROL_CYCLE_MSEC, robot_); + motion_modules_.push_back(module); + motion_modules_.unique(); } -void RobotisController::SetJointCtrlModuleCallback(const robotis_controller_msgs::JointCtrlModule::ConstPtr &msg) +void RobotisController::removeMotionModule(MotionModule *module) { - if(msg->joint_name.size() != msg->module_name.size()) - return; - - queue_mutex_.lock(); - - for(unsigned int idx = 0; idx < msg->joint_name.size(); idx++) - { - Dynamixel *_dxl = NULL; - std::map::iterator _dxl_it = robot->dxls.find((std::string)(msg->joint_name[idx])); - if(_dxl_it != robot->dxls.end()) - _dxl = _dxl_it->second; - else - continue; - - // none - if(msg->module_name[idx] == "" || msg->module_name[idx] == "none") - { - _dxl->ctrl_module_name = msg->module_name[idx]; - continue; - } - - // check whether the module is using this joint - for(std::list::iterator _m_it = motion_modules_.begin(); _m_it != motion_modules_.end(); _m_it++) - { - if((*_m_it)->GetModuleName() == msg->module_name[idx]) - { - if((*_m_it)->result.find(msg->joint_name[idx]) != (*_m_it)->result.end()) - { - _dxl->ctrl_module_name = msg->module_name[idx]; - break; - } - } - } - } - - for(std::list::iterator _m_it = motion_modules_.begin(); _m_it != motion_modules_.end(); _m_it++) - { - // set all modules -> disable - (*_m_it)->SetModuleEnable(false); - - // set all used modules -> enable - for(std::map::iterator _d_it = robot->dxls.begin(); _d_it != robot->dxls.end(); _d_it++) - { - if(_d_it->second->ctrl_module_name == (*_m_it)->GetModuleName()) - { - (*_m_it)->SetModuleEnable(true); - break; - } - } - } - - // TODO: set indirect address - // -> check module's control_mode - - queue_mutex_.unlock(); - - robotis_controller_msgs::JointCtrlModule _current_module_msg; - for(std::map::iterator _dxl_iter = robot->dxls.begin(); _dxl_iter != robot->dxls.end(); ++_dxl_iter) - { - _current_module_msg.joint_name.push_back(_dxl_iter->first); - _current_module_msg.module_name.push_back(_dxl_iter->second->ctrl_module_name); - } - - if(_current_module_msg.joint_name.size() == _current_module_msg.module_name.size()) - current_module_pub.publish(_current_module_msg); + motion_modules_.remove(module); } -bool RobotisController::GetCtrlModuleCallback(robotis_controller_msgs::GetJointModule::Request &req, robotis_controller_msgs::GetJointModule::Response &res) +void RobotisController::addSensorModule(SensorModule *module) { - for(unsigned int idx = 0; idx < req.joint_name.size(); idx++) + // check whether the module name already exists + for (std::list::iterator m_it = sensor_modules_.begin(); m_it != sensor_modules_.end(); m_it++) + { + if ((*m_it)->getModuleName() == module->getModuleName()) { - std::map::iterator _d_it = robot->dxls.find((std::string)(req.joint_name[idx])); - if(_d_it != robot->dxls.end()) - { - res.joint_name.push_back(req.joint_name[idx]); - res.module_name.push_back(_d_it->second->ctrl_module_name); - } + ROS_ERROR("Sensor Module Name [%s] already exist !!", module->getModuleName().c_str()); + return; } + } - if(res.joint_name.size() == 0) return false; - - return true; + module->initialize(CONTROL_CYCLE_MSEC, robot_); + sensor_modules_.push_back(module); + sensor_modules_.unique(); } -void RobotisController::SetCtrlModuleThread(std::string ctrl_module) +void RobotisController::removeSensorModule(SensorModule *module) { - // stop module - std::list _stop_modules; + sensor_modules_.remove(module); +} - if(ctrl_module == "" || ctrl_module == "none") +void RobotisController::syncWriteItemCallback(const robotis_controller_msgs::SyncWriteItem::ConstPtr &msg) +{ + for (int i = 0; i < msg->joint_name.size(); i++) + { + Dynamixel *dxl = robot_->dxls_[msg->joint_name[i]]; + ControlTableItem *item = dxl->ctrl_table_[msg->item_name]; + + dynamixel::PortHandler *port = robot_->ports_[dxl->port_name_]; + dynamixel::PacketHandler *packet_handler = dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + + if (item->access_type_ == Read) + continue; + + int idx = 0; + if (direct_sync_write_.size() == 0) { - // enqueue all modules in order to stop - for(std::list::iterator _m_it = motion_modules_.begin(); _m_it != motion_modules_.end(); _m_it++) - { - if((*_m_it)->GetModuleEnable() == true) _stop_modules.push_back(*_m_it); - } + direct_sync_write_.push_back(new dynamixel::GroupSyncWrite(port, packet_handler, item->address_, item->data_length_)); + idx = 0; } else { - for(std::list::iterator _m_it = motion_modules_.begin(); _m_it != motion_modules_.end(); _m_it++) - { - // if it exist - if((*_m_it)->GetModuleName() == ctrl_module) - { - // enqueue the module which lost control of joint in order to stop - for(std::map::iterator _result_it = (*_m_it)->result.begin(); _result_it != (*_m_it)->result.end(); _result_it++) - { - std::map::iterator _d_it = robot->dxls.find(_result_it->first); + for (idx = 0; idx < direct_sync_write_.size(); idx++) + { + if (direct_sync_write_[idx]->getPortHandler() == port && direct_sync_write_[idx]->getPacketHandler() == packet_handler) + break; + } - if(_d_it != robot->dxls.end()) - { - // enqueue - if(_d_it->second->ctrl_module_name != ctrl_module) - { - for(std::list::iterator _stop_m_it = motion_modules_.begin(); _stop_m_it != motion_modules_.end(); _stop_m_it++) - { - if((*_stop_m_it)->GetModuleName() == _d_it->second->ctrl_module_name && (*_stop_m_it)->GetModuleEnable() == true) - _stop_modules.push_back(*_stop_m_it); - } - } - } - } - - break; - } - } + if (idx == direct_sync_write_.size()) + direct_sync_write_.push_back(new dynamixel::GroupSyncWrite(port, packet_handler, item->address_, item->data_length_)); } - // stop the module - _stop_modules.unique(); - for(std::list::iterator _stop_m_it = _stop_modules.begin(); _stop_m_it != _stop_modules.end(); _stop_m_it++) + uint8_t *data = new uint8_t[item->data_length_]; + if (item->data_length_ == 1) + data[0] = (uint8_t) msg->value[i]; + else if (item->data_length_ == 2) { - (*_stop_m_it)->Stop(); + data[0] = DXL_LOBYTE((uint16_t )msg->value[i]); + data[1] = DXL_HIBYTE((uint16_t )msg->value[i]); } - - // wait to stop - for(std::list::iterator _stop_m_it = _stop_modules.begin(); _stop_m_it != _stop_modules.end(); _stop_m_it++) + else if (item->data_length_ == 4) { - while((*_stop_m_it)->IsRunning()) - usleep(CONTROL_CYCLE_MSEC * 1000); + data[0] = DXL_LOBYTE(DXL_LOWORD((uint32_t)msg->value[i])); + data[1] = DXL_HIBYTE(DXL_LOWORD((uint32_t)msg->value[i])); + data[2] = DXL_LOBYTE(DXL_HIWORD((uint32_t)msg->value[i])); + data[3] = DXL_HIBYTE(DXL_HIWORD((uint32_t)msg->value[i])); } + direct_sync_write_[idx]->addParam(dxl->id_, data); + delete[] data; + } +} - // set ctrl module - queue_mutex_.lock(); +void RobotisController::setControllerModeCallback(const std_msgs::String::ConstPtr &msg) +{ + if (msg->data == "DirectControlMode") + controller_mode_ = DirectControlMode; + else if (msg->data == "MotionModuleMode") + controller_mode_ = MotionModuleMode; +} - if(DEBUG_PRINT) ROS_INFO_STREAM("set module : " << ctrl_module); +void RobotisController::setJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg) +{ + if (controller_mode_ != DirectControlMode) + return; + + queue_mutex_.lock(); + + for (int i = 0; i < msg->name.size(); i++) + { + int32_t pos = 0; + + Dynamixel *dxl = robot_->dxls_[msg->name[i]]; + if (dxl == NULL) + continue; + + dxl->dxl_state_->goal_position_ = msg->position[i]; + pos = dxl->convertRadian2Value((double) msg->position[i]); + + uint8_t sync_write_data[4]; + sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(pos)); + sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(pos)); + sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(pos)); + sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(pos)); + + if (port_to_sync_write_position_[dxl->port_name_] != NULL) + port_to_sync_write_position_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); + } + + queue_mutex_.unlock(); +} + +void RobotisController::setCtrlModuleCallback(const std_msgs::String::ConstPtr &msg) +{ + std::string _module_name_to_set = msg->data; + + set_module_thread_ = boost::thread(boost::bind(&RobotisController::setCtrlModuleThread, this, _module_name_to_set)); +} + +void RobotisController::setJointCtrlModuleCallback(const robotis_controller_msgs::JointCtrlModule::ConstPtr &msg) +{ + if (msg->joint_name.size() != msg->module_name.size()) + return; + + queue_mutex_.lock(); + + for (unsigned int idx = 0; idx < msg->joint_name.size(); idx++) + { + Dynamixel *dxl = NULL; + std::map::iterator dxl_it = robot_->dxls_.find((std::string) (msg->joint_name[idx])); + if (dxl_it != robot_->dxls_.end()) + dxl = dxl_it->second; + else + continue; // none - if(ctrl_module == "" || ctrl_module == "none") + if (msg->module_name[idx] == "" || msg->module_name[idx] == "none") { - // set all modules -> disable - for(std::list::iterator _m_it = motion_modules_.begin(); _m_it != motion_modules_.end(); _m_it++) - { - (*_m_it)->SetModuleEnable(false); - } - - // set dxl's control module to "none" - for(std::map::iterator _d_it = robot->dxls.begin(); _d_it != robot->dxls.end(); _d_it++) - { - Dynamixel *_dxl = _d_it->second; - _dxl->ctrl_module_name = "none"; - - if(gazebo_mode == true) - continue; - - UINT32_T _pos_data = _dxl->ConvertRadian2Value(_dxl->dxl_state->goal_position + _dxl->dxl_state->position_offset); - UINT8_T _sync_write_data[4]; - _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_pos_data)); - _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_pos_data)); - _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_pos_data)); - _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_pos_data)); - - if(port_to_sync_write_position[_dxl->port_name] != NULL) - port_to_sync_write_position[_dxl->port_name]->AddParam(_dxl->id, _sync_write_data); - - if(port_to_sync_write_torque[_dxl->port_name] != NULL) - port_to_sync_write_torque[_dxl->port_name]->RemoveParam(_dxl->id); - if(port_to_sync_write_velocity[_dxl->port_name] != NULL) - port_to_sync_write_velocity[_dxl->port_name]->RemoveParam(_dxl->id); - } + dxl->ctrl_module_name_ = msg->module_name[idx]; + continue; } - else + + // check whether the module is using this joint + for (std::list::iterator m_it = motion_modules_.begin(); m_it != motion_modules_.end(); m_it++) { - // check whether the module exist - for(std::list::iterator _m_it = motion_modules_.begin(); _m_it != motion_modules_.end(); _m_it++) + if ((*m_it)->getModuleName() == msg->module_name[idx]) + { + if ((*m_it)->result_.find(msg->joint_name[idx]) != (*m_it)->result_.end()) { - // if it exist - if((*_m_it)->GetModuleName() == ctrl_module) + dxl->ctrl_module_name_ = msg->module_name[idx]; + break; + } + } + } + } + + for (std::list::iterator m_it = motion_modules_.begin(); m_it != motion_modules_.end(); m_it++) + { + // set all modules -> disable + (*m_it)->setModuleEnable(false); + + // set all used modules -> enable + for (std::map::iterator d_it = robot_->dxls_.begin(); d_it != robot_->dxls_.end(); d_it++) + { + if (d_it->second->ctrl_module_name_ == (*m_it)->getModuleName()) + { + (*m_it)->setModuleEnable(true); + break; + } + } + } + + // TODO: set indirect address + // -> check module's control_mode + + queue_mutex_.unlock(); + + robotis_controller_msgs::JointCtrlModule current_module_msg; + for (std::map::iterator dxl_iter = robot_->dxls_.begin(); dxl_iter != robot_->dxls_.end(); ++dxl_iter) + { + current_module_msg.joint_name.push_back(dxl_iter->first); + current_module_msg.module_name.push_back(dxl_iter->second->ctrl_module_name_); + } + + if (current_module_msg.joint_name.size() == current_module_msg.module_name.size()) + current_module_pub_.publish(current_module_msg); +} + +bool RobotisController::getCtrlModuleCallback(robotis_controller_msgs::GetJointModule::Request &req, + robotis_controller_msgs::GetJointModule::Response &res) +{ + for (unsigned int idx = 0; idx < req.joint_name.size(); idx++) + { + std::map::iterator d_it = robot_->dxls_.find((std::string) (req.joint_name[idx])); + if (d_it != robot_->dxls_.end()) + { + res.joint_name.push_back(req.joint_name[idx]); + res.module_name.push_back(d_it->second->ctrl_module_name_); + } + } + + if (res.joint_name.size() == 0) + return false; + + return true; +} + +void RobotisController::setCtrlModuleThread(std::string ctrl_module) +{ + // stop module + std::list stop_modules; + + if (ctrl_module == "" || ctrl_module == "none") + { + // enqueue all modules in order to stop + for (std::list::iterator m_it = motion_modules_.begin(); m_it != motion_modules_.end(); m_it++) + { + if ((*m_it)->getModuleEnable() == true) + stop_modules.push_back(*m_it); + } + } + else + { + for (std::list::iterator m_it = motion_modules_.begin(); m_it != motion_modules_.end(); m_it++) + { + // if it exist + if ((*m_it)->getModuleName() == ctrl_module) + { + // enqueue the module which lost control of joint in order to stop + for (std::map::iterator result_it = (*m_it)->result_.begin(); + result_it != (*m_it)->result_.end(); result_it++) + { + std::map::iterator d_it = robot_->dxls_.find(result_it->first); + + if (d_it != robot_->dxls_.end()) + { + // enqueue + if (d_it->second->ctrl_module_name_ != ctrl_module) { - CONTROL_MODE _mode = (*_m_it)->GetControlMode(); - for(std::map::iterator _result_it = (*_m_it)->result.begin(); _result_it != (*_m_it)->result.end(); _result_it++) + for (std::list::iterator stop_m_it = motion_modules_.begin(); + stop_m_it != motion_modules_.end(); stop_m_it++) + { + if (((*stop_m_it)->getModuleName() == d_it->second->ctrl_module_name_) && + ((*stop_m_it)->getModuleEnable() == true)) { - std::map::iterator _d_it = robot->dxls.find(_result_it->first); - if(_d_it != robot->dxls.end()) - { - Dynamixel *_dxl = _d_it->second; - _dxl->ctrl_module_name = ctrl_module; - - if(gazebo_mode == true) - continue; - - if(_mode == POSITION_CONTROL) - { - UINT32_T _pos_data = _dxl->ConvertRadian2Value(_dxl->dxl_state->goal_position + _dxl->dxl_state->position_offset); - UINT8_T _sync_write_data[4]; - _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_pos_data)); - _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_pos_data)); - _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_pos_data)); - _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_pos_data)); - - if(port_to_sync_write_position[_dxl->port_name] != NULL) - port_to_sync_write_position[_dxl->port_name]->AddParam(_dxl->id, _sync_write_data); - - if(port_to_sync_write_torque[_dxl->port_name] != NULL) - port_to_sync_write_torque[_dxl->port_name]->RemoveParam(_dxl->id); - if(port_to_sync_write_velocity[_dxl->port_name] != NULL) - port_to_sync_write_velocity[_dxl->port_name]->RemoveParam(_dxl->id); - } - else if(_mode == VELOCITY_CONTROL) - { - UINT32_T _vel_data = _dxl->ConvertVelocity2Value(_dxl->dxl_state->goal_velocity); - UINT8_T _sync_write_data[4]; - _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_vel_data)); - _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_vel_data)); - _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_vel_data)); - _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_vel_data)); - - if(port_to_sync_write_velocity[_dxl->port_name] != NULL) - port_to_sync_write_velocity[_dxl->port_name]->AddParam(_dxl->id, _sync_write_data); - - if(port_to_sync_write_torque[_dxl->port_name] != NULL) - port_to_sync_write_torque[_dxl->port_name]->RemoveParam(_dxl->id); - if(port_to_sync_write_position[_dxl->port_name] != NULL) - port_to_sync_write_position[_dxl->port_name]->RemoveParam(_dxl->id); - } - else if(_mode == CURRENT_CONTROL) - { - UINT32_T _curr_data = _dxl->ConvertCurrent2Value(_dxl->dxl_state->goal_current); - UINT8_T _sync_write_data[4]; - _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_curr_data)); - _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_curr_data)); - _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_curr_data)); - _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_curr_data)); - - if(port_to_sync_write_torque[_dxl->port_name] != NULL) - port_to_sync_write_torque[_dxl->port_name]->AddParam(_dxl->id, _sync_write_data); - - if(port_to_sync_write_velocity[_dxl->port_name] != NULL) - port_to_sync_write_velocity[_dxl->port_name]->RemoveParam(_dxl->id); - if(port_to_sync_write_position[_dxl->port_name] != NULL) - port_to_sync_write_position[_dxl->port_name]->RemoveParam(_dxl->id); - } - } + stop_modules.push_back(*stop_m_it); } - - break; + } } + } } + + break; + } + } + } + + // stop the module + stop_modules.unique(); + for (std::list::iterator stop_m_it = stop_modules.begin(); stop_m_it != stop_modules.end(); stop_m_it++) + { + (*stop_m_it)->stop(); + } + + // wait to stop + for (std::list::iterator stop_m_it = stop_modules.begin(); stop_m_it != stop_modules.end(); stop_m_it++) + { + while ((*stop_m_it)->isRunning()) + usleep(CONTROL_CYCLE_MSEC * 1000); + } + + // set ctrl module + queue_mutex_.lock(); + + if (DEBUG_PRINT) + ROS_INFO_STREAM("set module : " << ctrl_module); + + // none + if ((ctrl_module == "") || (ctrl_module == "none")) + { + // set all modules -> disable + for (std::list::iterator m_it = motion_modules_.begin(); m_it != motion_modules_.end(); m_it++) + { + (*m_it)->setModuleEnable(false); } - for(std::list::iterator _m_it = motion_modules_.begin(); _m_it != motion_modules_.end(); _m_it++) + // set dxl's control module to "none" + for (std::map::iterator d_it = robot_->dxls_.begin(); d_it != robot_->dxls_.end(); d_it++) { - // set all modules -> disable - (*_m_it)->SetModuleEnable(false); + Dynamixel *dxl = d_it->second; + dxl->ctrl_module_name_ = "none"; - // set all used modules -> enable - for(std::map::iterator _d_it = robot->dxls.begin(); _d_it != robot->dxls.end(); _d_it++) + if (gazebo_mode_ == true) + continue; + + uint32_t pos_data = dxl->convertRadian2Value(dxl->dxl_state_->goal_position_ + dxl->dxl_state_->position_offset_); + uint8_t sync_write_data[4] = { 0 }; + sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(pos_data)); + sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(pos_data)); + sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(pos_data)); + sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(pos_data)); + + if (port_to_sync_write_position_[dxl->port_name_] != NULL) + port_to_sync_write_position_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); + + if (port_to_sync_write_torque_[dxl->port_name_] != NULL) + port_to_sync_write_torque_[dxl->port_name_]->removeParam(dxl->id_); + if (port_to_sync_write_velocity_[dxl->port_name_] != NULL) + port_to_sync_write_velocity_[dxl->port_name_]->removeParam(dxl->id_); + } + } + else + { + // check whether the module exist + for (std::list::iterator m_it = motion_modules_.begin(); m_it != motion_modules_.end(); m_it++) + { + // if it exist + if ((*m_it)->getModuleName() == ctrl_module) + { + ControlMode mode = (*m_it)->getControlMode(); + for (std::map::iterator result_it = (*m_it)->result_.begin(); + result_it != (*m_it)->result_.end(); result_it++) { - if(_d_it->second->ctrl_module_name == (*_m_it)->GetModuleName()) + std::map::iterator d_it = robot_->dxls_.find(result_it->first); + if (d_it != robot_->dxls_.end()) + { + Dynamixel *dxl = d_it->second; + dxl->ctrl_module_name_ = ctrl_module; + + if (gazebo_mode_ == true) + continue; + + if (mode == PositionControl) { - (*_m_it)->SetModuleEnable(true); - break; + uint32_t pos_data = dxl->convertRadian2Value(dxl->dxl_state_->goal_position_ + dxl->dxl_state_->position_offset_); + uint8_t sync_write_data[4] = { 0 }; + sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(pos_data)); + sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(pos_data)); + sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(pos_data)); + sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(pos_data)); + + if (port_to_sync_write_position_[dxl->port_name_] != NULL) + port_to_sync_write_position_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); + + if (port_to_sync_write_torque_[dxl->port_name_] != NULL) + port_to_sync_write_torque_[dxl->port_name_]->removeParam(dxl->id_); + if (port_to_sync_write_velocity_[dxl->port_name_] != NULL) + port_to_sync_write_velocity_[dxl->port_name_]->removeParam(dxl->id_); } + else if (mode == VelocityControl) + { + uint32_t vel_data = dxl->convertVelocity2Value(dxl->dxl_state_->goal_velocity_); + uint8_t sync_write_data[4] = { 0 }; + sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(vel_data)); + sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(vel_data)); + sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(vel_data)); + sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(vel_data)); + + if (port_to_sync_write_velocity_[dxl->port_name_] != NULL) + port_to_sync_write_velocity_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); + + if (port_to_sync_write_torque_[dxl->port_name_] != NULL) + port_to_sync_write_torque_[dxl->port_name_]->removeParam(dxl->id_); + if (port_to_sync_write_position_[dxl->port_name_] != NULL) + port_to_sync_write_position_[dxl->port_name_]->removeParam(dxl->id_); + } + else if (mode == CurrentControl) + { + uint32_t curr_data = dxl->convertCurrent2Value(dxl->dxl_state_->goal_current_); + uint8_t sync_write_data[4] = { 0 }; + sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(curr_data)); + sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(curr_data)); + sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(curr_data)); + sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(curr_data)); + + if (port_to_sync_write_torque_[dxl->port_name_] != NULL) + port_to_sync_write_torque_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); + + if (port_to_sync_write_velocity_[dxl->port_name_] != NULL) + port_to_sync_write_velocity_[dxl->port_name_]->removeParam(dxl->id_); + if (port_to_sync_write_position_[dxl->port_name_] != NULL) + port_to_sync_write_position_[dxl->port_name_]->removeParam(dxl->id_); + } + } } - } - // TODO: set indirect address - // -> check module's control_mode - - queue_mutex_.unlock(); - - // publish current module - robotis_controller_msgs::JointCtrlModule _current_module_msg; - for(std::map::iterator _dxl_iter = robot->dxls.begin(); _dxl_iter != robot->dxls.end(); ++_dxl_iter) - { - _current_module_msg.joint_name.push_back(_dxl_iter->first); - _current_module_msg.module_name.push_back(_dxl_iter->second->ctrl_module_name); - } - - if(_current_module_msg.joint_name.size() == _current_module_msg.module_name.size()) - current_module_pub.publish(_current_module_msg); -} - -void RobotisController::GazeboJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg) -{ - queue_mutex_.lock(); - - for(unsigned int _i = 0; _i < msg->name.size(); _i++) - { - std::map::iterator _d_it = robot->dxls.find((std::string)msg->name[_i]); - if(_d_it != robot->dxls.end()) - { - _d_it->second->dxl_state->present_position = msg->position[_i]; - _d_it->second->dxl_state->present_velocity = msg->velocity[_i]; - _d_it->second->dxl_state->present_current = msg->effort[_i]; - } - } - - if(init_pose_loaded_ == false) - { - for(std::map::iterator _it = robot->dxls.begin(); _it != robot->dxls.end(); _it++) - _it->second->dxl_state->goal_position = _it->second->dxl_state->present_position; - init_pose_loaded_ = true; - } - - queue_mutex_.unlock(); -} - -bool RobotisController::CheckTimerStop() -{ - if(this->is_timer_running_) - { - if(DEBUG_PRINT == true) - ROS_WARN("Process Timer is running.. STOP the timer first."); - return false; - } - return true; -} - -int RobotisController::Ping(const std::string joint_name, UINT8_T *error) -{ - if(CheckTimerStop() == false) - return COMM_PORT_BUSY; - - Dynamixel *_dxl = robot->dxls[joint_name]; - if(_dxl == NULL) - return COMM_NOT_AVAILABLE; - - PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); - PortHandler *_port_handler = robot->ports[_dxl->port_name]; - - return _pkt_handler->Ping(_port_handler, _dxl->id, error); -} -int RobotisController::Ping(const std::string joint_name, UINT16_T* model_number, UINT8_T *error) -{ - if(CheckTimerStop() == false) - return COMM_PORT_BUSY; - - Dynamixel *_dxl = robot->dxls[joint_name]; - if(_dxl == NULL) - return COMM_NOT_AVAILABLE; - - PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); - PortHandler *_port_handler = robot->ports[_dxl->port_name]; - - return _pkt_handler->Ping(_port_handler, _dxl->id, model_number, error); -} - -int RobotisController::Action(const std::string joint_name) -{ - if(CheckTimerStop() == false) - return COMM_PORT_BUSY; - - Dynamixel *_dxl = robot->dxls[joint_name]; - if(_dxl == NULL) - return COMM_NOT_AVAILABLE; - - PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); - PortHandler *_port_handler = robot->ports[_dxl->port_name]; - - return _pkt_handler->Action(_port_handler, _dxl->id); -} -int RobotisController::Reboot(const std::string joint_name, UINT8_T *error) -{ - if(CheckTimerStop() == false) - return COMM_PORT_BUSY; - - Dynamixel *_dxl = robot->dxls[joint_name]; - if(_dxl == NULL) - return COMM_NOT_AVAILABLE; - - PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); - PortHandler *_port_handler = robot->ports[_dxl->port_name]; - - return _pkt_handler->Reboot(_port_handler, _dxl->id, error); -} -int RobotisController::FactoryReset(const std::string joint_name, UINT8_T option, UINT8_T *error) -{ - if(CheckTimerStop() == false) - return COMM_PORT_BUSY; - - Dynamixel *_dxl = robot->dxls[joint_name]; - if(_dxl == NULL) - return COMM_NOT_AVAILABLE; - - PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); - PortHandler *_port_handler = robot->ports[_dxl->port_name]; - - return _pkt_handler->FactoryReset(_port_handler, _dxl->id, option, error); -} - -int RobotisController::Read(const std::string joint_name, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error) -{ - if(CheckTimerStop() == false) - return COMM_PORT_BUSY; - - Dynamixel *_dxl = robot->dxls[joint_name]; - if(_dxl == NULL) - return COMM_NOT_AVAILABLE; - - PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); - PortHandler *_port_handler = robot->ports[_dxl->port_name]; - - return _pkt_handler->ReadTxRx(_port_handler, _dxl->id, address, length, data, error); -} - -int RobotisController::ReadCtrlItem(const std::string joint_name, const std::string item_name, UINT32_T *data, UINT8_T *error) -{ - if(CheckTimerStop() == false) - return COMM_PORT_BUSY; - - Dynamixel *_dxl = robot->dxls[joint_name]; - if(_dxl == NULL) - return COMM_NOT_AVAILABLE; - - ControlTableItem*_item = _dxl->ctrl_table[item_name]; - if(_item == NULL) - return COMM_NOT_AVAILABLE; - - PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); - PortHandler *_port_handler = robot->ports[_dxl->port_name]; - - int _result = COMM_NOT_AVAILABLE; - switch(_item->data_length) - { - case 1: - { - UINT8_T _data = 0; - _result = _pkt_handler->Read1ByteTxRx(_port_handler, _dxl->id, _item->address, &_data, error); - if(_result == COMM_SUCCESS) - *data = _data; break; + } } - case 2: + } + + for (std::list::iterator m_it = motion_modules_.begin(); m_it != motion_modules_.end(); m_it++) + { + // set all modules -> disable + (*m_it)->setModuleEnable(false); + + // set all used modules -> enable + for (std::map::iterator d_it = robot_->dxls_.begin(); d_it != robot_->dxls_.end(); d_it++) { - UINT16_T _data = 0; - _result = _pkt_handler->Read2ByteTxRx(_port_handler, _dxl->id, _item->address, &_data, error); - if(_result == COMM_SUCCESS) - *data = _data; + if (d_it->second->ctrl_module_name_ == (*m_it)->getModuleName()) + { + (*m_it)->setModuleEnable(true); break; + } } - case 4: + } + + // TODO: set indirect address + // -> check module's control_mode + + queue_mutex_.unlock(); + + // publish current module + robotis_controller_msgs::JointCtrlModule current_module_msg; + for (std::map::iterator dxl_iter = robot_->dxls_.begin(); dxl_iter != robot_->dxls_.end(); ++dxl_iter) + { + current_module_msg.joint_name.push_back(dxl_iter->first); + current_module_msg.module_name.push_back(dxl_iter->second->ctrl_module_name_); + } + + if (current_module_msg.joint_name.size() == current_module_msg.module_name.size()) + current_module_pub_.publish(current_module_msg); +} + +void RobotisController::gazeboJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg) +{ + queue_mutex_.lock(); + + for (unsigned int i = 0; i < msg->name.size(); i++) + { + std::map::iterator d_it = robot_->dxls_.find((std::string) msg->name[i]); + if (d_it != robot_->dxls_.end()) { - UINT32_T _data = 0; - _result = _pkt_handler->Read4ByteTxRx(_port_handler, _dxl->id, _item->address, &_data, error); - if(_result == COMM_SUCCESS) - *data = _data; - break; + d_it->second->dxl_state_->present_position_ = msg->position[i]; + d_it->second->dxl_state_->present_velocity_ = msg->velocity[i]; + d_it->second->dxl_state_->present_current_ = msg->effort[i]; } - default: - break; - } - return _result; + } + + if (init_pose_loaded_ == false) + { + for (std::map::iterator it = robot_->dxls_.begin(); it != robot_->dxls_.end(); it++) + it->second->dxl_state_->goal_position_ = it->second->dxl_state_->present_position_; + init_pose_loaded_ = true; + } + + queue_mutex_.unlock(); } -int RobotisController::Read1Byte(const std::string joint_name, UINT16_T address, UINT8_T *data, UINT8_T *error) +bool RobotisController::isTimerStopped() { - if(CheckTimerStop() == false) - return COMM_PORT_BUSY; - - Dynamixel *_dxl = robot->dxls[joint_name]; - if(_dxl == NULL) - return COMM_NOT_AVAILABLE; - - PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); - PortHandler *_port_handler = robot->ports[_dxl->port_name]; - - return _pkt_handler->Read1ByteTxRx(_port_handler, _dxl->id, address, data, error); + if (this->is_timer_running_) + { + if (DEBUG_PRINT == true) + ROS_WARN("Process Timer is running.. STOP the timer first."); + return false; + } + return true; } -int RobotisController::Read2Byte(const std::string joint_name, UINT16_T address, UINT16_T *data, UINT8_T *error) +int RobotisController::ping(const std::string joint_name, uint8_t *error) { - if(CheckTimerStop() == false) - return COMM_PORT_BUSY; - - Dynamixel *_dxl = robot->dxls[joint_name]; - if(_dxl == NULL) - return COMM_NOT_AVAILABLE; - - PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); - PortHandler *_port_handler = robot->ports[_dxl->port_name]; - - return _pkt_handler->Read2ByteTxRx(_port_handler, _dxl->id, address, data, error); + return ping(joint_name, 0, error); } - -int RobotisController::Read4Byte(const std::string joint_name, UINT16_T address, UINT32_T *data, UINT8_T *error) +int RobotisController::ping(const std::string joint_name, uint16_t* model_number, uint8_t *error) { - if(CheckTimerStop() == false) - return COMM_PORT_BUSY; + if (isTimerStopped() == false) + return COMM_PORT_BUSY; - Dynamixel *_dxl = robot->dxls[joint_name]; - if(_dxl == NULL) - return COMM_NOT_AVAILABLE; + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; - PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); - PortHandler *_port_handler = robot->ports[_dxl->port_name]; + dynamixel::PacketHandler *pkt_handler = dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; - return _pkt_handler->Read4ByteTxRx(_port_handler, _dxl->id, address, data, error); + return pkt_handler->ping(port_handler, dxl->id_, model_number, error); } -int RobotisController::Write(const std::string joint_name, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error) +int RobotisController::action(const std::string joint_name) { - if(CheckTimerStop() == false) - return COMM_PORT_BUSY; + if (isTimerStopped() == false) + return COMM_PORT_BUSY; - Dynamixel *_dxl = robot->dxls[joint_name]; - if(_dxl == NULL) - return COMM_NOT_AVAILABLE; + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; - PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); - PortHandler *_port_handler = robot->ports[_dxl->port_name]; + dynamixel::PacketHandler *pkt_handler = dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; - return _pkt_handler->WriteTxRx(_port_handler, _dxl->id, address, length, data, error); + return pkt_handler->action(port_handler, dxl->id_); } - -int RobotisController::WriteCtrlItem(const std::string joint_name, const std::string item_name, UINT32_T data, UINT8_T *error) +int RobotisController::reboot(const std::string joint_name, uint8_t *error) { - if(CheckTimerStop() == false) - return COMM_PORT_BUSY; + if (isTimerStopped() == false) + return COMM_PORT_BUSY; - Dynamixel *_dxl = robot->dxls[joint_name]; - if(_dxl == NULL) - return COMM_NOT_AVAILABLE; + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; - ControlTableItem*_item = _dxl->ctrl_table[item_name]; - if(_item == NULL) - return COMM_NOT_AVAILABLE; + dynamixel::PacketHandler *pkt_handler = dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; - PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); - PortHandler *_port_handler = robot->ports[_dxl->port_name]; - - int _result = COMM_NOT_AVAILABLE; - UINT8_T *_data = new UINT8_T[_item->data_length]; - if(_item->data_length == 1) - { - _data[0] = (UINT8_T)data; - _result = _pkt_handler->Write1ByteTxRx(_port_handler, _dxl->id, _item->address, data, error); - } - else if(_item->data_length == 2) - { - _data[0] = DXL_LOBYTE((UINT16_T)data); - _data[1] = DXL_HIBYTE((UINT16_T)data); - _result = _pkt_handler->Write2ByteTxRx(_port_handler, _dxl->id, _item->address, data, error); - } - else if(_item->data_length == 4) - { - _data[0] = DXL_LOBYTE(DXL_LOWORD((UINT32_T)data)); - _data[1] = DXL_HIBYTE(DXL_LOWORD((UINT32_T)data)); - _data[2] = DXL_LOBYTE(DXL_HIWORD((UINT32_T)data)); - _data[3] = DXL_HIBYTE(DXL_HIWORD((UINT32_T)data)); - _result = _pkt_handler->Write4ByteTxRx(_port_handler, _dxl->id, _item->address, data, error); - } - delete[] _data; - return _result; + return pkt_handler->reboot(port_handler, dxl->id_, error); } - -int RobotisController::Write1Byte(const std::string joint_name, UINT16_T address, UINT8_T data, UINT8_T *error) +int RobotisController::factoryReset(const std::string joint_name, uint8_t option, uint8_t *error) { - if(CheckTimerStop() == false) - return COMM_PORT_BUSY; + if (isTimerStopped() == false) + return COMM_PORT_BUSY; - Dynamixel *_dxl = robot->dxls[joint_name]; - if(_dxl == NULL) - return COMM_NOT_AVAILABLE; + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; - PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); - PortHandler *_port_handler = robot->ports[_dxl->port_name]; + dynamixel::PacketHandler *pkt_handler = dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; - return _pkt_handler->Write1ByteTxRx(_port_handler, _dxl->id, address, data, error); + return pkt_handler->factoryReset(port_handler, dxl->id_, option, error); } -int RobotisController::Write2Byte(const std::string joint_name, UINT16_T address, UINT16_T data, UINT8_T *error) +int RobotisController::read(const std::string joint_name, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error) { - if(CheckTimerStop() == false) - return COMM_PORT_BUSY; + if (isTimerStopped() == false) + return COMM_PORT_BUSY; - Dynamixel *_dxl = robot->dxls[joint_name]; - if(_dxl == NULL) - return COMM_NOT_AVAILABLE; + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; - PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); - PortHandler *_port_handler = robot->ports[_dxl->port_name]; + dynamixel::PacketHandler *pkt_handler = dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; - return _pkt_handler->Write2ByteTxRx(_port_handler, _dxl->id, address, data, error); + return pkt_handler->readTxRx(port_handler, dxl->id_, address, length, data, error); } -int RobotisController::Write4Byte(const std::string joint_name, UINT16_T address, UINT32_T data, UINT8_T *error) +int RobotisController::readCtrlItem(const std::string joint_name, const std::string item_name, uint32_t *data, uint8_t *error) { - if(CheckTimerStop() == false) - return COMM_PORT_BUSY; + if (isTimerStopped() == false) + return COMM_PORT_BUSY; - Dynamixel *_dxl = robot->dxls[joint_name]; - if(_dxl == NULL) - return COMM_NOT_AVAILABLE; + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; - PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); - PortHandler *_port_handler = robot->ports[_dxl->port_name]; + ControlTableItem *item = dxl->ctrl_table_[item_name]; + if (item == NULL) + return COMM_NOT_AVAILABLE; - return _pkt_handler->Write4ByteTxRx(_port_handler, _dxl->id, address, data, error); + dynamixel::PacketHandler *pkt_handler = dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; + + int result = COMM_NOT_AVAILABLE; + switch (item->data_length_) + { + case 1: + { + uint8_t read_data = 0; + result = pkt_handler->read1ByteTxRx(port_handler, dxl->id_, item->address_, &read_data, error); + if (result == COMM_SUCCESS) + *data = read_data; + break; + } + case 2: + { + uint16_t read_data = 0; + result = pkt_handler->read2ByteTxRx(port_handler, dxl->id_, item->address_, &read_data, error); + if (result == COMM_SUCCESS) + *data = read_data; + break; + } + case 4: + { + uint32_t read_data = 0; + result = pkt_handler->read4ByteTxRx(port_handler, dxl->id_, item->address_, &read_data, error); + if (result == COMM_SUCCESS) + *data = read_data; + break; + } + default: + break; + } + return result; } -int RobotisController::RegWrite(const std::string joint_name, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error) +int RobotisController::read1Byte(const std::string joint_name, uint16_t address, uint8_t *data, uint8_t *error) { - if(CheckTimerStop() == false) - return COMM_PORT_BUSY; + if (isTimerStopped() == false) + return COMM_PORT_BUSY; - Dynamixel *_dxl = robot->dxls[joint_name]; - if(_dxl == NULL) - return COMM_NOT_AVAILABLE; + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; - PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); - PortHandler *_port_handler = robot->ports[_dxl->port_name]; + dynamixel::PacketHandler *pkt_handler = dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; - return _pkt_handler->RegWriteTxRx(_port_handler, _dxl->id, address, length, data, error); + return pkt_handler->read1ByteTxRx(port_handler, dxl->id_, address, data, error); +} + +int RobotisController::read2Byte(const std::string joint_name, uint16_t address, uint16_t *data, uint8_t *error) +{ + if (isTimerStopped() == false) + return COMM_PORT_BUSY; + + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; + + dynamixel::PacketHandler *pkt_handler = dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; + + return pkt_handler->read2ByteTxRx(port_handler, dxl->id_, address, data, error); +} + +int RobotisController::read4Byte(const std::string joint_name, uint16_t address, uint32_t *data, uint8_t *error) +{ + if (isTimerStopped() == false) + return COMM_PORT_BUSY; + + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; + + dynamixel::PacketHandler *pkt_handler = dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; + + return pkt_handler->read4ByteTxRx(port_handler, dxl->id_, address, data, error); +} + +int RobotisController::write(const std::string joint_name, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error) +{ + if (isTimerStopped() == false) + return COMM_PORT_BUSY; + + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; + + dynamixel::PacketHandler *pkt_handler = dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; + + return pkt_handler->writeTxRx(port_handler, dxl->id_, address, length, data, error); +} + +int RobotisController::writeCtrlItem(const std::string joint_name, const std::string item_name, uint32_t data, uint8_t *error) +{ + if (isTimerStopped() == false) + return COMM_PORT_BUSY; + + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; + + ControlTableItem *item = dxl->ctrl_table_[item_name]; + if (item == NULL) + return COMM_NOT_AVAILABLE; + + dynamixel::PacketHandler *pkt_handler = dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; + + int result = COMM_NOT_AVAILABLE; + uint8_t *write_data = new uint8_t[item->data_length_]; + if (item->data_length_ == 1) + { + write_data[0] = (uint8_t) data; + result = pkt_handler->write1ByteTxRx(port_handler, dxl->id_, item->address_, data, error); + } + else if (item->data_length_ == 2) + { + write_data[0] = DXL_LOBYTE((uint16_t )data); + write_data[1] = DXL_HIBYTE((uint16_t )data); + result = pkt_handler->write2ByteTxRx(port_handler, dxl->id_, item->address_, data, error); + } + else if (item->data_length_ == 4) + { + write_data[0] = DXL_LOBYTE(DXL_LOWORD((uint32_t)data)); + write_data[1] = DXL_HIBYTE(DXL_LOWORD((uint32_t)data)); + write_data[2] = DXL_LOBYTE(DXL_HIWORD((uint32_t)data)); + write_data[3] = DXL_HIBYTE(DXL_HIWORD((uint32_t)data)); + result = pkt_handler->write4ByteTxRx(port_handler, dxl->id_, item->address_, data, error); + } + delete[] write_data; + return result; +} + +int RobotisController::write1Byte(const std::string joint_name, uint16_t address, uint8_t data, uint8_t *error) +{ + if (isTimerStopped() == false) + return COMM_PORT_BUSY; + + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; + + dynamixel::PacketHandler *pkt_handler = dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; + + return pkt_handler->write1ByteTxRx(port_handler, dxl->id_, address, data, error); +} + +int RobotisController::write2Byte(const std::string joint_name, uint16_t address, uint16_t data, uint8_t *error) +{ + if (isTimerStopped() == false) + return COMM_PORT_BUSY; + + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; + + dynamixel::PacketHandler *pkt_handler = dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; + + return pkt_handler->write2ByteTxRx(port_handler, dxl->id_, address, data, error); +} + +int RobotisController::write4Byte(const std::string joint_name, uint16_t address, uint32_t data, uint8_t *error) +{ + if (isTimerStopped() == false) + return COMM_PORT_BUSY; + + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; + + dynamixel::PacketHandler *pkt_handler = dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; + + return pkt_handler->write4ByteTxRx(port_handler, dxl->id_, address, data, error); +} + +int RobotisController::regWrite(const std::string joint_name, uint16_t address, uint16_t length, uint8_t *data, + uint8_t *error) +{ + if (isTimerStopped() == false) + return COMM_PORT_BUSY; + + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; + + dynamixel::PacketHandler *pkt_handler = dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; + + return pkt_handler->regWriteTxRx(port_handler, dxl->id_, address, length, data, error); } diff --git a/robotis_controller_msgs/package.xml b/robotis_controller_msgs/package.xml index 3ce5db7..8d54012 100644 --- a/robotis_controller_msgs/package.xml +++ b/robotis_controller_msgs/package.xml @@ -9,7 +9,7 @@ - robotis --> + ROBOTIS catkin @@ -21,10 +21,4 @@ std_msgs message_runtime - - - - - - \ No newline at end of file diff --git a/robotis_device/CMakeLists.txt b/robotis_device/CMakeLists.txt index 8bd7be5..6c53ff0 100644 --- a/robotis_device/CMakeLists.txt +++ b/robotis_device/CMakeLists.txt @@ -8,15 +8,6 @@ find_package(catkin REQUIRED COMPONENTS robotis_framework_common ) -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if you package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need catkin_package( INCLUDE_DIRS include LIBRARIES robotis_device @@ -24,38 +15,19 @@ catkin_package( # DEPENDS system_lib ) -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -# include_directories(include) include_directories( include ${catkin_INCLUDE_DIRS} ) -## Declare a C++ library add_library(robotis_device - src/robotis_device/Robot.cpp - src/robotis_device/Sensor.cpp - src/robotis_device/Dynamixel.cpp + src/robotis_device/robot.cpp + src/robotis_device/sensor.cpp + src/robotis_device/dynamixel.cpp ) -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure add_dependencies(robotis_device ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -## Declare a C++ executable -# add_executable(robotis_device_node src/robotis_device_node.cpp) - -## Add cmake target dependencies of the executable -## same as for the library above -# add_dependencies(robotis_device_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Specify libraries to link a library or executable target against target_link_libraries(robotis_device ${catkin_LIBRARIES} ) diff --git a/robotis_device/include/robotis_device/control_table_item.h b/robotis_device/include/robotis_device/control_table_item.h index e7b75fd..28a840a 100644 --- a/robotis_device/include/robotis_device/control_table_item.h +++ b/robotis_device/include/robotis_device/control_table_item.h @@ -1,54 +1,84 @@ +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * 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 ROBOTIS 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. + *******************************************************************************/ + /* - * ControlTableItem.h + * control_table_item.h * * Created on: 2015. 12. 16. * Author: zerom */ -#ifndef ROBOTIS_FRAMEWORK_ROBOTIS_DEVICE_INCLUDE_DEVICE_CONTROLTABLEITEM_H_ -#define ROBOTIS_FRAMEWORK_ROBOTIS_DEVICE_INCLUDE_DEVICE_CONTROLTABLEITEM_H_ +#ifndef ROBOTIS_DEVICE_CONTROL_TABLE_ITEM_H_ +#define ROBOTIS_DEVICE_CONTROL_TABLE_ITEM_H_ -#include +#include -namespace ROBOTIS +namespace robotis_framework { -enum ACCESS_TYPE { - READ, - READ_WRITE +enum AccessType { + Read, + ReadWrite }; -enum MEMORY_TYPE { - EEPROM, - RAM +enum MemoryType { + EEPROM, + RAM }; class ControlTableItem { public: - std::string item_name; - UINT16_T address; - ACCESS_TYPE access_type; - MEMORY_TYPE memory_type; - UINT8_T data_length; - INT32_T data_min_value; - INT32_T data_max_value; - bool is_signed; + std::string item_name_; + uint16_t address_; + AccessType access_type_; + MemoryType memory_type_; + uint8_t data_length_; + int32_t data_min_value_; + int32_t data_max_value_; + bool is_signed_; - ControlTableItem() - : item_name(""), - address(0), - access_type(READ), - memory_type(RAM), - data_length(0), - data_min_value(0), - data_max_value(0), - is_signed(false) - { } + ControlTableItem() + : item_name_(""), + address_(0), + access_type_(Read), + memory_type_(RAM), + data_length_(0), + data_min_value_(0), + data_max_value_(0), + is_signed_(false) + { } }; } -#endif /* ROBOTIS_FRAMEWORK_ROBOTIS_DEVICE_INCLUDE_DEVICE_CONTROLTABLEITEM_H_ */ +#endif /* ROBOTIS_DEVICE_CONTROL_TABLE_ITEM_H_ */ diff --git a/robotis_device/include/robotis_device/device.h b/robotis_device/include/robotis_device/device.h index d9fd0bf..0a5556e 100644 --- a/robotis_device/include/robotis_device/device.h +++ b/robotis_device/include/robotis_device/device.h @@ -1,37 +1,68 @@ +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * 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 ROBOTIS 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. + *******************************************************************************/ + /* - * Device.h + * device.h * * Created on: 2016. 5. 12. * Author: zerom */ -#ifndef ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_DEVICE_H_ -#define ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_DEVICE_H_ +#ifndef ROBOTIS_DEVICE_DEVICE_H_ +#define ROBOTIS_DEVICE_DEVICE_H_ #include #include #include -#include "ControlTableItem.h" -namespace ROBOTIS +#include "control_table_item.h" + +namespace robotis_framework { class Device { public: - UINT8_T id; - float protocol_version; - std::string model_name; - std::string port_name; + uint8_t id_; + float protocol_version_; + std::string model_name_; + std::string port_name_; - std::map ctrl_table; - std::vector bulk_read_items; + std::map ctrl_table_; + std::vector bulk_read_items_; - virtual ~Device() { } + virtual ~Device() { } }; } -#endif /* ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_DEVICE_H_ */ +#endif /* ROBOTIS_DEVICE_DEVICE_H_ */ diff --git a/robotis_device/include/robotis_device/dynamixel.h b/robotis_device/include/robotis_device/dynamixel.h index a0a9beb..916f4c5 100644 --- a/robotis_device/include/robotis_device/dynamixel.h +++ b/robotis_device/include/robotis_device/dynamixel.h @@ -1,61 +1,92 @@ +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * 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 ROBOTIS 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. + *******************************************************************************/ + /* - * Dynamixel.h + * dynamixel.h * * Created on: 2015. 12. 8. * Author: zerom */ -#ifndef ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_DYNAMIXEL_H_ -#define ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_DYNAMIXEL_H_ +#ifndef ROBOTIS_DEVICE_DYNAMIXEL_H_ +#define ROBOTIS_DEVICE_DYNAMIXEL_H_ #include #include #include -#include "Device.h" -#include "DynamixelState.h" -#include "ControlTableItem.h" -namespace ROBOTIS +#include "control_table_item.h" +#include "device.h" +#include "dynamixel_state.h" + +namespace robotis_framework { class Dynamixel : public Device { public: - std::string ctrl_module_name; - DynamixelState *dxl_state; + std::string ctrl_module_name_; + DynamixelState *dxl_state_; - double current_ratio; - double velocity_ratio; + double current_ratio_; + double velocity_ratio_; - INT32_T value_of_0_radian_position; - INT32_T value_of_min_radian_position; - INT32_T value_of_max_radian_position; - double min_radian; - double max_radian; + int32_t value_of_0_radian_position_; + int32_t value_of_min_radian_position_; + int32_t value_of_max_radian_position_; + double min_radian_; + double max_radian_; - ControlTableItem *torque_enable_item; - ControlTableItem *present_position_item; - ControlTableItem *present_velocity_item; - ControlTableItem *present_current_item; - ControlTableItem *goal_position_item; - ControlTableItem *goal_velocity_item; - ControlTableItem *goal_current_item; - ControlTableItem *position_p_gain_item; + ControlTableItem *torque_enable_item_; + ControlTableItem *present_position_item_; + ControlTableItem *present_velocity_item_; + ControlTableItem *present_current_item_; + ControlTableItem *goal_position_item_; + ControlTableItem *goal_velocity_item_; + ControlTableItem *goal_current_item_; + ControlTableItem *position_p_gain_item_; - Dynamixel(int id, std::string model_name, float protocol_version); + Dynamixel(int id, std::string model_name, float protocol_version); - double ConvertValue2Radian(INT32_T value); - INT32_T ConvertRadian2Value(double radian); + double convertValue2Radian(int32_t value); + int32_t convertRadian2Value(double radian); - double ConvertValue2Velocity(INT32_T value); - INT32_T ConvertVelocity2Value(double velocity); + double convertValue2Velocity(int32_t value); + int32_t convertVelocity2Value(double velocity); - double ConvertValue2Current(INT16_T value); - INT16_T ConvertCurrent2Value(double torque); + double convertValue2Current(int16_t value); + int16_t convertCurrent2Value(double current); }; } -#endif /* ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_DYNAMIXEL_H_ */ +#endif /* ROBOTIS_DEVICE_DYNAMIXEL_H_ */ diff --git a/robotis_device/include/robotis_device/dynamixel_state.h b/robotis_device/include/robotis_device/dynamixel_state.h index e3768d4..b5f4d4d 100644 --- a/robotis_device/include/robotis_device/dynamixel_state.h +++ b/robotis_device/include/robotis_device/dynamixel_state.h @@ -1,57 +1,86 @@ +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * 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 ROBOTIS 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. + *******************************************************************************/ + /* - * DynamixelState.h + * dynamixel_state.h * * Created on: 2015. 12. 8. * Author: zerom */ -#ifndef ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_DYNAMIXELSTATE_H_ -#define ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_DYNAMIXELSTATE_H_ +#ifndef ROBOTIS_DEVICE_DYNAMIXEL_STATE_H_ +#define ROBOTIS_DEVICE_DYNAMIXEL_STATE_H_ #include -#include "robotis_device/TimeStamp.h" -#include "robotis_framework_common/RobotisDef.h" +#include "time_stamp.h" #define INDIRECT_DATA_1 "indirect_data_1" #define INDIRECT_ADDRESS_1 "indirect_address_1" -namespace ROBOTIS +namespace robotis_framework { class DynamixelState { public: - TimeStamp update_time_stamp; + TimeStamp update_time_stamp_; - double present_position; - double present_velocity; - double present_current; - double goal_position; - double goal_velocity; - double goal_current; - double position_p_gain; + double present_position_; + double present_velocity_; + double present_current_; + double goal_position_; + double goal_velocity_; + double goal_current_; + double position_p_gain_; - std::map bulk_read_table; + std::map bulk_read_table_; - double position_offset; + double position_offset_; - DynamixelState() - : update_time_stamp(0, 0), - present_position(0.0), - present_velocity(0.0), - present_current(0.0), - goal_position(0.0), - goal_velocity(0.0), - goal_current(0.0), - position_p_gain(0.0), - position_offset(0.0) - { - bulk_read_table.clear(); - } + DynamixelState() + : update_time_stamp_(0, 0), + present_position_(0.0), + present_velocity_(0.0), + present_current_(0.0), + goal_position_(0.0), + goal_velocity_(0.0), + goal_current_(0.0), + position_p_gain_(0.0), + position_offset_(0.0) + { + bulk_read_table_.clear(); + } }; } -#endif /* ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_DYNAMIXELSTATE_H_ */ +#endif /* ROBOTIS_DEVICE_DYNAMIXEL_STATE_H_ */ diff --git a/robotis_device/include/robotis_device/robot.h b/robotis_device/include/robotis_device/robot.h index 2473e7c..4dfe5bc 100644 --- a/robotis_device/include/robotis_device/robot.h +++ b/robotis_device/include/robotis_device/robot.h @@ -1,38 +1,78 @@ +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * 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 ROBOTIS 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. + *******************************************************************************/ + /* - * Robot.h + * robot.h * * Created on: 2015. 12. 11. * Author: zerom */ -#ifndef ROBOTIS_FRAMEWORK_ROBOTIS_DEVICE_INCLUDE_DEVICE_ROBOT_H_ -#define ROBOTIS_FRAMEWORK_ROBOTIS_DEVICE_INCLUDE_DEVICE_ROBOT_H_ +#ifndef ROBOTIS_DEVICE_ROBOT_H_ +#define ROBOTIS_DEVICE_ROBOT_H_ #include -#include "Sensor.h" -#include "Dynamixel.h" -#include "dynamixel_sdk/PortHandler.h" -namespace ROBOTIS +#include "sensor.h" +#include "dynamixel.h" +#include "dynamixel_sdk/port_handler.h" + +#define DYNAMIXEL "dynamixel" +#define SENSOR "sensor" + +#define SESSION_PORT_INFO "port info" +#define SESSION_DEVICE_INFO "device info" + +#define SESSION_TYPE_INFO "type info" +#define SESSION_CONTROL_TABLE "control table" + +namespace robotis_framework { class Robot { public: - std::map ports; // string: port name - std::map port_default_device; // port name, default device name + std::map ports_; // string: port name + std::map port_default_device_; // port name, default device name - std::map dxls; // string: joint name - std::map sensors; // string: sensor name + std::map dxls_; // string: joint name + std::map sensors_; // string: sensor name - Robot(std::string robot_file_path, std::string dev_desc_dir_path); + Robot(std::string robot_file_path, std::string dev_desc_dir_path); - Sensor *getSensor(std::string path, int id, std::string port, float protocol_version); - Dynamixel *getDynamixel(std::string path, int id, std::string port, float protocol_version); + Sensor *getSensor(std::string path, int id, std::string port, float protocol_version); + Dynamixel *getDynamixel(std::string path, int id, std::string port, float protocol_version); }; } -#endif /* ROBOTIS_FRAMEWORK_ROBOTIS_DEVICE_INCLUDE_DEVICE_ROBOT_H_ */ +#endif /* ROBOTIS_DEVICE_ROBOT_H_ */ diff --git a/robotis_device/include/robotis_device/sensor.h b/robotis_device/include/robotis_device/sensor.h index 54df44f..2d1b757 100644 --- a/robotis_device/include/robotis_device/sensor.h +++ b/robotis_device/include/robotis_device/sensor.h @@ -1,32 +1,63 @@ +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * 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 ROBOTIS 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. + *******************************************************************************/ + /* - * Sensor.h + * sensor.h * * Created on: 2015. 12. 16. * Author: zerom */ -#ifndef ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_SENSOR_H_ -#define ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_SENSOR_H_ +#ifndef ROBOTIS_DEVICE_SENSOR_H_ +#define ROBOTIS_DEVICE_SENSOR_H_ #include #include #include -#include "Device.h" -#include "SensorState.h" -#include "ControlTableItem.h" -namespace ROBOTIS +#include "device.h" +#include "sensor_state.h" +#include "control_table_item.h" + +namespace robotis_framework { class Sensor : public Device { public: - SensorState *sensor_state; + SensorState *sensor_state_; - Sensor(int id, std::string model_name, float protocol_version); + Sensor(int id, std::string model_name, float protocol_version); }; } -#endif /* ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_SENSOR_H_ */ +#endif /* ROBOTIS_DEVICE_SENSOR_H_ */ diff --git a/robotis_device/include/robotis_device/sensor_state.h b/robotis_device/include/robotis_device/sensor_state.h index 4137b14..f40afa4 100644 --- a/robotis_device/include/robotis_device/sensor_state.h +++ b/robotis_device/include/robotis_device/sensor_state.h @@ -1,35 +1,64 @@ +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * 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 ROBOTIS 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. + *******************************************************************************/ + /* - * SensorState.h + * sensor_state.h * * Created on: 2016. 5. 16. * Author: zerom */ -#ifndef ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_SENSORSTATE_H_ -#define ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_SENSORSTATE_H_ +#ifndef ROBOTIS_DEVICE_SENSOR_STATE_H_ +#define ROBOTIS_DEVICE_SENSOR_STATE_H_ -#include "robotis_device/TimeStamp.h" -#include "robotis_framework_common/RobotisDef.h" +#include "time_stamp.h" -namespace ROBOTIS +namespace robotis_framework { class SensorState { public: - TimeStamp update_time_stamp; + TimeStamp update_time_stamp_; - std::map bulk_read_table; + std::map bulk_read_table_; - SensorState() - : update_time_stamp(0, 0) - { - bulk_read_table.clear(); - } + SensorState() + : update_time_stamp_(0, 0) + { + bulk_read_table_.clear(); + } }; } -#endif /* ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_SENSORSTATE_H_ */ +#endif /* ROBOTIS_DEVICE_SENSOR_STATE_H_ */ diff --git a/robotis_device/include/robotis_device/time_stamp.h b/robotis_device/include/robotis_device/time_stamp.h index 17b749a..b0de1e5 100644 --- a/robotis_device/include/robotis_device/time_stamp.h +++ b/robotis_device/include/robotis_device/time_stamp.h @@ -1,25 +1,59 @@ +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * 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 ROBOTIS 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. + *******************************************************************************/ + /* - * TimeStamp.h + * time_stamp.h * * Created on: 2016. 5. 16. * Author: zerom */ -#ifndef ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_TIMESTAMP_H_ -#define ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_TIMESTAMP_H_ +#ifndef ROBOTIS_DEVICE_TIME_STAMP_H_ +#define ROBOTIS_DEVICE_TIME_STAMP_H_ -namespace ROBOTIS +namespace robotis_framework { class TimeStamp { public: - long sec; - long nsec; - TimeStamp(long sec, long nsec) : sec(sec), nsec(nsec) { } + long sec_; + long nsec_; + + TimeStamp(long sec, long nsec) + : sec_(sec), + nsec_(nsec) + { } }; } -#endif /* ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_TIMESTAMP_H_ */ +#endif /* ROBOTIS_DEVICE_TIME_STAMP_H_ */ diff --git a/robotis_device/package.xml b/robotis_device/package.xml index be72665..8e494db 100644 --- a/robotis_device/package.xml +++ b/robotis_device/package.xml @@ -6,16 +6,12 @@ robotis - BSD - - robotis - catkin roscpp @@ -26,9 +22,4 @@ roscpp rospy dynamixel_sdk - - - - - \ No newline at end of file diff --git a/robotis_device/src/robotis_device/dynamixel.cpp b/robotis_device/src/robotis_device/dynamixel.cpp index 55108ca..c90e4eb 100644 --- a/robotis_device/src/robotis_device/dynamixel.cpp +++ b/robotis_device/src/robotis_device/dynamixel.cpp @@ -1,111 +1,147 @@ +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * 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 ROBOTIS 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. + *******************************************************************************/ + /* - * Dynamixel.cpp + * dynamixel.cpp * * Created on: 2015. 12. 8. * Author: zerom */ -#include "robotis_device/Dynamixel.h" +#include "robotis_device/dynamixel.h" -using namespace ROBOTIS; +using namespace robotis_framework; Dynamixel::Dynamixel(int id, std::string model_name, float protocol_version) - : ctrl_module_name("none"), - current_ratio(1.0), - velocity_ratio(1.0), - value_of_0_radian_position(0), - value_of_min_radian_position(0), - value_of_max_radian_position(0), - min_radian(-3.14159265), - max_radian(3.14159265), - torque_enable_item(0), - present_position_item(0), - present_velocity_item(0), - present_current_item(0), - goal_position_item(0), - goal_velocity_item(0), - goal_current_item(0), - position_p_gain_item(0) + : ctrl_module_name_("none"), + current_ratio_(1.0), + velocity_ratio_(1.0), + value_of_0_radian_position_(0), + value_of_min_radian_position_(0), + value_of_max_radian_position_(0), + min_radian_(-3.14159265), + max_radian_(3.14159265), + torque_enable_item_(0), + present_position_item_(0), + present_velocity_item_(0), + present_current_item_(0), + goal_position_item_(0), + goal_velocity_item_(0), + goal_current_item_(0), + position_p_gain_item_(0) { - this->id = id; - this->model_name = model_name; - this->port_name = ""; - this->protocol_version = protocol_version; + this->id_ = id; + this->model_name_ = model_name; + this->port_name_ = ""; + this->protocol_version_ = protocol_version; - ctrl_table.clear(); - dxl_state = new DynamixelState(); + ctrl_table_.clear(); + dxl_state_ = new DynamixelState(); - bulk_read_items.clear(); + bulk_read_items_.clear(); } -double Dynamixel::ConvertValue2Radian(INT32_T value) +double Dynamixel::convertValue2Radian(int32_t value) { - double _radian = 0.0; - if(value > value_of_0_radian_position) - { - if(max_radian <= 0) - return max_radian; - _radian = (double)(value - value_of_0_radian_position) * max_radian / (double)(value_of_max_radian_position - value_of_0_radian_position); - } - else if(value < value_of_0_radian_position) - { - if(min_radian >= 0) - return min_radian; - _radian = (double)(value - value_of_0_radian_position) * min_radian / (double)(value_of_min_radian_position - value_of_0_radian_position); - } + double radian = 0.0; + if (value > value_of_0_radian_position_) + { + if (max_radian_ <= 0) + return max_radian_; - if(_radian > max_radian) - return max_radian; - else if(_radian < min_radian) - return min_radian; + radian = (double) (value - value_of_0_radian_position_) * max_radian_ + / (double) (value_of_max_radian_position_ - value_of_0_radian_position_); + } + else if (value < value_of_0_radian_position_) + { + if (min_radian_ >= 0) + return min_radian_; - return _radian; + radian = (double) (value - value_of_0_radian_position_) * min_radian_ + / (double) (value_of_min_radian_position_ - value_of_0_radian_position_); + } + + if (radian > max_radian_) + return max_radian_; + else if (radian < min_radian_) + return min_radian_; + + return radian; } -INT32_T Dynamixel::ConvertRadian2Value(double radian) +int32_t Dynamixel::convertRadian2Value(double radian) { - //return radian * value_of_max_radian_position / max_radian; + int32_t value = 0; + if (radian > 0) + { + if (value_of_max_radian_position_ <= value_of_0_radian_position_) + return value_of_max_radian_position_; - INT32_T _value = 0; - if(radian > 0) - { - if(value_of_max_radian_position <= value_of_0_radian_position) - return value_of_max_radian_position; - _value = (radian * (value_of_max_radian_position - value_of_0_radian_position) / max_radian) + value_of_0_radian_position; - } - else if(radian < 0) - { - if(value_of_min_radian_position >= value_of_0_radian_position) - return value_of_min_radian_position; - _value = (radian * (value_of_min_radian_position - value_of_0_radian_position) / min_radian) + value_of_0_radian_position; - } - else - _value = value_of_0_radian_position; + value = (radian * (value_of_max_radian_position_ - value_of_0_radian_position_) / max_radian_) + + value_of_0_radian_position_; + } + else if (radian < 0) + { + if (value_of_min_radian_position_ >= value_of_0_radian_position_) + return value_of_min_radian_position_; - if(_value > value_of_max_radian_position) - return value_of_max_radian_position; - else if(_value < value_of_min_radian_position) - return value_of_min_radian_position; + value = (radian * (value_of_min_radian_position_ - value_of_0_radian_position_) / min_radian_) + + value_of_0_radian_position_; + } + else + value = value_of_0_radian_position_; - return _value; + if (value > value_of_max_radian_position_) + return value_of_max_radian_position_; + else if (value < value_of_min_radian_position_) + return value_of_min_radian_position_; + + return value; } -double Dynamixel::ConvertValue2Velocity(INT32_T value) +double Dynamixel::convertValue2Velocity(int32_t value) { - return (double)value * velocity_ratio; + return (double) value * velocity_ratio_; } -INT32_T Dynamixel::ConvertVelocity2Value(double velocity) +int32_t Dynamixel::convertVelocity2Value(double velocity) { - return (INT32_T)(velocity / velocity_ratio);; + return (int32_t) (velocity / velocity_ratio_);; } -double Dynamixel::ConvertValue2Current(INT16_T value) +double Dynamixel::convertValue2Current(int16_t value) { - return (double)value * current_ratio; + return (double) value * current_ratio_; } -INT16_T Dynamixel::ConvertCurrent2Value(double torque) +int16_t Dynamixel::convertCurrent2Value(double current) { - return (INT16_T)(torque / current_ratio); + return (int16_t) (current / current_ratio_); } diff --git a/robotis_device/src/robotis_device/robot.cpp b/robotis_device/src/robotis_device/robot.cpp index ff4d86b..1ae3fb9 100644 --- a/robotis_device/src/robotis_device/robot.cpp +++ b/robotis_device/src/robotis_device/robot.cpp @@ -1,5 +1,35 @@ +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * 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 ROBOTIS 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. + *******************************************************************************/ + /* - * Robot.cpp + * robot.cpp * * Created on: 2015. 12. 11. * Author: zerom @@ -8,404 +38,420 @@ #include #include #include -#include "../include/robotis_device/Robot.h" -using namespace ROBOTIS; +#include "robotis_device/robot.h" -static inline std::string <rim(std::string &s) { - s.erase(s.begin(), std::find_if(s.begin(), s.end(), std::not1(std::ptr_fun(std::isspace)))); - return s; +using namespace robotis_framework; + +static inline std::string <rim(std::string &s) +{ + s.erase(s.begin(), std::find_if(s.begin(), s.end(), std::not1(std::ptr_fun(std::isspace)))); + return s; } -static inline std::string &rtrim(std::string &s) { - s.erase(std::find_if(s.rbegin(), s.rend(), std::not1(std::ptr_fun(std::isspace))).base(), s.end()); - return s; +static inline std::string &rtrim(std::string &s) +{ + s.erase(std::find_if(s.rbegin(), s.rend(), std::not1(std::ptr_fun(std::isspace))).base(), s.end()); + return s; } -static inline std::string &trim(std::string &s) { - return ltrim(rtrim(s)); +static inline std::string &trim(std::string &s) +{ + return ltrim(rtrim(s)); } -static inline std::vector split(const std::string &text, char sep) { - std::vector tokens; - std::size_t start = 0, end = 0; - while((end = text.find(sep, start)) != (std::string::npos)) { - tokens.push_back(text.substr(start, end - start)); - trim(tokens.back()); - start = end + 1; - } - tokens.push_back(text.substr(start)); +static inline std::vector split(const std::string &text, char sep) +{ + std::vector tokens; + std::size_t start = 0, end = 0; + + while ((end = text.find(sep, start)) != (std::string::npos)) + { + tokens.push_back(text.substr(start, end - start)); trim(tokens.back()); - return tokens; + start = end + 1; + } + tokens.push_back(text.substr(start)); + trim(tokens.back()); + + return tokens; } Robot::Robot(std::string robot_file_path, std::string dev_desc_dir_path) { - if(dev_desc_dir_path.compare(dev_desc_dir_path.size()-1, 1, "/") != 0) - dev_desc_dir_path += "/"; + if (dev_desc_dir_path.compare(dev_desc_dir_path.size() - 1, 1, "/") != 0) + dev_desc_dir_path += "/"; - std::ifstream file(robot_file_path.c_str()); - if(file.is_open()) + std::ifstream file(robot_file_path.c_str()); + if (file.is_open()) + { + std::string session = ""; + std::string input_str; + while (!file.eof()) { - std::string session = ""; - std::string input_str; - while(!file.eof()) + std::getline(file, input_str); + + // remove comment ( # ) + std::size_t pos = input_str.find("#"); + if (pos != std::string::npos) + input_str = input_str.substr(0, pos); + + // trim + input_str = trim(input_str); + + // find session + if (!input_str.compare(0, 1, "[") && !input_str.compare(input_str.size() - 1, 1, "]")) + { + input_str = input_str.substr(1, input_str.size() - 2); + std::transform(input_str.begin(), input_str.end(), input_str.begin(), ::tolower); + session = trim(input_str); + continue; + } + + if (session == SESSION_PORT_INFO) + { + std::vector tokens = split(input_str, '|'); + if (tokens.size() != 3) + continue; + + std::cout << tokens[0] << " added. (baudrate: " << tokens[1] << ")" << std::endl; + + ports_[tokens[0]] = dynamixel::PortHandler::getPortHandler(tokens[0].c_str()); + ports_[tokens[0]]->setBaudRate(std::atoi(tokens[1].c_str())); + port_default_device_[tokens[0]] = tokens[2]; + } + else if (session == SESSION_DEVICE_INFO) + { + std::vector tokens = split(input_str, '|'); + if (tokens.size() != 7) + continue; + + if (tokens[0] == DYNAMIXEL) { - std::getline(file, input_str); + std::string file_path = dev_desc_dir_path + tokens[0] + "/" + tokens[3] + ".device"; + int id = std::atoi(tokens[2].c_str()); + std::string port = tokens[1]; + float protocol = std::atof(tokens[4].c_str()); + std::string dev_name = tokens[5]; - // remove comment ( # ) - std::size_t pos = input_str.find("#"); - if(pos != std::string::npos) - input_str = input_str.substr(0, pos); + dxls_[dev_name] = getDynamixel(file_path, id, port, protocol); - // trim - input_str = trim(input_str); - - // find session - if(!input_str.compare(0, 1, "[") && !input_str.compare(input_str.size()-1, 1, "]")) + Dynamixel *dxl = dxls_[dev_name]; + std::vector sub_tokens = split(tokens[6], ','); + if (sub_tokens.size() > 0) + { + std::map::iterator indirect_it = dxl->ctrl_table_.find(INDIRECT_ADDRESS_1); + if (indirect_it != dxl->ctrl_table_.end()) // INDIRECT_ADDRESS_1 exist { - input_str = input_str.substr(1, input_str.size()-2); - std::transform(input_str.begin(), input_str.end(), input_str.begin(), ::tolower); - session = trim(input_str); - continue; - } + uint16_t indirect_data_addr = dxl->ctrl_table_[INDIRECT_DATA_1]->address_; + for (int _i = 0; _i < sub_tokens.size(); _i++) + { + dxl->bulk_read_items_.push_back(new ControlTableItem()); + ControlTableItem *dest_item = dxl->bulk_read_items_[_i]; + ControlTableItem *src_item = dxl->ctrl_table_[sub_tokens[_i]]; - if(session == "port info") + dest_item->item_name_ = src_item->item_name_; + dest_item->address_ = indirect_data_addr; + dest_item->access_type_ = src_item->access_type_; + dest_item->memory_type_ = src_item->memory_type_; + dest_item->data_length_ = src_item->data_length_; + dest_item->data_min_value_ = src_item->data_min_value_; + dest_item->data_max_value_ = src_item->data_max_value_; + dest_item->is_signed_ = src_item->is_signed_; + + indirect_data_addr += dest_item->data_length_; + } + } + else // INDIRECT_ADDRESS_1 not exist { - std::vector tokens = split(input_str, '|'); - if(tokens.size() != 3) - continue; - - std::cout << tokens[0] << " added. (baudrate: " << tokens[1] << ")" << std::endl; - - ports[tokens[0]] = (PortHandler*)PortHandler::GetPortHandler(tokens[0].c_str()); - ports[tokens[0]]->SetBaudRate(std::atoi(tokens[1].c_str())); - port_default_device[tokens[0]] = tokens[2]; - } - else if(session == "device info") - { - std::vector tokens = split(input_str, '|'); - if(tokens.size() != 7) - continue; - - if(tokens[0] == "dynamixel") - { - std::string _file_path = dev_desc_dir_path + tokens[0] + "/" + tokens[3] + ".device"; - int _id = std::atoi(tokens[2].c_str()); - std::string _port = tokens[1]; - float _protocol = std::atof(tokens[4].c_str()); - std::string _dev_name = tokens[5]; - - dxls[_dev_name] = getDynamixel(_file_path, _id, _port, _protocol); - - Dynamixel *_dxl = dxls[_dev_name]; - std::vector sub_tokens = split(tokens[6], ','); - if(sub_tokens.size() > 0) - { - std::map::iterator _indirect_it = _dxl->ctrl_table.find(INDIRECT_ADDRESS_1); - if(_indirect_it != _dxl->ctrl_table.end()) // INDIRECT_ADDRESS_1 exist - { - UINT16_T _indirect_data_addr = _dxl->ctrl_table[INDIRECT_DATA_1]->address; - for(int _i = 0; _i < sub_tokens.size(); _i++) - { - _dxl->bulk_read_items.push_back(new ControlTableItem()); - ControlTableItem *_dest_item = _dxl->bulk_read_items[_i]; - ControlTableItem *_src_item = _dxl->ctrl_table[sub_tokens[_i]]; - - _dest_item->item_name = _src_item->item_name; - _dest_item->address = _indirect_data_addr; - _dest_item->access_type = _src_item->access_type; - _dest_item->memory_type = _src_item->memory_type; - _dest_item->data_length = _src_item->data_length; - _dest_item->data_min_value = _src_item->data_min_value; - _dest_item->data_max_value = _src_item->data_max_value; - _dest_item->is_signed = _src_item->is_signed; - - _indirect_data_addr += _dest_item->data_length; - } - } - else // INDIRECT_ADDRESS_1 not exist - { - for(int _i = 0; _i < sub_tokens.size(); _i++) - { - if(_dxl->ctrl_table[sub_tokens[_i]] != NULL) - _dxl->bulk_read_items.push_back(_dxl->ctrl_table[sub_tokens[_i]]); - } - } - } - } - else if(tokens[0] == "sensor") - { - std::string _file_path = dev_desc_dir_path + tokens[0] + "/" + tokens[3] + ".device"; - int _id = std::atoi(tokens[2].c_str()); - std::string _port = tokens[1]; - float _protocol = std::atof(tokens[4].c_str()); - std::string _dev_name = tokens[5]; - - sensors[_dev_name] = getSensor(_file_path, _id, _port, _protocol); - - Sensor *_sensor = sensors[_dev_name]; - std::vector sub_tokens = split(tokens[6], ','); - if(sub_tokens.size() > 0) - { - std::map::iterator _indirect_it = _sensor->ctrl_table.find(INDIRECT_ADDRESS_1); - if(_indirect_it != _sensor->ctrl_table.end()) // INDIRECT_ADDRESS_1 exist - { - UINT16_T _indirect_data_addr = _sensor->ctrl_table[INDIRECT_DATA_1]->address; - for(int _i = 0; _i < sub_tokens.size(); _i++) - { - _sensor->bulk_read_items.push_back(new ControlTableItem()); - ControlTableItem *_dest_item = _sensor->bulk_read_items[_i]; - ControlTableItem *_src_item = _sensor->ctrl_table[sub_tokens[_i]]; - - _dest_item->item_name = _src_item->item_name; - _dest_item->address = _indirect_data_addr; - _dest_item->access_type = _src_item->access_type; - _dest_item->memory_type = _src_item->memory_type; - _dest_item->data_length = _src_item->data_length; - _dest_item->data_min_value = _src_item->data_min_value; - _dest_item->data_max_value = _src_item->data_max_value; - _dest_item->is_signed = _src_item->is_signed; - - _indirect_data_addr += _dest_item->data_length; - } - } - else // INDIRECT_ADDRESS_1 exist - { - for(int _i = 0; _i < sub_tokens.size(); _i++) - _sensor->bulk_read_items.push_back(_sensor->ctrl_table[sub_tokens[_i]]); - } - } - } + for (int i = 0; i < sub_tokens.size(); i++) + { + if (dxl->ctrl_table_[sub_tokens[i]] != NULL) + dxl->bulk_read_items_.push_back(dxl->ctrl_table_[sub_tokens[i]]); + } } + } } - file.close(); - } - else - { - std::cout << "Unable to open file : " + robot_file_path << std::endl; + else if (tokens[0] == SENSOR) + { + std::string file_path = dev_desc_dir_path + tokens[0] + "/" + tokens[3] + ".device"; + int id = std::atoi(tokens[2].c_str()); + std::string port = tokens[1]; + float protocol = std::atof(tokens[4].c_str()); + std::string dev_name = tokens[5]; + + sensors_[dev_name] = getSensor(file_path, id, port, protocol); + + Sensor *sensor = sensors_[dev_name]; + std::vector sub_tokens = split(tokens[6], ','); + if (sub_tokens.size() > 0) + { + std::map::iterator indirect_it = sensor->ctrl_table_.find(INDIRECT_ADDRESS_1); + if (indirect_it != sensor->ctrl_table_.end()) // INDIRECT_ADDRESS_1 exist + { + uint16_t indirect_data_addr = sensor->ctrl_table_[INDIRECT_DATA_1]->address_; + for (int i = 0; i < sub_tokens.size(); i++) + { + sensor->bulk_read_items_.push_back(new ControlTableItem()); + ControlTableItem *dest_item = sensor->bulk_read_items_[i]; + ControlTableItem *src_item = sensor->ctrl_table_[sub_tokens[i]]; + + dest_item->item_name_ = src_item->item_name_; + dest_item->address_ = indirect_data_addr; + dest_item->access_type_ = src_item->access_type_; + dest_item->memory_type_ = src_item->memory_type_; + dest_item->data_length_ = src_item->data_length_; + dest_item->data_min_value_ = src_item->data_min_value_; + dest_item->data_max_value_ = src_item->data_max_value_; + dest_item->is_signed_ = src_item->is_signed_; + + indirect_data_addr += dest_item->data_length_; + } + } + else // INDIRECT_ADDRESS_1 exist + { + for (int i = 0; i < sub_tokens.size(); i++) + sensor->bulk_read_items_.push_back(sensor->ctrl_table_[sub_tokens[i]]); + } + } + } + } } + file.close(); + } + else + { + std::cout << "Unable to open file : " + robot_file_path << std::endl; + } } Sensor *Robot::getSensor(std::string path, int id, std::string port, float protocol_version) { - Sensor *_sensor; + Sensor *sensor; - // load file model_name.device - std::ifstream file(path.c_str()); - if(file.is_open()) + // load file model_name.device + std::ifstream file(path.c_str()); + if (file.is_open()) + { + std::string session = ""; + std::string input_str; + + while (!file.eof()) { - std::string _session = ""; - std::string _input_str; + std::getline(file, input_str); - while(!file.eof()) - { - std::getline(file, _input_str); + // remove comment ( # ) + std::size_t pos = input_str.find("#"); + if (pos != std::string::npos) + input_str = input_str.substr(0, pos); - // remove comment ( # ) - std::size_t pos = _input_str.find("#"); - if(pos != std::string::npos) - _input_str = _input_str.substr(0, pos); + // trim + input_str = trim(input_str); + if (input_str == "") + continue; - // trim - _input_str = trim(_input_str); - if(_input_str == "") - continue; + // find _session + if (!input_str.compare(0, 1, "[") && !input_str.compare(input_str.size() - 1, 1, "]")) + { + input_str = input_str.substr(1, input_str.size() - 2); + std::transform(input_str.begin(), input_str.end(), input_str.begin(), ::tolower); + session = trim(input_str); + continue; + } - // find _session - if(!_input_str.compare(0, 1, "[") && !_input_str.compare(_input_str.size()-1, 1, "]")) - { - _input_str = _input_str.substr(1, _input_str.size()-2); - std::transform(_input_str.begin(), _input_str.end(), _input_str.begin(), ::tolower); - _session = trim(_input_str); - continue; - } + if (session == SESSION_DEVICE_INFO) + { + std::vector tokens = split(input_str, '='); + if (tokens.size() != 2) + continue; - if(_session == "device info") - { - std::vector tokens = split(_input_str, '='); - if(tokens.size() != 2) - continue; + if (tokens[0] == "model_name") + sensor = new Sensor(id, tokens[1], protocol_version); + } + else if (session == SESSION_CONTROL_TABLE) + { + std::vector tokens = split(input_str, '|'); + if (tokens.size() != 8) + continue; - if(tokens[0] == "model_name") - _sensor = new Sensor(id, tokens[1], protocol_version); - } - else if(_session == "control table") - { - std::vector tokens = split(_input_str, '|'); - if(tokens.size() != 8) - continue; + ControlTableItem *item = new ControlTableItem(); + item->item_name_ = tokens[1]; + item->address_ = std::atoi(tokens[0].c_str()); + item->data_length_ = std::atoi(tokens[2].c_str()); - ControlTableItem *item = new ControlTableItem(); - item->item_name = tokens[1]; - item->address = std::atoi(tokens[0].c_str()); - item->data_length = std::atoi(tokens[2].c_str()); - if(tokens[3] == "R") - item->access_type = READ; - else if(tokens[3] == "RW") - item->access_type = READ_WRITE; - if(tokens[4] == "EEPROM") - item->memory_type = EEPROM; - else if(tokens[4] == "RAM") - item->memory_type = RAM; - item->data_min_value = std::atol(tokens[5].c_str()); - item->data_max_value = std::atol(tokens[6].c_str()); - if(tokens[7] == "Y") - item->is_signed = true; - else if(tokens[7] == "N") - item->is_signed = false; - _sensor->ctrl_table[tokens[1]] = item; - } - } - _sensor->port_name = port; + if (tokens[3] == "R") + item->access_type_ = Read; + else if (tokens[3] == "RW") + item->access_type_ = ReadWrite; - fprintf(stderr, "(%s) [ID:%3d] %14s added. \n", port.c_str(), _sensor->id, _sensor->model_name.c_str()); - //std::cout << "[ID:" << (int)(_sensor->id) << "] " << _sensor->model_name << " added. (" << port << ")" << std::endl; - file.close(); + if (tokens[4] == "EEPROM") + item->memory_type_ = EEPROM; + else if (tokens[4] == "RAM") + item->memory_type_ = RAM; + + item->data_min_value_ = std::atol(tokens[5].c_str()); + item->data_max_value_ = std::atol(tokens[6].c_str()); + + if (tokens[7] == "Y") + item->is_signed_ = true; + else if (tokens[7] == "N") + item->is_signed_ = false; + sensor->ctrl_table_[tokens[1]] = item; + } } - else - std::cout << "Unable to open file : " + path << std::endl; + sensor->port_name_ = port; - return _sensor; + fprintf(stderr, "(%s) [ID:%3d] %14s added. \n", port.c_str(), sensor->id_, sensor->model_name_.c_str()); + //std::cout << "[ID:" << (int)(_sensor->id) << "] " << _sensor->model_name << " added. (" << port << ")" << std::endl; + file.close(); + } + else + std::cout << "Unable to open file : " + path << std::endl; + + return sensor; } Dynamixel *Robot::getDynamixel(std::string path, int id, std::string port, float protocol_version) { - Dynamixel *_dxl; + Dynamixel *dxl; - // load file model_name.device - std::ifstream file(path.c_str()); - if(file.is_open()) + // load file model_name.device + std::ifstream file(path.c_str()); + if (file.is_open()) + { + std::string session = ""; + std::string input_str; + + std::string torque_enable_item_name = ""; + std::string present_position_item_name = ""; + std::string present_velocity_item_name = ""; + std::string present_current_item_name = ""; + std::string goal_position_item_name = ""; + std::string goal_velocity_item_name = ""; + std::string goal_current_item_name = ""; + + while (!file.eof()) { - std::string _session = ""; - std::string _input_str; + std::getline(file, input_str); - std::string _torque_enable_item_name = ""; - std::string _present_position_item_name = ""; - std::string _present_velocity_item_name = ""; - std::string _present_current_item_name = ""; - std::string _goal_position_item_name = ""; - std::string _goal_velocity_item_name = ""; - std::string _goal_current_item_name = ""; + // remove comment ( # ) + std::size_t pos = input_str.find("#"); + if (pos != std::string::npos) + input_str = input_str.substr(0, pos); - while(!file.eof()) - { - std::getline(file, _input_str); + // trim + input_str = trim(input_str); + if (input_str == "") + continue; - // remove comment ( # ) - std::size_t pos = _input_str.find("#"); - if(pos != std::string::npos) - _input_str = _input_str.substr(0, pos); + // find session + if (!input_str.compare(0, 1, "[") && !input_str.compare(input_str.size() - 1, 1, "]")) + { + input_str = input_str.substr(1, input_str.size() - 2); + std::transform(input_str.begin(), input_str.end(), input_str.begin(), ::tolower); + session = trim(input_str); + continue; + } - // trim - _input_str = trim(_input_str); - if(_input_str == "") - continue; + if (session == SESSION_DEVICE_INFO) + { + std::vector tokens = split(input_str, '='); + if (tokens.size() != 2) + continue; - // find _session - if(!_input_str.compare(0, 1, "[") && !_input_str.compare(_input_str.size()-1, 1, "]")) - { - _input_str = _input_str.substr(1, _input_str.size()-2); - std::transform(_input_str.begin(), _input_str.end(), _input_str.begin(), ::tolower); - _session = trim(_input_str); - continue; - } + if (tokens[0] == "model_name") + dxl = new Dynamixel(id, tokens[1], protocol_version); + } + else if (session == SESSION_TYPE_INFO) + { + std::vector tokens = split(input_str, '='); + if (tokens.size() != 2) + continue; - if(_session == "device info") - { - std::vector tokens = split(_input_str, '='); - if(tokens.size() != 2) - continue; + if (tokens[0] == "current_ratio") + dxl->current_ratio_ = std::atof(tokens[1].c_str()); + else if (tokens[0] == "velocity_ratio") + dxl->velocity_ratio_ = std::atof(tokens[1].c_str()); - if(tokens[0] == "model_name") - _dxl = new Dynamixel(id, tokens[1], protocol_version); - } - else if(_session == "type info") - { - std::vector tokens = split(_input_str, '='); - if(tokens.size() != 2) - continue; + else if (tokens[0] == "value_of_0_radian_position") + dxl->value_of_0_radian_position_ = std::atoi(tokens[1].c_str()); + else if (tokens[0] == "value_of_min_radian_position") + dxl->value_of_min_radian_position_ = std::atoi(tokens[1].c_str()); + else if (tokens[0] == "value_of_max_radian_position") + dxl->value_of_max_radian_position_ = std::atoi(tokens[1].c_str()); + else if (tokens[0] == "min_radian") + dxl->min_radian_ = std::atof(tokens[1].c_str()); + else if (tokens[0] == "max_radian") + dxl->max_radian_ = std::atof(tokens[1].c_str()); - if(tokens[0] == "current_ratio") - _dxl->current_ratio = std::atof(tokens[1].c_str()); - else if(tokens[0] == "velocity_ratio") - _dxl->velocity_ratio = std::atof(tokens[1].c_str()); + else if (tokens[0] == "torque_enable_item_name") + torque_enable_item_name = tokens[1]; + else if (tokens[0] == "present_position_item_name") + present_position_item_name = tokens[1]; + else if (tokens[0] == "present_velocity_item_name") + present_velocity_item_name = tokens[1]; + else if (tokens[0] == "present_current_item_name") + present_current_item_name = tokens[1]; + else if (tokens[0] == "goal_position_item_name") + goal_position_item_name = tokens[1]; + else if (tokens[0] == "goal_velocity_item_name") + goal_velocity_item_name = tokens[1]; + else if (tokens[0] == "goal_current_item_name") + goal_current_item_name = tokens[1]; + } + else if (session == SESSION_CONTROL_TABLE) + { + std::vector tokens = split(input_str, '|'); + if (tokens.size() != 8) + continue; - else if(tokens[0] == "value_of_0_radian_position") - _dxl->value_of_0_radian_position = std::atoi(tokens[1].c_str()); - else if(tokens[0] == "value_of_min_radian_position") - _dxl->value_of_min_radian_position = std::atoi(tokens[1].c_str()); - else if(tokens[0] == "value_of_max_radian_position") - _dxl->value_of_max_radian_position = std::atoi(tokens[1].c_str()); - else if(tokens[0] == "min_radian") - _dxl->min_radian = std::atof(tokens[1].c_str()); - else if(tokens[0] == "max_radian") - _dxl->max_radian = std::atof(tokens[1].c_str()); + ControlTableItem *item = new ControlTableItem(); + item->item_name_ = tokens[1]; + item->address_ = std::atoi(tokens[0].c_str()); + item->data_length_ = std::atoi(tokens[2].c_str()); - else if(tokens[0] == "torque_enable_item_name") - _torque_enable_item_name = tokens[1]; - else if(tokens[0] == "present_position_item_name") - _present_position_item_name = tokens[1]; - else if(tokens[0] == "present_velocity_item_name") - _present_velocity_item_name = tokens[1]; - else if(tokens[0] == "present_current_item_name") - _present_current_item_name = tokens[1]; - else if(tokens[0] == "goal_position_item_name") - _goal_position_item_name = tokens[1]; - else if(tokens[0] == "goal_velocity_item_name") - _goal_velocity_item_name = tokens[1]; - else if(tokens[0] == "goal_current_item_name") - _goal_current_item_name = tokens[1]; - } - else if(_session == "control table") - { - std::vector tokens = split(_input_str, '|'); - if(tokens.size() != 8) - continue; + if (tokens[3] == "R") + item->access_type_ = Read; + else if (tokens[3] == "RW") + item->access_type_ = ReadWrite; - ControlTableItem *item = new ControlTableItem(); - item->item_name = tokens[1]; - item->address = std::atoi(tokens[0].c_str()); - item->data_length = std::atoi(tokens[2].c_str()); - if(tokens[3] == "R") - item->access_type = READ; - else if(tokens[3] == "RW") - item->access_type = READ_WRITE; - if(tokens[4] == "EEPROM") - item->memory_type = EEPROM; - else if(tokens[4] == "RAM") - item->memory_type = RAM; - item->data_min_value = std::atol(tokens[5].c_str()); - item->data_max_value = std::atol(tokens[6].c_str()); - if(tokens[7] == "Y") - item->is_signed = true; - else if(tokens[7] == "N") - item->is_signed = false; - _dxl->ctrl_table[tokens[1]] = item; - } - } - _dxl->port_name = port; + if (tokens[4] == "EEPROM") + item->memory_type_ = EEPROM; + else if (tokens[4] == "RAM") + item->memory_type_ = RAM; - if(_dxl->ctrl_table[_torque_enable_item_name] != NULL) - _dxl->torque_enable_item = _dxl->ctrl_table[_torque_enable_item_name]; - if(_dxl->ctrl_table[_present_position_item_name] != NULL) - _dxl->present_position_item = _dxl->ctrl_table[_present_position_item_name]; - if(_dxl->ctrl_table[_present_velocity_item_name] != NULL) - _dxl->present_velocity_item = _dxl->ctrl_table[_present_velocity_item_name]; - if(_dxl->ctrl_table[_present_current_item_name] != NULL) - _dxl->present_current_item = _dxl->ctrl_table[_present_current_item_name]; - if(_dxl->ctrl_table[_goal_position_item_name] != NULL) - _dxl->goal_position_item = _dxl->ctrl_table[_goal_position_item_name]; - if(_dxl->ctrl_table[_goal_velocity_item_name] != NULL) - _dxl->goal_velocity_item = _dxl->ctrl_table[_goal_velocity_item_name]; - if(_dxl->ctrl_table[_goal_current_item_name] != NULL) - _dxl->goal_current_item = _dxl->ctrl_table[_goal_current_item_name]; + item->data_min_value_ = std::atol(tokens[5].c_str()); + item->data_max_value_ = std::atol(tokens[6].c_str()); - fprintf(stderr, "(%s) [ID:%3d] %14s added. \n", port.c_str(), _dxl->id, _dxl->model_name.c_str()); - //std::cout << "[ID:" << (int)(_dxl->id) << "] " << _dxl->model_name << " added. (" << port << ")" << std::endl; - file.close(); + if (tokens[7] == "Y") + item->is_signed_ = true; + else if (tokens[7] == "N") + item->is_signed_ = false; + dxl->ctrl_table_[tokens[1]] = item; + } } - else - std::cout << "Unable to open file : " + path << std::endl; + dxl->port_name_ = port; - return _dxl; + if (dxl->ctrl_table_[torque_enable_item_name] != NULL) + dxl->torque_enable_item_ = dxl->ctrl_table_[torque_enable_item_name]; + if (dxl->ctrl_table_[present_position_item_name] != NULL) + dxl->present_position_item_ = dxl->ctrl_table_[present_position_item_name]; + if (dxl->ctrl_table_[present_velocity_item_name] != NULL) + dxl->present_velocity_item_ = dxl->ctrl_table_[present_velocity_item_name]; + if (dxl->ctrl_table_[present_current_item_name] != NULL) + dxl->present_current_item_ = dxl->ctrl_table_[present_current_item_name]; + if (dxl->ctrl_table_[goal_position_item_name] != NULL) + dxl->goal_position_item_ = dxl->ctrl_table_[goal_position_item_name]; + if (dxl->ctrl_table_[goal_velocity_item_name] != NULL) + dxl->goal_velocity_item_ = dxl->ctrl_table_[goal_velocity_item_name]; + if (dxl->ctrl_table_[goal_current_item_name] != NULL) + dxl->goal_current_item_ = dxl->ctrl_table_[goal_current_item_name]; + + fprintf(stderr, "(%s) [ID:%3d] %14s added. \n", port.c_str(), dxl->id_, dxl->model_name_.c_str()); + //std::cout << "[ID:" << (int)(_dxl->id) << "] " << _dxl->model_name << " added. (" << port << ")" << std::endl; + file.close(); + } + else + std::cout << "Unable to open file : " + path << std::endl; + + return dxl; } diff --git a/robotis_device/src/robotis_device/sensor.cpp b/robotis_device/src/robotis_device/sensor.cpp index db95494..537989d 100644 --- a/robotis_device/src/robotis_device/sensor.cpp +++ b/robotis_device/src/robotis_device/sensor.cpp @@ -1,23 +1,53 @@ +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * 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 ROBOTIS 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. + *******************************************************************************/ + /* - * Sensor.cpp + * sensor.cpp * * Created on: 2016. 5. 11. * Author: zerom */ -#include "robotis_device/Sensor.h" +#include "robotis_device/sensor.h" -using namespace ROBOTIS; +using namespace robotis_framework; Sensor::Sensor(int id, std::string model_name, float protocol_version) { - this->id = id; - this->model_name = model_name; - this->port_name = ""; - this->protocol_version = protocol_version; - ctrl_table.clear(); + this->id_ = id; + this->model_name_ = model_name; + this->port_name_ = ""; + this->protocol_version_ = protocol_version; + ctrl_table_.clear(); - sensor_state = new SensorState(); + sensor_state_ = new SensorState(); - bulk_read_items.clear(); + bulk_read_items_.clear(); } diff --git a/robotis_framework_common/CMakeLists.txt b/robotis_framework_common/CMakeLists.txt index 17cd2cc..51f0f3f 100644 --- a/robotis_framework_common/CMakeLists.txt +++ b/robotis_framework_common/CMakeLists.txt @@ -5,15 +5,6 @@ find_package(catkin REQUIRED COMPONENTS roscpp ) -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if you package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need catkin_package( INCLUDE_DIRS include # LIBRARIES robotis_framework_common @@ -21,36 +12,7 @@ catkin_package( # DEPENDS system_lib ) -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -# include_directories(include) include_directories( ${catkin_INCLUDE_DIRS} ) -## Declare a C++ library -# add_library(robotis_framework_common -# src/${PROJECT_NAME}/robotis_framework_common.cpp -# ) - -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure -# add_dependencies(robotis_framework_common ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Declare a C++ executable -# add_executable(robotis_framework_common_node src/robotis_framework_common_node.cpp) - -## Add cmake target dependencies of the executable -## same as for the library above -# add_dependencies(robotis_framework_common_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Specify libraries to link a library or executable target against -# target_link_libraries(robotis_framework_common_node -# ${catkin_LIBRARIES} -# ) - diff --git a/robotis_framework_common/include/robotis_framework_common/motion_module.h b/robotis_framework_common/include/robotis_framework_common/motion_module.h index 0d0fb0f..c602f34 100644 --- a/robotis_framework_common/include/robotis_framework_common/motion_module.h +++ b/robotis_framework_common/include/robotis_framework_common/motion_module.h @@ -1,71 +1,101 @@ +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * 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 ROBOTIS 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. + *******************************************************************************/ + /* - * MotionModule.h + * motion_module.h * * Created on: 2016. 1. 15. * Author: zerom */ -#ifndef ROBOTIS_FRAMEWORK_COMMON_INCLUDE_ROBOTIS_FRAMEWORK_COMMON_MOTIONMODULE_H_ -#define ROBOTIS_FRAMEWORK_COMMON_INCLUDE_ROBOTIS_FRAMEWORK_COMMON_MOTIONMODULE_H_ +#ifndef ROBOTIS_FRAMEWORK_COMMON_MOTION_MODULE_H_ +#define ROBOTIS_FRAMEWORK_COMMON_MOTION_MODULE_H_ #include #include -#include "robotis_device/Robot.h" -#include "robotis_device/Dynamixel.h" -#include "robotis_framework_common/Singleton.h" +#include "singleton.h" +#include "robotis_device/robot.h" +#include "robotis_device/dynamixel.h" -namespace ROBOTIS +namespace robotis_framework { -enum CONTROL_MODE +enum ControlMode { - POSITION_CONTROL, - VELOCITY_CONTROL, - CURRENT_CONTROL + PositionControl, + VelocityControl, + CurrentControl }; class MotionModule { protected: - bool enable; - std::string module_name; - CONTROL_MODE control_mode; + bool enable_; + std::string module_name_; + ControlMode control_mode_; public: - std::map result; + std::map result_; - virtual ~MotionModule() { } + virtual ~MotionModule() { } - std::string GetModuleName() { return module_name; } - CONTROL_MODE GetControlMode() { return control_mode; } + std::string getModuleName() { return module_name_; } + ControlMode getControlMode() { return control_mode_; } - void SetModuleEnable(bool enable) - { - if(this->enable == enable) - return; - - this->enable = enable; - if(enable) - OnModuleEnable(); - else - OnModuleDisable(); - } - bool GetModuleEnable() { return enable; } + void setModuleEnable(bool enable) + { + if(this->enable_ == enable) + return; - virtual void OnModuleEnable() { } - virtual void OnModuleDisable() { } + this->enable_ = enable; + if(enable) + onModuleEnable(); + else + onModuleDisable(); + } + bool getModuleEnable() { return enable_; } - virtual void Initialize(const int control_cycle_msec, Robot *robot) = 0; - virtual void Process(std::map dxls, std::map sensors) = 0; + virtual void onModuleEnable() { } + virtual void onModuleDisable() { } - virtual void Stop() = 0; - virtual bool IsRunning() = 0; + virtual void initialize(const int control_cycle_msec, Robot *robot) = 0; + virtual void process(std::map dxls, std::map sensors) = 0; + + virtual void stop() = 0; + virtual bool isRunning() = 0; }; } -#endif /* ROBOTIS_FRAMEWORK_COMMON_INCLUDE_ROBOTIS_FRAMEWORK_COMMON_MOTIONMODULE_H_ */ +#endif /* ROBOTIS_FRAMEWORK_COMMON_MOTION_MODULE_H_ */ diff --git a/robotis_framework_common/include/robotis_framework_common/sensor_module.h b/robotis_framework_common/include/robotis_framework_common/sensor_module.h index 1c604e9..81699e2 100644 --- a/robotis_framework_common/include/robotis_framework_common/sensor_module.h +++ b/robotis_framework_common/include/robotis_framework_common/sensor_module.h @@ -1,41 +1,71 @@ +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * 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 ROBOTIS 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. + *******************************************************************************/ + /* - * SensorModule.h + * sensor_module.h * * Created on: 2016. 3. 30. * Author: zerom */ -#ifndef ROBOTIS_FRAMEWORK_COMMON_INCLUDE_ROBOTIS_FRAMEWORK_COMMON_SENSORMODULE_H_ -#define ROBOTIS_FRAMEWORK_COMMON_INCLUDE_ROBOTIS_FRAMEWORK_COMMON_SENSORMODULE_H_ +#ifndef ROBOTIS_FRAMEWORK_COMMON_SENSOR_MODULE_H_ +#define ROBOTIS_FRAMEWORK_COMMON_SENSOR_MODULE_H_ #include #include -#include "robotis_device/Robot.h" -#include "robotis_device/Dynamixel.h" -#include "robotis_framework_common/Singleton.h" +#include "singleton.h" +#include "robotis_device/robot.h" +#include "robotis_device/dynamixel.h" -namespace ROBOTIS +namespace robotis_framework { class SensorModule { protected: - std::string module_name; + std::string module_name_; public: - std::map result; + std::map result_; - virtual ~SensorModule() { } + virtual ~SensorModule() { } - std::string GetModuleName() { return module_name; } + std::string getModuleName() { return module_name_; } - virtual void Initialize(const int control_cycle_msec, Robot *robot) = 0; - virtual void Process(std::map dxls, std::map sensors) = 0; + virtual void initialize(const int control_cycle_msec, Robot *robot) = 0; + virtual void process(std::map dxls, std::map sensors) = 0; }; } -#endif /* ROBOTIS_FRAMEWORK_COMMON_INCLUDE_ROBOTIS_FRAMEWORK_COMMON_SENSORMODULE_H_ */ +#endif /* ROBOTIS_FRAMEWORK_COMMON_SENSOR_MODULE_H_ */ diff --git a/robotis_framework_common/include/robotis_framework_common/singleton.h b/robotis_framework_common/include/robotis_framework_common/singleton.h index 2b4c7d8..837b80b 100644 --- a/robotis_framework_common/include/robotis_framework_common/singleton.h +++ b/robotis_framework_common/include/robotis_framework_common/singleton.h @@ -1,44 +1,74 @@ +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * 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 ROBOTIS 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. + *******************************************************************************/ + /* - * Singleton.h + * singleton.h * * Created on: 2016. 5. 17. * Author: zerom */ -#ifndef ROBOTIS_FRAMEWORK_COMMON_INCLUDE_ROBOTIS_FRAMEWORK_COMMON_SINGLETON_H_ -#define ROBOTIS_FRAMEWORK_COMMON_INCLUDE_ROBOTIS_FRAMEWORK_COMMON_SINGLETON_H_ +#ifndef ROBOTIS_FRAMEWORK_COMMON_SINGLETON_H_ +#define ROBOTIS_FRAMEWORK_COMMON_SINGLETON_H_ -namespace ROBOTIS +namespace robotis_framework { template class Singleton { private: - static T *unique_instance_; + static T *unique_instance_; protected: - Singleton() { } - Singleton(Singleton const&) { } - Singleton& operator=(Singleton const&) { return *this; } + Singleton() { } + Singleton(Singleton const&) { } + Singleton& operator=(Singleton const&) { return *this; } public: - static T* GetInstance() - { - if(unique_instance_ == NULL) - unique_instance_ = new T; - return unique_instance_; - } + static T* getInstance() + { + if(unique_instance_ == NULL) + unique_instance_ = new T; + return unique_instance_; + } - static void DestroyInstance() + static void destroyInstance() + { + if(unique_instance_) { - if(unique_instance_) - { - delete unique_instance_; - unique_instance_ = NULL; - } + delete unique_instance_; + unique_instance_ = NULL; } + } }; template T* Singleton::unique_instance_ = NULL; @@ -46,4 +76,4 @@ template T* Singleton::unique_instance_ = NULL; } -#endif /* ROBOTIS_FRAMEWORK_COMMON_INCLUDE_ROBOTIS_FRAMEWORK_COMMON_SINGLETON_H_ */ +#endif /* ROBOTIS_FRAMEWORK_COMMON_SINGLETON_H_ */ diff --git a/robotis_framework_common/package.xml b/robotis_framework_common/package.xml index feec7f8..820f4a2 100644 --- a/robotis_framework_common/package.xml +++ b/robotis_framework_common/package.xml @@ -1,52 +1,15 @@ robotis_framework_common - 0.0.0 + 0.1.0 The robotis_framework_common package + robotis - - - - robotis - - - - - BSD - - - - - - - - - - - - - - - - - - - - - - - - + ROBOTIS + catkin roscpp roscpp - - - - - - - \ No newline at end of file diff --git a/robotis_math/CMakeLists.txt b/robotis_math/CMakeLists.txt index 147809f..4d6421d 100644 --- a/robotis_math/CMakeLists.txt +++ b/robotis_math/CMakeLists.txt @@ -42,9 +42,9 @@ include_directories( ## Declare a C++ library add_library(robotis_math - src/RobotisMathBase.cpp - src/RobotisTrajectoryCalculator.cpp - src/RobotisLinearAlgebra.cpp + src/robotis_math_base.cpp + src/robotis_trajectory_calculator.cpp + src/robotis_linear_algebra.cpp ) ## Add cmake target dependencies of the library diff --git a/robotis_math/include/robotis_math/robotis_linear_algebra.h b/robotis_math/include/robotis_math/robotis_linear_algebra.h index 6188c50..1b5032a 100644 --- a/robotis_math/include/robotis_math/robotis_linear_algebra.h +++ b/robotis_math/include/robotis_math/robotis_linear_algebra.h @@ -15,7 +15,7 @@ #include -namespace ROBOTIS +namespace robotis_framework { Eigen::MatrixXd transitionXYZ ( double position_x, double position_y, double position_z ); diff --git a/robotis_math/include/robotis_math/robotis_math.h b/robotis_math/include/robotis_math/robotis_math.h index 752038d..f2afce1 100644 --- a/robotis_math/include/robotis_math/robotis_math.h +++ b/robotis_math/include/robotis_math/robotis_math.h @@ -8,6 +8,6 @@ #ifndef ROBOTIS_MATH_H_ #define ROBOTIS_MATH_H_ -#include "RobotisTrajectoryCalculator.h" +#include "robotis_trajectory_calculator.h" #endif /* ROBOTIS_MATH_H_ */ diff --git a/robotis_math/include/robotis_math/robotis_math_base.h b/robotis_math/include/robotis_math/robotis_math_base.h index 7b7c713..059c4ef 100644 --- a/robotis_math/include/robotis_math/robotis_math_base.h +++ b/robotis_math/include/robotis_math/robotis_math_base.h @@ -10,7 +10,7 @@ #include -namespace ROBOTIS +namespace robotis_framework { #define PRINT_VAR(X) std::cout << #X << " : " << X << std::endl diff --git a/robotis_math/include/robotis_math/robotis_trajectory_calculator.h b/robotis_math/include/robotis_math/robotis_trajectory_calculator.h index 3d1ff70..fcadbeb 100644 --- a/robotis_math/include/robotis_math/robotis_trajectory_calculator.h +++ b/robotis_math/include/robotis_math/robotis_trajectory_calculator.h @@ -9,12 +9,12 @@ #define ROBOTIS_TRAJECTORY_CALCULATOR_H_ -#include "RobotisMathBase.h" -#include "RobotisLinearAlgebra.h" +#include "robotis_linear_algebra.h" +#include "robotis_math_base.h" // minimum jerk trajectory -namespace ROBOTIS +namespace robotis_framework { Eigen::MatrixXd minimum_jerk_tra( double pos_start , double vel_start , double accel_start, diff --git a/robotis_math/src/robotis_linear_algebra.cpp b/robotis_math/src/robotis_linear_algebra.cpp index 30bb5ce..97f9f92 100644 --- a/robotis_math/src/robotis_linear_algebra.cpp +++ b/robotis_math/src/robotis_linear_algebra.cpp @@ -6,9 +6,9 @@ */ -#include "robotis_math/RobotisLinearAlgebra.h" +#include "../include/robotis_math/robotis_linear_algebra.h" -namespace ROBOTIS +namespace robotis_framework { Eigen::MatrixXd transitionXYZ ( double position_x, double position_y, double position_z ) diff --git a/robotis_math/src/robotis_math_base.cpp b/robotis_math/src/robotis_math_base.cpp index 0afb845..2b9610b 100644 --- a/robotis_math/src/robotis_math_base.cpp +++ b/robotis_math/src/robotis_math_base.cpp @@ -5,12 +5,12 @@ * Author: jay */ -#include "robotis_math/RobotisMathBase.h" +#include "../include/robotis_math/robotis_math_base.h" -namespace ROBOTIS +namespace robotis_framework { double sign( double x ) diff --git a/robotis_math/src/robotis_trajectory_calculator.cpp b/robotis_math/src/robotis_trajectory_calculator.cpp index 665015b..4d91a2c 100644 --- a/robotis_math/src/robotis_trajectory_calculator.cpp +++ b/robotis_math/src/robotis_trajectory_calculator.cpp @@ -7,7 +7,7 @@ -#include "robotis_math/RobotisTrajectoryCalculator.h" +#include "../include/robotis_math/robotis_trajectory_calculator.h" /* @@ -19,7 +19,7 @@ -namespace ROBOTIS +namespace robotis_framework { Eigen::MatrixXd minimum_jerk_tra( double pos_start , double vel_start , double accel_start, From 7403b1aaf9015788ad0964ca3e562a0373021008 Mon Sep 17 00:00:00 2001 From: ROBOTIS-zerom Date: Fri, 3 Jun 2016 20:44:59 +0900 Subject: [PATCH 031/238] ROS C++ Coding Style is applied to robotis_math. --- .../robotis_math/robotis_linear_algebra.h | 32 ++++++++++++++++- .../include/robotis_math/robotis_math.h | 30 ++++++++++++++++ .../include/robotis_math/robotis_math_base.h | 32 ++++++++++++++++- .../robotis_trajectory_calculator.h | 32 ++++++++++++++++- robotis_math/src/robotis_linear_algebra.cpp | 34 ++++++++++++++++-- robotis_math/src/robotis_math_base.cpp | 36 ++++++++++++++++--- 6 files changed, 187 insertions(+), 9 deletions(-) diff --git a/robotis_math/include/robotis_math/robotis_linear_algebra.h b/robotis_math/include/robotis_math/robotis_linear_algebra.h index 1b5032a..9fecb82 100644 --- a/robotis_math/include/robotis_math/robotis_linear_algebra.h +++ b/robotis_math/include/robotis_math/robotis_linear_algebra.h @@ -1,5 +1,35 @@ +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * 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 ROBOTIS 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. + *******************************************************************************/ + /* - * RobotisLinearAlgebra.h + * robotis_linear_algebra.h * * Created on: Mar 18, 2016 * Author: jay diff --git a/robotis_math/include/robotis_math/robotis_math.h b/robotis_math/include/robotis_math/robotis_math.h index f2afce1..61c33e5 100644 --- a/robotis_math/include/robotis_math/robotis_math.h +++ b/robotis_math/include/robotis_math/robotis_math.h @@ -1,3 +1,33 @@ +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * 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 ROBOTIS 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. + *******************************************************************************/ + /* * robotis_math.h * diff --git a/robotis_math/include/robotis_math/robotis_math_base.h b/robotis_math/include/robotis_math/robotis_math_base.h index 059c4ef..436b0e4 100644 --- a/robotis_math/include/robotis_math/robotis_math_base.h +++ b/robotis_math/include/robotis_math/robotis_math_base.h @@ -1,5 +1,35 @@ +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * 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 ROBOTIS 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. + *******************************************************************************/ + /* - * RobotisMathBase.h + * robotis_math_base.h * * Created on: 2016. 3. 28. * Author: JaySong diff --git a/robotis_math/include/robotis_math/robotis_trajectory_calculator.h b/robotis_math/include/robotis_math/robotis_trajectory_calculator.h index fcadbeb..a1c3807 100644 --- a/robotis_math/include/robotis_math/robotis_trajectory_calculator.h +++ b/robotis_math/include/robotis_math/robotis_trajectory_calculator.h @@ -1,5 +1,35 @@ +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * 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 ROBOTIS 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. + *******************************************************************************/ + /* - * RobotisTrajectoryCalculator.h + * robotis_trajectory_calculator.h * * Created on: Mar 18, 2016 * Author: jay diff --git a/robotis_math/src/robotis_linear_algebra.cpp b/robotis_math/src/robotis_linear_algebra.cpp index 97f9f92..569a695 100644 --- a/robotis_math/src/robotis_linear_algebra.cpp +++ b/robotis_math/src/robotis_linear_algebra.cpp @@ -1,12 +1,42 @@ +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * 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 ROBOTIS 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. + *******************************************************************************/ + /* - * RobotisLinearAlgebra.cpp + * robotis_linear_algebra.cpp * * Created on: Mar 18, 2016 * Author: jay */ -#include "../include/robotis_math/robotis_linear_algebra.h" +#include "robotis_math/robotis_linear_algebra.h" namespace robotis_framework { diff --git a/robotis_math/src/robotis_math_base.cpp b/robotis_math/src/robotis_math_base.cpp index 2b9610b..b5c43d3 100644 --- a/robotis_math/src/robotis_math_base.cpp +++ b/robotis_math/src/robotis_math_base.cpp @@ -1,13 +1,41 @@ +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * 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 ROBOTIS 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. + *******************************************************************************/ + /* - * RobotisMathBase.cpp + * robotis_math_base.cpp * * Created on: Mar 18, 2016 * Author: jay */ -#include "../include/robotis_math/robotis_math_base.h" - - +#include "robotis_math/robotis_math_base.h" namespace robotis_framework From 3e8683bbf576a25d55e9ea21c6e996c9324f1fa9 Mon Sep 17 00:00:00 2001 From: ROBOTIS-zerom Date: Fri, 3 Jun 2016 20:50:22 +0900 Subject: [PATCH 032/238] ROS C++ Coding Style is applied --- robotis_math/include/robotis_math/robotis_linear_algebra.h | 4 ++-- robotis_math/include/robotis_math/robotis_math_base.h | 4 ++-- robotis_math/src/robotis_linear_algebra.cpp | 4 ++-- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/robotis_math/include/robotis_math/robotis_linear_algebra.h b/robotis_math/include/robotis_math/robotis_linear_algebra.h index 9fecb82..84b3e11 100644 --- a/robotis_math/include/robotis_math/robotis_linear_algebra.h +++ b/robotis_math/include/robotis_math/robotis_linear_algebra.h @@ -50,7 +50,7 @@ namespace robotis_framework Eigen::MatrixXd transitionXYZ ( double position_x, double position_y, double position_z ); Eigen::MatrixXd transformationXYZRPY ( double position_x, double position_y, double position_z , double roll , double pitch , double yaw ); -Eigen::MatrixXd InverseTransformation(Eigen::MatrixXd transform); +Eigen::MatrixXd inverseTransformation(Eigen::MatrixXd transform); Eigen::MatrixXd inertiaXYZ( double ixx, double ixy, double ixz , double iyy , double iyz, double izz ); @@ -70,7 +70,7 @@ Eigen::MatrixXd quaternion2rotation( Eigen::Quaterniond quaternion ); Eigen::MatrixXd rotation4d( double roll, double pitch, double yaw ); Eigen::MatrixXd hatto( Eigen::MatrixXd matrix3d ); -Eigen::MatrixXd Rodrigues( Eigen::MatrixXd hat_matrix , double angle ); +Eigen::MatrixXd rodrigues( Eigen::MatrixXd hat_matrix , double angle ); Eigen::MatrixXd rot2omega(Eigen::MatrixXd rotation ); Eigen::MatrixXd cross(Eigen::MatrixXd vector3d_a, Eigen::MatrixXd vector3d_b ); double dot(Eigen::MatrixXd vector3d_a, Eigen::MatrixXd vector3d_b ); diff --git a/robotis_math/include/robotis_math/robotis_math_base.h b/robotis_math/include/robotis_math/robotis_math_base.h index 436b0e4..8088051 100644 --- a/robotis_math/include/robotis_math/robotis_math_base.h +++ b/robotis_math/include/robotis_math/robotis_math_base.h @@ -46,8 +46,8 @@ namespace robotis_framework #define PRINT_VAR(X) std::cout << #X << " : " << X << std::endl #define PRINT_MAT(X) std::cout << #X << ":\n" << X << std::endl << std::endl -#define deg2rad (M_PI / 180.0) -#define rad2deg (180.0 / M_PI) +#define DEGREE2RADIAN (M_PI / 180.0) +#define RADIAN2DEGREE (180.0 / M_PI) inline double powDI(double a, int b) { diff --git a/robotis_math/src/robotis_linear_algebra.cpp b/robotis_math/src/robotis_linear_algebra.cpp index 569a695..60cdf90 100644 --- a/robotis_math/src/robotis_linear_algebra.cpp +++ b/robotis_math/src/robotis_linear_algebra.cpp @@ -75,7 +75,7 @@ Eigen::MatrixXd transformationXYZRPY ( double position_x, double position_y, dou return _transformation; } -Eigen::MatrixXd InverseTransformation(Eigen::MatrixXd transform) +Eigen::MatrixXd inverseTransformation(Eigen::MatrixXd transform) { Eigen::Vector3d vecBOA; //If T is Transform Matrix A from B, the BOA is translation component coordi. B to coordi. A Eigen::Vector3d vec_x, vec_y, vec_z; @@ -261,7 +261,7 @@ Eigen::MatrixXd hatto( Eigen::MatrixXd matrix3d ) return _hatto; } -Eigen::MatrixXd Rodrigues( Eigen::MatrixXd hat_matrix , double angle ) +Eigen::MatrixXd rodrigues( Eigen::MatrixXd hat_matrix , double angle ) { Eigen::MatrixXd _E = Eigen::MatrixXd::Identity( 3 , 3 ); Eigen::MatrixXd _Rodrigues = _E + hat_matrix * sin( angle ) + hat_matrix * hat_matrix * ( 1 - cos( angle ) ); From 2e7b2c6c3552e5e394a4464e38b5a5913f7fba86 Mon Sep 17 00:00:00 2001 From: sch Date: Tue, 7 Jun 2016 14:18:34 +0900 Subject: [PATCH 033/238] ROS C++ coding style is applied to robotis_math --- robotis_math/CMakeLists.txt | 25 +- .../robotis_math/robotis_linear_algebra.h | 47 +- .../include/robotis_math/robotis_math.h | 4 +- .../include/robotis_math/robotis_math_base.h | 6 +- .../robotis_trajectory_calculator.h | 28 +- robotis_math/package.xml | 26 +- robotis_math/src/robotis_linear_algebra.cpp | 352 ++++++------ robotis_math/src/robotis_math_base.cpp | 19 +- .../src/robotis_trajectory_calculator.cpp | 511 +++++++++--------- 9 files changed, 474 insertions(+), 544 deletions(-) diff --git a/robotis_math/CMakeLists.txt b/robotis_math/CMakeLists.txt index 4d6421d..7ce8d9a 100644 --- a/robotis_math/CMakeLists.txt +++ b/robotis_math/CMakeLists.txt @@ -1,9 +1,6 @@ cmake_minimum_required(VERSION 2.8.3) project(robotis_math) -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages find_package(catkin REQUIRED COMPONENTS roscpp cmake_modules @@ -11,16 +8,10 @@ find_package(catkin REQUIRED COMPONENTS find_package(Eigen REQUIRED) - ################################### ## catkin specific configuration ## ################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if you package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need + catkin_package( INCLUDE_DIRS include LIBRARIES robotis_math @@ -32,34 +23,20 @@ catkin_package( ## Build ## ########### -## Specify additional locations of header files -## Your package locations should be listed before other locations include_directories( include ${catkin_INCLUDE_DIRS} ${Eigen_INCLUDE_DIRS} ) -## Declare a C++ library add_library(robotis_math src/robotis_math_base.cpp src/robotis_trajectory_calculator.cpp src/robotis_linear_algebra.cpp ) -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure add_dependencies(robotis_math ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -## Declare a C++ executable -# add_executable(robotis_math_node src/robotis_math_node.cpp) - -## Add cmake target dependencies of the executable -## same as for the library above -#add_dependencies(robotis_math_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Specify libraries to link a library or executable target against target_link_libraries(robotis_math ${catkin_LIBRARIES} ) diff --git a/robotis_math/include/robotis_math/robotis_linear_algebra.h b/robotis_math/include/robotis_math/robotis_linear_algebra.h index 84b3e11..6751b2b 100644 --- a/robotis_math/include/robotis_math/robotis_linear_algebra.h +++ b/robotis_math/include/robotis_math/robotis_linear_algebra.h @@ -31,8 +31,8 @@ /* * robotis_linear_algebra.h * - * Created on: Mar 18, 2016 - * Author: jay + * Created on: June 6, 2016 + * Author: sch */ #ifndef ROBOTIS_LINEAR_ALGEBRA_H_ @@ -48,32 +48,27 @@ namespace robotis_framework { -Eigen::MatrixXd transitionXYZ ( double position_x, double position_y, double position_z ); -Eigen::MatrixXd transformationXYZRPY ( double position_x, double position_y, double position_z , double roll , double pitch , double yaw ); -Eigen::MatrixXd inverseTransformation(Eigen::MatrixXd transform); +Eigen::MatrixXd getTransitionXYZ(double position_x, double position_y, double position_z); +Eigen::MatrixXd getTransformationXYZRPY(double position_x, double position_y, double position_z , double roll, double pitch, double yaw); +Eigen::MatrixXd getInverseTransformation(Eigen::MatrixXd transform); +Eigen::MatrixXd getInertiaXYZ(double ixx, double ixy, double ixz , double iyy , double iyz, double izz); +Eigen::MatrixXd getRotationX(double angle); +Eigen::MatrixXd getRotationY(double angle); +Eigen::MatrixXd getRotationZ(double angle); +Eigen::MatrixXd getRotation4d(double roll, double pitch, double yaw); -Eigen::MatrixXd inertiaXYZ( double ixx, double ixy, double ixz , double iyy , double iyz, double izz ); +Eigen::MatrixXd convertRotationToRPY(Eigen::MatrixXd rotation); +Eigen::MatrixXd convertRPYToRotation(double roll, double pitch, double yaw); +Eigen::Quaterniond convertRPYToQuaternion(double roll, double pitch, double yaw); +Eigen::Quaterniond convertRotationToQuaternion(Eigen::MatrixXd rotation); +Eigen::MatrixXd convertQuaternionToRPY(Eigen::Quaterniond quaternion); +Eigen::MatrixXd convertQuaternionToRotation(Eigen::Quaterniond quaternion); -Eigen::MatrixXd rotationX( double angle ); -Eigen::MatrixXd rotationY( double angle ); -Eigen::MatrixXd rotationZ( double angle ); - -Eigen::MatrixXd rotation2rpy( Eigen::MatrixXd rotation ); -Eigen::MatrixXd rpy2rotation( double roll, double pitch, double yaw ); - -Eigen::Quaterniond rpy2quaternion( double roll, double pitch, double yaw ); -Eigen::Quaterniond rotation2quaternion( Eigen::MatrixXd rotation ); - -Eigen::MatrixXd quaternion2rpy( Eigen::Quaterniond quaternion ); -Eigen::MatrixXd quaternion2rotation( Eigen::Quaterniond quaternion ); - -Eigen::MatrixXd rotation4d( double roll, double pitch, double yaw ); - -Eigen::MatrixXd hatto( Eigen::MatrixXd matrix3d ); -Eigen::MatrixXd rodrigues( Eigen::MatrixXd hat_matrix , double angle ); -Eigen::MatrixXd rot2omega(Eigen::MatrixXd rotation ); -Eigen::MatrixXd cross(Eigen::MatrixXd vector3d_a, Eigen::MatrixXd vector3d_b ); -double dot(Eigen::MatrixXd vector3d_a, Eigen::MatrixXd vector3d_b ); +Eigen::MatrixXd hatto(Eigen::MatrixXd matrix3d); +Eigen::MatrixXd rodrigues(Eigen::MatrixXd hat_matrix, double angle); +Eigen::MatrixXd rot2omega(Eigen::MatrixXd rotation); +Eigen::MatrixXd cross(Eigen::MatrixXd vector3d_a, Eigen::MatrixXd vector3d_b); +double dot(Eigen::MatrixXd vector3d_a, Eigen::MatrixXd vector3d_b); } diff --git a/robotis_math/include/robotis_math/robotis_math.h b/robotis_math/include/robotis_math/robotis_math.h index 61c33e5..caa6dc9 100644 --- a/robotis_math/include/robotis_math/robotis_math.h +++ b/robotis_math/include/robotis_math/robotis_math.h @@ -31,8 +31,8 @@ /* * robotis_math.h * - * Created on: Mar 18, 2016 - * Author: jay + * Created on: June 6, 2016 + * Author: sch */ #ifndef ROBOTIS_MATH_H_ diff --git a/robotis_math/include/robotis_math/robotis_math_base.h b/robotis_math/include/robotis_math/robotis_math_base.h index 8088051..14c4bc3 100644 --- a/robotis_math/include/robotis_math/robotis_math_base.h +++ b/robotis_math/include/robotis_math/robotis_math_base.h @@ -31,8 +31,8 @@ /* * robotis_math_base.h * - * Created on: 2016. 3. 28. - * Author: JaySong + * Created on: June 6, 2016 + * Author: sch */ #ifndef ROBOTIS_MATH_BASE_H_ @@ -54,7 +54,7 @@ inline double powDI(double a, int b) return (b == 0 ? 1 : (b > 0 ? a * powDI(a, b - 1) : 1 / powDI(a, -b))); } -double sign( double x ); +double sign(double x); } diff --git a/robotis_math/include/robotis_math/robotis_trajectory_calculator.h b/robotis_math/include/robotis_math/robotis_trajectory_calculator.h index a1c3807..79857df 100644 --- a/robotis_math/include/robotis_math/robotis_trajectory_calculator.h +++ b/robotis_math/include/robotis_math/robotis_trajectory_calculator.h @@ -31,14 +31,13 @@ /* * robotis_trajectory_calculator.h * - * Created on: Mar 18, 2016 - * Author: jay + * Created on: June 6, 2016 + * Author: sch */ #ifndef ROBOTIS_TRAJECTORY_CALCULATOR_H_ #define ROBOTIS_TRAJECTORY_CALCULATOR_H_ - #include "robotis_linear_algebra.h" #include "robotis_math_base.h" @@ -47,21 +46,20 @@ namespace robotis_framework { -Eigen::MatrixXd minimum_jerk_tra( double pos_start , double vel_start , double accel_start, - double pos_end , double vel_end , double accel_end, - double smp_time , double mov_time ); +Eigen::MatrixXd calcMinimumJerkTra(double pos_start, double vel_start, double accel_start, + double pos_end, double vel_end, double accel_end, + double smp_time, double mov_time); -Eigen::MatrixXd minimum_jerk_tra_via_n_qdqddq( int via_num, - double pos_start , double vel_start , double accel_start , - Eigen::MatrixXd pos_via, Eigen::MatrixXd vel_via, Eigen::MatrixXd accel_via, - double pos_end, double vel_end, double accel_end, - double smp_time, Eigen::MatrixXd via_time, double mov_time ); +Eigen::MatrixXd calcMinimumJerkTraWithViaPoints(int via_num, + double pos_start, double vel_start, double accel_start, + Eigen::MatrixXd pos_via, Eigen::MatrixXd vel_via, Eigen::MatrixXd accel_via, + double pos_end, double vel_end, double accel_end, + double smp_time, Eigen::MatrixXd via_time, double mov_time); -Eigen::MatrixXd arc3d_tra( double smp_time, double mov_time, - Eigen::MatrixXd center_point, Eigen::MatrixXd normal_vector, Eigen::MatrixXd start_point, - double rotation_angle, double cross_ratio ); +Eigen::MatrixXd calcArc3dTra(double smp_time, double mov_time, + Eigen::MatrixXd center_point, Eigen::MatrixXd normal_vector, Eigen::MatrixXd start_point, + double rotation_angle, double cross_ratio); } - #endif /* ROBOTIS_TRAJECTORY_CALCULATOR_H_ */ diff --git a/robotis_math/package.xml b/robotis_math/package.xml index 506f261..29d618b 100644 --- a/robotis_math/package.xml +++ b/robotis_math/package.xml @@ -4,28 +4,10 @@ 0.0.0 The robotis_math package - - - - jay - - - - - - BSD - - - - - - - - - - - - + sch + BSD + + sch catkin roscpp diff --git a/robotis_math/src/robotis_linear_algebra.cpp b/robotis_math/src/robotis_linear_algebra.cpp index 60cdf90..d85d744 100644 --- a/robotis_math/src/robotis_linear_algebra.cpp +++ b/robotis_math/src/robotis_linear_algebra.cpp @@ -31,291 +31,265 @@ /* * robotis_linear_algebra.cpp * - * Created on: Mar 18, 2016 - * Author: jay + * Created on: June 6, 2016 + * Author: sch */ - #include "robotis_math/robotis_linear_algebra.h" namespace robotis_framework { -Eigen::MatrixXd transitionXYZ ( double position_x, double position_y, double position_z ) +Eigen::MatrixXd getTransitionXYZ(double position_x, double position_y, double position_z) { - Eigen::MatrixXd _position(3,1); + Eigen::MatrixXd position(3,1); - _position << position_x, - position_y, - position_z; + position << + position_x, + position_y, + position_z; - return _position; + return position; } -Eigen::MatrixXd transformationXYZRPY ( double position_x, double position_y, double position_z , double roll , double pitch , double yaw ) +Eigen::MatrixXd getTransformationXYZRPY(double position_x, double position_y, double position_z , double roll , double pitch , double yaw) { -// Eigen::MatrixXd _position(3,1); -// -// _position << position_x, -// position_y, -// position_z; -// -// Eigen::MatrixXd _rotation = rpy2rotation( roll , pitch , yaw ); -// -// Eigen::MatrixXd _transformation(4,4); -// -// _transformation << _rotation , _position, -// 0 , 0 , 0 , 1; + Eigen::MatrixXd transformation = getRotation4d(roll, pitch, yaw); + transformation.coeffRef(0,3) = position_x; + transformation.coeffRef(1,3) = position_y; + transformation.coeffRef(2,3) = position_z; - Eigen::MatrixXd _transformation = rotation4d(roll, pitch, yaw); - _transformation.coeffRef(0,3) = position_x; - _transformation.coeffRef(1,3) = position_y; - _transformation.coeffRef(2,3) = position_z; - - return _transformation; + return transformation; } -Eigen::MatrixXd inverseTransformation(Eigen::MatrixXd transform) +Eigen::MatrixXd getInverseTransformation(Eigen::MatrixXd transform) { - Eigen::Vector3d vecBOA; //If T is Transform Matrix A from B, the BOA is translation component coordi. B to coordi. A - Eigen::Vector3d vec_x, vec_y, vec_z; - Eigen::MatrixXd _invT(4,4); + // If T is Transform Matrix A from B, the BOA is translation component coordi. B to coordi. A - vecBOA.coeffRef(0) = -transform(0,3); vecBOA(1) = -transform(1,3); vecBOA(2) = -transform(2,3); - vec_x(0) = transform(0,0); vec_x(1) = transform(1,0); vec_x(2) = transform(2,0); - vec_y(0) = transform(0,1); vec_y(1) = transform(1,1); vec_y(2) = transform(2,1); - vec_z(0) = transform(0,2); vec_z(1) = transform(1,2); vec_z(2) = transform(2,2); -// -// -// // inv = [ x' | -AtoB??x ] -// // [ y' | -AtoB??y ] -// // [ z' | -AtoB??z ] -// // [ 0 0 0 | 1 ] -// - _invT << vec_x(0), vec_x(1), vec_x(2), vecBOA.dot(vec_x), - vec_y(0), vec_y(1), vec_y(2), vecBOA.dot(vec_y), - vec_z(0), vec_z(1), vec_z(2), vecBOA.dot(vec_z), - 0, 0, 0, 1; + Eigen::Vector3d vec_boa; + Eigen::Vector3d vec_x, vec_y, vec_z; + Eigen::MatrixXd inv_t(4,4); - return _invT; + vec_boa(0) = -transform(0,3); + vec_boa(1) = -transform(1,3); + vec_boa(2) = -transform(2,3); + + vec_x(0) = transform(0,0); vec_x(1) = transform(1,0); vec_x(2) = transform(2,0); + vec_y(0) = transform(0,1); vec_y(1) = transform(1,1); vec_y(2) = transform(2,1); + vec_z(0) = transform(0,2); vec_z(1) = transform(1,2); vec_z(2) = transform(2,2); + + inv_t << + vec_x(0), vec_x(1), vec_x(2), vec_boa.dot(vec_x), + vec_y(0), vec_y(1), vec_y(2), vec_boa.dot(vec_y), + vec_z(0), vec_z(1), vec_z(2), vec_boa.dot(vec_z), + 0, 0, 0, 1; + + return inv_t; } -Eigen::MatrixXd inertiaXYZ( double ixx, double ixy, double ixz , double iyy , double iyz, double izz ) +Eigen::MatrixXd getInertiaXYZ(double ixx, double ixy, double ixz , double iyy , double iyz, double izz) { - Eigen::MatrixXd _inertia(3,3); + Eigen::MatrixXd inertia(3,3); - _inertia << ixx , ixy , ixz, - ixy , iyy , iyz, - ixz , iyz , izz ; + inertia << + ixx, ixy, ixz, + ixy, iyy, iyz, + ixz, iyz, izz; - return _inertia; + return inertia; } -Eigen::MatrixXd rotationX( double angle ) +Eigen::MatrixXd getRotationX(double angle) { - Eigen::MatrixXd _rotation( 3 , 3 ); + Eigen::MatrixXd rotation(3,3); - _rotation << 1.0, 0.0, 0.0, - 0.0, cos( angle ), -sin( angle ), - 0.0, sin( angle ), cos( angle ); + rotation << + 1.0, 0.0, 0.0, + 0.0, cos(angle), -sin(angle), + 0.0, sin(angle), cos(angle); - return _rotation; + return rotation; } -Eigen::MatrixXd rotationY( double angle ) +Eigen::MatrixXd getRotationY(double angle) { - Eigen::MatrixXd _rotation( 3 , 3 ); + Eigen::MatrixXd rotation(3,3); - _rotation << cos( angle ), 0.0, sin( angle ), - 0.0, 1.0, 0.0, - -sin( angle ), 0.0, cos( angle ); + rotation << + cos(angle), 0.0, sin(angle), + 0.0, 1.0, 0.0, + -sin(angle), 0.0, cos(angle); - return _rotation; + return rotation; } -Eigen::MatrixXd rotationZ( double angle ) +Eigen::MatrixXd getRotationZ(double angle) { - Eigen::MatrixXd _rotation(3,3); + Eigen::MatrixXd rotation(3,3); - _rotation << cos( angle ), -sin( angle ), 0.0, - sin( angle ), cos( angle ), 0.0, - 0.0, 0.0, 1.0; + rotation << + cos(angle), -sin(angle), 0.0, + sin(angle), cos(angle), 0.0, + 0.0, 0.0, 1.0; - return _rotation; + return rotation; } -Eigen::MatrixXd rotation2rpy( Eigen::MatrixXd rotation ) +Eigen::MatrixXd getRotation4d(double roll, double pitch, double yaw ) { - Eigen::MatrixXd _rpy = Eigen::MatrixXd::Zero( 3 , 1 ); + double sr = sin(roll), cr = cos(roll); + double sp = sin(pitch), cp = cos(pitch); + double sy = sin(yaw), cy = cos(yaw); - _rpy.coeffRef( 0 , 0 ) = atan2( rotation.coeff( 2 , 1 ), rotation.coeff( 2 , 2 ) ); - _rpy.coeffRef( 1 , 0 ) = atan2( -rotation.coeff( 2 , 0 ), sqrt( pow( rotation.coeff( 2 , 1 ) , 2 ) + pow( rotation.coeff( 2 , 2 ) , 2 ) ) ); - _rpy.coeffRef( 2 , 0 ) = atan2 ( rotation.coeff( 1 , 0 ) , rotation.coeff( 0 , 0 ) ); + Eigen::MatrixXd mat_roll(4,4); + Eigen::MatrixXd mat_pitch(4,4); + Eigen::MatrixXd mat_yaw(4,4); - return _rpy; + mat_roll << + 1, 0, 0, 0, + 0, cr, -sr, 0, + 0, sr, cr, 0, + 0, 0, 0, 1; + + mat_pitch << + cp, 0, sp, 0, + 0, 1, 0, 0, + -sp, 0, cp, 0, + 0, 0, 0, 1; + + mat_yaw << + cy, -sy, 0, 0, + sy, cy, 0, 0, + 0, 0, 1, 0, + 0, 0, 0, 1; + + Eigen::MatrixXd mat_rpy = (mat_yaw*mat_pitch)*mat_roll; + + return mat_rpy; } -Eigen::MatrixXd rpy2rotation( double roll, double pitch, double yaw ) +Eigen::MatrixXd convertRotationToRPY(Eigen::MatrixXd rotation) { - Eigen::MatrixXd _rotation = rotationZ( yaw ) * rotationY( pitch ) * rotationX( roll ); + Eigen::MatrixXd rpy = Eigen::MatrixXd::Zero(3,1); - return _rotation; + rpy.coeffRef(0,0) = atan2(rotation.coeff(2,1), rotation.coeff(2,2)); + rpy.coeffRef(1,0) = atan2(-rotation.coeff(2,0), sqrt(pow(rotation.coeff(2,1), 2) + pow(rotation.coeff(2,2),2))); + rpy.coeffRef(2,0) = atan2 (rotation.coeff(1,0), rotation.coeff(0,0)); + + return rpy; } -Eigen::Quaterniond rpy2quaternion( double roll, double pitch, double yaw ) +Eigen::MatrixXd convertRPYToRotation(double roll, double pitch, double yaw) { - Eigen::MatrixXd _rotation = rpy2rotation( roll, pitch, yaw ); + Eigen::MatrixXd rotation = getRotationZ(yaw)*getRotationY(pitch)*getRotationX(roll); - Eigen::Matrix3d _rotation3d; - _rotation3d = _rotation.block<3,3>( 0 , 0); - - Eigen::Quaterniond _quaternion; - - _quaternion = _rotation3d; - - return _quaternion; + return rotation; } -Eigen::Quaterniond rotation2quaternion( Eigen::MatrixXd rotation ) +Eigen::Quaterniond convertRPYToQuaternion(double roll, double pitch, double yaw) { - Eigen::Matrix3d _rotation3d; + Eigen::MatrixXd rotation = convertRPYToRotation(roll,pitch,yaw); - _rotation3d = rotation.block<3,3>( 0 , 0); + Eigen::Matrix3d rotation3d; + rotation3d = rotation.block<3,3>(0,0); - Eigen::Quaterniond _quaternion; - _quaternion = _rotation3d; + Eigen::Quaterniond quaternion; + quaternion = rotation3d; - return _quaternion; + return quaternion; } -Eigen::MatrixXd quaternion2rpy( Eigen::Quaterniond quaternion ) +Eigen::Quaterniond convertRotationToQuaternion(Eigen::MatrixXd rotation) { - Eigen::MatrixXd _rpy = rotation2rpy( quaternion.toRotationMatrix() ); + Eigen::Matrix3d rotation3d; + rotation3d = rotation.block<3,3>(0,0); - return _rpy; + Eigen::Quaterniond quaternion; + quaternion = rotation3d; + + return quaternion; } -Eigen::MatrixXd quaternion2rotation( Eigen::Quaterniond quaternion ) +Eigen::MatrixXd convertQuaternionToRPY(Eigen::Quaterniond quaternion) { - Eigen::MatrixXd _rotation = quaternion.toRotationMatrix(); + Eigen::MatrixXd rpy = convertRotationToRPY(quaternion.toRotationMatrix()); - return _rotation; + return rpy; } -Eigen::MatrixXd rotation4d( double roll, double pitch, double yaw ) +Eigen::MatrixXd convertQuaternionToRotation(Eigen::Quaterniond quaternion) { -// Eigen::MatrixXd _rotation4d = Eigen::MatrixXd::Zero( 4 , 4 ); -// _rotation4d.coeffRef( 3 , 3 ) = 1.0; -// -// Eigen::MatrixXd _rotation = rotationZ( yaw ) * rotationY( pitch ) * rotationX( roll ); -// -//// _rotation4d.block<3,3>(0,0) = _rotation; -// _rotation4d.coeffRef(0,0) = _rotation.coeff(0,0); -// _rotation4d.coeffRef(0,1) = _rotation.coeff(0,1); -// _rotation4d.coeffRef(0,2) = _rotation.coeff(0,2); -// _rotation4d.coeffRef(1,0) = _rotation.coeff(1,0); -// _rotation4d.coeffRef(1,1) = _rotation.coeff(1,1); -// _rotation4d.coeffRef(1,2) = _rotation.coeff(1,2); -// _rotation4d.coeffRef(2,0) = _rotation.coeff(2,0); -// _rotation4d.coeffRef(2,1) = _rotation.coeff(2,1); -// _rotation4d.coeffRef(2,2) = _rotation.coeff(2,2); + Eigen::MatrixXd rotation = quaternion.toRotationMatrix(); -// return _rotation4d; - double sr = sin(roll), cr = cos(roll); - double sp = sin(pitch), cp = cos(pitch); - double sy = sin(yaw), cy = cos(yaw); - - Eigen::MatrixXd matRoll(4,4); - Eigen::MatrixXd matPitch(4,4); - Eigen::MatrixXd matYaw(4,4); - - matRoll << 1, 0, 0, 0, - 0, cr, -sr, 0, - 0, sr, cr, 0, - 0, 0, 0, 1; - - matPitch << cp, 0, sp, 0, - 0, 1, 0, 0, - -sp, 0, cp, 0, - 0, 0, 0, 1; - - matYaw << cy, -sy, 0, 0, - sy, cy, 0, 0, - 0, 0, 1, 0, - 0, 0, 0, 1; - - - return (matYaw*matPitch)*matRoll; + return rotation; } - - -Eigen::MatrixXd hatto( Eigen::MatrixXd matrix3d ) +Eigen::MatrixXd hatto(Eigen::MatrixXd matrix3d) { - Eigen::MatrixXd _hatto(3,3); + Eigen::MatrixXd hatto(3,3); - _hatto << 0.0, -matrix3d.coeff(2,0), matrix3d.coeff(1,0), - matrix3d.coeff(2,0), 0.0, -matrix3d.coeff(0,0), - -matrix3d.coeff(1,0), matrix3d.coeff(0,0), 0.0; + hatto << + 0.0, -matrix3d.coeff(2,0), matrix3d.coeff(1,0), + matrix3d.coeff(2,0), 0.0, -matrix3d.coeff(0,0), + -matrix3d.coeff(1,0), matrix3d.coeff(0,0), 0.0; - return _hatto; + return hatto; } -Eigen::MatrixXd rodrigues( Eigen::MatrixXd hat_matrix , double angle ) +Eigen::MatrixXd rodrigues(Eigen::MatrixXd hat_matrix, double angle) { - Eigen::MatrixXd _E = Eigen::MatrixXd::Identity( 3 , 3 ); - Eigen::MatrixXd _Rodrigues = _E + hat_matrix * sin( angle ) + hat_matrix * hat_matrix * ( 1 - cos( angle ) ); + Eigen::MatrixXd identity = Eigen::MatrixXd::Identity(3,3); + Eigen::MatrixXd rodrigues = identity+hat_matrix*sin(angle)+hat_matrix*hat_matrix*(1-cos(angle)); - return _Rodrigues; + return rodrigues; } -Eigen::MatrixXd rot2omega( Eigen::MatrixXd rotation ) +Eigen::MatrixXd rot2omega(Eigen::MatrixXd rotation) { - double _eps = 1e-12; + double eps = 1e-12; -// Eigen::MatrixXd _E = Eigen::MatrixXd::Identity( 3 , 3 ); + double alpha = (rotation.coeff(0,0)+rotation.coeff(1,1)+rotation.coeff(2,2)-1.0)/2.0; + double alpha_dash = fabs( alpha - 1.0 ); - double _alpha = ( rotation.coeff( 0 , 0 ) + rotation.coeff( 1 , 1 ) + rotation.coeff( 2 , 2 ) - 1.0 ) / 2.0 ; + Eigen::MatrixXd rot2omega(3,1); - double _alpha_plus = fabs( _alpha - 1.0 ); + if( alpha_dash < eps ) + { + rot2omega << + 0.0, + 0.0, + 0.0; + } + else + { + double theta = acos(alpha); - Eigen::MatrixXd _rot2omega( 3 , 1 ); + rot2omega << + rotation.coeff(2,1)-rotation.coeff(1,2), + rotation.coeff(0,2)-rotation.coeff(2,0), + rotation.coeff(1,0)-rotation.coeff(0,1); - if( _alpha_plus < _eps ) - { - _rot2omega << 0.0, - 0.0, - 0.0; - } - else - { - double _theta = acos( _alpha ); + rot2omega = 0.5*(theta/sin(theta))*rot2omega; + } - _rot2omega << rotation.coeff( 2 , 1 ) - rotation.coeff( 1 , 2 ), - rotation.coeff( 0 , 2 ) - rotation.coeff( 2 , 0 ), - rotation.coeff( 1 , 0 ) - rotation.coeff( 0 , 1 ); - - _rot2omega = 0.5 * ( _theta / sin( _theta ) ) * _rot2omega; - } - - return _rot2omega; + return rot2omega; } -Eigen::MatrixXd cross(Eigen::MatrixXd vector3d_a, Eigen::MatrixXd vector3d_b ) +Eigen::MatrixXd cross(Eigen::MatrixXd vector3d_a, Eigen::MatrixXd vector3d_b) { - Eigen::MatrixXd _cross( 3 , 1 ); + Eigen::MatrixXd cross(3,1); - _cross << vector3d_a.coeff( 1 , 0 ) * vector3d_b.coeff( 2 , 0 ) - vector3d_a.coeff( 2 , 0 ) * vector3d_b.coeff( 1 , 0 ), - vector3d_a.coeff( 2 , 0 ) * vector3d_b.coeff( 0 , 0 ) - vector3d_a.coeff( 0 , 0 ) * vector3d_b.coeff( 2 , 0 ), - vector3d_a.coeff( 0 , 0 ) * vector3d_b.coeff( 1 , 0 ) - vector3d_a.coeff( 1 , 0 ) * vector3d_b.coeff( 0 , 0 ); + cross << + vector3d_a.coeff(1,0)*vector3d_b.coeff(2,0)-vector3d_a.coeff(2,0)*vector3d_b.coeff(1,0), + vector3d_a.coeff(2,0)*vector3d_b.coeff(0,0)-vector3d_a.coeff(0,0)*vector3d_b.coeff(2,0), + vector3d_a.coeff(0,0)*vector3d_b.coeff(1,0)-vector3d_a.coeff(1,0)*vector3d_b.coeff(0,0); - return _cross; + return cross; } -double dot(Eigen::MatrixXd vector3d_a, Eigen::MatrixXd vector3d_b ) +double dot(Eigen::MatrixXd vector3d_a, Eigen::MatrixXd vector3d_b) { - return vector3d_a.dot(vector3d_b); - //return vector3d_a.coeff(0,0)*vector3d_b.coeff(0,0) + vector3d_a.coeff(1,0)*vector3d_b.coeff(1,0) + vector3d_a.coeff(2,0)*vector3d_b.coeff(2,0); + return vector3d_a.dot(vector3d_b); } } diff --git a/robotis_math/src/robotis_math_base.cpp b/robotis_math/src/robotis_math_base.cpp index b5c43d3..60eb1f4 100644 --- a/robotis_math/src/robotis_math_base.cpp +++ b/robotis_math/src/robotis_math_base.cpp @@ -31,24 +31,23 @@ /* * robotis_math_base.cpp * - * Created on: Mar 18, 2016 - * Author: jay + * Created on: June 6, 2016 + * Author: sch */ #include "robotis_math/robotis_math_base.h" - namespace robotis_framework { -double sign( double x ) +double sign(double x) { - if ( x < 0.0 ) - return -1.0; - else if ( x > 0.0) - return 1.0; - else - return 0.0; + if ( x < 0.0 ) + return -1.0; + else if ( x > 0.0) + return 1.0; + else + return 0.0; } } diff --git a/robotis_math/src/robotis_trajectory_calculator.cpp b/robotis_math/src/robotis_trajectory_calculator.cpp index 4d91a2c..4b776aa 100644 --- a/robotis_math/src/robotis_trajectory_calculator.cpp +++ b/robotis_math/src/robotis_trajectory_calculator.cpp @@ -1,30 +1,48 @@ -/* - * RobotisTrajectoryCalculator.cpp +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * All rights reserved. * - * Created on: Mar 18, 2016 - * Author: jay - */ - - - -#include "../include/robotis_math/robotis_trajectory_calculator.h" - + * 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 ROBOTIS 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. + *******************************************************************************/ /* - * trajectory.cpp + * robotis_trajectory_calculator.cpp * - * Created on: Jul 13, 2015 + * Created on: June 6, 2016 * Author: sch */ - +#include "../include/robotis_math/robotis_trajectory_calculator.h" namespace robotis_framework { -Eigen::MatrixXd minimum_jerk_tra( double pos_start , double vel_start , double accel_start, - double pos_end , double vel_end , double accel_end, - double smp_time , double mov_time ) +Eigen::MatrixXd calcMinimumJerkTra(double pos_start, double vel_start, double accel_start, + double pos_end, double vel_end, double accel_end, + double smp_time, double mov_time) /* simple minimum jerk trajectory @@ -37,288 +55,275 @@ Eigen::MatrixXd minimum_jerk_tra( double pos_start , double vel_start , double a accel_end : acceleration at final state smp_time : sampling time - mov_time : movement time -*/ + */ { - Eigen::MatrixXd ___C( 3 , 3 ); - Eigen::MatrixXd __C( 3 , 1 ); + Eigen::MatrixXd poly_matrix(3,3); + Eigen::MatrixXd poly_vector(3,1); - ___C << pow( mov_time , 3 ) , pow( mov_time , 4 ) , pow( mov_time , 5 ), - 3 * pow( mov_time , 2 ) , 4 * pow( mov_time , 3 ) , 5 * pow( mov_time , 4 ), - 6 * mov_time , 12 * pow( mov_time , 2 ) , 20 * pow( mov_time , 3 ); + poly_matrix << + pow(mov_time,3), pow(mov_time,4), pow(mov_time,5), + 3*pow(mov_time,2), 4*pow(mov_time,3), 5*pow(mov_time,4), + 6*mov_time, 12*pow(mov_time,2), 20*pow(mov_time,3); - __C << pos_end - pos_start - vel_start * mov_time - accel_start * pow( mov_time , 2 ) / 2, - vel_end - vel_start - accel_start * mov_time, - accel_end - accel_start ; + poly_vector << + pos_end-pos_start-vel_start*mov_time-accel_start*pow(mov_time,2)/2, + vel_end-vel_start-accel_start*mov_time, + accel_end-accel_start ; - Eigen::Matrix _A = ___C.inverse() * __C; + Eigen::Matrix poly_coeff = poly_matrix.inverse() * poly_vector; - double _time_steps; + double time_steps = mov_time/smp_time; + int all_time_steps = round(time_steps+1); - _time_steps = mov_time / smp_time; - int __time_steps = round( _time_steps + 1 ); + Eigen::MatrixXd time = Eigen::MatrixXd::Zero(all_time_steps,1); + Eigen::MatrixXd minimum_jerk_tra = Eigen::MatrixXd::Zero(all_time_steps,1); - Eigen::MatrixXd _time = Eigen::MatrixXd::Zero( __time_steps , 1 ); - Eigen::MatrixXd _tra = Eigen::MatrixXd::Zero( __time_steps , 1 ); + for (int step=0; step j){ - k = i ; - }else{ - k = j ; - } - A2.coeffRef(3*j-3,3*i-3) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),3)/6 ; - A2.coeffRef(3*j-3,3*i-2) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),4)/24 ; - A2.coeffRef(3*j-3,3*i-1) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),5)/120 ; - - A2.coeffRef(3*j-2,3*i-3) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),2)/2 ; - A2.coeffRef(3*j-2,3*i-2) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),3)/6 ; - A2.coeffRef(3*j-2,3*i-1) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),4)/24 ; - - A2.coeffRef(3*j-1,3*i-3) = via_time.coeff(k-1,0)-via_time.coeff(i-1,0) ; - A2.coeffRef(3*j-1,3*i-2) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),2)/2 ; - A2.coeffRef(3*j-1,3*i-1) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),3)/6 ; - } - } - - - Eigen::MatrixXd A3 = Eigen::MatrixXd::Zero(3,3*via_num+3); - - A3.coeffRef(0,0) = pow(mov_time,3); - A3.coeffRef(0,1) = pow(mov_time,4); - A3.coeffRef(0,2) = pow(mov_time,5); - - A3.coeffRef(1,0) = 3*pow(mov_time,2); - A3.coeffRef(1,1) = 4*pow(mov_time,3); - A3.coeffRef(1,2) = 5*pow(mov_time,4); - - A3.coeffRef(2,0) = 6*mov_time; - A3.coeffRef(2,1) = 12*pow(mov_time,2); - A3.coeffRef(2,2) = 20*pow(mov_time,3); - - for (i=1; i<=via_num; i++){ - A3.coeffRef(0,3*i) = pow(mov_time-via_time.coeff(i-1,0),3)/6 ; - A3.coeffRef(1,3*i) = pow(mov_time-via_time.coeff(i-1,0),2)/2 ; - A3.coeffRef(2,3*i) = mov_time-via_time.coeff(i-1,0) ; - - A3.coeffRef(0,3*i+1) = pow(mov_time-via_time.coeff(i-1,0),4)/24 ; - A3.coeffRef(1,3*i+1) = pow(mov_time-via_time.coeff(i-1,0),3)/6 ; - A3.coeffRef(2,3*i+1) = pow(mov_time-via_time.coeff(i-1,0),2)/2 ; - - A3.coeffRef(0,3*i+2) = pow(mov_time-via_time.coeff(i-1,0),5)/120 ; - A3.coeffRef(1,3*i+2) = pow(mov_time-via_time.coeff(i-1,0),4)/24 ; - A3.coeffRef(2,3*i+2) = pow(mov_time-via_time.coeff(i-1,0),3)/6 ; - } - - Eigen::MatrixXd A = Eigen::MatrixXd::Zero(3*via_num+3,3*via_num+3); - - A.block(0,0,3*via_num,3) = A1 ; - A.block(0,3,3*via_num,3*via_num) = A2 ; - A.block(3*via_num,0,3,3*via_num+3) = A3 ; - - /* Calculation Matrix C (coefficient of polynomial function) */ - - Eigen::MatrixXd C(3*via_num+3,1); - //C = A.inverse()*B; - C = A.colPivHouseholderQr().solve(B); - - /* Time */ - - int NN; - double N; - - N = mov_time/smp_time ; - NN = round(N) ; - - Eigen::MatrixXd Time = Eigen::MatrixXd::Zero(NN+1,1); - - for (i=1; i<=NN+1; i++){ - Time.coeffRef(i-1,0) = (i-1)*smp_time; - } - - /* Time_via */ - - Eigen::MatrixXd Time_via = Eigen::MatrixXd::Zero(via_num,1); - - for (i=1; i<=via_num; i++){ - Time_via.coeffRef(i-1,0) = round(via_time.coeff(i-1,0)/smp_time)+2; - } - - /* Minimum Jerk Trajectory with Via-points */ - - Eigen::MatrixXd _tra_jerk_via = Eigen::MatrixXd::Zero(NN+1,1); - - for (i=1; i<=NN+1; i++){ - _tra_jerk_via.coeffRef(i-1,0) = - pos_start + - vel_start*Time.coeff(i-1,0) + - 0.5*accel_start*pow(Time.coeff(i-1,0),2) + - C.coeff(0,0)*pow(Time.coeff(i-1,0),3) + - C.coeff(1,0)*pow(Time.coeff(i-1,0),4) + - C.coeff(2,0)*pow(Time.coeff(i-1,0),5) ; - } - - for (i=1; i<=via_num; i++){ - for (j=Time_via.coeff(i-1,0); j<=NN+1; j++){ - _tra_jerk_via.coeffRef(j-1,0) = - _tra_jerk_via.coeff(j-1,0) + - C.coeff(3*i,0)*pow((Time.coeff(j-1,0)-via_time.coeff(i-1,0)),3)/6 + - C.coeff(3*i+1,0)*pow((Time.coeff(j-1,0)-via_time.coeff(i-1,0)),4)/24 + - C.coeff(3*i+2,0)*pow((Time.coeff(j-1,0)-via_time.coeff(i-1,0)),5)/120 ; - - } - } - - return _tra_jerk_via; - -} - -Eigen::MatrixXd arc3d_tra( double smp_time, double mov_time, - Eigen::MatrixXd center_point, Eigen::MatrixXd normal_vector, Eigen::MatrixXd start_point, - double rotation_angle, double cross_ratio ) -{ - int _all_time_steps = int ( round( mov_time / smp_time ) ) + 1 ; - - Eigen::MatrixXd _angle_tra = minimum_jerk_tra( 0.0 , 0.0 , 0.0 , - rotation_angle , 0.0 , 0.0 , - smp_time , mov_time ); - - Eigen::MatrixXd _pt = Eigen::MatrixXd::Zero( 3 , _all_time_steps ); - - for (int i = 0; i < _all_time_steps; i++ ) + for (int i=1; i<=via_num; i++) + { + for (int j=1; j<=via_num; j++) { - double _t = ( ( double ) i ) * smp_time ; + int k; - double _th = _angle_tra.coeff( i , 0 );//( _t / mov_time ) * rotation_angle; + if (i > j) + k = i ; + else + k = j ; - Eigen::MatrixXd _w_wedge( 3 , 3 ); + poly_matrix_part2.coeffRef(3*j-3,3*i-3) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),3)/6 ; + poly_matrix_part2.coeffRef(3*j-3,3*i-2) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),4)/24 ; + poly_matrix_part2.coeffRef(3*j-3,3*i-1) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),5)/120 ; - _w_wedge << 0.0 , -normal_vector.coeff(2,0), normal_vector.coeff(1,0), - normal_vector.coeff(2,0), 0.0 , -normal_vector.coeff(0,0), - -normal_vector.coeff(1,0), normal_vector.coeff(0,0), 0.0 ; + poly_matrix_part2.coeffRef(3*j-2,3*i-3) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),2)/2 ; + poly_matrix_part2.coeffRef(3*j-2,3*i-2) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),3)/6 ; + poly_matrix_part2.coeffRef(3*j-2,3*i-1) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),4)/24 ; - Eigen::MatrixXd _E = Eigen::MatrixXd::Identity( 3 , 3 ); - - Eigen::MatrixXd _R = _E + _w_wedge * sin( _th ) + _w_wedge * _w_wedge * ( 1 - cos( _th ) ); - - double _cross = cross_ratio * ( 1.0 - 2.0 * ( abs ( 0.5 - _t/mov_time ) ) ); - - _pt.block( 0 , i , 3 , 1 ) = ( 1 + _cross ) * ( _R * ( start_point - center_point ) ) + center_point; + poly_matrix_part2.coeffRef(3*j-1,3*i-3) = via_time.coeff(k-1,0)-via_time.coeff(i-1,0) ; + poly_matrix_part2.coeffRef(3*j-1,3*i-2) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),2)/2 ; + poly_matrix_part2.coeffRef(3*j-1,3*i-1) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),3)/6 ; } + } - Eigen::MatrixXd _pt_trans = _pt.transpose(); - return _pt_trans; + Eigen::MatrixXd poly_matrix_part3 = Eigen::MatrixXd::Zero(3,3*via_num+3); + + poly_matrix_part3.coeffRef(0,0) = pow(mov_time,3); + poly_matrix_part3.coeffRef(0,1) = pow(mov_time,4); + poly_matrix_part3.coeffRef(0,2) = pow(mov_time,5); + + poly_matrix_part3.coeffRef(1,0) = 3*pow(mov_time,2); + poly_matrix_part3.coeffRef(1,1) = 4*pow(mov_time,3); + poly_matrix_part3.coeffRef(1,2) = 5*pow(mov_time,4); + + poly_matrix_part3.coeffRef(2,0) = 6*mov_time; + poly_matrix_part3.coeffRef(2,1) = 12*pow(mov_time,2); + poly_matrix_part3.coeffRef(2,2) = 20*pow(mov_time,3); + + for (int i=1; i<=via_num; i++) + { + poly_matrix_part3.coeffRef(0,3*i) = pow(mov_time-via_time.coeff(i-1,0),3)/6 ; + poly_matrix_part3.coeffRef(1,3*i) = pow(mov_time-via_time.coeff(i-1,0),2)/2 ; + poly_matrix_part3.coeffRef(2,3*i) = mov_time-via_time.coeff(i-1,0) ; + + poly_matrix_part3.coeffRef(0,3*i+1) = pow(mov_time-via_time.coeff(i-1,0),4)/24 ; + poly_matrix_part3.coeffRef(1,3*i+1) = pow(mov_time-via_time.coeff(i-1,0),3)/6 ; + poly_matrix_part3.coeffRef(2,3*i+1) = pow(mov_time-via_time.coeff(i-1,0),2)/2 ; + + poly_matrix_part3.coeffRef(0,3*i+2) = pow(mov_time-via_time.coeff(i-1,0),5)/120 ; + poly_matrix_part3.coeffRef(1,3*i+2) = pow(mov_time-via_time.coeff(i-1,0),4)/24 ; + poly_matrix_part3.coeffRef(2,3*i+2) = pow(mov_time-via_time.coeff(i-1,0),3)/6 ; + } + + Eigen::MatrixXd poly_matrix = Eigen::MatrixXd::Zero(3*via_num+3,3*via_num+3); + + poly_matrix.block(0,0,3*via_num,3) = poly_matrix_part1 ; + poly_matrix.block(0,3,3*via_num,3*via_num) = poly_matrix_part2 ; + poly_matrix.block(3*via_num,0,3,3*via_num+3) = poly_matrix_part3 ; + + Eigen::MatrixXd poly_coeff(3*via_num+3,1); + //C = A.inverse()*B; + poly_coeff = poly_matrix.colPivHouseholderQr().solve(poly_vector); + + int all_time_steps; + all_time_steps = round(mov_time/smp_time) ; + + Eigen::MatrixXd time_vector = Eigen::MatrixXd::Zero(all_time_steps+1,1); + + for (int i=1; i<=all_time_steps+1; i++) + time_vector.coeffRef(i-1,0) = (i-1)*smp_time; + + Eigen::MatrixXd via_time_vector = Eigen::MatrixXd::Zero(via_num,1); + + for (int i=1; i<=via_num; i++) + via_time_vector.coeffRef(i-1,0) = round(via_time.coeff(i-1,0)/smp_time)+2; + + Eigen::MatrixXd minimum_jerk_tra_with_via_points = Eigen::MatrixXd::Zero(all_time_steps+1,1); + + for (int i=1; i<=all_time_steps+1; i++) + { + minimum_jerk_tra_with_via_points.coeffRef(i-1,0) = + pos_start + + vel_start*time_vector.coeff(i-1,0) + + 0.5*accel_start*pow(time_vector.coeff(i-1,0),2) + + poly_coeff.coeff(0,0)*pow(time_vector.coeff(i-1,0),3) + + poly_coeff.coeff(1,0)*pow(time_vector.coeff(i-1,0),4) + + poly_coeff.coeff(2,0)*pow(time_vector.coeff(i-1,0),5) ; + } + + for (int i=1; i<=via_num; i++) + { + for (int j=via_time_vector.coeff(i-1,0); j<=all_time_steps+1; j++) + { + minimum_jerk_tra_with_via_points.coeffRef(j-1,0) = + minimum_jerk_tra_with_via_points.coeff(j-1,0) + + poly_coeff.coeff(3*i,0)*pow((time_vector.coeff(j-1,0)-via_time.coeff(i-1,0)),3)/6 + + poly_coeff.coeff(3*i+1,0)*pow((time_vector.coeff(j-1,0)-via_time.coeff(i-1,0)),4)/24 + + poly_coeff.coeff(3*i+2,0)*pow((time_vector.coeff(j-1,0)-via_time.coeff(i-1,0)),5)/120 ; + + } + } + + return minimum_jerk_tra_with_via_points; +} + +Eigen::MatrixXd calcArc3dTra(double smp_time, double mov_time, + Eigen::MatrixXd center_point, Eigen::MatrixXd normal_vector, Eigen::MatrixXd start_point, + double rotation_angle, double cross_ratio ) +{ + int all_time_steps = int (round(mov_time/smp_time))+1; + + Eigen::MatrixXd angle_tra = calcMinimumJerkTra(0.0, 0.0, 0.0, + rotation_angle, 0.0, 0.0, + smp_time, mov_time); + + Eigen::MatrixXd arc_tra = Eigen::MatrixXd::Zero(3,all_time_steps); + + for (int step = 0; step < all_time_steps; step++ ) + { + double time = ((double)step)*smp_time; + double theta = angle_tra.coeff(step,0); + + Eigen::MatrixXd weight_wedge(3,3); + + weight_wedge << + 0.0, -normal_vector.coeff(2,0), normal_vector.coeff(1,0), + normal_vector.coeff(2,0), 0.0, -normal_vector.coeff(0,0), + -normal_vector.coeff(1,0), normal_vector.coeff(0,0), 0.0; + + Eigen::MatrixXd identity = Eigen::MatrixXd::Identity(3,3); + Eigen::MatrixXd rotation = identity + weight_wedge*sin(theta) + weight_wedge*weight_wedge*(1-cos(theta)); + double cross = cross_ratio*(1.0-2.0*(abs(0.5-(time/mov_time)))); + + arc_tra.block(0,step,3,1) = (1+cross)*(rotation*(start_point-center_point))+center_point; + } + + Eigen::MatrixXd act_tra_trans = arc_tra.transpose(); + + return act_tra_trans; } From 36f627f0350a5ddb0a53b7e687f50f5716538dc0 Mon Sep 17 00:00:00 2001 From: sch Date: Tue, 7 Jun 2016 16:54:01 +0900 Subject: [PATCH 034/238] ROS C++ coding style is applied to robotis_math -version2 --- .../robotis_math/robotis_linear_algebra.h | 18 +++++++-------- .../include/robotis_math/robotis_math.h | 8 +++---- .../include/robotis_math/robotis_math_base.h | 8 +++---- .../robotis_trajectory_calculator.h | 8 +++---- robotis_math/src/robotis_linear_algebra.cpp | 22 +++++++++---------- robotis_math/src/robotis_math_base.cpp | 2 +- .../src/robotis_trajectory_calculator.cpp | 2 +- 7 files changed, 34 insertions(+), 34 deletions(-) diff --git a/robotis_math/include/robotis_math/robotis_linear_algebra.h b/robotis_math/include/robotis_math/robotis_linear_algebra.h index 6751b2b..a460709 100644 --- a/robotis_math/include/robotis_math/robotis_linear_algebra.h +++ b/robotis_math/include/robotis_math/robotis_linear_algebra.h @@ -31,12 +31,12 @@ /* * robotis_linear_algebra.h * - * Created on: June 6, 2016 + * Created on: June 7, 2016 * Author: sch */ -#ifndef ROBOTIS_LINEAR_ALGEBRA_H_ -#define ROBOTIS_LINEAR_ALGEBRA_H_ +#ifndef ROBOTIS_MATH_ROBOTIS_LINEAR_ALGEBRA_H_ +#define ROBOTIS_MATH_ROBOTIS_LINEAR_ALGEBRA_H_ #include @@ -64,14 +64,14 @@ Eigen::Quaterniond convertRotationToQuaternion(Eigen::MatrixXd rotation); Eigen::MatrixXd convertQuaternionToRPY(Eigen::Quaterniond quaternion); Eigen::MatrixXd convertQuaternionToRotation(Eigen::Quaterniond quaternion); -Eigen::MatrixXd hatto(Eigen::MatrixXd matrix3d); -Eigen::MatrixXd rodrigues(Eigen::MatrixXd hat_matrix, double angle); -Eigen::MatrixXd rot2omega(Eigen::MatrixXd rotation); -Eigen::MatrixXd cross(Eigen::MatrixXd vector3d_a, Eigen::MatrixXd vector3d_b); -double dot(Eigen::MatrixXd vector3d_a, Eigen::MatrixXd vector3d_b); +Eigen::MatrixXd calcHatto(Eigen::MatrixXd matrix3d); +Eigen::MatrixXd calcRodrigues(Eigen::MatrixXd hat_matrix, double angle); +Eigen::MatrixXd convertRotToOmega(Eigen::MatrixXd rotation); +Eigen::MatrixXd calcCross(Eigen::MatrixXd vector3d_a, Eigen::MatrixXd vector3d_b); +double calcInner(Eigen::MatrixXd vector3d_a, Eigen::MatrixXd vector3d_b); } -#endif /* ROBOTIS_LINEAR_ALGEBRA_H_ */ +#endif /* ROBOTIS_MATH_ROBOTIS_LINEAR_ALGEBRA_H_ */ diff --git a/robotis_math/include/robotis_math/robotis_math.h b/robotis_math/include/robotis_math/robotis_math.h index caa6dc9..f78f4be 100644 --- a/robotis_math/include/robotis_math/robotis_math.h +++ b/robotis_math/include/robotis_math/robotis_math.h @@ -31,13 +31,13 @@ /* * robotis_math.h * - * Created on: June 6, 2016 + * Created on: June 7, 2016 * Author: sch */ -#ifndef ROBOTIS_MATH_H_ -#define ROBOTIS_MATH_H_ +#ifndef ROBOTIS_MATH_ROBOTIS_MATH_H_ +#define ROBOTIS_MATH_ROBOTIS_MATH_H_ #include "robotis_trajectory_calculator.h" -#endif /* ROBOTIS_MATH_H_ */ +#endif /* ROBOTIS_MATH_ROBOTIS_MATH_H_ */ diff --git a/robotis_math/include/robotis_math/robotis_math_base.h b/robotis_math/include/robotis_math/robotis_math_base.h index 14c4bc3..e54caca 100644 --- a/robotis_math/include/robotis_math/robotis_math_base.h +++ b/robotis_math/include/robotis_math/robotis_math_base.h @@ -31,12 +31,12 @@ /* * robotis_math_base.h * - * Created on: June 6, 2016 + * Created on: June 7, 2016 * Author: sch */ -#ifndef ROBOTIS_MATH_BASE_H_ -#define ROBOTIS_MATH_BASE_H_ +#ifndef ROBOTIS_MATH_ROBOTIS_MATH_BASE_H_ +#define ROBOTIS_MATH_ROBOTIS_MATH_BASE_H_ #include @@ -60,4 +60,4 @@ double sign(double x); -#endif /* ROBOTIS_MATH_BASE_H_ */ +#endif /* ROBOTIS_MATH_ROBOTIS_MATH_BASE_H_ */ diff --git a/robotis_math/include/robotis_math/robotis_trajectory_calculator.h b/robotis_math/include/robotis_math/robotis_trajectory_calculator.h index 79857df..56bf644 100644 --- a/robotis_math/include/robotis_math/robotis_trajectory_calculator.h +++ b/robotis_math/include/robotis_math/robotis_trajectory_calculator.h @@ -31,12 +31,12 @@ /* * robotis_trajectory_calculator.h * - * Created on: June 6, 2016 + * Created on: June 7, 2016 * Author: sch */ -#ifndef ROBOTIS_TRAJECTORY_CALCULATOR_H_ -#define ROBOTIS_TRAJECTORY_CALCULATOR_H_ +#ifndef ROBOTIS_MATH_ROBOTIS_TRAJECTORY_CALCULATOR_H_ +#define ROBOTIS_MATH_ROBOTIS_TRAJECTORY_CALCULATOR_H_ #include "robotis_linear_algebra.h" #include "robotis_math_base.h" @@ -62,4 +62,4 @@ Eigen::MatrixXd calcArc3dTra(double smp_time, double mov_time, } -#endif /* ROBOTIS_TRAJECTORY_CALCULATOR_H_ */ +#endif /* ROBOTIS_MATH_ROBOTIS_TRAJECTORY_CALCULATOR_H_ */ diff --git a/robotis_math/src/robotis_linear_algebra.cpp b/robotis_math/src/robotis_linear_algebra.cpp index d85d744..328089c 100644 --- a/robotis_math/src/robotis_linear_algebra.cpp +++ b/robotis_math/src/robotis_linear_algebra.cpp @@ -31,7 +31,7 @@ /* * robotis_linear_algebra.cpp * - * Created on: June 6, 2016 + * Created on: June 7, 2016 * Author: sch */ @@ -224,7 +224,7 @@ Eigen::MatrixXd convertQuaternionToRotation(Eigen::Quaterniond quaternion) return rotation; } -Eigen::MatrixXd hatto(Eigen::MatrixXd matrix3d) +Eigen::MatrixXd calcHatto(Eigen::MatrixXd matrix3d) { Eigen::MatrixXd hatto(3,3); @@ -236,7 +236,7 @@ Eigen::MatrixXd hatto(Eigen::MatrixXd matrix3d) return hatto; } -Eigen::MatrixXd rodrigues(Eigen::MatrixXd hat_matrix, double angle) +Eigen::MatrixXd calcRodrigues(Eigen::MatrixXd hat_matrix, double angle) { Eigen::MatrixXd identity = Eigen::MatrixXd::Identity(3,3); Eigen::MatrixXd rodrigues = identity+hat_matrix*sin(angle)+hat_matrix*hat_matrix*(1-cos(angle)); @@ -244,18 +244,18 @@ Eigen::MatrixXd rodrigues(Eigen::MatrixXd hat_matrix, double angle) return rodrigues; } -Eigen::MatrixXd rot2omega(Eigen::MatrixXd rotation) +Eigen::MatrixXd convertRotToOmega(Eigen::MatrixXd rotation) { double eps = 1e-12; double alpha = (rotation.coeff(0,0)+rotation.coeff(1,1)+rotation.coeff(2,2)-1.0)/2.0; double alpha_dash = fabs( alpha - 1.0 ); - Eigen::MatrixXd rot2omega(3,1); + Eigen::MatrixXd rot_to_omega(3,1); if( alpha_dash < eps ) { - rot2omega << + rot_to_omega << 0.0, 0.0, 0.0; @@ -264,18 +264,18 @@ Eigen::MatrixXd rot2omega(Eigen::MatrixXd rotation) { double theta = acos(alpha); - rot2omega << + rot_to_omega << rotation.coeff(2,1)-rotation.coeff(1,2), rotation.coeff(0,2)-rotation.coeff(2,0), rotation.coeff(1,0)-rotation.coeff(0,1); - rot2omega = 0.5*(theta/sin(theta))*rot2omega; + rot_to_omega = 0.5*(theta/sin(theta))*rot_to_omega; } - return rot2omega; + return rot_to_omega; } -Eigen::MatrixXd cross(Eigen::MatrixXd vector3d_a, Eigen::MatrixXd vector3d_b) +Eigen::MatrixXd calcCross(Eigen::MatrixXd vector3d_a, Eigen::MatrixXd vector3d_b) { Eigen::MatrixXd cross(3,1); @@ -287,7 +287,7 @@ Eigen::MatrixXd cross(Eigen::MatrixXd vector3d_a, Eigen::MatrixXd vector3d_b) return cross; } -double dot(Eigen::MatrixXd vector3d_a, Eigen::MatrixXd vector3d_b) +double calcInner(Eigen::MatrixXd vector3d_a, Eigen::MatrixXd vector3d_b) { return vector3d_a.dot(vector3d_b); } diff --git a/robotis_math/src/robotis_math_base.cpp b/robotis_math/src/robotis_math_base.cpp index 60eb1f4..bd37d51 100644 --- a/robotis_math/src/robotis_math_base.cpp +++ b/robotis_math/src/robotis_math_base.cpp @@ -31,7 +31,7 @@ /* * robotis_math_base.cpp * - * Created on: June 6, 2016 + * Created on: June 7, 2016 * Author: sch */ diff --git a/robotis_math/src/robotis_trajectory_calculator.cpp b/robotis_math/src/robotis_trajectory_calculator.cpp index 4b776aa..5f66ffc 100644 --- a/robotis_math/src/robotis_trajectory_calculator.cpp +++ b/robotis_math/src/robotis_trajectory_calculator.cpp @@ -31,7 +31,7 @@ /* * robotis_trajectory_calculator.cpp * - * Created on: June 6, 2016 + * Created on: June 7, 2016 * Author: sch */ From 65dec07b1a7f8df6c36baee71afa940c63412ee6 Mon Sep 17 00:00:00 2001 From: Jay-Song Date: Tue, 14 Jun 2016 11:05:28 +0900 Subject: [PATCH 035/238] Add getTranslation4D Func --- .../include/robotis_math/robotis_linear_algebra.h | 1 + robotis_math/src/robotis_linear_algebra.cpp | 13 +++++++++++++ 2 files changed, 14 insertions(+) diff --git a/robotis_math/include/robotis_math/robotis_linear_algebra.h b/robotis_math/include/robotis_math/robotis_linear_algebra.h index a460709..9c62268 100644 --- a/robotis_math/include/robotis_math/robotis_linear_algebra.h +++ b/robotis_math/include/robotis_math/robotis_linear_algebra.h @@ -56,6 +56,7 @@ Eigen::MatrixXd getRotationX(double angle); Eigen::MatrixXd getRotationY(double angle); Eigen::MatrixXd getRotationZ(double angle); Eigen::MatrixXd getRotation4d(double roll, double pitch, double yaw); +Eigen::MatrixXd getTranslation4D(double position_x, double position_y, double position_z); Eigen::MatrixXd convertRotationToRPY(Eigen::MatrixXd rotation); Eigen::MatrixXd convertRPYToRotation(double roll, double pitch, double yaw); diff --git a/robotis_math/src/robotis_linear_algebra.cpp b/robotis_math/src/robotis_linear_algebra.cpp index 328089c..7683810 100644 --- a/robotis_math/src/robotis_linear_algebra.cpp +++ b/robotis_math/src/robotis_linear_algebra.cpp @@ -168,6 +168,19 @@ Eigen::MatrixXd getRotation4d(double roll, double pitch, double yaw ) return mat_rpy; } +Eigen::MatrixXd getTranslation4D(double position_x, double position_y, double position_z) +{ + Eigen::MatrixXd mat_translation(4,4); + + mat_translation << + 1, 0, 0, position_x, + 0, 1, 0, position_y, + 0, 0, 1, position_z, + 0, 0, 0, 1; + + return mat_translation; +} + Eigen::MatrixXd convertRotationToRPY(Eigen::MatrixXd rotation) { Eigen::MatrixXd rpy = Eigen::MatrixXd::Zero(3,1); From b6ab5951b9dc1887160f8ee72bf84512236064cd Mon Sep 17 00:00:00 2001 From: ROBOTIS-zerom Date: Tue, 14 Jun 2016 11:10:54 +0900 Subject: [PATCH 036/238] - moved robotis_controller_msgs package to ROBOTIS-Framework-msgs repository --- robotis_controller_msgs/CMakeLists.txt | 48 ------------------- .../msg/JointCtrlModule.msg | 2 - robotis_controller_msgs/msg/StatusMsg.msg | 10 ---- robotis_controller_msgs/msg/SyncWriteItem.msg | 3 -- robotis_controller_msgs/package.xml | 24 ---------- .../srv/GetJointModule.srv | 4 -- 6 files changed, 91 deletions(-) delete mode 100644 robotis_controller_msgs/CMakeLists.txt delete mode 100644 robotis_controller_msgs/msg/JointCtrlModule.msg delete mode 100644 robotis_controller_msgs/msg/StatusMsg.msg delete mode 100644 robotis_controller_msgs/msg/SyncWriteItem.msg delete mode 100644 robotis_controller_msgs/package.xml delete mode 100644 robotis_controller_msgs/srv/GetJointModule.srv diff --git a/robotis_controller_msgs/CMakeLists.txt b/robotis_controller_msgs/CMakeLists.txt deleted file mode 100644 index d9f015c..0000000 --- a/robotis_controller_msgs/CMakeLists.txt +++ /dev/null @@ -1,48 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(robotis_controller_msgs) - -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS - sensor_msgs - std_msgs - message_generation -) - -## Generate messages in the 'msg' folder -add_message_files( - FILES - SyncWriteItem.msg - JointCtrlModule.msg - StatusMsg.msg -) - -## Generate services in the 'srv' folder -add_service_files( - FILES - GetJointModule.srv -) - # Service1.srv -# Service2.srv -# ) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - -## Generate added messages and services with any dependencies listed here -generate_messages( - DEPENDENCIES - std_msgs - sensor_msgs -) - -catkin_package( -# INCLUDE_DIRS include - CATKIN_DEPENDS sensor_msgs std_msgs -) - diff --git a/robotis_controller_msgs/msg/JointCtrlModule.msg b/robotis_controller_msgs/msg/JointCtrlModule.msg deleted file mode 100644 index b91eb4d..0000000 --- a/robotis_controller_msgs/msg/JointCtrlModule.msg +++ /dev/null @@ -1,2 +0,0 @@ -string[] joint_name -string[] module_name \ No newline at end of file diff --git a/robotis_controller_msgs/msg/StatusMsg.msg b/robotis_controller_msgs/msg/StatusMsg.msg deleted file mode 100644 index 47b706c..0000000 --- a/robotis_controller_msgs/msg/StatusMsg.msg +++ /dev/null @@ -1,10 +0,0 @@ -# Status Constants -uint8 STATUS_UNKNOWN = 0 -uint8 STATUS_INFO = 1 -uint8 STATUS_WARN = 2 -uint8 STATUS_ERROR = 3 - -std_msgs/Header header -uint8 type -string module_name -string status_msg \ No newline at end of file diff --git a/robotis_controller_msgs/msg/SyncWriteItem.msg b/robotis_controller_msgs/msg/SyncWriteItem.msg deleted file mode 100644 index 4d602b6..0000000 --- a/robotis_controller_msgs/msg/SyncWriteItem.msg +++ /dev/null @@ -1,3 +0,0 @@ -string item_name -string[] joint_name -uint32[] value \ No newline at end of file diff --git a/robotis_controller_msgs/package.xml b/robotis_controller_msgs/package.xml deleted file mode 100644 index 8d54012..0000000 --- a/robotis_controller_msgs/package.xml +++ /dev/null @@ -1,24 +0,0 @@ - - - robotis_controller_msgs - 0.1.0 - The robotis_controller_msgs package - robotis - - BSD - - - - ROBOTIS - - catkin - - sensor_msgs - std_msgs - message_generation - - sensor_msgs - std_msgs - message_runtime - - \ No newline at end of file diff --git a/robotis_controller_msgs/srv/GetJointModule.srv b/robotis_controller_msgs/srv/GetJointModule.srv deleted file mode 100644 index bedde91..0000000 --- a/robotis_controller_msgs/srv/GetJointModule.srv +++ /dev/null @@ -1,4 +0,0 @@ -string[] joint_name ---- -string[] joint_name -string[] module_name \ No newline at end of file From 509c2cf483bb5517b3787ee888e405469511ed46 Mon Sep 17 00:00:00 2001 From: ROBOTIS-zerom Date: Thu, 23 Jun 2016 14:31:44 +0900 Subject: [PATCH 037/238] - startTimer() : after bulkread txpacket(), need some sleep() --- .../src/robotis_controller/robotis_controller.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/robotis_controller/src/robotis_controller/robotis_controller.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp index 8d8275d..340ee50 100644 --- a/robotis_controller/src/robotis_controller/robotis_controller.cpp +++ b/robotis_controller/src/robotis_controller/robotis_controller.cpp @@ -631,6 +631,8 @@ void RobotisController::startTimer() it->second->txPacket(); } + usleep(8 * 1000); + int error; struct sched_param param; pthread_attr_t attr; From a3e386e14e17252c1e54405db074ee58883383b8 Mon Sep 17 00:00:00 2001 From: ROBOTIS-zerom Date: Tue, 28 Jun 2016 15:51:49 +0900 Subject: [PATCH 038/238] - fixed typos - changed ROS_INFO -> fprintf (for processing speed) --- .../robotis_controller/robotis_controller.cpp | 22 +++++++++---------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/robotis_controller/src/robotis_controller/robotis_controller.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp index 340ee50..30b565e 100644 --- a/robotis_controller/src/robotis_controller/robotis_controller.cpp +++ b/robotis_controller/src/robotis_controller/robotis_controller.cpp @@ -433,7 +433,7 @@ void RobotisController::initializeDevice(const std::string init_file_path) if (dxl == NULL) { - ROS_WARN("Joint [%s] does not found.", joint_name.c_str()); + ROS_WARN("Joint [%s] was not found.", joint_name.c_str()); continue; } if (DEBUG_PRINT) @@ -451,7 +451,7 @@ void RobotisController::initializeDevice(const std::string init_file_path) ControlTableItem *item = dxl->ctrl_table_[item_name]; if (item == NULL) { - ROS_WARN("Control Item [%s] does not found.", item_name.c_str()); + ROS_WARN("Control Item [%s] was not found.", item_name.c_str()); continue; } @@ -596,9 +596,9 @@ void *RobotisController::timerThread(void *param) { if (controller->DEBUG_PRINT == true) { - ROS_WARN("[RobotisController::ThreadProc] NEXT TIME < CURR TIME.. (%f)[%ld.%09ld / %ld.%09ld]", - delta_nsec / 1000000.0, (long )next_time.tv_sec, (long )next_time.tv_nsec, - (long )curr_time.tv_sec, (long )curr_time.tv_nsec); + fprintf(stderr, "[RobotisController::ThreadProc] NEXT TIME < CURR TIME.. (%f)[%ld.%09ld / %ld.%09ld]", + delta_nsec / 1000000.0, (long )next_time.tv_sec, (long )next_time.tv_nsec, + (long )curr_time.tv_sec, (long )curr_time.tv_nsec); } // next_time = curr_time + 3 msec @@ -866,7 +866,7 @@ void RobotisController::process() if (DEBUG_PRINT) { time_duration = ros::Time::now() - start_time; - ROS_INFO("(%2.6f) BulkRead Rx & update state", time_duration.nsec * 0.000001); + fprintf(stderr, "(%2.6f) BulkRead Rx & update state", time_duration.nsec * 0.000001); } // Call SensorModule Process() @@ -885,7 +885,7 @@ void RobotisController::process() if (DEBUG_PRINT) { time_duration = ros::Time::now() - start_time; - ROS_INFO("(%2.6f) SensorModule Process() & save result", time_duration.nsec * 0.000001); + fprintf(stderr, "(%2.6f) SensorModule Process() & save result", time_duration.nsec * 0.000001); } if (controller_mode_ == MotionModuleMode) @@ -917,7 +917,7 @@ void RobotisController::process() if (result_state == NULL) { - ROS_INFO("[%s] %s", (*module_it)->getModuleName().c_str(), joint_name.c_str()); + fprintf(stderr, "[%s] %s", (*module_it)->getModuleName().c_str(), joint_name.c_str()); continue; } @@ -1005,7 +1005,7 @@ void RobotisController::process() if (DEBUG_PRINT) { time_duration = ros::Time::now() - start_time; - ROS_INFO("(%2.6f) MotionModule Process() & save result", time_duration.nsec * 0.000001); + fprintf(stderr, "(%2.6f) MotionModule Process() & save result", time_duration.nsec * 0.000001); } // SyncWrite @@ -1087,7 +1087,7 @@ void RobotisController::process() if (DEBUG_PRINT) { time_duration = ros::Time::now() - start_time; - ROS_INFO("(%2.6f) SyncWrite & BulkRead Tx", time_duration.nsec * 0.000001); + fprintf(stderr, "(%2.6f) SyncWrite & BulkRead Tx", time_duration.nsec * 0.000001); } // publish present & goal position @@ -1114,7 +1114,7 @@ void RobotisController::process() if (DEBUG_PRINT) { time_duration = ros::Time::now() - start_time; - ROS_WARN("(%2.6f) Process() DONE", time_duration.nsec * 0.000001); + fprintf(stderr, "(%2.6f) Process() DONE", time_duration.nsec * 0.000001); } is_process_running = false; From 6318166216570f952fe8f00619faae48248dc9e5 Mon Sep 17 00:00:00 2001 From: ROBOTIS-zerom Date: Tue, 28 Jun 2016 16:38:53 +0900 Subject: [PATCH 039/238] adjusted position min/max value. (MX-28, XM-430) --- robotis_device/devices/dynamixel/MX-28.device | 2 +- robotis_device/devices/dynamixel/XM-430.device | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/robotis_device/devices/dynamixel/MX-28.device b/robotis_device/devices/dynamixel/MX-28.device index f1cfc21..ebcad6e 100644 --- a/robotis_device/devices/dynamixel/MX-28.device +++ b/robotis_device/devices/dynamixel/MX-28.device @@ -47,7 +47,7 @@ position_p_gain_item_name = position_p_gain 30 | goal_position | 2 | RW | RAM | -28672 | 28672 | Y 32 | goal_velocity | 2 | RW | RAM | 0 | 1023 | N 34 | goal_torque | 2 | RW | RAM | 0 | 1023 | N - 36 | present_position | 2 | R | RAM | 0 | 4095 | N + 36 | present_position | 2 | R | RAM | -32768 | 32767 | Y 38 | present_velocity | 2 | R | RAM | 0 | 2048 | N 40 | present_load | 2 | R | RAM | 0 | 2048 | N 42 | present_voltage | 1 | R | RAM | 50 | 250 | N diff --git a/robotis_device/devices/dynamixel/XM-430.device b/robotis_device/devices/dynamixel/XM-430.device index 9f17d37..7fcec12 100644 --- a/robotis_device/devices/dynamixel/XM-430.device +++ b/robotis_device/devices/dynamixel/XM-430.device @@ -59,14 +59,14 @@ position_p_gain_item_name = position_p_gain 104 | goal_velocity | 4 | RW | RAM | 0 | 1023 | N 108 | profile_acceleration | 4 | RW | RAM | 0 | 32767 | N 112 | profile_velocity | 4 | RW | RAM | 0 | 1023 | N - 116 | goal_position | 4 | RW | RAM | 0 | 4095 | N + 116 | goal_position | 4 | RW | RAM | -1048575 | 1048575 | Y 120 | realtime_tick | 2 | R | RAM | 0 | 32767 | N 122 | moving | 1 | R | RAM | 0 | 1 | N 123 | moving_status | 1 | R | RAM | 0 | 255 | N 124 | present_pwm | 2 | R | RAM | 0 | 885 | N 126 | present_current | 2 | R | RAM | 0 | 1193 | N 128 | present_velocity | 4 | R | RAM | 0 | 1023 | N - 132 | present_position | 4 | R | RAM | 0 | 4095 | N + 132 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y 136 | velocity_trajectory | 4 | R | RAM | 0 | 1023 | N 140 | position_trajectory | 4 | R | RAM | 0 | 4095 | N 144 | present_input_voltage | 2 | R | RAM | 95 | 160 | N From e3381ca4240eff975758408e2704f0976d694d8e Mon Sep 17 00:00:00 2001 From: ROBOTIS-zerom Date: Tue, 28 Jun 2016 17:23:18 +0900 Subject: [PATCH 040/238] - added device file for MX-64 / MX-106 --- .../devices/dynamixel/MX-106.device | 63 +++++++++++++++++++ robotis_device/devices/dynamixel/MX-64.device | 63 +++++++++++++++++++ 2 files changed, 126 insertions(+) create mode 100644 robotis_device/devices/dynamixel/MX-106.device create mode 100644 robotis_device/devices/dynamixel/MX-64.device diff --git a/robotis_device/devices/dynamixel/MX-106.device b/robotis_device/devices/dynamixel/MX-106.device new file mode 100644 index 0000000..23b16f1 --- /dev/null +++ b/robotis_device/devices/dynamixel/MX-106.device @@ -0,0 +1,63 @@ +[device info] +model_name = MX-106 +device_type = dynamixel + +[type info] +value_of_0_radian_position = 2048 +value_of_min_radian_position = 0 +value_of_max_radian_position = 4095 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = +position_d_gain_item_name = position_d_gain +position_i_gain_item_name = position_i_gain +position_p_gain_item_name = position_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 2 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 3 | ID | 1 | RW | EEPROM | 0 | 252 | N + 4 | baudrate | 1 | RW | EEPROM | 0 | 252 | N + 5 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 6 | CW_angle_limit | 2 | RW | EEPROM | 0 | 4095 | N + 8 | CCW_angle_limit | 2 | RW | EEPROM | 0 | 4095 | N + 10 | drive_mode | 1 | RW | EEPROM | 0 | 3 | N + 11 | max_temperature_limit | 1 | RW | EEPROM | 0 | 99 | N + 12 | min_voltage_limit | 1 | RW | EEPROM | 0 | 250 | N + 13 | max_voltage_limit | 1 | RW | EEPROM | 0 | 250 | N + 14 | max_torque | 2 | RW | EEPROM | 0 | 1023 | N + 16 | status_return_level | 1 | RW | EEPROM | 0 | 2 | N + 17 | alarm_LED | 1 | RW | EEPROM | 0 | 127 | N + 18 | alarm_shutdown | 1 | RW | EEPROM | 0 | 127 | N + 20 | multi_turn_offset | 2 | RW | EEPROM | -26624 | 26624 | Y + 22 | resolution_dividor | 1 | RW | EEPROM | 1 | 255 | N + 24 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 25 | LED | 1 | RW | RAM | 0 | 1 | N + 26 | position_d_gain | 1 | RW | RAM | 0 | 254 | N + 27 | position_i_gain | 1 | RW | RAM | 0 | 254 | N + 28 | position_p_gain | 1 | RW | RAM | 0 | 254 | N + 30 | goal_position | 2 | RW | RAM | -28672 | 28672 | Y + 32 | goal_velocity | 2 | RW | RAM | 0 | 1023 | N + 34 | goal_torque | 2 | RW | RAM | 0 | 1023 | N + 36 | present_position | 2 | R | RAM | -32768 | 32767 | Y + 38 | present_velocity | 2 | R | RAM | 0 | 2048 | N + 40 | present_load | 2 | R | RAM | 0 | 2048 | N + 42 | present_voltage | 1 | R | RAM | 50 | 250 | N + 43 | present_temperature | 1 | R | RAM | 0 | 99 | N + 44 | registered_instruction | 1 | R | RAM | 0 | 1 | N + 46 | is_moving | 1 | R | RAM | 0 | 1 | N + 47 | EEPROM_lock | 1 | RW | RAM | 0 | 1 | N + 48 | punch | 2 | RW | RAM | 0 | 1023 | N + 68 | current_consumption | 2 | RW | RAM | 0 | 4095 | N + 70 | torque_control_mode | 1 | RW | RAM | 0 | 1 | N + 71 | torque_control_goal | 2 | RW | RAM | 0 | 2047 | N + 73 | goal_acceleration | 1 | RW | RAM | 0 | 254 | N + diff --git a/robotis_device/devices/dynamixel/MX-64.device b/robotis_device/devices/dynamixel/MX-64.device new file mode 100644 index 0000000..2988c83 --- /dev/null +++ b/robotis_device/devices/dynamixel/MX-64.device @@ -0,0 +1,63 @@ +[device info] +model_name = MX-64 +device_type = dynamixel + +[type info] +value_of_0_radian_position = 2048 +value_of_min_radian_position = 0 +value_of_max_radian_position = 4095 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = +position_d_gain_item_name = position_d_gain +position_i_gain_item_name = position_i_gain +position_p_gain_item_name = position_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 2 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 3 | ID | 1 | RW | EEPROM | 0 | 252 | N + 4 | baudrate | 1 | RW | EEPROM | 0 | 252 | N + 5 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 6 | CW_angle_limit | 2 | RW | EEPROM | 0 | 4095 | N + 8 | CCW_angle_limit | 2 | RW | EEPROM | 0 | 4095 | N + 10 | drive_mode | 1 | RW | EEPROM | 0 | 3 | N + 11 | max_temperature_limit | 1 | RW | EEPROM | 0 | 99 | N + 12 | min_voltage_limit | 1 | RW | EEPROM | 0 | 250 | N + 13 | max_voltage_limit | 1 | RW | EEPROM | 0 | 250 | N + 14 | max_torque | 2 | RW | EEPROM | 0 | 1023 | N + 16 | status_return_level | 1 | RW | EEPROM | 0 | 2 | N + 17 | alarm_LED | 1 | RW | EEPROM | 0 | 127 | N + 18 | alarm_shutdown | 1 | RW | EEPROM | 0 | 127 | N + 20 | multi_turn_offset | 2 | RW | EEPROM | -26624 | 26624 | Y + 22 | resolution_dividor | 1 | RW | EEPROM | 1 | 255 | N + 24 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 25 | LED | 1 | RW | RAM | 0 | 1 | N + 26 | position_d_gain | 1 | RW | RAM | 0 | 254 | N + 27 | position_i_gain | 1 | RW | RAM | 0 | 254 | N + 28 | position_p_gain | 1 | RW | RAM | 0 | 254 | N + 30 | goal_position | 2 | RW | RAM | -28672 | 28672 | Y + 32 | goal_velocity | 2 | RW | RAM | 0 | 1023 | N + 34 | goal_torque | 2 | RW | RAM | 0 | 1023 | N + 36 | present_position | 2 | R | RAM | -32768 | 32767 | Y + 38 | present_velocity | 2 | R | RAM | 0 | 2048 | N + 40 | present_load | 2 | R | RAM | 0 | 2048 | N + 42 | present_voltage | 1 | R | RAM | 50 | 250 | N + 43 | present_temperature | 1 | R | RAM | 0 | 99 | N + 44 | registered_instruction | 1 | R | RAM | 0 | 1 | N + 46 | is_moving | 1 | R | RAM | 0 | 1 | N + 47 | EEPROM_lock | 1 | RW | RAM | 0 | 1 | N + 48 | punch | 2 | RW | RAM | 0 | 1023 | N + 68 | current_consumption | 2 | RW | RAM | 0 | 4095 | N + 70 | torque_control_mode | 1 | RW | RAM | 0 | 1 | N + 71 | torque_control_goal | 2 | RW | RAM | 0 | 2047 | N + 73 | goal_acceleration | 1 | RW | RAM | 0 | 254 | N + From 68f2ac81f05f33b86bf34594a94eaaf513df5f3e Mon Sep 17 00:00:00 2001 From: ROBOTIS-zerom Date: Wed, 6 Jul 2016 16:26:40 +0900 Subject: [PATCH 041/238] - modified torque control code --- .../robotis_controller/robotis_controller.h | 4 +- .../robotis_controller/robotis_controller.cpp | 62 ++++++++++++++----- .../devices/dynamixel/H42-20-S300-R.device | 2 +- .../devices/dynamixel/H54-100-S500-R.device | 2 +- .../devices/dynamixel/H54-200-B500-R.device | 2 +- .../devices/dynamixel/H54-200-S500-R.device | 2 +- .../devices/dynamixel/L54-30-S400-R.device | 2 - .../devices/dynamixel/L54-30-S500-R.device | 2 - .../devices/dynamixel/L54-50-S290-R.device | 2 - .../devices/dynamixel/L54-50-S500-R.device | 2 - .../devices/dynamixel/M42-10-S260-R.device | 2 - .../devices/dynamixel/M54-40-S250-R.device | 2 - .../devices/dynamixel/M54-60-S250-R.device | 2 - .../devices/dynamixel/XM-430.device | 2 +- .../include/robotis_device/dynamixel.h | 8 +-- .../include/robotis_device/dynamixel_state.h | 4 +- .../src/robotis_device/dynamixel.cpp | 16 ++--- robotis_device/src/robotis_device/robot.cpp | 8 +-- 18 files changed, 72 insertions(+), 54 deletions(-) diff --git a/robotis_controller/include/robotis_controller/robotis_controller.h b/robotis_controller/include/robotis_controller/robotis_controller.h index e2c8b64..e6befef 100644 --- a/robotis_controller/include/robotis_controller/robotis_controller.h +++ b/robotis_controller/include/robotis_controller/robotis_controller.h @@ -115,7 +115,9 @@ public: ros::Publisher present_joint_state_pub_; ros::Publisher current_module_pub_; - std::map gazebo_joint_pub_; + std::map gazebo_joint_position_pub_; + std::map gazebo_joint_velocity_pub_; + std::map gazebo_joint_effort_pub_; static void *timerThread(void *param); diff --git a/robotis_controller/src/robotis_controller/robotis_controller.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp index 30b565e..cd8fd0f 100644 --- a/robotis_controller/src/robotis_controller/robotis_controller.cpp +++ b/robotis_controller/src/robotis_controller/robotis_controller.cpp @@ -159,8 +159,8 @@ void RobotisController::initializeSyncWrite() else if ((dxl->present_current_item_ != 0) && (dxl->bulk_read_items_[i]->item_name_ == dxl->present_current_item_->item_name_)) { - dxl->dxl_state_->present_current_ = dxl->convertValue2Current(read_data); - dxl->dxl_state_->goal_current_ = dxl->dxl_state_->present_current_; + dxl->dxl_state_->present_current_ = dxl->convertValue2Torque(read_data); + dxl->dxl_state_->goal_torque_ = dxl->dxl_state_->present_current_; } } } @@ -557,8 +557,12 @@ void RobotisController::msgQueueThread() { for (std::map::iterator it = robot_->dxls_.begin(); it != robot_->dxls_.end(); it++) { - gazebo_joint_pub_[it->first] = ros_node.advertise( - "/" + gazebo_robot_name_ + "/" + it->first + "_pos/command", 1); + gazebo_joint_position_pub_[it->first] = ros_node.advertise( + "/" + gazebo_robot_name_ + "/" + it->first + "_position/command", 1); + gazebo_joint_velocity_pub_[it->first] = ros_node.advertise( + "/" + gazebo_robot_name_ + "/" + it->first + "_velocity/command", 1); + gazebo_joint_effort_pub_[it->first] = ros_node.advertise( + "/" + gazebo_robot_name_ + "/" + it->first + "_effort/command", 1); } } @@ -814,13 +818,13 @@ void RobotisController::process() else if (dxl->present_velocity_item_ != 0 && item->item_name_ == dxl->present_velocity_item_->item_name_) dxl->dxl_state_->present_velocity_ = dxl->convertValue2Velocity(data); else if (dxl->present_current_item_ != 0 && item->item_name_ == dxl->present_current_item_->item_name_) - dxl->dxl_state_->present_current_ = dxl->convertValue2Current(data); + dxl->dxl_state_->present_current_ = dxl->convertValue2Torque(data); else if (dxl->goal_position_item_ != 0 && item->item_name_ == dxl->goal_position_item_->item_name_) dxl->dxl_state_->goal_position_ = dxl->convertValue2Radian(data) - dxl->dxl_state_->position_offset_; // remove offset else if (dxl->goal_velocity_item_ != 0 && item->item_name_ == dxl->goal_velocity_item_->item_name_) dxl->dxl_state_->goal_velocity_ = dxl->convertValue2Velocity(data); else if (dxl->goal_current_item_ != 0 && item->item_name_ == dxl->goal_current_item_->item_name_) - dxl->dxl_state_->goal_current_ = dxl->convertValue2Current(data); + dxl->dxl_state_->goal_torque_ = dxl->convertValue2Torque(data); else dxl->dxl_state_->bulk_read_table_[item->item_name_] = data; } @@ -982,11 +986,11 @@ void RobotisController::process() } else if ((*module_it)->getControlMode() == CurrentControl) { - dxl_state->goal_current_ = result_state->goal_current_; + dxl_state->goal_torque_ = result_state->goal_torque_; if (gazebo_mode_ == false) { - uint32_t curr_data = dxl->convertCurrent2Value(dxl_state->goal_current_); + uint32_t curr_data = dxl->convertTorque2Value(dxl_state->goal_torque_); uint8_t sync_write_data[2] = { 0 }; sync_write_data[0] = DXL_LOBYTE(curr_data); sync_write_data[1] = DXL_HIBYTE(curr_data); @@ -1041,13 +1045,39 @@ void RobotisController::process() } else if (gazebo_mode_ == true) { - std_msgs::Float64 joint_msg; - - for (std::map::iterator dxl_it = robot_->dxls_.begin(); dxl_it != robot_->dxls_.end(); - dxl_it++) + for (std::list::iterator module_it = motion_modules_.begin(); module_it != motion_modules_.end(); module_it++) { - joint_msg.data = dxl_it->second->dxl_state_->goal_position_; - gazebo_joint_pub_[dxl_it->first].publish(joint_msg); + if ((*module_it)->getModuleEnable() == false) + continue; + + std_msgs::Float64 joint_msg; + + for (std::map::iterator dxl_it = robot_->dxls_.begin(); dxl_it != robot_->dxls_.end(); + dxl_it++) + { + std::string joint_name = dxl_it->first; + Dynamixel *dxl = dxl_it->second; + DynamixelState *dxl_state = dxl_it->second->dxl_state_; + + if (dxl->ctrl_module_name_ == (*module_it)->getModuleName()) + { + if ((*module_it)->getControlMode() == PositionControl) + { + joint_msg.data = dxl_state->goal_position_; + gazebo_joint_position_pub_[joint_name].publish(joint_msg); + } + else if ((*module_it)->getControlMode() == VelocityControl) + { + joint_msg.data = dxl_state->goal_velocity_; + gazebo_joint_velocity_pub_[joint_name].publish(joint_msg); + } + else if ((*module_it)->getControlMode() == CurrentControl) + { + joint_msg.data = dxl_state->goal_torque_; + gazebo_joint_effort_pub_[joint_name].publish(joint_msg); + } + } + } } } } @@ -1104,7 +1134,7 @@ void RobotisController::process() goal_state.name.push_back(joint_name); goal_state.position.push_back(dxl->dxl_state_->goal_position_); goal_state.velocity.push_back(dxl->dxl_state_->goal_velocity_); - goal_state.effort.push_back(dxl->dxl_state_->goal_current_); + goal_state.effort.push_back(dxl->dxl_state_->goal_torque_); } // -> publish present joint_states & goal joint states topic @@ -1510,7 +1540,7 @@ void RobotisController::setCtrlModuleThread(std::string ctrl_module) } else if (mode == CurrentControl) { - uint32_t curr_data = dxl->convertCurrent2Value(dxl->dxl_state_->goal_current_); + uint32_t curr_data = dxl->convertTorque2Value(dxl->dxl_state_->goal_torque_); uint8_t sync_write_data[4] = { 0 }; sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(curr_data)); sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(curr_data)); diff --git a/robotis_device/devices/dynamixel/H42-20-S300-R.device b/robotis_device/devices/dynamixel/H42-20-S300-R.device index 1ed385f..e348f42 100644 --- a/robotis_device/devices/dynamixel/H42-20-S300-R.device +++ b/robotis_device/devices/dynamixel/H42-20-S300-R.device @@ -3,7 +3,7 @@ model_name = H42-20-S300-R device_type = dynamixel [type info] -current_ratio = 4.0283203125 +torque_to_current_value_ratio = 27.15146 value_of_0_radian_position = 0 value_of_min_radian_position = -151900 diff --git a/robotis_device/devices/dynamixel/H54-100-S500-R.device b/robotis_device/devices/dynamixel/H54-100-S500-R.device index 09ef828..cdf81d0 100644 --- a/robotis_device/devices/dynamixel/H54-100-S500-R.device +++ b/robotis_device/devices/dynamixel/H54-100-S500-R.device @@ -3,7 +3,7 @@ model_name = H54-100-S500-R device_type = dynamixel [type info] -current_ratio = 16.11328125 +torque_to_current_value_ratio = 9.66026 value_of_0_radian_position = 0 value_of_min_radian_position = -250950 diff --git a/robotis_device/devices/dynamixel/H54-200-B500-R.device b/robotis_device/devices/dynamixel/H54-200-B500-R.device index 8791d7c..d002f97 100644 --- a/robotis_device/devices/dynamixel/H54-200-B500-R.device +++ b/robotis_device/devices/dynamixel/H54-200-B500-R.device @@ -3,7 +3,7 @@ model_name = H54-200-B500-R device_type = dynamixel [type info] -current_ratio = 16.11328125 +torque_to_current_value_ratio = 9.09201 value_of_0_radian_position = 0 value_of_min_radian_position = -250950 diff --git a/robotis_device/devices/dynamixel/H54-200-S500-R.device b/robotis_device/devices/dynamixel/H54-200-S500-R.device index dba268c..7e18900 100644 --- a/robotis_device/devices/dynamixel/H54-200-S500-R.device +++ b/robotis_device/devices/dynamixel/H54-200-S500-R.device @@ -3,7 +3,7 @@ model_name = H54-200-S500-R device_type = dynamixel [type info] -current_ratio = 16.11328125 +torque_to_current_value_ratio = 9.09201 value_of_0_radian_position = 0 value_of_min_radian_position = -250950 diff --git a/robotis_device/devices/dynamixel/L54-30-S400-R.device b/robotis_device/devices/dynamixel/L54-30-S400-R.device index 8cdcff3..d684e67 100644 --- a/robotis_device/devices/dynamixel/L54-30-S400-R.device +++ b/robotis_device/devices/dynamixel/L54-30-S400-R.device @@ -3,8 +3,6 @@ model_name = L54-30-S400-R device_type = dynamixel [type info] -current_ratio = 16.11328125 - value_of_0_radian_position = 0 value_of_min_radian_position = -144198 value_of_max_radian_position = 144198 diff --git a/robotis_device/devices/dynamixel/L54-30-S500-R.device b/robotis_device/devices/dynamixel/L54-30-S500-R.device index db5a0b4..68848a2 100644 --- a/robotis_device/devices/dynamixel/L54-30-S500-R.device +++ b/robotis_device/devices/dynamixel/L54-30-S500-R.device @@ -3,8 +3,6 @@ model_name = L54-30-S500-R device_type = dynamixel [type info] -current_ratio = 16.11328125 - value_of_0_radian_position = 0 value_of_min_radian_position = -180684 value_of_max_radian_position = 180684 diff --git a/robotis_device/devices/dynamixel/L54-50-S290-R.device b/robotis_device/devices/dynamixel/L54-50-S290-R.device index cbd0453..3cbd843 100644 --- a/robotis_device/devices/dynamixel/L54-50-S290-R.device +++ b/robotis_device/devices/dynamixel/L54-50-S290-R.device @@ -3,8 +3,6 @@ model_name = L54-50-S290-R device_type = dynamixel [type info] -current_ratio = 16.11328125 - value_of_0_radian_position = 0 value_of_min_radian_position = -103860 value_of_max_radian_position = 103860 diff --git a/robotis_device/devices/dynamixel/L54-50-S500-R.device b/robotis_device/devices/dynamixel/L54-50-S500-R.device index d72f2ef..bfbf0c6 100644 --- a/robotis_device/devices/dynamixel/L54-50-S500-R.device +++ b/robotis_device/devices/dynamixel/L54-50-S500-R.device @@ -3,8 +3,6 @@ model_name = L54-50-S500-R device_type = dynamixel [type info] -current_ratio = 16.11328125 - value_of_0_radian_position = 0 value_of_min_radian_position = -180684 value_of_max_radian_position = 180684 diff --git a/robotis_device/devices/dynamixel/M42-10-S260-R.device b/robotis_device/devices/dynamixel/M42-10-S260-R.device index 8c96988..f21bb44 100644 --- a/robotis_device/devices/dynamixel/M42-10-S260-R.device +++ b/robotis_device/devices/dynamixel/M42-10-S260-R.device @@ -3,8 +3,6 @@ model_name = M42-10-S260-R device_type = dynamixel [type info] -current_ratio = 4.0283203125 - value_of_0_radian_position = 0 value_of_min_radian_position = -131584 value_of_max_radian_position = 131584 diff --git a/robotis_device/devices/dynamixel/M54-40-S250-R.device b/robotis_device/devices/dynamixel/M54-40-S250-R.device index 2bc7c1c..d9424ad 100644 --- a/robotis_device/devices/dynamixel/M54-40-S250-R.device +++ b/robotis_device/devices/dynamixel/M54-40-S250-R.device @@ -3,8 +3,6 @@ model_name = M54-40-S250-R device_type = dynamixel [type info] -current_ratio = 16.11328125 - value_of_0_radian_position = 0 value_of_min_radian_position = -125700 value_of_max_radian_position = 125700 diff --git a/robotis_device/devices/dynamixel/M54-60-S250-R.device b/robotis_device/devices/dynamixel/M54-60-S250-R.device index 72382de..284cac9 100644 --- a/robotis_device/devices/dynamixel/M54-60-S250-R.device +++ b/robotis_device/devices/dynamixel/M54-60-S250-R.device @@ -3,8 +3,6 @@ model_name = M54-60-S250-R device_type = dynamixel [type info] -current_ratio = 16.11328125 - value_of_0_radian_position = 0 value_of_min_radian_position = -125700 value_of_max_radian_position = 125700 diff --git a/robotis_device/devices/dynamixel/XM-430.device b/robotis_device/devices/dynamixel/XM-430.device index 7fcec12..3f33b73 100644 --- a/robotis_device/devices/dynamixel/XM-430.device +++ b/robotis_device/devices/dynamixel/XM-430.device @@ -3,7 +3,7 @@ model_name = XM-430 device_type = dynamixel [type info] -current_ratio = 2.69 +torque_to_current_value_ratio = 178.842161 value_of_0_radian_position = 2048 value_of_min_radian_position = 0 diff --git a/robotis_device/include/robotis_device/dynamixel.h b/robotis_device/include/robotis_device/dynamixel.h index 916f4c5..349020a 100644 --- a/robotis_device/include/robotis_device/dynamixel.h +++ b/robotis_device/include/robotis_device/dynamixel.h @@ -56,8 +56,8 @@ public: std::string ctrl_module_name_; DynamixelState *dxl_state_; - double current_ratio_; - double velocity_ratio_; + double velocity_to_value_ratio_; + double torque_to_current_value_ratio_; int32_t value_of_0_radian_position_; int32_t value_of_min_radian_position_; @@ -82,8 +82,8 @@ public: double convertValue2Velocity(int32_t value); int32_t convertVelocity2Value(double velocity); - double convertValue2Current(int16_t value); - int16_t convertCurrent2Value(double current); + double convertValue2Torque(int16_t value); + int16_t convertTorque2Value(double torque); }; } diff --git a/robotis_device/include/robotis_device/dynamixel_state.h b/robotis_device/include/robotis_device/dynamixel_state.h index b5f4d4d..833c0f3 100644 --- a/robotis_device/include/robotis_device/dynamixel_state.h +++ b/robotis_device/include/robotis_device/dynamixel_state.h @@ -58,7 +58,7 @@ public: double present_current_; double goal_position_; double goal_velocity_; - double goal_current_; + double goal_torque_; double position_p_gain_; std::map bulk_read_table_; @@ -72,7 +72,7 @@ public: present_current_(0.0), goal_position_(0.0), goal_velocity_(0.0), - goal_current_(0.0), + goal_torque_(0.0), position_p_gain_(0.0), position_offset_(0.0) { diff --git a/robotis_device/src/robotis_device/dynamixel.cpp b/robotis_device/src/robotis_device/dynamixel.cpp index c90e4eb..ba316b2 100644 --- a/robotis_device/src/robotis_device/dynamixel.cpp +++ b/robotis_device/src/robotis_device/dynamixel.cpp @@ -41,8 +41,8 @@ using namespace robotis_framework; Dynamixel::Dynamixel(int id, std::string model_name, float protocol_version) : ctrl_module_name_("none"), - current_ratio_(1.0), - velocity_ratio_(1.0), + torque_to_current_value_ratio_(1.0), + velocity_to_value_ratio_(1.0), value_of_0_radian_position_(0), value_of_min_radian_position_(0), value_of_max_radian_position_(0), @@ -128,20 +128,20 @@ int32_t Dynamixel::convertRadian2Value(double radian) double Dynamixel::convertValue2Velocity(int32_t value) { - return (double) value * velocity_ratio_; + return (double) value / velocity_to_value_ratio_; } int32_t Dynamixel::convertVelocity2Value(double velocity) { - return (int32_t) (velocity / velocity_ratio_);; + return (int32_t) (velocity * velocity_to_value_ratio_);; } -double Dynamixel::convertValue2Current(int16_t value) +double Dynamixel::convertValue2Torque(int16_t value) { - return (double) value * current_ratio_; + return (double) value / torque_to_current_value_ratio_; } -int16_t Dynamixel::convertCurrent2Value(double current) +int16_t Dynamixel::convertTorque2Value(double torque) { - return (int16_t) (current / current_ratio_); + return (int16_t) (torque * torque_to_current_value_ratio_); } diff --git a/robotis_device/src/robotis_device/robot.cpp b/robotis_device/src/robotis_device/robot.cpp index 1ae3fb9..900a9b9 100644 --- a/robotis_device/src/robotis_device/robot.cpp +++ b/robotis_device/src/robotis_device/robot.cpp @@ -366,10 +366,10 @@ Dynamixel *Robot::getDynamixel(std::string path, int id, std::string port, float if (tokens.size() != 2) continue; - if (tokens[0] == "current_ratio") - dxl->current_ratio_ = std::atof(tokens[1].c_str()); - else if (tokens[0] == "velocity_ratio") - dxl->velocity_ratio_ = std::atof(tokens[1].c_str()); + if (tokens[0] == "torque_to_current_value_ratio") + dxl->torque_to_current_value_ratio_ = std::atof(tokens[1].c_str()); + else if (tokens[0] == "velocity_to_value_ratio") + dxl->velocity_to_value_ratio_ = std::atof(tokens[1].c_str()); else if (tokens[0] == "value_of_0_radian_position") dxl->value_of_0_radian_position_ = std::atoi(tokens[1].c_str()); From 55e2f6e0f8df4648d7c9508920e5052ec93261a1 Mon Sep 17 00:00:00 2001 From: ROBOTIS-zerom Date: Wed, 6 Jul 2016 16:35:54 +0900 Subject: [PATCH 042/238] - rename (present_current_ -> present_torque_) --- .../src/robotis_controller/robotis_controller.cpp | 10 +++++----- .../include/robotis_device/dynamixel_state.h | 4 ++-- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/robotis_controller/src/robotis_controller/robotis_controller.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp index cd8fd0f..479f38b 100644 --- a/robotis_controller/src/robotis_controller/robotis_controller.cpp +++ b/robotis_controller/src/robotis_controller/robotis_controller.cpp @@ -159,8 +159,8 @@ void RobotisController::initializeSyncWrite() else if ((dxl->present_current_item_ != 0) && (dxl->bulk_read_items_[i]->item_name_ == dxl->present_current_item_->item_name_)) { - dxl->dxl_state_->present_current_ = dxl->convertValue2Torque(read_data); - dxl->dxl_state_->goal_torque_ = dxl->dxl_state_->present_current_; + dxl->dxl_state_->present_torque_ = dxl->convertValue2Torque(read_data); + dxl->dxl_state_->goal_torque_ = dxl->dxl_state_->present_torque_; } } } @@ -818,7 +818,7 @@ void RobotisController::process() else if (dxl->present_velocity_item_ != 0 && item->item_name_ == dxl->present_velocity_item_->item_name_) dxl->dxl_state_->present_velocity_ = dxl->convertValue2Velocity(data); else if (dxl->present_current_item_ != 0 && item->item_name_ == dxl->present_current_item_->item_name_) - dxl->dxl_state_->present_current_ = dxl->convertValue2Torque(data); + dxl->dxl_state_->present_torque_ = dxl->convertValue2Torque(data); else if (dxl->goal_position_item_ != 0 && item->item_name_ == dxl->goal_position_item_->item_name_) dxl->dxl_state_->goal_position_ = dxl->convertValue2Radian(data) - dxl->dxl_state_->position_offset_; // remove offset else if (dxl->goal_velocity_item_ != 0 && item->item_name_ == dxl->goal_velocity_item_->item_name_) @@ -1129,7 +1129,7 @@ void RobotisController::process() present_state.name.push_back(joint_name); present_state.position.push_back(dxl->dxl_state_->present_position_); present_state.velocity.push_back(dxl->dxl_state_->present_velocity_); - present_state.effort.push_back(dxl->dxl_state_->present_current_); + present_state.effort.push_back(dxl->dxl_state_->present_torque_); goal_state.name.push_back(joint_name); goal_state.position.push_back(dxl->dxl_state_->goal_position_); @@ -1607,7 +1607,7 @@ void RobotisController::gazeboJointStatesCallback(const sensor_msgs::JointState: { d_it->second->dxl_state_->present_position_ = msg->position[i]; d_it->second->dxl_state_->present_velocity_ = msg->velocity[i]; - d_it->second->dxl_state_->present_current_ = msg->effort[i]; + d_it->second->dxl_state_->present_torque_ = msg->effort[i]; } } diff --git a/robotis_device/include/robotis_device/dynamixel_state.h b/robotis_device/include/robotis_device/dynamixel_state.h index 833c0f3..a318a1f 100644 --- a/robotis_device/include/robotis_device/dynamixel_state.h +++ b/robotis_device/include/robotis_device/dynamixel_state.h @@ -55,7 +55,7 @@ public: double present_position_; double present_velocity_; - double present_current_; + double present_torque_; double goal_position_; double goal_velocity_; double goal_torque_; @@ -69,7 +69,7 @@ public: : update_time_stamp_(0, 0), present_position_(0.0), present_velocity_(0.0), - present_current_(0.0), + present_torque_(0.0), goal_position_(0.0), goal_velocity_(0.0), goal_torque_(0.0), From 27915069263fe93fc69b2bb26098990aea677a97 Mon Sep 17 00:00:00 2001 From: ROBOTIS-zerom Date: Wed, 6 Jul 2016 18:24:33 +0900 Subject: [PATCH 043/238] - rename ControlMode(CurrentControl -> TorqueControl) - rename (port_to_sync_write_torque_ -> port_to_sync_write_current_) --- .../robotis_controller/robotis_controller.h | 2 +- .../robotis_controller/robotis_controller.cpp | 40 +++++++++---------- .../robotis_framework_common/motion_module.h | 2 +- 3 files changed, 22 insertions(+), 22 deletions(-) diff --git a/robotis_controller/include/robotis_controller/robotis_controller.h b/robotis_controller/include/robotis_controller/robotis_controller.h index e6befef..ae91370 100644 --- a/robotis_controller/include/robotis_controller/robotis_controller.h +++ b/robotis_controller/include/robotis_controller/robotis_controller.h @@ -107,7 +107,7 @@ public: /* sync write */ std::map port_to_sync_write_position_; std::map port_to_sync_write_velocity_; - std::map port_to_sync_write_torque_; + std::map port_to_sync_write_current_; std::map port_to_sync_write_position_p_gain_; /* publisher */ diff --git a/robotis_controller/src/robotis_controller/robotis_controller.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp index 479f38b..dfa8ffe 100644 --- a/robotis_controller/src/robotis_controller/robotis_controller.cpp +++ b/robotis_controller/src/robotis_controller/robotis_controller.cpp @@ -106,8 +106,8 @@ void RobotisController::initializeSyncWrite() if (it->second != NULL) it->second->clearParam(); } - for (std::map::iterator it = port_to_sync_write_torque_.begin(); - it != port_to_sync_write_torque_.end(); it++) + for (std::map::iterator it = port_to_sync_write_current_.begin(); + it != port_to_sync_write_current_.end(); it++) { if (it->second != NULL) it->second->clearParam(); @@ -231,7 +231,7 @@ bool RobotisController::initialize(const std::string robot_file_path, const std: if (default_device->goal_current_item_ != 0) { - port_to_sync_write_torque_[port_name] + port_to_sync_write_current_[port_name] = new dynamixel::GroupSyncWrite(port, default_pkt_handler, default_device->goal_current_item_->address_, @@ -707,8 +707,8 @@ void RobotisController::stopTimer() if (it->second != NULL) it->second->clearParam(); } - for (std::map::iterator it = port_to_sync_write_torque_.begin(); - it != port_to_sync_write_torque_.end(); it++) + for (std::map::iterator it = port_to_sync_write_current_.begin(); + it != port_to_sync_write_current_.end(); it++) { if (it->second != NULL) it->second->clearParam(); @@ -984,7 +984,7 @@ void RobotisController::process() port_to_sync_write_velocity_[dxl->port_name_]->changeParam(dxl->id_, sync_write_data); } } - else if ((*module_it)->getControlMode() == CurrentControl) + else if ((*module_it)->getControlMode() == TorqueControl) { dxl_state->goal_torque_ = result_state->goal_torque_; @@ -995,8 +995,8 @@ void RobotisController::process() sync_write_data[0] = DXL_LOBYTE(curr_data); sync_write_data[1] = DXL_HIBYTE(curr_data); - if (port_to_sync_write_torque_[dxl->port_name_] != NULL) - port_to_sync_write_torque_[dxl->port_name_]->changeParam(dxl->id_, sync_write_data); + if (port_to_sync_write_current_[dxl->port_name_] != NULL) + port_to_sync_write_current_[dxl->port_name_]->changeParam(dxl->id_, sync_write_data); } } } @@ -1036,8 +1036,8 @@ void RobotisController::process() if (it->second != NULL) it->second->txPacket(); } - for (std::map::iterator it = port_to_sync_write_torque_.begin(); - it != port_to_sync_write_torque_.end(); it++) + for (std::map::iterator it = port_to_sync_write_current_.begin(); + it != port_to_sync_write_current_.end(); it++) { if (it->second != NULL) it->second->txPacket(); @@ -1071,7 +1071,7 @@ void RobotisController::process() joint_msg.data = dxl_state->goal_velocity_; gazebo_joint_velocity_pub_[joint_name].publish(joint_msg); } - else if ((*module_it)->getControlMode() == CurrentControl) + else if ((*module_it)->getControlMode() == TorqueControl) { joint_msg.data = dxl_state->goal_torque_; gazebo_joint_effort_pub_[joint_name].publish(joint_msg); @@ -1477,8 +1477,8 @@ void RobotisController::setCtrlModuleThread(std::string ctrl_module) if (port_to_sync_write_position_[dxl->port_name_] != NULL) port_to_sync_write_position_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); - if (port_to_sync_write_torque_[dxl->port_name_] != NULL) - port_to_sync_write_torque_[dxl->port_name_]->removeParam(dxl->id_); + if (port_to_sync_write_current_[dxl->port_name_] != NULL) + port_to_sync_write_current_[dxl->port_name_]->removeParam(dxl->id_); if (port_to_sync_write_velocity_[dxl->port_name_] != NULL) port_to_sync_write_velocity_[dxl->port_name_]->removeParam(dxl->id_); } @@ -1516,8 +1516,8 @@ void RobotisController::setCtrlModuleThread(std::string ctrl_module) if (port_to_sync_write_position_[dxl->port_name_] != NULL) port_to_sync_write_position_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); - if (port_to_sync_write_torque_[dxl->port_name_] != NULL) - port_to_sync_write_torque_[dxl->port_name_]->removeParam(dxl->id_); + if (port_to_sync_write_current_[dxl->port_name_] != NULL) + port_to_sync_write_current_[dxl->port_name_]->removeParam(dxl->id_); if (port_to_sync_write_velocity_[dxl->port_name_] != NULL) port_to_sync_write_velocity_[dxl->port_name_]->removeParam(dxl->id_); } @@ -1533,12 +1533,12 @@ void RobotisController::setCtrlModuleThread(std::string ctrl_module) if (port_to_sync_write_velocity_[dxl->port_name_] != NULL) port_to_sync_write_velocity_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); - if (port_to_sync_write_torque_[dxl->port_name_] != NULL) - port_to_sync_write_torque_[dxl->port_name_]->removeParam(dxl->id_); + if (port_to_sync_write_current_[dxl->port_name_] != NULL) + port_to_sync_write_current_[dxl->port_name_]->removeParam(dxl->id_); if (port_to_sync_write_position_[dxl->port_name_] != NULL) port_to_sync_write_position_[dxl->port_name_]->removeParam(dxl->id_); } - else if (mode == CurrentControl) + else if (mode == TorqueControl) { uint32_t curr_data = dxl->convertTorque2Value(dxl->dxl_state_->goal_torque_); uint8_t sync_write_data[4] = { 0 }; @@ -1547,8 +1547,8 @@ void RobotisController::setCtrlModuleThread(std::string ctrl_module) sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(curr_data)); sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(curr_data)); - if (port_to_sync_write_torque_[dxl->port_name_] != NULL) - port_to_sync_write_torque_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); + if (port_to_sync_write_current_[dxl->port_name_] != NULL) + port_to_sync_write_current_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); if (port_to_sync_write_velocity_[dxl->port_name_] != NULL) port_to_sync_write_velocity_[dxl->port_name_]->removeParam(dxl->id_); diff --git a/robotis_framework_common/include/robotis_framework_common/motion_module.h b/robotis_framework_common/include/robotis_framework_common/motion_module.h index c602f34..e85d938 100644 --- a/robotis_framework_common/include/robotis_framework_common/motion_module.h +++ b/robotis_framework_common/include/robotis_framework_common/motion_module.h @@ -53,7 +53,7 @@ enum ControlMode { PositionControl, VelocityControl, - CurrentControl + TorqueControl }; class MotionModule From e8792df69730ff53c02fa4afb638c37de20c9912 Mon Sep 17 00:00:00 2001 From: ROBOTIS-zerom Date: Mon, 25 Jul 2016 11:35:12 +0900 Subject: [PATCH 044/238] added XM-430-W210 / XM-430-W350 device file. --- .../robotis_controller/robotis_controller.cpp | 10 +-- .../devices/dynamixel/XM-430-W210.device | 78 +++++++++++++++++++ .../devices/dynamixel/XM-430-W350.device | 78 +++++++++++++++++++ .../devices/dynamixel/XM-430.device | 2 +- 4 files changed, 162 insertions(+), 6 deletions(-) create mode 100644 robotis_device/devices/dynamixel/XM-430-W210.device create mode 100644 robotis_device/devices/dynamixel/XM-430-W350.device diff --git a/robotis_controller/src/robotis_controller/robotis_controller.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp index dfa8ffe..292b2ff 100644 --- a/robotis_controller/src/robotis_controller/robotis_controller.cpp +++ b/robotis_controller/src/robotis_controller/robotis_controller.cpp @@ -870,7 +870,7 @@ void RobotisController::process() if (DEBUG_PRINT) { time_duration = ros::Time::now() - start_time; - fprintf(stderr, "(%2.6f) BulkRead Rx & update state", time_duration.nsec * 0.000001); + fprintf(stderr, "(%2.6f) BulkRead Rx & update state \n", time_duration.nsec * 0.000001); } // Call SensorModule Process() @@ -889,7 +889,7 @@ void RobotisController::process() if (DEBUG_PRINT) { time_duration = ros::Time::now() - start_time; - fprintf(stderr, "(%2.6f) SensorModule Process() & save result", time_duration.nsec * 0.000001); + fprintf(stderr, "(%2.6f) SensorModule Process() & save result \n", time_duration.nsec * 0.000001); } if (controller_mode_ == MotionModuleMode) @@ -1009,7 +1009,7 @@ void RobotisController::process() if (DEBUG_PRINT) { time_duration = ros::Time::now() - start_time; - fprintf(stderr, "(%2.6f) MotionModule Process() & save result", time_duration.nsec * 0.000001); + fprintf(stderr, "(%2.6f) MotionModule Process() & save result \n", time_duration.nsec * 0.000001); } // SyncWrite @@ -1117,7 +1117,7 @@ void RobotisController::process() if (DEBUG_PRINT) { time_duration = ros::Time::now() - start_time; - fprintf(stderr, "(%2.6f) SyncWrite & BulkRead Tx", time_duration.nsec * 0.000001); + fprintf(stderr, "(%2.6f) SyncWrite & BulkRead Tx \n", time_duration.nsec * 0.000001); } // publish present & goal position @@ -1144,7 +1144,7 @@ void RobotisController::process() if (DEBUG_PRINT) { time_duration = ros::Time::now() - start_time; - fprintf(stderr, "(%2.6f) Process() DONE", time_duration.nsec * 0.000001); + fprintf(stderr, "(%2.6f) Process() DONE \n", time_duration.nsec * 0.000001); } is_process_running = false; diff --git a/robotis_device/devices/dynamixel/XM-430-W210.device b/robotis_device/devices/dynamixel/XM-430-W210.device new file mode 100644 index 0000000..178fc4f --- /dev/null +++ b/robotis_device/devices/dynamixel/XM-430-W210.device @@ -0,0 +1,78 @@ +[device info] +model_name = XM-430-W210 +device_type = dynamixel + +[type info] +torque_to_current_value_ratio = 235.53647082 +velocity_to_value_ratio = 41.707853 + +value_of_0_radian_position = 2048 +value_of_min_radian_position = 0 +value_of_max_radian_position = 4095 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_current +position_d_gain_item_name = position_d_gain +position_i_gain_item_name = position_i_gain +position_p_gain_item_name = position_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 7 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 16 | N + 20 | homing_offset | 4 | RW | EEPROM | -1044479 | 1044479 | Y + 24 | moving_threshold | 4 | RW | EEPROM | 0 | 1023 | N + 31 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 32 | max_voltage_limit | 2 | RW | EEPROM | 95 | 160 | N + 34 | min_voltage_limit | 2 | RW | EEPROM | 95 | 160 | N + 36 | pwm_limit | 2 | RW | EEPROM | 0 | 885 | N + 38 | current_limit | 2 | RW | EEPROM | 0 | 1193 | N + 40 | acceleration_limit | 4 | RW | EEPROM | 0 | 32767 | N + 44 | velocity_limit | 4 | RW | EEPROM | 0 | 1023 | N + 48 | max_position_limit | 4 | RW | EEPROM | 0 | 4095 | N + 52 | min_position_limit | 4 | RW | EEPROM | 0 | 4095 | N + 63 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 64 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 65 | LED | 1 | RW | RAM | 0 | 1 | N + 68 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 69 | registered_instruction | 1 | R | RAM | 0 | 1 | N + 70 | hardware_error_status | 1 | R | RAM | 0 | 255 | N + 76 | velocity_i_gain | 2 | RW | RAM | 0 | 32767 | N + 78 | velocity_p_gain | 2 | RW | RAM | 0 | 32767 | N + 80 | position_d_gain | 2 | RW | RAM | 0 | 32767 | N + 82 | position_i_gain | 2 | RW | RAM | 0 | 32767 | N + 84 | position_p_gain | 2 | RW | RAM | 0 | 32767 | N + 88 | feedforward_2nd_gain | 2 | RW | RAM | 0 | 32767 | N + 90 | feedforward_1st_gain | 2 | RW | RAM | 0 | 32767 | N + 100 | goal_pwm | 2 | RW | RAM | 0 | 885 | N + 102 | goal_current | 2 | RW | RAM | 0 | 1193 | N + 104 | goal_velocity | 4 | RW | RAM | 0 | 1023 | N + 108 | profile_acceleration | 4 | RW | RAM | 0 | 32767 | N + 112 | profile_velocity | 4 | RW | RAM | 0 | 1023 | N + 116 | goal_position | 4 | RW | RAM | -1048575 | 1048575 | Y + 120 | realtime_tick | 2 | R | RAM | 0 | 32767 | N + 122 | moving | 1 | R | RAM | 0 | 1 | N + 123 | moving_status | 1 | R | RAM | 0 | 255 | N + 124 | present_pwm | 2 | R | RAM | 0 | 885 | N + 126 | present_current | 2 | R | RAM | 0 | 1193 | N + 128 | present_velocity | 4 | R | RAM | 0 | 1023 | N + 132 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 136 | velocity_trajectory | 4 | R | RAM | 0 | 1023 | N + 140 | position_trajectory | 4 | R | RAM | 0 | 4095 | N + 144 | present_input_voltage | 2 | R | RAM | 95 | 160 | N + 146 | present_temperature | 1 | R | RAM | 0 | 100 | N + 168 | indirect_address_1 | 2 | RW | RAM | 0 | 65535 | N + 224 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 578 | indirect_address_29 | 2 | RW | RAM | 0 | 65535 | N + 634 | indirect_data_29 | 1 | RW | RAM | 0 | 255 | N diff --git a/robotis_device/devices/dynamixel/XM-430-W350.device b/robotis_device/devices/dynamixel/XM-430-W350.device new file mode 100644 index 0000000..ed6c17f --- /dev/null +++ b/robotis_device/devices/dynamixel/XM-430-W350.device @@ -0,0 +1,78 @@ +[device info] +model_name = XM-430-W350 +device_type = dynamixel + +[type info] +torque_to_current_value_ratio = 149.795386991 +velocity_to_value_ratio = 41.707853 + +value_of_0_radian_position = 2048 +value_of_min_radian_position = 0 +value_of_max_radian_position = 4095 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_current +position_d_gain_item_name = position_d_gain +position_i_gain_item_name = position_i_gain +position_p_gain_item_name = position_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 7 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 16 | N + 20 | homing_offset | 4 | RW | EEPROM | -1044479 | 1044479 | Y + 24 | moving_threshold | 4 | RW | EEPROM | 0 | 1023 | N + 31 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 32 | max_voltage_limit | 2 | RW | EEPROM | 95 | 160 | N + 34 | min_voltage_limit | 2 | RW | EEPROM | 95 | 160 | N + 36 | pwm_limit | 2 | RW | EEPROM | 0 | 885 | N + 38 | current_limit | 2 | RW | EEPROM | 0 | 1193 | N + 40 | acceleration_limit | 4 | RW | EEPROM | 0 | 32767 | N + 44 | velocity_limit | 4 | RW | EEPROM | 0 | 1023 | N + 48 | max_position_limit | 4 | RW | EEPROM | 0 | 4095 | N + 52 | min_position_limit | 4 | RW | EEPROM | 0 | 4095 | N + 63 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 64 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 65 | LED | 1 | RW | RAM | 0 | 1 | N + 68 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 69 | registered_instruction | 1 | R | RAM | 0 | 1 | N + 70 | hardware_error_status | 1 | R | RAM | 0 | 255 | N + 76 | velocity_i_gain | 2 | RW | RAM | 0 | 32767 | N + 78 | velocity_p_gain | 2 | RW | RAM | 0 | 32767 | N + 80 | position_d_gain | 2 | RW | RAM | 0 | 32767 | N + 82 | position_i_gain | 2 | RW | RAM | 0 | 32767 | N + 84 | position_p_gain | 2 | RW | RAM | 0 | 32767 | N + 88 | feedforward_2nd_gain | 2 | RW | RAM | 0 | 32767 | N + 90 | feedforward_1st_gain | 2 | RW | RAM | 0 | 32767 | N + 100 | goal_pwm | 2 | RW | RAM | 0 | 885 | N + 102 | goal_current | 2 | RW | RAM | 0 | 1193 | N + 104 | goal_velocity | 4 | RW | RAM | 0 | 1023 | N + 108 | profile_acceleration | 4 | RW | RAM | 0 | 32767 | N + 112 | profile_velocity | 4 | RW | RAM | 0 | 1023 | N + 116 | goal_position | 4 | RW | RAM | -1048575 | 1048575 | Y + 120 | realtime_tick | 2 | R | RAM | 0 | 32767 | N + 122 | moving | 1 | R | RAM | 0 | 1 | N + 123 | moving_status | 1 | R | RAM | 0 | 255 | N + 124 | present_pwm | 2 | R | RAM | 0 | 885 | N + 126 | present_current | 2 | R | RAM | 0 | 1193 | N + 128 | present_velocity | 4 | R | RAM | 0 | 1023 | N + 132 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 136 | velocity_trajectory | 4 | R | RAM | 0 | 1023 | N + 140 | position_trajectory | 4 | R | RAM | 0 | 4095 | N + 144 | present_input_voltage | 2 | R | RAM | 95 | 160 | N + 146 | present_temperature | 1 | R | RAM | 0 | 100 | N + 168 | indirect_address_1 | 2 | RW | RAM | 0 | 65535 | N + 224 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 578 | indirect_address_29 | 2 | RW | RAM | 0 | 65535 | N + 634 | indirect_data_29 | 1 | RW | RAM | 0 | 255 | N diff --git a/robotis_device/devices/dynamixel/XM-430.device b/robotis_device/devices/dynamixel/XM-430.device index 3f33b73..67d523f 100644 --- a/robotis_device/devices/dynamixel/XM-430.device +++ b/robotis_device/devices/dynamixel/XM-430.device @@ -3,7 +3,7 @@ model_name = XM-430 device_type = dynamixel [type info] -torque_to_current_value_ratio = 178.842161 +torque_to_current_value_ratio = 149.795386991 value_of_0_radian_position = 2048 value_of_min_radian_position = 0 From 2420333894face622e5ed828b6d8b42ff4fc993a Mon Sep 17 00:00:00 2001 From: ROBOTIS-zerom Date: Thu, 28 Jul 2016 17:04:50 +0900 Subject: [PATCH 045/238] - fixed robotis_device build_depend. --- robotis_device/CMakeLists.txt | 1 - robotis_device/package.xml | 1 - 2 files changed, 2 deletions(-) diff --git a/robotis_device/CMakeLists.txt b/robotis_device/CMakeLists.txt index 6c53ff0..e492450 100644 --- a/robotis_device/CMakeLists.txt +++ b/robotis_device/CMakeLists.txt @@ -5,7 +5,6 @@ find_package(catkin REQUIRED COMPONENTS roscpp rospy dynamixel_sdk - robotis_framework_common ) catkin_package( diff --git a/robotis_device/package.xml b/robotis_device/package.xml index 8e494db..6f7294a 100644 --- a/robotis_device/package.xml +++ b/robotis_device/package.xml @@ -17,7 +17,6 @@ roscpp rospy dynamixel_sdk - robotis_framework_common roscpp rospy From 6df646ae090c36ca0364fa1e8ed12c110c271139 Mon Sep 17 00:00:00 2001 From: ROBOTIS-zerom Date: Thu, 28 Jul 2016 17:07:38 +0900 Subject: [PATCH 046/238] - modify the code simple (using auto / range based for loop) --- robotis_controller/CMakeLists.txt | 2 + .../robotis_controller/robotis_controller.cpp | 304 ++++++++---------- 2 files changed, 140 insertions(+), 166 deletions(-) diff --git a/robotis_controller/CMakeLists.txt b/robotis_controller/CMakeLists.txt index 42486cc..97c4a6d 100644 --- a/robotis_controller/CMakeLists.txt +++ b/robotis_controller/CMakeLists.txt @@ -1,6 +1,8 @@ cmake_minimum_required(VERSION 2.8.3) project(robotis_controller) +set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}") + find_package(catkin REQUIRED COMPONENTS roscpp roslib diff --git a/robotis_controller/src/robotis_controller/robotis_controller.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp index 292b2ff..c568d82 100644 --- a/robotis_controller/src/robotis_controller/robotis_controller.cpp +++ b/robotis_controller/src/robotis_controller/robotis_controller.cpp @@ -62,14 +62,9 @@ void RobotisController::initializeSyncWrite() return; ROS_INFO("FIRST BULKREAD"); - // bulkread twice - for (std::map::iterator it = port_to_bulk_read_.begin(); - it != port_to_bulk_read_.end(); it++) - { - it->second->txRxPacket(); - } - for (std::map::iterator it = port_to_bulk_read_.begin(); - it != port_to_bulk_read_.end(); it++) + for (auto& it : port_to_bulk_read_) + it.second->txRxPacket(); + for(auto& it : port_to_bulk_read_) { int error_count = 0; int result = COMM_SUCCESS; @@ -81,43 +76,39 @@ void RobotisController::initializeSyncWrite() exit(-1); } usleep(10 * 1000); - result = it->second->txRxPacket(); + result = it.second->txRxPacket(); } while (result != COMM_SUCCESS); } init_pose_loaded_ = true; ROS_INFO("FIRST BULKREAD END"); // clear syncwrite param setting - for (std::map::iterator it = port_to_sync_write_position_.begin(); - it != port_to_sync_write_position_.end(); it++) + for (auto& it : port_to_sync_write_position_) { - if (it->second != NULL) - it->second->clearParam(); + if (it.second != NULL) + it.second->clearParam(); } - for (std::map::iterator it = port_to_sync_write_position_p_gain_.begin(); - it != port_to_sync_write_position_p_gain_.end(); it++) + for (auto& it : port_to_sync_write_position_p_gain_) { - if (it->second != NULL) - it->second->clearParam(); + if (it.second != NULL) + it.second->clearParam(); } - for (std::map::iterator it = port_to_sync_write_velocity_.begin(); - it != port_to_sync_write_velocity_.end(); it++) + for (auto& it : port_to_sync_write_velocity_) { - if (it->second != NULL) - it->second->clearParam(); + if (it.second != NULL) + it.second->clearParam(); } - for (std::map::iterator it = port_to_sync_write_current_.begin(); - it != port_to_sync_write_current_.end(); it++) + for (auto& it : port_to_sync_write_current_) { - if (it->second != NULL) - it->second->clearParam(); + if (it.second != NULL) + it.second->clearParam(); } // set init syncwrite param(from data of bulkread) - for (std::map::iterator it = robot_->dxls_.begin(); it != robot_->dxls_.end(); it++) + for (auto& it : robot_->dxls_) { - std::string joint_name = it->first; - Dynamixel *dxl = it->second; + std::string joint_name = it.first; + Dynamixel *dxl = it.second; for (int i = 0; i < dxl->bulk_read_items_.size(); i++) { @@ -180,11 +171,10 @@ bool RobotisController::initialize(const std::string robot_file_path, const std: return true; } - for (std::map::iterator it = robot_->ports_.begin(); - it != robot_->ports_.end(); it++) + for (auto& it : robot_->ports_) { - std::string port_name = it->first; - dynamixel::PortHandler *port = it->second; + std::string port_name = it.first; + dynamixel::PortHandler *port = it.second; dynamixel::PacketHandler *default_pkt_handler = dynamixel::PacketHandler::getPacketHandler(2.0); if (port->setBaudRate(port->getBaudRate()) == false) @@ -195,8 +185,8 @@ bool RobotisController::initialize(const std::string robot_file_path, const std: // get the default device info of the port std::string default_device_name = robot_->port_default_device_[port_name]; - std::map::iterator dxl_it = robot_->dxls_.find(default_device_name); - std::map::iterator sensor_it = robot_->sensors_.find(default_device_name); + auto dxl_it = robot_->dxls_.find(default_device_name); + auto sensor_it = robot_->sensors_.find(default_device_name); if (dxl_it != robot_->dxls_.end()) { Dynamixel *default_device = dxl_it->second; @@ -248,10 +238,10 @@ bool RobotisController::initialize(const std::string robot_file_path, const std: } // (for loop) check all dxls are connected. - for (std::map::iterator it = robot_->dxls_.begin(); it != robot_->dxls_.end(); it++) + for (auto& it : robot_->dxls_) { - std::string joint_name = it->first; - Dynamixel *dxl = it->second; + std::string joint_name = it.first; + Dynamixel *dxl = it.second; if (ping(joint_name) != 0) { @@ -264,10 +254,10 @@ bool RobotisController::initialize(const std::string robot_file_path, const std: initializeDevice(init_file_path); // [ BulkRead ] StartAddress : Present Position , Length : 10 ( Position/Velocity/Current ) - for (std::map::iterator it = robot_->dxls_.begin(); it != robot_->dxls_.end(); it++) + for (auto& it : robot_->dxls_) { - std::string joint_name = it->first; - Dynamixel *dxl = it->second; + std::string joint_name = it.first; + Dynamixel *dxl = it.second; if (dxl == NULL) continue; @@ -283,7 +273,7 @@ bool RobotisController::initialize(const std::string robot_file_path, const std: // } // calculate bulk read start address & data length - std::map::iterator indirect_addr_it = dxl->ctrl_table_.find(INDIRECT_ADDRESS_1); + auto indirect_addr_it = dxl->ctrl_table_.find(INDIRECT_ADDRESS_1); if (indirect_addr_it != dxl->ctrl_table_.end()) // INDIRECT_ADDRESS_1 exist { if (dxl->bulk_read_items_.size() != 0) @@ -338,10 +328,10 @@ bool RobotisController::initialize(const std::string robot_file_path, const std: writeCtrlItem(joint_name, dxl->torque_enable_item_->item_name_, 1); } - for (std::map::iterator it = robot_->sensors_.begin(); it != robot_->sensors_.end(); it++) + for (auto& it : robot_->sensors_) { - std::string sensor_name = it->first; - Sensor *sensor = it->second; + std::string sensor_name = it.first; + Sensor *sensor = it.second; if (sensor == NULL) continue; @@ -350,7 +340,7 @@ bool RobotisController::initialize(const std::string robot_file_path, const std: int bulkread_data_length = 0; // calculate bulk read start address & data length - std::map::iterator indirect_addr_it = sensor->ctrl_table_.find(INDIRECT_ADDRESS_1); + auto indirect_addr_it = sensor->ctrl_table_.find(INDIRECT_ADDRESS_1); if (indirect_addr_it != sensor->ctrl_table_.end()) // INDIRECT_ADDRESS_1 exist { if (sensor->bulk_read_items_.size() != 0) @@ -427,7 +417,7 @@ void RobotisController::initializeDevice(const std::string init_file_path) continue; Dynamixel *dxl = NULL; - std::map::iterator dxl_it = robot_->dxls_.find(joint_name); + auto dxl_it = robot_->dxls_.find(joint_name); if (dxl_it != robot_->dxls_.end()) dxl = dxl_it->second; @@ -555,14 +545,14 @@ void RobotisController::msgQueueThread() if (gazebo_mode_ == true) { - for (std::map::iterator it = robot_->dxls_.begin(); it != robot_->dxls_.end(); it++) + for (auto& it : robot_->dxls_) { - gazebo_joint_position_pub_[it->first] = ros_node.advertise( - "/" + gazebo_robot_name_ + "/" + it->first + "_position/command", 1); - gazebo_joint_velocity_pub_[it->first] = ros_node.advertise( - "/" + gazebo_robot_name_ + "/" + it->first + "_velocity/command", 1); - gazebo_joint_effort_pub_[it->first] = ros_node.advertise( - "/" + gazebo_robot_name_ + "/" + it->first + "_effort/command", 1); + gazebo_joint_position_pub_[it.first] = ros_node.advertise( + "/" + gazebo_robot_name_ + "/" + it.first + "_position/command", 1); + gazebo_joint_velocity_pub_[it.first] = ros_node.advertise( + "/" + gazebo_robot_name_ + "/" + it.first + "_velocity/command", 1); + gazebo_joint_effort_pub_[it.first] = ros_node.advertise( + "/" + gazebo_robot_name_ + "/" + it.first + "_effort/command", 1); } } @@ -629,10 +619,9 @@ void RobotisController::startTimer() { initializeSyncWrite(); - for (std::map::iterator it = port_to_bulk_read_.begin(); - it != port_to_bulk_read_.end(); it++) + for (auto& it : port_to_bulk_read_) { - it->second->txPacket(); + it.second->txPacket(); } usleep(8 * 1000); @@ -682,36 +671,31 @@ void RobotisController::stopTimer() if ((error = pthread_join(this->timer_thread_, NULL)) != 0) exit(-1); - for (std::map::iterator it = port_to_bulk_read_.begin(); - it != port_to_bulk_read_.end(); it++) + for (auto& it : port_to_bulk_read_) { - if (it->second != NULL) - it->second->rxPacket(); + if (it.second != NULL) + it.second->rxPacket(); } - for (std::map::iterator it = port_to_sync_write_position_.begin(); - it != port_to_sync_write_position_.end(); it++) + for (auto& it : port_to_sync_write_position_) { - if (it->second != NULL) - it->second->clearParam(); + if (it.second != NULL) + it.second->clearParam(); } - for (std::map::iterator it = port_to_sync_write_position_p_gain_.begin(); - it != port_to_sync_write_position_p_gain_.end(); it++) + for (auto& it : port_to_sync_write_position_p_gain_) { - if (it->second != NULL) - it->second->clearParam(); + if (it.second != NULL) + it.second->clearParam(); } - for (std::map::iterator it = port_to_sync_write_velocity_.begin(); - it != port_to_sync_write_velocity_.end(); it++) + for (auto& it : port_to_sync_write_velocity_) { - if (it->second != NULL) - it->second->clearParam(); + if (it.second != NULL) + it.second->clearParam(); } - for (std::map::iterator it = port_to_sync_write_current_.begin(); - it != port_to_sync_write_current_.end(); it++) + for (auto& it : port_to_sync_write_current_) { - if (it->second != NULL) - it->second->clearParam(); + if (it.second != NULL) + it.second->clearParam(); } } else @@ -752,7 +736,7 @@ void RobotisController::loadOffset(const std::string path) std::string joint_name = it->first.as(); double offset = it->second.as(); - std::map::iterator dxl_it = robot_->dxls_.find(joint_name); + auto dxl_it = robot_->dxls_.find(joint_name); if (dxl_it != robot_->dxls_.end()) dxl_it->second->dxl_state_->position_offset_ = offset; } @@ -786,21 +770,19 @@ void RobotisController::process() if (gazebo_mode_ == false) { // BulkRead Rx - for (std::map::iterator it = port_to_bulk_read_.begin(); - it != port_to_bulk_read_.end(); it++) + for (auto& it : port_to_bulk_read_) { - robot_->ports_[it->first]->setPacketTimeout(0.0); - it->second->rxPacket(); + robot_->ports_[it.first]->setPacketTimeout(0.0); + it.second->rxPacket(); } if (robot_->dxls_.size() > 0) { - for (std::map::iterator dxl_it = robot_->dxls_.begin(); - dxl_it != robot_->dxls_.end(); dxl_it++) + for (auto& dxl_it : robot_->dxls_) { - Dynamixel *dxl = dxl_it->second; - std::string port_name = dxl_it->second->port_name_; - std::string joint_name = dxl_it->first; + Dynamixel *dxl = dxl_it.second; + std::string port_name = dxl_it.second->port_name_; + std::string joint_name = dxl_it.first; if (dxl->bulk_read_items_.size() > 0) { @@ -838,12 +820,11 @@ void RobotisController::process() if (robot_->sensors_.size() > 0) { - for (std::map::iterator sensor_it = robot_->sensors_.begin(); - sensor_it != robot_->sensors_.end(); sensor_it++) + for (auto& sensor_it : robot_->sensors_) { - Sensor *sensor = sensor_it->second; - std::string port_name = sensor_it->second->port_name_; - std::string sensor_name = sensor_it->first; + Sensor *sensor = sensor_it.second; + std::string port_name = sensor_it.second->port_name_; + std::string sensor_name = sensor_it.first; if (sensor->bulk_read_items_.size() > 0) { @@ -877,12 +858,12 @@ void RobotisController::process() // -> for loop : call SensorModule list -> Process() if (sensor_modules_.size() > 0) { - for (std::list::iterator module_it = sensor_modules_.begin(); module_it != sensor_modules_.end(); module_it++) + for (auto module_it = sensor_modules_.begin(); module_it != sensor_modules_.end(); module_it++) { (*module_it)->process(robot_->dxls_, robot_->sensors_); - for (std::map::iterator it = (*module_it)->result_.begin(); it != (*module_it)->result_.end(); it++) - sensor_result_[it->first] = it->second; + for (auto& it : (*module_it)->result_) + sensor_result_[it.first] = it.second; } } @@ -900,7 +881,7 @@ void RobotisController::process() { queue_mutex_.lock(); - for (std::list::iterator module_it = motion_modules_.begin(); module_it != motion_modules_.end(); module_it++) + for (auto module_it = motion_modules_.begin(); module_it != motion_modules_.end(); module_it++) { if ((*module_it)->getModuleEnable() == false) continue; @@ -908,11 +889,11 @@ void RobotisController::process() (*module_it)->process(robot_->dxls_, sensor_result_); // for loop : joint list - for (std::map::iterator dxl_it = robot_->dxls_.begin(); dxl_it != robot_->dxls_.end(); dxl_it++) + for (auto& dxl_it : robot_->dxls_) { - std::string joint_name = dxl_it->first; - Dynamixel *dxl = dxl_it->second; - DynamixelState *dxl_state = dxl_it->second->dxl_state_; + std::string joint_name = dxl_it.first; + Dynamixel *dxl = dxl_it.second; + DynamixelState *dxl_state = dxl_it.second->dxl_state_; if (dxl->ctrl_module_name_ == (*module_it)->getModuleName()) { @@ -1017,47 +998,42 @@ void RobotisController::process() { if (port_to_sync_write_position_p_gain_.size() > 0) { - for (std::map::iterator it = port_to_sync_write_position_p_gain_.begin(); - it != port_to_sync_write_position_p_gain_.end(); it++) + for (auto& it : port_to_sync_write_position_p_gain_) { - it->second->txPacket(); - it->second->clearParam(); + it.second->txPacket(); + it.second->clearParam(); } } - for (std::map::iterator it = port_to_sync_write_position_.begin(); - it != port_to_sync_write_position_.end(); it++) + for (auto& it : port_to_sync_write_position_) { - if (it->second != NULL) - it->second->txPacket(); + if (it.second != NULL) + it.second->txPacket(); } - for (std::map::iterator it = port_to_sync_write_velocity_.begin(); - it != port_to_sync_write_velocity_.end(); it++) + for (auto& it : port_to_sync_write_velocity_) { - if (it->second != NULL) - it->second->txPacket(); + if (it.second != NULL) + it.second->txPacket(); } - for (std::map::iterator it = port_to_sync_write_current_.begin(); - it != port_to_sync_write_current_.end(); it++) + for (auto& it : port_to_sync_write_current_) { - if (it->second != NULL) - it->second->txPacket(); + if (it.second != NULL) + it.second->txPacket(); } } else if (gazebo_mode_ == true) { - for (std::list::iterator module_it = motion_modules_.begin(); module_it != motion_modules_.end(); module_it++) + for (auto module_it = motion_modules_.begin(); module_it != motion_modules_.end(); module_it++) { if ((*module_it)->getModuleEnable() == false) continue; std_msgs::Float64 joint_msg; - for (std::map::iterator dxl_it = robot_->dxls_.begin(); dxl_it != robot_->dxls_.end(); - dxl_it++) + for (auto& dxl_it : robot_->dxls_) { - std::string joint_name = dxl_it->first; - Dynamixel *dxl = dxl_it->second; - DynamixelState *dxl_state = dxl_it->second->dxl_state_; + std::string joint_name = dxl_it.first; + Dynamixel *dxl = dxl_it.second; + DynamixelState *dxl_state = dxl_it.second->dxl_state_; if (dxl->ctrl_module_name_ == (*module_it)->getModuleName()) { @@ -1085,11 +1061,10 @@ void RobotisController::process() { queue_mutex_.lock(); - for (std::map::iterator it = port_to_sync_write_position_.begin(); - it != port_to_sync_write_position_.end(); it++) + for (auto& it : port_to_sync_write_position_) { - it->second->txPacket(); - it->second->clearParam(); + it.second->txPacket(); + it.second->clearParam(); } if (direct_sync_write_.size() > 0) @@ -1110,8 +1085,8 @@ void RobotisController::process() // BulkRead Tx if (gazebo_mode_ == false) { - for (std::map::iterator it = port_to_bulk_read_.begin(); it != port_to_bulk_read_.end(); it++) - it->second->txPacket(); + for (auto& it : port_to_bulk_read_) + it.second->txPacket(); } if (DEBUG_PRINT) @@ -1121,10 +1096,10 @@ void RobotisController::process() } // publish present & goal position - for (std::map::iterator dxl_it = robot_->dxls_.begin(); dxl_it != robot_->dxls_.end(); dxl_it++) + for (auto& dxl_it : robot_->dxls_) { - std::string joint_name = dxl_it->first; - Dynamixel *dxl = dxl_it->second; + std::string joint_name = dxl_it.first; + Dynamixel *dxl = dxl_it.second; present_state.name.push_back(joint_name); present_state.position.push_back(dxl->dxl_state_->present_position_); @@ -1153,7 +1128,7 @@ void RobotisController::process() void RobotisController::addMotionModule(MotionModule *module) { // check whether the module name already exists - for (std::list::iterator m_it = motion_modules_.begin(); m_it != motion_modules_.end(); m_it++) + for (auto m_it = motion_modules_.begin(); m_it != motion_modules_.end(); m_it++) { if ((*m_it)->getModuleName() == module->getModuleName()) { @@ -1175,7 +1150,7 @@ void RobotisController::removeMotionModule(MotionModule *module) void RobotisController::addSensorModule(SensorModule *module) { // check whether the module name already exists - for (std::list::iterator m_it = sensor_modules_.begin(); m_it != sensor_modules_.end(); m_it++) + for (auto m_it = sensor_modules_.begin(); m_it != sensor_modules_.end(); m_it++) { if ((*m_it)->getModuleName() == module->getModuleName()) { @@ -1301,7 +1276,7 @@ void RobotisController::setJointCtrlModuleCallback(const robotis_controller_msgs for (unsigned int idx = 0; idx < msg->joint_name.size(); idx++) { Dynamixel *dxl = NULL; - std::map::iterator dxl_it = robot_->dxls_.find((std::string) (msg->joint_name[idx])); + auto dxl_it = robot_->dxls_.find((std::string) (msg->joint_name[idx])); if (dxl_it != robot_->dxls_.end()) dxl = dxl_it->second; else @@ -1315,7 +1290,7 @@ void RobotisController::setJointCtrlModuleCallback(const robotis_controller_msgs } // check whether the module is using this joint - for (std::list::iterator m_it = motion_modules_.begin(); m_it != motion_modules_.end(); m_it++) + for (auto m_it = motion_modules_.begin(); m_it != motion_modules_.end(); m_it++) { if ((*m_it)->getModuleName() == msg->module_name[idx]) { @@ -1328,15 +1303,15 @@ void RobotisController::setJointCtrlModuleCallback(const robotis_controller_msgs } } - for (std::list::iterator m_it = motion_modules_.begin(); m_it != motion_modules_.end(); m_it++) + for (auto m_it = motion_modules_.begin(); m_it != motion_modules_.end(); m_it++) { // set all modules -> disable (*m_it)->setModuleEnable(false); // set all used modules -> enable - for (std::map::iterator d_it = robot_->dxls_.begin(); d_it != robot_->dxls_.end(); d_it++) + for (auto& d_it : robot_->dxls_) { - if (d_it->second->ctrl_module_name_ == (*m_it)->getModuleName()) + if (d_it.second->ctrl_module_name_ == (*m_it)->getModuleName()) { (*m_it)->setModuleEnable(true); break; @@ -1350,10 +1325,10 @@ void RobotisController::setJointCtrlModuleCallback(const robotis_controller_msgs queue_mutex_.unlock(); robotis_controller_msgs::JointCtrlModule current_module_msg; - for (std::map::iterator dxl_iter = robot_->dxls_.begin(); dxl_iter != robot_->dxls_.end(); ++dxl_iter) + for (auto& dxl_iter : robot_->dxls_) { - current_module_msg.joint_name.push_back(dxl_iter->first); - current_module_msg.module_name.push_back(dxl_iter->second->ctrl_module_name_); + current_module_msg.joint_name.push_back(dxl_iter.first); + current_module_msg.module_name.push_back(dxl_iter.second->ctrl_module_name_); } if (current_module_msg.joint_name.size() == current_module_msg.module_name.size()) @@ -1365,7 +1340,7 @@ bool RobotisController::getCtrlModuleCallback(robotis_controller_msgs::GetJointM { for (unsigned int idx = 0; idx < req.joint_name.size(); idx++) { - std::map::iterator d_it = robot_->dxls_.find((std::string) (req.joint_name[idx])); + auto d_it = robot_->dxls_.find((std::string) (req.joint_name[idx])); if (d_it != robot_->dxls_.end()) { res.joint_name.push_back(req.joint_name[idx]); @@ -1387,7 +1362,7 @@ void RobotisController::setCtrlModuleThread(std::string ctrl_module) if (ctrl_module == "" || ctrl_module == "none") { // enqueue all modules in order to stop - for (std::list::iterator m_it = motion_modules_.begin(); m_it != motion_modules_.end(); m_it++) + for (auto m_it = motion_modules_.begin(); m_it != motion_modules_.end(); m_it++) { if ((*m_it)->getModuleEnable() == true) stop_modules.push_back(*m_it); @@ -1395,24 +1370,22 @@ void RobotisController::setCtrlModuleThread(std::string ctrl_module) } else { - for (std::list::iterator m_it = motion_modules_.begin(); m_it != motion_modules_.end(); m_it++) + for (auto m_it = motion_modules_.begin(); m_it != motion_modules_.end(); m_it++) { // if it exist if ((*m_it)->getModuleName() == ctrl_module) { // enqueue the module which lost control of joint in order to stop - for (std::map::iterator result_it = (*m_it)->result_.begin(); - result_it != (*m_it)->result_.end(); result_it++) + for (auto& result_it : (*m_it)->result_) { - std::map::iterator d_it = robot_->dxls_.find(result_it->first); + auto d_it = robot_->dxls_.find(result_it.first); if (d_it != robot_->dxls_.end()) { // enqueue if (d_it->second->ctrl_module_name_ != ctrl_module) { - for (std::list::iterator stop_m_it = motion_modules_.begin(); - stop_m_it != motion_modules_.end(); stop_m_it++) + for (auto stop_m_it = motion_modules_.begin(); stop_m_it != motion_modules_.end(); stop_m_it++) { if (((*stop_m_it)->getModuleName() == d_it->second->ctrl_module_name_) && ((*stop_m_it)->getModuleEnable() == true)) @@ -1431,13 +1404,13 @@ void RobotisController::setCtrlModuleThread(std::string ctrl_module) // stop the module stop_modules.unique(); - for (std::list::iterator stop_m_it = stop_modules.begin(); stop_m_it != stop_modules.end(); stop_m_it++) + for (auto stop_m_it = stop_modules.begin(); stop_m_it != stop_modules.end(); stop_m_it++) { (*stop_m_it)->stop(); } // wait to stop - for (std::list::iterator stop_m_it = stop_modules.begin(); stop_m_it != stop_modules.end(); stop_m_it++) + for (auto stop_m_it = stop_modules.begin(); stop_m_it != stop_modules.end(); stop_m_it++) { while ((*stop_m_it)->isRunning()) usleep(CONTROL_CYCLE_MSEC * 1000); @@ -1453,15 +1426,15 @@ void RobotisController::setCtrlModuleThread(std::string ctrl_module) if ((ctrl_module == "") || (ctrl_module == "none")) { // set all modules -> disable - for (std::list::iterator m_it = motion_modules_.begin(); m_it != motion_modules_.end(); m_it++) + for (auto m_it = motion_modules_.begin(); m_it != motion_modules_.end(); m_it++) { (*m_it)->setModuleEnable(false); } // set dxl's control module to "none" - for (std::map::iterator d_it = robot_->dxls_.begin(); d_it != robot_->dxls_.end(); d_it++) + for (auto& d_it : robot_->dxls_) { - Dynamixel *dxl = d_it->second; + Dynamixel *dxl = d_it.second; dxl->ctrl_module_name_ = "none"; if (gazebo_mode_ == true) @@ -1486,16 +1459,15 @@ void RobotisController::setCtrlModuleThread(std::string ctrl_module) else { // check whether the module exist - for (std::list::iterator m_it = motion_modules_.begin(); m_it != motion_modules_.end(); m_it++) + for (auto m_it = motion_modules_.begin(); m_it != motion_modules_.end(); m_it++) { // if it exist if ((*m_it)->getModuleName() == ctrl_module) { ControlMode mode = (*m_it)->getControlMode(); - for (std::map::iterator result_it = (*m_it)->result_.begin(); - result_it != (*m_it)->result_.end(); result_it++) + for (auto& result_it : (*m_it)->result_) { - std::map::iterator d_it = robot_->dxls_.find(result_it->first); + auto d_it = robot_->dxls_.find(result_it.first); if (d_it != robot_->dxls_.end()) { Dynamixel *dxl = d_it->second; @@ -1563,15 +1535,15 @@ void RobotisController::setCtrlModuleThread(std::string ctrl_module) } } - for (std::list::iterator m_it = motion_modules_.begin(); m_it != motion_modules_.end(); m_it++) + for (auto m_it = motion_modules_.begin(); m_it != motion_modules_.end(); m_it++) { // set all modules -> disable (*m_it)->setModuleEnable(false); // set all used modules -> enable - for (std::map::iterator d_it = robot_->dxls_.begin(); d_it != robot_->dxls_.end(); d_it++) + for (auto& d_it : robot_->dxls_) { - if (d_it->second->ctrl_module_name_ == (*m_it)->getModuleName()) + if (d_it.second->ctrl_module_name_ == (*m_it)->getModuleName()) { (*m_it)->setModuleEnable(true); break; @@ -1586,10 +1558,10 @@ void RobotisController::setCtrlModuleThread(std::string ctrl_module) // publish current module robotis_controller_msgs::JointCtrlModule current_module_msg; - for (std::map::iterator dxl_iter = robot_->dxls_.begin(); dxl_iter != robot_->dxls_.end(); ++dxl_iter) + for (auto& dxl_iter : robot_->dxls_) { - current_module_msg.joint_name.push_back(dxl_iter->first); - current_module_msg.module_name.push_back(dxl_iter->second->ctrl_module_name_); + current_module_msg.joint_name.push_back(dxl_iter.first); + current_module_msg.module_name.push_back(dxl_iter.second->ctrl_module_name_); } if (current_module_msg.joint_name.size() == current_module_msg.module_name.size()) @@ -1602,7 +1574,7 @@ void RobotisController::gazeboJointStatesCallback(const sensor_msgs::JointState: for (unsigned int i = 0; i < msg->name.size(); i++) { - std::map::iterator d_it = robot_->dxls_.find((std::string) msg->name[i]); + auto d_it = robot_->dxls_.find((std::string) msg->name[i]); if (d_it != robot_->dxls_.end()) { d_it->second->dxl_state_->present_position_ = msg->position[i]; @@ -1613,8 +1585,8 @@ void RobotisController::gazeboJointStatesCallback(const sensor_msgs::JointState: if (init_pose_loaded_ == false) { - for (std::map::iterator it = robot_->dxls_.begin(); it != robot_->dxls_.end(); it++) - it->second->dxl_state_->goal_position_ = it->second->dxl_state_->present_position_; + for (auto& it : robot_->dxls_) + it.second->dxl_state_->goal_position_ = it.second->dxl_state_->present_position_; init_pose_loaded_ = true; } From 54d93f872c7afe1a9f53ea5584615ee0b9b29cc5 Mon Sep 17 00:00:00 2001 From: Jay-Song Date: Wed, 10 Aug 2016 10:48:42 +0900 Subject: [PATCH 047/238] Add new functions to robotis_math step data define fifth polynomial trajectory --- .../robotis_math/robotis_linear_algebra.h | 3 + .../robotis_trajectory_calculator.h | 44 +++ .../include/robotis_math/step_data_define.h | 83 +++++ robotis_math/src/robotis_linear_algebra.cpp | 14 + .../src/robotis_trajectory_calculator.cpp | 284 ++++++++++++++++++ 5 files changed, 428 insertions(+) create mode 100644 robotis_math/include/robotis_math/step_data_define.h diff --git a/robotis_math/include/robotis_math/robotis_linear_algebra.h b/robotis_math/include/robotis_math/robotis_linear_algebra.h index 9c62268..f8cd69e 100644 --- a/robotis_math/include/robotis_math/robotis_linear_algebra.h +++ b/robotis_math/include/robotis_math/robotis_linear_algebra.h @@ -44,6 +44,7 @@ #define EIGEN_NO_STATIC_ASSERT #include +#include "step_data_define.h" namespace robotis_framework { @@ -71,6 +72,8 @@ Eigen::MatrixXd convertRotToOmega(Eigen::MatrixXd rotation); Eigen::MatrixXd calcCross(Eigen::MatrixXd vector3d_a, Eigen::MatrixXd vector3d_b); double calcInner(Eigen::MatrixXd vector3d_a, Eigen::MatrixXd vector3d_b); +Pose3D getPose3DfromTransformMatrix(Eigen::MatrixXd transform); + } diff --git a/robotis_math/include/robotis_math/robotis_trajectory_calculator.h b/robotis_math/include/robotis_math/robotis_trajectory_calculator.h index 56bf644..09eaa36 100644 --- a/robotis_math/include/robotis_math/robotis_trajectory_calculator.h +++ b/robotis_math/include/robotis_math/robotis_trajectory_calculator.h @@ -60,6 +60,50 @@ Eigen::MatrixXd calcArc3dTra(double smp_time, double mov_time, Eigen::MatrixXd center_point, Eigen::MatrixXd normal_vector, Eigen::MatrixXd start_point, double rotation_angle, double cross_ratio); + +class FifthOrderPolynomialTrajectory +{ +public: + FifthOrderPolynomialTrajectory(double initial_time, double initial_pos, double initial_vel, double initial_acc, + double final_time, double final_pos, double final_vel, double final_acc); + FifthOrderPolynomialTrajectory(); + ~FifthOrderPolynomialTrajectory(); + + bool changeTrajectory(double final_pos, double final_vel, double final_acc); + bool changeTrajectory(double final_time, double final_pos, double final_vel, double final_acc); + bool changeTrajectory(double initial_time, double initial_pos, double initial_vel, double initial_acc, + double final_time, double final_pos, double final_vel, double final_acc); + + double getPosition(double time); + double getVelocity(double time); + double getAcceleration(double time); + + void setTime(double time); + double getPosition(); + double getVelocity(); + double getAcceleration(); + + double initial_time_; + double initial_pos_; + double initial_vel_; + double initial_acc_; + + double current_time_; + double current_pos_; + double current_vel_; + double current_acc_; + + double final_time_; + double final_pos_; + double final_vel_; + double final_acc_; + + Eigen::MatrixXd position_coeff_; + Eigen::MatrixXd velocity_coeff_; + Eigen::MatrixXd acceleration_coeff_; + Eigen::MatrixXd time_variables_; +}; + } #endif /* ROBOTIS_MATH_ROBOTIS_TRAJECTORY_CALCULATOR_H_ */ diff --git a/robotis_math/include/robotis_math/step_data_define.h b/robotis_math/include/robotis_math/step_data_define.h new file mode 100644 index 0000000..135d445 --- /dev/null +++ b/robotis_math/include/robotis_math/step_data_define.h @@ -0,0 +1,83 @@ +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * 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 ROBOTIS 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. + *******************************************************************************/ + +/* + * step_data_define.h + * + * Created on: 2016. 8. 10. + * Author: Jay Song + */ + +#ifndef ROBOTIS_MATH_STEP_DATA_DEFINE_H_ +#define ROBOTIS_MATH_STEP_DATA_DEFINE_H_ + +namespace robotis_framework +{ + +typedef struct +{ + double x, y, z; +} Position3D; + +typedef struct +{ + double x, y, z, roll, pitch, yaw; +} Pose3D; + +typedef struct +{ + int moving_foot; + double foot_z_swap, body_z_swap; + double shoulder_swing_gain, elbow_swing_gain; + double waist_roll_angle, waist_pitch_angle, waist_yaw_angle; + Pose3D left_foot_pose; + Pose3D right_foot_pose; + Pose3D body_pose; +} StepPositionData; + +typedef struct +{ + int walking_state; + double abs_step_time, dsp_ratio; + double start_time_delay_ratio_x, start_time_delay_ratio_y, start_time_delay_ratio_z; + double start_time_delay_ratio_roll, start_time_delay_ratio_pitch, start_time_delay_ratio_yaw; + double finish_time_advance_ratio_x, finish_time_advance_ratio_y, finish_time_advance_ratio_z; + double finish_time_advance_ratio_roll, finish_time_advance_ratio_pitch, finish_time_advance_ratio_yaw; +} StepTimeData; + +typedef struct +{ + StepPositionData position_data; + StepTimeData time_data; +} StepData; + +} + +#endif /* ROBOTIS_MATH_STEP_DATA_DEFINE_H_ */ diff --git a/robotis_math/src/robotis_linear_algebra.cpp b/robotis_math/src/robotis_linear_algebra.cpp index 7683810..293fef3 100644 --- a/robotis_math/src/robotis_linear_algebra.cpp +++ b/robotis_math/src/robotis_linear_algebra.cpp @@ -305,4 +305,18 @@ double calcInner(Eigen::MatrixXd vector3d_a, Eigen::MatrixXd vector3d_b) return vector3d_a.dot(vector3d_b); } +Pose3D getPose3DfromTransformMatrix(Eigen::MatrixXd transform) +{ + Pose3D pose_3d; + + pose_3d.x = transform.coeff(0, 3); + pose_3d.y = transform.coeff(1, 3); + pose_3d.z = transform.coeff(2, 3); + pose_3d.roll = atan2( transform.coeff(2,1), transform.coeff(2,2)); + pose_3d.pitch = atan2(-transform.coeff(2,0), sqrt(transform.coeff(2,1)*transform.coeff(2,1) + transform.coeff(2,2)*transform.coeff(2,2)) ); + pose_3d.yaw = atan2( transform.coeff(1,0), transform.coeff(0,0)); + + return pose_3d; +} + } diff --git a/robotis_math/src/robotis_trajectory_calculator.cpp b/robotis_math/src/robotis_trajectory_calculator.cpp index 5f66ffc..9c8554d 100644 --- a/robotis_math/src/robotis_trajectory_calculator.cpp +++ b/robotis_math/src/robotis_trajectory_calculator.cpp @@ -327,4 +327,288 @@ Eigen::MatrixXd calcArc3dTra(double smp_time, double mov_time, } + +FifthOrderPolynomialTrajectory::FifthOrderPolynomialTrajectory(double initial_time, double initial_pos, double initial_vel, double initial_acc, + double final_time, double final_pos, double final_vel, double final_acc) +{ + position_coeff_.resize(6, 1); + velocity_coeff_.resize(6, 1); + acceleration_coeff_.resize(6, 1); + time_variables_.resize(1, 6); + + position_coeff_.fill(0); + velocity_coeff_.fill(0); + acceleration_coeff_.fill(0); + time_variables_.fill(0); + + if(final_time > initial_time) + { + initial_time_ = initial_time; + initial_pos_ = initial_pos; + initial_vel_ = initial_vel; + initial_acc_ = initial_acc; + + current_time_ = initial_time; + current_pos_ = initial_pos; + current_vel_ = initial_vel; + current_acc_ = initial_acc; + + final_time_ = final_time; + final_pos_ = final_pos; + final_vel_ = final_vel; + final_acc_ = final_acc; + + Eigen::MatrixXd time_mat; + Eigen::MatrixXd conditions_mat; + + time_mat.resize(6,6); + time_mat << powDI(initial_time_, 5), powDI(initial_time_, 4), powDI(initial_time_, 3), powDI(initial_time_, 2), initial_time_, 1.0, + 5.0*powDI(initial_time_, 4), 4.0*powDI(initial_time_, 3), 3.0*powDI(initial_time_, 2), 2.0*initial_time_, 1.0, 0.0, + 20.0*powDI(initial_time_, 3), 12.0*powDI(initial_time_, 2), 6.0*initial_time_, 2.0, 0.0, 0.0; + powDI(final_time_, 5), powDI(final_time_, 4), powDI(final_time_, 3), powDI(final_time_, 2), final_time_, 1.0, + 5.0*powDI(final_time_, 4), 4.0*powDI(final_time_, 3), 3.0*powDI(final_time_, 2), 2.0*final_time_, 1.0, 0.0, + 20.0*powDI(final_time_, 3), 12.0*powDI(final_time_, 2), 6.0*final_time_, 2.0, 0.0, 0.0; + + conditions_mat.resize(6,1); + conditions_mat << initial_pos_, initial_vel_, initial_acc_, final_pos_, final_vel_, final_acc_; + + position_coeff_ = time_mat.inverse() * conditions_mat; + velocity_coeff_ << 0.0, + 5.0*position_coeff_.coeff(0,0), + 4.0*position_coeff_.coeff(0,1), + 3.0*position_coeff_.coeff(0,2), + 2.0*position_coeff_.coeff(0,3), + 1.0*position_coeff_.coeff(0,4); + acceleration_coeff_ << 0.0, + 0.0, + 20.0*position_coeff_.coeff(0,0), + 12.0*position_coeff_.coeff(0,1), + 6.0*position_coeff_.coeff(0,2), + 2.0*position_coeff_.coeff(0,3); + } + +} + +FifthOrderPolynomialTrajectory::FifthOrderPolynomialTrajectory() +{ + initial_time_ = 0; + initial_pos_ = 0; + initial_vel_ = 0; + initial_acc_ = 0; + + current_time_ = 0; + current_pos_ = 0; + current_vel_ = 0; + current_acc_ = 0; + + final_time_ = 0; + final_pos_ = 0; + final_vel_ = 0; + final_acc_ = 0; + + position_coeff_.resize(6, 1); + velocity_coeff_.resize(6, 1); + acceleration_coeff_.resize(6, 1); + time_variables_.resize(1, 6); + + position_coeff_.fill(0); + velocity_coeff_.fill(0); + acceleration_coeff_.fill(0); + time_variables_.fill(0); +} + +FifthOrderPolynomialTrajectory::~FifthOrderPolynomialTrajectory() +{ + +} + +bool FifthOrderPolynomialTrajectory::changeTrajectory(double final_pos, double final_vel, double final_acc) +{ + final_pos_ = final_pos; + final_vel_ = final_vel; + final_acc_ = final_acc; + + Eigen::MatrixXd time_mat; + Eigen::MatrixXd conditions_mat; + + time_mat.resize(6,6); + time_mat << powDI(initial_time_, 5), powDI(initial_time_, 4), powDI(initial_time_, 3), powDI(initial_time_, 2), initial_time_, 1.0, + 5.0*powDI(initial_time_, 4), 4.0*powDI(initial_time_, 3), 3.0*powDI(initial_time_, 2), 2.0*initial_time_, 1.0, 0.0, + 20.0*powDI(initial_time_, 3), 12.0*powDI(initial_time_, 2), 6.0*initial_time_, 2.0, 0.0, 0.0; + powDI(final_time_, 5), powDI(final_time_, 4), powDI(final_time_, 3), powDI(final_time_, 2), final_time_, 1.0, + 5.0*powDI(final_time_, 4), 4.0*powDI(final_time_, 3), 3.0*powDI(final_time_, 2), 2.0*final_time_, 1.0, 0.0, + 20.0*powDI(final_time_, 3), 12.0*powDI(final_time_, 2), 6.0*final_time_, 2.0, 0.0, 0.0; + + conditions_mat.resize(6,1); + conditions_mat << initial_pos_, initial_vel_, initial_acc_, final_pos_, final_vel_, final_acc_; + + position_coeff_ = time_mat.inverse() * conditions_mat; + velocity_coeff_ << 0.0, + 5.0*position_coeff_.coeff(0,0), + 4.0*position_coeff_.coeff(0,1), + 3.0*position_coeff_.coeff(0,2), + 2.0*position_coeff_.coeff(0,3), + 1.0*position_coeff_.coeff(0,4); + acceleration_coeff_ << 0.0, + 0.0, + 20.0*position_coeff_.coeff(0,0), + 12.0*position_coeff_.coeff(0,1), + 6.0*position_coeff_.coeff(0,2), + 2.0*position_coeff_.coeff(0,3); + return true; +} + +bool FifthOrderPolynomialTrajectory::changeTrajectory(double final_time, double final_pos, double final_vel, double final_acc) +{ + if(final_time < initial_time_) + return false; + + final_time_ = final_time; + return changeTrajectory(final_pos, final_vel, final_acc); +} + +bool FifthOrderPolynomialTrajectory::changeTrajectory(double initial_time, double initial_pos, double initial_vel, double initial_acc, + double final_time, double final_pos, double final_vel, double final_acc) +{ + if(final_time < initial_time) + return false; + + initial_time_ = initial_time; + initial_pos_ = initial_pos; + initial_vel_ = initial_vel; + initial_acc_ = initial_acc; + + final_time_ = final_time; + + return changeTrajectory(final_pos, final_vel, final_acc); +} + +double FifthOrderPolynomialTrajectory::getPosition(double time) +{ + if(time >= final_time_) + { + current_time_ = final_time_; + current_pos_ = final_pos_; + current_vel_ = final_vel_; + current_acc_ = final_acc_; + return final_pos_; + } + else if(time <= initial_time_ ) + { + current_time_ = initial_time_; + current_pos_ = initial_pos_; + current_vel_ = initial_vel_; + current_acc_ = initial_acc_; + return initial_pos_; + } + else + { + current_time_ = time; + time_variables_ << powDI(time, 5), powDI(time, 4), powDI(time, 3), powDI(time, 2), time, 1.0; + current_pos_ = (time_variables_ * position_coeff_).coeff(0,0); + current_vel_ = (time_variables_ * velocity_coeff_).coeff(0,0); + current_acc_ = (time_variables_ * acceleration_coeff_).coeff(0,0); + return current_pos_; + } +} + +double FifthOrderPolynomialTrajectory::getVelocity(double time) +{ + if(time >= final_time_) + { + current_time_ = final_time_; + current_pos_ = final_pos_; + current_vel_ = final_vel_; + current_acc_ = final_acc_; + return final_vel_; + } + else if(time <= initial_time_ ) + { + current_time_ = initial_time_; + current_pos_ = initial_pos_; + current_vel_ = initial_vel_; + current_acc_ = initial_acc_; + return initial_vel_; + } + else + { + current_time_ = time; + time_variables_ << powDI(time, 5), powDI(time, 4), powDI(time, 3), powDI(time, 2), time, 1.0; + current_pos_ = (time_variables_ * position_coeff_).coeff(0,0); + current_vel_ = (time_variables_ * velocity_coeff_).coeff(0,0); + current_acc_ = (time_variables_ * acceleration_coeff_).coeff(0,0); + return current_vel_; + } +} + +double FifthOrderPolynomialTrajectory::getAcceleration(double time) +{ + if(time >= final_time_) + { + current_time_ = final_time_; + current_pos_ = final_pos_; + current_vel_ = final_vel_; + current_acc_ = final_acc_; + return final_acc_; + } + else if(time <= initial_time_ ) + { + current_time_ = initial_time_; + current_pos_ = initial_pos_; + current_vel_ = initial_vel_; + current_acc_ = initial_acc_; + return initial_acc_; + } + else + { + current_time_ = time; + time_variables_ << powDI(time, 5), powDI(time, 4), powDI(time, 3), powDI(time, 2), time, 1.0; + current_pos_ = (time_variables_ * position_coeff_).coeff(0,0); + current_vel_ = (time_variables_ * velocity_coeff_).coeff(0,0); + current_acc_ = (time_variables_ * acceleration_coeff_).coeff(0,0); + return current_acc_; + } +} + +void FifthOrderPolynomialTrajectory::setTime(double time) +{ + if(time >= final_time_) + { + current_time_ = final_time_; + current_pos_ = final_pos_; + current_vel_ = final_vel_; + current_acc_ = final_acc_; + } + else if(time <= initial_time_ ) + { + current_time_ = initial_time_; + current_pos_ = initial_pos_; + current_vel_ = initial_vel_; + current_acc_ = initial_acc_; + } + else + { + current_time_ = time; + time_variables_ << powDI(time, 5), powDI(time, 4), powDI(time, 3), powDI(time, 2), time, 1.0; + current_pos_ = (time_variables_ * position_coeff_).coeff(0,0); + current_vel_ = (time_variables_ * velocity_coeff_).coeff(0,0); + current_acc_ = (time_variables_ * acceleration_coeff_).coeff(0,0); + } +} + +double FifthOrderPolynomialTrajectory::getPosition() +{ + return current_pos_; +} + +double FifthOrderPolynomialTrajectory::getVelocity() +{ + return current_vel_; +} + +double FifthOrderPolynomialTrajectory::getAcceleration() +{ + return current_acc_; +} + } From 525ab6cf756ea4340333be00169bc35e790bfeb4 Mon Sep 17 00:00:00 2001 From: s-changhyun Date: Wed, 10 Aug 2016 14:40:32 +0900 Subject: [PATCH 048/238] add function --- .../robotis_controller/robotis_controller.h | 2 +- .../robotis_trajectory_calculator.h | 4 + .../src/robotis_trajectory_calculator.cpp | 80 ++++++++++++++++++- 3 files changed, 82 insertions(+), 4 deletions(-) diff --git a/robotis_controller/include/robotis_controller/robotis_controller.h b/robotis_controller/include/robotis_controller/robotis_controller.h index ae91370..a24f870 100644 --- a/robotis_controller/include/robotis_controller/robotis_controller.h +++ b/robotis_controller/include/robotis_controller/robotis_controller.h @@ -93,7 +93,7 @@ private: void initializeSyncWrite(); public: - static const int CONTROL_CYCLE_MSEC = 8; // 8 msec + static const int CONTROL_CYCLE_MSEC = 4; // 8 msec bool DEBUG_PRINT; Robot *robot_; diff --git a/robotis_math/include/robotis_math/robotis_trajectory_calculator.h b/robotis_math/include/robotis_math/robotis_trajectory_calculator.h index 56bf644..1d1f532 100644 --- a/robotis_math/include/robotis_math/robotis_trajectory_calculator.h +++ b/robotis_math/include/robotis_math/robotis_trajectory_calculator.h @@ -50,6 +50,10 @@ Eigen::MatrixXd calcMinimumJerkTra(double pos_start, double vel_start, double ac double pos_end, double vel_end, double accel_end, double smp_time, double mov_time); +Eigen::MatrixXd calcMinimumJerkTraPlus(double pos_start, double vel_start, double accel_start, + double pos_end, double vel_end, double accel_end, + double smp_time, double mov_time); + Eigen::MatrixXd calcMinimumJerkTraWithViaPoints(int via_num, double pos_start, double vel_start, double accel_start, Eigen::MatrixXd pos_via, Eigen::MatrixXd vel_via, Eigen::MatrixXd accel_via, diff --git a/robotis_math/src/robotis_trajectory_calculator.cpp b/robotis_math/src/robotis_trajectory_calculator.cpp index 5f66ffc..bddb6c1 100644 --- a/robotis_math/src/robotis_trajectory_calculator.cpp +++ b/robotis_math/src/robotis_trajectory_calculator.cpp @@ -40,9 +40,11 @@ namespace robotis_framework { -Eigen::MatrixXd calcMinimumJerkTra(double pos_start, double vel_start, double accel_start, - double pos_end, double vel_end, double accel_end, - double smp_time, double mov_time) +Eigen::MatrixXd calcMinimumJerkTra( + double pos_start, double vel_start, double accel_start, + double pos_end, double vel_end, double accel_end, + double smp_time, double mov_time + ) /* simple minimum jerk trajectory @@ -97,6 +99,78 @@ Eigen::MatrixXd calcMinimumJerkTra(double pos_start, double vel_start, double ac return minimum_jerk_tra; } +Eigen::MatrixXd calcMinimumJerkTraPlus( + double pos_start, double vel_start, double accel_start, + double pos_end, double vel_end, double accel_end, + double smp_time, double mov_time + ) +/* + simple minimum jerk trajectory + + pos_start : position at initial state + vel_start : velocity at initial state + accel_start : acceleration at initial state + + pos_end : position at final state + vel_end : velocity at final state + accel_end : acceleration at final state + + smp_time : sampling time + mov_time : movement time + */ + +{ + Eigen::MatrixXd poly_matrix(3,3); + Eigen::MatrixXd poly_vector(3,1); + + poly_matrix << + pow(mov_time,3), pow(mov_time,4), pow(mov_time,5), + 3*pow(mov_time,2), 4*pow(mov_time,3), 5*pow(mov_time,4), + 6*mov_time, 12*pow(mov_time,2), 20*pow(mov_time,3); + + poly_vector << + pos_end-pos_start-vel_start*mov_time-accel_start*pow(mov_time,2)/2, + vel_end-vel_start-accel_start*mov_time, + accel_end-accel_start ; + + Eigen::Matrix poly_coeff = poly_matrix.inverse() * poly_vector; + + double time_steps = mov_time/smp_time; + int all_time_steps = round(time_steps+1); + + Eigen::MatrixXd time = Eigen::MatrixXd::Zero(all_time_steps,1); + Eigen::MatrixXd minimum_jerk_tra = Eigen::MatrixXd::Zero(all_time_steps,3); + + for (int step=0; step Date: Wed, 10 Aug 2016 20:50:56 +0900 Subject: [PATCH 049/238] fix bug --- .../include/robotis_controller/robotis_controller.h | 2 +- robotis_math/src/robotis_trajectory_calculator.cpp | 5 +++-- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/robotis_controller/include/robotis_controller/robotis_controller.h b/robotis_controller/include/robotis_controller/robotis_controller.h index a24f870..ae91370 100644 --- a/robotis_controller/include/robotis_controller/robotis_controller.h +++ b/robotis_controller/include/robotis_controller/robotis_controller.h @@ -93,7 +93,7 @@ private: void initializeSyncWrite(); public: - static const int CONTROL_CYCLE_MSEC = 4; // 8 msec + static const int CONTROL_CYCLE_MSEC = 8; // 8 msec bool DEBUG_PRINT; Robot *robot_; diff --git a/robotis_math/src/robotis_trajectory_calculator.cpp b/robotis_math/src/robotis_trajectory_calculator.cpp index bc3b359..c436310 100644 --- a/robotis_math/src/robotis_trajectory_calculator.cpp +++ b/robotis_math/src/robotis_trajectory_calculator.cpp @@ -35,7 +35,7 @@ * Author: sch */ -#include "../include/robotis_math/robotis_trajectory_calculator.h" +#include "robotis_math/robotis_trajectory_calculator.h" namespace robotis_framework { @@ -508,7 +508,7 @@ bool FifthOrderPolynomialTrajectory::changeTrajectory(double final_pos, double time_mat.resize(6,6); time_mat << powDI(initial_time_, 5), powDI(initial_time_, 4), powDI(initial_time_, 3), powDI(initial_time_, 2), initial_time_, 1.0, 5.0*powDI(initial_time_, 4), 4.0*powDI(initial_time_, 3), 3.0*powDI(initial_time_, 2), 2.0*initial_time_, 1.0, 0.0, - 20.0*powDI(initial_time_, 3), 12.0*powDI(initial_time_, 2), 6.0*initial_time_, 2.0, 0.0, 0.0; + 20.0*powDI(initial_time_, 3), 12.0*powDI(initial_time_, 2), 6.0*initial_time_, 2.0, 0.0, 0.0, powDI(final_time_, 5), powDI(final_time_, 4), powDI(final_time_, 3), powDI(final_time_, 2), final_time_, 1.0, 5.0*powDI(final_time_, 4), 4.0*powDI(final_time_, 3), 3.0*powDI(final_time_, 2), 2.0*final_time_, 1.0, 0.0, 20.0*powDI(final_time_, 3), 12.0*powDI(final_time_, 2), 6.0*final_time_, 2.0, 0.0, 0.0; @@ -529,6 +529,7 @@ bool FifthOrderPolynomialTrajectory::changeTrajectory(double final_pos, double 12.0*position_coeff_.coeff(0,1), 6.0*position_coeff_.coeff(0,2), 2.0*position_coeff_.coeff(0,3); + return true; } From ed7f92e0dfd33f63f4b582f2cfad5af9461a7a42 Mon Sep 17 00:00:00 2001 From: ROBOTIS-zerom Date: Thu, 11 Aug 2016 20:02:06 +0900 Subject: [PATCH 050/238] - SyncWriteItem bug fixed. --- .../robotis_controller/robotis_controller.cpp | 30 +++++++++++++++---- 1 file changed, 25 insertions(+), 5 deletions(-) diff --git a/robotis_controller/src/robotis_controller/robotis_controller.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp index c568d82..0029e4f 100644 --- a/robotis_controller/src/robotis_controller/robotis_controller.cpp +++ b/robotis_controller/src/robotis_controller/robotis_controller.cpp @@ -1173,11 +1173,31 @@ void RobotisController::syncWriteItemCallback(const robotis_controller_msgs::Syn { for (int i = 0; i < msg->joint_name.size(); i++) { - Dynamixel *dxl = robot_->dxls_[msg->joint_name[i]]; - ControlTableItem *item = dxl->ctrl_table_[msg->item_name]; + Device *device; - dynamixel::PortHandler *port = robot_->ports_[dxl->port_name_]; - dynamixel::PacketHandler *packet_handler = dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + auto d_it1 = robot_->dxls_.find(msg->joint_name[i]); + if (d_it1 != robot_->dxls_.end()) + { + device = d_it1->second; + } + else + { + auto d_it2 = robot_->sensors_.find(msg->joint_name[i]); + if (d_it2 != robot_->sensors_.end()) + { + device = d_it2->second; + } + else + { + // could not find the device + continue; + } + } + + ControlTableItem *item = device->ctrl_table_[msg->item_name]; + + dynamixel::PortHandler *port = robot_->ports_[device->port_name_]; + dynamixel::PacketHandler *packet_handler = dynamixel::PacketHandler::getPacketHandler(device->protocol_version_); if (item->access_type_ == Read) continue; @@ -1215,7 +1235,7 @@ void RobotisController::syncWriteItemCallback(const robotis_controller_msgs::Syn data[2] = DXL_LOBYTE(DXL_HIWORD((uint32_t)msg->value[i])); data[3] = DXL_HIBYTE(DXL_HIWORD((uint32_t)msg->value[i])); } - direct_sync_write_[idx]->addParam(dxl->id_, data); + direct_sync_write_[idx]->addParam(device->id_, data); delete[] data; } } From f1f9c888867f58ef0f48f3671730e1e77e662a79 Mon Sep 17 00:00:00 2001 From: Jay-Song Date: Thu, 11 Aug 2016 20:07:58 +0900 Subject: [PATCH 051/238] Remove robotis_math robotis_math is moved to ROBOTIS_Math repository --- robotis_math/CMakeLists.txt | 42 -- .../robotis_math/robotis_linear_algebra.h | 81 -- .../include/robotis_math/robotis_math.h | 43 -- .../include/robotis_math/robotis_math_base.h | 63 -- .../robotis_trajectory_calculator.h | 113 --- .../include/robotis_math/step_data_define.h | 83 --- robotis_math/package.xml | 19 - robotis_math/src/robotis_linear_algebra.cpp | 322 -------- robotis_math/src/robotis_math_base.cpp | 53 -- .../src/robotis_trajectory_calculator.cpp | 689 ------------------ 10 files changed, 1508 deletions(-) delete mode 100644 robotis_math/CMakeLists.txt delete mode 100644 robotis_math/include/robotis_math/robotis_linear_algebra.h delete mode 100644 robotis_math/include/robotis_math/robotis_math.h delete mode 100644 robotis_math/include/robotis_math/robotis_math_base.h delete mode 100644 robotis_math/include/robotis_math/robotis_trajectory_calculator.h delete mode 100644 robotis_math/include/robotis_math/step_data_define.h delete mode 100644 robotis_math/package.xml delete mode 100644 robotis_math/src/robotis_linear_algebra.cpp delete mode 100644 robotis_math/src/robotis_math_base.cpp delete mode 100644 robotis_math/src/robotis_trajectory_calculator.cpp diff --git a/robotis_math/CMakeLists.txt b/robotis_math/CMakeLists.txt deleted file mode 100644 index 7ce8d9a..0000000 --- a/robotis_math/CMakeLists.txt +++ /dev/null @@ -1,42 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(robotis_math) - -find_package(catkin REQUIRED COMPONENTS - roscpp - cmake_modules -) - -find_package(Eigen REQUIRED) - -################################### -## catkin specific configuration ## -################################### - -catkin_package( - INCLUDE_DIRS include - LIBRARIES robotis_math - CATKIN_DEPENDS roscpp -# DEPENDS system_lib -) - -########### -## Build ## -########### - -include_directories( - include - ${catkin_INCLUDE_DIRS} - ${Eigen_INCLUDE_DIRS} -) - -add_library(robotis_math - src/robotis_math_base.cpp - src/robotis_trajectory_calculator.cpp - src/robotis_linear_algebra.cpp -) - -add_dependencies(robotis_math ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -target_link_libraries(robotis_math - ${catkin_LIBRARIES} -) diff --git a/robotis_math/include/robotis_math/robotis_linear_algebra.h b/robotis_math/include/robotis_math/robotis_linear_algebra.h deleted file mode 100644 index f8cd69e..0000000 --- a/robotis_math/include/robotis_math/robotis_linear_algebra.h +++ /dev/null @@ -1,81 +0,0 @@ -/******************************************************************************* - * Copyright (c) 2016, ROBOTIS CO., LTD. - * 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 ROBOTIS 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. - *******************************************************************************/ - -/* - * robotis_linear_algebra.h - * - * Created on: June 7, 2016 - * Author: sch - */ - -#ifndef ROBOTIS_MATH_ROBOTIS_LINEAR_ALGEBRA_H_ -#define ROBOTIS_MATH_ROBOTIS_LINEAR_ALGEBRA_H_ - -#include - -#define EIGEN_NO_DEBUG -#define EIGEN_NO_STATIC_ASSERT - -#include -#include "step_data_define.h" - -namespace robotis_framework -{ - -Eigen::MatrixXd getTransitionXYZ(double position_x, double position_y, double position_z); -Eigen::MatrixXd getTransformationXYZRPY(double position_x, double position_y, double position_z , double roll, double pitch, double yaw); -Eigen::MatrixXd getInverseTransformation(Eigen::MatrixXd transform); -Eigen::MatrixXd getInertiaXYZ(double ixx, double ixy, double ixz , double iyy , double iyz, double izz); -Eigen::MatrixXd getRotationX(double angle); -Eigen::MatrixXd getRotationY(double angle); -Eigen::MatrixXd getRotationZ(double angle); -Eigen::MatrixXd getRotation4d(double roll, double pitch, double yaw); -Eigen::MatrixXd getTranslation4D(double position_x, double position_y, double position_z); - -Eigen::MatrixXd convertRotationToRPY(Eigen::MatrixXd rotation); -Eigen::MatrixXd convertRPYToRotation(double roll, double pitch, double yaw); -Eigen::Quaterniond convertRPYToQuaternion(double roll, double pitch, double yaw); -Eigen::Quaterniond convertRotationToQuaternion(Eigen::MatrixXd rotation); -Eigen::MatrixXd convertQuaternionToRPY(Eigen::Quaterniond quaternion); -Eigen::MatrixXd convertQuaternionToRotation(Eigen::Quaterniond quaternion); - -Eigen::MatrixXd calcHatto(Eigen::MatrixXd matrix3d); -Eigen::MatrixXd calcRodrigues(Eigen::MatrixXd hat_matrix, double angle); -Eigen::MatrixXd convertRotToOmega(Eigen::MatrixXd rotation); -Eigen::MatrixXd calcCross(Eigen::MatrixXd vector3d_a, Eigen::MatrixXd vector3d_b); -double calcInner(Eigen::MatrixXd vector3d_a, Eigen::MatrixXd vector3d_b); - -Pose3D getPose3DfromTransformMatrix(Eigen::MatrixXd transform); - -} - - - -#endif /* ROBOTIS_MATH_ROBOTIS_LINEAR_ALGEBRA_H_ */ diff --git a/robotis_math/include/robotis_math/robotis_math.h b/robotis_math/include/robotis_math/robotis_math.h deleted file mode 100644 index f78f4be..0000000 --- a/robotis_math/include/robotis_math/robotis_math.h +++ /dev/null @@ -1,43 +0,0 @@ -/******************************************************************************* - * Copyright (c) 2016, ROBOTIS CO., LTD. - * 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 ROBOTIS 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. - *******************************************************************************/ - -/* - * robotis_math.h - * - * Created on: June 7, 2016 - * Author: sch - */ - -#ifndef ROBOTIS_MATH_ROBOTIS_MATH_H_ -#define ROBOTIS_MATH_ROBOTIS_MATH_H_ - -#include "robotis_trajectory_calculator.h" - -#endif /* ROBOTIS_MATH_ROBOTIS_MATH_H_ */ diff --git a/robotis_math/include/robotis_math/robotis_math_base.h b/robotis_math/include/robotis_math/robotis_math_base.h deleted file mode 100644 index e54caca..0000000 --- a/robotis_math/include/robotis_math/robotis_math_base.h +++ /dev/null @@ -1,63 +0,0 @@ -/******************************************************************************* - * Copyright (c) 2016, ROBOTIS CO., LTD. - * 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 ROBOTIS 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. - *******************************************************************************/ - -/* - * robotis_math_base.h - * - * Created on: June 7, 2016 - * Author: sch - */ - -#ifndef ROBOTIS_MATH_ROBOTIS_MATH_BASE_H_ -#define ROBOTIS_MATH_ROBOTIS_MATH_BASE_H_ - -#include - -namespace robotis_framework -{ - -#define PRINT_VAR(X) std::cout << #X << " : " << X << std::endl -#define PRINT_MAT(X) std::cout << #X << ":\n" << X << std::endl << std::endl - -#define DEGREE2RADIAN (M_PI / 180.0) -#define RADIAN2DEGREE (180.0 / M_PI) - -inline double powDI(double a, int b) -{ - return (b == 0 ? 1 : (b > 0 ? a * powDI(a, b - 1) : 1 / powDI(a, -b))); -} - -double sign(double x); - -} - - - -#endif /* ROBOTIS_MATH_ROBOTIS_MATH_BASE_H_ */ diff --git a/robotis_math/include/robotis_math/robotis_trajectory_calculator.h b/robotis_math/include/robotis_math/robotis_trajectory_calculator.h deleted file mode 100644 index 670264f..0000000 --- a/robotis_math/include/robotis_math/robotis_trajectory_calculator.h +++ /dev/null @@ -1,113 +0,0 @@ -/******************************************************************************* - * Copyright (c) 2016, ROBOTIS CO., LTD. - * 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 ROBOTIS 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. - *******************************************************************************/ - -/* - * robotis_trajectory_calculator.h - * - * Created on: June 7, 2016 - * Author: sch - */ - -#ifndef ROBOTIS_MATH_ROBOTIS_TRAJECTORY_CALCULATOR_H_ -#define ROBOTIS_MATH_ROBOTIS_TRAJECTORY_CALCULATOR_H_ - -#include "robotis_linear_algebra.h" -#include "robotis_math_base.h" - -// minimum jerk trajectory - -namespace robotis_framework -{ - -Eigen::MatrixXd calcMinimumJerkTra(double pos_start, double vel_start, double accel_start, - double pos_end, double vel_end, double accel_end, - double smp_time, double mov_time); - -Eigen::MatrixXd calcMinimumJerkTraPlus(double pos_start, double vel_start, double accel_start, - double pos_end, double vel_end, double accel_end, - double smp_time, double mov_time); - -Eigen::MatrixXd calcMinimumJerkTraWithViaPoints(int via_num, - double pos_start, double vel_start, double accel_start, - Eigen::MatrixXd pos_via, Eigen::MatrixXd vel_via, Eigen::MatrixXd accel_via, - double pos_end, double vel_end, double accel_end, - double smp_time, Eigen::MatrixXd via_time, double mov_time); - -Eigen::MatrixXd calcArc3dTra(double smp_time, double mov_time, - Eigen::MatrixXd center_point, Eigen::MatrixXd normal_vector, Eigen::MatrixXd start_point, - double rotation_angle, double cross_ratio); - - -class FifthOrderPolynomialTrajectory -{ -public: - FifthOrderPolynomialTrajectory(double initial_time, double initial_pos, double initial_vel, double initial_acc, - double final_time, double final_pos, double final_vel, double final_acc); - FifthOrderPolynomialTrajectory(); - ~FifthOrderPolynomialTrajectory(); - - bool changeTrajectory(double final_pos, double final_vel, double final_acc); - bool changeTrajectory(double final_time, double final_pos, double final_vel, double final_acc); - bool changeTrajectory(double initial_time, double initial_pos, double initial_vel, double initial_acc, - double final_time, double final_pos, double final_vel, double final_acc); - - double getPosition(double time); - double getVelocity(double time); - double getAcceleration(double time); - - void setTime(double time); - double getPosition(); - double getVelocity(); - double getAcceleration(); - - double initial_time_; - double initial_pos_; - double initial_vel_; - double initial_acc_; - - double current_time_; - double current_pos_; - double current_vel_; - double current_acc_; - - double final_time_; - double final_pos_; - double final_vel_; - double final_acc_; - - Eigen::MatrixXd position_coeff_; - Eigen::MatrixXd velocity_coeff_; - Eigen::MatrixXd acceleration_coeff_; - Eigen::MatrixXd time_variables_; -}; - -} - -#endif /* ROBOTIS_MATH_ROBOTIS_TRAJECTORY_CALCULATOR_H_ */ diff --git a/robotis_math/include/robotis_math/step_data_define.h b/robotis_math/include/robotis_math/step_data_define.h deleted file mode 100644 index 135d445..0000000 --- a/robotis_math/include/robotis_math/step_data_define.h +++ /dev/null @@ -1,83 +0,0 @@ -/******************************************************************************* - * Copyright (c) 2016, ROBOTIS CO., LTD. - * 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 ROBOTIS 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. - *******************************************************************************/ - -/* - * step_data_define.h - * - * Created on: 2016. 8. 10. - * Author: Jay Song - */ - -#ifndef ROBOTIS_MATH_STEP_DATA_DEFINE_H_ -#define ROBOTIS_MATH_STEP_DATA_DEFINE_H_ - -namespace robotis_framework -{ - -typedef struct -{ - double x, y, z; -} Position3D; - -typedef struct -{ - double x, y, z, roll, pitch, yaw; -} Pose3D; - -typedef struct -{ - int moving_foot; - double foot_z_swap, body_z_swap; - double shoulder_swing_gain, elbow_swing_gain; - double waist_roll_angle, waist_pitch_angle, waist_yaw_angle; - Pose3D left_foot_pose; - Pose3D right_foot_pose; - Pose3D body_pose; -} StepPositionData; - -typedef struct -{ - int walking_state; - double abs_step_time, dsp_ratio; - double start_time_delay_ratio_x, start_time_delay_ratio_y, start_time_delay_ratio_z; - double start_time_delay_ratio_roll, start_time_delay_ratio_pitch, start_time_delay_ratio_yaw; - double finish_time_advance_ratio_x, finish_time_advance_ratio_y, finish_time_advance_ratio_z; - double finish_time_advance_ratio_roll, finish_time_advance_ratio_pitch, finish_time_advance_ratio_yaw; -} StepTimeData; - -typedef struct -{ - StepPositionData position_data; - StepTimeData time_data; -} StepData; - -} - -#endif /* ROBOTIS_MATH_STEP_DATA_DEFINE_H_ */ diff --git a/robotis_math/package.xml b/robotis_math/package.xml deleted file mode 100644 index 29d618b..0000000 --- a/robotis_math/package.xml +++ /dev/null @@ -1,19 +0,0 @@ - - - robotis_math - 0.0.0 - The robotis_math package - - sch - BSD - - sch - - catkin - roscpp - cmake_modules - - roscpp - cmake_modules - - \ No newline at end of file diff --git a/robotis_math/src/robotis_linear_algebra.cpp b/robotis_math/src/robotis_linear_algebra.cpp deleted file mode 100644 index 293fef3..0000000 --- a/robotis_math/src/robotis_linear_algebra.cpp +++ /dev/null @@ -1,322 +0,0 @@ -/******************************************************************************* - * Copyright (c) 2016, ROBOTIS CO., LTD. - * 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 ROBOTIS 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. - *******************************************************************************/ - -/* - * robotis_linear_algebra.cpp - * - * Created on: June 7, 2016 - * Author: sch - */ - -#include "robotis_math/robotis_linear_algebra.h" - -namespace robotis_framework -{ - -Eigen::MatrixXd getTransitionXYZ(double position_x, double position_y, double position_z) -{ - Eigen::MatrixXd position(3,1); - - position << - position_x, - position_y, - position_z; - - return position; -} - -Eigen::MatrixXd getTransformationXYZRPY(double position_x, double position_y, double position_z , double roll , double pitch , double yaw) -{ - Eigen::MatrixXd transformation = getRotation4d(roll, pitch, yaw); - transformation.coeffRef(0,3) = position_x; - transformation.coeffRef(1,3) = position_y; - transformation.coeffRef(2,3) = position_z; - - return transformation; -} - -Eigen::MatrixXd getInverseTransformation(Eigen::MatrixXd transform) -{ - // If T is Transform Matrix A from B, the BOA is translation component coordi. B to coordi. A - - Eigen::Vector3d vec_boa; - Eigen::Vector3d vec_x, vec_y, vec_z; - Eigen::MatrixXd inv_t(4,4); - - vec_boa(0) = -transform(0,3); - vec_boa(1) = -transform(1,3); - vec_boa(2) = -transform(2,3); - - vec_x(0) = transform(0,0); vec_x(1) = transform(1,0); vec_x(2) = transform(2,0); - vec_y(0) = transform(0,1); vec_y(1) = transform(1,1); vec_y(2) = transform(2,1); - vec_z(0) = transform(0,2); vec_z(1) = transform(1,2); vec_z(2) = transform(2,2); - - inv_t << - vec_x(0), vec_x(1), vec_x(2), vec_boa.dot(vec_x), - vec_y(0), vec_y(1), vec_y(2), vec_boa.dot(vec_y), - vec_z(0), vec_z(1), vec_z(2), vec_boa.dot(vec_z), - 0, 0, 0, 1; - - return inv_t; -} - -Eigen::MatrixXd getInertiaXYZ(double ixx, double ixy, double ixz , double iyy , double iyz, double izz) -{ - Eigen::MatrixXd inertia(3,3); - - inertia << - ixx, ixy, ixz, - ixy, iyy, iyz, - ixz, iyz, izz; - - return inertia; -} - -Eigen::MatrixXd getRotationX(double angle) -{ - Eigen::MatrixXd rotation(3,3); - - rotation << - 1.0, 0.0, 0.0, - 0.0, cos(angle), -sin(angle), - 0.0, sin(angle), cos(angle); - - return rotation; -} - -Eigen::MatrixXd getRotationY(double angle) -{ - Eigen::MatrixXd rotation(3,3); - - rotation << - cos(angle), 0.0, sin(angle), - 0.0, 1.0, 0.0, - -sin(angle), 0.0, cos(angle); - - return rotation; -} - -Eigen::MatrixXd getRotationZ(double angle) -{ - Eigen::MatrixXd rotation(3,3); - - rotation << - cos(angle), -sin(angle), 0.0, - sin(angle), cos(angle), 0.0, - 0.0, 0.0, 1.0; - - return rotation; -} - -Eigen::MatrixXd getRotation4d(double roll, double pitch, double yaw ) -{ - double sr = sin(roll), cr = cos(roll); - double sp = sin(pitch), cp = cos(pitch); - double sy = sin(yaw), cy = cos(yaw); - - Eigen::MatrixXd mat_roll(4,4); - Eigen::MatrixXd mat_pitch(4,4); - Eigen::MatrixXd mat_yaw(4,4); - - mat_roll << - 1, 0, 0, 0, - 0, cr, -sr, 0, - 0, sr, cr, 0, - 0, 0, 0, 1; - - mat_pitch << - cp, 0, sp, 0, - 0, 1, 0, 0, - -sp, 0, cp, 0, - 0, 0, 0, 1; - - mat_yaw << - cy, -sy, 0, 0, - sy, cy, 0, 0, - 0, 0, 1, 0, - 0, 0, 0, 1; - - Eigen::MatrixXd mat_rpy = (mat_yaw*mat_pitch)*mat_roll; - - return mat_rpy; -} - -Eigen::MatrixXd getTranslation4D(double position_x, double position_y, double position_z) -{ - Eigen::MatrixXd mat_translation(4,4); - - mat_translation << - 1, 0, 0, position_x, - 0, 1, 0, position_y, - 0, 0, 1, position_z, - 0, 0, 0, 1; - - return mat_translation; -} - -Eigen::MatrixXd convertRotationToRPY(Eigen::MatrixXd rotation) -{ - Eigen::MatrixXd rpy = Eigen::MatrixXd::Zero(3,1); - - rpy.coeffRef(0,0) = atan2(rotation.coeff(2,1), rotation.coeff(2,2)); - rpy.coeffRef(1,0) = atan2(-rotation.coeff(2,0), sqrt(pow(rotation.coeff(2,1), 2) + pow(rotation.coeff(2,2),2))); - rpy.coeffRef(2,0) = atan2 (rotation.coeff(1,0), rotation.coeff(0,0)); - - return rpy; -} - -Eigen::MatrixXd convertRPYToRotation(double roll, double pitch, double yaw) -{ - Eigen::MatrixXd rotation = getRotationZ(yaw)*getRotationY(pitch)*getRotationX(roll); - - return rotation; -} - -Eigen::Quaterniond convertRPYToQuaternion(double roll, double pitch, double yaw) -{ - Eigen::MatrixXd rotation = convertRPYToRotation(roll,pitch,yaw); - - Eigen::Matrix3d rotation3d; - rotation3d = rotation.block<3,3>(0,0); - - Eigen::Quaterniond quaternion; - quaternion = rotation3d; - - return quaternion; -} - -Eigen::Quaterniond convertRotationToQuaternion(Eigen::MatrixXd rotation) -{ - Eigen::Matrix3d rotation3d; - rotation3d = rotation.block<3,3>(0,0); - - Eigen::Quaterniond quaternion; - quaternion = rotation3d; - - return quaternion; -} - -Eigen::MatrixXd convertQuaternionToRPY(Eigen::Quaterniond quaternion) -{ - Eigen::MatrixXd rpy = convertRotationToRPY(quaternion.toRotationMatrix()); - - return rpy; -} - -Eigen::MatrixXd convertQuaternionToRotation(Eigen::Quaterniond quaternion) -{ - Eigen::MatrixXd rotation = quaternion.toRotationMatrix(); - - return rotation; -} - -Eigen::MatrixXd calcHatto(Eigen::MatrixXd matrix3d) -{ - Eigen::MatrixXd hatto(3,3); - - hatto << - 0.0, -matrix3d.coeff(2,0), matrix3d.coeff(1,0), - matrix3d.coeff(2,0), 0.0, -matrix3d.coeff(0,0), - -matrix3d.coeff(1,0), matrix3d.coeff(0,0), 0.0; - - return hatto; -} - -Eigen::MatrixXd calcRodrigues(Eigen::MatrixXd hat_matrix, double angle) -{ - Eigen::MatrixXd identity = Eigen::MatrixXd::Identity(3,3); - Eigen::MatrixXd rodrigues = identity+hat_matrix*sin(angle)+hat_matrix*hat_matrix*(1-cos(angle)); - - return rodrigues; -} - -Eigen::MatrixXd convertRotToOmega(Eigen::MatrixXd rotation) -{ - double eps = 1e-12; - - double alpha = (rotation.coeff(0,0)+rotation.coeff(1,1)+rotation.coeff(2,2)-1.0)/2.0; - double alpha_dash = fabs( alpha - 1.0 ); - - Eigen::MatrixXd rot_to_omega(3,1); - - if( alpha_dash < eps ) - { - rot_to_omega << - 0.0, - 0.0, - 0.0; - } - else - { - double theta = acos(alpha); - - rot_to_omega << - rotation.coeff(2,1)-rotation.coeff(1,2), - rotation.coeff(0,2)-rotation.coeff(2,0), - rotation.coeff(1,0)-rotation.coeff(0,1); - - rot_to_omega = 0.5*(theta/sin(theta))*rot_to_omega; - } - - return rot_to_omega; -} - -Eigen::MatrixXd calcCross(Eigen::MatrixXd vector3d_a, Eigen::MatrixXd vector3d_b) -{ - Eigen::MatrixXd cross(3,1); - - cross << - vector3d_a.coeff(1,0)*vector3d_b.coeff(2,0)-vector3d_a.coeff(2,0)*vector3d_b.coeff(1,0), - vector3d_a.coeff(2,0)*vector3d_b.coeff(0,0)-vector3d_a.coeff(0,0)*vector3d_b.coeff(2,0), - vector3d_a.coeff(0,0)*vector3d_b.coeff(1,0)-vector3d_a.coeff(1,0)*vector3d_b.coeff(0,0); - - return cross; -} - -double calcInner(Eigen::MatrixXd vector3d_a, Eigen::MatrixXd vector3d_b) -{ - return vector3d_a.dot(vector3d_b); -} - -Pose3D getPose3DfromTransformMatrix(Eigen::MatrixXd transform) -{ - Pose3D pose_3d; - - pose_3d.x = transform.coeff(0, 3); - pose_3d.y = transform.coeff(1, 3); - pose_3d.z = transform.coeff(2, 3); - pose_3d.roll = atan2( transform.coeff(2,1), transform.coeff(2,2)); - pose_3d.pitch = atan2(-transform.coeff(2,0), sqrt(transform.coeff(2,1)*transform.coeff(2,1) + transform.coeff(2,2)*transform.coeff(2,2)) ); - pose_3d.yaw = atan2( transform.coeff(1,0), transform.coeff(0,0)); - - return pose_3d; -} - -} diff --git a/robotis_math/src/robotis_math_base.cpp b/robotis_math/src/robotis_math_base.cpp deleted file mode 100644 index bd37d51..0000000 --- a/robotis_math/src/robotis_math_base.cpp +++ /dev/null @@ -1,53 +0,0 @@ -/******************************************************************************* - * Copyright (c) 2016, ROBOTIS CO., LTD. - * 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 ROBOTIS 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. - *******************************************************************************/ - -/* - * robotis_math_base.cpp - * - * Created on: June 7, 2016 - * Author: sch - */ - -#include "robotis_math/robotis_math_base.h" - -namespace robotis_framework -{ - -double sign(double x) -{ - if ( x < 0.0 ) - return -1.0; - else if ( x > 0.0) - return 1.0; - else - return 0.0; -} - -} diff --git a/robotis_math/src/robotis_trajectory_calculator.cpp b/robotis_math/src/robotis_trajectory_calculator.cpp deleted file mode 100644 index c436310..0000000 --- a/robotis_math/src/robotis_trajectory_calculator.cpp +++ /dev/null @@ -1,689 +0,0 @@ -/******************************************************************************* - * Copyright (c) 2016, ROBOTIS CO., LTD. - * 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 ROBOTIS 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. - *******************************************************************************/ - -/* - * robotis_trajectory_calculator.cpp - * - * Created on: June 7, 2016 - * Author: sch - */ - -#include "robotis_math/robotis_trajectory_calculator.h" - -namespace robotis_framework -{ - -Eigen::MatrixXd calcMinimumJerkTra( - double pos_start, double vel_start, double accel_start, - double pos_end, double vel_end, double accel_end, - double smp_time, double mov_time - ) -/* - simple minimum jerk trajectory - - pos_start : position at initial state - vel_start : velocity at initial state - accel_start : acceleration at initial state - - pos_end : position at final state - vel_end : velocity at final state - accel_end : acceleration at final state - - smp_time : sampling time - mov_time : movement time - */ - -{ - Eigen::MatrixXd poly_matrix(3,3); - Eigen::MatrixXd poly_vector(3,1); - - poly_matrix << - pow(mov_time,3), pow(mov_time,4), pow(mov_time,5), - 3*pow(mov_time,2), 4*pow(mov_time,3), 5*pow(mov_time,4), - 6*mov_time, 12*pow(mov_time,2), 20*pow(mov_time,3); - - poly_vector << - pos_end-pos_start-vel_start*mov_time-accel_start*pow(mov_time,2)/2, - vel_end-vel_start-accel_start*mov_time, - accel_end-accel_start ; - - Eigen::Matrix poly_coeff = poly_matrix.inverse() * poly_vector; - - double time_steps = mov_time/smp_time; - int all_time_steps = round(time_steps+1); - - Eigen::MatrixXd time = Eigen::MatrixXd::Zero(all_time_steps,1); - Eigen::MatrixXd minimum_jerk_tra = Eigen::MatrixXd::Zero(all_time_steps,1); - - for (int step=0; step poly_coeff = poly_matrix.inverse() * poly_vector; - - double time_steps = mov_time/smp_time; - int all_time_steps = round(time_steps+1); - - Eigen::MatrixXd time = Eigen::MatrixXd::Zero(all_time_steps,1); - Eigen::MatrixXd minimum_jerk_tra = Eigen::MatrixXd::Zero(all_time_steps,3); - - for (int step=0; step j) - k = i ; - else - k = j ; - - poly_matrix_part2.coeffRef(3*j-3,3*i-3) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),3)/6 ; - poly_matrix_part2.coeffRef(3*j-3,3*i-2) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),4)/24 ; - poly_matrix_part2.coeffRef(3*j-3,3*i-1) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),5)/120 ; - - poly_matrix_part2.coeffRef(3*j-2,3*i-3) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),2)/2 ; - poly_matrix_part2.coeffRef(3*j-2,3*i-2) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),3)/6 ; - poly_matrix_part2.coeffRef(3*j-2,3*i-1) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),4)/24 ; - - poly_matrix_part2.coeffRef(3*j-1,3*i-3) = via_time.coeff(k-1,0)-via_time.coeff(i-1,0) ; - poly_matrix_part2.coeffRef(3*j-1,3*i-2) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),2)/2 ; - poly_matrix_part2.coeffRef(3*j-1,3*i-1) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),3)/6 ; - } - } - - - Eigen::MatrixXd poly_matrix_part3 = Eigen::MatrixXd::Zero(3,3*via_num+3); - - poly_matrix_part3.coeffRef(0,0) = pow(mov_time,3); - poly_matrix_part3.coeffRef(0,1) = pow(mov_time,4); - poly_matrix_part3.coeffRef(0,2) = pow(mov_time,5); - - poly_matrix_part3.coeffRef(1,0) = 3*pow(mov_time,2); - poly_matrix_part3.coeffRef(1,1) = 4*pow(mov_time,3); - poly_matrix_part3.coeffRef(1,2) = 5*pow(mov_time,4); - - poly_matrix_part3.coeffRef(2,0) = 6*mov_time; - poly_matrix_part3.coeffRef(2,1) = 12*pow(mov_time,2); - poly_matrix_part3.coeffRef(2,2) = 20*pow(mov_time,3); - - for (int i=1; i<=via_num; i++) - { - poly_matrix_part3.coeffRef(0,3*i) = pow(mov_time-via_time.coeff(i-1,0),3)/6 ; - poly_matrix_part3.coeffRef(1,3*i) = pow(mov_time-via_time.coeff(i-1,0),2)/2 ; - poly_matrix_part3.coeffRef(2,3*i) = mov_time-via_time.coeff(i-1,0) ; - - poly_matrix_part3.coeffRef(0,3*i+1) = pow(mov_time-via_time.coeff(i-1,0),4)/24 ; - poly_matrix_part3.coeffRef(1,3*i+1) = pow(mov_time-via_time.coeff(i-1,0),3)/6 ; - poly_matrix_part3.coeffRef(2,3*i+1) = pow(mov_time-via_time.coeff(i-1,0),2)/2 ; - - poly_matrix_part3.coeffRef(0,3*i+2) = pow(mov_time-via_time.coeff(i-1,0),5)/120 ; - poly_matrix_part3.coeffRef(1,3*i+2) = pow(mov_time-via_time.coeff(i-1,0),4)/24 ; - poly_matrix_part3.coeffRef(2,3*i+2) = pow(mov_time-via_time.coeff(i-1,0),3)/6 ; - } - - Eigen::MatrixXd poly_matrix = Eigen::MatrixXd::Zero(3*via_num+3,3*via_num+3); - - poly_matrix.block(0,0,3*via_num,3) = poly_matrix_part1 ; - poly_matrix.block(0,3,3*via_num,3*via_num) = poly_matrix_part2 ; - poly_matrix.block(3*via_num,0,3,3*via_num+3) = poly_matrix_part3 ; - - Eigen::MatrixXd poly_coeff(3*via_num+3,1); - //C = A.inverse()*B; - poly_coeff = poly_matrix.colPivHouseholderQr().solve(poly_vector); - - int all_time_steps; - all_time_steps = round(mov_time/smp_time) ; - - Eigen::MatrixXd time_vector = Eigen::MatrixXd::Zero(all_time_steps+1,1); - - for (int i=1; i<=all_time_steps+1; i++) - time_vector.coeffRef(i-1,0) = (i-1)*smp_time; - - Eigen::MatrixXd via_time_vector = Eigen::MatrixXd::Zero(via_num,1); - - for (int i=1; i<=via_num; i++) - via_time_vector.coeffRef(i-1,0) = round(via_time.coeff(i-1,0)/smp_time)+2; - - Eigen::MatrixXd minimum_jerk_tra_with_via_points = Eigen::MatrixXd::Zero(all_time_steps+1,1); - - for (int i=1; i<=all_time_steps+1; i++) - { - minimum_jerk_tra_with_via_points.coeffRef(i-1,0) = - pos_start + - vel_start*time_vector.coeff(i-1,0) + - 0.5*accel_start*pow(time_vector.coeff(i-1,0),2) + - poly_coeff.coeff(0,0)*pow(time_vector.coeff(i-1,0),3) + - poly_coeff.coeff(1,0)*pow(time_vector.coeff(i-1,0),4) + - poly_coeff.coeff(2,0)*pow(time_vector.coeff(i-1,0),5) ; - } - - for (int i=1; i<=via_num; i++) - { - for (int j=via_time_vector.coeff(i-1,0); j<=all_time_steps+1; j++) - { - minimum_jerk_tra_with_via_points.coeffRef(j-1,0) = - minimum_jerk_tra_with_via_points.coeff(j-1,0) + - poly_coeff.coeff(3*i,0)*pow((time_vector.coeff(j-1,0)-via_time.coeff(i-1,0)),3)/6 + - poly_coeff.coeff(3*i+1,0)*pow((time_vector.coeff(j-1,0)-via_time.coeff(i-1,0)),4)/24 + - poly_coeff.coeff(3*i+2,0)*pow((time_vector.coeff(j-1,0)-via_time.coeff(i-1,0)),5)/120 ; - - } - } - - return minimum_jerk_tra_with_via_points; -} - -Eigen::MatrixXd calcArc3dTra(double smp_time, double mov_time, - Eigen::MatrixXd center_point, Eigen::MatrixXd normal_vector, Eigen::MatrixXd start_point, - double rotation_angle, double cross_ratio ) -{ - int all_time_steps = int (round(mov_time/smp_time))+1; - - Eigen::MatrixXd angle_tra = calcMinimumJerkTra(0.0, 0.0, 0.0, - rotation_angle, 0.0, 0.0, - smp_time, mov_time); - - Eigen::MatrixXd arc_tra = Eigen::MatrixXd::Zero(3,all_time_steps); - - for (int step = 0; step < all_time_steps; step++ ) - { - double time = ((double)step)*smp_time; - double theta = angle_tra.coeff(step,0); - - Eigen::MatrixXd weight_wedge(3,3); - - weight_wedge << - 0.0, -normal_vector.coeff(2,0), normal_vector.coeff(1,0), - normal_vector.coeff(2,0), 0.0, -normal_vector.coeff(0,0), - -normal_vector.coeff(1,0), normal_vector.coeff(0,0), 0.0; - - Eigen::MatrixXd identity = Eigen::MatrixXd::Identity(3,3); - Eigen::MatrixXd rotation = identity + weight_wedge*sin(theta) + weight_wedge*weight_wedge*(1-cos(theta)); - double cross = cross_ratio*(1.0-2.0*(abs(0.5-(time/mov_time)))); - - arc_tra.block(0,step,3,1) = (1+cross)*(rotation*(start_point-center_point))+center_point; - } - - Eigen::MatrixXd act_tra_trans = arc_tra.transpose(); - - return act_tra_trans; -} - - - -FifthOrderPolynomialTrajectory::FifthOrderPolynomialTrajectory(double initial_time, double initial_pos, double initial_vel, double initial_acc, - double final_time, double final_pos, double final_vel, double final_acc) -{ - position_coeff_.resize(6, 1); - velocity_coeff_.resize(6, 1); - acceleration_coeff_.resize(6, 1); - time_variables_.resize(1, 6); - - position_coeff_.fill(0); - velocity_coeff_.fill(0); - acceleration_coeff_.fill(0); - time_variables_.fill(0); - - if(final_time > initial_time) - { - initial_time_ = initial_time; - initial_pos_ = initial_pos; - initial_vel_ = initial_vel; - initial_acc_ = initial_acc; - - current_time_ = initial_time; - current_pos_ = initial_pos; - current_vel_ = initial_vel; - current_acc_ = initial_acc; - - final_time_ = final_time; - final_pos_ = final_pos; - final_vel_ = final_vel; - final_acc_ = final_acc; - - Eigen::MatrixXd time_mat; - Eigen::MatrixXd conditions_mat; - - time_mat.resize(6,6); - time_mat << powDI(initial_time_, 5), powDI(initial_time_, 4), powDI(initial_time_, 3), powDI(initial_time_, 2), initial_time_, 1.0, - 5.0*powDI(initial_time_, 4), 4.0*powDI(initial_time_, 3), 3.0*powDI(initial_time_, 2), 2.0*initial_time_, 1.0, 0.0, - 20.0*powDI(initial_time_, 3), 12.0*powDI(initial_time_, 2), 6.0*initial_time_, 2.0, 0.0, 0.0; - powDI(final_time_, 5), powDI(final_time_, 4), powDI(final_time_, 3), powDI(final_time_, 2), final_time_, 1.0, - 5.0*powDI(final_time_, 4), 4.0*powDI(final_time_, 3), 3.0*powDI(final_time_, 2), 2.0*final_time_, 1.0, 0.0, - 20.0*powDI(final_time_, 3), 12.0*powDI(final_time_, 2), 6.0*final_time_, 2.0, 0.0, 0.0; - - conditions_mat.resize(6,1); - conditions_mat << initial_pos_, initial_vel_, initial_acc_, final_pos_, final_vel_, final_acc_; - - position_coeff_ = time_mat.inverse() * conditions_mat; - velocity_coeff_ << 0.0, - 5.0*position_coeff_.coeff(0,0), - 4.0*position_coeff_.coeff(0,1), - 3.0*position_coeff_.coeff(0,2), - 2.0*position_coeff_.coeff(0,3), - 1.0*position_coeff_.coeff(0,4); - acceleration_coeff_ << 0.0, - 0.0, - 20.0*position_coeff_.coeff(0,0), - 12.0*position_coeff_.coeff(0,1), - 6.0*position_coeff_.coeff(0,2), - 2.0*position_coeff_.coeff(0,3); - } - -} - -FifthOrderPolynomialTrajectory::FifthOrderPolynomialTrajectory() -{ - initial_time_ = 0; - initial_pos_ = 0; - initial_vel_ = 0; - initial_acc_ = 0; - - current_time_ = 0; - current_pos_ = 0; - current_vel_ = 0; - current_acc_ = 0; - - final_time_ = 0; - final_pos_ = 0; - final_vel_ = 0; - final_acc_ = 0; - - position_coeff_.resize(6, 1); - velocity_coeff_.resize(6, 1); - acceleration_coeff_.resize(6, 1); - time_variables_.resize(1, 6); - - position_coeff_.fill(0); - velocity_coeff_.fill(0); - acceleration_coeff_.fill(0); - time_variables_.fill(0); -} - -FifthOrderPolynomialTrajectory::~FifthOrderPolynomialTrajectory() -{ - -} - -bool FifthOrderPolynomialTrajectory::changeTrajectory(double final_pos, double final_vel, double final_acc) -{ - final_pos_ = final_pos; - final_vel_ = final_vel; - final_acc_ = final_acc; - - Eigen::MatrixXd time_mat; - Eigen::MatrixXd conditions_mat; - - time_mat.resize(6,6); - time_mat << powDI(initial_time_, 5), powDI(initial_time_, 4), powDI(initial_time_, 3), powDI(initial_time_, 2), initial_time_, 1.0, - 5.0*powDI(initial_time_, 4), 4.0*powDI(initial_time_, 3), 3.0*powDI(initial_time_, 2), 2.0*initial_time_, 1.0, 0.0, - 20.0*powDI(initial_time_, 3), 12.0*powDI(initial_time_, 2), 6.0*initial_time_, 2.0, 0.0, 0.0, - powDI(final_time_, 5), powDI(final_time_, 4), powDI(final_time_, 3), powDI(final_time_, 2), final_time_, 1.0, - 5.0*powDI(final_time_, 4), 4.0*powDI(final_time_, 3), 3.0*powDI(final_time_, 2), 2.0*final_time_, 1.0, 0.0, - 20.0*powDI(final_time_, 3), 12.0*powDI(final_time_, 2), 6.0*final_time_, 2.0, 0.0, 0.0; - - conditions_mat.resize(6,1); - conditions_mat << initial_pos_, initial_vel_, initial_acc_, final_pos_, final_vel_, final_acc_; - - position_coeff_ = time_mat.inverse() * conditions_mat; - velocity_coeff_ << 0.0, - 5.0*position_coeff_.coeff(0,0), - 4.0*position_coeff_.coeff(0,1), - 3.0*position_coeff_.coeff(0,2), - 2.0*position_coeff_.coeff(0,3), - 1.0*position_coeff_.coeff(0,4); - acceleration_coeff_ << 0.0, - 0.0, - 20.0*position_coeff_.coeff(0,0), - 12.0*position_coeff_.coeff(0,1), - 6.0*position_coeff_.coeff(0,2), - 2.0*position_coeff_.coeff(0,3); - - return true; -} - -bool FifthOrderPolynomialTrajectory::changeTrajectory(double final_time, double final_pos, double final_vel, double final_acc) -{ - if(final_time < initial_time_) - return false; - - final_time_ = final_time; - return changeTrajectory(final_pos, final_vel, final_acc); -} - -bool FifthOrderPolynomialTrajectory::changeTrajectory(double initial_time, double initial_pos, double initial_vel, double initial_acc, - double final_time, double final_pos, double final_vel, double final_acc) -{ - if(final_time < initial_time) - return false; - - initial_time_ = initial_time; - initial_pos_ = initial_pos; - initial_vel_ = initial_vel; - initial_acc_ = initial_acc; - - final_time_ = final_time; - - return changeTrajectory(final_pos, final_vel, final_acc); -} - -double FifthOrderPolynomialTrajectory::getPosition(double time) -{ - if(time >= final_time_) - { - current_time_ = final_time_; - current_pos_ = final_pos_; - current_vel_ = final_vel_; - current_acc_ = final_acc_; - return final_pos_; - } - else if(time <= initial_time_ ) - { - current_time_ = initial_time_; - current_pos_ = initial_pos_; - current_vel_ = initial_vel_; - current_acc_ = initial_acc_; - return initial_pos_; - } - else - { - current_time_ = time; - time_variables_ << powDI(time, 5), powDI(time, 4), powDI(time, 3), powDI(time, 2), time, 1.0; - current_pos_ = (time_variables_ * position_coeff_).coeff(0,0); - current_vel_ = (time_variables_ * velocity_coeff_).coeff(0,0); - current_acc_ = (time_variables_ * acceleration_coeff_).coeff(0,0); - return current_pos_; - } -} - -double FifthOrderPolynomialTrajectory::getVelocity(double time) -{ - if(time >= final_time_) - { - current_time_ = final_time_; - current_pos_ = final_pos_; - current_vel_ = final_vel_; - current_acc_ = final_acc_; - return final_vel_; - } - else if(time <= initial_time_ ) - { - current_time_ = initial_time_; - current_pos_ = initial_pos_; - current_vel_ = initial_vel_; - current_acc_ = initial_acc_; - return initial_vel_; - } - else - { - current_time_ = time; - time_variables_ << powDI(time, 5), powDI(time, 4), powDI(time, 3), powDI(time, 2), time, 1.0; - current_pos_ = (time_variables_ * position_coeff_).coeff(0,0); - current_vel_ = (time_variables_ * velocity_coeff_).coeff(0,0); - current_acc_ = (time_variables_ * acceleration_coeff_).coeff(0,0); - return current_vel_; - } -} - -double FifthOrderPolynomialTrajectory::getAcceleration(double time) -{ - if(time >= final_time_) - { - current_time_ = final_time_; - current_pos_ = final_pos_; - current_vel_ = final_vel_; - current_acc_ = final_acc_; - return final_acc_; - } - else if(time <= initial_time_ ) - { - current_time_ = initial_time_; - current_pos_ = initial_pos_; - current_vel_ = initial_vel_; - current_acc_ = initial_acc_; - return initial_acc_; - } - else - { - current_time_ = time; - time_variables_ << powDI(time, 5), powDI(time, 4), powDI(time, 3), powDI(time, 2), time, 1.0; - current_pos_ = (time_variables_ * position_coeff_).coeff(0,0); - current_vel_ = (time_variables_ * velocity_coeff_).coeff(0,0); - current_acc_ = (time_variables_ * acceleration_coeff_).coeff(0,0); - return current_acc_; - } -} - -void FifthOrderPolynomialTrajectory::setTime(double time) -{ - if(time >= final_time_) - { - current_time_ = final_time_; - current_pos_ = final_pos_; - current_vel_ = final_vel_; - current_acc_ = final_acc_; - } - else if(time <= initial_time_ ) - { - current_time_ = initial_time_; - current_pos_ = initial_pos_; - current_vel_ = initial_vel_; - current_acc_ = initial_acc_; - } - else - { - current_time_ = time; - time_variables_ << powDI(time, 5), powDI(time, 4), powDI(time, 3), powDI(time, 2), time, 1.0; - current_pos_ = (time_variables_ * position_coeff_).coeff(0,0); - current_vel_ = (time_variables_ * velocity_coeff_).coeff(0,0); - current_acc_ = (time_variables_ * acceleration_coeff_).coeff(0,0); - } -} - -double FifthOrderPolynomialTrajectory::getPosition() -{ - return current_pos_; -} - -double FifthOrderPolynomialTrajectory::getVelocity() -{ - return current_vel_; -} - -double FifthOrderPolynomialTrajectory::getAcceleration() -{ - return current_acc_; -} - -} From a415fff32f067decd588766053234a542cd6a43f Mon Sep 17 00:00:00 2001 From: ROBOTIS-zerom Date: Fri, 12 Aug 2016 10:47:06 +0900 Subject: [PATCH 052/238] - delete dynamixel_sdk (Use ROBOTIS-GIT/DynamixelSDK repository) --- dynamixel_sdk/CMakeLists.txt | 38 - dynamixel_sdk/include/dynamixel_sdk.h | 59 - .../include/dynamixel_sdk/group_bulk_read.h | 92 -- .../include/dynamixel_sdk/group_bulk_write.h | 87 -- .../include/dynamixel_sdk/group_sync_read.h | 91 -- .../include/dynamixel_sdk/group_sync_write.h | 86 -- .../include/dynamixel_sdk/packet_handler.h | 164 --- .../include/dynamixel_sdk/port_handler.h | 94 -- .../dynamixel_sdk/protocol1_packet_handler.h | 129 -- .../dynamixel_sdk/protocol2_packet_handler.h | 134 --- .../dynamixel_sdk_linux/port_handler_linux.h | 93 -- .../port_handler_windows.h | 93 -- dynamixel_sdk/package.xml | 15 - .../src/dynamixel_sdk/group_bulk_read.cpp | 237 ---- .../src/dynamixel_sdk/group_bulk_write.cpp | 168 --- .../src/dynamixel_sdk/group_sync_read.cpp | 203 ---- .../src/dynamixel_sdk/group_sync_write.cpp | 147 --- .../src/dynamixel_sdk/packet_handler.cpp | 60 - .../src/dynamixel_sdk/port_handler.cpp | 63 - .../protocol1_packet_handler.cpp | 727 ------------ .../protocol2_packet_handler.cpp | 1036 ----------------- .../port_handler_linux.cpp | 276 ----- .../port_handler_windows.cpp | 245 ---- 23 files changed, 4337 deletions(-) delete mode 100644 dynamixel_sdk/CMakeLists.txt delete mode 100644 dynamixel_sdk/include/dynamixel_sdk.h delete mode 100644 dynamixel_sdk/include/dynamixel_sdk/group_bulk_read.h delete mode 100644 dynamixel_sdk/include/dynamixel_sdk/group_bulk_write.h delete mode 100644 dynamixel_sdk/include/dynamixel_sdk/group_sync_read.h delete mode 100644 dynamixel_sdk/include/dynamixel_sdk/group_sync_write.h delete mode 100644 dynamixel_sdk/include/dynamixel_sdk/packet_handler.h delete mode 100644 dynamixel_sdk/include/dynamixel_sdk/port_handler.h delete mode 100644 dynamixel_sdk/include/dynamixel_sdk/protocol1_packet_handler.h delete mode 100644 dynamixel_sdk/include/dynamixel_sdk/protocol2_packet_handler.h delete mode 100644 dynamixel_sdk/include/dynamixel_sdk_linux/port_handler_linux.h delete mode 100644 dynamixel_sdk/include/dynamixel_sdk_windows/port_handler_windows.h delete mode 100644 dynamixel_sdk/package.xml delete mode 100644 dynamixel_sdk/src/dynamixel_sdk/group_bulk_read.cpp delete mode 100644 dynamixel_sdk/src/dynamixel_sdk/group_bulk_write.cpp delete mode 100644 dynamixel_sdk/src/dynamixel_sdk/group_sync_read.cpp delete mode 100644 dynamixel_sdk/src/dynamixel_sdk/group_sync_write.cpp delete mode 100644 dynamixel_sdk/src/dynamixel_sdk/packet_handler.cpp delete mode 100644 dynamixel_sdk/src/dynamixel_sdk/port_handler.cpp delete mode 100644 dynamixel_sdk/src/dynamixel_sdk/protocol1_packet_handler.cpp delete mode 100644 dynamixel_sdk/src/dynamixel_sdk/protocol2_packet_handler.cpp delete mode 100644 dynamixel_sdk/src/dynamixel_sdk_linux/port_handler_linux.cpp delete mode 100644 dynamixel_sdk/src/dynamixel_sdk_windows/port_handler_windows.cpp diff --git a/dynamixel_sdk/CMakeLists.txt b/dynamixel_sdk/CMakeLists.txt deleted file mode 100644 index 658dfd4..0000000 --- a/dynamixel_sdk/CMakeLists.txt +++ /dev/null @@ -1,38 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(dynamixel_sdk) - -find_package(catkin REQUIRED COMPONENTS - roscpp -) - -catkin_package( - INCLUDE_DIRS include - LIBRARIES dynamixel_sdk -# CATKIN_DEPENDS roscpp -# DEPENDS system_lib -) - -# include_directories(include) -include_directories( - include - ${catkin_INCLUDE_DIRS} -) - -## Declare a C++ library -add_library(dynamixel_sdk - src/${PROJECT_NAME}/packet_handler.cpp - src/${PROJECT_NAME}/protocol1_packet_handler.cpp - src/${PROJECT_NAME}/protocol2_packet_handler.cpp - src/${PROJECT_NAME}/group_sync_read.cpp - src/${PROJECT_NAME}/group_sync_write.cpp - src/${PROJECT_NAME}/group_bulk_read.cpp - src/${PROJECT_NAME}/group_bulk_write.cpp - src/${PROJECT_NAME}/port_handler.cpp - src/${PROJECT_NAME}_linux/port_handler_linux.cpp -) - -## Specify libraries to link a library or executable target against -target_link_libraries(dynamixel_sdk - ${catkin_LIBRARIES} -) - diff --git a/dynamixel_sdk/include/dynamixel_sdk.h b/dynamixel_sdk/include/dynamixel_sdk.h deleted file mode 100644 index a0fdf2a..0000000 --- a/dynamixel_sdk/include/dynamixel_sdk.h +++ /dev/null @@ -1,59 +0,0 @@ -/******************************************************************************* -* Copyright (c) 2016, ROBOTIS CO., LTD. -* 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 ROBOTIS 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. -*******************************************************************************/ - -/* Author: zerom, Leon Ryu Woon Jung */ - -/* - * dynamixel_sdk.h - * - * Created on: 2016. 3. 8. - */ - -#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_DYNAMIXELSDK_H_ -#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_DYNAMIXELSDK_H_ - - -#include "dynamixel_sdk/group_bulk_read.h" -#include "dynamixel_sdk/group_bulk_write.h" -#include "dynamixel_sdk/group_sync_read.h" -#include "dynamixel_sdk/group_sync_write.h" -#include "dynamixel_sdk/protocol1_packet_handler.h" -#include "dynamixel_sdk/protocol2_packet_handler.h" - -#ifdef __linux__ - #include "dynamixel_sdk_linux/port_handler_linux.h" -#endif - -#if defined(_WIN32) || defined(_WIN64) - #include "dynamixel_sdk_windows/port_handler_windows.h" -#endif - - -#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_DYNAMIXELSDK_H_ */ diff --git a/dynamixel_sdk/include/dynamixel_sdk/group_bulk_read.h b/dynamixel_sdk/include/dynamixel_sdk/group_bulk_read.h deleted file mode 100644 index 29b0dba..0000000 --- a/dynamixel_sdk/include/dynamixel_sdk/group_bulk_read.h +++ /dev/null @@ -1,92 +0,0 @@ -/******************************************************************************* -* Copyright (c) 2016, ROBOTIS CO., LTD. -* 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 ROBOTIS 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. -*******************************************************************************/ - -/* Author: zerom, Leon Ryu Woon Jung */ - -/* - * group_bulk_read.h - * - * Created on: 2016. 1. 28. - * Author: zerom, leon - */ - -#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPBULKREAD_H_ -#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPBULKREAD_H_ - - -#include -#include -#include "port_handler.h" -#include "packet_handler.h" - -namespace dynamixel -{ - -class WINDECLSPEC GroupBulkRead -{ - private: - PortHandler *port_; - PacketHandler *ph_; - - std::vector id_list_; - std::map address_list_; // - std::map length_list_; // - std::map data_list_; // - - bool last_result_; - bool is_param_changed_; - - uint8_t *param_; - - void makeParam(); - - public: - GroupBulkRead(PortHandler *port, PacketHandler *ph); - ~GroupBulkRead() { clearParam(); } - - PortHandler *getPortHandler() { return port_; } - PacketHandler *getPacketHandler() { return ph_; } - - bool addParam (uint8_t id, uint16_t start_address, uint16_t data_length); - void removeParam (uint8_t id); - void clearParam (); - - int txPacket(); - int rxPacket(); - int txRxPacket(); - - bool isAvailable (uint8_t id, uint16_t address, uint16_t data_length); - uint32_t getData (uint8_t id, uint16_t address, uint16_t data_length); -}; - -} - - -#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPBULKREAD_H_ */ diff --git a/dynamixel_sdk/include/dynamixel_sdk/group_bulk_write.h b/dynamixel_sdk/include/dynamixel_sdk/group_bulk_write.h deleted file mode 100644 index 71bd9ec..0000000 --- a/dynamixel_sdk/include/dynamixel_sdk/group_bulk_write.h +++ /dev/null @@ -1,87 +0,0 @@ -/******************************************************************************* -* Copyright (c) 2016, ROBOTIS CO., LTD. -* 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 ROBOTIS 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. -*******************************************************************************/ - -/* Author: zerom, Leon Ryu Woon Jung */ - -/* - * group_bulk_write.h - * - * Created on: 2016. 2. 2. - */ - -#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPBULKWRITE_H_ -#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPBULKWRITE_H_ - - -#include -#include -#include "port_handler.h" -#include "packet_handler.h" - -namespace dynamixel -{ - -class WINDECLSPEC GroupBulkWrite -{ - private: - PortHandler *port_; - PacketHandler *ph_; - - std::vector id_list_; - std::map address_list_; // - std::map length_list_; // - std::map data_list_; // - - bool is_param_changed_; - - uint8_t *param_; - uint16_t param_length_; - - void makeParam(); - - public: - GroupBulkWrite(PortHandler *port, PacketHandler *ph); - ~GroupBulkWrite() { clearParam(); } - - PortHandler *getPortHandler() { return port_; } - PacketHandler *getPacketHandler() { return ph_; } - - bool addParam (uint8_t id, uint16_t start_address, uint16_t data_length, uint8_t *data); - void removeParam (uint8_t id); - bool changeParam (uint8_t id, uint16_t start_address, uint16_t data_length, uint8_t *data); - void clearParam (); - - int txPacket(); -}; - -} - - -#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPBULKWRITE_H_ */ diff --git a/dynamixel_sdk/include/dynamixel_sdk/group_sync_read.h b/dynamixel_sdk/include/dynamixel_sdk/group_sync_read.h deleted file mode 100644 index ec2a208..0000000 --- a/dynamixel_sdk/include/dynamixel_sdk/group_sync_read.h +++ /dev/null @@ -1,91 +0,0 @@ -/******************************************************************************* -* Copyright (c) 2016, ROBOTIS CO., LTD. -* 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 ROBOTIS 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. -*******************************************************************************/ - -/* Author: zerom, Leon Ryu Woon Jung */ - -/* - * group_sync_read.h - * - * Created on: 2016. 2. 2. - */ - -#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPSYNCREAD_H_ -#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPSYNCREAD_H_ - - -#include -#include -#include "port_handler.h" -#include "packet_handler.h" - -namespace dynamixel -{ - -class WINDECLSPEC GroupSyncRead -{ - private: - PortHandler *port_; - PacketHandler *ph_; - - std::vector id_list_; - std::map data_list_; // - - bool last_result_; - bool is_param_changed_; - - uint8_t *param_; - uint16_t start_address_; - uint16_t data_length_; - - void makeParam(); - - public: - GroupSyncRead(PortHandler *port, PacketHandler *ph, uint16_t start_address, uint16_t data_length); - ~GroupSyncRead() { clearParam(); } - - PortHandler *getPortHandler() { return port_; } - PacketHandler *getPacketHandler() { return ph_; } - - bool addParam (uint8_t id); - void removeParam (uint8_t id); - void clearParam (); - - int txPacket(); - int rxPacket(); - int txRxPacket(); - - bool isAvailable (uint8_t id, uint16_t address, uint16_t data_length); - uint32_t getData (uint8_t id, uint16_t address, uint16_t data_length); -}; - -} - - -#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPSYNCREAD_H_ */ diff --git a/dynamixel_sdk/include/dynamixel_sdk/group_sync_write.h b/dynamixel_sdk/include/dynamixel_sdk/group_sync_write.h deleted file mode 100644 index 1c85666..0000000 --- a/dynamixel_sdk/include/dynamixel_sdk/group_sync_write.h +++ /dev/null @@ -1,86 +0,0 @@ -/******************************************************************************* -* Copyright (c) 2016, ROBOTIS CO., LTD. -* 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 ROBOTIS 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. -*******************************************************************************/ - -/* Author: zerom, Leon Ryu Woon Jung */ - -/* - * group_sync_write.h - * - * Created on: 2016. 1. 28. - */ - -#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPSYNCWRITE_H_ -#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPSYNCWRITE_H_ - - -#include -#include -#include "port_handler.h" -#include "packet_handler.h" - -namespace dynamixel -{ - -class WINDECLSPEC GroupSyncWrite -{ - private: - PortHandler *port_; - PacketHandler *ph_; - - std::vector id_list_; - std::map data_list_; // - - bool is_param_changed_; - - uint8_t *param_; - uint16_t start_address_; - uint16_t data_length_; - - void makeParam(); - - public: - GroupSyncWrite(PortHandler *port, PacketHandler *ph, uint16_t start_address, uint16_t data_length); - ~GroupSyncWrite() { clearParam(); } - - PortHandler *getPortHandler() { return port_; } - PacketHandler *getPacketHandler() { return ph_; } - - bool addParam (uint8_t id, uint8_t *data); - void removeParam (uint8_t id); - bool changeParam (uint8_t id, uint8_t *data); - void clearParam (); - - int txPacket(); -}; - -} - - -#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPSYNCWRITE_H_ */ diff --git a/dynamixel_sdk/include/dynamixel_sdk/packet_handler.h b/dynamixel_sdk/include/dynamixel_sdk/packet_handler.h deleted file mode 100644 index 3816cbb..0000000 --- a/dynamixel_sdk/include/dynamixel_sdk/packet_handler.h +++ /dev/null @@ -1,164 +0,0 @@ -/******************************************************************************* -* Copyright (c) 2016, ROBOTIS CO., LTD. -* 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 ROBOTIS 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. -*******************************************************************************/ - -/* Author: zerom, Leon Ryu Woon Jung */ - -/* - * packet_handler.h - * - * Created on: 2016. 1. 26. - */ - -#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PACKETHANDLER_H_ -#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PACKETHANDLER_H_ - - -#include -#include -#include "port_handler.h" - -#define BROADCAST_ID 0xFE // 254 -#define MAX_ID 0xFC // 252 - -/* Macro for Control Table Value */ -#define DXL_MAKEWORD(a, b) ((unsigned short)(((unsigned char)(((unsigned long)(a)) & 0xff)) | ((unsigned short)((unsigned char)(((unsigned long)(b)) & 0xff))) << 8)) -#define DXL_MAKEDWORD(a, b) ((unsigned int)(((unsigned short)(((unsigned long)(a)) & 0xffff)) | ((unsigned int)((unsigned short)(((unsigned long)(b)) & 0xffff))) << 16)) -#define DXL_LOWORD(l) ((unsigned short)(((unsigned long)(l)) & 0xffff)) -#define DXL_HIWORD(l) ((unsigned short)((((unsigned long)(l)) >> 16) & 0xffff)) -#define DXL_LOBYTE(w) ((unsigned char)(((unsigned long)(w)) & 0xff)) -#define DXL_HIBYTE(w) ((unsigned char)((((unsigned long)(w)) >> 8) & 0xff)) - -/* Instruction for DXL Protocol */ -#define INST_PING 1 -#define INST_READ 2 -#define INST_WRITE 3 -#define INST_REG_WRITE 4 -#define INST_ACTION 5 -#define INST_FACTORY_RESET 6 -#define INST_SYNC_WRITE 131 // 0x83 -#define INST_BULK_READ 146 // 0x92 -// --- Only for 2.0 --- // -#define INST_REBOOT 8 -#define INST_STATUS 85 // 0x55 -#define INST_SYNC_READ 130 // 0x82 -#define INST_BULK_WRITE 147 // 0x93 - -// Communication Result -#define COMM_SUCCESS 0 // tx or rx packet communication success -#define COMM_PORT_BUSY -1000 // Port is busy (in use) -#define COMM_TX_FAIL -1001 // Failed transmit instruction packet -#define COMM_RX_FAIL -1002 // Failed get status packet -#define COMM_TX_ERROR -2000 // Incorrect instruction packet -#define COMM_RX_WAITING -3000 // Now recieving status packet -#define COMM_RX_TIMEOUT -3001 // There is no status packet -#define COMM_RX_CORRUPT -3002 // Incorrect status packet -#define COMM_NOT_AVAILABLE -9000 // - -namespace dynamixel -{ - -class WINDECLSPEC PacketHandler -{ - protected: - PacketHandler() { } - - public: - static PacketHandler *getPacketHandler(float protocol_version = 2.0); - - virtual ~PacketHandler() { } - - virtual float getProtocolVersion() = 0; - - virtual void printTxRxResult(int result) = 0; - virtual void printRxPacketError(uint8_t error) = 0; - - virtual int txPacket (PortHandler *port, uint8_t *txpacket) = 0; - virtual int rxPacket (PortHandler *port, uint8_t *rxpacket) = 0; - virtual int txRxPacket (PortHandler *port, uint8_t *txpacket, uint8_t *rxpacket, uint8_t *error = 0) = 0; - - virtual int ping (PortHandler *port, uint8_t id, uint8_t *error = 0) = 0; - virtual int ping (PortHandler *port, uint8_t id, uint16_t *model_number, uint8_t *error = 0) = 0; - - // broadcastPing - virtual int broadcastPing (PortHandler *port, std::vector &id_list) = 0; - - virtual int action (PortHandler *port, uint8_t id) = 0; - virtual int reboot (PortHandler *port, uint8_t id, uint8_t *error = 0) = 0; - virtual int factoryReset (PortHandler *port, uint8_t id, uint8_t option = 0, uint8_t *error = 0) = 0; - - - virtual int readTx (PortHandler *port, uint8_t id, uint16_t address, uint16_t length) = 0; - virtual int readRx (PortHandler *port, uint16_t length, uint8_t *data, uint8_t *error = 0) = 0; - virtual int readTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0) = 0; - - virtual int read1ByteTx (PortHandler *port, uint8_t id, uint16_t address) = 0; - virtual int read1ByteRx (PortHandler *port, uint8_t *data, uint8_t *error = 0) = 0; - virtual int read1ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint8_t *data, uint8_t *error = 0) = 0; - - virtual int read2ByteTx (PortHandler *port, uint8_t id, uint16_t address) = 0; - virtual int read2ByteRx (PortHandler *port, uint16_t *data, uint8_t *error = 0) = 0; - virtual int read2ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t *data, uint8_t *error = 0) = 0; - - virtual int read4ByteTx (PortHandler *port, uint8_t id, uint16_t address) = 0; - virtual int read4ByteRx (PortHandler *port, uint32_t *data, uint8_t *error = 0) = 0; - virtual int read4ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint32_t *data, uint8_t *error = 0) = 0; - - virtual int writeTxOnly (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data) = 0; - virtual int writeTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0) = 0; - - virtual int write1ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint8_t data) = 0; - virtual int write1ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint8_t data, uint8_t *error = 0) = 0; - - virtual int write2ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint16_t data) = 0; - virtual int write2ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t data, uint8_t *error = 0) = 0; - - virtual int write4ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint32_t data) = 0; - virtual int write4ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint32_t data, uint8_t *error = 0) = 0; - - virtual int regWriteTxOnly (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data) = 0; - virtual int regWriteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0) = 0; - - virtual int syncReadTx (PortHandler *port, uint16_t start_address, uint16_t data_length, uint8_t *param, uint16_t param_length) = 0; - // SyncReadRx -> GroupSyncRead class - // SyncReadTxRx -> GroupSyncRead class - - virtual int syncWriteTxOnly (PortHandler *port, uint16_t start_address, uint16_t data_length, uint8_t *param, uint16_t param_length) = 0; - - virtual int bulkReadTx (PortHandler *port, uint8_t *param, uint16_t param_length) = 0; - // BulkReadRx -> GroupBulkRead class - // BulkReadTxRx -> GroupBulkRead class - - virtual int bulkWriteTxOnly (PortHandler *port, uint8_t *param, uint16_t param_length) = 0; -}; - -} - - -#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PACKETHANDLER_H_ */ diff --git a/dynamixel_sdk/include/dynamixel_sdk/port_handler.h b/dynamixel_sdk/include/dynamixel_sdk/port_handler.h deleted file mode 100644 index dfc83fd..0000000 --- a/dynamixel_sdk/include/dynamixel_sdk/port_handler.h +++ /dev/null @@ -1,94 +0,0 @@ -/******************************************************************************* -* Copyright (c) 2016, ROBOTIS CO., LTD. -* 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 ROBOTIS 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. -*******************************************************************************/ - -/* Author: zerom, Leon Ryu Woon Jung */ - -/* - * port_handler.h - * - * Created on: 2016. 1. 26. - */ - -#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PORTHANDLER_H_ -#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PORTHANDLER_H_ - -#undef __linux__ -#define __linux__ - -#ifdef __linux__ -#define WINDECLSPEC -#elif defined(_WIN32) || defined(_WIN64) -#ifdef WINDLLEXPORT -#define WINDECLSPEC __declspec(dllexport) -#else -#define WINDECLSPEC __declspec(dllimport) -#endif -#endif - -#include - -namespace dynamixel -{ - -class WINDECLSPEC PortHandler -{ - public: - static const int DEFAULT_BAUDRATE_ = 1000000; - - static PortHandler *getPortHandler(const char *port_name); - - bool is_using_; - - virtual ~PortHandler() { } - - virtual bool openPort() = 0; - virtual void closePort() = 0; - virtual void clearPort() = 0; - - virtual void setPortName(const char* port_name) = 0; - virtual char *getPortName() = 0; - - virtual bool setBaudRate(const int baudrate) = 0; - virtual int getBaudRate() = 0; - - virtual int getBytesAvailable() = 0; - - virtual int readPort(uint8_t *packet, int length) = 0; - virtual int writePort(uint8_t *packet, int length) = 0; - - virtual void setPacketTimeout(uint16_t packet_length) = 0; - virtual void setPacketTimeout(double msec) = 0; - virtual bool isPacketTimeout() = 0; -}; - -} - - -#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PORTHANDLER_H_ */ diff --git a/dynamixel_sdk/include/dynamixel_sdk/protocol1_packet_handler.h b/dynamixel_sdk/include/dynamixel_sdk/protocol1_packet_handler.h deleted file mode 100644 index 758556f..0000000 --- a/dynamixel_sdk/include/dynamixel_sdk/protocol1_packet_handler.h +++ /dev/null @@ -1,129 +0,0 @@ -/******************************************************************************* -* Copyright (c) 2016, ROBOTIS CO., LTD. -* 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 ROBOTIS 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. -*******************************************************************************/ - -/* Author: zerom, Leon Ryu Woon Jung */ - -/* - * protocol1_packet_handler.h - * - * Created on: 2016. 1. 26. - */ - -#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL1PACKETHANDLER_H_ -#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL1PACKETHANDLER_H_ - - -#include "packet_handler.h" - -namespace dynamixel -{ - -class WINDECLSPEC Protocol1PacketHandler : public PacketHandler -{ - private: - static Protocol1PacketHandler *unique_instance_; - - Protocol1PacketHandler(); - - public: - static Protocol1PacketHandler *getInstance() { return unique_instance_; } - - virtual ~Protocol1PacketHandler() { } - - float getProtocolVersion() { return 1.0; } - - void printTxRxResult(int result); - void printRxPacketError(uint8_t error); - - int txPacket (PortHandler *port, uint8_t *txpacket); - int rxPacket (PortHandler *port, uint8_t *rxpacket); - int txRxPacket (PortHandler *port, uint8_t *txpacket, uint8_t *rxpacket, uint8_t *error = 0); - - int ping (PortHandler *port, uint8_t id, uint8_t *error = 0); - int ping (PortHandler *port, uint8_t id, uint16_t *model_number, uint8_t *error = 0); - - // broadcastPing - int broadcastPing (PortHandler *port, std::vector &id_list); - - int action (PortHandler *port, uint8_t id); - int reboot (PortHandler *port, uint8_t id, uint8_t *error = 0); - int factoryReset (PortHandler *port, uint8_t id, uint8_t option, uint8_t *error = 0); - - - int readTx (PortHandler *port, uint8_t id, uint16_t address, uint16_t length); - int readRx (PortHandler *port, uint16_t length, uint8_t *data, uint8_t *error = 0); - int readTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0); - - int read1ByteTx (PortHandler *port, uint8_t id, uint16_t address); - int read1ByteRx (PortHandler *port, uint8_t *data, uint8_t *error = 0); - int read1ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint8_t *data, uint8_t *error = 0); - - int read2ByteTx (PortHandler *port, uint8_t id, uint16_t address); - int read2ByteRx (PortHandler *port, uint16_t *data, uint8_t *error = 0); - int read2ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t *data, uint8_t *error = 0); - - int read4ByteTx (PortHandler *port, uint8_t id, uint16_t address); - int read4ByteRx (PortHandler *port, uint32_t *data, uint8_t *error = 0); - int read4ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint32_t *data, uint8_t *error = 0); - - int writeTxOnly (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data); - int writeTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0); - - int write1ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint8_t data); - int write1ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint8_t data, uint8_t *error = 0); - - int write2ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint16_t data); - int write2ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t data, uint8_t *error = 0); - - int write4ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint32_t data); - int write4ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint32_t data, uint8_t *error = 0); - - int regWriteTxOnly (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data); - int regWriteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0); - - int syncReadTx (PortHandler *port, uint16_t start_address, uint16_t data_length, uint8_t *param, uint16_t param_length); - // SyncReadRx -> GroupSyncRead class - // SyncReadTxRx -> GroupSyncRead class - - // param : ID1 DATA0 DATA1 ... DATAn ID2 DATA0 DATA1 ... DATAn ID3 DATA0 DATA1 ... DATAn - int syncWriteTxOnly (PortHandler *port, uint16_t start_address, uint16_t data_length, uint8_t *param, uint16_t param_length); - - // param : LEN1 ID1 ADDR1 LEN2 ID2 ADDR2 ... - int bulkReadTx (PortHandler *port, uint8_t *param, uint16_t param_length); - // BulkReadRx -> GroupBulkRead class - // BulkReadTxRx -> GroupBulkRead class - - int bulkWriteTxOnly (PortHandler *port, uint8_t *param, uint16_t param_length); -}; - -} - - -#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL1PACKETHANDLER_H_ */ diff --git a/dynamixel_sdk/include/dynamixel_sdk/protocol2_packet_handler.h b/dynamixel_sdk/include/dynamixel_sdk/protocol2_packet_handler.h deleted file mode 100644 index ff7e2d1..0000000 --- a/dynamixel_sdk/include/dynamixel_sdk/protocol2_packet_handler.h +++ /dev/null @@ -1,134 +0,0 @@ -/******************************************************************************* -* Copyright (c) 2016, ROBOTIS CO., LTD. -* 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 ROBOTIS 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. -*******************************************************************************/ - -/* Author: zerom, Leon Ryu Woon Jung */ - -/* - * protocol2_packet_handler.h - * - * Created on: 2016. 1. 26. - */ - -#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL2PACKETHANDLER_H_ -#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL2PACKETHANDLER_H_ - - -#include "packet_handler.h" - -namespace dynamixel -{ - -class WINDECLSPEC Protocol2PacketHandler : public PacketHandler -{ - private: - static Protocol2PacketHandler *unique_instance_; - - Protocol2PacketHandler(); - - uint16_t updateCRC(uint16_t crc_accum, uint8_t *data_blk_ptr, uint16_t data_blk_size); - void addStuffing(uint8_t *packet); - void removeStuffing(uint8_t *packet); - - public: - static Protocol2PacketHandler *getInstance() { return unique_instance_; } - - virtual ~Protocol2PacketHandler() { } - - float getProtocolVersion() { return 2.0; } - - void printTxRxResult(int result); - void printRxPacketError(uint8_t error); - - int txPacket (PortHandler *port, uint8_t *txpacket); - int rxPacket (PortHandler *port, uint8_t *rxpacket); - int txRxPacket (PortHandler *port, uint8_t *txpacket, uint8_t *rxpacket, uint8_t *error = 0); - - int ping (PortHandler *port, uint8_t id, uint8_t *error = 0); - int ping (PortHandler *port, uint8_t id, uint16_t *model_number, uint8_t *error = 0); - - // broadcastPing - int broadcastPing (PortHandler *port, std::vector &id_list); - - int action (PortHandler *port, uint8_t id); - int reboot (PortHandler *port, uint8_t id, uint8_t *error = 0); - int factoryReset (PortHandler *port, uint8_t id, uint8_t option, uint8_t *error = 0); - - - int readTx (PortHandler *port, uint8_t id, uint16_t address, uint16_t length); - int readRx (PortHandler *port, uint16_t length, uint8_t *data, uint8_t *error = 0); - int readTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0); - - int read1ByteTx (PortHandler *port, uint8_t id, uint16_t address); - int read1ByteRx (PortHandler *port, uint8_t *data, uint8_t *error = 0); - int read1ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint8_t *data, uint8_t *error = 0); - - int read2ByteTx (PortHandler *port, uint8_t id, uint16_t address); - int read2ByteRx (PortHandler *port, uint16_t *data, uint8_t *error = 0); - int read2ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t *data, uint8_t *error = 0); - - int read4ByteTx (PortHandler *port, uint8_t id, uint16_t address); - int read4ByteRx (PortHandler *port, uint32_t *data, uint8_t *error = 0); - int read4ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint32_t *data, uint8_t *error = 0); - - int writeTxOnly (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data); - int writeTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0); - - int write1ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint8_t data); - int write1ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint8_t data, uint8_t *error = 0); - - int write2ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint16_t data); - int write2ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t data, uint8_t *error = 0); - - int write4ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint32_t data); - int write4ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint32_t data, uint8_t *error = 0); - - int regWriteTxOnly (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data); - int regWriteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0); - - int syncReadTx (PortHandler *port, uint16_t start_address, uint16_t data_length, uint8_t *param, uint16_t param_length); - // SyncReadRx -> GroupSyncRead class - // SyncReadTxRx -> GroupSyncRead class - - // param : ID1 DATA0 DATA1 ... DATAn ID2 DATA0 DATA1 ... DATAn ID3 DATA0 DATA1 ... DATAn - int syncWriteTxOnly (PortHandler *port, uint16_t start_address, uint16_t data_length, uint8_t *param, uint16_t param_length); - - // param : ID1 ADDR_L1 ADDR_H1 LEN_L1 LEN_H1 ID2 ADDR_L2 ADDR_H2 LEN_L2 LEN_H2 ... - int bulkReadTx (PortHandler *port, uint8_t *param, uint16_t param_length); - // BulkReadRx -> GroupBulkRead class - // BulkReadTxRx -> GroupBulkRead class - - // param : ID1 START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H DATA0 DATA1 ... DATAn ID2 START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H DATA0 DATA1 ... DATAn - int bulkWriteTxOnly (PortHandler *port, uint8_t *param, uint16_t param_length); -}; - -} - - -#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL2PACKETHANDLER_H_ */ diff --git a/dynamixel_sdk/include/dynamixel_sdk_linux/port_handler_linux.h b/dynamixel_sdk/include/dynamixel_sdk_linux/port_handler_linux.h deleted file mode 100644 index 3a9ed24..0000000 --- a/dynamixel_sdk/include/dynamixel_sdk_linux/port_handler_linux.h +++ /dev/null @@ -1,93 +0,0 @@ -/******************************************************************************* -* Copyright (c) 2016, ROBOTIS CO., LTD. -* 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 ROBOTIS 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. -*******************************************************************************/ - -/* Author: zerom, Leon Ryu Woon Jung */ - -/* - * port_handler_linux.h - * - * Created on: 2016. 1. 26. - */ - -#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_LINUX_PORTHANDLERLINUX_H_ -#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_LINUX_PORTHANDLERLINUX_H_ - - -#include "dynamixel_sdk/port_handler.h" - -namespace dynamixel -{ - -class PortHandlerLinux : public PortHandler -{ - private: - int socket_fd_; - int baudrate_; - char port_name_[30]; - - double packet_start_time_; - double packet_timeout_; - double tx_time_per_byte; - - bool setupPort(const int cflag_baud); - bool setCustomBaudrate(int speed); - int getCFlagBaud(const int baudrate); - - double getCurrentTime(); - double getTimeSinceStart(); - - public: - PortHandlerLinux(const char *port_name); - virtual ~PortHandlerLinux() { closePort(); } - - bool openPort(); - void closePort(); - void clearPort(); - - void setPortName(const char *port_name); - char *getPortName(); - - bool setBaudRate(const int baudrate); - int getBaudRate(); - - int getBytesAvailable(); - - int readPort(uint8_t *packet, int length); - int writePort(uint8_t *packet, int length); - - void setPacketTimeout(uint16_t packet_length); - void setPacketTimeout(double msec); - bool isPacketTimeout(); -}; - -} - - -#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_LINUX_PORTHANDLERLINUX_H_ */ diff --git a/dynamixel_sdk/include/dynamixel_sdk_windows/port_handler_windows.h b/dynamixel_sdk/include/dynamixel_sdk_windows/port_handler_windows.h deleted file mode 100644 index 09583cb..0000000 --- a/dynamixel_sdk/include/dynamixel_sdk_windows/port_handler_windows.h +++ /dev/null @@ -1,93 +0,0 @@ -/******************************************************************************* -* Copyright (c) 2016, ROBOTIS CO., LTD. -* 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 ROBOTIS 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. -*******************************************************************************/ - -/* Author: Leon Ryu Woon Jung */ - -/* -* port_handler_windows.h -* -* Created on: 2016. 4. 26. -*/ - -#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_WINDOWS_PORTHANDLERWINDOWS_H_ -#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_WINDOWS_PORTHANDLERWINDOWS_H_ - -#include - -#include "dynamixel_sdk/port_handler.h" - -namespace dynamixel -{ -class WINDECLSPEC PortHandlerWindows : public PortHandler -{ - private: - HANDLE serial_handle_; - LARGE_INTEGER freq_, counter_; - - int baudrate_; - char port_name_[30]; - - double packet_start_time_; - double packet_timeout_; - double tx_time_per_byte_; - - bool setupPort(const int baudrate); - - double getCurrentTime(); - double getTimeSinceStart(); - - public: - PortHandlerWindows(const char *port_name); - virtual ~PortHandlerWindows() { closePort(); } - - bool openPort(); - void closePort(); - void clearPort(); - - void setPortName(const char *port_name); - char *getPortName(); - - bool setBaudRate(const int baudrate); - int getBaudRate(); - - int getBytesAvailable(); - - int readPort(uint8_t *packet, int length); - int writePort(uint8_t *packet, int length); - - void setPacketTimeout(uint16_t packet_length); - void setPacketTimeout(double msec); - bool isPacketTimeout(); -}; - -} - - -#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_LINUX_PORTHANDLERWINDOWS_H_ */ diff --git a/dynamixel_sdk/package.xml b/dynamixel_sdk/package.xml deleted file mode 100644 index ae8e683..0000000 --- a/dynamixel_sdk/package.xml +++ /dev/null @@ -1,15 +0,0 @@ - - - dynamixel_sdk - 0.1.0 - The dynamixel_sdk package - - robotis - BSD - - ROBOTIS - - catkin - roscpp - roscpp - \ No newline at end of file diff --git a/dynamixel_sdk/src/dynamixel_sdk/group_bulk_read.cpp b/dynamixel_sdk/src/dynamixel_sdk/group_bulk_read.cpp deleted file mode 100644 index 7ae113b..0000000 --- a/dynamixel_sdk/src/dynamixel_sdk/group_bulk_read.cpp +++ /dev/null @@ -1,237 +0,0 @@ -/******************************************************************************* -* Copyright (c) 2016, ROBOTIS CO., LTD. -* 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 ROBOTIS 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. -*******************************************************************************/ - -/* Author: zerom, Leon Ryu Woon Jung */ - -/* - * group_bulk_read.cpp - * - * Created on: 2016. 1. 28. - */ -#if defined(_WIN32) || defined(_WIN64) -#define WINDLLEXPORT -#endif - -#include -#include -#include "dynamixel_sdk/group_bulk_read.h" - -using namespace dynamixel; - -GroupBulkRead::GroupBulkRead(PortHandler *port, PacketHandler *ph) - : port_(port), - ph_(ph), - last_result_(false), - is_param_changed_(false), - param_(0) -{ - clearParam(); -} - -void GroupBulkRead::makeParam() -{ - if (id_list_.size() == 0) - return; - - if (param_ != 0) - delete[] param_; - param_ = 0; - - if (ph_->getProtocolVersion() == 1.0) - { - param_ = new uint8_t[id_list_.size() * 3]; // ID(1) + ADDR(1) + LENGTH(1) - } - else // 2.0 - { - param_ = new uint8_t[id_list_.size() * 5]; // ID(1) + ADDR(2) + LENGTH(2) - } - - int idx = 0; - for (unsigned int i = 0; i < id_list_.size(); i++) - { - uint8_t id = id_list_[i]; - if (ph_->getProtocolVersion() == 1.0) - { - param_[idx++] = (uint8_t)length_list_[id]; // LEN - param_[idx++] = id; // ID - param_[idx++] = (uint8_t)address_list_[id]; // ADDR - } - else // 2.0 - { - param_[idx++] = id; // ID - param_[idx++] = DXL_LOBYTE(address_list_[id]); // ADDR_L - param_[idx++] = DXL_HIBYTE(address_list_[id]); // ADDR_H - param_[idx++] = DXL_LOBYTE(length_list_[id]); // LEN_L - param_[idx++] = DXL_HIBYTE(length_list_[id]); // LEN_H - } - } -} - -bool GroupBulkRead::addParam(uint8_t id, uint16_t start_address, uint16_t data_length) -{ - if (std::find(id_list_.begin(), id_list_.end(), id) != id_list_.end()) // id already exist - return false; - - id_list_.push_back(id); - length_list_[id] = data_length; - address_list_[id] = start_address; - data_list_[id] = new uint8_t[data_length]; - - is_param_changed_ = true; - return true; -} - -void GroupBulkRead::removeParam(uint8_t id) -{ - std::vector::iterator it = std::find(id_list_.begin(), id_list_.end(), id); - if (it == id_list_.end()) // NOT exist - return; - - id_list_.erase(it); - address_list_.erase(id); - length_list_.erase(id); - delete[] data_list_[id]; - data_list_.erase(id); - - is_param_changed_ = true; -} - -void GroupBulkRead::clearParam() -{ - if (id_list_.size() == 0) - return; - - for (unsigned int i = 0; i < id_list_.size(); i++) - delete[] data_list_[id_list_[i]]; - - id_list_.clear(); - address_list_.clear(); - length_list_.clear(); - data_list_.clear(); - if (param_ != 0) - delete[] param_; - param_ = 0; -} - -int GroupBulkRead::txPacket() -{ - if (id_list_.size() == 0) - return COMM_NOT_AVAILABLE; - - if (is_param_changed_ == true) - makeParam(); - - if (ph_->getProtocolVersion() == 1.0) - { - return ph_->bulkReadTx(port_, param_, id_list_.size() * 3); - } - else // 2.0 - { - return ph_->bulkReadTx(port_, param_, id_list_.size() * 5); - } -} - -int GroupBulkRead::rxPacket() -{ - int cnt = id_list_.size(); - int result = COMM_RX_FAIL; - - last_result_ = false; - - if (cnt == 0) - return COMM_NOT_AVAILABLE; - - for (int i = 0; i < cnt; i++) - { - uint8_t id = id_list_[i]; - - result = ph_->readRx(port_, length_list_[id], data_list_[id]); - if (result != COMM_SUCCESS) - { - fprintf(stderr, "[GroupBulkRead::rxPacket] ID %d result : %d !!!!!!!!!!\n", id, result); - return result; - } - } - - if (result == COMM_SUCCESS) - last_result_ = true; - - return result; -} - -int GroupBulkRead::txRxPacket() -{ - int result = COMM_TX_FAIL; - - result = txPacket(); - if (result != COMM_SUCCESS) - return result; - - return rxPacket(); -} - -bool GroupBulkRead::isAvailable(uint8_t id, uint16_t address, uint16_t data_length) -{ - uint16_t start_addr; - - if (last_result_ == false || data_list_.find(id) == data_list_.end()) - return false; - - start_addr = address_list_[id]; - - if (address < start_addr || start_addr + length_list_[id] - data_length < address) - return false; - - return true; -} - -uint32_t GroupBulkRead::getData(uint8_t id, uint16_t address, uint16_t data_length) -{ - if (isAvailable(id, address, data_length) == false) - return 0; - - uint16_t start_addr = address_list_[id]; - - switch(data_length) - { - case 1: - return data_list_[id][address - start_addr]; - - case 2: - return DXL_MAKEWORD(data_list_[id][address - start_addr], data_list_[id][address - start_addr + 1]); - - case 4: - return DXL_MAKEDWORD(DXL_MAKEWORD(data_list_[id][address - start_addr + 0], data_list_[id][address - start_addr + 1]), - DXL_MAKEWORD(data_list_[id][address - start_addr + 2], data_list_[id][address - start_addr + 3])); - - default: - return 0; - } -} diff --git a/dynamixel_sdk/src/dynamixel_sdk/group_bulk_write.cpp b/dynamixel_sdk/src/dynamixel_sdk/group_bulk_write.cpp deleted file mode 100644 index 7b84151..0000000 --- a/dynamixel_sdk/src/dynamixel_sdk/group_bulk_write.cpp +++ /dev/null @@ -1,168 +0,0 @@ -/******************************************************************************* -* Copyright (c) 2016, ROBOTIS CO., LTD. -* 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 ROBOTIS 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. -*******************************************************************************/ - -/* Author: zerom, Leon Ryu Woon Jung */ - -/* - * group_bulk_write.cpp - * - * Created on: 2016. 2. 2. - */ -#if defined(_WIN32) || defined(_WIN64) -#define WINDLLEXPORT -#endif - -#include -#include "dynamixel_sdk/group_bulk_write.h" - -using namespace dynamixel; - -GroupBulkWrite::GroupBulkWrite(PortHandler *port, PacketHandler *ph) - : port_(port), - ph_(ph), - is_param_changed_(false), - param_(0), - param_length_(0) -{ - clearParam(); -} - -void GroupBulkWrite::makeParam() -{ - if (ph_->getProtocolVersion() == 1.0 || id_list_.size() == 0) - return; - - if (param_ != 0) - delete[] param_; - param_ = 0; - - param_length_ = 0; - for (unsigned int i = 0; i < id_list_.size(); i++) - param_length_ += 1 + 2 + 2 + length_list_[id_list_[i]]; - - param_ = new uint8_t[param_length_]; - - int idx = 0; - for (unsigned int i = 0; i < id_list_.size(); i++) - { - uint8_t id = id_list_[i]; - if (data_list_[id] == 0) - return; - - param_[idx++] = id; - param_[idx++] = DXL_LOBYTE(address_list_[id]); - param_[idx++] = DXL_HIBYTE(address_list_[id]); - param_[idx++] = DXL_LOBYTE(length_list_[id]); - param_[idx++] = DXL_HIBYTE(length_list_[id]); - for (int c = 0; c < length_list_[id]; c++) - param_[idx++] = (data_list_[id])[c]; - } -} - -bool GroupBulkWrite::addParam(uint8_t id, uint16_t start_address, uint16_t data_length, uint8_t *data) -{ - if (ph_->getProtocolVersion() == 1.0) - return false; - - if (std::find(id_list_.begin(), id_list_.end(), id) != id_list_.end()) // id already exist - return false; - - id_list_.push_back(id); - address_list_[id] = start_address; - length_list_[id] = data_length; - data_list_[id] = new uint8_t[data_length]; - for (int c = 0; c < data_length; c++) - data_list_[id][c] = data[c]; - - is_param_changed_ = true; - return true; -} -void GroupBulkWrite::removeParam(uint8_t id) -{ - if (ph_->getProtocolVersion() == 1.0) - return; - - std::vector::iterator it = std::find(id_list_.begin(), id_list_.end(), id); - if (it == id_list_.end()) // NOT exist - return; - - id_list_.erase(it); - address_list_.erase(id); - length_list_.erase(id); - delete[] data_list_[id]; - data_list_.erase(id); - - is_param_changed_ = true; -} -bool GroupBulkWrite::changeParam(uint8_t id, uint16_t start_address, uint16_t data_length, uint8_t *data) -{ - if (ph_->getProtocolVersion() == 1.0) - return false; - - std::vector::iterator it = std::find(id_list_.begin(), id_list_.end(), id); - if (it == id_list_.end()) // NOT exist - return false; - - address_list_[id] = start_address; - length_list_[id] = data_length; - delete[] data_list_[id]; - data_list_[id] = new uint8_t[data_length]; - for (int c = 0; c < data_length; c++) - data_list_[id][c] = data[c]; - - is_param_changed_ = true; - return true; -} -void GroupBulkWrite::clearParam() -{ - if (ph_->getProtocolVersion() == 1.0 || id_list_.size() == 0) - return; - - for (unsigned int i = 0; i < id_list_.size(); i++) - delete[] data_list_[id_list_[i]]; - - id_list_.clear(); - address_list_.clear(); - length_list_.clear(); - data_list_.clear(); - if (param_ != 0) - delete[] param_; - param_ = 0; -} -int GroupBulkWrite::txPacket() -{ - if (ph_->getProtocolVersion() == 1.0 || id_list_.size() == 0) - return COMM_NOT_AVAILABLE; - - if (is_param_changed_ == true) - makeParam(); - - return ph_->bulkWriteTxOnly(port_, param_, param_length_); -} diff --git a/dynamixel_sdk/src/dynamixel_sdk/group_sync_read.cpp b/dynamixel_sdk/src/dynamixel_sdk/group_sync_read.cpp deleted file mode 100644 index 13818cc..0000000 --- a/dynamixel_sdk/src/dynamixel_sdk/group_sync_read.cpp +++ /dev/null @@ -1,203 +0,0 @@ -/******************************************************************************* -* Copyright (c) 2016, ROBOTIS CO., LTD. -* 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 ROBOTIS 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. -*******************************************************************************/ - -/* Author: zerom, Leon Ryu Woon Jung */ - -/* - * group_sync_read.cpp - * - * Created on: 2016. 2. 2. - */ -#if defined(_WIN32) || defined(_WIN64) -#define WINDLLEXPORT -#endif - -#include -#include "dynamixel_sdk/group_sync_read.h" - -using namespace dynamixel; - -GroupSyncRead::GroupSyncRead(PortHandler *port, PacketHandler *ph, uint16_t start_address, uint16_t data_length) - : port_(port), - ph_(ph), - last_result_(false), - is_param_changed_(false), - param_(0), - start_address_(start_address), - data_length_(data_length) -{ - clearParam(); -} - -void GroupSyncRead::makeParam() -{ - if (ph_->getProtocolVersion() == 1.0 || id_list_.size() == 0) - return; - - if (param_ != 0) - delete[] param_; - param_ = 0; - - param_ = new uint8_t[id_list_.size() * 1]; // ID(1) - - int idx = 0; - for (unsigned int i = 0; i < id_list_.size(); i++) - param_[idx++] = id_list_[i]; -} - -bool GroupSyncRead::addParam(uint8_t id) -{ - if (ph_->getProtocolVersion() == 1.0) - return false; - - if (std::find(id_list_.begin(), id_list_.end(), id) != id_list_.end()) // id already exist - return false; - - id_list_.push_back(id); - data_list_[id] = new uint8_t[data_length_]; - - is_param_changed_ = true; - return true; -} -void GroupSyncRead::removeParam(uint8_t id) -{ - if (ph_->getProtocolVersion() == 1.0) - return; - - std::vector::iterator it = std::find(id_list_.begin(), id_list_.end(), id); - if (it == id_list_.end()) // NOT exist - return; - - id_list_.erase(it); - delete[] data_list_[id]; - data_list_.erase(id); - - is_param_changed_ = true; -} -void GroupSyncRead::clearParam() -{ - if (ph_->getProtocolVersion() == 1.0 || id_list_.size() == 0) - return; - - for (unsigned int i = 0; i < id_list_.size(); i++) - delete[] data_list_[id_list_[i]]; - - id_list_.clear(); - data_list_.clear(); - if (param_ != 0) - delete[] param_; - param_ = 0; -} - -int GroupSyncRead::txPacket() -{ - if (ph_->getProtocolVersion() == 1.0 || id_list_.size() == 0) - return COMM_NOT_AVAILABLE; - - if (is_param_changed_ == true) - makeParam(); - - return ph_->syncReadTx(port_, start_address_, data_length_, param_, (uint16_t)id_list_.size() * 1); -} - -int GroupSyncRead::rxPacket() -{ - last_result_ = false; - - if (ph_->getProtocolVersion() == 1.0) - return COMM_NOT_AVAILABLE; - - int cnt = id_list_.size(); - int result = COMM_RX_FAIL; - - if (cnt == 0) - return COMM_NOT_AVAILABLE; - - for (int i = 0; i < cnt; i++) - { - uint8_t id = id_list_[i]; - - result = ph_->readRx(port_, data_length_, data_list_[id]); - if (result != COMM_SUCCESS) - return result; - } - - if (result == COMM_SUCCESS) - last_result_ = true; - - return result; -} - -int GroupSyncRead::txRxPacket() -{ - if (ph_->getProtocolVersion() == 1.0) - return COMM_NOT_AVAILABLE; - - int result = COMM_TX_FAIL; - - result = txPacket(); - if (result != COMM_SUCCESS) - return result; - - return rxPacket(); -} - -bool GroupSyncRead::isAvailable(uint8_t id, uint16_t address, uint16_t data_length) -{ - if (ph_->getProtocolVersion() == 1.0 || last_result_ == false || data_list_.find(id) == data_list_.end()) - return false; - - if (address < start_address_ || start_address_ + data_length_ - data_length < address) - return false; - - return true; -} - -uint32_t GroupSyncRead::getData(uint8_t id, uint16_t address, uint16_t data_length) -{ - if (isAvailable(id, address, data_length) == false) - return 0; - - switch(data_length) - { - case 1: - return data_list_[id][address - start_address_]; - - case 2: - return DXL_MAKEWORD(data_list_[id][address - start_address_], data_list_[id][address - start_address_ + 1]); - - case 4: - return DXL_MAKEDWORD(DXL_MAKEWORD(data_list_[id][address - start_address_ + 0], data_list_[id][address - start_address_ + 1]), - DXL_MAKEWORD(data_list_[id][address - start_address_ + 2], data_list_[id][address - start_address_ + 3])); - - default: - return 0; - } -} diff --git a/dynamixel_sdk/src/dynamixel_sdk/group_sync_write.cpp b/dynamixel_sdk/src/dynamixel_sdk/group_sync_write.cpp deleted file mode 100644 index 94dea6f..0000000 --- a/dynamixel_sdk/src/dynamixel_sdk/group_sync_write.cpp +++ /dev/null @@ -1,147 +0,0 @@ -/******************************************************************************* -* Copyright (c) 2016, ROBOTIS CO., LTD. -* 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 ROBOTIS 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. -*******************************************************************************/ - -/* Author: zerom, Leon Ryu Woon Jung */ - -/* - * group_sync_write.cpp - * - * Created on: 2016. 1. 28. - */ -#if defined(_WIN32) || defined(_WIN64) -#define WINDLLEXPORT -#endif - -#include -#include "dynamixel_sdk/group_sync_write.h" - -using namespace dynamixel; - -GroupSyncWrite::GroupSyncWrite(PortHandler *port, PacketHandler *ph, uint16_t start_address, uint16_t data_length) - : port_(port), - ph_(ph), - is_param_changed_(false), - param_(0), - start_address_(start_address), - data_length_(data_length) -{ - clearParam(); -} - -void GroupSyncWrite::makeParam() -{ - if (id_list_.size() == 0) return; - - if (param_ != 0) - delete[] param_; - param_ = 0; - - param_ = new uint8_t[id_list_.size() * (1 + data_length_)]; // ID(1) + DATA(data_length) - - int idx = 0; - for (unsigned int i = 0; i < id_list_.size(); i++) - { - uint8_t id = id_list_[i]; - if (data_list_[id] == 0) - return; - - param_[idx++] = id; - for (int c = 0; c < data_length_; c++) - param_[idx++] = (data_list_[id])[c]; - } -} - -bool GroupSyncWrite::addParam(uint8_t id, uint8_t *data) -{ - if (std::find(id_list_.begin(), id_list_.end(), id) != id_list_.end()) // id already exist - return false; - - id_list_.push_back(id); - data_list_[id] = new uint8_t[data_length_]; - for (int c = 0; c < data_length_; c++) - data_list_[id][c] = data[c]; - - is_param_changed_ = true; - return true; -} - -void GroupSyncWrite::removeParam(uint8_t id) -{ - std::vector::iterator it = std::find(id_list_.begin(), id_list_.end(), id); - if (it == id_list_.end()) // NOT exist - return; - - id_list_.erase(it); - delete[] data_list_[id]; - data_list_.erase(id); - - is_param_changed_ = true; -} - -bool GroupSyncWrite::changeParam(uint8_t id, uint8_t *data) -{ - std::vector::iterator it = std::find(id_list_.begin(), id_list_.end(), id); - if (it == id_list_.end()) // NOT exist - return false; - - delete[] data_list_[id]; - data_list_[id] = new uint8_t[data_length_]; - for (int c = 0; c < data_length_; c++) - data_list_[id][c] = data[c]; - - is_param_changed_ = true; - return true; -} - -void GroupSyncWrite::clearParam() -{ - if (id_list_.size() == 0) - return; - - for (unsigned int i = 0; i < id_list_.size(); i++) - delete[] data_list_[id_list_[i]]; - - id_list_.clear(); - data_list_.clear(); - if (param_ != 0) - delete[] param_; - param_ = 0; -} - -int GroupSyncWrite::txPacket() -{ - if (id_list_.size() == 0) - return COMM_NOT_AVAILABLE; - - if (is_param_changed_ == true) - makeParam(); - - return ph_->syncWriteTxOnly(port_, start_address_, data_length_, param_, id_list_.size() * (1 + data_length_)); -} diff --git a/dynamixel_sdk/src/dynamixel_sdk/packet_handler.cpp b/dynamixel_sdk/src/dynamixel_sdk/packet_handler.cpp deleted file mode 100644 index 04fdc20..0000000 --- a/dynamixel_sdk/src/dynamixel_sdk/packet_handler.cpp +++ /dev/null @@ -1,60 +0,0 @@ -/******************************************************************************* -* Copyright (c) 2016, ROBOTIS CO., LTD. -* 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 ROBOTIS 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. -*******************************************************************************/ - -/* Author: zerom, Leon Ryu Woon Jung */ - -/* - * packet_handler.cpp - * - * Created on: 2016. 1. 26. - */ -#if defined(_WIN32) || defined(_WIN64) -#define WINDLLEXPORT -#endif - -#include "dynamixel_sdk/packet_handler.h" -#include "dynamixel_sdk/protocol1_packet_handler.h" -#include "dynamixel_sdk/protocol2_packet_handler.h" - -using namespace dynamixel; - -PacketHandler *PacketHandler::getPacketHandler(float protocol_version) -{ - if (protocol_version == 1.0) - { - return (PacketHandler *)(Protocol1PacketHandler::getInstance()); - } - else if (protocol_version == 2.0) - { - return (PacketHandler *)(Protocol2PacketHandler::getInstance()); - } - - return (PacketHandler *)(Protocol2PacketHandler::getInstance()); -} diff --git a/dynamixel_sdk/src/dynamixel_sdk/port_handler.cpp b/dynamixel_sdk/src/dynamixel_sdk/port_handler.cpp deleted file mode 100644 index 14c0c28..0000000 --- a/dynamixel_sdk/src/dynamixel_sdk/port_handler.cpp +++ /dev/null @@ -1,63 +0,0 @@ -/******************************************************************************* -* Copyright (c) 2016, ROBOTIS CO., LTD. -* 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 ROBOTIS 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. -*******************************************************************************/ - -/* Author: zerom, Leon Ryu Woon Jung */ - -/* - * port_handler.cpp - * - * Created on: 2016. 2. 5. - */ -#if defined(_WIN32) || defined(_WIN64) -#define WINDLLEXPORT -#endif - -#include "dynamixel_sdk/port_handler.h" - -#ifdef __linux__ - #include "dynamixel_sdk_linux/port_handler_linux.h" -#endif - -#if defined(_WIN32) || defined(_WIN64) - #include "dynamixel_sdk_windows/port_handler_windows.h" -#endif - -using namespace dynamixel; - -PortHandler *PortHandler::getPortHandler(const char *port_name) -{ -#ifdef __linux__ - return (PortHandler *)(new PortHandlerLinux(port_name)); -#endif - -#if defined(_WIN32) || defined(_WIN64) - return (PortHandler *)(new PortHandlerWindows(port_name)); -#endif -} diff --git a/dynamixel_sdk/src/dynamixel_sdk/protocol1_packet_handler.cpp b/dynamixel_sdk/src/dynamixel_sdk/protocol1_packet_handler.cpp deleted file mode 100644 index 5129164..0000000 --- a/dynamixel_sdk/src/dynamixel_sdk/protocol1_packet_handler.cpp +++ /dev/null @@ -1,727 +0,0 @@ -/******************************************************************************* -* Copyright (c) 2016, ROBOTIS CO., LTD. -* 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 ROBOTIS 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. -*******************************************************************************/ - -/* Author: zerom, Leon Ryu Woon Jung */ - -/* - * protocol1_packet_handler.cpp - * - * Created on: 2016. 1. 26. - */ -#if defined(_WIN32) || defined(_WIN64) -#define WINDLLEXPORT -#endif - -#include -#include -#include "dynamixel_sdk/protocol1_packet_handler.h" - -#define TXPACKET_MAX_LEN (250) -#define RXPACKET_MAX_LEN (250) - -///////////////// for Protocol 1.0 Packet ///////////////// -#define PKT_HEADER0 0 -#define PKT_HEADER1 1 -#define PKT_ID 2 -#define PKT_LENGTH 3 -#define PKT_INSTRUCTION 4 -#define PKT_ERROR 4 -#define PKT_PARAMETER0 5 - -///////////////// Protocol 1.0 Error bit ///////////////// -#define ERRBIT_VOLTAGE 1 // Supplied voltage is out of the range (operating volatage set in the control table) -#define ERRBIT_ANGLE 2 // Goal position is written out of the range (from CW angle limit to CCW angle limit) -#define ERRBIT_OVERHEAT 4 // Temperature is out of the range (operating temperature set in the control table) -#define ERRBIT_RANGE 8 // Command(setting value) is out of the range for use. -#define ERRBIT_CHECKSUM 16 // Instruction packet checksum is incorrect. -#define ERRBIT_OVERLOAD 32 // The current load cannot be controlled by the set torque. -#define ERRBIT_INSTRUCTION 64 // Undefined instruction or delivering the action command without the reg_write command. - -using namespace dynamixel; - -Protocol1PacketHandler *Protocol1PacketHandler::unique_instance_ = new Protocol1PacketHandler(); - -Protocol1PacketHandler::Protocol1PacketHandler() { } - -void Protocol1PacketHandler::printTxRxResult(int result) -{ - switch(result) - { - case COMM_SUCCESS: - printf("[TxRxResult] Communication success.\n"); - break; - - case COMM_PORT_BUSY: - printf("[TxRxResult] Port is in use!\n"); - break; - - case COMM_TX_FAIL: - printf("[TxRxResult] Failed transmit instruction packet!\n"); - break; - - case COMM_RX_FAIL: - printf("[TxRxResult] Failed get status packet from device!\n"); - break; - - case COMM_TX_ERROR: - printf("[TxRxResult] Incorrect instruction packet!\n"); - break; - - case COMM_RX_WAITING: - printf("[TxRxResult] Now recieving status packet!\n"); - break; - - case COMM_RX_TIMEOUT: - printf("[TxRxResult] There is no status packet!\n"); - break; - - case COMM_RX_CORRUPT: - printf("[TxRxResult] Incorrect status packet!\n"); - break; - - case COMM_NOT_AVAILABLE: - printf("[TxRxResult] Protocol does not support This function!\n"); - break; - - default: - break; - } -} - -void Protocol1PacketHandler::printRxPacketError(uint8_t error) -{ - if (error & ERRBIT_VOLTAGE) - printf("[RxPacketError] Input voltage error!\n"); - - if (error & ERRBIT_ANGLE) - printf("[RxPacketError] Angle limit error!\n"); - - if (error & ERRBIT_OVERHEAT) - printf("[RxPacketError] Overheat error!\n"); - - if (error & ERRBIT_RANGE) - printf("[RxPacketError] Out of range error!\n"); - - if (error & ERRBIT_CHECKSUM) - printf("[RxPacketError] Checksum error!\n"); - - if (error & ERRBIT_OVERLOAD) - printf("[RxPacketError] Overload error!\n"); - - if (error & ERRBIT_INSTRUCTION) - printf("[RxPacketError] Instruction code error!\n"); -} - -int Protocol1PacketHandler::txPacket(PortHandler *port, uint8_t *txpacket) -{ - uint8_t checksum = 0; - uint8_t total_packet_length = txpacket[PKT_LENGTH] + 4; // 4: HEADER0 HEADER1 ID LENGTH - uint8_t written_packet_length = 0; - - if (port->is_using_) - return COMM_PORT_BUSY; - port->is_using_ = true; - - // check max packet length - if (total_packet_length > TXPACKET_MAX_LEN) - { - port->is_using_ = false; - return COMM_TX_ERROR; - } - - // make packet header - txpacket[PKT_HEADER0] = 0xFF; - txpacket[PKT_HEADER1] = 0xFF; - - // add a checksum to the packet - for (int idx = 2; idx < total_packet_length - 1; idx++) // except header, checksum - checksum += txpacket[idx]; - txpacket[total_packet_length - 1] = ~checksum; - - // tx packet - port->clearPort(); - written_packet_length = port->writePort(txpacket, total_packet_length); - if (total_packet_length != written_packet_length) - { - port->is_using_ = false; - return COMM_TX_FAIL; - } - - return COMM_SUCCESS; -} - -int Protocol1PacketHandler::rxPacket(PortHandler *port, uint8_t *rxpacket) -{ - int result = COMM_TX_FAIL; - - uint8_t checksum = 0; - uint8_t rx_length = 0; - uint8_t wait_length = 6; // minimum length (HEADER0 HEADER1 ID LENGTH ERROR CHKSUM) - - while(true) - { - rx_length += port->readPort(&rxpacket[rx_length], wait_length - rx_length); - if (rx_length >= wait_length) - { - uint8_t idx = 0; - - // find packet header - for (idx = 0; idx < (rx_length - 1); idx++) - { - if (rxpacket[idx] == 0xFF && rxpacket[idx+1] == 0xFF) - break; - } - - if (idx == 0) // found at the beginning of the packet - { - if (rxpacket[PKT_ID] > 0xFD || // unavailable ID - rxpacket[PKT_LENGTH] > RXPACKET_MAX_LEN || // unavailable Length - rxpacket[PKT_ERROR] >= 0x64) // unavailable Error - { - // remove the first byte in the packet - for (uint8_t s = 0; s < rx_length - 1; s++) - rxpacket[s] = rxpacket[1 + s]; - //memcpy(&rxpacket[0], &rxpacket[idx], rx_length - idx); - rx_length -= 1; - continue; - } - - // re-calculate the exact length of the rx packet - if (wait_length != rxpacket[PKT_LENGTH] + PKT_LENGTH + 1) - { - wait_length = rxpacket[PKT_LENGTH] + PKT_LENGTH + 1; - continue; - } - - if (rx_length < wait_length) - { - // check timeout - if (port->isPacketTimeout() == true) - { - if (rx_length == 0) - { - result = COMM_RX_TIMEOUT; - } - else - { - result = COMM_RX_CORRUPT; - } - break; - } - else - { - continue; - } - } - - // calculate checksum - for (int i = 2; i < wait_length - 1; i++) // except header, checksum - checksum += rxpacket[i]; - checksum = ~checksum; - - // verify checksum - if (rxpacket[wait_length - 1] == checksum) - { - result = COMM_SUCCESS; - } - else - { - result = COMM_RX_CORRUPT; - } - break; - } - else - { - // remove unnecessary packets - for (uint8_t s = 0; s < rx_length - idx; s++) - rxpacket[s] = rxpacket[idx + s]; - //memcpy(&rxpacket[0], &rxpacket[idx], rx_length - idx); - rx_length -= idx; - } - } - else - { - // check timeout - if (port->isPacketTimeout() == true) - { - if (rx_length == 0) - { - result = COMM_RX_TIMEOUT; - } - else - { - result = COMM_RX_CORRUPT; - } - break; - } - } - } - port->is_using_ = false; - - return result; -} - -// NOT for BulkRead instruction -int Protocol1PacketHandler::txRxPacket(PortHandler *port, uint8_t *txpacket, uint8_t *rxpacket, uint8_t *error) -{ - int result = COMM_TX_FAIL; - - // tx packet - result = txPacket(port, txpacket); - if (result != COMM_SUCCESS) - return result; - - // (ID == Broadcast ID && NOT BulkRead) == no need to wait for status packet - // (Instruction == action) == no need to wait for status packet - if ((txpacket[PKT_ID] == BROADCAST_ID && txpacket[PKT_INSTRUCTION] != INST_BULK_READ) || - (txpacket[PKT_INSTRUCTION] == INST_ACTION)) - { - port->is_using_ = false; - return result; - } - - // set packet timeout - if (txpacket[PKT_INSTRUCTION] == INST_READ) - { - port->setPacketTimeout((uint16_t)(txpacket[PKT_PARAMETER0+1] + 6)); - } - else - { - port->setPacketTimeout((uint16_t)6); - } - - // rx packet - result = rxPacket(port, rxpacket); - // check txpacket ID == rxpacket ID - if (txpacket[PKT_ID] != rxpacket[PKT_ID]) - result = rxPacket(port, rxpacket); - - if (result == COMM_SUCCESS && txpacket[PKT_ID] != BROADCAST_ID) - { - if (error != 0) - *error = (uint8_t)rxpacket[PKT_ERROR]; - } - return result; -} - -int Protocol1PacketHandler::ping(PortHandler *port, uint8_t id, uint8_t *error) -{ - return ping(port, id, 0, error); -} - -int Protocol1PacketHandler::ping(PortHandler *port, uint8_t id, uint16_t *model_number, uint8_t *error) -{ - int result = COMM_TX_FAIL; - - uint8_t txpacket[6] = {0}; - uint8_t rxpacket[6] = {0}; - - if (id >= BROADCAST_ID) - return COMM_NOT_AVAILABLE; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH] = 2; - txpacket[PKT_INSTRUCTION] = INST_PING; - - result = txRxPacket(port, txpacket, rxpacket, error); - if (result == COMM_SUCCESS && model_number != 0) - { - uint8_t data_read[2] = {0}; - result = readTxRx(port, id, 0, 2, data_read); // Address 0 : Model Number - if (result == COMM_SUCCESS) *model_number = DXL_MAKEWORD(data_read[0], data_read[1]); - } - - return result; -} - -int Protocol1PacketHandler::broadcastPing(PortHandler *port, std::vector &id_list) -{ - return COMM_NOT_AVAILABLE; -} - -int Protocol1PacketHandler::action(PortHandler *port, uint8_t id) -{ - uint8_t txpacket[6] = {0}; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH] = 2; - txpacket[PKT_INSTRUCTION] = INST_ACTION; - - return txRxPacket(port, txpacket, 0); -} - -int Protocol1PacketHandler::reboot(PortHandler *port, uint8_t id, uint8_t *error) -{ - return COMM_NOT_AVAILABLE; -} - -int Protocol1PacketHandler::factoryReset(PortHandler *port, uint8_t id, uint8_t option, uint8_t *error) -{ - uint8_t txpacket[6] = {0}; - uint8_t rxpacket[6] = {0}; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH] = 2; - txpacket[PKT_INSTRUCTION] = INST_FACTORY_RESET; - - return txRxPacket(port, txpacket, rxpacket, error); -} - -int Protocol1PacketHandler::readTx(PortHandler *port, uint8_t id, uint16_t address, uint16_t length) -{ - int result = COMM_TX_FAIL; - - uint8_t txpacket[8] = {0}; - - if (id >= BROADCAST_ID) - return COMM_NOT_AVAILABLE; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH] = 4; - txpacket[PKT_INSTRUCTION] = INST_READ; - txpacket[PKT_PARAMETER0+0] = (uint8_t)address; - txpacket[PKT_PARAMETER0+1] = (uint8_t)length; - - result = txPacket(port, txpacket); - - // set packet timeout - if (result == COMM_SUCCESS) - port->setPacketTimeout((uint16_t)(length+6)); - - return result; -} - -int Protocol1PacketHandler::readRx(PortHandler *port, uint16_t length, uint8_t *data, uint8_t *error) -{ - int result = COMM_TX_FAIL; - uint8_t *rxpacket = (uint8_t *)malloc(RXPACKET_MAX_LEN);//(length+6); - //uint8_t *rxpacket = new uint8_t[length+6]; - - result = rxPacket(port, rxpacket); - if (result == COMM_SUCCESS) - { - if (error != 0) - { - *error = (uint8_t)rxpacket[PKT_ERROR]; - } - for (uint8_t s = 0; s < length; s++) - { - data[s] = rxpacket[PKT_PARAMETER0 + s]; - } - //memcpy(data, &rxpacket[PKT_PARAMETER0], length); - } - - free(rxpacket); - //delete[] rxpacket; - return result; -} - -int Protocol1PacketHandler::readTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error) -{ - int result = COMM_TX_FAIL; - - uint8_t txpacket[8] = {0}; - uint8_t *rxpacket = (uint8_t *)malloc(RXPACKET_MAX_LEN);//(length+6); - - if (id >= BROADCAST_ID) - return COMM_NOT_AVAILABLE; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH] = 4; - txpacket[PKT_INSTRUCTION] = INST_READ; - txpacket[PKT_PARAMETER0+0] = (uint8_t)address; - txpacket[PKT_PARAMETER0+1] = (uint8_t)length; - - result = txRxPacket(port, txpacket, rxpacket, error); - if (result == COMM_SUCCESS) - { - if (error != 0) - { - *error = (uint8_t)rxpacket[PKT_ERROR]; - } - for (uint8_t s = 0; s < length; s++) - { - data[s] = rxpacket[PKT_PARAMETER0 + s]; - } - //memcpy(data, &rxpacket[PKT_PARAMETER0], length); - } - - free(rxpacket); - //delete[] rxpacket; - return result; -} - -int Protocol1PacketHandler::read1ByteTx(PortHandler *port, uint8_t id, uint16_t address) -{ - return readTx(port, id, address, 1); -} -int Protocol1PacketHandler::read1ByteRx(PortHandler *port, uint8_t *data, uint8_t *error) -{ - uint8_t data_read[1] = {0}; - int result = readRx(port, 1, data_read, error); - if (result == COMM_SUCCESS) - *data = data_read[0]; - return result; -} -int Protocol1PacketHandler::read1ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint8_t *data, uint8_t *error) -{ - uint8_t data_read[1] = {0}; - int result = readTxRx(port, id, address, 1, data_read, error); - if (result == COMM_SUCCESS) - *data = data_read[0]; - return result; -} - -int Protocol1PacketHandler::read2ByteTx(PortHandler *port, uint8_t id, uint16_t address) -{ - return readTx(port, id, address, 2); -} -int Protocol1PacketHandler::read2ByteRx(PortHandler *port, uint16_t *data, uint8_t *error) -{ - uint8_t data_read[2] = {0}; - int result = readRx(port, 2, data_read, error); - if (result == COMM_SUCCESS) - *data = DXL_MAKEWORD(data_read[0], data_read[1]); - return result; -} -int Protocol1PacketHandler::read2ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t *data, uint8_t *error) -{ - uint8_t data_read[2] = {0}; - int result = readTxRx(port, id, address, 2, data_read, error); - if (result == COMM_SUCCESS) - *data = DXL_MAKEWORD(data_read[0], data_read[1]); - return result; -} - -int Protocol1PacketHandler::read4ByteTx(PortHandler *port, uint8_t id, uint16_t address) -{ - return COMM_NOT_AVAILABLE; -} -int Protocol1PacketHandler::read4ByteRx(PortHandler *port, uint32_t *data, uint8_t *error) -{ - return COMM_NOT_AVAILABLE; -} -int Protocol1PacketHandler::read4ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint32_t *data, uint8_t *error) -{ - return COMM_NOT_AVAILABLE; -} - -int Protocol1PacketHandler::writeTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data) -{ - int result = COMM_TX_FAIL; - - uint8_t *txpacket = (uint8_t *)malloc(length+7); - //uint8_t *txpacket = new uint8_t[length+7]; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH] = length+3; - txpacket[PKT_INSTRUCTION] = INST_WRITE; - txpacket[PKT_PARAMETER0] = (uint8_t)address; - - for (uint8_t s = 0; s < length; s++) - txpacket[PKT_PARAMETER0+1+s] = data[s]; - //memcpy(&txpacket[PKT_PARAMETER0+1], data, length); - - result = txPacket(port, txpacket); - port->is_using_ = false; - - free(txpacket); - //delete[] txpacket; - return result; -} - -int Protocol1PacketHandler::writeTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error) -{ - int result = COMM_TX_FAIL; - - uint8_t *txpacket = (uint8_t *)malloc(length+7); //#6->7 - //uint8_t *txpacket = new uint8_t[length+7]; - uint8_t rxpacket[6] = {0}; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH] = length+3; - txpacket[PKT_INSTRUCTION] = INST_WRITE; - txpacket[PKT_PARAMETER0] = (uint8_t)address; - - for (uint8_t s = 0; s < length; s++) - txpacket[PKT_PARAMETER0+1+s] = data[s]; - //memcpy(&txpacket[PKT_PARAMETER0+1], data, length); - - result = txRxPacket(port, txpacket, rxpacket, error); - - free(txpacket); - //delete[] txpacket; - return result; -} - -int Protocol1PacketHandler::write1ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint8_t data) -{ - uint8_t data_write[1] = { data }; - return writeTxOnly(port, id, address, 1, data_write); -} -int Protocol1PacketHandler::write1ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint8_t data, uint8_t *error) -{ - uint8_t data_write[1] = { data }; - return writeTxRx(port, id, address, 1, data_write, error); -} - -int Protocol1PacketHandler::write2ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint16_t data) -{ - uint8_t data_write[2] = { DXL_LOBYTE(data), DXL_HIBYTE(data) }; - return writeTxOnly(port, id, address, 2, data_write); -} -int Protocol1PacketHandler::write2ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t data, uint8_t *error) -{ - uint8_t data_write[2] = { DXL_LOBYTE(data), DXL_HIBYTE(data) }; - return writeTxRx(port, id, address, 2, data_write, error); -} - -int Protocol1PacketHandler::write4ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint32_t data) -{ - return COMM_NOT_AVAILABLE; -} -int Protocol1PacketHandler::write4ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint32_t data, uint8_t *error) -{ - return COMM_NOT_AVAILABLE; -} - -int Protocol1PacketHandler::regWriteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data) -{ - int result = COMM_TX_FAIL; - - uint8_t *txpacket = (uint8_t *)malloc(length+6); - //uint8_t *txpacket = new uint8_t[length+6]; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH] = length+3; - txpacket[PKT_INSTRUCTION] = INST_REG_WRITE; - txpacket[PKT_PARAMETER0] = (uint8_t)address; - - for (uint8_t s = 0; s < length; s++) - txpacket[PKT_PARAMETER0+1+s] = data[s]; - //memcpy(&txpacket[PKT_PARAMETER0+1], data, length); - - result = txPacket(port, txpacket); - port->is_using_ = false; - - free(txpacket); - //delete[] txpacket; - return result; -} - -int Protocol1PacketHandler::regWriteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error) -{ - int result = COMM_TX_FAIL; - - uint8_t *txpacket = (uint8_t *)malloc(length+6); - //uint8_t *txpacket = new uint8_t[length+6]; - uint8_t rxpacket[6] = {0}; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH] = length+3; - txpacket[PKT_INSTRUCTION] = INST_REG_WRITE; - txpacket[PKT_PARAMETER0] = (uint8_t)address; - - for (uint8_t s = 0; s < length; s++) - txpacket[PKT_PARAMETER0+1+s] = data[s]; - //memcpy(&txpacket[PKT_PARAMETER0+1], data, length); - - result = txRxPacket(port, txpacket, rxpacket, error); - - free(txpacket); - //delete[] txpacket; - return result; -} - -int Protocol1PacketHandler::syncReadTx(PortHandler *port, uint16_t start_address, uint16_t data_length, uint8_t *param, uint16_t param_length) -{ - return COMM_NOT_AVAILABLE; -} - -int Protocol1PacketHandler::syncWriteTxOnly(PortHandler *port, uint16_t start_address, uint16_t data_length, uint8_t *param, uint16_t param_length) -{ - int result = COMM_TX_FAIL; - - uint8_t *txpacket = (uint8_t *)malloc(param_length+8); - // 8: HEADER0 HEADER1 ID LEN INST START_ADDR DATA_LEN ... CHKSUM - //uint8_t *txpacket = new uint8_t[param_length + 8]; - - txpacket[PKT_ID] = BROADCAST_ID; - txpacket[PKT_LENGTH] = param_length + 4; // 4: INST START_ADDR DATA_LEN ... CHKSUM - txpacket[PKT_INSTRUCTION] = INST_SYNC_WRITE; - txpacket[PKT_PARAMETER0+0] = start_address; - txpacket[PKT_PARAMETER0+1] = data_length; - - for (uint8_t s = 0; s < param_length; s++) - txpacket[PKT_PARAMETER0+2+s] = param[s]; - //memcpy(&txpacket[PKT_PARAMETER0+2], param, param_length); - - result = txRxPacket(port, txpacket, 0, 0); - - free(txpacket); - //delete[] txpacket; - return result; -} - -int Protocol1PacketHandler::bulkReadTx(PortHandler *port, uint8_t *param, uint16_t param_length) -{ - int result = COMM_TX_FAIL; - - uint8_t *txpacket = (uint8_t *)malloc(param_length+7); - // 7: HEADER0 HEADER1 ID LEN INST 0x00 ... CHKSUM - //uint8_t *txpacket = new uint8_t[param_length + 7]; - - txpacket[PKT_ID] = BROADCAST_ID; - txpacket[PKT_LENGTH] = param_length + 3; // 3: INST 0x00 ... CHKSUM - txpacket[PKT_INSTRUCTION] = INST_BULK_READ; - txpacket[PKT_PARAMETER0+0] = 0x00; - - for (uint8_t s = 0; s < param_length; s++) - txpacket[PKT_PARAMETER0+1+s] = param[s]; - //memcpy(&txpacket[PKT_PARAMETER0+1], param, param_length); - - result = txPacket(port, txpacket); - if (result == COMM_SUCCESS) - { - int wait_length = 0; - for (int i = 0; i < param_length; i += 3) - wait_length += param[i] + 7; - port->setPacketTimeout((uint16_t)wait_length); - } - - free(txpacket); - //delete[] txpacket; - return result; -} - -int Protocol1PacketHandler::bulkWriteTxOnly(PortHandler *port, uint8_t *param, uint16_t param_length) -{ - return COMM_NOT_AVAILABLE; -} diff --git a/dynamixel_sdk/src/dynamixel_sdk/protocol2_packet_handler.cpp b/dynamixel_sdk/src/dynamixel_sdk/protocol2_packet_handler.cpp deleted file mode 100644 index f814229..0000000 --- a/dynamixel_sdk/src/dynamixel_sdk/protocol2_packet_handler.cpp +++ /dev/null @@ -1,1036 +0,0 @@ -/******************************************************************************* -* Copyright (c) 2016, ROBOTIS CO., LTD. -* 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 ROBOTIS 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. -*******************************************************************************/ - -/* Author: zerom, Leon Ryu Woon Jung */ - -/* - * protocol2_packet_handler.cpp - * - * Created on: 2016. 1. 26. - */ - -#if defined(_WIN32) || defined(_WIN64) -#define WINDLLEXPORT -#endif - -#include -#include -#include -#include "dynamixel_sdk/protocol2_packet_handler.h" - -#define TXPACKET_MAX_LEN (4*1024) -#define RXPACKET_MAX_LEN (4*1024) - -///////////////// for Protocol 2.0 Packet ///////////////// -#define PKT_HEADER0 0 -#define PKT_HEADER1 1 -#define PKT_HEADER2 2 -#define PKT_RESERVED 3 -#define PKT_ID 4 -#define PKT_LENGTH_L 5 -#define PKT_LENGTH_H 6 -#define PKT_INSTRUCTION 7 -#define PKT_ERROR 8 -#define PKT_PARAMETER0 8 - -///////////////// Protocol 2.0 Error bit ///////////////// -#define ERRNUM_RESULT_FAIL 1 // Failed to process the instruction packet. -#define ERRNUM_INSTRUCTION 2 // Instruction error -#define ERRNUM_CRC 3 // CRC check error -#define ERRNUM_DATA_RANGE 4 // Data range error -#define ERRNUM_DATA_LENGTH 5 // Data length error -#define ERRNUM_DATA_LIMIT 6 // Data limit error -#define ERRNUM_ACCESS 7 // Access error - -#define ERRBIT_ALERT 128 //When the device has a problem, this bit is set to 1. Check "Device Status Check" value. - -using namespace dynamixel; - -Protocol2PacketHandler *Protocol2PacketHandler::unique_instance_ = new Protocol2PacketHandler(); - -Protocol2PacketHandler::Protocol2PacketHandler() { } - -void Protocol2PacketHandler::printTxRxResult(int result) -{ - switch(result) - { - case COMM_SUCCESS: - printf("[TxRxResult] Communication success.\n"); - break; - - case COMM_PORT_BUSY: - printf("[TxRxResult] Port is in use!\n"); - break; - - case COMM_TX_FAIL: - printf("[TxRxResult] Failed transmit instruction packet!\n"); - break; - - case COMM_RX_FAIL: - printf("[TxRxResult] Failed get status packet from device!\n"); - break; - - case COMM_TX_ERROR: - printf("[TxRxResult] Incorrect instruction packet!\n"); - break; - - case COMM_RX_WAITING: - printf("[TxRxResult] Now recieving status packet!\n"); - break; - - case COMM_RX_TIMEOUT: - printf("[TxRxResult] There is no status packet!\n"); - break; - - case COMM_RX_CORRUPT: - printf("[TxRxResult] Incorrect status packet!\n"); - break; - - case COMM_NOT_AVAILABLE: - printf("[TxRxResult] Protocol does not support This function!\n"); - break; - - default: - break; - } -} - -void Protocol2PacketHandler::printRxPacketError(uint8_t error) -{ - if (error & ERRBIT_ALERT) - printf("[RxPacketError] Hardware error occurred. Check the error at Control Table (Hardware Error Status)!\n"); - - int not_alert_error = error & ~ERRBIT_ALERT; - - switch(not_alert_error) - { - case 0: - break; - - case ERRNUM_RESULT_FAIL: - printf("[RxPacketError] Failed to process the instruction packet!\n"); - break; - - case ERRNUM_INSTRUCTION: - printf("[RxPacketError] Undefined instruction or incorrect instruction!\n"); - break; - - case ERRNUM_CRC: - printf("[RxPacketError] CRC doesn't match!\n"); - break; - - case ERRNUM_DATA_RANGE: - printf("[RxPacketError] The data value is out of range!\n"); - break; - - case ERRNUM_DATA_LENGTH: - printf("[RxPacketError] The data length does not match as expected!\n"); - break; - - case ERRNUM_DATA_LIMIT: - printf("[RxPacketError] The data value exceeds the limit value!\n"); - break; - - case ERRNUM_ACCESS: - printf("[RxPacketError] Writing or Reading is not available to target address!\n"); - break; - - default: - printf("[RxPacketError] Unknown error code!\n"); - break; - } -} - -unsigned short Protocol2PacketHandler::updateCRC(uint16_t crc_accum, uint8_t *data_blk_ptr, uint16_t data_blk_size) -{ - uint16_t i; - uint16_t crc_table[256] = {0x0000, - 0x8005, 0x800F, 0x000A, 0x801B, 0x001E, 0x0014, 0x8011, - 0x8033, 0x0036, 0x003C, 0x8039, 0x0028, 0x802D, 0x8027, - 0x0022, 0x8063, 0x0066, 0x006C, 0x8069, 0x0078, 0x807D, - 0x8077, 0x0072, 0x0050, 0x8055, 0x805F, 0x005A, 0x804B, - 0x004E, 0x0044, 0x8041, 0x80C3, 0x00C6, 0x00CC, 0x80C9, - 0x00D8, 0x80DD, 0x80D7, 0x00D2, 0x00F0, 0x80F5, 0x80FF, - 0x00FA, 0x80EB, 0x00EE, 0x00E4, 0x80E1, 0x00A0, 0x80A5, - 0x80AF, 0x00AA, 0x80BB, 0x00BE, 0x00B4, 0x80B1, 0x8093, - 0x0096, 0x009C, 0x8099, 0x0088, 0x808D, 0x8087, 0x0082, - 0x8183, 0x0186, 0x018C, 0x8189, 0x0198, 0x819D, 0x8197, - 0x0192, 0x01B0, 0x81B5, 0x81BF, 0x01BA, 0x81AB, 0x01AE, - 0x01A4, 0x81A1, 0x01E0, 0x81E5, 0x81EF, 0x01EA, 0x81FB, - 0x01FE, 0x01F4, 0x81F1, 0x81D3, 0x01D6, 0x01DC, 0x81D9, - 0x01C8, 0x81CD, 0x81C7, 0x01C2, 0x0140, 0x8145, 0x814F, - 0x014A, 0x815B, 0x015E, 0x0154, 0x8151, 0x8173, 0x0176, - 0x017C, 0x8179, 0x0168, 0x816D, 0x8167, 0x0162, 0x8123, - 0x0126, 0x012C, 0x8129, 0x0138, 0x813D, 0x8137, 0x0132, - 0x0110, 0x8115, 0x811F, 0x011A, 0x810B, 0x010E, 0x0104, - 0x8101, 0x8303, 0x0306, 0x030C, 0x8309, 0x0318, 0x831D, - 0x8317, 0x0312, 0x0330, 0x8335, 0x833F, 0x033A, 0x832B, - 0x032E, 0x0324, 0x8321, 0x0360, 0x8365, 0x836F, 0x036A, - 0x837B, 0x037E, 0x0374, 0x8371, 0x8353, 0x0356, 0x035C, - 0x8359, 0x0348, 0x834D, 0x8347, 0x0342, 0x03C0, 0x83C5, - 0x83CF, 0x03CA, 0x83DB, 0x03DE, 0x03D4, 0x83D1, 0x83F3, - 0x03F6, 0x03FC, 0x83F9, 0x03E8, 0x83ED, 0x83E7, 0x03E2, - 0x83A3, 0x03A6, 0x03AC, 0x83A9, 0x03B8, 0x83BD, 0x83B7, - 0x03B2, 0x0390, 0x8395, 0x839F, 0x039A, 0x838B, 0x038E, - 0x0384, 0x8381, 0x0280, 0x8285, 0x828F, 0x028A, 0x829B, - 0x029E, 0x0294, 0x8291, 0x82B3, 0x02B6, 0x02BC, 0x82B9, - 0x02A8, 0x82AD, 0x82A7, 0x02A2, 0x82E3, 0x02E6, 0x02EC, - 0x82E9, 0x02F8, 0x82FD, 0x82F7, 0x02F2, 0x02D0, 0x82D5, - 0x82DF, 0x02DA, 0x82CB, 0x02CE, 0x02C4, 0x82C1, 0x8243, - 0x0246, 0x024C, 0x8249, 0x0258, 0x825D, 0x8257, 0x0252, - 0x0270, 0x8275, 0x827F, 0x027A, 0x826B, 0x026E, 0x0264, - 0x8261, 0x0220, 0x8225, 0x822F, 0x022A, 0x823B, 0x023E, - 0x0234, 0x8231, 0x8213, 0x0216, 0x021C, 0x8219, 0x0208, - 0x820D, 0x8207, 0x0202 }; - - for (uint16_t j = 0; j < data_blk_size; j++) - { - i = ((uint16_t)(crc_accum >> 8) ^ *data_blk_ptr++) & 0xFF; - crc_accum = (crc_accum << 8) ^ crc_table[i]; - } - - return crc_accum; -} - -void Protocol2PacketHandler::addStuffing(uint8_t *packet) -{ - int i = 0, index = 0; - int packet_length_in = DXL_MAKEWORD(packet[PKT_LENGTH_L], packet[PKT_LENGTH_H]); - int packet_length_out = packet_length_in; - uint8_t temp[TXPACKET_MAX_LEN] = {0}; - - for (uint8_t s = PKT_HEADER0; s <= PKT_LENGTH_H; s++) - temp[s] = packet[s]; // FF FF FD XX ID LEN_L LEN_H - //memcpy(temp, packet, PKT_LENGTH_H+1); - index = PKT_INSTRUCTION; - for (i = 0; i < packet_length_in - 2; i++) // except CRC - { - temp[index++] = packet[i+PKT_INSTRUCTION]; - if (packet[i+PKT_INSTRUCTION] == 0xFD && packet[i+PKT_INSTRUCTION-1] == 0xFF && packet[i+PKT_INSTRUCTION-2] == 0xFF) - { // FF FF FD - temp[index++] = 0xFD; - packet_length_out++; - } - } - temp[index++] = packet[PKT_INSTRUCTION+packet_length_in-2]; - temp[index++] = packet[PKT_INSTRUCTION+packet_length_in-1]; - - - ////////////////////////// - if (packet_length_in != packet_length_out) - packet = (uint8_t *)realloc(packet, index * sizeof(uint8_t)); - - /////////////////////////// - - for (uint8_t s = 0; s < index; s++) - packet[s] = temp[s]; - //memcpy(packet, temp, index); - packet[PKT_LENGTH_L] = DXL_LOBYTE(packet_length_out); - packet[PKT_LENGTH_H] = DXL_HIBYTE(packet_length_out); -} - -void Protocol2PacketHandler::removeStuffing(uint8_t *packet) -{ - int i = 0, index = 0; - int packet_length_in = DXL_MAKEWORD(packet[PKT_LENGTH_L], packet[PKT_LENGTH_H]); - int packet_length_out = packet_length_in; - - index = PKT_INSTRUCTION; - for (i = 0; i < packet_length_in - 2; i++) // except CRC - { - if (packet[i+PKT_INSTRUCTION] == 0xFD && packet[i+PKT_INSTRUCTION+1] == 0xFD && packet[i+PKT_INSTRUCTION-1] == 0xFF && packet[i+PKT_INSTRUCTION-2] == 0xFF) - { // FF FF FD FD - packet_length_out--; - i++; - } - packet[index++] = packet[i+PKT_INSTRUCTION]; - } - packet[index++] = packet[PKT_INSTRUCTION+packet_length_in-2]; - packet[index++] = packet[PKT_INSTRUCTION+packet_length_in-1]; - - packet[PKT_LENGTH_L] = DXL_LOBYTE(packet_length_out); - packet[PKT_LENGTH_H] = DXL_HIBYTE(packet_length_out); -} - -int Protocol2PacketHandler::txPacket(PortHandler *port, uint8_t *txpacket) -{ - uint16_t total_packet_length = 0; - uint16_t written_packet_length = 0; - - if (port->is_using_) - return COMM_PORT_BUSY; - port->is_using_ = true; - - // byte stuffing for header - addStuffing(txpacket); - - // check max packet length - total_packet_length = DXL_MAKEWORD(txpacket[PKT_LENGTH_L], txpacket[PKT_LENGTH_H]) + 7; - // 7: HEADER0 HEADER1 HEADER2 RESERVED ID LENGTH_L LENGTH_H - if (total_packet_length > TXPACKET_MAX_LEN) - { - port->is_using_ = false; - return COMM_TX_ERROR; - } - - // make packet header - txpacket[PKT_HEADER0] = 0xFF; - txpacket[PKT_HEADER1] = 0xFF; - txpacket[PKT_HEADER2] = 0xFD; - txpacket[PKT_RESERVED] = 0x00; - - // add CRC16 - uint16_t crc = updateCRC(0, txpacket, total_packet_length - 2); // 2: CRC16 - txpacket[total_packet_length - 2] = DXL_LOBYTE(crc); - txpacket[total_packet_length - 1] = DXL_HIBYTE(crc); - - // tx packet - port->clearPort(); - written_packet_length = port->writePort(txpacket, total_packet_length); - if (total_packet_length != written_packet_length) - { - port->is_using_ = false; - return COMM_TX_FAIL; - } - - return COMM_SUCCESS; -} - -int Protocol2PacketHandler::rxPacket(PortHandler *port, uint8_t *rxpacket) -{ - int result = COMM_TX_FAIL; - - uint16_t rx_length = 0; - uint16_t wait_length = 11; // minimum length (HEADER0 HEADER1 HEADER2 RESERVED ID LENGTH_L LENGTH_H INST ERROR CRC16_L CRC16_H) - - while(true) - { - rx_length += port->readPort(&rxpacket[rx_length], wait_length - rx_length); - if (rx_length >= wait_length) - { - uint16_t idx = 0; - - // find packet header - for (idx = 0; idx < (rx_length - 3); idx++) - { - if ((rxpacket[idx] == 0xFF) && (rxpacket[idx+1] == 0xFF) && (rxpacket[idx+2] == 0xFD) && (rxpacket[idx+3] != 0xFD)) - break; - } - - if (idx == 0) // found at the beginning of the packet - { - if (rxpacket[PKT_RESERVED] != 0x00 || - rxpacket[PKT_ID] > 0xFC || - DXL_MAKEWORD(rxpacket[PKT_LENGTH_L], rxpacket[PKT_LENGTH_H]) > RXPACKET_MAX_LEN || - rxpacket[PKT_INSTRUCTION] != 0x55) - { - // remove the first byte in the packet - for (uint8_t s = 0; s < rx_length - 1; s++) - rxpacket[s] = rxpacket[1 + s]; - //memcpy(&rxpacket[0], &rxpacket[idx], rx_length - idx); - rx_length -= 1; - continue; - } - - // re-calculate the exact length of the rx packet - if (wait_length != DXL_MAKEWORD(rxpacket[PKT_LENGTH_L], rxpacket[PKT_LENGTH_H]) + PKT_LENGTH_H + 1) - { - wait_length = DXL_MAKEWORD(rxpacket[PKT_LENGTH_L], rxpacket[PKT_LENGTH_H]) + PKT_LENGTH_H + 1; - continue; - } - - if (rx_length < wait_length) - { - // check timeout - if (port->isPacketTimeout() == true) - { - if (rx_length == 0) - { - result = COMM_RX_TIMEOUT; - } - else - { - result = COMM_RX_CORRUPT; - } - break; - } - else - { - continue; - } - } - - // verify CRC16 - uint16_t crc = DXL_MAKEWORD(rxpacket[wait_length-2], rxpacket[wait_length-1]); - if (updateCRC(0, rxpacket, wait_length - 2) == crc) - { - result = COMM_SUCCESS; - } - else - { - result = COMM_RX_CORRUPT; - } - break; - } - else - { - // remove unnecessary packets - for (uint8_t s = 0; s < rx_length - idx; s++) - rxpacket[s] = rxpacket[idx + s]; - //memcpy(&rxpacket[0], &rxpacket[idx], rx_length - idx); - rx_length -= idx; - } - } - else - { - // check timeout - if (port->isPacketTimeout() == true) - { - if (rx_length == 0) - { - result = COMM_RX_TIMEOUT; - } - else - { - result = COMM_RX_CORRUPT; - } - break; - } - } - } - port->is_using_ = false; - - if (result == COMM_SUCCESS) - removeStuffing(rxpacket); - - return result; -} - -// NOT for BulkRead / SyncRead instruction -int Protocol2PacketHandler::txRxPacket(PortHandler *port, uint8_t *txpacket, uint8_t *rxpacket, uint8_t *error) -{ - int result = COMM_TX_FAIL; - - // tx packet - result = txPacket(port, txpacket); - if (result != COMM_SUCCESS) - return result; - - // (ID == Broadcast ID && NOT BulkRead) == no need to wait for status packet - // (Instruction == action) == no need to wait for status packet - if ((txpacket[PKT_ID] == BROADCAST_ID && txpacket[PKT_INSTRUCTION] != INST_BULK_READ) || - (txpacket[PKT_ID] == BROADCAST_ID && txpacket[PKT_INSTRUCTION] != INST_SYNC_READ) || - (txpacket[PKT_INSTRUCTION] == INST_ACTION)) - { - port->is_using_ = false; - return result; - } - - // set packet timeout - if (txpacket[PKT_INSTRUCTION] == INST_READ) - { - port->setPacketTimeout((uint16_t)(DXL_MAKEWORD(txpacket[PKT_PARAMETER0+2], txpacket[PKT_PARAMETER0+3]) + 11)); - } - else - { - port->setPacketTimeout((uint16_t)11); - // HEADER0 HEADER1 HEADER2 RESERVED ID LENGTH_L LENGTH_H INST ERROR CRC16_L CRC16_H - } - - // rx packet - result = rxPacket(port, rxpacket); - // check txpacket ID == rxpacket ID - if (txpacket[PKT_ID] != rxpacket[PKT_ID]) - result = rxPacket(port, rxpacket); - - if (result == COMM_SUCCESS && txpacket[PKT_ID] != BROADCAST_ID) - { - if (error != 0) - *error = (uint8_t)rxpacket[PKT_ERROR]; - } - - return result; -} - -int Protocol2PacketHandler::ping(PortHandler *port, uint8_t id, uint8_t *error) -{ - return ping(port, id, 0, error); -} - -int Protocol2PacketHandler::ping(PortHandler *port, uint8_t id, uint16_t *model_number, uint8_t *error) -{ - int result = COMM_TX_FAIL; - - uint8_t txpacket[10] = {0}; - uint8_t rxpacket[14] = {0}; - - if (id >= BROADCAST_ID) - return COMM_NOT_AVAILABLE; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH_L] = 3; - txpacket[PKT_LENGTH_H] = 0; - txpacket[PKT_INSTRUCTION] = INST_PING; - - result = txRxPacket(port, txpacket, rxpacket, error); - if (result == COMM_SUCCESS && model_number != 0) - *model_number = DXL_MAKEWORD(rxpacket[PKT_PARAMETER0+1], rxpacket[PKT_PARAMETER0+2]); - - return result; -} - -int Protocol2PacketHandler::broadcastPing(PortHandler *port, std::vector &id_list) -{ - const int STATUS_LENGTH = 14; - int result = COMM_TX_FAIL; - - id_list.clear(); - - uint16_t rx_length = 0; - uint16_t wait_length = STATUS_LENGTH * MAX_ID; - - uint8_t txpacket[10] = {0}; - uint8_t rxpacket[STATUS_LENGTH * MAX_ID] = {0}; - - txpacket[PKT_ID] = BROADCAST_ID; - txpacket[PKT_LENGTH_L] = 3; - txpacket[PKT_LENGTH_H] = 0; - txpacket[PKT_INSTRUCTION] = INST_PING; - - result = txPacket(port, txpacket); - if (result != COMM_SUCCESS) - { - port->is_using_ = false; - return result; - } - - // set rx timeout - port->setPacketTimeout((uint16_t)(wait_length * 30)); - - while(1) - { - rx_length += port->readPort(&rxpacket[rx_length], wait_length - rx_length); - if (port->isPacketTimeout() == true)// || rx_length >= wait_length) - break; - } - - port->is_using_ = false; - - if (rx_length == 0) - return COMM_RX_TIMEOUT; - - while(1) - { - if (rx_length < STATUS_LENGTH) - return COMM_RX_CORRUPT; - - uint16_t idx = 0; - - // find packet header - for (idx = 0; idx < (rx_length - 2); idx++) - { - if (rxpacket[idx] == 0xFF && rxpacket[idx+1] == 0xFF && rxpacket[idx+2] == 0xFD) - break; - } - - if (idx == 0) // found at the beginning of the packet - { - // verify CRC16 - uint16_t crc = DXL_MAKEWORD(rxpacket[STATUS_LENGTH-2], rxpacket[STATUS_LENGTH-1]); - - if (updateCRC(0, rxpacket, STATUS_LENGTH - 2) == crc) - { - result = COMM_SUCCESS; - - id_list.push_back(rxpacket[PKT_ID]); - - for (uint8_t s = 0; s < rx_length - STATUS_LENGTH; s++) - rxpacket[s] = rxpacket[STATUS_LENGTH + s]; - rx_length -= STATUS_LENGTH; - - if (rx_length == 0) - return result; - } - else - { - result = COMM_RX_CORRUPT; - - // remove header (0xFF 0xFF 0xFD) - for (uint8_t s = 0; s < rx_length - 3; s++) - rxpacket[s] = rxpacket[3 + s]; - rx_length -= 3; - } - } - else - { - // remove unnecessary packets - for (uint8_t s = 0; s < rx_length - idx; s++) - rxpacket[s] = rxpacket[idx + s]; - rx_length -= idx; - } - } - - return result; -} - -int Protocol2PacketHandler::action(PortHandler *port, uint8_t id) -{ - uint8_t txpacket[10] = {0}; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH_L] = 3; - txpacket[PKT_LENGTH_H] = 0; - txpacket[PKT_INSTRUCTION] = INST_ACTION; - - return txRxPacket(port, txpacket, 0); -} - -int Protocol2PacketHandler::reboot(PortHandler *port, uint8_t id, uint8_t *error) -{ - uint8_t txpacket[10] = {0}; - uint8_t rxpacket[11] = {0}; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH_L] = 3; - txpacket[PKT_LENGTH_H] = 0; - txpacket[PKT_INSTRUCTION] = INST_REBOOT; - - return txRxPacket(port, txpacket, rxpacket, error); -} - -int Protocol2PacketHandler::factoryReset(PortHandler *port, uint8_t id, uint8_t option, uint8_t *error) -{ - uint8_t txpacket[11] = {0}; - uint8_t rxpacket[11] = {0}; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH_L] = 4; - txpacket[PKT_LENGTH_H] = 0; - txpacket[PKT_INSTRUCTION] = INST_FACTORY_RESET; - txpacket[PKT_PARAMETER0] = option; - - return txRxPacket(port, txpacket, rxpacket, error); -} - -int Protocol2PacketHandler::readTx(PortHandler *port, uint8_t id, uint16_t address, uint16_t length) -{ - int result = COMM_TX_FAIL; - - uint8_t txpacket[14] = {0}; - - if (id >= BROADCAST_ID) - return COMM_NOT_AVAILABLE; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH_L] = 7; - txpacket[PKT_LENGTH_H] = 0; - txpacket[PKT_INSTRUCTION] = INST_READ; - txpacket[PKT_PARAMETER0+0] = (uint8_t)DXL_LOBYTE(address); - txpacket[PKT_PARAMETER0+1] = (uint8_t)DXL_HIBYTE(address); - txpacket[PKT_PARAMETER0+2] = (uint8_t)DXL_LOBYTE(length); - txpacket[PKT_PARAMETER0+3] = (uint8_t)DXL_HIBYTE(length); - - result = txPacket(port, txpacket); - - // set packet timeout - if (result == COMM_SUCCESS) - port->setPacketTimeout((uint16_t)(length + 11)); - - return result; -} - -int Protocol2PacketHandler::readRx(PortHandler *port, uint16_t length, uint8_t *data, uint8_t *error) -{ - int result = COMM_TX_FAIL; - uint8_t *rxpacket = (uint8_t *)malloc(RXPACKET_MAX_LEN); - //(length + 11 + (length/3)); // (length/3): consider stuffing - //uint8_t *rxpacket = new uint8_t[length + 11 + (length/3)]; // (length/3): consider stuffing - - result = rxPacket(port, rxpacket); - if (result == COMM_SUCCESS) - { - if (error != 0) - *error = (uint8_t)rxpacket[PKT_ERROR]; - for (uint8_t s = 0; s < length; s++) - data[s] = rxpacket[PKT_PARAMETER0 + 1 + s]; - //memcpy(data, &rxpacket[PKT_PARAMETER0+1], length); - } - - free(rxpacket); - //delete[] rxpacket; - return result; -} - -int Protocol2PacketHandler::readTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error) -{ - int result = COMM_TX_FAIL; - - uint8_t txpacket[14] = {0}; - uint8_t *rxpacket = (uint8_t *)malloc(RXPACKET_MAX_LEN); - //(length + 11 + (length/3)); // (length/3): consider stuffing - - if (id >= BROADCAST_ID) - return COMM_NOT_AVAILABLE; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH_L] = 7; - txpacket[PKT_LENGTH_H] = 0; - txpacket[PKT_INSTRUCTION] = INST_READ; - txpacket[PKT_PARAMETER0+0] = (uint8_t)DXL_LOBYTE(address); - txpacket[PKT_PARAMETER0+1] = (uint8_t)DXL_HIBYTE(address); - txpacket[PKT_PARAMETER0+2] = (uint8_t)DXL_LOBYTE(length); - txpacket[PKT_PARAMETER0+3] = (uint8_t)DXL_HIBYTE(length); - - result = txRxPacket(port, txpacket, rxpacket, error); - if (result == COMM_SUCCESS) - { - if (error != 0) - *error = (uint8_t)rxpacket[PKT_ERROR]; - for (uint8_t s = 0; s < length; s++) - data[s] = rxpacket[PKT_PARAMETER0 + 1 + s]; - //memcpy(data, &rxpacket[PKT_PARAMETER0+1], length); - } - - free(rxpacket); - //delete[] rxpacket; - return result; -} - -int Protocol2PacketHandler::read1ByteTx(PortHandler *port, uint8_t id, uint16_t address) -{ - return readTx(port, id, address, 1); -} -int Protocol2PacketHandler::read1ByteRx(PortHandler *port, uint8_t *data, uint8_t *error) -{ - uint8_t data_read[1] = {0}; - int result = readRx(port, 1, data_read, error); - if (result == COMM_SUCCESS) - *data = data_read[0]; - return result; -} -int Protocol2PacketHandler::read1ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint8_t *data, uint8_t *error) -{ - uint8_t data_read[1] = {0}; - int result = readTxRx(port, id, address, 1, data_read, error); - if (result == COMM_SUCCESS) - *data = data_read[0]; - return result; -} - -int Protocol2PacketHandler::read2ByteTx(PortHandler *port, uint8_t id, uint16_t address) -{ - return readTx(port, id, address, 2); -} -int Protocol2PacketHandler::read2ByteRx(PortHandler *port, uint16_t *data, uint8_t *error) -{ - uint8_t data_read[2] = {0}; - int result = readRx(port, 2, data_read, error); - if (result == COMM_SUCCESS) - *data = DXL_MAKEWORD(data_read[0], data_read[1]); - return result; -} -int Protocol2PacketHandler::read2ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t *data, uint8_t *error) -{ - uint8_t data_read[2] = {0}; - int result = readTxRx(port, id, address, 2, data_read, error); - if (result == COMM_SUCCESS) - *data = DXL_MAKEWORD(data_read[0], data_read[1]); - return result; -} - -int Protocol2PacketHandler::read4ByteTx(PortHandler *port, uint8_t id, uint16_t address) -{ - return readTx(port, id, address, 4); -} -int Protocol2PacketHandler::read4ByteRx(PortHandler *port, uint32_t *data, uint8_t *error) -{ - uint8_t data_read[4] = {0}; - int result = readRx(port, 4, data_read, error); - if (result == COMM_SUCCESS) - *data = DXL_MAKEDWORD(DXL_MAKEWORD(data_read[0], data_read[1]), DXL_MAKEWORD(data_read[2], data_read[3])); - return result; -} -int Protocol2PacketHandler::read4ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint32_t *data, uint8_t *error) -{ - uint8_t data_read[4] = {0}; - int result = readTxRx(port, id, address, 4, data_read, error); - if (result == COMM_SUCCESS) - *data = DXL_MAKEDWORD(DXL_MAKEWORD(data_read[0], data_read[1]), DXL_MAKEWORD(data_read[2], data_read[3])); - return result; -} - - -int Protocol2PacketHandler::writeTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data) -{ - int result = COMM_TX_FAIL; - - uint8_t *txpacket = (uint8_t *)malloc(length+12); - //uint8_t *txpacket = new uint8_t[length+12]; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length+5); - txpacket[PKT_LENGTH_H] = DXL_HIBYTE(length+5); - txpacket[PKT_INSTRUCTION] = INST_WRITE; - txpacket[PKT_PARAMETER0+0] = (uint8_t)DXL_LOBYTE(address); - txpacket[PKT_PARAMETER0+1] = (uint8_t)DXL_HIBYTE(address); - - for (uint8_t s = 0; s < length; s++) - txpacket[PKT_PARAMETER0+2+s] = data[s]; - //memcpy(&txpacket[PKT_PARAMETER0+2], data, length); - - result = txPacket(port, txpacket); - port->is_using_ = false; - - free(txpacket); - //delete[] txpacket; - return result; -} - -int Protocol2PacketHandler::writeTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error) -{ - int result = COMM_TX_FAIL; - - uint8_t *txpacket = (uint8_t *)malloc(length + 12); - //uint8_t *txpacket = new uint8_t[length+12]; - uint8_t rxpacket[11] = {0}; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length+5); - txpacket[PKT_LENGTH_H] = DXL_HIBYTE(length+5); - txpacket[PKT_INSTRUCTION] = INST_WRITE; - txpacket[PKT_PARAMETER0+0] = (uint8_t)DXL_LOBYTE(address); - txpacket[PKT_PARAMETER0+1] = (uint8_t)DXL_HIBYTE(address); - - for (uint8_t s = 0; s < length; s++) - txpacket[PKT_PARAMETER0+2+s] = data[s]; - //memcpy(&txpacket[PKT_PARAMETER0+2], data, length); - - result = txRxPacket(port, txpacket, rxpacket, error); - - free(txpacket); - //delete[] txpacket; - return result; -} - -int Protocol2PacketHandler::write1ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint8_t data) -{ - uint8_t data_write[1] = { data }; - return writeTxOnly(port, id, address, 1, data_write); -} -int Protocol2PacketHandler::write1ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint8_t data, uint8_t *error) -{ - uint8_t data_write[1] = { data }; - return writeTxRx(port, id, address, 1, data_write, error); -} - -int Protocol2PacketHandler::write2ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint16_t data) -{ - uint8_t data_write[2] = { DXL_LOBYTE(data), DXL_HIBYTE(data) }; - return writeTxOnly(port, id, address, 2, data_write); -} -int Protocol2PacketHandler::write2ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t data, uint8_t *error) -{ - uint8_t data_write[2] = { DXL_LOBYTE(data), DXL_HIBYTE(data) }; - return writeTxRx(port, id, address, 2, data_write, error); -} - -int Protocol2PacketHandler::write4ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint32_t data) -{ - uint8_t data_write[4] = { DXL_LOBYTE(DXL_LOWORD(data)), DXL_HIBYTE(DXL_LOWORD(data)), DXL_LOBYTE(DXL_HIWORD(data)), DXL_HIBYTE(DXL_HIWORD(data)) }; - return writeTxOnly(port, id, address, 4, data_write); -} -int Protocol2PacketHandler::write4ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint32_t data, uint8_t *error) -{ - uint8_t data_write[4] = { DXL_LOBYTE(DXL_LOWORD(data)), DXL_HIBYTE(DXL_LOWORD(data)), DXL_LOBYTE(DXL_HIWORD(data)), DXL_HIBYTE(DXL_HIWORD(data)) }; - return writeTxRx(port, id, address, 4, data_write, error); -} - -int Protocol2PacketHandler::regWriteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data) -{ - int result = COMM_TX_FAIL; - - uint8_t *txpacket = (uint8_t *)malloc(length + 12); - //uint8_t *txpacket = new uint8_t[length+12]; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length+5); - txpacket[PKT_LENGTH_H] = DXL_HIBYTE(length+5); - txpacket[PKT_INSTRUCTION] = INST_REG_WRITE; - txpacket[PKT_PARAMETER0+0] = (uint8_t)DXL_LOBYTE(address); - txpacket[PKT_PARAMETER0+1] = (uint8_t)DXL_HIBYTE(address); - - for (uint8_t s = 0; s < length; s++) - txpacket[PKT_PARAMETER0+2+s] = data[s]; - //memcpy(&txpacket[PKT_PARAMETER0+2], data, length); - - result = txPacket(port, txpacket); - port->is_using_ = false; - - free(txpacket); - //delete[] txpacket; - return result; -} - -int Protocol2PacketHandler::regWriteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error) -{ - int result = COMM_TX_FAIL; - - uint8_t *txpacket = (uint8_t *)malloc(length + 12); - //uint8_t *txpacket = new uint8_t[length+12]; - uint8_t rxpacket[11] = {0}; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length+5); - txpacket[PKT_LENGTH_H] = DXL_HIBYTE(length+5); - txpacket[PKT_INSTRUCTION] = INST_REG_WRITE; - txpacket[PKT_PARAMETER0+0] = (uint8_t)DXL_LOBYTE(address); - txpacket[PKT_PARAMETER0+1] = (uint8_t)DXL_HIBYTE(address); - - for (uint8_t s = 0; s < length; s++) - txpacket[PKT_PARAMETER0+2+s] = data[s]; - //memcpy(&txpacket[PKT_PARAMETER0+2], data, length); - - result = txRxPacket(port, txpacket, rxpacket, error); - - free(txpacket); - //delete[] txpacket; - return result; -} - -int Protocol2PacketHandler::syncReadTx(PortHandler *port, uint16_t start_address, uint16_t data_length, uint8_t *param, uint16_t param_length) -{ - int result = COMM_TX_FAIL; - - uint8_t *txpacket = (uint8_t *)malloc(param_length + 14); - // 14: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H - - txpacket[PKT_ID] = BROADCAST_ID; - txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H - txpacket[PKT_LENGTH_H] = DXL_HIBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H - txpacket[PKT_INSTRUCTION] = INST_SYNC_READ; - txpacket[PKT_PARAMETER0+0] = DXL_LOBYTE(start_address); - txpacket[PKT_PARAMETER0+1] = DXL_HIBYTE(start_address); - txpacket[PKT_PARAMETER0+2] = DXL_LOBYTE(data_length); - txpacket[PKT_PARAMETER0+3] = DXL_HIBYTE(data_length); - - for (uint8_t s = 0; s < param_length; s++) - txpacket[PKT_PARAMETER0+4+s] = param[s]; - //memcpy(&txpacket[PKT_PARAMETER0+4], param, param_length); - - result = txPacket(port, txpacket); - if (result == COMM_SUCCESS) - port->setPacketTimeout((uint16_t)((11 + data_length) * param_length)); - - free(txpacket); - return result; -} - -int Protocol2PacketHandler::syncWriteTxOnly(PortHandler *port, uint16_t start_address, uint16_t data_length, uint8_t *param, uint16_t param_length) -{ - int result = COMM_TX_FAIL; - - uint8_t *txpacket = (uint8_t *)malloc(param_length + 14); - //uint8_t *txpacket = new uint8_t[param_length + 14]; - // 14: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H - - txpacket[PKT_ID] = BROADCAST_ID; - txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H - txpacket[PKT_LENGTH_H] = DXL_HIBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H - txpacket[PKT_INSTRUCTION] = INST_SYNC_WRITE; - txpacket[PKT_PARAMETER0+0] = DXL_LOBYTE(start_address); - txpacket[PKT_PARAMETER0+1] = DXL_HIBYTE(start_address); - txpacket[PKT_PARAMETER0+2] = DXL_LOBYTE(data_length); - txpacket[PKT_PARAMETER0+3] = DXL_HIBYTE(data_length); - - for (uint8_t s = 0; s < param_length; s++) - txpacket[PKT_PARAMETER0+4+s] = param[s]; - //memcpy(&txpacket[PKT_PARAMETER0+4], param, param_length); - - result = txRxPacket(port, txpacket, 0, 0); - - free(txpacket); - //delete[] txpacket; - return result; -} - -int Protocol2PacketHandler::bulkReadTx(PortHandler *port, uint8_t *param, uint16_t param_length) -{ - int result = COMM_TX_FAIL; - - uint8_t *txpacket = (uint8_t *)malloc(param_length + 10); - //uint8_t *txpacket = new uint8_t[param_length + 10]; - // 10: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST CRC16_L CRC16_H - - txpacket[PKT_ID] = BROADCAST_ID; - txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H - txpacket[PKT_LENGTH_H] = DXL_HIBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H - txpacket[PKT_INSTRUCTION] = INST_BULK_READ; - - for (uint8_t s = 0; s < param_length; s++) - txpacket[PKT_PARAMETER0+s] = param[s]; - //memcpy(&txpacket[PKT_PARAMETER0], param, param_length); - - result = txPacket(port, txpacket); - if (result == COMM_SUCCESS) - { - int wait_length = 0; - for (int i = 0; i < param_length; i += 5) - wait_length += DXL_MAKEWORD(param[i+3], param[i+4]) + 10; - port->setPacketTimeout((uint16_t)wait_length); - } - - free(txpacket); - //delete[] txpacket; - return result; -} - -int Protocol2PacketHandler::bulkWriteTxOnly(PortHandler *port, uint8_t *param, uint16_t param_length) -{ - int result = COMM_TX_FAIL; - - uint8_t *txpacket = (uint8_t *)malloc(param_length + 10); - //uint8_t *txpacket = new uint8_t[param_length + 10]; - // 10: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST CRC16_L CRC16_H - - txpacket[PKT_ID] = BROADCAST_ID; - txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H - txpacket[PKT_LENGTH_H] = DXL_HIBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H - txpacket[PKT_INSTRUCTION] = INST_BULK_WRITE; - - for (uint8_t s = 0; s < param_length; s++) - txpacket[PKT_PARAMETER0+s] = param[s]; - //memcpy(&txpacket[PKT_PARAMETER0], param, param_length); - - result = txRxPacket(port, txpacket, 0, 0); - - free(txpacket); - //delete[] txpacket; - return result; -} diff --git a/dynamixel_sdk/src/dynamixel_sdk_linux/port_handler_linux.cpp b/dynamixel_sdk/src/dynamixel_sdk_linux/port_handler_linux.cpp deleted file mode 100644 index 0cc11e0..0000000 --- a/dynamixel_sdk/src/dynamixel_sdk_linux/port_handler_linux.cpp +++ /dev/null @@ -1,276 +0,0 @@ -/******************************************************************************* -* Copyright (c) 2016, ROBOTIS CO., LTD. -* 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 ROBOTIS 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. -*******************************************************************************/ - -/* Author: zerom, Leon Ryu Woon Jung */ - -/* - * port_handler_linux.cpp - * - * Created on: 2016. 1. 26. - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "dynamixel_sdk_linux/port_handler_linux.h" - -#define LATENCY_TIMER 4 // msec (USB latency timer) - -using namespace dynamixel; - -PortHandlerLinux::PortHandlerLinux(const char *port_name) - : socket_fd_(-1), - baudrate_(DEFAULT_BAUDRATE_), - packet_start_time_(0.0), - packet_timeout_(0.0), - tx_time_per_byte(0.0) -{ - is_using_ = false; - setPortName(port_name); -} - -bool PortHandlerLinux::openPort() -{ - return setBaudRate(baudrate_); -} - -void PortHandlerLinux::closePort() -{ - if(socket_fd_ != -1) - close(socket_fd_); - socket_fd_ = -1; -} - -void PortHandlerLinux::clearPort() -{ - tcflush(socket_fd_, TCIOFLUSH); -} - -void PortHandlerLinux::setPortName(const char *port_name) -{ - strcpy(port_name_, port_name); -} - -char *PortHandlerLinux::getPortName() -{ - return port_name_; -} - -// TODO: baud number ?? -bool PortHandlerLinux::setBaudRate(const int baudrate) -{ - int baud = getCFlagBaud(baudrate); - - closePort(); - - if(baud <= 0) // custom baudrate - { - setupPort(B38400); - baudrate_ = baudrate; - return setCustomBaudrate(baudrate); - } - else - { - baudrate_ = baudrate; - return setupPort(baud); - } -} - -int PortHandlerLinux::getBaudRate() -{ - return baudrate_; -} - -int PortHandlerLinux::getBytesAvailable() -{ - int bytes_available; - ioctl(socket_fd_, FIONREAD, &bytes_available); - return bytes_available; -} - -int PortHandlerLinux::readPort(uint8_t *packet, int length) -{ - return read(socket_fd_, packet, length); -} - -int PortHandlerLinux::writePort(uint8_t *packet, int length) -{ - return write(socket_fd_, packet, length); -} - -void PortHandlerLinux::setPacketTimeout(uint16_t packet_length) -{ - packet_start_time_ = getCurrentTime(); - packet_timeout_ = (tx_time_per_byte * (double)packet_length) + (LATENCY_TIMER * 2.0) + 2.0; -} - -void PortHandlerLinux::setPacketTimeout(double msec) -{ - packet_start_time_ = getCurrentTime(); - packet_timeout_ = msec; -} - -bool PortHandlerLinux::isPacketTimeout() -{ - if(getTimeSinceStart() > packet_timeout_) - { - packet_timeout_ = 0; - return true; - } - return false; -} - -double PortHandlerLinux::getCurrentTime() -{ - struct timespec tv; - clock_gettime( CLOCK_REALTIME, &tv); - return ((double)tv.tv_sec*1000.0 + (double)tv.tv_nsec*0.001*0.001); -} - -double PortHandlerLinux::getTimeSinceStart() -{ - double time; - - time = getCurrentTime() - packet_start_time_; - if(time < 0.0) - packet_start_time_ = getCurrentTime(); - - return time; -} - -bool PortHandlerLinux::setupPort(int cflag_baud) -{ - struct termios newtio; - - socket_fd_ = open(port_name_, O_RDWR|O_NOCTTY|O_NONBLOCK); - if(socket_fd_ < 0) - { - printf("[PortHandlerLinux::SetupPort] Error opening serial port!\n"); - return false; - } - - bzero(&newtio, sizeof(newtio)); // clear struct for new port settings - - newtio.c_cflag = cflag_baud | CS8 | CLOCAL | CREAD; - newtio.c_iflag = IGNPAR; - newtio.c_oflag = 0; - newtio.c_lflag = 0; - newtio.c_cc[VTIME] = 0; - newtio.c_cc[VMIN] = 0; - - // clean the buffer and activate the settings for the port - tcflush(socket_fd_, TCIFLUSH); - tcsetattr(socket_fd_, TCSANOW, &newtio); - - tx_time_per_byte = (1000.0 / (double)baudrate_) * 10.0; - return true; -} - -bool PortHandlerLinux::setCustomBaudrate(int speed) -{ - // try to set a custom divisor - struct serial_struct ss; - if(ioctl(socket_fd_, TIOCGSERIAL, &ss) != 0) - { - printf("[PortHandlerLinux::SetCustomBaudrate] TIOCGSERIAL failed!\n"); - return false; - } - - ss.flags = (ss.flags & ~ASYNC_SPD_MASK) | ASYNC_SPD_CUST; - ss.custom_divisor = (ss.baud_base + (speed / 2)) / speed; - int closest_speed = ss.baud_base / ss.custom_divisor; - - if(closest_speed < speed * 98 / 100 || closest_speed > speed * 102 / 100) - { - printf("[PortHandlerLinux::SetCustomBaudrate] Cannot set speed to %d, closest is %d \n", speed, closest_speed); - return false; - } - - if(ioctl(socket_fd_, TIOCSSERIAL, &ss) < 0) - { - printf("[PortHandlerLinux::SetCustomBaudrate] TIOCSSERIAL failed!\n"); - return false; - } - - tx_time_per_byte = (1000.0 / (double)speed) * 10.0; - return true; -} - -int PortHandlerLinux::getCFlagBaud(int baudrate) -{ - switch(baudrate) - { - case 9600: - return B9600; - case 19200: - return B19200; - case 38400: - return B38400; - case 57600: - return B57600; - case 115200: - return B115200; - case 230400: - return B230400; - case 460800: - return B460800; - case 500000: - return B500000; - case 576000: - return B576000; - case 921600: - return B921600; - case 1000000: - return B1000000; - case 1152000: - return B1152000; - case 1500000: - return B1500000; - case 2000000: - return B2000000; - case 2500000: - return B2500000; - case 3000000: - return B3000000; - case 3500000: - return B3500000; - case 4000000: - return B4000000; - default: - return -1; - } -} diff --git a/dynamixel_sdk/src/dynamixel_sdk_windows/port_handler_windows.cpp b/dynamixel_sdk/src/dynamixel_sdk_windows/port_handler_windows.cpp deleted file mode 100644 index c47e76f..0000000 --- a/dynamixel_sdk/src/dynamixel_sdk_windows/port_handler_windows.cpp +++ /dev/null @@ -1,245 +0,0 @@ -/******************************************************************************* -* Copyright (c) 2016, ROBOTIS CO., LTD. -* 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 ROBOTIS 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. -*******************************************************************************/ - -/* Author: Leon Ryu Woon Jung */ - -/* -* PortHandlerWindows.cpp -* -* Created on: 2016. 4. 06. -*/ -#if defined(_WIN32) || defined(_WIN64) -#define WINDLLEXPORT -#endif - -#include "dynamixel_sdk_windows/port_handler_windows.h" - -#include -#include -#include - -#define LATENCY_TIMER 16 // msec (USB latency timer) - -using namespace dynamixel; - -PortHandlerWindows::PortHandlerWindows(const char *port_name) - : serial_handle_(INVALID_HANDLE_VALUE), - baudrate_(DEFAULT_BAUDRATE_), - packet_start_time_(0.0), - packet_timeout_(0.0), - tx_time_per_byte_(0.0) -{ - is_using_ = false; - - char buffer[15]; - sprintf_s(buffer, sizeof(buffer), "\\\\.\\", port_name); - setPortName(port_name); -} - -bool PortHandlerWindows::openPort() -{ - return setBaudRate(baudrate_); -} - -void PortHandlerWindows::closePort() -{ - if (serial_handle_ != INVALID_HANDLE_VALUE) - { - CloseHandle(serial_handle_); - serial_handle_ = INVALID_HANDLE_VALUE; - } -} - -void PortHandlerWindows::clearPort() -{ - PurgeComm(serial_handle_, PURGE_RXABORT | PURGE_RXCLEAR); -} - -void PortHandlerWindows::setPortName(const char *port_name) -{ - strcpy_s(port_name_, sizeof(port_name_), port_name); -} - -char *PortHandlerWindows::getPortName() -{ - return port_name_; -} - -bool PortHandlerWindows::setBaudRate(const int baudrate) -{ - closePort(); - - baudrate_ = baudrate; - return setupPort(baudrate); -} - -int PortHandlerWindows::getBaudRate() -{ - return baudrate_; -} - -int PortHandlerWindows::getBytesAvailable() -{ - DWORD retbyte = 2; - BOOL res = DeviceIoControl(serial_handle_, GENERIC_READ | GENERIC_WRITE, NULL, 0, 0, 0, &retbyte, (LPOVERLAPPED)NULL); - - printf("%d", (int)res); - return (int)retbyte; -} - -int PortHandlerWindows::readPort(uint8_t *packet, int length) -{ - DWORD dwRead = 0; - - if (ReadFile(serial_handle_, packet, (DWORD)length, &dwRead, NULL) == FALSE) - return -1; - - return (int)dwRead; -} - -int PortHandlerWindows::writePort(uint8_t *packet, int length) -{ - DWORD dwWrite = 0; - - if (WriteFile(serial_handle_, packet, (DWORD)length, &dwWrite, NULL) == FALSE) - return -1; - - return (int)dwWrite; -} - -void PortHandlerWindows::setPacketTimeout(uint16_t packet_length) -{ - packet_start_time_ = getCurrentTime(); - packet_timeout_ = (tx_time_per_byte_ * (double)packet_length) + (LATENCY_TIMER * 2.0) + 2.0; -} - -void PortHandlerWindows::setPacketTimeout(double msec) -{ - packet_start_time_ = getCurrentTime(); - packet_timeout_ = msec; -} - -bool PortHandlerWindows::isPacketTimeout() -{ - if (getTimeSinceStart() > packet_timeout_) - { - packet_timeout_ = 0; - return true; - } - return false; -} - -double PortHandlerWindows::getCurrentTime() -{ - QueryPerformanceCounter(&counter_); - QueryPerformanceFrequency(&freq_); - return (double)counter_.QuadPart / (double)freq_.QuadPart * 1000.0; -} - -double PortHandlerWindows::getTimeSinceStart() -{ - double time; - - time = getCurrentTime() - packet_start_time_; - if (time < 0.0) packet_start_time_ = getCurrentTime(); - - return time; -} - -bool PortHandlerWindows::setupPort(int baudrate) -{ - DCB dcb; - COMMTIMEOUTS timeouts; - DWORD dwError; - - closePort(); - - serial_handle_ = CreateFileA(port_name_, GENERIC_READ | GENERIC_WRITE, 0, NULL, OPEN_EXISTING, FILE_ATTRIBUTE_NORMAL, NULL); - if (serial_handle_ == INVALID_HANDLE_VALUE) - { - printf("[PortHandlerWindows::SetupPort] Error opening serial port!\n"); - return false; - } - - dcb.DCBlength = sizeof(DCB); - if (GetCommState(serial_handle_, &dcb) == FALSE) - goto DXL_HAL_OPEN_ERROR; - - // Set baudrate - dcb.BaudRate = (DWORD)baudrate; - dcb.ByteSize = 8; // Data bit = 8bit - dcb.Parity = NOPARITY; // No parity - dcb.StopBits = ONESTOPBIT; // Stop bit = 1 - dcb.fParity = NOPARITY; // No Parity check - dcb.fBinary = 1; // Binary mode - dcb.fNull = 0; // Get Null byte - dcb.fAbortOnError = 0; - dcb.fErrorChar = 0; - // Not using XOn/XOff - dcb.fOutX = 0; - dcb.fInX = 0; - // Not using H/W flow control - dcb.fDtrControl = DTR_CONTROL_DISABLE; - dcb.fRtsControl = RTS_CONTROL_DISABLE; - dcb.fDsrSensitivity = 0; - dcb.fOutxDsrFlow = 0; - dcb.fOutxCtsFlow = 0; - - if (SetCommState(serial_handle_, &dcb) == FALSE) - goto DXL_HAL_OPEN_ERROR; - - if (SetCommMask(serial_handle_, 0) == FALSE) // Not using Comm event - goto DXL_HAL_OPEN_ERROR; - if (SetupComm(serial_handle_, 4096, 4096) == FALSE) // Buffer size (Rx,Tx) - goto DXL_HAL_OPEN_ERROR; - if (PurgeComm(serial_handle_, PURGE_TXABORT | PURGE_TXCLEAR | PURGE_RXABORT | PURGE_RXCLEAR) == FALSE) // Clear buffer - goto DXL_HAL_OPEN_ERROR; - if (ClearCommError(serial_handle_, &dwError, NULL) == FALSE) - goto DXL_HAL_OPEN_ERROR; - - if (GetCommTimeouts(serial_handle_, &timeouts) == FALSE) - goto DXL_HAL_OPEN_ERROR; - // Timeout (Not using timeout) - // Immediatly return - timeouts.ReadIntervalTimeout = 0; - timeouts.ReadTotalTimeoutMultiplier = 0; - timeouts.ReadTotalTimeoutConstant = 1; // must not be zero. - timeouts.WriteTotalTimeoutMultiplier = 0; - timeouts.WriteTotalTimeoutConstant = 0; - if (SetCommTimeouts(serial_handle_, &timeouts) == FALSE) - goto DXL_HAL_OPEN_ERROR; - - tx_time_per_byte_ = (1000.0 / (double)baudrate_) * 10.0; - return true; - -DXL_HAL_OPEN_ERROR: - closePort(); - return false; -} From 0b212111274038fe65a06aed86cfcfde24edf67c Mon Sep 17 00:00:00 2001 From: ROBOTIS-zerom Date: Fri, 12 Aug 2016 16:47:19 +0900 Subject: [PATCH 053/238] - added velocity p/i/d gain and position i/d gain sync_write code. --- .../robotis_controller/robotis_controller.h | 5 + .../robotis_controller/robotis_controller.cpp | 242 +++++++++++++++++- .../devices/dynamixel/GRIPPER.device | 3 + .../devices/dynamixel/H42-20-S300-R.device | 3 + .../devices/dynamixel/H54-100-S500-R.device | 3 + .../devices/dynamixel/H54-200-B500-R.device | 3 + .../devices/dynamixel/H54-200-S500-R.device | 3 + .../devices/dynamixel/L42-10-S300-R.device | 3 + .../devices/dynamixel/L54-30-S400-R.device | 3 + .../devices/dynamixel/L54-30-S500-R.device | 3 + .../devices/dynamixel/L54-50-S290-R.device | 3 + .../devices/dynamixel/L54-50-S500-R.device | 3 + .../devices/dynamixel/M42-10-S260-R.device | 3 + .../devices/dynamixel/M54-40-S250-R.device | 3 + .../devices/dynamixel/M54-60-S250-R.device | 3 + .../devices/dynamixel/MX-106.device | 3 + robotis_device/devices/dynamixel/MX-28.device | 3 + robotis_device/devices/dynamixel/MX-64.device | 3 + .../devices/dynamixel/XM-430-W210.device | 3 + .../devices/dynamixel/XM-430-W350.device | 3 + .../devices/dynamixel/XM-430.device | 3 + .../include/robotis_device/dynamixel.h | 5 + .../include/robotis_device/dynamixel_state.h | 10 + .../src/robotis_device/dynamixel.cpp | 7 +- 24 files changed, 319 insertions(+), 7 deletions(-) diff --git a/robotis_controller/include/robotis_controller/robotis_controller.h b/robotis_controller/include/robotis_controller/robotis_controller.h index ae91370..65d7821 100644 --- a/robotis_controller/include/robotis_controller/robotis_controller.h +++ b/robotis_controller/include/robotis_controller/robotis_controller.h @@ -109,6 +109,11 @@ public: std::map port_to_sync_write_velocity_; std::map port_to_sync_write_current_; std::map port_to_sync_write_position_p_gain_; + std::map port_to_sync_write_position_i_gain_; + std::map port_to_sync_write_position_d_gain_; + std::map port_to_sync_write_velocity_p_gain_; + std::map port_to_sync_write_velocity_i_gain_; + std::map port_to_sync_write_velocity_d_gain_; /* publisher */ ros::Publisher goal_joint_state_pub_; diff --git a/robotis_controller/src/robotis_controller/robotis_controller.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp index 0029e4f..35c81b1 100644 --- a/robotis_controller/src/robotis_controller/robotis_controller.cpp +++ b/robotis_controller/src/robotis_controller/robotis_controller.cpp @@ -93,11 +93,36 @@ void RobotisController::initializeSyncWrite() if (it.second != NULL) it.second->clearParam(); } + for (auto& it : port_to_sync_write_position_i_gain_) + { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto& it : port_to_sync_write_position_d_gain_) + { + if (it.second != NULL) + it.second->clearParam(); + } for (auto& it : port_to_sync_write_velocity_) { if (it.second != NULL) it.second->clearParam(); } + for (auto& it : port_to_sync_write_velocity_p_gain_) + { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto& it : port_to_sync_write_velocity_i_gain_) + { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto& it : port_to_sync_write_velocity_d_gain_) + { + if (it.second != NULL) + it.second->clearParam(); + } for (auto& it : port_to_sync_write_current_) { if (it.second != NULL) @@ -141,12 +166,37 @@ void RobotisController::initializeSyncWrite() { dxl->dxl_state_->position_p_gain_ = read_data; } + else if ((dxl->position_i_gain_item_ != 0) && + (dxl->bulk_read_items_[i]->item_name_ == dxl->position_i_gain_item_->item_name_)) + { + dxl->dxl_state_->position_i_gain_ = read_data; + } + else if ((dxl->position_d_gain_item_ != 0) && + (dxl->bulk_read_items_[i]->item_name_ == dxl->position_d_gain_item_->item_name_)) + { + dxl->dxl_state_->position_d_gain_ = read_data; + } else if ((dxl->present_velocity_item_ != 0) && (dxl->bulk_read_items_[i]->item_name_ == dxl->present_velocity_item_->item_name_)) { dxl->dxl_state_->present_velocity_ = dxl->convertValue2Velocity(read_data); dxl->dxl_state_->goal_velocity_ = dxl->dxl_state_->present_velocity_; } + else if ((dxl->velocity_p_gain_item_ != 0) && + (dxl->bulk_read_items_[i]->item_name_ == dxl->velocity_p_gain_item_->item_name_)) + { + dxl->dxl_state_->velocity_p_gain_ = read_data; + } + else if ((dxl->velocity_i_gain_item_ != 0) && + (dxl->bulk_read_items_[i]->item_name_ == dxl->velocity_i_gain_item_->item_name_)) + { + dxl->dxl_state_->velocity_i_gain_ = read_data; + } + else if ((dxl->velocity_d_gain_item_ != 0) && + (dxl->bulk_read_items_[i]->item_name_ == dxl->velocity_d_gain_item_->item_name_)) + { + dxl->dxl_state_->velocity_d_gain_ = read_data; + } else if ((dxl->present_current_item_ != 0) && (dxl->bulk_read_items_[i]->item_name_ == dxl->present_current_item_->item_name_)) { @@ -210,6 +260,24 @@ bool RobotisController::initialize(const std::string robot_file_path, const std: default_device->position_p_gain_item_->data_length_); } + if (default_device->position_i_gain_item_ != 0) + { + port_to_sync_write_position_i_gain_[port_name] + = new dynamixel::GroupSyncWrite(port, + default_pkt_handler, + default_device->position_i_gain_item_->address_, + default_device->position_i_gain_item_->data_length_); + } + + if (default_device->position_d_gain_item_ != 0) + { + port_to_sync_write_position_d_gain_[port_name] + = new dynamixel::GroupSyncWrite(port, + default_pkt_handler, + default_device->position_d_gain_item_->address_, + default_device->position_d_gain_item_->data_length_); + } + if (default_device->goal_velocity_item_ != 0) { port_to_sync_write_velocity_[port_name] @@ -219,6 +287,33 @@ bool RobotisController::initialize(const std::string robot_file_path, const std: default_device->goal_velocity_item_->data_length_); } + if (default_device->velocity_p_gain_item_ != 0) + { + port_to_sync_write_velocity_p_gain_[port_name] + = new dynamixel::GroupSyncWrite(port, + default_pkt_handler, + default_device->velocity_p_gain_item_->address_, + default_device->velocity_p_gain_item_->data_length_); + } + + if (default_device->velocity_i_gain_item_ != 0) + { + port_to_sync_write_velocity_i_gain_[port_name] + = new dynamixel::GroupSyncWrite(port, + default_pkt_handler, + default_device->velocity_i_gain_item_->address_, + default_device->velocity_i_gain_item_->data_length_); + } + + if (default_device->velocity_d_gain_item_ != 0) + { + port_to_sync_write_velocity_d_gain_[port_name] + = new dynamixel::GroupSyncWrite(port, + default_pkt_handler, + default_device->velocity_d_gain_item_->address_, + default_device->velocity_d_gain_item_->data_length_); + } + if (default_device->goal_current_item_ != 0) { port_to_sync_write_current_[port_name] @@ -687,11 +782,36 @@ void RobotisController::stopTimer() if (it.second != NULL) it.second->clearParam(); } + for (auto& it : port_to_sync_write_position_i_gain_) + { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto& it : port_to_sync_write_position_d_gain_) + { + if (it.second != NULL) + it.second->clearParam(); + } for (auto& it : port_to_sync_write_velocity_) { if (it.second != NULL) it.second->clearParam(); } + for (auto& it : port_to_sync_write_velocity_p_gain_) + { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto& it : port_to_sync_write_velocity_i_gain_) + { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto& it : port_to_sync_write_velocity_d_gain_) + { + if (it.second != NULL) + it.second->clearParam(); + } for (auto& it : port_to_sync_write_current_) { if (it.second != NULL) @@ -937,14 +1057,42 @@ void RobotisController::process() if (dxl_state->position_p_gain_ != result_state->position_p_gain_) { dxl_state->position_p_gain_ = result_state->position_p_gain_; - uint8_t sync_write_p_gain[4] = { 0 }; - sync_write_p_gain[0] = DXL_LOBYTE(DXL_LOWORD(dxl_state->position_p_gain_)); - sync_write_p_gain[1] = DXL_HIBYTE(DXL_LOWORD(dxl_state->position_p_gain_)); - sync_write_p_gain[2] = DXL_LOBYTE(DXL_HIWORD(dxl_state->position_p_gain_)); - sync_write_p_gain[3] = DXL_HIBYTE(DXL_HIWORD(dxl_state->position_p_gain_)); + uint8_t sync_write_data[4] = { 0 }; + sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(dxl_state->position_p_gain_)); + sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(dxl_state->position_p_gain_)); + sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(dxl_state->position_p_gain_)); + sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(dxl_state->position_p_gain_)); if (port_to_sync_write_position_p_gain_[dxl->port_name_] != NULL) - port_to_sync_write_position_p_gain_[dxl->port_name_]->addParam(dxl->id_, sync_write_p_gain); + port_to_sync_write_position_p_gain_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); + } + + // if position i gain value is changed -> sync write + if (dxl_state->position_i_gain_ != result_state->position_i_gain_) + { + dxl_state->position_i_gain_ = result_state->position_i_gain_; + uint8_t sync_write_data[4] = { 0 }; + sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(dxl_state->position_i_gain_)); + sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(dxl_state->position_i_gain_)); + sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(dxl_state->position_i_gain_)); + sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(dxl_state->position_i_gain_)); + + if (port_to_sync_write_position_i_gain_[dxl->port_name_] != NULL) + port_to_sync_write_position_i_gain_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); + } + + // if position d gain value is changed -> sync write + if (dxl_state->position_d_gain_ != result_state->position_d_gain_) + { + dxl_state->position_d_gain_ = result_state->position_d_gain_; + uint8_t sync_write_data[4] = { 0 }; + sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(dxl_state->position_d_gain_)); + sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(dxl_state->position_d_gain_)); + sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(dxl_state->position_d_gain_)); + sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(dxl_state->position_d_gain_)); + + if (port_to_sync_write_position_d_gain_[dxl->port_name_] != NULL) + port_to_sync_write_position_d_gain_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); } } } @@ -963,6 +1111,48 @@ void RobotisController::process() if (port_to_sync_write_velocity_[dxl->port_name_] != NULL) port_to_sync_write_velocity_[dxl->port_name_]->changeParam(dxl->id_, sync_write_data); + + // if velocity p gain gain value is changed -> sync write + if (dxl_state->velocity_p_gain_ != result_state->velocity_p_gain_) + { + dxl_state->velocity_p_gain_ = result_state->velocity_p_gain_; + uint8_t sync_write_data[4] = { 0 }; + sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(dxl_state->velocity_p_gain_)); + sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(dxl_state->velocity_p_gain_)); + sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(dxl_state->velocity_p_gain_)); + sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(dxl_state->velocity_p_gain_)); + + if (port_to_sync_write_velocity_p_gain_[dxl->port_name_] != NULL) + port_to_sync_write_velocity_p_gain_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); + } + + // if velocity i gain value is changed -> sync write + if (dxl_state->velocity_i_gain_ != result_state->velocity_i_gain_) + { + dxl_state->velocity_i_gain_ = result_state->velocity_i_gain_; + uint8_t sync_write_data[4] = { 0 }; + sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(dxl_state->velocity_i_gain_)); + sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(dxl_state->velocity_i_gain_)); + sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(dxl_state->velocity_i_gain_)); + sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(dxl_state->velocity_i_gain_)); + + if (port_to_sync_write_velocity_i_gain_[dxl->port_name_] != NULL) + port_to_sync_write_velocity_i_gain_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); + } + + // if velocity d gain value is changed -> sync write + if (dxl_state->velocity_d_gain_ != result_state->velocity_d_gain_) + { + dxl_state->velocity_d_gain_ = result_state->velocity_d_gain_; + uint8_t sync_write_data[4] = { 0 }; + sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(dxl_state->velocity_d_gain_)); + sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(dxl_state->velocity_d_gain_)); + sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(dxl_state->velocity_d_gain_)); + sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(dxl_state->velocity_d_gain_)); + + if (port_to_sync_write_velocity_d_gain_[dxl->port_name_] != NULL) + port_to_sync_write_velocity_d_gain_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); + } } } else if ((*module_it)->getControlMode() == TorqueControl) @@ -1004,6 +1194,46 @@ void RobotisController::process() it.second->clearParam(); } } + if (port_to_sync_write_position_i_gain_.size() > 0) + { + for (auto& it : port_to_sync_write_position_i_gain_) + { + it.second->txPacket(); + it.second->clearParam(); + } + } + if (port_to_sync_write_position_d_gain_.size() > 0) + { + for (auto& it : port_to_sync_write_position_d_gain_) + { + it.second->txPacket(); + it.second->clearParam(); + } + } + if (port_to_sync_write_velocity_p_gain_.size() > 0) + { + for (auto& it : port_to_sync_write_velocity_p_gain_) + { + it.second->txPacket(); + it.second->clearParam(); + } + } + if (port_to_sync_write_velocity_i_gain_.size() > 0) + { + for (auto& it : port_to_sync_write_velocity_i_gain_) + { + it.second->txPacket(); + it.second->clearParam(); + } + } + if (port_to_sync_write_velocity_d_gain_.size() > 0) + { + for (auto& it : port_to_sync_write_velocity_d_gain_) + { + it.second->txPacket(); + it.second->clearParam(); + } + } for (auto& it : port_to_sync_write_position_) { if (it.second != NULL) diff --git a/robotis_device/devices/dynamixel/GRIPPER.device b/robotis_device/devices/dynamixel/GRIPPER.device index e95ff17..4267c99 100644 --- a/robotis_device/devices/dynamixel/GRIPPER.device +++ b/robotis_device/devices/dynamixel/GRIPPER.device @@ -19,6 +19,9 @@ goal_current_item_name = goal_torque position_d_gain_item_name = position_i_gain_item_name = position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain [control table] # addr | item name | length | access | memory | min value | max value | signed diff --git a/robotis_device/devices/dynamixel/H42-20-S300-R.device b/robotis_device/devices/dynamixel/H42-20-S300-R.device index e348f42..95686ff 100644 --- a/robotis_device/devices/dynamixel/H42-20-S300-R.device +++ b/robotis_device/devices/dynamixel/H42-20-S300-R.device @@ -21,6 +21,9 @@ goal_current_item_name = goal_torque position_d_gain_item_name = position_i_gain_item_name = position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain [control table] # addr | item name | length | access | memory | min value | max value | signed diff --git a/robotis_device/devices/dynamixel/H54-100-S500-R.device b/robotis_device/devices/dynamixel/H54-100-S500-R.device index cdf81d0..a251ddd 100644 --- a/robotis_device/devices/dynamixel/H54-100-S500-R.device +++ b/robotis_device/devices/dynamixel/H54-100-S500-R.device @@ -21,6 +21,9 @@ goal_current_item_name = goal_torque position_d_gain_item_name = position_i_gain_item_name = position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain [control table] # addr | item name | length | access | memory | min value | max value | signed diff --git a/robotis_device/devices/dynamixel/H54-200-B500-R.device b/robotis_device/devices/dynamixel/H54-200-B500-R.device index d002f97..20a280a 100644 --- a/robotis_device/devices/dynamixel/H54-200-B500-R.device +++ b/robotis_device/devices/dynamixel/H54-200-B500-R.device @@ -21,6 +21,9 @@ goal_current_item_name = goal_torque position_d_gain_item_name = position_i_gain_item_name = position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain [control table] # addr | item name | length | access | memory | min value | max value | signed diff --git a/robotis_device/devices/dynamixel/H54-200-S500-R.device b/robotis_device/devices/dynamixel/H54-200-S500-R.device index 7e18900..5b7dc76 100644 --- a/robotis_device/devices/dynamixel/H54-200-S500-R.device +++ b/robotis_device/devices/dynamixel/H54-200-S500-R.device @@ -21,6 +21,9 @@ goal_current_item_name = goal_torque position_d_gain_item_name = position_i_gain_item_name = position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain [control table] # addr | item name | length | access | memory | min value | max value | signed diff --git a/robotis_device/devices/dynamixel/L42-10-S300-R.device b/robotis_device/devices/dynamixel/L42-10-S300-R.device index 8a4ca5a..84e556d 100644 --- a/robotis_device/devices/dynamixel/L42-10-S300-R.device +++ b/robotis_device/devices/dynamixel/L42-10-S300-R.device @@ -19,6 +19,9 @@ goal_current_item_name = goal_torque position_d_gain_item_name = position_i_gain_item_name = position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain [control table] # addr | item name | length | access | memory | min value | max value | signed diff --git a/robotis_device/devices/dynamixel/L54-30-S400-R.device b/robotis_device/devices/dynamixel/L54-30-S400-R.device index d684e67..9dc5d36 100644 --- a/robotis_device/devices/dynamixel/L54-30-S400-R.device +++ b/robotis_device/devices/dynamixel/L54-30-S400-R.device @@ -19,6 +19,9 @@ goal_current_item_name = goal_torque position_d_gain_item_name = position_i_gain_item_name = position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain [control table] # addr | item name | length | access | memory | min value | max value | signed diff --git a/robotis_device/devices/dynamixel/L54-30-S500-R.device b/robotis_device/devices/dynamixel/L54-30-S500-R.device index 68848a2..fa15941 100644 --- a/robotis_device/devices/dynamixel/L54-30-S500-R.device +++ b/robotis_device/devices/dynamixel/L54-30-S500-R.device @@ -19,6 +19,9 @@ goal_current_item_name = goal_torque position_d_gain_item_name = position_i_gain_item_name = position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain [control table] # addr | item name | length | access | memory | min value | max value | signed diff --git a/robotis_device/devices/dynamixel/L54-50-S290-R.device b/robotis_device/devices/dynamixel/L54-50-S290-R.device index 3cbd843..8c2f1a2 100644 --- a/robotis_device/devices/dynamixel/L54-50-S290-R.device +++ b/robotis_device/devices/dynamixel/L54-50-S290-R.device @@ -19,6 +19,9 @@ goal_current_item_name = goal_torque position_d_gain_item_name = position_i_gain_item_name = position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain [control table] # addr | item name | length | access | memory | min value | max value | signed diff --git a/robotis_device/devices/dynamixel/L54-50-S500-R.device b/robotis_device/devices/dynamixel/L54-50-S500-R.device index bfbf0c6..991e159 100644 --- a/robotis_device/devices/dynamixel/L54-50-S500-R.device +++ b/robotis_device/devices/dynamixel/L54-50-S500-R.device @@ -19,6 +19,9 @@ goal_current_item_name = goal_torque position_d_gain_item_name = position_i_gain_item_name = position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain [control table] # addr | item name | length | access | memory | min value | max value | signed diff --git a/robotis_device/devices/dynamixel/M42-10-S260-R.device b/robotis_device/devices/dynamixel/M42-10-S260-R.device index f21bb44..4c25631 100644 --- a/robotis_device/devices/dynamixel/M42-10-S260-R.device +++ b/robotis_device/devices/dynamixel/M42-10-S260-R.device @@ -19,6 +19,9 @@ goal_current_item_name = goal_torque position_d_gain_item_name = position_i_gain_item_name = position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain [control table] # addr | item name | length | access | memory | min value | max value | signed diff --git a/robotis_device/devices/dynamixel/M54-40-S250-R.device b/robotis_device/devices/dynamixel/M54-40-S250-R.device index d9424ad..76796b1 100644 --- a/robotis_device/devices/dynamixel/M54-40-S250-R.device +++ b/robotis_device/devices/dynamixel/M54-40-S250-R.device @@ -19,6 +19,9 @@ goal_current_item_name = goal_torque position_d_gain_item_name = position_i_gain_item_name = position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain [control table] # addr | item name | length | access | memory | min value | max value | signed diff --git a/robotis_device/devices/dynamixel/M54-60-S250-R.device b/robotis_device/devices/dynamixel/M54-60-S250-R.device index 284cac9..a6cfa2a 100644 --- a/robotis_device/devices/dynamixel/M54-60-S250-R.device +++ b/robotis_device/devices/dynamixel/M54-60-S250-R.device @@ -19,6 +19,9 @@ goal_current_item_name = goal_torque position_d_gain_item_name = position_i_gain_item_name = position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain [control table] # addr | item name | length | access | memory | min value | max value | signed diff --git a/robotis_device/devices/dynamixel/MX-106.device b/robotis_device/devices/dynamixel/MX-106.device index 23b16f1..4b8c63d 100644 --- a/robotis_device/devices/dynamixel/MX-106.device +++ b/robotis_device/devices/dynamixel/MX-106.device @@ -19,6 +19,9 @@ goal_current_item_name = position_d_gain_item_name = position_d_gain position_i_gain_item_name = position_i_gain position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = +velocity_p_gain_item_name = [control table] # addr | item name | length | access | memory | min value | max value | signed diff --git a/robotis_device/devices/dynamixel/MX-28.device b/robotis_device/devices/dynamixel/MX-28.device index ebcad6e..ff67f2a 100644 --- a/robotis_device/devices/dynamixel/MX-28.device +++ b/robotis_device/devices/dynamixel/MX-28.device @@ -19,6 +19,9 @@ goal_current_item_name = position_d_gain_item_name = position_d_gain position_i_gain_item_name = position_i_gain position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = +velocity_p_gain_item_name = [control table] # addr | item name | length | access | memory | min value | max value | signed diff --git a/robotis_device/devices/dynamixel/MX-64.device b/robotis_device/devices/dynamixel/MX-64.device index 2988c83..06c0a4a 100644 --- a/robotis_device/devices/dynamixel/MX-64.device +++ b/robotis_device/devices/dynamixel/MX-64.device @@ -19,6 +19,9 @@ goal_current_item_name = position_d_gain_item_name = position_d_gain position_i_gain_item_name = position_i_gain position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = +velocity_p_gain_item_name = [control table] # addr | item name | length | access | memory | min value | max value | signed diff --git a/robotis_device/devices/dynamixel/XM-430-W210.device b/robotis_device/devices/dynamixel/XM-430-W210.device index 178fc4f..f42ae57 100644 --- a/robotis_device/devices/dynamixel/XM-430-W210.device +++ b/robotis_device/devices/dynamixel/XM-430-W210.device @@ -22,6 +22,9 @@ goal_current_item_name = goal_current position_d_gain_item_name = position_d_gain position_i_gain_item_name = position_i_gain position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain [control table] # addr | item name | length | access | memory | min value | max value | signed diff --git a/robotis_device/devices/dynamixel/XM-430-W350.device b/robotis_device/devices/dynamixel/XM-430-W350.device index ed6c17f..ee14157 100644 --- a/robotis_device/devices/dynamixel/XM-430-W350.device +++ b/robotis_device/devices/dynamixel/XM-430-W350.device @@ -22,6 +22,9 @@ goal_current_item_name = goal_current position_d_gain_item_name = position_d_gain position_i_gain_item_name = position_i_gain position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain [control table] # addr | item name | length | access | memory | min value | max value | signed diff --git a/robotis_device/devices/dynamixel/XM-430.device b/robotis_device/devices/dynamixel/XM-430.device index 67d523f..c3a7bad 100644 --- a/robotis_device/devices/dynamixel/XM-430.device +++ b/robotis_device/devices/dynamixel/XM-430.device @@ -21,6 +21,9 @@ goal_current_item_name = goal_current position_d_gain_item_name = position_d_gain position_i_gain_item_name = position_i_gain position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain [control table] # addr | item name | length | access | memory | min value | max value | signed diff --git a/robotis_device/include/robotis_device/dynamixel.h b/robotis_device/include/robotis_device/dynamixel.h index 349020a..adff01b 100644 --- a/robotis_device/include/robotis_device/dynamixel.h +++ b/robotis_device/include/robotis_device/dynamixel.h @@ -73,6 +73,11 @@ public: ControlTableItem *goal_velocity_item_; ControlTableItem *goal_current_item_; ControlTableItem *position_p_gain_item_; + ControlTableItem *position_i_gain_item_; + ControlTableItem *position_d_gain_item_; + ControlTableItem *velocity_p_gain_item_; + ControlTableItem *velocity_i_gain_item_; + ControlTableItem *velocity_d_gain_item_; Dynamixel(int id, std::string model_name, float protocol_version); diff --git a/robotis_device/include/robotis_device/dynamixel_state.h b/robotis_device/include/robotis_device/dynamixel_state.h index a318a1f..5f4ae36 100644 --- a/robotis_device/include/robotis_device/dynamixel_state.h +++ b/robotis_device/include/robotis_device/dynamixel_state.h @@ -60,6 +60,11 @@ public: double goal_velocity_; double goal_torque_; double position_p_gain_; + double position_i_gain_; + double position_d_gain_; + double velocity_p_gain_; + double velocity_i_gain_; + double velocity_d_gain_; std::map bulk_read_table_; @@ -74,6 +79,11 @@ public: goal_velocity_(0.0), goal_torque_(0.0), position_p_gain_(0.0), + position_i_gain_(0.0), + position_d_gain_(0.0), + velocity_p_gain_(0.0), + velocity_i_gain_(0.0), + velocity_d_gain_(0.0), position_offset_(0.0) { bulk_read_table_.clear(); diff --git a/robotis_device/src/robotis_device/dynamixel.cpp b/robotis_device/src/robotis_device/dynamixel.cpp index ba316b2..cdd5470 100644 --- a/robotis_device/src/robotis_device/dynamixel.cpp +++ b/robotis_device/src/robotis_device/dynamixel.cpp @@ -55,7 +55,12 @@ Dynamixel::Dynamixel(int id, std::string model_name, float protocol_version) goal_position_item_(0), goal_velocity_item_(0), goal_current_item_(0), - position_p_gain_item_(0) + position_p_gain_item_(0), + position_i_gain_item_(0), + position_d_gain_item_(0), + velocity_p_gain_item_(0), + velocity_i_gain_item_(0), + velocity_d_gain_item_(0) { this->id_ = id; this->model_name_ = model_name; From 745144ccfa6f76f2bd65539dc0f67a7dd009b45b Mon Sep 17 00:00:00 2001 From: ROBOTIS-zerom Date: Fri, 12 Aug 2016 17:37:59 +0900 Subject: [PATCH 054/238] develop branch -> master branch --- dynamixel_sdk/CMakeLists.txt | 38 - dynamixel_sdk/include/DynamixelSDK.h | 29 - .../include/dynamixel_sdk/GroupBulkRead.h | 61 - .../include/dynamixel_sdk/GroupBulkWrite.h | 57 - .../include/dynamixel_sdk/GroupSyncRead.h | 61 - .../include/dynamixel_sdk/GroupSyncWrite.h | 56 - .../include/dynamixel_sdk/PacketHandler.h | 134 -- .../include/dynamixel_sdk/PortHandler.h | 60 - .../dynamixel_sdk/Protocol1PacketHandler.h | 98 - .../dynamixel_sdk/Protocol2PacketHandler.h | 103 - .../include/dynamixel_sdk/RobotisDef.h | 21 - .../dynamixel_sdk_linux/PortHandlerLinux.h | 62 - dynamixel_sdk/package.xml | 15 - .../src/dynamixel_sdk/GroupBulkRead.cpp | 199 -- .../src/dynamixel_sdk/GroupBulkWrite.cpp | 137 -- .../src/dynamixel_sdk/GroupSyncRead.cpp | 172 -- .../src/dynamixel_sdk/GroupSyncWrite.cpp | 117 - .../src/dynamixel_sdk/PacketHandler.cpp | 25 - .../src/dynamixel_sdk/PortHandler.cpp | 32 - .../dynamixel_sdk/Protocol1PacketHandler.cpp | 670 ------ .../dynamixel_sdk/Protocol2PacketHandler.cpp | 985 -------- .../dynamixel_sdk_linux/PortHandlerLinux.cpp | 245 -- robotis_controller/CMakeLists.txt | 22 +- .../robotis_controller/RobotisController.h | 145 -- .../robotis_controller/robotis_controller.h | 183 ++ robotis_controller/package.xml | 4 - .../robotis_controller/RobotisController.cpp | 1592 ------------ .../robotis_controller/robotis_controller.cpp | 2144 +++++++++++++++++ robotis_controller_msgs/CMakeLists.txt | 48 - .../msg/JointCtrlModule.msg | 2 - robotis_controller_msgs/msg/StatusMsg.msg | 10 - robotis_controller_msgs/msg/SyncWriteItem.msg | 3 - robotis_controller_msgs/package.xml | 30 - .../srv/GetJointModule.srv | 4 - robotis_device/CMakeLists.txt | 35 +- .../devices/dynamixel/GRIPPER.device | 3 + .../devices/dynamixel/H42-20-S300-R.device | 5 +- .../devices/dynamixel/H54-100-S500-R.device | 5 +- .../devices/dynamixel/H54-200-B500-R.device | 5 +- .../devices/dynamixel/H54-200-S500-R.device | 5 +- .../devices/dynamixel/L42-10-S300-R.device | 3 + .../devices/dynamixel/L54-30-S400-R.device | 5 +- .../devices/dynamixel/L54-30-S500-R.device | 5 +- .../devices/dynamixel/L54-50-S290-R.device | 5 +- .../devices/dynamixel/L54-50-S500-R.device | 5 +- .../devices/dynamixel/M42-10-S260-R.device | 5 +- .../devices/dynamixel/M54-40-S250-R.device | 5 +- .../devices/dynamixel/M54-60-S250-R.device | 5 +- .../devices/dynamixel/MX-106.device | 66 + robotis_device/devices/dynamixel/MX-28.device | 5 +- robotis_device/devices/dynamixel/MX-64.device | 66 + .../devices/dynamixel/XM-430-W210.device | 81 + .../devices/dynamixel/XM-430-W350.device | 81 + .../devices/dynamixel/XM-430.device | 9 +- .../include/robotis_device/ControlTableItem.h | 54 - .../include/robotis_device/Device.h | 37 - .../include/robotis_device/Dynamixel.h | 60 - .../include/robotis_device/DynamixelState.h | 55 - robotis_device/include/robotis_device/Robot.h | 38 - .../include/robotis_device/Sensor.h | 32 - .../include/robotis_device/SensorState.h | 35 - .../include/robotis_device/TimeStamp.h | 25 - .../robotis_device/control_table_item.h | 84 + .../include/robotis_device/device.h | 68 + .../include/robotis_device/dynamixel.h | 97 + .../include/robotis_device/dynamixel_state.h | 96 + robotis_device/include/robotis_device/robot.h | 78 + .../include/robotis_device/sensor.h | 63 + .../include/robotis_device/sensor_state.h | 64 + .../include/robotis_device/time_stamp.h | 59 + robotis_device/package.xml | 10 - .../src/robotis_device/Dynamixel.cpp | 110 - robotis_device/src/robotis_device/Robot.cpp | 408 ---- robotis_device/src/robotis_device/Sensor.cpp | 23 - .../src/robotis_device/dynamixel.cpp | 152 ++ robotis_device/src/robotis_device/robot.cpp | 457 ++++ robotis_device/src/robotis_device/sensor.cpp | 53 + robotis_framework_common/CMakeLists.txt | 38 - .../robotis_framework_common/MotionModule.h | 51 - .../robotis_framework_common/RobotisDef.h | 21 - .../robotis_framework_common/SensorModule.h | 38 - .../robotis_framework_common/Singleton.h | 49 - .../robotis_framework_common/motion_module.h | 101 + .../robotis_framework_common/sensor_module.h | 71 + .../robotis_framework_common/singleton.h | 79 + robotis_framework_common/package.xml | 45 +- robotis_math/CMakeLists.txt | 65 - .../robotis_math/RobotisLinearAlgebra.h | 52 - .../include/robotis_math/RobotisMath.h | 13 - .../include/robotis_math/RobotisMathBase.h | 33 - .../RobotisTrajectoryCalculator.h | 37 - robotis_math/package.xml | 37 - robotis_math/src/RobotisLinearAlgebra.cpp | 291 --- robotis_math/src/RobotisMathBase.cpp | 26 - .../src/RobotisTrajectoryCalculator.cpp | 325 --- 95 files changed, 4206 insertions(+), 7292 deletions(-) delete mode 100644 dynamixel_sdk/CMakeLists.txt delete mode 100644 dynamixel_sdk/include/DynamixelSDK.h delete mode 100644 dynamixel_sdk/include/dynamixel_sdk/GroupBulkRead.h delete mode 100644 dynamixel_sdk/include/dynamixel_sdk/GroupBulkWrite.h delete mode 100644 dynamixel_sdk/include/dynamixel_sdk/GroupSyncRead.h delete mode 100644 dynamixel_sdk/include/dynamixel_sdk/GroupSyncWrite.h delete mode 100644 dynamixel_sdk/include/dynamixel_sdk/PacketHandler.h delete mode 100644 dynamixel_sdk/include/dynamixel_sdk/PortHandler.h delete mode 100644 dynamixel_sdk/include/dynamixel_sdk/Protocol1PacketHandler.h delete mode 100644 dynamixel_sdk/include/dynamixel_sdk/Protocol2PacketHandler.h delete mode 100644 dynamixel_sdk/include/dynamixel_sdk/RobotisDef.h delete mode 100644 dynamixel_sdk/include/dynamixel_sdk_linux/PortHandlerLinux.h delete mode 100644 dynamixel_sdk/package.xml delete mode 100644 dynamixel_sdk/src/dynamixel_sdk/GroupBulkRead.cpp delete mode 100644 dynamixel_sdk/src/dynamixel_sdk/GroupBulkWrite.cpp delete mode 100644 dynamixel_sdk/src/dynamixel_sdk/GroupSyncRead.cpp delete mode 100644 dynamixel_sdk/src/dynamixel_sdk/GroupSyncWrite.cpp delete mode 100644 dynamixel_sdk/src/dynamixel_sdk/PacketHandler.cpp delete mode 100644 dynamixel_sdk/src/dynamixel_sdk/PortHandler.cpp delete mode 100644 dynamixel_sdk/src/dynamixel_sdk/Protocol1PacketHandler.cpp delete mode 100644 dynamixel_sdk/src/dynamixel_sdk/Protocol2PacketHandler.cpp delete mode 100644 dynamixel_sdk/src/dynamixel_sdk_linux/PortHandlerLinux.cpp delete mode 100644 robotis_controller/include/robotis_controller/RobotisController.h create mode 100644 robotis_controller/include/robotis_controller/robotis_controller.h delete mode 100644 robotis_controller/src/robotis_controller/RobotisController.cpp create mode 100644 robotis_controller/src/robotis_controller/robotis_controller.cpp delete mode 100644 robotis_controller_msgs/CMakeLists.txt delete mode 100644 robotis_controller_msgs/msg/JointCtrlModule.msg delete mode 100644 robotis_controller_msgs/msg/StatusMsg.msg delete mode 100644 robotis_controller_msgs/msg/SyncWriteItem.msg delete mode 100644 robotis_controller_msgs/package.xml delete mode 100644 robotis_controller_msgs/srv/GetJointModule.srv create mode 100644 robotis_device/devices/dynamixel/MX-106.device create mode 100644 robotis_device/devices/dynamixel/MX-64.device create mode 100644 robotis_device/devices/dynamixel/XM-430-W210.device create mode 100644 robotis_device/devices/dynamixel/XM-430-W350.device delete mode 100644 robotis_device/include/robotis_device/ControlTableItem.h delete mode 100644 robotis_device/include/robotis_device/Device.h delete mode 100644 robotis_device/include/robotis_device/Dynamixel.h delete mode 100644 robotis_device/include/robotis_device/DynamixelState.h delete mode 100644 robotis_device/include/robotis_device/Robot.h delete mode 100644 robotis_device/include/robotis_device/Sensor.h delete mode 100644 robotis_device/include/robotis_device/SensorState.h delete mode 100644 robotis_device/include/robotis_device/TimeStamp.h create mode 100644 robotis_device/include/robotis_device/control_table_item.h create mode 100644 robotis_device/include/robotis_device/device.h create mode 100644 robotis_device/include/robotis_device/dynamixel.h create mode 100644 robotis_device/include/robotis_device/dynamixel_state.h create mode 100644 robotis_device/include/robotis_device/robot.h create mode 100644 robotis_device/include/robotis_device/sensor.h create mode 100644 robotis_device/include/robotis_device/sensor_state.h create mode 100644 robotis_device/include/robotis_device/time_stamp.h delete mode 100644 robotis_device/src/robotis_device/Dynamixel.cpp delete mode 100644 robotis_device/src/robotis_device/Robot.cpp delete mode 100644 robotis_device/src/robotis_device/Sensor.cpp create mode 100644 robotis_device/src/robotis_device/dynamixel.cpp create mode 100644 robotis_device/src/robotis_device/robot.cpp create mode 100644 robotis_device/src/robotis_device/sensor.cpp delete mode 100644 robotis_framework_common/include/robotis_framework_common/MotionModule.h delete mode 100644 robotis_framework_common/include/robotis_framework_common/RobotisDef.h delete mode 100644 robotis_framework_common/include/robotis_framework_common/SensorModule.h delete mode 100644 robotis_framework_common/include/robotis_framework_common/Singleton.h create mode 100644 robotis_framework_common/include/robotis_framework_common/motion_module.h create mode 100644 robotis_framework_common/include/robotis_framework_common/sensor_module.h create mode 100644 robotis_framework_common/include/robotis_framework_common/singleton.h delete mode 100644 robotis_math/CMakeLists.txt delete mode 100644 robotis_math/include/robotis_math/RobotisLinearAlgebra.h delete mode 100644 robotis_math/include/robotis_math/RobotisMath.h delete mode 100644 robotis_math/include/robotis_math/RobotisMathBase.h delete mode 100644 robotis_math/include/robotis_math/RobotisTrajectoryCalculator.h delete mode 100644 robotis_math/package.xml delete mode 100644 robotis_math/src/RobotisLinearAlgebra.cpp delete mode 100644 robotis_math/src/RobotisMathBase.cpp delete mode 100644 robotis_math/src/RobotisTrajectoryCalculator.cpp diff --git a/dynamixel_sdk/CMakeLists.txt b/dynamixel_sdk/CMakeLists.txt deleted file mode 100644 index b3b5c84..0000000 --- a/dynamixel_sdk/CMakeLists.txt +++ /dev/null @@ -1,38 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(dynamixel_sdk) - -find_package(catkin REQUIRED COMPONENTS - roscpp -) - -catkin_package( - INCLUDE_DIRS include - LIBRARIES dynamixel_sdk -# CATKIN_DEPENDS roscpp -# DEPENDS system_lib -) - -# include_directories(include) -include_directories( - include - ${catkin_INCLUDE_DIRS} -) - -## Declare a C++ library -add_library(dynamixel_sdk - src/${PROJECT_NAME}/PacketHandler.cpp - src/${PROJECT_NAME}/Protocol1PacketHandler.cpp - src/${PROJECT_NAME}/Protocol2PacketHandler.cpp - src/${PROJECT_NAME}/GroupSyncRead.cpp - src/${PROJECT_NAME}/GroupSyncWrite.cpp - src/${PROJECT_NAME}/GroupBulkRead.cpp - src/${PROJECT_NAME}/GroupBulkWrite.cpp - src/${PROJECT_NAME}/PortHandler.cpp - src/${PROJECT_NAME}_linux/PortHandlerLinux.cpp -) - -## Specify libraries to link a library or executable target against -target_link_libraries(dynamixel_sdk - ${catkin_LIBRARIES} -) - diff --git a/dynamixel_sdk/include/DynamixelSDK.h b/dynamixel_sdk/include/DynamixelSDK.h deleted file mode 100644 index 0d34655..0000000 --- a/dynamixel_sdk/include/DynamixelSDK.h +++ /dev/null @@ -1,29 +0,0 @@ -/* - * DynamixelSDK.h - * - * Created on: 2016. 3. 8. - * Author: zerom, leon - */ - -#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_DYNAMIXELSDK_H_ -#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_DYNAMIXELSDK_H_ - - -#include "dynamixel_sdk/RobotisDef.h" -#include "dynamixel_sdk/GroupBulkRead.h" -#include "dynamixel_sdk/GroupBulkWrite.h" -#include "dynamixel_sdk/GroupSyncRead.h" -#include "dynamixel_sdk/GroupSyncWrite.h" -#include "dynamixel_sdk/Protocol1PacketHandler.h" -#include "dynamixel_sdk/Protocol2PacketHandler.h" - -#ifdef __linux__ - #include "dynamixel_sdk_linux/PortHandlerLinux.h" -#endif - -#if defined(_WIN32) || defined(_WIN64) - #include "dynamixel_sdk_windows/PortHandlerWindows.h" -#endif - - -#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_DYNAMIXELSDK_H_ */ diff --git a/dynamixel_sdk/include/dynamixel_sdk/GroupBulkRead.h b/dynamixel_sdk/include/dynamixel_sdk/GroupBulkRead.h deleted file mode 100644 index bd3374b..0000000 --- a/dynamixel_sdk/include/dynamixel_sdk/GroupBulkRead.h +++ /dev/null @@ -1,61 +0,0 @@ -/* - * GroupBulkRead.h - * - * Created on: 2016. 1. 28. - * Author: zerom, leon - */ - -#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPBULKREAD_H_ -#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPBULKREAD_H_ - - -#include -#include -#include "RobotisDef.h" -#include "PortHandler.h" -#include "PacketHandler.h" - -namespace ROBOTIS -{ - -class WINDECLSPEC GroupBulkRead -{ -private: - PortHandler *port_; - PacketHandler *ph_; - - std::vector id_list_; - std::map address_list_; // - std::map length_list_; // - std::map data_list_; // - - bool last_result_; - bool is_param_changed_; - - UINT8_T *param_; - - void MakeParam(); - -public: - GroupBulkRead(PortHandler *port, PacketHandler *ph); - ~GroupBulkRead() { ClearParam(); } - - PortHandler *GetPortHandler() { return port_; } - PacketHandler *GetPacketHandler() { return ph_; } - - bool AddParam (UINT8_T id, UINT16_T start_address, UINT16_T data_length); - void RemoveParam (UINT8_T id); - void ClearParam (); - - int TxPacket(); - int RxPacket(); - int TxRxPacket(); - - bool IsAvailable (UINT8_T id, UINT16_T address, UINT16_T data_length); - UINT32_T GetData (UINT8_T id, UINT16_T address, UINT16_T data_length); -}; - -} - - -#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPBULKREAD_H_ */ diff --git a/dynamixel_sdk/include/dynamixel_sdk/GroupBulkWrite.h b/dynamixel_sdk/include/dynamixel_sdk/GroupBulkWrite.h deleted file mode 100644 index 3efbb80..0000000 --- a/dynamixel_sdk/include/dynamixel_sdk/GroupBulkWrite.h +++ /dev/null @@ -1,57 +0,0 @@ -/* - * GroupBulkWrite.h - * - * Created on: 2016. 2. 2. - * Author: zerom, leon - */ - -#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPBULKWRITE_H_ -#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPBULKWRITE_H_ - - -#include -#include -#include "RobotisDef.h" -#include "PortHandler.h" -#include "PacketHandler.h" - -namespace ROBOTIS -{ - -class WINDECLSPEC GroupBulkWrite -{ -private: - PortHandler *port_; - PacketHandler *ph_; - - std::vector id_list_; - std::map address_list_; // - std::map length_list_; // - std::map data_list_; // - - bool is_param_changed_; - - UINT8_T *param_; - UINT16_T param_length_; - - void MakeParam(); - -public: - GroupBulkWrite(PortHandler *port, PacketHandler *ph); - ~GroupBulkWrite() { ClearParam(); } - - PortHandler *GetPortHandler() { return port_; } - PacketHandler *GetPacketHandler() { return ph_; } - - bool AddParam (UINT8_T id, UINT16_T start_address, UINT16_T data_length, UINT8_T *data); - void RemoveParam (UINT8_T id); - bool ChangeParam (UINT8_T id, UINT16_T start_address, UINT16_T data_length, UINT8_T *data); - void ClearParam (); - - int TxPacket(); -}; - -} - - -#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPBULKWRITE_H_ */ diff --git a/dynamixel_sdk/include/dynamixel_sdk/GroupSyncRead.h b/dynamixel_sdk/include/dynamixel_sdk/GroupSyncRead.h deleted file mode 100644 index b6599de..0000000 --- a/dynamixel_sdk/include/dynamixel_sdk/GroupSyncRead.h +++ /dev/null @@ -1,61 +0,0 @@ -/* - * GroupSyncRead.h - * - * Created on: 2016. 2. 2. - * Author: zerom, leon - */ - -#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPSYNCREAD_H_ -#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPSYNCREAD_H_ - - -#include -#include -#include "RobotisDef.h" -#include "PortHandler.h" -#include "PacketHandler.h" - -namespace ROBOTIS -{ - -class WINDECLSPEC GroupSyncRead -{ -private: - PortHandler *port_; - PacketHandler *ph_; - - std::vector id_list_; - std::map data_list_; // - - bool last_result_; - bool is_param_changed_; - - UINT8_T *param_; - UINT16_T start_address_; - UINT16_T data_length_; - - void MakeParam(); - -public: - GroupSyncRead(PortHandler *port, PacketHandler *ph, UINT16_T start_address, UINT16_T data_length); - ~GroupSyncRead() { ClearParam(); } - - PortHandler *GetPortHandler() { return port_; } - PacketHandler *GetPacketHandler() { return ph_; } - - bool AddParam (UINT8_T id); - void RemoveParam (UINT8_T id); - void ClearParam (); - - int TxPacket(); - int RxPacket(); - int TxRxPacket(); - - bool IsAvailable (UINT8_T id, UINT16_T address, UINT16_T data_length); - UINT32_T GetData (UINT8_T id, UINT16_T address, UINT16_T data_length); -}; - -} - - -#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPSYNCREAD_H_ */ diff --git a/dynamixel_sdk/include/dynamixel_sdk/GroupSyncWrite.h b/dynamixel_sdk/include/dynamixel_sdk/GroupSyncWrite.h deleted file mode 100644 index 1ff03b2..0000000 --- a/dynamixel_sdk/include/dynamixel_sdk/GroupSyncWrite.h +++ /dev/null @@ -1,56 +0,0 @@ -/* - * GroupSyncWrite.h - * - * Created on: 2016. 1. 28. - * Author: zerom, leon - */ - -#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPSYNCWRITE_H_ -#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPSYNCWRITE_H_ - - -#include -#include -#include "RobotisDef.h" -#include "PortHandler.h" -#include "PacketHandler.h" - -namespace ROBOTIS -{ - -class WINDECLSPEC GroupSyncWrite -{ -private: - PortHandler *port_; - PacketHandler *ph_; - - std::vector id_list_; - std::map data_list_; // - - bool is_param_changed_; - - UINT8_T *param_; - UINT16_T start_address_; - UINT16_T data_length_; - - void MakeParam(); - -public: - GroupSyncWrite(PortHandler *port, PacketHandler *ph, UINT16_T start_address, UINT16_T data_length); - ~GroupSyncWrite() { ClearParam(); } - - PortHandler *GetPortHandler() { return port_; } - PacketHandler *GetPacketHandler() { return ph_; } - - bool AddParam (UINT8_T id, UINT8_T *data); - void RemoveParam (UINT8_T id); - bool ChangeParam (UINT8_T id, UINT8_T *data); - void ClearParam (); - - int TxPacket(); -}; - -} - - -#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPSYNCWRITE_H_ */ diff --git a/dynamixel_sdk/include/dynamixel_sdk/PacketHandler.h b/dynamixel_sdk/include/dynamixel_sdk/PacketHandler.h deleted file mode 100644 index 921d380..0000000 --- a/dynamixel_sdk/include/dynamixel_sdk/PacketHandler.h +++ /dev/null @@ -1,134 +0,0 @@ -/* - * PacketHandler.h - * - * Created on: 2016. 1. 26. - * Author: zerom, leon - */ - -#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PACKETHANDLER_H_ -#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PACKETHANDLER_H_ - - -#include -#include -#include "RobotisDef.h" -#include "PortHandler.h" - -#define BROADCAST_ID 0xFE // 254 -#define MAX_ID 0xFC // 252 - -/* Macro for Control Table Value */ -#define DXL_MAKEWORD(a, b) ((unsigned short)(((unsigned char)(((unsigned long)(a)) & 0xff)) | ((unsigned short)((unsigned char)(((unsigned long)(b)) & 0xff))) << 8)) -#define DXL_MAKEDWORD(a, b) ((unsigned int)(((unsigned short)(((unsigned long)(a)) & 0xffff)) | ((unsigned int)((unsigned short)(((unsigned long)(b)) & 0xffff))) << 16)) -#define DXL_LOWORD(l) ((unsigned short)(((unsigned long)(l)) & 0xffff)) -#define DXL_HIWORD(l) ((unsigned short)((((unsigned long)(l)) >> 16) & 0xffff)) -#define DXL_LOBYTE(w) ((unsigned char)(((unsigned long)(w)) & 0xff)) -#define DXL_HIBYTE(w) ((unsigned char)((((unsigned long)(w)) >> 8) & 0xff)) - -/* Instruction for DXL Protocol */ -#define INST_PING 1 -#define INST_READ 2 -#define INST_WRITE 3 -#define INST_REG_WRITE 4 -#define INST_ACTION 5 -#define INST_FACTORY_RESET 6 -#define INST_SYNC_WRITE 131 // 0x83 -#define INST_BULK_READ 146 // 0x92 -// --- Only for 2.0 --- // -#define INST_REBOOT 8 -#define INST_STATUS 85 // 0x55 -#define INST_SYNC_READ 130 // 0x82 -#define INST_BULK_WRITE 147 // 0x93 - -// Communication Result -#define COMM_SUCCESS 0 // tx or rx packet communication success -#define COMM_PORT_BUSY -1000 // Port is busy (in use) -#define COMM_TX_FAIL -1001 // Failed transmit instruction packet -#define COMM_RX_FAIL -1002 // Failed get status packet -#define COMM_TX_ERROR -2000 // Incorrect instruction packet -#define COMM_RX_WAITING -3000 // Now recieving status packet -#define COMM_RX_TIMEOUT -3001 // There is no status packet -#define COMM_RX_CORRUPT -3002 // Incorrect status packet -#define COMM_NOT_AVAILABLE -9000 // - -namespace ROBOTIS -{ - -class WINDECLSPEC PacketHandler -{ -protected: - PacketHandler() { } - -public: - static PacketHandler *GetPacketHandler(float protocol_version = 2.0); - - virtual ~PacketHandler() { } - - virtual float GetProtocolVersion() = 0; - - virtual void PrintTxRxResult(int result) = 0; - virtual void PrintRxPacketError(UINT8_T error) = 0; - - virtual int TxPacket (PortHandler *port, UINT8_T *txpacket) = 0; - virtual int RxPacket (PortHandler *port, UINT8_T *rxpacket) = 0; - virtual int TxRxPacket (PortHandler *port, UINT8_T *txpacket, UINT8_T *rxpacket, UINT8_T *error = 0) = 0; - - virtual int Ping (PortHandler *port, UINT8_T id, UINT8_T *error = 0) = 0; - virtual int Ping (PortHandler *port, UINT8_T id, UINT16_T *model_number, UINT8_T *error = 0) = 0; - - // BroadcastPing - virtual int BroadcastPing (PortHandler *port, std::vector &id_list) = 0; - - virtual int Action (PortHandler *port, UINT8_T id) = 0; - virtual int Reboot (PortHandler *port, UINT8_T id, UINT8_T *error = 0) = 0; - virtual int FactoryReset (PortHandler *port, UINT8_T id, UINT8_T option = 0, UINT8_T *error = 0) = 0; - - - virtual int ReadTx (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length) = 0; - virtual int ReadRx (PortHandler *port, UINT16_T length, UINT8_T *data, UINT8_T *error = 0) = 0; - virtual int ReadTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error = 0) = 0; - - virtual int Read1ByteTx (PortHandler *port, UINT8_T id, UINT16_T address) = 0; - virtual int Read1ByteRx (PortHandler *port, UINT8_T *data, UINT8_T *error = 0) = 0; - virtual int Read1ByteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T *data, UINT8_T *error = 0) = 0; - - virtual int Read2ByteTx (PortHandler *port, UINT8_T id, UINT16_T address) = 0; - virtual int Read2ByteRx (PortHandler *port, UINT16_T *data, UINT8_T *error = 0) = 0; - virtual int Read2ByteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T *data, UINT8_T *error = 0) = 0; - - virtual int Read4ByteTx (PortHandler *port, UINT8_T id, UINT16_T address) = 0; - virtual int Read4ByteRx (PortHandler *port, UINT32_T *data, UINT8_T *error = 0) = 0; - virtual int Read4ByteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T *data, UINT8_T *error = 0) = 0; - - virtual int WriteTxOnly (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data) = 0; - virtual int WriteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error = 0) = 0; - - virtual int Write1ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T data) = 0; - virtual int Write1ByteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T data, UINT8_T *error = 0) = 0; - - virtual int Write2ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T data) = 0; - virtual int Write2ByteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T data, UINT8_T *error = 0) = 0; - - virtual int Write4ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T data) = 0; - virtual int Write4ByteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T data, UINT8_T *error = 0) = 0; - - virtual int RegWriteTxOnly (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data) = 0; - virtual int RegWriteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error = 0) = 0; - - virtual int SyncReadTx (PortHandler *port, UINT16_T start_address, UINT16_T data_length, UINT8_T *param, UINT16_T param_length) = 0; - // SyncReadRx -> GroupSyncRead class - // SyncReadTxRx -> GroupSyncRead class - - virtual int SyncWriteTxOnly (PortHandler *port, UINT16_T start_address, UINT16_T data_length, UINT8_T *param, UINT16_T param_length) = 0; - - virtual int BulkReadTx (PortHandler *port, UINT8_T *param, UINT16_T param_length) = 0; - // BulkReadRx -> GroupBulkRead class - // BulkReadTxRx -> GroupBulkRead class - - virtual int BulkWriteTxOnly (PortHandler *port, UINT8_T *param, UINT16_T param_length) = 0; -}; - -} - - -#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PACKETHANDLER_H_ */ diff --git a/dynamixel_sdk/include/dynamixel_sdk/PortHandler.h b/dynamixel_sdk/include/dynamixel_sdk/PortHandler.h deleted file mode 100644 index 6d3e6dd..0000000 --- a/dynamixel_sdk/include/dynamixel_sdk/PortHandler.h +++ /dev/null @@ -1,60 +0,0 @@ -/* - * PortHandler.h - * - * Created on: 2016. 1. 26. - * Author: zerom, leon - */ - -#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PORTHANDLER_H_ -#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PORTHANDLER_H_ - -#ifdef __linux__ -#define WINDECLSPEC -#elif defined(_WIN32) || defined(_WIN64) -#ifdef WINDLLEXPORT -#define WINDECLSPEC __declspec(dllexport) -#else -#define WINDECLSPEC __declspec(dllimport) -#endif -#endif - -#include "RobotisDef.h" - -namespace ROBOTIS -{ - -class WINDECLSPEC PortHandler -{ -public: - static const int DEFAULT_BAUDRATE = 1000000; - - static PortHandler *GetPortHandler(const char *port_name); - - bool is_using; - - virtual ~PortHandler() { } - - virtual bool OpenPort() = 0; - virtual void ClosePort() = 0; - virtual void ClearPort() = 0; - - virtual void SetPortName(const char* port_name) = 0; - virtual char *GetPortName() = 0; - - virtual bool SetBaudRate(const int baudrate) = 0; - virtual int GetBaudRate() = 0; - - virtual int GetBytesAvailable() = 0; - - virtual int ReadPort(UINT8_T *packet, int length) = 0; - virtual int WritePort(UINT8_T *packet, int length) = 0; - - virtual void SetPacketTimeout(UINT16_T packet_length) = 0; - virtual void SetPacketTimeout(double msec) = 0; - virtual bool IsPacketTimeout() = 0; -}; - -} - - -#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PORTHANDLER_H_ */ diff --git a/dynamixel_sdk/include/dynamixel_sdk/Protocol1PacketHandler.h b/dynamixel_sdk/include/dynamixel_sdk/Protocol1PacketHandler.h deleted file mode 100644 index 1932d27..0000000 --- a/dynamixel_sdk/include/dynamixel_sdk/Protocol1PacketHandler.h +++ /dev/null @@ -1,98 +0,0 @@ -/* - * Protocol1PacketHandler.h - * - * Created on: 2016. 1. 26. - * Author: zerom, leon - */ - -#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL1PACKETHANDLER_H_ -#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL1PACKETHANDLER_H_ - - -#include "PacketHandler.h" - -namespace ROBOTIS -{ - -class WINDECLSPEC Protocol1PacketHandler : public PacketHandler -{ -private: - static Protocol1PacketHandler *unique_instance_; - - Protocol1PacketHandler(); - -public: - static Protocol1PacketHandler *GetInstance() { return unique_instance_; } - - virtual ~Protocol1PacketHandler() { } - - float GetProtocolVersion() { return 1.0; } - - void PrintTxRxResult(int result); - void PrintRxPacketError(UINT8_T error); - - int TxPacket (PortHandler *port, UINT8_T *txpacket); - int RxPacket (PortHandler *port, UINT8_T *rxpacket); - int TxRxPacket (PortHandler *port, UINT8_T *txpacket, UINT8_T *rxpacket, UINT8_T *error = 0); - - int Ping (PortHandler *port, UINT8_T id, UINT8_T *error = 0); - int Ping (PortHandler *port, UINT8_T id, UINT16_T *model_number, UINT8_T *error = 0); - - // BroadcastPing - int BroadcastPing (PortHandler *port, std::vector &id_list); - - int Action (PortHandler *port, UINT8_T id); - int Reboot (PortHandler *port, UINT8_T id, UINT8_T *error = 0); - int FactoryReset (PortHandler *port, UINT8_T id, UINT8_T option, UINT8_T *error = 0); - - - int ReadTx (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length); - int ReadRx (PortHandler *port, UINT16_T length, UINT8_T *data, UINT8_T *error = 0); - int ReadTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error = 0); - - int Read1ByteTx (PortHandler *port, UINT8_T id, UINT16_T address); - int Read1ByteRx (PortHandler *port, UINT8_T *data, UINT8_T *error = 0); - int Read1ByteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T *data, UINT8_T *error = 0); - - int Read2ByteTx (PortHandler *port, UINT8_T id, UINT16_T address); - int Read2ByteRx (PortHandler *port, UINT16_T *data, UINT8_T *error = 0); - int Read2ByteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T *data, UINT8_T *error = 0); - - int Read4ByteTx (PortHandler *port, UINT8_T id, UINT16_T address); - int Read4ByteRx (PortHandler *port, UINT32_T *data, UINT8_T *error = 0); - int Read4ByteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T *data, UINT8_T *error = 0); - - int WriteTxOnly (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data); - int WriteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error = 0); - - int Write1ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T data); - int Write1ByteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T data, UINT8_T *error = 0); - - int Write2ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T data); - int Write2ByteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T data, UINT8_T *error = 0); - - int Write4ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T data); - int Write4ByteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T data, UINT8_T *error = 0); - - int RegWriteTxOnly (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data); - int RegWriteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error = 0); - - int SyncReadTx (PortHandler *port, UINT16_T start_address, UINT16_T data_length, UINT8_T *param, UINT16_T param_length); - // SyncReadRx -> GroupSyncRead class - // SyncReadTxRx -> GroupSyncRead class - - // param : ID1 DATA0 DATA1 ... DATAn ID2 DATA0 DATA1 ... DATAn ID3 DATA0 DATA1 ... DATAn - int SyncWriteTxOnly (PortHandler *port, UINT16_T start_address, UINT16_T data_length, UINT8_T *param, UINT16_T param_length); - - // param : LEN1 ID1 ADDR1 LEN2 ID2 ADDR2 ... - int BulkReadTx (PortHandler *port, UINT8_T *param, UINT16_T param_length); - // BulkReadRx -> GroupBulkRead class - // BulkReadTxRx -> GroupBulkRead class - - int BulkWriteTxOnly (PortHandler *port, UINT8_T *param, UINT16_T param_length); -}; - -} - - -#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL1PACKETHANDLER_H_ */ diff --git a/dynamixel_sdk/include/dynamixel_sdk/Protocol2PacketHandler.h b/dynamixel_sdk/include/dynamixel_sdk/Protocol2PacketHandler.h deleted file mode 100644 index 101413c..0000000 --- a/dynamixel_sdk/include/dynamixel_sdk/Protocol2PacketHandler.h +++ /dev/null @@ -1,103 +0,0 @@ -/* - * Protocol2PacketHandler.h - * - * Created on: 2016. 1. 26. - * Author: zerom, leon - */ - -#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL2PACKETHANDLER_H_ -#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL2PACKETHANDLER_H_ - - -#include "PacketHandler.h" - -namespace ROBOTIS -{ - -class WINDECLSPEC Protocol2PacketHandler : public PacketHandler -{ -private: - static Protocol2PacketHandler *unique_instance_; - - Protocol2PacketHandler(); - - UINT16_T UpdateCRC(UINT16_T crc_accum, UINT8_T *data_blk_ptr, UINT16_T data_blk_size); - void AddStuffing(UINT8_T *packet); - void RemoveStuffing(UINT8_T *packet); - -public: - static Protocol2PacketHandler *GetInstance() { return unique_instance_; } - - virtual ~Protocol2PacketHandler() { } - - float GetProtocolVersion() { return 2.0; } - - void PrintTxRxResult(int result); - void PrintRxPacketError(UINT8_T error); - - int TxPacket (PortHandler *port, UINT8_T *txpacket); - int RxPacket (PortHandler *port, UINT8_T *rxpacket); - int TxRxPacket (PortHandler *port, UINT8_T *txpacket, UINT8_T *rxpacket, UINT8_T *error = 0); - - int Ping (PortHandler *port, UINT8_T id, UINT8_T *error = 0); - int Ping (PortHandler *port, UINT8_T id, UINT16_T *model_number, UINT8_T *error = 0); - - // BroadcastPing - int BroadcastPing (PortHandler *port, std::vector &id_list); - - int Action (PortHandler *port, UINT8_T id); - int Reboot (PortHandler *port, UINT8_T id, UINT8_T *error = 0); - int FactoryReset (PortHandler *port, UINT8_T id, UINT8_T option, UINT8_T *error = 0); - - - int ReadTx (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length); - int ReadRx (PortHandler *port, UINT16_T length, UINT8_T *data, UINT8_T *error = 0); - int ReadTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error = 0); - - int Read1ByteTx (PortHandler *port, UINT8_T id, UINT16_T address); - int Read1ByteRx (PortHandler *port, UINT8_T *data, UINT8_T *error = 0); - int Read1ByteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T *data, UINT8_T *error = 0); - - int Read2ByteTx (PortHandler *port, UINT8_T id, UINT16_T address); - int Read2ByteRx (PortHandler *port, UINT16_T *data, UINT8_T *error = 0); - int Read2ByteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T *data, UINT8_T *error = 0); - - int Read4ByteTx (PortHandler *port, UINT8_T id, UINT16_T address); - int Read4ByteRx (PortHandler *port, UINT32_T *data, UINT8_T *error = 0); - int Read4ByteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T *data, UINT8_T *error = 0); - - int WriteTxOnly (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data); - int WriteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error = 0); - - int Write1ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T data); - int Write1ByteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T data, UINT8_T *error = 0); - - int Write2ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T data); - int Write2ByteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T data, UINT8_T *error = 0); - - int Write4ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T data); - int Write4ByteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T data, UINT8_T *error = 0); - - int RegWriteTxOnly (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data); - int RegWriteTxRx (PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error = 0); - - int SyncReadTx (PortHandler *port, UINT16_T start_address, UINT16_T data_length, UINT8_T *param, UINT16_T param_length); - // SyncReadRx -> GroupSyncRead class - // SyncReadTxRx -> GroupSyncRead class - - // param : ID1 DATA0 DATA1 ... DATAn ID2 DATA0 DATA1 ... DATAn ID3 DATA0 DATA1 ... DATAn - int SyncWriteTxOnly (PortHandler *port, UINT16_T start_address, UINT16_T data_length, UINT8_T *param, UINT16_T param_length); - - // param : ID1 ADDR_L1 ADDR_H1 LEN_L1 LEN_H1 ID2 ADDR_L2 ADDR_H2 LEN_L2 LEN_H2 ... - int BulkReadTx (PortHandler *port, UINT8_T *param, UINT16_T param_length); - // BulkReadRx -> GroupBulkRead class - // BulkReadTxRx -> GroupBulkRead class - - // param : ID1 START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H DATA0 DATA1 ... DATAn ID2 START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H DATA0 DATA1 ... DATAn - int BulkWriteTxOnly (PortHandler *port, UINT8_T *param, UINT16_T param_length); -}; - -} - - -#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL2PACKETHANDLER_H_ */ diff --git a/dynamixel_sdk/include/dynamixel_sdk/RobotisDef.h b/dynamixel_sdk/include/dynamixel_sdk/RobotisDef.h deleted file mode 100644 index 95ab804..0000000 --- a/dynamixel_sdk/include/dynamixel_sdk/RobotisDef.h +++ /dev/null @@ -1,21 +0,0 @@ -/* - * RobotisDef.h - * - * Created on: 2016. 1. 27. - * Author: zerom, leon - */ - -#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_ROBOTISDEF_H_ -#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_ROBOTISDEF_H_ - - -typedef char INT8_T; -typedef short int INT16_T; -typedef int INT32_T; - -typedef unsigned char UINT8_T; -typedef unsigned short int UINT16_T; -typedef unsigned int UINT32_T; - - -#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_ROBOTISDEF_H_ */ diff --git a/dynamixel_sdk/include/dynamixel_sdk_linux/PortHandlerLinux.h b/dynamixel_sdk/include/dynamixel_sdk_linux/PortHandlerLinux.h deleted file mode 100644 index 6ff516e..0000000 --- a/dynamixel_sdk/include/dynamixel_sdk_linux/PortHandlerLinux.h +++ /dev/null @@ -1,62 +0,0 @@ -/* - * PortHandlerLinux.h - * - * Created on: 2016. 1. 26. - * Author: zerom, leon - */ - -#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_LINUX_PORTHANDLERLINUX_H_ -#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_LINUX_PORTHANDLERLINUX_H_ - - -#include "dynamixel_sdk/PortHandler.h" - -namespace ROBOTIS -{ - -class PortHandlerLinux : public PortHandler -{ -private: - int socket_fd_; - int baudrate_; - char port_name_[30]; - - double packet_start_time_; - double packet_timeout_; - double tx_time_per_byte; - - bool SetupPort(const int cflag_baud); - bool SetCustomBaudrate(int speed); - int GetCFlagBaud(const int baudrate); - - double GetCurrentTime(); - double GetTimeSinceStart(); - -public: - PortHandlerLinux(const char *port_name); - virtual ~PortHandlerLinux() { ClosePort(); } - - bool OpenPort(); - void ClosePort(); - void ClearPort(); - - void SetPortName(const char *port_name); - char *GetPortName(); - - bool SetBaudRate(const int baudrate); - int GetBaudRate(); - - int GetBytesAvailable(); - - int ReadPort(UINT8_T *packet, int length); - int WritePort(UINT8_T *packet, int length); - - void SetPacketTimeout(UINT16_T packet_length); - void SetPacketTimeout(double msec); - bool IsPacketTimeout(); -}; - -} - - -#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_LINUX_PORTHANDLERLINUX_H_ */ diff --git a/dynamixel_sdk/package.xml b/dynamixel_sdk/package.xml deleted file mode 100644 index ae8e683..0000000 --- a/dynamixel_sdk/package.xml +++ /dev/null @@ -1,15 +0,0 @@ - - - dynamixel_sdk - 0.1.0 - The dynamixel_sdk package - - robotis - BSD - - ROBOTIS - - catkin - roscpp - roscpp - \ No newline at end of file diff --git a/dynamixel_sdk/src/dynamixel_sdk/GroupBulkRead.cpp b/dynamixel_sdk/src/dynamixel_sdk/GroupBulkRead.cpp deleted file mode 100644 index f248ee8..0000000 --- a/dynamixel_sdk/src/dynamixel_sdk/GroupBulkRead.cpp +++ /dev/null @@ -1,199 +0,0 @@ -/* - * GroupBulkRead.cpp - * - * Created on: 2016. 1. 28. - * Author: zerom, leon - */ -#if defined(_WIN32) || defined(_WIN64) -#define WINDLLEXPORT -#endif - -#include -#include -#include "dynamixel_sdk/GroupBulkRead.h" - -using namespace ROBOTIS; - -GroupBulkRead::GroupBulkRead(PortHandler *port, PacketHandler *ph) - : port_(port), - ph_(ph), - last_result_(false), - is_param_changed_(false), - param_(0) -{ - ClearParam(); -} - -void GroupBulkRead::MakeParam() -{ - if(id_list_.size() == 0) - return; - - if(param_ != 0) - delete[] param_; - param_ = 0; - - if(ph_->GetProtocolVersion() == 1.0) - param_ = new UINT8_T[id_list_.size() * 3]; // ID(1) + ADDR(1) + LENGTH(1) - else // 2.0 - param_ = new UINT8_T[id_list_.size() * 5]; // ID(1) + ADDR(2) + LENGTH(2) - - int _idx = 0; - for(unsigned int _i = 0; _i < id_list_.size(); _i++) - { - UINT8_T _id = id_list_[_i]; - if(ph_->GetProtocolVersion() == 1.0) - { - param_[_idx++] = (UINT8_T)length_list_[_id]; // LEN - param_[_idx++] = _id; // ID - param_[_idx++] = (UINT8_T)address_list_[_id]; // ADDR - } - else // 2.0 - { - param_[_idx++] = _id; // ID - param_[_idx++] = DXL_LOBYTE(address_list_[_id]); // ADDR_L - param_[_idx++] = DXL_HIBYTE(address_list_[_id]); // ADDR_H - param_[_idx++] = DXL_LOBYTE(length_list_[_id]); // LEN_L - param_[_idx++] = DXL_HIBYTE(length_list_[_id]); // LEN_H - } - } -} - -bool GroupBulkRead::AddParam(UINT8_T id, UINT16_T start_address, UINT16_T data_length) -{ - if(std::find(id_list_.begin(), id_list_.end(), id) != id_list_.end()) // id already exist - return false; - - id_list_.push_back(id); - length_list_[id] = data_length; - address_list_[id] = start_address; - data_list_[id] = new UINT8_T[data_length]; - - is_param_changed_ = true; - return true; -} - -void GroupBulkRead::RemoveParam(UINT8_T id) -{ - std::vector::iterator it = std::find(id_list_.begin(), id_list_.end(), id); - if(it == id_list_.end()) // NOT exist - return; - - id_list_.erase(it); - address_list_.erase(id); - length_list_.erase(id); - delete[] data_list_[id]; - data_list_.erase(id); - - is_param_changed_ = true; -} - -void GroupBulkRead::ClearParam() -{ - if(id_list_.size() == 0) - return; - - for(unsigned int _i = 0; _i < id_list_.size(); _i++) - delete[] data_list_[id_list_[_i]]; - - id_list_.clear(); - address_list_.clear(); - length_list_.clear(); - data_list_.clear(); - if(param_ != 0) - delete[] param_; - param_ = 0; -} - -int GroupBulkRead::TxPacket() -{ - if(id_list_.size() == 0) - return COMM_NOT_AVAILABLE; - - if(is_param_changed_ == true) - MakeParam(); - - if(ph_->GetProtocolVersion() == 1.0) - return ph_->BulkReadTx(port_, param_, id_list_.size() * 3); - else // 2.0 - return ph_->BulkReadTx(port_, param_, id_list_.size() * 5); -} - -int GroupBulkRead::RxPacket() -{ - int _cnt = id_list_.size(); - int _result = COMM_RX_FAIL; - - last_result_ = false; - - if(_cnt == 0) - return COMM_NOT_AVAILABLE; - - for(int _i = 0; _i < _cnt; _i++) - { - UINT8_T _id = id_list_[_i]; - - _result = ph_->ReadRx(port_, length_list_[_id], data_list_[_id]); - if(_result != COMM_SUCCESS) - { - fprintf(stderr, "[GroupBulkRead::RxPacket] ID %d result : %d !!!!!!!!!!\n", _id, _result); - return _result; - } - } - - if(_result == COMM_SUCCESS) - last_result_ = true; - - return _result; -} - -int GroupBulkRead::TxRxPacket() -{ - int _result = COMM_TX_FAIL; - - _result = TxPacket(); - if(_result != COMM_SUCCESS) - return _result; - - return RxPacket(); -} - -bool GroupBulkRead::IsAvailable(UINT8_T id, UINT16_T address, UINT16_T data_length) -{ - UINT16_T _start_addr, _data_length; - - if(last_result_ == false || data_list_.find(id) == data_list_.end()) - return false; - - _start_addr = address_list_[id]; - _data_length = length_list_[id]; - - if(address < _start_addr || _start_addr + _data_length - data_length < address) - return false; - - return true; -} - -UINT32_T GroupBulkRead::GetData(UINT8_T id, UINT16_T address, UINT16_T data_length) -{ - if(IsAvailable(id, address, data_length) == false) - return 0; - - UINT16_T _start_addr = address_list_[id]; - - switch(data_length) - { - case 1: - return data_list_[id][address - _start_addr]; - - case 2: - return DXL_MAKEWORD(data_list_[id][address - _start_addr], data_list_[id][address - _start_addr + 1]); - - case 4: - return DXL_MAKEDWORD(DXL_MAKEWORD(data_list_[id][address - _start_addr + 0], data_list_[id][address - _start_addr + 1]), - DXL_MAKEWORD(data_list_[id][address - _start_addr + 2], data_list_[id][address - _start_addr + 3])); - - default: - return 0; - } -} diff --git a/dynamixel_sdk/src/dynamixel_sdk/GroupBulkWrite.cpp b/dynamixel_sdk/src/dynamixel_sdk/GroupBulkWrite.cpp deleted file mode 100644 index 973793b..0000000 --- a/dynamixel_sdk/src/dynamixel_sdk/GroupBulkWrite.cpp +++ /dev/null @@ -1,137 +0,0 @@ -/* - * GroupBulkWrite.cpp - * - * Created on: 2016. 2. 2. - * Author: zerom, leon - */ -#if defined(_WIN32) || defined(_WIN64) -#define WINDLLEXPORT -#endif - -#include -#include "dynamixel_sdk/GroupBulkWrite.h" - -using namespace ROBOTIS; - -GroupBulkWrite::GroupBulkWrite(PortHandler *port, PacketHandler *ph) - : port_(port), - ph_(ph), - is_param_changed_(false), - param_(0), - param_length_(0) -{ - ClearParam(); -} - -void GroupBulkWrite::MakeParam() -{ - if(ph_->GetProtocolVersion() == 1.0 || id_list_.size() == 0) - return; - - if(param_ != 0) - delete[] param_; - param_ = 0; - - param_length_ = 0; - for(unsigned int _i = 0; _i < id_list_.size(); _i++) - param_length_ += 1 + 2 + 2 + length_list_[id_list_[_i]]; - - param_ = new UINT8_T[param_length_]; - - int _idx = 0; - for(unsigned int _i = 0; _i < id_list_.size(); _i++) - { - UINT8_T _id = id_list_[_i]; - if(data_list_[_id] == 0) - return; - - param_[_idx++] = _id; - param_[_idx++] = DXL_LOBYTE(address_list_[_id]); - param_[_idx++] = DXL_HIBYTE(address_list_[_id]); - param_[_idx++] = DXL_LOBYTE(length_list_[_id]); - param_[_idx++] = DXL_HIBYTE(length_list_[_id]); - for(int _c = 0; _c < length_list_[_id]; _c++) - param_[_idx++] = (data_list_[_id])[_c]; - } -} - -bool GroupBulkWrite::AddParam(UINT8_T id, UINT16_T start_address, UINT16_T data_length, UINT8_T *data) -{ - if(ph_->GetProtocolVersion() == 1.0) - return false; - - if(std::find(id_list_.begin(), id_list_.end(), id) != id_list_.end()) // id already exist - return false; - - id_list_.push_back(id); - address_list_[id] = start_address; - length_list_[id] = data_length; - data_list_[id] = new UINT8_T[data_length]; - for(int _c = 0; _c < data_length; _c++) - data_list_[id][_c] = data[_c]; - - is_param_changed_ = true; - return true; -} -void GroupBulkWrite::RemoveParam(UINT8_T id) -{ - if(ph_->GetProtocolVersion() == 1.0) - return; - - std::vector::iterator it = std::find(id_list_.begin(), id_list_.end(), id); - if(it == id_list_.end()) // NOT exist - return; - - id_list_.erase(it); - address_list_.erase(id); - length_list_.erase(id); - delete[] data_list_[id]; - data_list_.erase(id); - - is_param_changed_ = true; -} -bool GroupBulkWrite::ChangeParam(UINT8_T id, UINT16_T start_address, UINT16_T data_length, UINT8_T *data) -{ - if(ph_->GetProtocolVersion() == 1.0) - return false; - - std::vector::iterator it = std::find(id_list_.begin(), id_list_.end(), id); - if(it == id_list_.end()) // NOT exist - return false; - - address_list_[id] = start_address; - length_list_[id] = data_length; - delete[] data_list_[id]; - data_list_[id] = new UINT8_T[data_length]; - for(int _c = 0; _c < data_length; _c++) - data_list_[id][_c] = data[_c]; - - is_param_changed_ = true; - return true; -} -void GroupBulkWrite::ClearParam() -{ - if(ph_->GetProtocolVersion() == 1.0 || id_list_.size() == 0) - return; - - for(unsigned int _i = 0; _i < id_list_.size(); _i++) - delete[] data_list_[id_list_[_i]]; - - id_list_.clear(); - address_list_.clear(); - length_list_.clear(); - data_list_.clear(); - if(param_ != 0) - delete[] param_; - param_ = 0; -} -int GroupBulkWrite::TxPacket() -{ - if(ph_->GetProtocolVersion() == 1.0 || id_list_.size() == 0) - return COMM_NOT_AVAILABLE; - - if(is_param_changed_ == true) - MakeParam(); - - return ph_->BulkWriteTxOnly(port_, param_, param_length_); -} diff --git a/dynamixel_sdk/src/dynamixel_sdk/GroupSyncRead.cpp b/dynamixel_sdk/src/dynamixel_sdk/GroupSyncRead.cpp deleted file mode 100644 index fede283..0000000 --- a/dynamixel_sdk/src/dynamixel_sdk/GroupSyncRead.cpp +++ /dev/null @@ -1,172 +0,0 @@ -/* - * GroupSyncRead.cpp - * - * Created on: 2016. 2. 2. - * Author: zerom, leon - */ -#if defined(_WIN32) || defined(_WIN64) -#define WINDLLEXPORT -#endif - -#include -#include "dynamixel_sdk/GroupSyncRead.h" - -using namespace ROBOTIS; - -GroupSyncRead::GroupSyncRead(PortHandler *port, PacketHandler *ph, UINT16_T start_address, UINT16_T data_length) - : port_(port), - ph_(ph), - last_result_(false), - is_param_changed_(false), - param_(0), - start_address_(start_address), - data_length_(data_length) -{ - ClearParam(); -} - -void GroupSyncRead::MakeParam() -{ - if(ph_->GetProtocolVersion() == 1.0 || id_list_.size() == 0) - return; - - if(param_ != 0) - delete[] param_; - param_ = 0; - - param_ = new UINT8_T[id_list_.size() * 1]; // ID(1) - - int _idx = 0; - for(unsigned int _i = 0; _i < id_list_.size(); _i++) - param_[_idx++] = id_list_[_i]; -} - -bool GroupSyncRead::AddParam(UINT8_T id) -{ - if(ph_->GetProtocolVersion() == 1.0) - return false; - - if(std::find(id_list_.begin(), id_list_.end(), id) != id_list_.end()) // id already exist - return false; - - id_list_.push_back(id); - data_list_[id] = new UINT8_T[data_length_]; - - is_param_changed_ = true; - return true; -} -void GroupSyncRead::RemoveParam(UINT8_T id) -{ - if(ph_->GetProtocolVersion() == 1.0) - return; - - std::vector::iterator it = std::find(id_list_.begin(), id_list_.end(), id); - if(it == id_list_.end()) // NOT exist - return; - - id_list_.erase(it); - delete[] data_list_[id]; - data_list_.erase(id); - - is_param_changed_ = true; -} -void GroupSyncRead::ClearParam() -{ - if(ph_->GetProtocolVersion() == 1.0 || id_list_.size() == 0) - return; - - for(unsigned int _i = 0; _i < id_list_.size(); _i++) - delete[] data_list_[id_list_[_i]]; - - id_list_.clear(); - data_list_.clear(); - if(param_ != 0) - delete[] param_; - param_ = 0; -} - -int GroupSyncRead::TxPacket() -{ - if(ph_->GetProtocolVersion() == 1.0 || id_list_.size() == 0) - return COMM_NOT_AVAILABLE; - - if(is_param_changed_ == true) - MakeParam(); - - return ph_->SyncReadTx(port_, start_address_, data_length_, param_, (UINT16_T)id_list_.size() * 1); -} - -int GroupSyncRead::RxPacket() -{ - last_result_ = false; - - if(ph_->GetProtocolVersion() == 1.0) - return COMM_NOT_AVAILABLE; - - int _cnt = id_list_.size(); - int _result = COMM_RX_FAIL; - - if(_cnt == 0) - return COMM_NOT_AVAILABLE; - - for(int _i = 0; _i < _cnt; _i++) - { - UINT8_T _id = id_list_[_i]; - - _result = ph_->ReadRx(port_, data_length_, data_list_[_id]); - if(_result != COMM_SUCCESS) - return _result; - } - - if(_result == COMM_SUCCESS) - last_result_ = true; - - return _result; -} - -int GroupSyncRead::TxRxPacket() -{ - if(ph_->GetProtocolVersion() == 1.0) - return COMM_NOT_AVAILABLE; - - int _result = COMM_TX_FAIL; - - _result = TxPacket(); - if(_result != COMM_SUCCESS) - return _result; - - return RxPacket(); -} - -bool GroupSyncRead::IsAvailable(UINT8_T id, UINT16_T address, UINT16_T data_length) -{ - if(ph_->GetProtocolVersion() == 1.0 || last_result_ == false || data_list_.find(id) == data_list_.end()) - return false; - - if(address < start_address_ || start_address_ + data_length_ - data_length < address) - return false; - - return true; -} - -UINT32_T GroupSyncRead::GetData(UINT8_T id, UINT16_T address, UINT16_T data_length) -{ - if(IsAvailable(id, address, data_length) == false) - return 0; - - switch(data_length) - { - case 1: - return data_list_[id][address - start_address_]; - - case 2: - return DXL_MAKEWORD(data_list_[id][address - start_address_], data_list_[id][address - start_address_ + 1]); - - case 4: - return DXL_MAKEDWORD(DXL_MAKEWORD(data_list_[id][address - start_address_ + 0], data_list_[id][address - start_address_ + 1]), - DXL_MAKEWORD(data_list_[id][address - start_address_ + 2], data_list_[id][address - start_address_ + 3])); - - default: - return 0; - } -} diff --git a/dynamixel_sdk/src/dynamixel_sdk/GroupSyncWrite.cpp b/dynamixel_sdk/src/dynamixel_sdk/GroupSyncWrite.cpp deleted file mode 100644 index 8a61435..0000000 --- a/dynamixel_sdk/src/dynamixel_sdk/GroupSyncWrite.cpp +++ /dev/null @@ -1,117 +0,0 @@ -/* - * GroupSyncWrite.cpp - * - * Created on: 2016. 1. 28. - * Author: zerom, leon - */ -#if defined(_WIN32) || defined(_WIN64) -#define WINDLLEXPORT -#endif - -#include -#include "dynamixel_sdk/GroupSyncWrite.h" - -using namespace ROBOTIS; - -GroupSyncWrite::GroupSyncWrite(PortHandler *port, PacketHandler *ph, UINT16_T start_address, UINT16_T data_length) - : port_(port), - ph_(ph), - is_param_changed_(false), - param_(0), - start_address_(start_address), - data_length_(data_length) -{ - ClearParam(); -} - -void GroupSyncWrite::MakeParam() -{ - if(id_list_.size() == 0) - return; - - if(param_ != 0) - delete[] param_; - param_ = 0; - - param_ = new UINT8_T[id_list_.size() * (1 + data_length_)]; // ID(1) + DATA(data_length) - - int _idx = 0; - for(unsigned int _i = 0; _i < id_list_.size(); _i++) - { - UINT8_T _id = id_list_[_i]; - if(data_list_[_id] == 0) - return; - - param_[_idx++] = _id; - for(int _c = 0; _c < data_length_; _c++) - param_[_idx++] = (data_list_[_id])[_c]; - } -} - -bool GroupSyncWrite::AddParam(UINT8_T id, UINT8_T *data) -{ - if(std::find(id_list_.begin(), id_list_.end(), id) != id_list_.end()) // id already exist - return false; - - id_list_.push_back(id); - data_list_[id] = new UINT8_T[data_length_]; - for(int _c = 0; _c < data_length_; _c++) - data_list_[id][_c] = data[_c]; - - is_param_changed_ = true; - return true; -} - -void GroupSyncWrite::RemoveParam(UINT8_T id) -{ - std::vector::iterator it = std::find(id_list_.begin(), id_list_.end(), id); - if(it == id_list_.end()) // NOT exist - return; - - id_list_.erase(it); - delete[] data_list_[id]; - data_list_.erase(id); - - is_param_changed_ = true; -} - -bool GroupSyncWrite::ChangeParam(UINT8_T id, UINT8_T *data) -{ - std::vector::iterator it = std::find(id_list_.begin(), id_list_.end(), id); - if(it == id_list_.end()) // NOT exist - return false; - - delete[] data_list_[id]; - data_list_[id] = new UINT8_T[data_length_]; - for(int _c = 0; _c < data_length_; _c++) - data_list_[id][_c] = data[_c]; - - is_param_changed_ = true; - return true; -} - -void GroupSyncWrite::ClearParam() -{ - if(id_list_.size() == 0) - return; - - for(unsigned int _i = 0; _i < id_list_.size(); _i++) - delete[] data_list_[id_list_[_i]]; - - id_list_.clear(); - data_list_.clear(); - if(param_ != 0) - delete[] param_; - param_ = 0; -} - -int GroupSyncWrite::TxPacket() -{ - if(id_list_.size() == 0) - return COMM_NOT_AVAILABLE; - - if(is_param_changed_ == true) - MakeParam(); - - return ph_->SyncWriteTxOnly(port_, start_address_, data_length_, param_, id_list_.size() * (1 + data_length_)); -} diff --git a/dynamixel_sdk/src/dynamixel_sdk/PacketHandler.cpp b/dynamixel_sdk/src/dynamixel_sdk/PacketHandler.cpp deleted file mode 100644 index 8ad1c48..0000000 --- a/dynamixel_sdk/src/dynamixel_sdk/PacketHandler.cpp +++ /dev/null @@ -1,25 +0,0 @@ -/* - * PacketHandler.cpp - * - * Created on: 2016. 1. 26. - * Author: zerom, leon - */ -#if defined(_WIN32) || defined(_WIN64) -#define WINDLLEXPORT -#endif - -#include "dynamixel_sdk/PacketHandler.h" -#include "dynamixel_sdk/Protocol1PacketHandler.h" -#include "dynamixel_sdk/Protocol2PacketHandler.h" - -using namespace ROBOTIS; - -PacketHandler *PacketHandler::GetPacketHandler(float protocol_version) -{ - if(protocol_version == 1.0) - return (PacketHandler *)(Protocol1PacketHandler::GetInstance()); - else if(protocol_version == 2.0) - return (PacketHandler *)(Protocol2PacketHandler::GetInstance()); - - return (PacketHandler *)(Protocol2PacketHandler::GetInstance()); -} diff --git a/dynamixel_sdk/src/dynamixel_sdk/PortHandler.cpp b/dynamixel_sdk/src/dynamixel_sdk/PortHandler.cpp deleted file mode 100644 index ff4355c..0000000 --- a/dynamixel_sdk/src/dynamixel_sdk/PortHandler.cpp +++ /dev/null @@ -1,32 +0,0 @@ -/* - * PortHandler.cpp - * - * Created on: 2016. 2. 5. - * Author: zerom, leon - */ -#if defined(_WIN32) || defined(_WIN64) -#define WINDLLEXPORT -#endif - -#include "dynamixel_sdk/PortHandler.h" - -#ifdef __linux__ - #include "dynamixel_sdk_linux/PortHandlerLinux.h" -#endif - -#if defined(_WIN32) || defined(_WIN64) - #include "dynamixel_sdk_windows/PortHandlerWindows.h" -#endif - -using namespace ROBOTIS; - -PortHandler *PortHandler::GetPortHandler(const char *port_name) -{ -#ifdef __linux__ - return (PortHandler *)(new PortHandlerLinux(port_name)); -#endif - -#if defined(_WIN32) || defined(_WIN64) - return (PortHandler *)(new PortHandlerWindows(port_name)); -#endif -} diff --git a/dynamixel_sdk/src/dynamixel_sdk/Protocol1PacketHandler.cpp b/dynamixel_sdk/src/dynamixel_sdk/Protocol1PacketHandler.cpp deleted file mode 100644 index 66877e7..0000000 --- a/dynamixel_sdk/src/dynamixel_sdk/Protocol1PacketHandler.cpp +++ /dev/null @@ -1,670 +0,0 @@ -/* - * Protocol1PacketHandler.cpp - * - * Created on: 2016. 1. 26. - * Author: zerom, leon - */ -#if defined(_WIN32) || defined(_WIN64) -#define WINDLLEXPORT -#endif - -#include -#include -#include "dynamixel_sdk/Protocol1PacketHandler.h" - -#define TXPACKET_MAX_LEN (250) -#define RXPACKET_MAX_LEN (250) - -///////////////// for Protocol 1.0 Packet ///////////////// -#define PKT_HEADER0 0 -#define PKT_HEADER1 1 -#define PKT_ID 2 -#define PKT_LENGTH 3 -#define PKT_INSTRUCTION 4 -#define PKT_ERROR 4 -#define PKT_PARAMETER0 5 - -///////////////// Protocol 1.0 Error bit ///////////////// -#define ERRBIT_VOLTAGE 1 // Supplied voltage is out of the range (operating volatage set in the control table) -#define ERRBIT_ANGLE 2 // Goal position is written out of the range (from CW angle limit to CCW angle limit) -#define ERRBIT_OVERHEAT 4 // Temperature is out of the range (operating temperature set in the control table) -#define ERRBIT_RANGE 8 // Command(setting value) is out of the range for use. -#define ERRBIT_CHECKSUM 16 // Instruction packet checksum is incorrect. -#define ERRBIT_OVERLOAD 32 // The current load cannot be controlled by the set torque. -#define ERRBIT_INSTRUCTION 64 // Undefined instruction or delivering the action command without the reg_write command. - -using namespace ROBOTIS; - -Protocol1PacketHandler *Protocol1PacketHandler::unique_instance_ = new Protocol1PacketHandler(); - -Protocol1PacketHandler::Protocol1PacketHandler() { } - -void Protocol1PacketHandler::PrintTxRxResult(int result) -{ - switch(result) - { - case COMM_SUCCESS: - printf("[TxRxResult] Communication success.\n"); - break; - - case COMM_PORT_BUSY: - printf("[TxRxResult] Port is in use!\n"); - break; - - case COMM_TX_FAIL: - printf("[TxRxResult] Failed transmit instruction packet!\n"); - break; - - case COMM_RX_FAIL: - printf("[TxRxResult] Failed get status packet from device!\n"); - break; - - case COMM_TX_ERROR: - printf("[TxRxResult] Incorrect instruction packet!\n"); - break; - - case COMM_RX_WAITING: - printf("[TxRxResult] Now recieving status packet!\n"); - break; - - case COMM_RX_TIMEOUT: - printf("[TxRxResult] There is no status packet!\n"); - break; - - case COMM_RX_CORRUPT: - printf("[TxRxResult] Incorrect status packet!\n"); - break; - - case COMM_NOT_AVAILABLE: - printf("[TxRxResult] Protocol does not support This function!\n"); - break; - - default: - break; - } -} - -void Protocol1PacketHandler::PrintRxPacketError(UINT8_T error) -{ - if(error & ERRBIT_VOLTAGE) - printf("[RxPacketError] Input voltage error!\n"); - - if(error & ERRBIT_ANGLE) - printf("[RxPacketError] Angle limit error!\n"); - - if(error & ERRBIT_OVERHEAT) - printf("[RxPacketError] Overheat error!\n"); - - if(error & ERRBIT_RANGE) - printf("[RxPacketError] Out of range error!\n"); - - if(error & ERRBIT_CHECKSUM) - printf("[RxPacketError] Checksum error!\n"); - - if(error & ERRBIT_OVERLOAD) - printf("[RxPacketError] Overload error!\n"); - - if(error & ERRBIT_INSTRUCTION) - printf("[RxPacketError] Instruction code error!\n"); -} - -int Protocol1PacketHandler::TxPacket(PortHandler *port, UINT8_T *txpacket) -{ - UINT8_T _checksum = 0; - UINT8_T _total_packet_length = txpacket[PKT_LENGTH] + 4; // 4: HEADER0 HEADER1 ID LENGTH - UINT8_T _written_packet_length = 0; - - if(port->is_using) - return COMM_PORT_BUSY; - port->is_using = true; - - // check max packet length - if(_total_packet_length > TXPACKET_MAX_LEN) - { - port->is_using = false; - return COMM_TX_ERROR; - } - - // make packet header - txpacket[PKT_HEADER0] = 0xFF; - txpacket[PKT_HEADER1] = 0xFF; - - // add a checksum to the packet - for(int _idx = 2; _idx < _total_packet_length - 1; _idx++) // except header, checksum - _checksum += txpacket[_idx]; - txpacket[_total_packet_length - 1] = ~_checksum; - - // tx packet - port->ClearPort(); - _written_packet_length = port->WritePort(txpacket, _total_packet_length); - if(_total_packet_length != _written_packet_length) - { - port->is_using = false; - return COMM_TX_FAIL; - } - - return COMM_SUCCESS; -} - -int Protocol1PacketHandler::RxPacket(PortHandler *port, UINT8_T *rxpacket) -{ - int _result = COMM_TX_FAIL; - - UINT8_T _checksum = 0; - UINT8_T _rx_length = 0; - UINT8_T _wait_length = 6; // minimum length ( HEADER0 HEADER1 ID LENGTH ERROR CHKSUM ) - - while(true) - { - _rx_length += port->ReadPort(&rxpacket[_rx_length], _wait_length - _rx_length); - if(_rx_length >= _wait_length) - { - UINT8_T _idx = 0; - - // find packet header - for(_idx = 0; _idx < (_rx_length - 1); _idx++) - { - if(rxpacket[_idx] == 0xFF && rxpacket[_idx+1] == 0xFF) - break; - } - - if(_idx == 0) // found at the beginning of the packet - { - if(rxpacket[PKT_ID] > 0xFD || // unavailable ID - rxpacket[PKT_LENGTH] > RXPACKET_MAX_LEN || // unavailable Length - rxpacket[PKT_ERROR] >= 0x64) // unavailable Error - { - // remove the first byte in the packet - for(UINT8_T _s = 0; _s < _rx_length - 1; _s++) - rxpacket[_s] = rxpacket[1 + _s]; - //memcpy(&rxpacket[0], &rxpacket[_idx], _rx_length - _idx); - _rx_length -= 1; - continue; - } - - // re-calculate the exact length of the rx packet - if(_wait_length != rxpacket[PKT_LENGTH] + PKT_LENGTH + 1) - { - _wait_length = rxpacket[PKT_LENGTH] + PKT_LENGTH + 1; - continue; - } - - if(_rx_length < _wait_length) - { - // check timeout - if(port->IsPacketTimeout() == true) - { - if(_rx_length == 0) - _result = COMM_RX_TIMEOUT; - else - _result = COMM_RX_CORRUPT; - break; - } - else - continue; - } - - // calculate checksum - for(int _i = 2; _i < _wait_length - 1; _i++) // except header, checksum - _checksum += rxpacket[_i]; - _checksum = ~_checksum; - - // verify checksum - if(rxpacket[_wait_length - 1] == _checksum) - _result = COMM_SUCCESS; - else - _result = COMM_RX_CORRUPT; - break; - } - else - { - // remove unnecessary packets - for(UINT8_T _s = 0; _s < _rx_length - _idx; _s++) - rxpacket[_s] = rxpacket[_idx + _s]; - //memcpy(&rxpacket[0], &rxpacket[_idx], _rx_length - _idx); - _rx_length -= _idx; - } - } - else - { - // check timeout - if(port->IsPacketTimeout() == true) - { - if(_rx_length == 0) - _result = COMM_RX_TIMEOUT; - else - _result = COMM_RX_CORRUPT; - break; - } - } - } - port->is_using = false; - - return _result; -} - -// NOT for BulkRead instruction -int Protocol1PacketHandler::TxRxPacket(PortHandler *port, UINT8_T *txpacket, UINT8_T *rxpacket, UINT8_T *error) -{ - int _result = COMM_TX_FAIL; - - // tx packet - _result = TxPacket(port, txpacket); - if(_result != COMM_SUCCESS) - return _result; - - // (ID == Broadcast ID && NOT BulkRead) == no need to wait for status packet - // (Instruction == Action) == no need to wait for status packet - if((txpacket[PKT_ID] == BROADCAST_ID && txpacket[PKT_INSTRUCTION] != INST_BULK_READ) || - (txpacket[PKT_INSTRUCTION] == INST_ACTION)) - { - port->is_using = false; - return _result; - } - - // set packet timeout - if(txpacket[PKT_INSTRUCTION] == INST_READ) - port->SetPacketTimeout((UINT16_T)(txpacket[PKT_PARAMETER0+1] + 6)); - else - port->SetPacketTimeout((UINT16_T)6); - - // rx packet - _result = RxPacket(port, rxpacket); - // check txpacket ID == rxpacket ID - if(txpacket[PKT_ID] != rxpacket[PKT_ID]) - _result = RxPacket(port, rxpacket); - - if(_result == COMM_SUCCESS && txpacket[PKT_ID] != BROADCAST_ID) - { - if(error != 0) - *error = (UINT8_T)rxpacket[PKT_ERROR]; - } - - return _result; -} - -int Protocol1PacketHandler::Ping(PortHandler *port, UINT8_T id, UINT8_T *error) -{ - return Ping(port, id, 0, error); -} - -int Protocol1PacketHandler::Ping(PortHandler *port, UINT8_T id, UINT16_T *model_number, UINT8_T *error) -{ - int _result = COMM_TX_FAIL; - - UINT8_T txpacket[6] = {0}; - UINT8_T rxpacket[6] = {0}; - - if(id >= BROADCAST_ID) - return COMM_NOT_AVAILABLE; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH] = 2; - txpacket[PKT_INSTRUCTION] = INST_PING; - - _result = TxRxPacket(port, txpacket, rxpacket, error); - if(_result == COMM_SUCCESS && model_number != 0) - { - UINT8_T _data[2] = {0}; - _result = ReadTxRx(port, id, 0, 2, _data); // Address 0 : Model Number - if(_result == COMM_SUCCESS) - *model_number = DXL_MAKEWORD(_data[0], _data[1]); - } - - return _result; -} - -int Protocol1PacketHandler::BroadcastPing(PortHandler *port, std::vector &id_list) -{ - return COMM_NOT_AVAILABLE; -} - -int Protocol1PacketHandler::Action(PortHandler *port, UINT8_T id) -{ - UINT8_T txpacket[6] = {0}; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH] = 2; - txpacket[PKT_INSTRUCTION] = INST_ACTION; - - return TxRxPacket(port, txpacket, 0); -} - -int Protocol1PacketHandler::Reboot(PortHandler *port, UINT8_T id, UINT8_T *error) -{ - return COMM_NOT_AVAILABLE; -} - -int Protocol1PacketHandler::FactoryReset(PortHandler *port, UINT8_T id, UINT8_T option, UINT8_T *error) -{ - UINT8_T txpacket[6] = {0}; - UINT8_T rxpacket[6] = {0}; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH] = 2; - txpacket[PKT_INSTRUCTION] = INST_FACTORY_RESET; - - return TxRxPacket(port, txpacket, rxpacket, error); -} - -int Protocol1PacketHandler::ReadTx(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length) -{ - int _result = COMM_TX_FAIL; - - UINT8_T txpacket[8] = {0}; - - if(id >= BROADCAST_ID) - return COMM_NOT_AVAILABLE; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH] = 4; - txpacket[PKT_INSTRUCTION] = INST_READ; - txpacket[PKT_PARAMETER0+0] = (UINT8_T)address; - txpacket[PKT_PARAMETER0+1] = (UINT8_T)length; - - _result = TxPacket(port, txpacket); - - // set packet timeout - if(_result == COMM_SUCCESS) - port->SetPacketTimeout((UINT16_T)(length+6)); - - return _result; -} - -int Protocol1PacketHandler::ReadRx(PortHandler *port, UINT16_T length, UINT8_T *data, UINT8_T *error) -{ - int _result = COMM_TX_FAIL; - UINT8_T *rxpacket = (UINT8_T *)malloc(RXPACKET_MAX_LEN);//(length+6); - //UINT8_T *rxpacket = new UINT8_T[length+6]; - - _result = RxPacket(port, rxpacket); - if(_result == COMM_SUCCESS) - { - if(error != 0) - *error = (UINT8_T)rxpacket[PKT_ERROR]; - for(UINT8_T _s = 0; _s < length; _s++) - data[_s] = rxpacket[PKT_PARAMETER0 + _s]; - //memcpy(data, &rxpacket[PKT_PARAMETER0], length); - } - - free(rxpacket); - //delete[] rxpacket; - return _result; -} - -int Protocol1PacketHandler::ReadTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error) -{ - int _result = COMM_TX_FAIL; - - UINT8_T txpacket[8] = {0}; - UINT8_T *rxpacket = (UINT8_T *)malloc(RXPACKET_MAX_LEN);//(length+6); - - if(id >= BROADCAST_ID) - return COMM_NOT_AVAILABLE; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH] = 4; - txpacket[PKT_INSTRUCTION] = INST_READ; - txpacket[PKT_PARAMETER0+0] = (UINT8_T)address; - txpacket[PKT_PARAMETER0+1] = (UINT8_T)length; - - _result = TxRxPacket(port, txpacket, rxpacket, error); - if(_result == COMM_SUCCESS) - { - if(error != 0) - *error = (UINT8_T)rxpacket[PKT_ERROR]; - for(UINT8_T _s = 0; _s < length; _s++) - data[_s] = rxpacket[PKT_PARAMETER0 + _s]; - //memcpy(data, &rxpacket[PKT_PARAMETER0], length); - } - - free(rxpacket); - //delete[] rxpacket; - return _result; -} - -int Protocol1PacketHandler::Read1ByteTx(PortHandler *port, UINT8_T id, UINT16_T address) -{ - return ReadTx(port, id, address, 1); -} -int Protocol1PacketHandler::Read1ByteRx(PortHandler *port, UINT8_T *data, UINT8_T *error) -{ - UINT8_T _data[1] = {0}; - int _result = ReadRx(port, 1, _data, error); - if(_result == COMM_SUCCESS) - *data = _data[0]; - return _result; -} -int Protocol1PacketHandler::Read1ByteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T *data, UINT8_T *error) -{ - UINT8_T _data[1] = {0}; - int _result = ReadTxRx(port, id, address, 1, _data, error); - if(_result == COMM_SUCCESS) - *data = _data[0]; - return _result; -} - -int Protocol1PacketHandler::Read2ByteTx(PortHandler *port, UINT8_T id, UINT16_T address) -{ - return ReadTx(port, id, address, 2); -} -int Protocol1PacketHandler::Read2ByteRx(PortHandler *port, UINT16_T *data, UINT8_T *error) -{ - UINT8_T _data[2] = {0}; - int _result = ReadRx(port, 2, _data, error); - if(_result == COMM_SUCCESS) - *data = DXL_MAKEWORD(_data[0], _data[1]); - return _result; -} -int Protocol1PacketHandler::Read2ByteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T *data, UINT8_T *error) -{ - UINT8_T _data[2] = {0}; - int _result = ReadTxRx(port, id, address, 2, _data, error); - if(_result == COMM_SUCCESS) - *data = DXL_MAKEWORD(_data[0], _data[1]); - return _result; -} - -int Protocol1PacketHandler::Read4ByteTx(PortHandler *port, UINT8_T id, UINT16_T address) -{ - return COMM_NOT_AVAILABLE; -} -int Protocol1PacketHandler::Read4ByteRx(PortHandler *port, UINT32_T *data, UINT8_T *error) -{ - return COMM_NOT_AVAILABLE; -} -int Protocol1PacketHandler::Read4ByteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T *data, UINT8_T *error) -{ - return COMM_NOT_AVAILABLE; -} - -int Protocol1PacketHandler::WriteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data) -{ - int _result = COMM_TX_FAIL; - - UINT8_T *txpacket = (UINT8_T *)malloc(length+7); - //UINT8_T *txpacket = new UINT8_T[length+7]; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH] = length+3; - txpacket[PKT_INSTRUCTION] = INST_WRITE; - txpacket[PKT_PARAMETER0] = (UINT8_T)address; - - for(UINT8_T _s = 0; _s < length; _s++) - txpacket[PKT_PARAMETER0+1+_s] = data[_s]; - //memcpy(&txpacket[PKT_PARAMETER0+1], data, length); - - _result = TxPacket(port, txpacket); - port->is_using = false; - - free(txpacket); - //delete[] txpacket; - return _result; -} - -int Protocol1PacketHandler::WriteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error) -{ - int _result = COMM_TX_FAIL; - - UINT8_T *txpacket = (UINT8_T *)malloc(length+6); - //UINT8_T *txpacket = new UINT8_T[length+6]; - UINT8_T rxpacket[6] = {0}; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH] = length+3; - txpacket[PKT_INSTRUCTION] = INST_WRITE; - txpacket[PKT_PARAMETER0] = (UINT8_T)address; - - for(UINT8_T _s = 0; _s < length; _s++) - txpacket[PKT_PARAMETER0+1+_s] = data[_s]; - //memcpy(&txpacket[PKT_PARAMETER0+1], data, length); - - _result = TxRxPacket(port, txpacket, rxpacket, error); - - free(txpacket); - //delete[] txpacket; - return _result; -} - -int Protocol1PacketHandler::Write1ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T data) -{ - UINT8_T _data[1] = { data }; - return WriteTxOnly(port, id, address, 1, _data); -} -int Protocol1PacketHandler::Write1ByteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T data, UINT8_T *error) -{ - UINT8_T _data[1] = { data }; - return WriteTxRx(port, id, address, 1, _data, error); -} - -int Protocol1PacketHandler::Write2ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T data) -{ - UINT8_T _data[2] = { DXL_LOBYTE(data), DXL_HIBYTE(data) }; - return WriteTxOnly(port, id, address, 2, _data); -} -int Protocol1PacketHandler::Write2ByteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T data, UINT8_T *error) -{ - UINT8_T _data[2] = { DXL_LOBYTE(data), DXL_HIBYTE(data) }; - return WriteTxRx(port, id, address, 2, _data, error); -} - -int Protocol1PacketHandler::Write4ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T data) -{ - return COMM_NOT_AVAILABLE; -} -int Protocol1PacketHandler::Write4ByteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T data, UINT8_T *error) -{ - return COMM_NOT_AVAILABLE; -} - -int Protocol1PacketHandler::RegWriteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data) -{ - int _result = COMM_TX_FAIL; - - UINT8_T *txpacket = (UINT8_T *)malloc(length+6); - //UINT8_T *txpacket = new UINT8_T[length+6]; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH] = length+3; - txpacket[PKT_INSTRUCTION] = INST_REG_WRITE; - txpacket[PKT_PARAMETER0] = (UINT8_T)address; - - for(UINT8_T _s = 0; _s < length; _s++) - txpacket[PKT_PARAMETER0+1+_s] = data[_s]; - //memcpy(&txpacket[PKT_PARAMETER0+1], data, length); - - _result = TxPacket(port, txpacket); - port->is_using = false; - - free(txpacket); - //delete[] txpacket; - return _result; -} - -int Protocol1PacketHandler::RegWriteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error) -{ - int _result = COMM_TX_FAIL; - - UINT8_T *txpacket = (UINT8_T *)malloc(length+6); - //UINT8_T *txpacket = new UINT8_T[length+6]; - UINT8_T rxpacket[6] = {0}; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH] = length+3; - txpacket[PKT_INSTRUCTION] = INST_REG_WRITE; - txpacket[PKT_PARAMETER0] = (UINT8_T)address; - - for(UINT8_T _s = 0; _s < length; _s++) - txpacket[PKT_PARAMETER0+1+_s] = data[_s]; - //memcpy(&txpacket[PKT_PARAMETER0+1], data, length); - - _result = TxRxPacket(port, txpacket, rxpacket, error); - - free(txpacket); - //delete[] txpacket; - return _result; -} - -int Protocol1PacketHandler::SyncReadTx(PortHandler *port, UINT16_T start_address, UINT16_T data_length, UINT8_T *param, UINT16_T param_length) -{ - return COMM_NOT_AVAILABLE; -} - -int Protocol1PacketHandler::SyncWriteTxOnly(PortHandler *port, UINT16_T start_address, UINT16_T data_length, UINT8_T *param, UINT16_T param_length) -{ - int _result = COMM_TX_FAIL; - - UINT8_T *txpacket = (UINT8_T *)malloc(param_length+8); // 8: HEADER0 HEADER1 ID LEN INST START_ADDR DATA_LEN ... CHKSUM - //UINT8_T *txpacket = new UINT8_T[param_length + 8]; // 8: HEADER0 HEADER1 ID LEN INST START_ADDR DATA_LEN ... CHKSUM - - txpacket[PKT_ID] = BROADCAST_ID; - txpacket[PKT_LENGTH] = param_length + 4; // 4: INST START_ADDR DATA_LEN ... CHKSUM - txpacket[PKT_INSTRUCTION] = INST_SYNC_WRITE; - txpacket[PKT_PARAMETER0+0] = start_address; - txpacket[PKT_PARAMETER0+1] = data_length; - - for(UINT8_T _s = 0; _s < param_length; _s++) - txpacket[PKT_PARAMETER0+2+_s] = param[_s]; - //memcpy(&txpacket[PKT_PARAMETER0+2], param, param_length); - - _result = TxRxPacket(port, txpacket, 0, 0); - - free(txpacket); - //delete[] txpacket; - return _result; -} - -int Protocol1PacketHandler::BulkReadTx(PortHandler *port, UINT8_T *param, UINT16_T param_length) -{ - int _result = COMM_TX_FAIL; - - UINT8_T *txpacket = (UINT8_T *)malloc(param_length+7); // 7: HEADER0 HEADER1 ID LEN INST 0x00 ... CHKSUM - //UINT8_T *txpacket = new UINT8_T[param_length + 7]; // 7: HEADER0 HEADER1 ID LEN INST 0x00 ... CHKSUM - - txpacket[PKT_ID] = BROADCAST_ID; - txpacket[PKT_LENGTH] = param_length + 3; // 3: INST 0x00 ... CHKSUM - txpacket[PKT_INSTRUCTION] = INST_BULK_READ; - txpacket[PKT_PARAMETER0+0] = 0x00; - - for(UINT8_T _s = 0; _s < param_length; _s++) - txpacket[PKT_PARAMETER0+1+_s] = param[_s]; - //memcpy(&txpacket[PKT_PARAMETER0+1], param, param_length); - - _result = TxPacket(port, txpacket); - if(_result == COMM_SUCCESS) - { - int _wait_length = 0; - for(int _i = 0; _i < param_length; _i += 3) - _wait_length += param[_i] + 7; - port->SetPacketTimeout((UINT16_T)_wait_length); - } - - free(txpacket); - //delete[] txpacket; - return _result; -} - -int Protocol1PacketHandler::BulkWriteTxOnly(PortHandler *port, UINT8_T *param, UINT16_T param_length) -{ - return COMM_NOT_AVAILABLE; -} diff --git a/dynamixel_sdk/src/dynamixel_sdk/Protocol2PacketHandler.cpp b/dynamixel_sdk/src/dynamixel_sdk/Protocol2PacketHandler.cpp deleted file mode 100644 index 771fee7..0000000 --- a/dynamixel_sdk/src/dynamixel_sdk/Protocol2PacketHandler.cpp +++ /dev/null @@ -1,985 +0,0 @@ -/* - * Protocol2PacketHandler.cpp - * - * Created on: 2016. 1. 26. - * Author: zerom, leon - */ - -#if defined(_WIN32) || defined(_WIN64) -#define WINDLLEXPORT -#endif - -#include -#include -#include -#include "dynamixel_sdk/Protocol2PacketHandler.h" - -#define TXPACKET_MAX_LEN (4*1024) -#define RXPACKET_MAX_LEN (4*1024) - -///////////////// for Protocol 2.0 Packet ///////////////// -#define PKT_HEADER0 0 -#define PKT_HEADER1 1 -#define PKT_HEADER2 2 -#define PKT_RESERVED 3 -#define PKT_ID 4 -#define PKT_LENGTH_L 5 -#define PKT_LENGTH_H 6 -#define PKT_INSTRUCTION 7 -#define PKT_ERROR 8 -#define PKT_PARAMETER0 8 - -///////////////// Protocol 2.0 Error bit ///////////////// -#define ERRNUM_RESULT_FAIL 1 // Failed to process the instruction packet. -#define ERRNUM_INSTRUCTION 2 // Instruction error -#define ERRNUM_CRC 3 // CRC check error -#define ERRNUM_DATA_RANGE 4 // Data range error -#define ERRNUM_DATA_LENGTH 5 // Data length error -#define ERRNUM_DATA_LIMIT 6 // Data limit error -#define ERRNUM_ACCESS 7 // Access error - -#define ERRBIT_ALERT 128 //When the device has a problem, this bit is set to 1. Check "Device Status Check" value. - -using namespace ROBOTIS; - -Protocol2PacketHandler *Protocol2PacketHandler::unique_instance_ = new Protocol2PacketHandler(); - -Protocol2PacketHandler::Protocol2PacketHandler() { } - -void Protocol2PacketHandler::PrintTxRxResult(int result) -{ - switch(result) - { - case COMM_SUCCESS: - printf("[TxRxResult] Communication success.\n"); - break; - - case COMM_PORT_BUSY: - printf("[TxRxResult] Port is in use!\n"); - break; - - case COMM_TX_FAIL: - printf("[TxRxResult] Failed transmit instruction packet!\n"); - break; - - case COMM_RX_FAIL: - printf("[TxRxResult] Failed get status packet from device!\n"); - break; - - case COMM_TX_ERROR: - printf("[TxRxResult] Incorrect instruction packet!\n"); - break; - - case COMM_RX_WAITING: - printf("[TxRxResult] Now recieving status packet!\n"); - break; - - case COMM_RX_TIMEOUT: - printf("[TxRxResult] There is no status packet!\n"); - break; - - case COMM_RX_CORRUPT: - printf("[TxRxResult] Incorrect status packet!\n"); - break; - - case COMM_NOT_AVAILABLE: - printf("[TxRxResult] Protocol does not support This function!\n"); - break; - - default: - break; - } -} - -void Protocol2PacketHandler::PrintRxPacketError(UINT8_T error) -{ - if(error & ERRBIT_ALERT) - printf("[RxPacketError] Hardware error occurred. Check the error at Control Table (Hardware Error Status)!\n"); - - int _error = error & ~ERRBIT_ALERT; - - switch(_error) - { - case 0: - break; - - case ERRNUM_RESULT_FAIL: - printf("[RxPacketError] Failed to process the instruction packet!\n"); - break; - - case ERRNUM_INSTRUCTION: - printf("[RxPacketError] Undefined instruction or incorrect instruction!\n"); - break; - - case ERRNUM_CRC: - printf("[RxPacketError] CRC doesn't match!\n"); - break; - - case ERRNUM_DATA_RANGE: - printf("[RxPacketError] The data value is out of range!\n"); - break; - - case ERRNUM_DATA_LENGTH: - printf("[RxPacketError] The data length does not match as expected!\n"); - break; - - case ERRNUM_DATA_LIMIT: - printf("[RxPacketError] The data value exceeds the limit value!\n"); - break; - - case ERRNUM_ACCESS: - printf("[RxPacketError] Writing or Reading is not available to target address!\n"); - break; - - default: - printf("[RxPacketError] Unknown error code!\n"); - break; - } -} - -unsigned short Protocol2PacketHandler::UpdateCRC(UINT16_T crc_accum, UINT8_T *data_blk_ptr, UINT16_T data_blk_size) -{ - UINT16_T i, j; - UINT16_T crc_table[256] = {0x0000, - 0x8005, 0x800F, 0x000A, 0x801B, 0x001E, 0x0014, 0x8011, - 0x8033, 0x0036, 0x003C, 0x8039, 0x0028, 0x802D, 0x8027, - 0x0022, 0x8063, 0x0066, 0x006C, 0x8069, 0x0078, 0x807D, - 0x8077, 0x0072, 0x0050, 0x8055, 0x805F, 0x005A, 0x804B, - 0x004E, 0x0044, 0x8041, 0x80C3, 0x00C6, 0x00CC, 0x80C9, - 0x00D8, 0x80DD, 0x80D7, 0x00D2, 0x00F0, 0x80F5, 0x80FF, - 0x00FA, 0x80EB, 0x00EE, 0x00E4, 0x80E1, 0x00A0, 0x80A5, - 0x80AF, 0x00AA, 0x80BB, 0x00BE, 0x00B4, 0x80B1, 0x8093, - 0x0096, 0x009C, 0x8099, 0x0088, 0x808D, 0x8087, 0x0082, - 0x8183, 0x0186, 0x018C, 0x8189, 0x0198, 0x819D, 0x8197, - 0x0192, 0x01B0, 0x81B5, 0x81BF, 0x01BA, 0x81AB, 0x01AE, - 0x01A4, 0x81A1, 0x01E0, 0x81E5, 0x81EF, 0x01EA, 0x81FB, - 0x01FE, 0x01F4, 0x81F1, 0x81D3, 0x01D6, 0x01DC, 0x81D9, - 0x01C8, 0x81CD, 0x81C7, 0x01C2, 0x0140, 0x8145, 0x814F, - 0x014A, 0x815B, 0x015E, 0x0154, 0x8151, 0x8173, 0x0176, - 0x017C, 0x8179, 0x0168, 0x816D, 0x8167, 0x0162, 0x8123, - 0x0126, 0x012C, 0x8129, 0x0138, 0x813D, 0x8137, 0x0132, - 0x0110, 0x8115, 0x811F, 0x011A, 0x810B, 0x010E, 0x0104, - 0x8101, 0x8303, 0x0306, 0x030C, 0x8309, 0x0318, 0x831D, - 0x8317, 0x0312, 0x0330, 0x8335, 0x833F, 0x033A, 0x832B, - 0x032E, 0x0324, 0x8321, 0x0360, 0x8365, 0x836F, 0x036A, - 0x837B, 0x037E, 0x0374, 0x8371, 0x8353, 0x0356, 0x035C, - 0x8359, 0x0348, 0x834D, 0x8347, 0x0342, 0x03C0, 0x83C5, - 0x83CF, 0x03CA, 0x83DB, 0x03DE, 0x03D4, 0x83D1, 0x83F3, - 0x03F6, 0x03FC, 0x83F9, 0x03E8, 0x83ED, 0x83E7, 0x03E2, - 0x83A3, 0x03A6, 0x03AC, 0x83A9, 0x03B8, 0x83BD, 0x83B7, - 0x03B2, 0x0390, 0x8395, 0x839F, 0x039A, 0x838B, 0x038E, - 0x0384, 0x8381, 0x0280, 0x8285, 0x828F, 0x028A, 0x829B, - 0x029E, 0x0294, 0x8291, 0x82B3, 0x02B6, 0x02BC, 0x82B9, - 0x02A8, 0x82AD, 0x82A7, 0x02A2, 0x82E3, 0x02E6, 0x02EC, - 0x82E9, 0x02F8, 0x82FD, 0x82F7, 0x02F2, 0x02D0, 0x82D5, - 0x82DF, 0x02DA, 0x82CB, 0x02CE, 0x02C4, 0x82C1, 0x8243, - 0x0246, 0x024C, 0x8249, 0x0258, 0x825D, 0x8257, 0x0252, - 0x0270, 0x8275, 0x827F, 0x027A, 0x826B, 0x026E, 0x0264, - 0x8261, 0x0220, 0x8225, 0x822F, 0x022A, 0x823B, 0x023E, - 0x0234, 0x8231, 0x8213, 0x0216, 0x021C, 0x8219, 0x0208, - 0x820D, 0x8207, 0x0202 }; - - for(j = 0; j < data_blk_size; j++) - { - i = ((UINT16_T)(crc_accum >> 8) ^ *data_blk_ptr++) & 0xFF; - crc_accum = (crc_accum << 8) ^ crc_table[i]; - } - - return crc_accum; -} - -void Protocol2PacketHandler::AddStuffing(UINT8_T *packet) -{ - int i = 0, index = 0; - int packet_length_in = DXL_MAKEWORD(packet[PKT_LENGTH_L], packet[PKT_LENGTH_H]); - int packet_length_out = packet_length_in; - UINT8_T temp[TXPACKET_MAX_LEN] = {0}; - - for(UINT8_T _s = PKT_HEADER0; _s <= PKT_LENGTH_H; _s++) - temp[_s] = packet[_s]; // FF FF FD XX ID LEN_L LEN_H - //memcpy(temp, packet, PKT_LENGTH_H+1); - index = PKT_INSTRUCTION; - for( i = 0; i < packet_length_in - 2; i++) // except CRC - { - temp[index++] = packet[i+PKT_INSTRUCTION]; - if(packet[i+PKT_INSTRUCTION] == 0xFD && packet[i+PKT_INSTRUCTION-1] == 0xFF && packet[i+PKT_INSTRUCTION-2] == 0xFF) - { // FF FF FD - temp[index++] = 0xFD; - packet_length_out++; - } - } - temp[index++] = packet[PKT_INSTRUCTION+packet_length_in-2]; - temp[index++] = packet[PKT_INSTRUCTION+packet_length_in-1]; - - - ////////////////////////// - if(packet_length_in != packet_length_out) - packet = (UINT8_T *)realloc(packet, index * sizeof(UINT8_T)); - - /////////////////////////// - - for(UINT8_T _s = 0; _s < index; _s++) - packet[_s] = temp[_s]; - //memcpy(packet, temp, index); - packet[PKT_LENGTH_L] = DXL_LOBYTE(packet_length_out); - packet[PKT_LENGTH_H] = DXL_HIBYTE(packet_length_out); -} - -void Protocol2PacketHandler::RemoveStuffing(UINT8_T *packet) -{ - int i = 0, index = 0; - int packet_length_in = DXL_MAKEWORD(packet[PKT_LENGTH_L], packet[PKT_LENGTH_H]); - int packet_length_out = packet_length_in; - - index = PKT_INSTRUCTION; - for( i = 0; i < packet_length_in - 2; i++) // except CRC - { - if(packet[i+PKT_INSTRUCTION] == 0xFD && packet[i+PKT_INSTRUCTION+1] == 0xFD && packet[i+PKT_INSTRUCTION-1] == 0xFF && packet[i+PKT_INSTRUCTION-2] == 0xFF) - { // FF FF FD FD - packet_length_out--; - i++; - } - packet[index++] = packet[i+PKT_INSTRUCTION]; - } - packet[index++] = packet[PKT_INSTRUCTION+packet_length_in-2]; - packet[index++] = packet[PKT_INSTRUCTION+packet_length_in-1]; - - packet[PKT_LENGTH_L] = DXL_LOBYTE(packet_length_out); - packet[PKT_LENGTH_H] = DXL_HIBYTE(packet_length_out); -} - -int Protocol2PacketHandler::TxPacket(PortHandler *port, UINT8_T *txpacket) -{ - UINT16_T _total_packet_length = 0; - UINT16_T _written_packet_length = 0; - - if(port->is_using) - return COMM_PORT_BUSY; - port->is_using = true; - - // byte stuffing for header - AddStuffing(txpacket); - - // check max packet length - _total_packet_length = DXL_MAKEWORD(txpacket[PKT_LENGTH_L], txpacket[PKT_LENGTH_H]) + 7; - // 7: HEADER0 HEADER1 HEADER2 RESERVED ID LENGTH_L LENGTH_H - if(_total_packet_length > TXPACKET_MAX_LEN) - { - port->is_using = false; - return COMM_TX_ERROR; - } - - // make packet header - txpacket[PKT_HEADER0] = 0xFF; - txpacket[PKT_HEADER1] = 0xFF; - txpacket[PKT_HEADER2] = 0xFD; - txpacket[PKT_RESERVED] = 0x00; - - // add CRC16 - UINT16_T crc = UpdateCRC(0, txpacket, _total_packet_length - 2); // 2: CRC16 - txpacket[_total_packet_length - 2] = DXL_LOBYTE(crc); - txpacket[_total_packet_length - 1] = DXL_HIBYTE(crc); - - // tx packet - port->ClearPort(); - _written_packet_length = port->WritePort(txpacket, _total_packet_length); - if(_total_packet_length != _written_packet_length) - { - port->is_using = false; - return COMM_TX_FAIL; - } - - return COMM_SUCCESS; -} - -int Protocol2PacketHandler::RxPacket(PortHandler *port, UINT8_T *rxpacket) -{ - int _result = COMM_TX_FAIL; - - UINT16_T _rx_length = 0; - UINT16_T _wait_length = 11; - // minimum length ( HEADER0 HEADER1 HEADER2 RESERVED ID LENGTH_L LENGTH_H INST ERROR CRC16_L CRC16_H ) - - while(true) - { - _rx_length += port->ReadPort(&rxpacket[_rx_length], _wait_length - _rx_length); - if(_rx_length >= _wait_length) - { - UINT16_T _idx = 0; - - // find packet header - for(_idx = 0; _idx < (_rx_length - 3); _idx++) - { - if((rxpacket[_idx] == 0xFF) && (rxpacket[_idx+1] == 0xFF) && (rxpacket[_idx+2] == 0xFD) && (rxpacket[_idx+3] != 0xFD)) - break; - } - - if(_idx == 0) // found at the beginning of the packet - { - if(rxpacket[PKT_RESERVED] != 0x00 || - rxpacket[PKT_ID] > 0xFC || - DXL_MAKEWORD(rxpacket[PKT_LENGTH_L], rxpacket[PKT_LENGTH_H]) > RXPACKET_MAX_LEN || - rxpacket[PKT_INSTRUCTION] != 0x55) - { - // remove the first byte in the packet - for(UINT8_T _s = 0; _s < _rx_length - 1; _s++) - rxpacket[_s] = rxpacket[1 + _s]; - //memcpy(&rxpacket[0], &rxpacket[_idx], _rx_length - _idx); - _rx_length -= 1; - continue; - } - - // re-calculate the exact length of the rx packet - if(_wait_length != DXL_MAKEWORD(rxpacket[PKT_LENGTH_L], rxpacket[PKT_LENGTH_H]) + PKT_LENGTH_H + 1) - { - _wait_length = DXL_MAKEWORD(rxpacket[PKT_LENGTH_L], rxpacket[PKT_LENGTH_H]) + PKT_LENGTH_H + 1; - continue; - } - - if(_rx_length < _wait_length) - { - // check timeout - if(port->IsPacketTimeout() == true) - { - if(_rx_length == 0) - _result = COMM_RX_TIMEOUT; - else - _result = COMM_RX_CORRUPT; - break; - } - else - continue; - } - - // verify CRC16 - UINT16_T crc = DXL_MAKEWORD(rxpacket[_wait_length-2], rxpacket[_wait_length-1]); - if(UpdateCRC(0, rxpacket, _wait_length - 2) == crc) - _result = COMM_SUCCESS; - else - _result = COMM_RX_CORRUPT; - break; - } - else - { - // remove unnecessary packets - for(UINT8_T _s = 0; _s < _rx_length - _idx; _s++) - rxpacket[_s] = rxpacket[_idx + _s]; - //memcpy(&rxpacket[0], &rxpacket[_idx], _rx_length - _idx); - _rx_length -= _idx; - } - } - else - { - // check timeout - if(port->IsPacketTimeout() == true) - { - if(_rx_length == 0) - _result = COMM_RX_TIMEOUT; - else - _result = COMM_RX_CORRUPT; - break; - } - } - } - port->is_using = false; - - if(_result == COMM_SUCCESS) - RemoveStuffing(rxpacket); - - return _result; -} - -// NOT for BulkRead / SyncRead instruction -int Protocol2PacketHandler::TxRxPacket(PortHandler *port, UINT8_T *txpacket, UINT8_T *rxpacket, UINT8_T *error) -{ - int _result = COMM_TX_FAIL; - - // tx packet - _result = TxPacket(port, txpacket); - if(_result != COMM_SUCCESS) - return _result; - - // (ID == Broadcast ID && NOT BulkRead) == no need to wait for status packet - // (Instruction == Action) == no need to wait for status packet - if((txpacket[PKT_ID] == BROADCAST_ID && txpacket[PKT_INSTRUCTION] != INST_BULK_READ) || - (txpacket[PKT_ID] == BROADCAST_ID && txpacket[PKT_INSTRUCTION] != INST_SYNC_READ) || - (txpacket[PKT_INSTRUCTION] == INST_ACTION)) - { - port->is_using = false; - return _result; - } - - // set packet timeout - if(txpacket[PKT_INSTRUCTION] == INST_READ) - port->SetPacketTimeout((UINT16_T)(DXL_MAKEWORD(txpacket[PKT_PARAMETER0+2], txpacket[PKT_PARAMETER0+3]) + 11)); - else - port->SetPacketTimeout((UINT16_T)11); // HEADER0 HEADER1 HEADER2 RESERVED ID LENGTH_L LENGTH_H INST ERROR CRC16_L CRC16_H - - // rx packet - _result = RxPacket(port, rxpacket); - // check txpacket ID == rxpacket ID - if(txpacket[PKT_ID] != rxpacket[PKT_ID]) - _result = RxPacket(port, rxpacket); - - if(_result == COMM_SUCCESS && txpacket[PKT_ID] != BROADCAST_ID) - { - if(error != 0) - *error = (UINT8_T)rxpacket[PKT_ERROR]; - } - - return _result; -} - -int Protocol2PacketHandler::Ping(PortHandler *port, UINT8_T id, UINT8_T *error) -{ - return Ping(port, id, 0, error); -} - -int Protocol2PacketHandler::Ping(PortHandler *port, UINT8_T id, UINT16_T *model_number, UINT8_T *error) -{ - int _result = COMM_TX_FAIL; - - UINT8_T txpacket[10] = {0}; - UINT8_T rxpacket[14] = {0}; - - if(id >= BROADCAST_ID) - return COMM_NOT_AVAILABLE; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH_L] = 3; - txpacket[PKT_LENGTH_H] = 0; - txpacket[PKT_INSTRUCTION] = INST_PING; - - _result = TxRxPacket(port, txpacket, rxpacket, error); - if(_result == COMM_SUCCESS && model_number != 0) - *model_number = DXL_MAKEWORD(rxpacket[PKT_PARAMETER0+1], rxpacket[PKT_PARAMETER0+2]); - - return _result; -} - -int Protocol2PacketHandler::BroadcastPing(PortHandler *port, std::vector &id_list) -{ - const int STATUS_LENGTH = 14; - int _result = COMM_TX_FAIL; - - id_list.clear(); - - UINT16_T _rx_length = 0; - UINT16_T _wait_length = STATUS_LENGTH * MAX_ID; - - UINT8_T txpacket[10] = {0}; - UINT8_T rxpacket[STATUS_LENGTH * MAX_ID] = {0}; - - txpacket[PKT_ID] = BROADCAST_ID; - txpacket[PKT_LENGTH_L] = 3; - txpacket[PKT_LENGTH_H] = 0; - txpacket[PKT_INSTRUCTION] = INST_PING; - - _result = TxPacket(port, txpacket); - if(_result != COMM_SUCCESS) - { - port->is_using = false; - return _result; - } - - // set rx timeout - port->SetPacketTimeout((UINT16_T)(_wait_length * 30)); - - while(1) - { - _rx_length += port->ReadPort(&rxpacket[_rx_length], _wait_length - _rx_length); - if(port->IsPacketTimeout() == true)// || _rx_length >= _wait_length) - break; - } - - port->is_using = false; - - if(_rx_length == 0) - return COMM_RX_TIMEOUT; - - while(1) - { - if(_rx_length < STATUS_LENGTH) - return COMM_RX_CORRUPT; - - UINT16_T _idx = 0; - - // find packet header - for(_idx = 0; _idx < (_rx_length - 2); _idx++) - { - if(rxpacket[_idx] == 0xFF && rxpacket[_idx+1] == 0xFF && rxpacket[_idx+2] == 0xFD) - break; - } - - if(_idx == 0) // found at the beginning of the packet - { - // verify CRC16 - UINT16_T crc = DXL_MAKEWORD(rxpacket[STATUS_LENGTH-2], rxpacket[STATUS_LENGTH-1]); - - if(UpdateCRC(0, rxpacket, STATUS_LENGTH - 2) == crc) - { - _result = COMM_SUCCESS; - - id_list.push_back(rxpacket[PKT_ID]); - - for(UINT8_T _s = 0; _s < _rx_length - STATUS_LENGTH; _s++) - rxpacket[_s] = rxpacket[STATUS_LENGTH + _s]; - _rx_length -= STATUS_LENGTH; - - if(_rx_length == 0) - return _result; - } - else - { - _result = COMM_RX_CORRUPT; - - // remove header (0xFF 0xFF 0xFD) - for(UINT8_T _s = 0; _s < _rx_length - 3; _s++) - rxpacket[_s] = rxpacket[3 + _s]; - _rx_length -= 3; - } - } - else - { - // remove unnecessary packets - for(UINT8_T _s = 0; _s < _rx_length - _idx; _s++) - rxpacket[_s] = rxpacket[_idx + _s]; - _rx_length -= _idx; - } - } - - return _result; -} - -int Protocol2PacketHandler::Action(PortHandler *port, UINT8_T id) -{ - UINT8_T txpacket[10] = {0}; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH_L] = 3; - txpacket[PKT_LENGTH_H] = 0; - txpacket[PKT_INSTRUCTION] = INST_ACTION; - - return TxRxPacket(port, txpacket, 0); -} - -int Protocol2PacketHandler::Reboot(PortHandler *port, UINT8_T id, UINT8_T *error) -{ - UINT8_T txpacket[10] = {0}; - UINT8_T rxpacket[11] = {0}; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH_L] = 3; - txpacket[PKT_LENGTH_H] = 0; - txpacket[PKT_INSTRUCTION] = INST_REBOOT; - - return TxRxPacket(port, txpacket, rxpacket, error); -} - -int Protocol2PacketHandler::FactoryReset(PortHandler *port, UINT8_T id, UINT8_T option, UINT8_T *error) -{ - UINT8_T txpacket[11] = {0}; - UINT8_T rxpacket[11] = {0}; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH_L] = 4; - txpacket[PKT_LENGTH_H] = 0; - txpacket[PKT_INSTRUCTION] = INST_FACTORY_RESET; - txpacket[PKT_PARAMETER0] = option; - - return TxRxPacket(port, txpacket, rxpacket, error); -} - -int Protocol2PacketHandler::ReadTx(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length) -{ - int _result = COMM_TX_FAIL; - - UINT8_T txpacket[14] = {0}; - - if(id >= BROADCAST_ID) - return COMM_NOT_AVAILABLE; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH_L] = 7; - txpacket[PKT_LENGTH_H] = 0; - txpacket[PKT_INSTRUCTION] = INST_READ; - txpacket[PKT_PARAMETER0+0] = (UINT8_T)DXL_LOBYTE(address); - txpacket[PKT_PARAMETER0+1] = (UINT8_T)DXL_HIBYTE(address); - txpacket[PKT_PARAMETER0+2] = (UINT8_T)DXL_LOBYTE(length); - txpacket[PKT_PARAMETER0+3] = (UINT8_T)DXL_HIBYTE(length); - - _result = TxPacket(port, txpacket); - - // set packet timeout - if(_result == COMM_SUCCESS) - port->SetPacketTimeout((UINT16_T)(length + 11)); - - return _result; -} - -int Protocol2PacketHandler::ReadRx(PortHandler *port, UINT16_T length, UINT8_T *data, UINT8_T *error) -{ - int _result = COMM_TX_FAIL; - UINT8_T *rxpacket = (UINT8_T *)malloc(RXPACKET_MAX_LEN);//(length + 11 + (length/3)); // (length/3): consider stuffing - //UINT8_T *rxpacket = new UINT8_T[length + 11 + (length/3)]; // (length/3): consider stuffing - - _result = RxPacket(port, rxpacket); - if(_result == COMM_SUCCESS) - { - if(error != 0) - *error = (UINT8_T)rxpacket[PKT_ERROR]; - for(UINT8_T _s = 0; _s < length; _s++) - data[_s] = rxpacket[PKT_PARAMETER0 + 1 + _s]; - //memcpy(data, &rxpacket[PKT_PARAMETER0+1], length); - } - - free(rxpacket); - //delete[] rxpacket; - return _result; -} - -int Protocol2PacketHandler::ReadTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error) -{ - int _result = COMM_TX_FAIL; - - UINT8_T txpacket[14] = {0}; - UINT8_T *rxpacket = (UINT8_T *)malloc(RXPACKET_MAX_LEN);//(length + 11 + (length/3)); // (length/3): consider stuffing - - if(id >= BROADCAST_ID) - return COMM_NOT_AVAILABLE; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH_L] = 7; - txpacket[PKT_LENGTH_H] = 0; - txpacket[PKT_INSTRUCTION] = INST_READ; - txpacket[PKT_PARAMETER0+0] = (UINT8_T)DXL_LOBYTE(address); - txpacket[PKT_PARAMETER0+1] = (UINT8_T)DXL_HIBYTE(address); - txpacket[PKT_PARAMETER0+2] = (UINT8_T)DXL_LOBYTE(length); - txpacket[PKT_PARAMETER0+3] = (UINT8_T)DXL_HIBYTE(length); - - _result = TxRxPacket(port, txpacket, rxpacket, error); - if(_result == COMM_SUCCESS) - { - if(error != 0) - *error = (UINT8_T)rxpacket[PKT_ERROR]; - for(UINT8_T _s = 0; _s < length; _s++) - data[_s] = rxpacket[PKT_PARAMETER0 + 1 + _s]; - //memcpy(data, &rxpacket[PKT_PARAMETER0+1], length); - } - - free(rxpacket); - //delete[] rxpacket; - return _result; -} - -int Protocol2PacketHandler::Read1ByteTx(PortHandler *port, UINT8_T id, UINT16_T address) -{ - return ReadTx(port, id, address, 1); -} -int Protocol2PacketHandler::Read1ByteRx(PortHandler *port, UINT8_T *data, UINT8_T *error) -{ - UINT8_T _data[1] = {0}; - int _result = ReadRx(port, 1, _data, error); - if(_result == COMM_SUCCESS) - *data = _data[0]; - return _result; -} -int Protocol2PacketHandler::Read1ByteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T *data, UINT8_T *error) -{ - UINT8_T _data[1] = {0}; - int _result = ReadTxRx(port, id, address, 1, _data, error); - if(_result == COMM_SUCCESS) - *data = _data[0]; - return _result; -} - -int Protocol2PacketHandler::Read2ByteTx(PortHandler *port, UINT8_T id, UINT16_T address) -{ - return ReadTx(port, id, address, 2); -} -int Protocol2PacketHandler::Read2ByteRx(PortHandler *port, UINT16_T *data, UINT8_T *error) -{ - UINT8_T _data[2] = {0}; - int _result = ReadRx(port, 2, _data, error); - if(_result == COMM_SUCCESS) - *data = DXL_MAKEWORD(_data[0], _data[1]); - return _result; -} -int Protocol2PacketHandler::Read2ByteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T *data, UINT8_T *error) -{ - UINT8_T _data[2] = {0}; - int _result = ReadTxRx(port, id, address, 2, _data, error); - if(_result == COMM_SUCCESS) - *data = DXL_MAKEWORD(_data[0], _data[1]); - return _result; -} - -int Protocol2PacketHandler::Read4ByteTx(PortHandler *port, UINT8_T id, UINT16_T address) -{ - return ReadTx(port, id, address, 4); -} -int Protocol2PacketHandler::Read4ByteRx(PortHandler *port, UINT32_T *data, UINT8_T *error) -{ - UINT8_T _data[4] = {0}; - int _result = ReadRx(port, 4, _data, error); - if(_result == COMM_SUCCESS) - *data = DXL_MAKEDWORD(DXL_MAKEWORD(_data[0], _data[1]), DXL_MAKEWORD(_data[2], _data[3])); - return _result; -} -int Protocol2PacketHandler::Read4ByteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T *data, UINT8_T *error) -{ - UINT8_T _data[4] = {0}; - int _result = ReadTxRx(port, id, address, 4, _data, error); - if(_result == COMM_SUCCESS) - *data = DXL_MAKEDWORD(DXL_MAKEWORD(_data[0], _data[1]), DXL_MAKEWORD(_data[2], _data[3])); - return _result; -} - - -int Protocol2PacketHandler::WriteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data) -{ - int _result = COMM_TX_FAIL; - - UINT8_T *txpacket = (UINT8_T *)malloc(length+12); - //UINT8_T *txpacket = new UINT8_T[length+12]; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length+5); - txpacket[PKT_LENGTH_H] = DXL_HIBYTE(length+5); - txpacket[PKT_INSTRUCTION] = INST_WRITE; - txpacket[PKT_PARAMETER0+0] = (UINT8_T)DXL_LOBYTE(address); - txpacket[PKT_PARAMETER0+1] = (UINT8_T)DXL_HIBYTE(address); - - for(UINT8_T _s = 0; _s < length; _s++) - txpacket[PKT_PARAMETER0+2+_s] = data[_s]; - //memcpy(&txpacket[PKT_PARAMETER0+2], data, length); - - _result = TxPacket(port, txpacket); - port->is_using = false; - - free(txpacket); - //delete[] txpacket; - return _result; -} - -int Protocol2PacketHandler::WriteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error) -{ - int _result = COMM_TX_FAIL; - - UINT8_T *txpacket = (UINT8_T *)malloc(length + 12); - //UINT8_T *txpacket = new UINT8_T[length+12]; - UINT8_T rxpacket[11] = {0}; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length+5); - txpacket[PKT_LENGTH_H] = DXL_HIBYTE(length+5); - txpacket[PKT_INSTRUCTION] = INST_WRITE; - txpacket[PKT_PARAMETER0+0] = (UINT8_T)DXL_LOBYTE(address); - txpacket[PKT_PARAMETER0+1] = (UINT8_T)DXL_HIBYTE(address); - - for(UINT8_T _s = 0; _s < length; _s++) - txpacket[PKT_PARAMETER0+2+_s] = data[_s]; - //memcpy(&txpacket[PKT_PARAMETER0+2], data, length); - - _result = TxRxPacket(port, txpacket, rxpacket, error); - - free(txpacket); - //delete[] txpacket; - return _result; -} - -int Protocol2PacketHandler::Write1ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T data) -{ - UINT8_T _data[1] = { data }; - return WriteTxOnly(port, id, address, 1, _data); -} -int Protocol2PacketHandler::Write1ByteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T data, UINT8_T *error) -{ - UINT8_T _data[1] = { data }; - return WriteTxRx(port, id, address, 1, _data, error); -} - -int Protocol2PacketHandler::Write2ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T data) -{ - UINT8_T _data[2] = { DXL_LOBYTE(data), DXL_HIBYTE(data) }; - return WriteTxOnly(port, id, address, 2, _data); -} -int Protocol2PacketHandler::Write2ByteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T data, UINT8_T *error) -{ - UINT8_T _data[2] = { DXL_LOBYTE(data), DXL_HIBYTE(data) }; - return WriteTxRx(port, id, address, 2, _data, error); -} - -int Protocol2PacketHandler::Write4ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T data) -{ - UINT8_T _data[4] = { DXL_LOBYTE(DXL_LOWORD(data)), DXL_HIBYTE(DXL_LOWORD(data)), DXL_LOBYTE(DXL_HIWORD(data)), DXL_HIBYTE(DXL_HIWORD(data)) }; - return WriteTxOnly(port, id, address, 4, _data); -} -int Protocol2PacketHandler::Write4ByteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T data, UINT8_T *error) -{ - UINT8_T _data[4] = { DXL_LOBYTE(DXL_LOWORD(data)), DXL_HIBYTE(DXL_LOWORD(data)), DXL_LOBYTE(DXL_HIWORD(data)), DXL_HIBYTE(DXL_HIWORD(data)) }; - return WriteTxRx(port, id, address, 4, _data, error); -} - -int Protocol2PacketHandler::RegWriteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data) -{ - int _result = COMM_TX_FAIL; - - UINT8_T *txpacket = (UINT8_T *)malloc(length + 12); - //UINT8_T *txpacket = new UINT8_T[length+12]; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length+5); - txpacket[PKT_LENGTH_H] = DXL_HIBYTE(length+5); - txpacket[PKT_INSTRUCTION] = INST_REG_WRITE; - txpacket[PKT_PARAMETER0+0] = (UINT8_T)DXL_LOBYTE(address); - txpacket[PKT_PARAMETER0+1] = (UINT8_T)DXL_HIBYTE(address); - - for(UINT8_T _s = 0; _s < length; _s++) - txpacket[PKT_PARAMETER0+2+_s] = data[_s]; - //memcpy(&txpacket[PKT_PARAMETER0+2], data, length); - - _result = TxPacket(port, txpacket); - port->is_using = false; - - free(txpacket); - //delete[] txpacket; - return _result; -} - -int Protocol2PacketHandler::RegWriteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error) -{ - int _result = COMM_TX_FAIL; - - UINT8_T *txpacket = (UINT8_T *)malloc(length + 12); - //UINT8_T *txpacket = new UINT8_T[length+12]; - UINT8_T rxpacket[11] = {0}; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length+5); - txpacket[PKT_LENGTH_H] = DXL_HIBYTE(length+5); - txpacket[PKT_INSTRUCTION] = INST_REG_WRITE; - txpacket[PKT_PARAMETER0+0] = (UINT8_T)DXL_LOBYTE(address); - txpacket[PKT_PARAMETER0+1] = (UINT8_T)DXL_HIBYTE(address); - - for(UINT8_T _s = 0; _s < length; _s++) - txpacket[PKT_PARAMETER0+2+_s] = data[_s]; - //memcpy(&txpacket[PKT_PARAMETER0+2], data, length); - - _result = TxRxPacket(port, txpacket, rxpacket, error); - - free(txpacket); - //delete[] txpacket; - return _result; -} - -int Protocol2PacketHandler::SyncReadTx(PortHandler *port, UINT16_T start_address, UINT16_T data_length, UINT8_T *param, UINT16_T param_length) -{ - int _result = COMM_TX_FAIL; - - UINT8_T *txpacket = (UINT8_T *)malloc(param_length + 14); - // 14: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H - - txpacket[PKT_ID] = BROADCAST_ID; - txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H - txpacket[PKT_LENGTH_H] = DXL_HIBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H - txpacket[PKT_INSTRUCTION] = INST_SYNC_READ; - txpacket[PKT_PARAMETER0+0] = DXL_LOBYTE(start_address); - txpacket[PKT_PARAMETER0+1] = DXL_HIBYTE(start_address); - txpacket[PKT_PARAMETER0+2] = DXL_LOBYTE(data_length); - txpacket[PKT_PARAMETER0+3] = DXL_HIBYTE(data_length); - - for(UINT8_T _s = 0; _s < param_length; _s++) - txpacket[PKT_PARAMETER0+4+_s] = param[_s]; - //memcpy(&txpacket[PKT_PARAMETER0+4], param, param_length); - - _result = TxPacket(port, txpacket); - if(_result == COMM_SUCCESS) - port->SetPacketTimeout((UINT16_T)((11 + data_length) * param_length)); - - free(txpacket); - return _result; -} - -int Protocol2PacketHandler::SyncWriteTxOnly(PortHandler *port, UINT16_T start_address, UINT16_T data_length, UINT8_T *param, UINT16_T param_length) -{ - int _result = COMM_TX_FAIL; - - UINT8_T *txpacket = (UINT8_T *)malloc(param_length + 14); - //UINT8_T *txpacket = new UINT8_T[param_length + 14]; - // 14: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H - - txpacket[PKT_ID] = BROADCAST_ID; - txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H - txpacket[PKT_LENGTH_H] = DXL_HIBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H - txpacket[PKT_INSTRUCTION] = INST_SYNC_WRITE; - txpacket[PKT_PARAMETER0+0] = DXL_LOBYTE(start_address); - txpacket[PKT_PARAMETER0+1] = DXL_HIBYTE(start_address); - txpacket[PKT_PARAMETER0+2] = DXL_LOBYTE(data_length); - txpacket[PKT_PARAMETER0+3] = DXL_HIBYTE(data_length); - - for(UINT8_T _s = 0; _s < param_length; _s++) - txpacket[PKT_PARAMETER0+4+_s] = param[_s]; - //memcpy(&txpacket[PKT_PARAMETER0+4], param, param_length); - - _result = TxRxPacket(port, txpacket, 0, 0); - - free(txpacket); - //delete[] txpacket; - return _result; -} - -int Protocol2PacketHandler::BulkReadTx(PortHandler *port, UINT8_T *param, UINT16_T param_length) -{ - int _result = COMM_TX_FAIL; - - UINT8_T *txpacket = (UINT8_T *)malloc(param_length + 10); - //UINT8_T *txpacket = new UINT8_T[param_length + 10]; - // 10: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST CRC16_L CRC16_H - - txpacket[PKT_ID] = BROADCAST_ID; - txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H - txpacket[PKT_LENGTH_H] = DXL_HIBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H - txpacket[PKT_INSTRUCTION] = INST_BULK_READ; - - for(UINT8_T _s = 0; _s < param_length; _s++) - txpacket[PKT_PARAMETER0+_s] = param[_s]; - //memcpy(&txpacket[PKT_PARAMETER0], param, param_length); - - _result = TxPacket(port, txpacket); - if(_result == COMM_SUCCESS) - { - int _wait_length = 0; - for(int _i = 0; _i < param_length; _i += 5) - _wait_length += DXL_MAKEWORD(param[_i+3], param[_i+4]) + 10; - port->SetPacketTimeout((UINT16_T)_wait_length); - } - - free(txpacket); - //delete[] txpacket; - return _result; -} - -int Protocol2PacketHandler::BulkWriteTxOnly(PortHandler *port, UINT8_T *param, UINT16_T param_length) -{ - int _result = COMM_TX_FAIL; - - UINT8_T *txpacket = (UINT8_T *)malloc(param_length + 10); - //UINT8_T *txpacket = new UINT8_T[param_length + 10]; - // 10: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST CRC16_L CRC16_H - - txpacket[PKT_ID] = BROADCAST_ID; - txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H - txpacket[PKT_LENGTH_H] = DXL_HIBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H - txpacket[PKT_INSTRUCTION] = INST_BULK_WRITE; - - for(UINT8_T _s = 0; _s < param_length; _s++) - txpacket[PKT_PARAMETER0+_s] = param[_s]; - //memcpy(&txpacket[PKT_PARAMETER0], param, param_length); - - _result = TxRxPacket(port, txpacket, 0, 0); - - free(txpacket); - //delete[] txpacket; - return _result; -} diff --git a/dynamixel_sdk/src/dynamixel_sdk_linux/PortHandlerLinux.cpp b/dynamixel_sdk/src/dynamixel_sdk_linux/PortHandlerLinux.cpp deleted file mode 100644 index 96c3f75..0000000 --- a/dynamixel_sdk/src/dynamixel_sdk_linux/PortHandlerLinux.cpp +++ /dev/null @@ -1,245 +0,0 @@ -/* - * PortHandlerLinux.cpp - * - * Created on: 2016. 1. 26. - * Author: zerom, leon - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "dynamixel_sdk_linux/PortHandlerLinux.h" - -#define LATENCY_TIMER 4 // msec (USB latency timer) - -using namespace ROBOTIS; - -PortHandlerLinux::PortHandlerLinux(const char *port_name) - : socket_fd_(-1), - baudrate_(DEFAULT_BAUDRATE), - packet_start_time_(0.0), - packet_timeout_(0.0), - tx_time_per_byte(0.0) -{ - is_using = false; - SetPortName(port_name); -} - -bool PortHandlerLinux::OpenPort() -{ - return SetBaudRate(baudrate_); -} - -void PortHandlerLinux::ClosePort() -{ - if(socket_fd_ != -1) - close(socket_fd_); - socket_fd_ = -1; -} - -void PortHandlerLinux::ClearPort() -{ - tcflush(socket_fd_, TCIOFLUSH); -} - -void PortHandlerLinux::SetPortName(const char *port_name) -{ - strcpy(port_name_, port_name); -} - -char *PortHandlerLinux::GetPortName() -{ - return port_name_; -} - -// TODO: baud number ?? -bool PortHandlerLinux::SetBaudRate(const int baudrate) -{ - int _baud = GetCFlagBaud(baudrate); - - ClosePort(); - - if(_baud <= 0) // custom baudrate - { - SetupPort(B38400); - baudrate_ = baudrate; - return SetCustomBaudrate(baudrate); - } - else - { - baudrate_ = baudrate; - return SetupPort(_baud); - } -} - -int PortHandlerLinux::GetBaudRate() -{ - return baudrate_; -} - -int PortHandlerLinux::GetBytesAvailable() -{ - int _bytes_available; - ioctl(socket_fd_, FIONREAD, &_bytes_available); - return _bytes_available; -} - -int PortHandlerLinux::ReadPort(UINT8_T *packet, int length) -{ - return read(socket_fd_, packet, length); -} - -int PortHandlerLinux::WritePort(UINT8_T *packet, int length) -{ - return write(socket_fd_, packet, length); -} - -void PortHandlerLinux::SetPacketTimeout(UINT16_T packet_length) -{ - packet_start_time_ = GetCurrentTime(); - packet_timeout_ = (tx_time_per_byte * (double)packet_length) + (LATENCY_TIMER * 2.0) + 2.0; -} - -void PortHandlerLinux::SetPacketTimeout(double msec) -{ - packet_start_time_ = GetCurrentTime(); - packet_timeout_ = msec; -} - -bool PortHandlerLinux::IsPacketTimeout() -{ - if(GetTimeSinceStart() > packet_timeout_) - { - packet_timeout_ = 0; - return true; - } - return false; -} - -double PortHandlerLinux::GetCurrentTime() -{ - struct timespec _tv; - clock_gettime( CLOCK_REALTIME, &_tv); - return ((double)_tv.tv_sec*1000.0 + (double)_tv.tv_nsec*0.001*0.001); -} - -double PortHandlerLinux::GetTimeSinceStart() -{ - double _time; - - _time = GetCurrentTime() - packet_start_time_; - if(_time < 0.0) - packet_start_time_ = GetCurrentTime(); - - return _time; -} - -bool PortHandlerLinux::SetupPort(int cflag_baud) -{ - struct termios newtio; - - socket_fd_ = open(port_name_, O_RDWR|O_NOCTTY|O_NONBLOCK); - if(socket_fd_ < 0) - { - printf("[PortHandlerLinux::SetupPort] Error opening serial port!\n"); - return false; - } - - bzero(&newtio, sizeof(newtio)); // clear struct for new port settings - - newtio.c_cflag = cflag_baud | CS8 | CLOCAL | CREAD; - newtio.c_iflag = IGNPAR; - newtio.c_oflag = 0; - newtio.c_lflag = 0; - newtio.c_cc[VTIME] = 0; - newtio.c_cc[VMIN] = 0; - - // clean the buffer and activate the settings for the port - tcflush(socket_fd_, TCIFLUSH); - tcsetattr(socket_fd_, TCSANOW, &newtio); - - tx_time_per_byte = (1000.0 / (double)baudrate_) * 10.0; - return true; -} - -bool PortHandlerLinux::SetCustomBaudrate(int speed) -{ - // try to set a custom divisor - struct serial_struct ss; - if(ioctl(socket_fd_, TIOCGSERIAL, &ss) != 0) - { - printf("[PortHandlerLinux::SetCustomBaudrate] TIOCGSERIAL failed!\n"); - return false; - } - - ss.flags = (ss.flags & ~ASYNC_SPD_MASK) | ASYNC_SPD_CUST; - ss.custom_divisor = (ss.baud_base + (speed / 2)) / speed; - int closest_speed = ss.baud_base / ss.custom_divisor; - - if(closest_speed < speed * 98 / 100 || closest_speed > speed * 102 / 100) - { - printf("[PortHandlerLinux::SetCustomBaudrate] Cannot set speed to %d, closest is %d \n", speed, closest_speed); - return false; - } - - if(ioctl(socket_fd_, TIOCSSERIAL, &ss) < 0) - { - printf("[PortHandlerLinux::SetCustomBaudrate] TIOCSSERIAL failed!\n"); - return false; - } - - tx_time_per_byte = (1000.0 / (double)speed) * 10.0; - return true; -} - -int PortHandlerLinux::GetCFlagBaud(int baudrate) -{ - switch(baudrate) - { - case 9600: - return B9600; - case 19200: - return B19200; - case 38400: - return B38400; - case 57600: - return B57600; - case 115200: - return B115200; - case 230400: - return B230400; - case 460800: - return B460800; - case 500000: - return B500000; - case 576000: - return B576000; - case 921600: - return B921600; - case 1000000: - return B1000000; - case 1152000: - return B1152000; - case 1500000: - return B1500000; - case 2000000: - return B2000000; - case 2500000: - return B2500000; - case 3000000: - return B3000000; - case 3500000: - return B3500000; - case 4000000: - return B4000000; - default: - return -1; - } -} diff --git a/robotis_controller/CMakeLists.txt b/robotis_controller/CMakeLists.txt index 5865116..97c4a6d 100644 --- a/robotis_controller/CMakeLists.txt +++ b/robotis_controller/CMakeLists.txt @@ -1,6 +1,8 @@ cmake_minimum_required(VERSION 2.8.3) project(robotis_controller) +set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}") + find_package(catkin REQUIRED COMPONENTS roscpp roslib @@ -13,15 +15,6 @@ find_package(catkin REQUIRED COMPONENTS dynamixel_sdk ) -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if you package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need catkin_package( INCLUDE_DIRS include LIBRARIES robotis_controller @@ -29,24 +22,15 @@ catkin_package( # DEPENDS system_lib ) -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -# include_directories(include) include_directories( include ${catkin_INCLUDE_DIRS} ) -## Declare a cpp library add_library(robotis_controller - src/robotis_controller/RobotisController.cpp + src/robotis_controller/robotis_controller.cpp ) -## Specify libraries to link a library or executable target against target_link_libraries(robotis_controller yaml-cpp ${catkin_LIBRARIES} diff --git a/robotis_controller/include/robotis_controller/RobotisController.h b/robotis_controller/include/robotis_controller/RobotisController.h deleted file mode 100644 index 46eb104..0000000 --- a/robotis_controller/include/robotis_controller/RobotisController.h +++ /dev/null @@ -1,145 +0,0 @@ -/* - * RobotisController.h - * - * Created on: 2016. 1. 15. - * Author: zerom - */ - -#ifndef ROBOTIS_FRAMEWORK_ROBOTIS_CONTROLLER_INCLUDE_ROBOTIS_CONTROLLER_ROBOTISCONTROLLER_H_ -#define ROBOTIS_FRAMEWORK_ROBOTIS_CONTROLLER_INCLUDE_ROBOTIS_CONTROLLER_ROBOTISCONTROLLER_H_ - - -#include -#include -#include -#include -#include -#include - -#include "robotis_device/Robot.h" -#include "robotis_framework_common/MotionModule.h" -#include "robotis_framework_common/SensorModule.h" - -#include "robotis_controller_msgs/SyncWriteItem.h" -#include "robotis_controller_msgs/JointCtrlModule.h" -#include "robotis_controller_msgs/GetJointModule.h" - -// TODO: TEMPORARY CODE !! -#include "dynamixel_sdk/GroupBulkRead.h" -#include "dynamixel_sdk/GroupSyncWrite.h" - -namespace ROBOTIS -{ - -enum CONTROLLER_MODE -{ - MOTION_MODULE_MODE, - DIRECT_CONTROL_MODE -}; - -class RobotisController : public Singleton -{ -private: - boost::thread queue_thread_; - boost::thread gazebo_thread_; - boost::thread set_module_thread_; - boost::mutex queue_mutex_; - - bool init_pose_loaded_; - bool is_timer_running_; - bool stop_timer_; - pthread_t timer_thread_; - CONTROLLER_MODE controller_mode_; - - std::list motion_modules_; - std::list sensor_modules_; - std::vector direct_sync_write_; - - std::map sensor_result_; - - void QueueThread(); - void GazeboThread(); - void SetCtrlModuleThread(std::string ctrl_module); - - bool CheckTimerStop(); - void InitSyncWrite(); - -public: - static const int CONTROL_CYCLE_MSEC = 8; // 8 msec - - bool DEBUG_PRINT; - Robot *robot; - - bool gazebo_mode; - std::string gazebo_robot_name; - - // TODO: TEMPORARY CODE !! - std::map port_to_bulk_read; - std::map port_to_sync_write_position; - std::map port_to_sync_write_velocity; - std::map port_to_sync_write_torque; - - /* publisher */ - ros::Publisher goal_joint_state_pub; - ros::Publisher present_joint_state_pub; - ros::Publisher current_module_pub; - - std::map gazebo_joint_pub; - - static void *ThreadProc(void *param); - - RobotisController(); - - bool Initialize(const std::string robot_file_path, const std::string init_file_path); - void InitDevice(const std::string init_file_path); - void Process(); - - void AddMotionModule(MotionModule *module); - void RemoveMotionModule(MotionModule *module); - void AddSensorModule(SensorModule *module); - void RemoveSensorModule(SensorModule *module); - - void StartTimer(); - void StopTimer(); - bool IsTimerRunning(); - - void LoadOffset(const std::string path); - - /* ROS Topic Callback Functions */ - void SyncWriteItemCallback(const robotis_controller_msgs::SyncWriteItem::ConstPtr &msg); - void SetControllerModeCallback(const std_msgs::String::ConstPtr &msg); - void SetJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg); - void SetJointCtrlModuleCallback(const robotis_controller_msgs::JointCtrlModule::ConstPtr &msg); - void SetCtrlModuleCallback(const std_msgs::String::ConstPtr &msg); - bool GetCtrlModuleCallback(robotis_controller_msgs::GetJointModule::Request &req, robotis_controller_msgs::GetJointModule::Response &res); - - void GazeboJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg); - - int Ping (const std::string joint_name, UINT8_T *error = 0); - int Ping (const std::string joint_name, UINT16_T* model_number, UINT8_T *error = 0); - - int Action (const std::string joint_name); - int Reboot (const std::string joint_name, UINT8_T *error = 0); - int FactoryReset(const std::string joint_name, UINT8_T option = 0, UINT8_T *error = 0); - - int Read (const std::string joint_name, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error = 0); - int ReadCtrlItem(const std::string joint_name, const std::string item_name, UINT32_T *data, UINT8_T *error = 0); - - int Read1Byte (const std::string joint_name, UINT16_T address, UINT8_T *data, UINT8_T *error = 0); - int Read2Byte (const std::string joint_name, UINT16_T address, UINT16_T *data, UINT8_T *error = 0); - int Read4Byte (const std::string joint_name, UINT16_T address, UINT32_T *data, UINT8_T *error = 0); - - int Write (const std::string joint_name, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error = 0); - int WriteCtrlItem(const std::string joint_name, const std::string item_name, UINT32_T data, UINT8_T *error = 0); - - int Write1Byte (const std::string joint_name, UINT16_T address, UINT8_T data, UINT8_T *error = 0); - int Write2Byte (const std::string joint_name, UINT16_T address, UINT16_T data, UINT8_T *error = 0); - int Write4Byte (const std::string joint_name, UINT16_T address, UINT32_T data, UINT8_T *error = 0); - - int RegWrite (const std::string joint_name, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error = 0); -}; - -} - - -#endif /* ROBOTIS_FRAMEWORK_ROBOTIS_CONTROLLER_INCLUDE_ROBOTIS_CONTROLLER_ROBOTISCONTROLLER_H_ */ diff --git a/robotis_controller/include/robotis_controller/robotis_controller.h b/robotis_controller/include/robotis_controller/robotis_controller.h new file mode 100644 index 0000000..65d7821 --- /dev/null +++ b/robotis_controller/include/robotis_controller/robotis_controller.h @@ -0,0 +1,183 @@ +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * 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 ROBOTIS 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. + *******************************************************************************/ + +/* + * robotis_controller.h + * + * Created on: 2016. 1. 15. + * Author: zerom + */ + +#ifndef ROBOTIS_CONTROLLER_ROBOTIS_CONTROLLER_H_ +#define ROBOTIS_CONTROLLER_ROBOTIS_CONTROLLER_H_ + + +#include +#include +#include +#include +#include +#include + +#include "robotis_controller_msgs/SyncWriteItem.h" +#include "robotis_controller_msgs/JointCtrlModule.h" +#include "robotis_controller_msgs/GetJointModule.h" + +#include "robotis_device/robot.h" +#include "robotis_framework_common/motion_module.h" +#include "robotis_framework_common/sensor_module.h" +#include "dynamixel_sdk/group_bulk_read.h" +#include "dynamixel_sdk/group_sync_write.h" + +namespace robotis_framework +{ + +enum ControllerMode +{ + MotionModuleMode, + DirectControlMode +}; + +class RobotisController : public Singleton +{ +private: + boost::thread queue_thread_; + boost::thread gazebo_thread_; + boost::thread set_module_thread_; + boost::mutex queue_mutex_; + + bool init_pose_loaded_; + bool is_timer_running_; + bool stop_timer_; + pthread_t timer_thread_; + ControllerMode controller_mode_; + + std::list motion_modules_; + std::list sensor_modules_; + std::vector direct_sync_write_; + + std::map sensor_result_; + + void gazeboTimerThread(); + void msgQueueThread(); + void setCtrlModuleThread(std::string ctrl_module); + + bool isTimerStopped(); + void initializeSyncWrite(); + +public: + static const int CONTROL_CYCLE_MSEC = 8; // 8 msec + + bool DEBUG_PRINT; + Robot *robot_; + + bool gazebo_mode_; + std::string gazebo_robot_name_; + + /* bulk read */ + std::map port_to_bulk_read_; + + /* sync write */ + std::map port_to_sync_write_position_; + std::map port_to_sync_write_velocity_; + std::map port_to_sync_write_current_; + std::map port_to_sync_write_position_p_gain_; + std::map port_to_sync_write_position_i_gain_; + std::map port_to_sync_write_position_d_gain_; + std::map port_to_sync_write_velocity_p_gain_; + std::map port_to_sync_write_velocity_i_gain_; + std::map port_to_sync_write_velocity_d_gain_; + + /* publisher */ + ros::Publisher goal_joint_state_pub_; + ros::Publisher present_joint_state_pub_; + ros::Publisher current_module_pub_; + + std::map gazebo_joint_position_pub_; + std::map gazebo_joint_velocity_pub_; + std::map gazebo_joint_effort_pub_; + + static void *timerThread(void *param); + + RobotisController(); + + bool initialize(const std::string robot_file_path, const std::string init_file_path); + void initializeDevice(const std::string init_file_path); + void process(); + + void addMotionModule(MotionModule *module); + void removeMotionModule(MotionModule *module); + void addSensorModule(SensorModule *module); + void removeSensorModule(SensorModule *module); + + void startTimer(); + void stopTimer(); + bool isTimerRunning(); + + void loadOffset(const std::string path); + + /* ROS Topic Callback Functions */ + void syncWriteItemCallback(const robotis_controller_msgs::SyncWriteItem::ConstPtr &msg); + void setControllerModeCallback(const std_msgs::String::ConstPtr &msg); + void setJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg); + void setJointCtrlModuleCallback(const robotis_controller_msgs::JointCtrlModule::ConstPtr &msg); + void setCtrlModuleCallback(const std_msgs::String::ConstPtr &msg); + bool getCtrlModuleCallback(robotis_controller_msgs::GetJointModule::Request &req, robotis_controller_msgs::GetJointModule::Response &res); + + void gazeboJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg); + + int ping (const std::string joint_name, uint8_t *error = 0); + int ping (const std::string joint_name, uint16_t* model_number, uint8_t *error = 0); + + int action (const std::string joint_name); + int reboot (const std::string joint_name, uint8_t *error = 0); + int factoryReset(const std::string joint_name, uint8_t option = 0, uint8_t *error = 0); + + int read (const std::string joint_name, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0); + int readCtrlItem(const std::string joint_name, const std::string item_name, uint32_t *data, uint8_t *error = 0); + + int read1Byte (const std::string joint_name, uint16_t address, uint8_t *data, uint8_t *error = 0); + int read2Byte (const std::string joint_name, uint16_t address, uint16_t *data, uint8_t *error = 0); + int read4Byte (const std::string joint_name, uint16_t address, uint32_t *data, uint8_t *error = 0); + + int write (const std::string joint_name, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0); + int writeCtrlItem(const std::string joint_name, const std::string item_name, uint32_t data, uint8_t *error = 0); + + int write1Byte (const std::string joint_name, uint16_t address, uint8_t data, uint8_t *error = 0); + int write2Byte (const std::string joint_name, uint16_t address, uint16_t data, uint8_t *error = 0); + int write4Byte (const std::string joint_name, uint16_t address, uint32_t data, uint8_t *error = 0); + + int regWrite (const std::string joint_name, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0); +}; + +} + + +#endif /* ROBOTIS_CONTROLLER_ROBOTIS_CONTROLLER_H_ */ diff --git a/robotis_controller/package.xml b/robotis_controller/package.xml index 8a25f7e..6dcf6d2 100644 --- a/robotis_controller/package.xml +++ b/robotis_controller/package.xml @@ -6,16 +6,12 @@ ROBOTIS - BSD - - ROBOTIS - catkin roscpp diff --git a/robotis_controller/src/robotis_controller/RobotisController.cpp b/robotis_controller/src/robotis_controller/RobotisController.cpp deleted file mode 100644 index e93e58c..0000000 --- a/robotis_controller/src/robotis_controller/RobotisController.cpp +++ /dev/null @@ -1,1592 +0,0 @@ -/* - * RobotisController.cpp - * - * Created on: 2016. 1. 15. - * Author: zerom - */ - -#include -#include - -#include "robotis_controller/RobotisController.h" - -using namespace ROBOTIS; - -RobotisController::RobotisController() -: is_timer_running_(false), - stop_timer_(false), - init_pose_loaded_(false), - timer_thread_(0), - controller_mode_(MOTION_MODULE_MODE), - DEBUG_PRINT(false), - robot(0), - gazebo_mode(false), - gazebo_robot_name("robotis") -{ - direct_sync_write_.clear(); -} - -void RobotisController::InitSyncWrite() -{ - if(gazebo_mode == true) - return; - - ROS_INFO("FIRST BULKREAD"); - // bulkread twice - for(std::map::iterator _it = port_to_bulk_read.begin(); _it != port_to_bulk_read.end(); _it++) - _it->second->TxRxPacket(); - for(std::map::iterator _it = port_to_bulk_read.begin(); _it != port_to_bulk_read.end(); _it++) - { - int _error_cnt = 0; - int _result = COMM_SUCCESS; - do { - if(++_error_cnt > 10) - { - ROS_ERROR("[RobotisController] bulk read fail!!"); - exit(-1); - } - usleep(10*1000); - _result = _it->second->TxRxPacket(); - } while (_result != COMM_SUCCESS); - } - init_pose_loaded_ = true; - ROS_INFO("FIRST BULKREAD END"); - - // clear syncwrite param setting - for(std::map::iterator _it = port_to_sync_write_position.begin(); _it != port_to_sync_write_position.end(); _it++) - _it->second->ClearParam(); - for(std::map::iterator _it = port_to_sync_write_velocity.begin(); _it != port_to_sync_write_velocity.end(); _it++) - _it->second->ClearParam(); - for(std::map::iterator _it = port_to_sync_write_torque.begin(); _it != port_to_sync_write_torque.end(); _it++) - _it->second->ClearParam(); - - // set init syncwrite param(from data of bulkread) - for(std::map::iterator _it = robot->dxls.begin(); _it != robot->dxls.end(); _it++) - { - std::string _joint_name = _it->first; - Dynamixel *_dxl = _it->second; - - for(int _i = 0; _i < _dxl->bulk_read_items.size(); _i++) - { - UINT32_T _read_data = 0; - UINT8_T _sync_write_data[4]; - - if(port_to_bulk_read[_dxl->port_name]->IsAvailable(_dxl->id, - _dxl->bulk_read_items[_i]->address, - _dxl->bulk_read_items[_i]->data_length) == true) - { - _read_data = port_to_bulk_read[_dxl->port_name]->GetData(_dxl->id, - _dxl->bulk_read_items[_i]->address, - _dxl->bulk_read_items[_i]->data_length); - - _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_read_data)); - _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_read_data)); - _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_read_data)); - _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_read_data)); - - if(_dxl->present_position_item != 0 && _dxl->bulk_read_items[_i]->item_name == _dxl->present_position_item->item_name) - { - _dxl->dxl_state->present_position = _dxl->ConvertValue2Radian(_read_data) - _dxl->dxl_state->position_offset; // remove offset - _dxl->dxl_state->goal_position = _dxl->dxl_state->present_position; - - port_to_sync_write_position[_dxl->port_name]->AddParam(_dxl->id, _sync_write_data); - } - else if(_dxl->present_velocity_item != 0 && _dxl->bulk_read_items[_i]->item_name == _dxl->present_velocity_item->item_name) - { - _dxl->dxl_state->present_velocity = _dxl->ConvertValue2Velocity(_read_data); - _dxl->dxl_state->goal_velocity = _dxl->dxl_state->present_velocity; - } - else if(_dxl->present_current_item != 0 && _dxl->bulk_read_items[_i]->item_name == _dxl->present_current_item->item_name) - { - _dxl->dxl_state->present_current = _dxl->ConvertValue2Current(_read_data); - _dxl->dxl_state->goal_current = _dxl->dxl_state->present_current; - } - } - } - } -} - -bool RobotisController::Initialize(const std::string robot_file_path, const std::string init_file_path) -{ - std::string _dev_desc_dir_path = ros::package::getPath("robotis_device") + "/devices"; - - // load robot info : port , device - robot = new Robot(robot_file_path, _dev_desc_dir_path); - - if(gazebo_mode == true) - { - queue_thread_ = boost::thread(boost::bind(&RobotisController::QueueThread, this)); - return true; - } - - for(std::map::iterator _it = robot->ports.begin(); _it != robot->ports.end(); _it++) - { - std::string _port_name = _it->first; - PortHandler *_port = _it->second; - - PacketHandler *_default_pkt_handler = PacketHandler::GetPacketHandler(2.0); - - if(_port->SetBaudRate(_port->GetBaudRate()) == false) - { - ROS_ERROR("PORT [%s] SETUP ERROR! (baudrate: %d)", _port_name.c_str(), _port->GetBaudRate()); - exit(-1); - } - - // get the default device info of the port - std::string _default_device_name = robot->port_default_device[_port_name]; - std::map::iterator _dxl_it = robot->dxls.find(_default_device_name); - std::map::iterator _sensor_it = robot->sensors.find(_default_device_name); - if(_dxl_it != robot->dxls.end()) - { - Dynamixel *_default_device = _dxl_it->second; - _default_pkt_handler = PacketHandler::GetPacketHandler(_default_device->protocol_version); - - port_to_sync_write_position[_port_name] = new GroupSyncWrite(_port, - _default_pkt_handler, - _default_device->goal_position_item->address, - _default_device->goal_position_item->data_length); - - port_to_sync_write_velocity[_port_name] = new GroupSyncWrite(_port, - _default_pkt_handler, - _default_device->goal_velocity_item->address, - _default_device->goal_velocity_item->data_length); - - port_to_sync_write_torque[_port_name] = new GroupSyncWrite(_port, - _default_pkt_handler, - _default_device->goal_current_item->address, - _default_device->goal_current_item->data_length); - } - else if(_sensor_it != robot->sensors.end()) - { - Sensor *_default_device = _sensor_it->second; - _default_pkt_handler = PacketHandler::GetPacketHandler(_default_device->protocol_version); - } - - port_to_bulk_read[_port_name] = new GroupBulkRead(_port, _default_pkt_handler); - } - - // (for loop) check all dxls are connected. - for(std::map::iterator _it = robot->dxls.begin(); _it != robot->dxls.end(); _it++) - { - std::string _joint_name = _it->first; - Dynamixel *_dxl = _it->second; - - if(Ping(_joint_name) != 0) - { - usleep(10*1000); - if(Ping(_joint_name) != 0) - ROS_ERROR("JOINT[%s] does NOT respond!!", _joint_name.c_str()); - } - } - - InitDevice(init_file_path); - - // [ BulkRead ] StartAddress : Present Position , Length : 10 ( Position/Velocity/Current ) - for(std::map::iterator _it = robot->dxls.begin(); _it != robot->dxls.end(); _it++) - { - std::string _joint_name = _it->first; - Dynamixel *_dxl = _it->second; - - if(_dxl == NULL) - continue; - - int _bulkread_start_addr = 0; - int _bulkread_data_length = 0; - - // bulk read default : present position - if(_dxl->present_position_item != 0) - { - _bulkread_start_addr = _dxl->present_position_item->address; - _bulkread_data_length = _dxl->present_position_item->data_length; - } - - // TODO: modifing - std::map::iterator _indirect_addr_it = _dxl->ctrl_table.find(INDIRECT_ADDRESS_1); - if(_indirect_addr_it != _dxl->ctrl_table.end()) // INDIRECT_ADDRESS_1 exist - { - if(_dxl->bulk_read_items.size() != 0) - { - _bulkread_start_addr = _dxl->bulk_read_items[0]->address; - _bulkread_data_length = 0; - - // set indirect address - int _indirect_addr = _indirect_addr_it->second->address; - for(int _i = 0; _i < _dxl->bulk_read_items.size(); _i++) - { - int _addr_leng = _dxl->bulk_read_items[_i]->data_length; - - _bulkread_data_length += _addr_leng; - for(int _l = 0; _l < _addr_leng; _l++) - { -// ROS_WARN("[%12s] INDIR_ADDR: %d, ITEM_ADDR: %d", _joint_name.c_str(), _indirect_addr, _dxl->ctrl_table[_dxl->bulk_read_items[_i]->item_name]->address + _l); - Write2Byte(_joint_name, _indirect_addr, _dxl->ctrl_table[_dxl->bulk_read_items[_i]->item_name]->address + _l); - _indirect_addr += 2; - } - } - } - } - else // INDIRECT_ADDRESS_1 NOT exist - { - if(_dxl->bulk_read_items.size() != 0) - { - _bulkread_start_addr = _dxl->bulk_read_items[0]->address; - _bulkread_data_length = 0; - - ControlTableItem *_last_item = _dxl->bulk_read_items[0]; - - for(int _i = 0; _i < _dxl->bulk_read_items.size(); _i++) - { - int _addr = _dxl->bulk_read_items[_i]->address; - if(_addr < _bulkread_start_addr) - _bulkread_start_addr = _addr; - else if(_last_item->address < _addr) - _last_item = _dxl->bulk_read_items[_i]; - } - - _bulkread_data_length = _last_item->address - _bulkread_start_addr + _last_item->data_length; - } - } - -// ROS_WARN("[%12s] start_addr: %d, data_length: %d", _joint_name.c_str(), _bulkread_start_addr, _bulkread_data_length); - port_to_bulk_read[_dxl->port_name]->AddParam(_dxl->id, _bulkread_start_addr, _bulkread_data_length); - - // Torque ON - if(WriteCtrlItem(_joint_name, _dxl->torque_enable_item->item_name, 1) != COMM_SUCCESS) - WriteCtrlItem(_joint_name, _dxl->torque_enable_item->item_name, 1); - } - - queue_thread_ = boost::thread(boost::bind(&RobotisController::QueueThread, this)); - return true; -} - -void RobotisController::InitDevice(const std::string init_file_path) -{ - // device initialize - if(DEBUG_PRINT) ROS_WARN("INIT FILE LOAD"); - - YAML::Node _doc; - try{ - _doc = YAML::LoadFile(init_file_path.c_str()); - - for(YAML::const_iterator _it_doc = _doc.begin(); _it_doc != _doc.end(); _it_doc++) - { - std::string _joint_name = _it_doc->first.as(); - - YAML::Node _joint_node = _doc[_joint_name]; - if(_joint_node.size() == 0) - continue; - - Dynamixel *_dxl = NULL; - std::map::iterator _dxl_it = robot->dxls.find(_joint_name); - if(_dxl_it != robot->dxls.end()) - _dxl = _dxl_it->second; - - if(_dxl == NULL) - { - ROS_WARN("Joint [%s] does not found.", _joint_name.c_str()); - continue; - } - if(DEBUG_PRINT) ROS_INFO("JOINT_NAME: %s", _joint_name.c_str()); - - for(YAML::const_iterator _it_joint = _joint_node.begin(); _it_joint != _joint_node.end(); _it_joint++) - { - std::string _item_name = _it_joint->first.as(); - - if(DEBUG_PRINT) ROS_INFO(" ITEM_NAME: %s", _item_name.c_str()); - - UINT32_T _value = _it_joint->second.as(); - - ControlTableItem *_item = _dxl->ctrl_table[_item_name]; - if(_item == NULL) - { - ROS_WARN("Control Item [%s] does not found.", _item_name.c_str()); - continue; - } - - if(_item->memory_type == EEPROM) - { - UINT8_T _data8 = 0; - UINT16_T _data16 = 0; - UINT32_T _data32 = 0; - - switch(_item->data_length) - { - case 1: - Read1Byte(_joint_name, _item->address, &_data8); - if(_data8 == _value) - continue; - break; - case 2: - Read2Byte(_joint_name, _item->address, &_data16); - if(_data16 == _value) - continue; - break; - case 4: - Read4Byte(_joint_name, _item->address, &_data32); - if(_data32 == _value) - continue; - break; - default: - break; - } - } - - switch(_item->data_length) - { - case 1: - Write1Byte(_joint_name, _item->address, (UINT8_T)_value); - break; - case 2: - Write2Byte(_joint_name, _item->address, (UINT16_T)_value); - break; - case 4: - Write4Byte(_joint_name, _item->address, _value); - break; - default: - break; - } - - if(_item->memory_type == EEPROM) - { - // Write to EEPROM -> delay is required (max delay: 55 msec per byte) - usleep(_item->data_length * 55 * 1000); - } - } - } - } catch(const std::exception& e) { - ROS_INFO("Dynamixel Init file not found."); - } -} - -void RobotisController::GazeboThread() -{ - ros::Rate gazebo_rate(1000 / CONTROL_CYCLE_MSEC); - - while(!stop_timer_) - { - if(init_pose_loaded_ == true) - Process(); - gazebo_rate.sleep(); - } -} - -void RobotisController::QueueThread() -{ - ros::NodeHandle _ros_node; - ros::CallbackQueue _callback_queue; - - _ros_node.setCallbackQueue(&_callback_queue); - - /* subscriber */ - ros::Subscriber _sync_write_item_sub = _ros_node.subscribe("/robotis/sync_write_item", 10, &RobotisController::SyncWriteItemCallback, this); - ros::Subscriber _joint_ctrl_modules_sub = _ros_node.subscribe("/robotis/set_joint_ctrl_modules", 10, &RobotisController::SetJointCtrlModuleCallback, this); - ros::Subscriber _enable_ctrl_module_sub = _ros_node.subscribe("/robotis/enable_ctrl_module", 10, &RobotisController::SetCtrlModuleCallback, this); - ros::Subscriber _control_mode_sub = _ros_node.subscribe("/robotis/set_control_mode", 10, &RobotisController::SetControllerModeCallback, this); - ros::Subscriber _joint_states_sub = _ros_node.subscribe("/robotis/set_joint_states", 10, &RobotisController::SetJointStatesCallback, this); - - /* publisher */ - goal_joint_state_pub = _ros_node.advertise("/robotis/goal_joint_states", 10); - present_joint_state_pub = _ros_node.advertise("/robotis/present_joint_states", 10); - current_module_pub = _ros_node.advertise("/robotis/present_joint_ctrl_modules", 10); - - ros::Subscriber _gazebo_joint_states_sub; - if(gazebo_mode == true) - { - _gazebo_joint_states_sub = _ros_node.subscribe("/" + gazebo_robot_name + "/joint_states", 10, &RobotisController::GazeboJointStatesCallback, this); - - for(std::map::iterator _it = robot->dxls.begin(); _it != robot->dxls.end(); _it++) - gazebo_joint_pub[_it->first] = _ros_node.advertise("/" + gazebo_robot_name + "/" + _it->first + "_pos/command", 1); - } - - /* service */ - ros::ServiceServer _joint_module_server = _ros_node.advertiseService("/robotis/get_present_joint_ctrl_modules", &RobotisController::GetCtrlModuleCallback, this); - - while(_ros_node.ok()) - { - _callback_queue.callAvailable(); - usleep(100); - } -} - -void *RobotisController::ThreadProc(void *param) -{ - RobotisController *controller = (RobotisController *)param; - static struct timespec next_time; - static struct timespec curr_time; - - ROS_INFO("controller::thread_proc"); - - clock_gettime(CLOCK_MONOTONIC, &next_time); - - while(!controller->stop_timer_) - { - next_time.tv_sec += (next_time.tv_nsec + CONTROL_CYCLE_MSEC * 1000000) / 1000000000; - next_time.tv_nsec = (next_time.tv_nsec + CONTROL_CYCLE_MSEC * 1000000) % 1000000000; - - controller->Process(); - - clock_gettime(CLOCK_MONOTONIC, &curr_time); - long delta_nsec = (next_time.tv_sec - curr_time.tv_sec) * 1000000000 + (next_time.tv_nsec - curr_time.tv_nsec); - if(delta_nsec < -100000 ) - { - if(controller->DEBUG_PRINT == true) - ROS_WARN("[RobotisController::ThreadProc] NEXT TIME < CURR TIME.. (%f)[%ld.%09ld / %ld.%09ld]", delta_nsec/1000000.0, (long)next_time.tv_sec, (long)next_time.tv_nsec, (long)curr_time.tv_sec, (long)curr_time.tv_nsec); - // next_time = curr_time + 3 msec - next_time.tv_sec = curr_time.tv_sec + (curr_time.tv_nsec + 3000000) / 1000000000; - next_time.tv_nsec = (curr_time.tv_nsec + 3000000) % 1000000000; - } - - clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &next_time, NULL); - } - return 0; -} - -void RobotisController::StartTimer() -{ - if(this->is_timer_running_ == true) - return; - - if(this->gazebo_mode == true) - { - // create and start the thread - gazebo_thread_ = boost::thread(boost::bind(&RobotisController::GazeboThread, this)); - } - else - { - InitSyncWrite(); - - for(std::map::iterator _it = port_to_bulk_read.begin(); _it != port_to_bulk_read.end(); _it++) - _it->second->TxPacket(); - - int error; - struct sched_param param; - pthread_attr_t attr; - - pthread_attr_init(&attr); - - error = pthread_attr_setschedpolicy(&attr, SCHED_RR); - if(error != 0) - ROS_ERROR("pthread_attr_setschedpolicy error = %d\n",error); - error = pthread_attr_setinheritsched(&attr,PTHREAD_EXPLICIT_SCHED); - if(error != 0) - ROS_ERROR("pthread_attr_setinheritsched error = %d\n",error); - - memset(¶m, 0, sizeof(param)); - param.sched_priority = 31;// RT - error = pthread_attr_setschedparam(&attr, ¶m); - if(error != 0) - ROS_ERROR("pthread_attr_setschedparam error = %d\n",error); - - // create and start the thread - if((error = pthread_create(&this->timer_thread_, &attr, this->ThreadProc, this))!= 0) { - ROS_ERROR("timer thread create fail!!"); - exit(-1); - } - } - - this->is_timer_running_ = true; -} - -void RobotisController::StopTimer() -{ - int error = 0; - - // set the flag to stop the thread - if(this->is_timer_running_) - { - this->stop_timer_ = true; - - if(this->gazebo_mode == false) - { - // wait until the thread is stopped. - if((error = pthread_join(this->timer_thread_, NULL)) != 0) - exit(-1); - - for(std::map::iterator _it = port_to_bulk_read.begin(); _it != port_to_bulk_read.end(); _it++) - _it->second->RxPacket(); - - for(std::map::iterator _it = port_to_sync_write_position.begin(); _it != port_to_sync_write_position.end(); _it++) - _it->second->ClearParam(); - for(std::map::iterator _it = port_to_sync_write_velocity.begin(); _it != port_to_sync_write_velocity.end(); _it++) - _it->second->ClearParam(); - for(std::map::iterator _it = port_to_sync_write_torque.begin(); _it != port_to_sync_write_torque.end(); _it++) - _it->second->ClearParam(); - } - else - { - // wait until the thread is stopped. - gazebo_thread_.join(); - } - - this->stop_timer_ = false; - this->is_timer_running_ = false; - } -} - -bool RobotisController::IsTimerRunning() -{ - return this->is_timer_running_; -} - -void RobotisController::LoadOffset(const std::string path) -{ - YAML::Node _doc; - try{ - _doc = YAML::LoadFile(path.c_str()); - } catch(const std::exception& e) { - ROS_ERROR("Fail to load offset yaml."); - return; - } - - YAML::Node _offset_node = _doc["offset"]; - if(_offset_node.size() == 0) - return; - - ROS_INFO("Load offsets..."); - for(YAML::const_iterator _it = _offset_node.begin(); _it != _offset_node.end(); _it++) - { - std::string _joint_name = _it->first.as(); - double _offset = _it->second.as(); - - std::map::iterator _dxl_it = robot->dxls.find(_joint_name); - if(_dxl_it != robot->dxls.end()) - _dxl_it->second->dxl_state->position_offset = _offset; - } -} - -void RobotisController::Process() -{ - // avoid duplicated function call - static bool _is_process_running = false; - if(_is_process_running == true) - return; - _is_process_running = true; - - // ROS_INFO("Controller::Process()"); - bool _do_sync_write = false; - - sensor_msgs::JointState _goal_state; - sensor_msgs::JointState _present_state; - - -// ros::Time _now = ros::Time::now(); - - - // BulkRead Rx - if(gazebo_mode == false) - { - for(std::map::iterator _it = port_to_bulk_read.begin(); _it != port_to_bulk_read.end(); _it++) - _it->second->RxPacket(); - } - -// ros::Duration _dur = ros::Time::now() - _now; -// double _msec = _dur.nsec * 0.000001; -// -// if(_msec > 8) std::cout << "Process duration : " << _msec << std::endl; - - // -> save to Robot->dxls[]->dynamixel_state.present_position - if(robot->dxls.size() > 0) - { - for(std::map::iterator dxl_it = robot->dxls.begin(); dxl_it != robot->dxls.end(); dxl_it++) - { - UINT32_T _data = 0; - - std::string _port_name = dxl_it->second->port_name; - std::string _joint_name = dxl_it->first; - Dynamixel *_dxl = dxl_it->second; - - _present_state.header.stamp = ros::Time::now(); - _goal_state.header.stamp = _present_state.header.stamp; - - if(gazebo_mode == false && _dxl->bulk_read_items.size() > 0) - { - for(int _i = 0; _i < _dxl->bulk_read_items.size(); _i++) - { - ControlTableItem *_item = _dxl->bulk_read_items[_i]; - if(port_to_bulk_read[_port_name]->IsAvailable(_dxl->id, _item->address, _item->data_length) == true) - { - _data = port_to_bulk_read[_port_name]->GetData(_dxl->id, _item->address, _item->data_length); - - // TODO: change dxl_state - if(_dxl->present_position_item != 0 && _item->item_name == _dxl->present_position_item->item_name) - _dxl->dxl_state->present_position = _dxl->ConvertValue2Radian(_data) - _dxl->dxl_state->position_offset; // remove offset - else if(_dxl->present_velocity_item != 0 && _item->item_name == _dxl->present_velocity_item->item_name) - _dxl->dxl_state->present_velocity = _dxl->ConvertValue2Velocity(_data); - else if(_dxl->present_current_item != 0 && _item->item_name == _dxl->present_current_item->item_name) - _dxl->dxl_state->present_current = _dxl->ConvertValue2Current(_data); - else if(_dxl->goal_position_item != 0 && _item->item_name == _dxl->goal_position_item->item_name) - _dxl->dxl_state->goal_position = _dxl->ConvertValue2Radian(_data) - _dxl->dxl_state->position_offset; // remove offset - else if(_dxl->goal_velocity_item != 0 && _item->item_name == _dxl->goal_velocity_item->item_name) - _dxl->dxl_state->goal_velocity = _dxl->ConvertValue2Velocity(_data); - else if(_dxl->goal_current_item != 0 && _item->item_name == _dxl->goal_current_item->item_name) - _dxl->dxl_state->goal_current = _dxl->ConvertValue2Current(_data); - else - _dxl->dxl_state->bulk_read_table[_item->item_name] = _data; - } - } - - // -> update time stamp to Robot->dxls[]->dynamixel_state.update_time_stamp - _dxl->dxl_state->update_time_stamp = TimeStamp(_present_state.header.stamp.sec, _present_state.header.stamp.nsec); - } - - _present_state.name.push_back(_joint_name); - // TODO: should be converted to rad, rad/s, Nm - _present_state.position.push_back(_dxl->dxl_state->present_position); - _present_state.velocity.push_back(_dxl->dxl_state->present_velocity); - _present_state.effort.push_back(_dxl->dxl_state->present_current); - - _goal_state.name.push_back(_joint_name); - _goal_state.position.push_back(_dxl->dxl_state->goal_position); - _goal_state.velocity.push_back(_dxl->dxl_state->goal_velocity); - _goal_state.effort.push_back(_dxl->dxl_state->goal_current); - } - } - - // -> publish present joint_states & goal joint states topic - present_joint_state_pub.publish(_present_state); - goal_joint_state_pub.publish(_goal_state); - - if(robot->sensors.size() > 0) - { - for(std::map::iterator sensor_it = robot->sensors.begin(); sensor_it != robot->sensors.end(); sensor_it++) - { - UINT32_T _data = 0; - - std::string _port_name = sensor_it->second->port_name; - std::string _sensor_name = sensor_it->first; - Sensor *_sensor = sensor_it->second; - - if(gazebo_mode == false && _sensor->bulk_read_items.size() > 0) - { - for(int _i = 0; _i < _sensor->bulk_read_items.size(); _i++) - { - ControlTableItem *_item = _sensor->bulk_read_items[_i]; - if(port_to_bulk_read[_port_name]->IsAvailable(_sensor->id, _item->address, _item->data_length) == true) - { - _data = port_to_bulk_read[_port_name]->GetData(_sensor->id, _item->address, _item->data_length); - - // TODO: change sensor_state - _sensor->sensor_state->bulk_read_table[_item->item_name] = _data; - } - } - - // -> update time stamp to Robot->dxls[]->dynamixel_state.update_time_stamp - ros::Time _now = ros::Time::now(); - _sensor->sensor_state->update_time_stamp = TimeStamp(_now.sec, _now.nsec); - } - } - } - - // Call SensorModule Process() - // -> for loop : call SensorModule list -> Process() - if(sensor_modules_.size() > 0) - { - for(std::list::iterator _module_it = sensor_modules_.begin(); _module_it != sensor_modules_.end(); _module_it++) - { - (*_module_it)->Process(robot->dxls, robot->sensors); - - for(std::map::iterator _it = (*_module_it)->result.begin(); _it != (*_module_it)->result.end(); _it++) - sensor_result_[_it->first] = _it->second; - } - } - - if(controller_mode_ == MOTION_MODULE_MODE) - { - // Call MotionModule Process() - // -> for loop : call MotionModule list -> Process() - if(motion_modules_.size() > 0) - { - queue_mutex_.lock(); - - for(std::list::iterator module_it = motion_modules_.begin(); module_it != motion_modules_.end(); module_it++) - { -// ros::Time _before = ros::Time::now(); - - if((*module_it)->enable == false) - continue; - - (*module_it)->Process(robot->dxls, sensor_result_); - -// ros::Duration _dur = ros::Time::now() - _before; -// double _msec = _dur.sec * 1e+3 + _dur.nsec * 0.000001; - -// std::cout << "Process duration ["<< (*module_it)->module_name << "] : " << _msec << std::endl; - - - ros::Time _before = ros::Time::now(); - - // for loop : joint list - for(std::map::iterator dxl_it = robot->dxls.begin(); dxl_it != robot->dxls.end(); dxl_it++) - { - std::string _joint_name = dxl_it->first; - Dynamixel *_dxl = dxl_it->second; - - DynamixelState *_dxl_state = _dxl->dxl_state; - if(_dxl->ctrl_module_name == (*module_it)->module_name) - { - _do_sync_write = true; - // ROS_INFO("Set goal value"); - DynamixelState *_result_state = (*module_it)->result[_joint_name]; - - if(_result_state == NULL) { - ROS_INFO("[%s] %s", (*module_it)->module_name.c_str(), _joint_name.c_str()); - continue; - } - - // TODO: check update time stamp ? - - if((*module_it)->control_mode == POSITION_CONTROL) - { -// if(_result_state->goal_position == 0 && _dxl->id == 3) -// ROS_INFO("[MODULE:%s][ID:3] goal position = %f", (*module_it)->module_name.c_str(), _dxl_state->goal_position); - _dxl_state->goal_position = _result_state->goal_position; - - if(gazebo_mode == false) - { - // add offset - UINT32_T _pos_data = _dxl->ConvertRadian2Value(_dxl_state->goal_position + _dxl_state->position_offset); - UINT8_T _sync_write_data[4]; - _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_pos_data)); - _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_pos_data)); - _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_pos_data)); - _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_pos_data)); - - if(abs(_pos_data) > 151800) - printf("goal_pos : %f | position_offset : %f | pos_data : %d\n", _dxl_state->goal_position , _dxl_state->position_offset, _pos_data); - - port_to_sync_write_position[_dxl->port_name]->ChangeParam(_dxl->id, _sync_write_data); - } - } - else if((*module_it)->control_mode == VELOCITY_CONTROL) - { - _dxl_state->goal_velocity = _result_state->goal_velocity; - - if(gazebo_mode == false) - { - UINT32_T _vel_data = _dxl->ConvertVelocity2Value(_dxl_state->goal_velocity); - UINT8_T _sync_write_data[4]; - _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_vel_data)); - _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_vel_data)); - _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_vel_data)); - _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_vel_data)); - - port_to_sync_write_velocity[_dxl->port_name]->ChangeParam(_dxl->id, _sync_write_data); - } - } - else if((*module_it)->control_mode == CURRENT_CONTROL) - { - _dxl_state->goal_current = _result_state->goal_current; - - if(gazebo_mode == false) - { - UINT32_T _torq_data = _dxl->ConvertCurrent2Value(_dxl_state->goal_current); - UINT8_T _sync_write_data[2]; - _sync_write_data[0] = DXL_LOBYTE(_torq_data); - _sync_write_data[1] = DXL_HIBYTE(_torq_data); - - port_to_sync_write_torque[_dxl->port_name]->ChangeParam(_dxl->id, _sync_write_data); - } - } - } - } - - ros::Duration _dur = ros::Time::now() - _before; - double _msec = _dur.sec * 1e+3 + _dur.nsec * 0.000001; - - //std::cout << "Process duration ["<< (*module_it)->module_name << "] : " << _msec << std::endl; - - } - - // std::cout << " ------------------------------------------- " << std::endl; - queue_mutex_.unlock(); - } - - // TODO: Combine the result && SyncWrite - // -> SyncWrite - if(gazebo_mode == false && _do_sync_write) - { - for(std::map::iterator _it = port_to_sync_write_position.begin(); _it != port_to_sync_write_position.end(); _it++) - _it->second->TxPacket(); - for(std::map::iterator _it = port_to_sync_write_velocity.begin(); _it != port_to_sync_write_velocity.end(); _it++) - _it->second->TxPacket(); - for(std::map::iterator _it = port_to_sync_write_torque.begin(); _it != port_to_sync_write_torque.end(); _it++) - _it->second->TxPacket(); - } - else if(gazebo_mode == true) - { - std_msgs::Float64 _joint_msg; - - for(std::map::iterator dxl_it = robot->dxls.begin(); dxl_it != robot->dxls.end(); dxl_it++) - { - _joint_msg.data = dxl_it->second->dxl_state->goal_position; - gazebo_joint_pub[dxl_it->first].publish(_joint_msg); - } - } - } - else if(controller_mode_ == DIRECT_CONTROL_MODE) - { - queue_mutex_.lock(); - - for(std::map::iterator _it = port_to_sync_write_position.begin(); _it != port_to_sync_write_position.end(); _it++) - { - _it->second->TxPacket(); - _it->second->ClearParam(); - } - - if(direct_sync_write_.size() > 0) - { - for(int _i = 0; _i < direct_sync_write_.size(); _i++) - { - direct_sync_write_[_i]->TxPacket(); - direct_sync_write_[_i]->ClearParam(); - } - direct_sync_write_.clear(); - } - - queue_mutex_.unlock(); - } - - // TODO: User Read/Write - - // BulkRead Tx - if(gazebo_mode == false) - { - for(std::map::iterator _it = port_to_bulk_read.begin(); _it != port_to_bulk_read.end(); _it++) - _it->second->TxPacket(); - } - - // ros::Duration _dur = ros::Time::now() - _now; - // double _msec = _dur.nsec * 0.000001; - // - // if(_msec > 8) std::cout << "Process duration : " << _msec << std::endl; - - _is_process_running = false; -} - -void RobotisController::AddMotionModule(MotionModule *module) -{ - module->Initialize(CONTROL_CYCLE_MSEC, robot); - motion_modules_.push_back(module); - motion_modules_.unique(); -} - -void RobotisController::RemoveMotionModule(MotionModule *module) -{ - motion_modules_.remove(module); -} - -void RobotisController::AddSensorModule(SensorModule *module) -{ - module->Initialize(CONTROL_CYCLE_MSEC, robot); - sensor_modules_.push_back(module); - sensor_modules_.unique(); -} - -void RobotisController::RemoveSensorModule(SensorModule *module) -{ - sensor_modules_.remove(module); -} - -void RobotisController::SyncWriteItemCallback(const robotis_controller_msgs::SyncWriteItem::ConstPtr &msg) -{ - for(int _i = 0; _i < msg->joint_name.size(); _i++) - { - Dynamixel *_dxl = robot->dxls[msg->joint_name[_i]]; - ControlTableItem *_item = _dxl->ctrl_table[msg->item_name]; - - PortHandler *_port = robot->ports[_dxl->port_name]; - PacketHandler *_packet_handler= PacketHandler::GetPacketHandler(_dxl->protocol_version); - - if(_item->access_type == READ) - continue; - - int _idx = 0; - if(direct_sync_write_.size() == 0) - { - direct_sync_write_.push_back(new GroupSyncWrite(_port, _packet_handler, _item->address, _item->data_length)); - _idx = 0; - } - else - { - for(_idx = 0; _idx < direct_sync_write_.size(); _idx++) - { - if(direct_sync_write_[_idx]->GetPortHandler() == _port && - direct_sync_write_[_idx]->GetPacketHandler() == _packet_handler) - break; - } - - if(_idx == direct_sync_write_.size()) - direct_sync_write_.push_back(new GroupSyncWrite(_port, _packet_handler, _item->address, _item->data_length)); - } - - UINT8_T *_data = new UINT8_T[_item->data_length]; - if(_item->data_length == 1) - _data[0] = (UINT8_T)msg->value[_i]; - else if(_item->data_length == 2) - { - _data[0] = DXL_LOBYTE((UINT16_T)msg->value[_i]); - _data[1] = DXL_HIBYTE((UINT16_T)msg->value[_i]); - } - else if(_item->data_length == 4) - { - _data[0] = DXL_LOBYTE(DXL_LOWORD((UINT32_T)msg->value[_i])); - _data[1] = DXL_HIBYTE(DXL_LOWORD((UINT32_T)msg->value[_i])); - _data[2] = DXL_LOBYTE(DXL_HIWORD((UINT32_T)msg->value[_i])); - _data[3] = DXL_HIBYTE(DXL_HIWORD((UINT32_T)msg->value[_i])); - } - direct_sync_write_[_idx]->AddParam(_dxl->id, _data); - delete[] _data; - } -} - -void RobotisController::SetControllerModeCallback(const std_msgs::String::ConstPtr &msg) -{ - if(msg->data == "DIRECT_CONTROL_MODE") - controller_mode_ = DIRECT_CONTROL_MODE; - else if(msg->data == "MOTION_MODULE_MODE") - controller_mode_ = MOTION_MODULE_MODE; -} - -void RobotisController::SetJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg) -{ - if(controller_mode_ != DIRECT_CONTROL_MODE) - return; - - queue_mutex_.lock(); - - for(int _i = 0; _i < msg->name.size(); _i++) - { - INT32_T _pos = 0; - - Dynamixel *_dxl = robot->dxls[msg->name[_i]]; - if(_dxl == NULL) - continue; - - _dxl->dxl_state->goal_position = msg->position[_i]; - _pos = _dxl->ConvertRadian2Value((double)msg->position[_i]); - - UINT8_T _sync_write_data[4]; - _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_pos)); - _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_pos)); - _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_pos)); - _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_pos)); - - port_to_sync_write_position[_dxl->port_name]->AddParam(_dxl->id, _sync_write_data); - } - - queue_mutex_.unlock(); -} - -void RobotisController::SetCtrlModuleCallback(const std_msgs::String::ConstPtr &msg) -{ - std::string _module_name_to_set = msg->data; - - set_module_thread_ = boost::thread(boost::bind(&RobotisController::SetCtrlModuleThread, this, _module_name_to_set)); -} - -void RobotisController::SetJointCtrlModuleCallback(const robotis_controller_msgs::JointCtrlModule::ConstPtr &msg) -{ - if(msg->joint_name.size() != msg->module_name.size()) - return; - - queue_mutex_.lock(); - - for(unsigned int idx = 0; idx < msg->joint_name.size(); idx++) - { - Dynamixel *_dxl = NULL; - std::map::iterator _dxl_it = robot->dxls.find((std::string)(msg->joint_name[idx])); - if(_dxl_it != robot->dxls.end()) - _dxl = _dxl_it->second; - else - continue; - - // none - if(msg->module_name[idx] == "" || msg->module_name[idx] == "none") - { - _dxl->ctrl_module_name = msg->module_name[idx]; - continue; - } - - // check whether the module is using this joint - for(std::list::iterator _m_it = motion_modules_.begin(); _m_it != motion_modules_.end(); _m_it++) - { - if((*_m_it)->module_name == msg->module_name[idx]) - { - if((*_m_it)->result.find(msg->joint_name[idx]) != (*_m_it)->result.end()) - { - _dxl->ctrl_module_name = msg->module_name[idx]; - break; - } - } - } - } - - for(std::list::iterator _m_it = motion_modules_.begin(); _m_it != motion_modules_.end(); _m_it++) - { - // set all modules -> disable - (*_m_it)->enable = false; - - // set all used modules -> enable - for(std::map::iterator _d_it = robot->dxls.begin(); _d_it != robot->dxls.end(); _d_it++) - { - if(_d_it->second->ctrl_module_name == (*_m_it)->module_name) - { - (*_m_it)->enable = true; - break; - } - } - } - - // TODO: set indirect address - // -> check module's control_mode - - queue_mutex_.unlock(); - - robotis_controller_msgs::JointCtrlModule _current_module_msg; - for(std::map::iterator _dxl_iter = robot->dxls.begin(); _dxl_iter != robot->dxls.end(); ++_dxl_iter) - { - _current_module_msg.joint_name.push_back(_dxl_iter->first); - _current_module_msg.module_name.push_back(_dxl_iter->second->ctrl_module_name); - } - - if(_current_module_msg.joint_name.size() == _current_module_msg.module_name.size()) - current_module_pub.publish(_current_module_msg); -} - -bool RobotisController::GetCtrlModuleCallback(robotis_controller_msgs::GetJointModule::Request &req, robotis_controller_msgs::GetJointModule::Response &res) -{ - for(unsigned int idx = 0; idx < req.joint_name.size(); idx++) - { - std::map::iterator _d_it = robot->dxls.find((std::string)(req.joint_name[idx])); - if(_d_it != robot->dxls.end()) - { - res.joint_name.push_back(req.joint_name[idx]); - res.module_name.push_back(_d_it->second->ctrl_module_name); - } - } - - if(res.joint_name.size() == 0) return false; - - return true; -} - -void RobotisController::SetCtrlModuleThread(std::string ctrl_module) -{ - // stop module - std::list _stop_modules; - - if(ctrl_module == "" || ctrl_module == "none") - { - // enqueue all modules in order to stop - for(std::list::iterator _m_it = motion_modules_.begin(); _m_it != motion_modules_.end(); _m_it++) - { - if((*_m_it)->enable == true) _stop_modules.push_back(*_m_it); - } - } - else - { - for(std::list::iterator _m_it = motion_modules_.begin(); _m_it != motion_modules_.end(); _m_it++) - { - // if it exist - if((*_m_it)->module_name == ctrl_module) - { - // enqueue the module which lost control of joint in order to stop - for(std::map::iterator _result_it = (*_m_it)->result.begin(); _result_it != (*_m_it)->result.end(); _result_it++) - { - std::map::iterator _d_it = robot->dxls.find(_result_it->first); - - if(_d_it != robot->dxls.end()) - { - // enqueue - if(_d_it->second->ctrl_module_name != ctrl_module) - { - for(std::list::iterator _stop_m_it = motion_modules_.begin(); _stop_m_it != motion_modules_.end(); _stop_m_it++) - { - if((*_stop_m_it)->module_name == _d_it->second->ctrl_module_name && (*_stop_m_it)->enable == true) - _stop_modules.push_back(*_stop_m_it); - } - } - } - } - - break; - } - } - } - - // stop the module - _stop_modules.unique(); - for(std::list::iterator _stop_m_it = _stop_modules.begin(); _stop_m_it != _stop_modules.end(); _stop_m_it++) - { - (*_stop_m_it)->Stop(); - } - - // wait to stop - for(std::list::iterator _stop_m_it = _stop_modules.begin(); _stop_m_it != _stop_modules.end(); _stop_m_it++) - { - while((*_stop_m_it)->IsRunning()) - usleep(CONTROL_CYCLE_MSEC * 1000); - } - - // set ctrl module - queue_mutex_.lock(); - - if(DEBUG_PRINT) ROS_INFO_STREAM("set module : " << ctrl_module); - - // none - if(ctrl_module == "" || ctrl_module == "none") - { - // set all modules -> disable - for(std::list::iterator _m_it = motion_modules_.begin(); _m_it != motion_modules_.end(); _m_it++) - { - (*_m_it)->enable = false; - } - - // set dxl's control module to "none" - for(std::map::iterator _d_it = robot->dxls.begin(); _d_it != robot->dxls.end(); _d_it++) - { - Dynamixel *_dxl = _d_it->second; - _dxl->ctrl_module_name = "none"; - - UINT32_T _pos_data = _dxl->ConvertRadian2Value(_dxl->dxl_state->goal_position + _dxl->dxl_state->position_offset); - UINT8_T _sync_write_data[4]; - _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_pos_data)); - _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_pos_data)); - _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_pos_data)); - _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_pos_data)); - - port_to_sync_write_position[_dxl->port_name]->AddParam(_dxl->id, _sync_write_data); - - port_to_sync_write_torque[_dxl->port_name]->RemoveParam(_dxl->id); - port_to_sync_write_velocity[_dxl->port_name]->RemoveParam(_dxl->id); - } - } - else - { - // check whether the module exist - for(std::list::iterator _m_it = motion_modules_.begin(); _m_it != motion_modules_.end(); _m_it++) - { - // if it exist - if((*_m_it)->module_name == ctrl_module) - { - CONTROL_MODE _mode = (*_m_it)->control_mode; - for(std::map::iterator _result_it = (*_m_it)->result.begin(); _result_it != (*_m_it)->result.end(); _result_it++) - { - std::map::iterator _d_it = robot->dxls.find(_result_it->first); - if(_d_it != robot->dxls.end()) - { - Dynamixel *_dxl = _d_it->second; - _dxl->ctrl_module_name = ctrl_module; - - if(_mode == POSITION_CONTROL) - { - UINT32_T _pos_data = _dxl->ConvertRadian2Value(_dxl->dxl_state->goal_position + _dxl->dxl_state->position_offset); - UINT8_T _sync_write_data[4]; - _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_pos_data)); - _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_pos_data)); - _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_pos_data)); - _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_pos_data)); - - port_to_sync_write_position[_dxl->port_name]->AddParam(_dxl->id, _sync_write_data); - - port_to_sync_write_torque[_dxl->port_name]->RemoveParam(_dxl->id); - port_to_sync_write_velocity[_dxl->port_name]->RemoveParam(_dxl->id); - } - else if(_mode == VELOCITY_CONTROL) - { - UINT32_T _vel_data = _dxl->ConvertVelocity2Value(_dxl->dxl_state->goal_velocity); - UINT8_T _sync_write_data[4]; - _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_vel_data)); - _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_vel_data)); - _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_vel_data)); - _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_vel_data)); - - port_to_sync_write_velocity[_dxl->port_name]->AddParam(_dxl->id, _sync_write_data); - - port_to_sync_write_torque[_dxl->port_name]->RemoveParam(_dxl->id); - port_to_sync_write_position[_dxl->port_name]->RemoveParam(_dxl->id); - } - else if(_mode == CURRENT_CONTROL) - { - UINT32_T _curr_data = _dxl->ConvertCurrent2Value(_dxl->dxl_state->goal_current); - UINT8_T _sync_write_data[4]; - _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_curr_data)); - _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_curr_data)); - _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_curr_data)); - _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_curr_data)); - - port_to_sync_write_torque[_dxl->port_name]->AddParam(_dxl->id, _sync_write_data); - - port_to_sync_write_velocity[_dxl->port_name]->RemoveParam(_dxl->id); - port_to_sync_write_position[_dxl->port_name]->RemoveParam(_dxl->id); - } - } - } - - break; - } - } - } - - for(std::list::iterator _m_it = motion_modules_.begin(); _m_it != motion_modules_.end(); _m_it++) - { - // set all modules -> disable - (*_m_it)->enable = false; - - // set all used modules -> enable - for(std::map::iterator _d_it = robot->dxls.begin(); _d_it != robot->dxls.end(); _d_it++) - { - if(_d_it->second->ctrl_module_name == (*_m_it)->module_name) - { - (*_m_it)->enable = true; - break; - } - } - } - - // TODO: set indirect address - // -> check module's control_mode - - queue_mutex_.unlock(); - - // publish current module - robotis_controller_msgs::JointCtrlModule _current_module_msg; - for(std::map::iterator _dxl_iter = robot->dxls.begin(); _dxl_iter != robot->dxls.end(); ++_dxl_iter) - { - _current_module_msg.joint_name.push_back(_dxl_iter->first); - _current_module_msg.module_name.push_back(_dxl_iter->second->ctrl_module_name); - } - - if(_current_module_msg.joint_name.size() == _current_module_msg.module_name.size()) - current_module_pub.publish(_current_module_msg); -} - -void RobotisController::GazeboJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg) -{ - for(unsigned int _i = 0; _i < msg->name.size(); _i++) - { - std::map::iterator _d_it = robot->dxls.find((std::string)msg->name[_i]); - if(_d_it != robot->dxls.end()) - { - _d_it->second->dxl_state->present_position = msg->position[_i]; - _d_it->second->dxl_state->present_velocity = msg->velocity[_i]; - _d_it->second->dxl_state->present_current = msg->effort[_i]; - } - } - - if(init_pose_loaded_ == false) - { - for(std::map::iterator _it = robot->dxls.begin(); _it != robot->dxls.end(); _it++) - _it->second->dxl_state->goal_position = _it->second->dxl_state->present_position; - init_pose_loaded_ = true; - } -} - -bool RobotisController::CheckTimerStop() -{ - if(this->is_timer_running_) - { - if(DEBUG_PRINT == true) - ROS_WARN("Process Timer is running.. STOP the timer first."); - return false; - } - return true; -} - -int RobotisController::Ping(const std::string joint_name, UINT8_T *error) -{ - if(CheckTimerStop() == false) - return COMM_PORT_BUSY; - - Dynamixel *_dxl = robot->dxls[joint_name]; - if(_dxl == NULL) - return COMM_NOT_AVAILABLE; - - PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); - PortHandler *_port_handler = robot->ports[_dxl->port_name]; - - return _pkt_handler->Ping(_port_handler, _dxl->id, error); -} -int RobotisController::Ping(const std::string joint_name, UINT16_T* model_number, UINT8_T *error) -{ - if(CheckTimerStop() == false) - return COMM_PORT_BUSY; - - Dynamixel *_dxl = robot->dxls[joint_name]; - if(_dxl == NULL) - return COMM_NOT_AVAILABLE; - - PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); - PortHandler *_port_handler = robot->ports[_dxl->port_name]; - - return _pkt_handler->Ping(_port_handler, _dxl->id, model_number, error); -} - -int RobotisController::Action(const std::string joint_name) -{ - if(CheckTimerStop() == false) - return COMM_PORT_BUSY; - - Dynamixel *_dxl = robot->dxls[joint_name]; - if(_dxl == NULL) - return COMM_NOT_AVAILABLE; - - PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); - PortHandler *_port_handler = robot->ports[_dxl->port_name]; - - return _pkt_handler->Action(_port_handler, _dxl->id); -} -int RobotisController::Reboot(const std::string joint_name, UINT8_T *error) -{ - if(CheckTimerStop() == false) - return COMM_PORT_BUSY; - - Dynamixel *_dxl = robot->dxls[joint_name]; - if(_dxl == NULL) - return COMM_NOT_AVAILABLE; - - PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); - PortHandler *_port_handler = robot->ports[_dxl->port_name]; - - return _pkt_handler->Reboot(_port_handler, _dxl->id, error); -} -int RobotisController::FactoryReset(const std::string joint_name, UINT8_T option, UINT8_T *error) -{ - if(CheckTimerStop() == false) - return COMM_PORT_BUSY; - - Dynamixel *_dxl = robot->dxls[joint_name]; - if(_dxl == NULL) - return COMM_NOT_AVAILABLE; - - PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); - PortHandler *_port_handler = robot->ports[_dxl->port_name]; - - return _pkt_handler->FactoryReset(_port_handler, _dxl->id, option, error); -} - -int RobotisController::Read(const std::string joint_name, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error) -{ - if(CheckTimerStop() == false) - return COMM_PORT_BUSY; - - Dynamixel *_dxl = robot->dxls[joint_name]; - if(_dxl == NULL) - return COMM_NOT_AVAILABLE; - - PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); - PortHandler *_port_handler = robot->ports[_dxl->port_name]; - - return _pkt_handler->ReadTxRx(_port_handler, _dxl->id, address, length, data, error); -} - -int RobotisController::ReadCtrlItem(const std::string joint_name, const std::string item_name, UINT32_T *data, UINT8_T *error) -{ - if(CheckTimerStop() == false) - return COMM_PORT_BUSY; - - Dynamixel *_dxl = robot->dxls[joint_name]; - if(_dxl == NULL) - return COMM_NOT_AVAILABLE; - - ControlTableItem*_item = _dxl->ctrl_table[item_name]; - if(_item == NULL) - return COMM_NOT_AVAILABLE; - - PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); - PortHandler *_port_handler = robot->ports[_dxl->port_name]; - - int _result = COMM_NOT_AVAILABLE; - switch(_item->data_length) - { - case 1: - { - UINT8_T _data = 0; - _result = _pkt_handler->Read1ByteTxRx(_port_handler, _dxl->id, _item->address, &_data, error); - if(_result == COMM_SUCCESS) - *data = _data; - break; - } - case 2: - { - UINT16_T _data = 0; - _result = _pkt_handler->Read2ByteTxRx(_port_handler, _dxl->id, _item->address, &_data, error); - if(_result == COMM_SUCCESS) - *data = _data; - break; - } - case 4: - { - UINT32_T _data = 0; - _result = _pkt_handler->Read4ByteTxRx(_port_handler, _dxl->id, _item->address, &_data, error); - if(_result == COMM_SUCCESS) - *data = _data; - break; - } - default: - break; - } - return _result; -} - -int RobotisController::Read1Byte(const std::string joint_name, UINT16_T address, UINT8_T *data, UINT8_T *error) -{ - if(CheckTimerStop() == false) - return COMM_PORT_BUSY; - - Dynamixel *_dxl = robot->dxls[joint_name]; - if(_dxl == NULL) - return COMM_NOT_AVAILABLE; - - PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); - PortHandler *_port_handler = robot->ports[_dxl->port_name]; - - return _pkt_handler->Read1ByteTxRx(_port_handler, _dxl->id, address, data, error); -} - -int RobotisController::Read2Byte(const std::string joint_name, UINT16_T address, UINT16_T *data, UINT8_T *error) -{ - if(CheckTimerStop() == false) - return COMM_PORT_BUSY; - - Dynamixel *_dxl = robot->dxls[joint_name]; - if(_dxl == NULL) - return COMM_NOT_AVAILABLE; - - PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); - PortHandler *_port_handler = robot->ports[_dxl->port_name]; - - return _pkt_handler->Read2ByteTxRx(_port_handler, _dxl->id, address, data, error); -} - -int RobotisController::Read4Byte(const std::string joint_name, UINT16_T address, UINT32_T *data, UINT8_T *error) -{ - if(CheckTimerStop() == false) - return COMM_PORT_BUSY; - - Dynamixel *_dxl = robot->dxls[joint_name]; - if(_dxl == NULL) - return COMM_NOT_AVAILABLE; - - PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); - PortHandler *_port_handler = robot->ports[_dxl->port_name]; - - return _pkt_handler->Read4ByteTxRx(_port_handler, _dxl->id, address, data, error); -} - -int RobotisController::Write(const std::string joint_name, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error) -{ - if(CheckTimerStop() == false) - return COMM_PORT_BUSY; - - Dynamixel *_dxl = robot->dxls[joint_name]; - if(_dxl == NULL) - return COMM_NOT_AVAILABLE; - - PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); - PortHandler *_port_handler = robot->ports[_dxl->port_name]; - - return _pkt_handler->WriteTxRx(_port_handler, _dxl->id, address, length, data, error); -} - -int RobotisController::WriteCtrlItem(const std::string joint_name, const std::string item_name, UINT32_T data, UINT8_T *error) -{ - if(CheckTimerStop() == false) - return COMM_PORT_BUSY; - - Dynamixel *_dxl = robot->dxls[joint_name]; - if(_dxl == NULL) - return COMM_NOT_AVAILABLE; - - ControlTableItem*_item = _dxl->ctrl_table[item_name]; - if(_item == NULL) - return COMM_NOT_AVAILABLE; - - PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); - PortHandler *_port_handler = robot->ports[_dxl->port_name]; - - int _result = COMM_NOT_AVAILABLE; - UINT8_T *_data = new UINT8_T[_item->data_length]; - if(_item->data_length == 1) - { - _data[0] = (UINT8_T)data; - _result = _pkt_handler->Write1ByteTxRx(_port_handler, _dxl->id, _item->address, data, error); - } - else if(_item->data_length == 2) - { - _data[0] = DXL_LOBYTE((UINT16_T)data); - _data[1] = DXL_HIBYTE((UINT16_T)data); - _result = _pkt_handler->Write2ByteTxRx(_port_handler, _dxl->id, _item->address, data, error); - } - else if(_item->data_length == 4) - { - _data[0] = DXL_LOBYTE(DXL_LOWORD((UINT32_T)data)); - _data[1] = DXL_HIBYTE(DXL_LOWORD((UINT32_T)data)); - _data[2] = DXL_LOBYTE(DXL_HIWORD((UINT32_T)data)); - _data[3] = DXL_HIBYTE(DXL_HIWORD((UINT32_T)data)); - _result = _pkt_handler->Write4ByteTxRx(_port_handler, _dxl->id, _item->address, data, error); - } - delete[] _data; - return _result; -} - -int RobotisController::Write1Byte(const std::string joint_name, UINT16_T address, UINT8_T data, UINT8_T *error) -{ - if(CheckTimerStop() == false) - return COMM_PORT_BUSY; - - Dynamixel *_dxl = robot->dxls[joint_name]; - if(_dxl == NULL) - return COMM_NOT_AVAILABLE; - - PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); - PortHandler *_port_handler = robot->ports[_dxl->port_name]; - - return _pkt_handler->Write1ByteTxRx(_port_handler, _dxl->id, address, data, error); -} - -int RobotisController::Write2Byte(const std::string joint_name, UINT16_T address, UINT16_T data, UINT8_T *error) -{ - if(CheckTimerStop() == false) - return COMM_PORT_BUSY; - - Dynamixel *_dxl = robot->dxls[joint_name]; - if(_dxl == NULL) - return COMM_NOT_AVAILABLE; - - PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); - PortHandler *_port_handler = robot->ports[_dxl->port_name]; - - return _pkt_handler->Write2ByteTxRx(_port_handler, _dxl->id, address, data, error); -} - -int RobotisController::Write4Byte(const std::string joint_name, UINT16_T address, UINT32_T data, UINT8_T *error) -{ - if(CheckTimerStop() == false) - return COMM_PORT_BUSY; - - Dynamixel *_dxl = robot->dxls[joint_name]; - if(_dxl == NULL) - return COMM_NOT_AVAILABLE; - - PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); - PortHandler *_port_handler = robot->ports[_dxl->port_name]; - - return _pkt_handler->Write4ByteTxRx(_port_handler, _dxl->id, address, data, error); -} - -int RobotisController::RegWrite(const std::string joint_name, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error) -{ - if(CheckTimerStop() == false) - return COMM_PORT_BUSY; - - Dynamixel *_dxl = robot->dxls[joint_name]; - if(_dxl == NULL) - return COMM_NOT_AVAILABLE; - - PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); - PortHandler *_port_handler = robot->ports[_dxl->port_name]; - - return _pkt_handler->RegWriteTxRx(_port_handler, _dxl->id, address, length, data, error); -} - diff --git a/robotis_controller/src/robotis_controller/robotis_controller.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp new file mode 100644 index 0000000..35c81b1 --- /dev/null +++ b/robotis_controller/src/robotis_controller/robotis_controller.cpp @@ -0,0 +1,2144 @@ +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * 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 ROBOTIS 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. + *******************************************************************************/ + +/* + * robotis_controller.cpp + * + * Created on: 2016. 1. 15. + * Author: zerom + */ + +#include +#include + +#include "robotis_controller/robotis_controller.h" + +using namespace robotis_framework; + +RobotisController::RobotisController() + : is_timer_running_(false), + stop_timer_(false), + init_pose_loaded_(false), + timer_thread_(0), + controller_mode_(MotionModuleMode), + DEBUG_PRINT(false), + robot_(0), + gazebo_mode_(false), + gazebo_robot_name_("robotis") +{ + direct_sync_write_.clear(); +} + +void RobotisController::initializeSyncWrite() +{ + if (gazebo_mode_ == true) + return; + + ROS_INFO("FIRST BULKREAD"); + for (auto& it : port_to_bulk_read_) + it.second->txRxPacket(); + for(auto& it : port_to_bulk_read_) + { + int error_count = 0; + int result = COMM_SUCCESS; + do + { + if (++error_count > 10) + { + ROS_ERROR("[RobotisController] bulk read fail!!"); + exit(-1); + } + usleep(10 * 1000); + result = it.second->txRxPacket(); + } while (result != COMM_SUCCESS); + } + init_pose_loaded_ = true; + ROS_INFO("FIRST BULKREAD END"); + + // clear syncwrite param setting + for (auto& it : port_to_sync_write_position_) + { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto& it : port_to_sync_write_position_p_gain_) + { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto& it : port_to_sync_write_position_i_gain_) + { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto& it : port_to_sync_write_position_d_gain_) + { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto& it : port_to_sync_write_velocity_) + { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto& it : port_to_sync_write_velocity_p_gain_) + { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto& it : port_to_sync_write_velocity_i_gain_) + { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto& it : port_to_sync_write_velocity_d_gain_) + { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto& it : port_to_sync_write_current_) + { + if (it.second != NULL) + it.second->clearParam(); + } + + // set init syncwrite param(from data of bulkread) + for (auto& it : robot_->dxls_) + { + std::string joint_name = it.first; + Dynamixel *dxl = it.second; + + for (int i = 0; i < dxl->bulk_read_items_.size(); i++) + { + uint32_t read_data = 0; + uint8_t sync_write_data[4]; + + if (port_to_bulk_read_[dxl->port_name_]->isAvailable(dxl->id_, + dxl->bulk_read_items_[i]->address_, + dxl->bulk_read_items_[i]->data_length_) == true) + { + read_data = port_to_bulk_read_[dxl->port_name_]->getData(dxl->id_, + dxl->bulk_read_items_[i]->address_, + dxl->bulk_read_items_[i]->data_length_); + + sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(read_data)); + sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(read_data)); + sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(read_data)); + sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(read_data)); + + if ((dxl->present_position_item_ != 0) && + (dxl->bulk_read_items_[i]->item_name_ == dxl->present_position_item_->item_name_)) + { + dxl->dxl_state_->present_position_ = dxl->convertValue2Radian(read_data) - dxl->dxl_state_->position_offset_; // remove offset + dxl->dxl_state_->goal_position_ = dxl->dxl_state_->present_position_; + + port_to_sync_write_position_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); + } + else if ((dxl->position_p_gain_item_ != 0) && + (dxl->bulk_read_items_[i]->item_name_ == dxl->position_p_gain_item_->item_name_)) + { + dxl->dxl_state_->position_p_gain_ = read_data; + } + else if ((dxl->position_i_gain_item_ != 0) && + (dxl->bulk_read_items_[i]->item_name_ == dxl->position_i_gain_item_->item_name_)) + { + dxl->dxl_state_->position_i_gain_ = read_data; + } + else if ((dxl->position_d_gain_item_ != 0) && + (dxl->bulk_read_items_[i]->item_name_ == dxl->position_d_gain_item_->item_name_)) + { + dxl->dxl_state_->position_d_gain_ = read_data; + } + else if ((dxl->present_velocity_item_ != 0) && + (dxl->bulk_read_items_[i]->item_name_ == dxl->present_velocity_item_->item_name_)) + { + dxl->dxl_state_->present_velocity_ = dxl->convertValue2Velocity(read_data); + dxl->dxl_state_->goal_velocity_ = dxl->dxl_state_->present_velocity_; + } + else if ((dxl->velocity_p_gain_item_ != 0) && + (dxl->bulk_read_items_[i]->item_name_ == dxl->velocity_p_gain_item_->item_name_)) + { + dxl->dxl_state_->velocity_p_gain_ = read_data; + } + else if ((dxl->velocity_i_gain_item_ != 0) && + (dxl->bulk_read_items_[i]->item_name_ == dxl->velocity_i_gain_item_->item_name_)) + { + dxl->dxl_state_->velocity_i_gain_ = read_data; + } + else if ((dxl->velocity_d_gain_item_ != 0) && + (dxl->bulk_read_items_[i]->item_name_ == dxl->velocity_d_gain_item_->item_name_)) + { + dxl->dxl_state_->velocity_d_gain_ = read_data; + } + else if ((dxl->present_current_item_ != 0) && + (dxl->bulk_read_items_[i]->item_name_ == dxl->present_current_item_->item_name_)) + { + dxl->dxl_state_->present_torque_ = dxl->convertValue2Torque(read_data); + dxl->dxl_state_->goal_torque_ = dxl->dxl_state_->present_torque_; + } + } + } + } +} + +bool RobotisController::initialize(const std::string robot_file_path, const std::string init_file_path) +{ + std::string dev_desc_dir_path = ros::package::getPath("robotis_device") + "/devices"; + + // load robot info : port , device + robot_ = new Robot(robot_file_path, dev_desc_dir_path); + + if (gazebo_mode_ == true) + { + queue_thread_ = boost::thread(boost::bind(&RobotisController::msgQueueThread, this)); + return true; + } + + for (auto& it : robot_->ports_) + { + std::string port_name = it.first; + dynamixel::PortHandler *port = it.second; + dynamixel::PacketHandler *default_pkt_handler = dynamixel::PacketHandler::getPacketHandler(2.0); + + if (port->setBaudRate(port->getBaudRate()) == false) + { + ROS_ERROR("PORT [%s] SETUP ERROR! (baudrate: %d)", port_name.c_str(), port->getBaudRate()); + exit(-1); + } + + // get the default device info of the port + std::string default_device_name = robot_->port_default_device_[port_name]; + auto dxl_it = robot_->dxls_.find(default_device_name); + auto sensor_it = robot_->sensors_.find(default_device_name); + if (dxl_it != robot_->dxls_.end()) + { + Dynamixel *default_device = dxl_it->second; + default_pkt_handler = dynamixel::PacketHandler::getPacketHandler(default_device->protocol_version_); + + if (default_device->goal_position_item_ != 0) + { + port_to_sync_write_position_[port_name] + = new dynamixel::GroupSyncWrite(port, + default_pkt_handler, + default_device->goal_position_item_->address_, + default_device->goal_position_item_->data_length_); + } + + if (default_device->position_p_gain_item_ != 0) + { + port_to_sync_write_position_p_gain_[port_name] + = new dynamixel::GroupSyncWrite(port, + default_pkt_handler, + default_device->position_p_gain_item_->address_, + default_device->position_p_gain_item_->data_length_); + } + + if (default_device->position_i_gain_item_ != 0) + { + port_to_sync_write_position_i_gain_[port_name] + = new dynamixel::GroupSyncWrite(port, + default_pkt_handler, + default_device->position_i_gain_item_->address_, + default_device->position_i_gain_item_->data_length_); + } + + if (default_device->position_d_gain_item_ != 0) + { + port_to_sync_write_position_d_gain_[port_name] + = new dynamixel::GroupSyncWrite(port, + default_pkt_handler, + default_device->position_d_gain_item_->address_, + default_device->position_d_gain_item_->data_length_); + } + + if (default_device->goal_velocity_item_ != 0) + { + port_to_sync_write_velocity_[port_name] + = new dynamixel::GroupSyncWrite(port, + default_pkt_handler, + default_device->goal_velocity_item_->address_, + default_device->goal_velocity_item_->data_length_); + } + + if (default_device->velocity_p_gain_item_ != 0) + { + port_to_sync_write_velocity_p_gain_[port_name] + = new dynamixel::GroupSyncWrite(port, + default_pkt_handler, + default_device->velocity_p_gain_item_->address_, + default_device->velocity_p_gain_item_->data_length_); + } + + if (default_device->velocity_i_gain_item_ != 0) + { + port_to_sync_write_velocity_i_gain_[port_name] + = new dynamixel::GroupSyncWrite(port, + default_pkt_handler, + default_device->velocity_i_gain_item_->address_, + default_device->velocity_i_gain_item_->data_length_); + } + + if (default_device->velocity_d_gain_item_ != 0) + { + port_to_sync_write_velocity_d_gain_[port_name] + = new dynamixel::GroupSyncWrite(port, + default_pkt_handler, + default_device->velocity_d_gain_item_->address_, + default_device->velocity_d_gain_item_->data_length_); + } + + if (default_device->goal_current_item_ != 0) + { + port_to_sync_write_current_[port_name] + = new dynamixel::GroupSyncWrite(port, + default_pkt_handler, + default_device->goal_current_item_->address_, + default_device->goal_current_item_->data_length_); + } + } + else if (sensor_it != robot_->sensors_.end()) + { + Sensor *_default_device = sensor_it->second; + default_pkt_handler = dynamixel::PacketHandler::getPacketHandler(_default_device->protocol_version_); + } + + port_to_bulk_read_[port_name] = new dynamixel::GroupBulkRead(port, default_pkt_handler); + } + + // (for loop) check all dxls are connected. + for (auto& it : robot_->dxls_) + { + std::string joint_name = it.first; + Dynamixel *dxl = it.second; + + if (ping(joint_name) != 0) + { + usleep(10 * 1000); + if (ping(joint_name) != 0) + ROS_ERROR("JOINT[%s] does NOT respond!!", joint_name.c_str()); + } + } + + initializeDevice(init_file_path); + + // [ BulkRead ] StartAddress : Present Position , Length : 10 ( Position/Velocity/Current ) + for (auto& it : robot_->dxls_) + { + std::string joint_name = it.first; + Dynamixel *dxl = it.second; + + if (dxl == NULL) + continue; + + int bulkread_start_addr = 0; + int bulkread_data_length = 0; + +// // bulk read default : present position +// if(dxl->present_position_item != 0) +// { +// bulkread_start_addr = dxl->present_position_item->address; +// bulkread_data_length = dxl->present_position_item->data_length; +// } + + // calculate bulk read start address & data length + auto indirect_addr_it = dxl->ctrl_table_.find(INDIRECT_ADDRESS_1); + if (indirect_addr_it != dxl->ctrl_table_.end()) // INDIRECT_ADDRESS_1 exist + { + if (dxl->bulk_read_items_.size() != 0) + { + bulkread_start_addr = dxl->bulk_read_items_[0]->address_; + bulkread_data_length = 0; + + // set indirect address + int indirect_addr = indirect_addr_it->second->address_; + for (int i = 0; i < dxl->bulk_read_items_.size(); i++) + { + int addr_leng = dxl->bulk_read_items_[i]->data_length_; + + bulkread_data_length += addr_leng; + for (int l = 0; l < addr_leng; l++) + { +// ROS_WARN("[%12s] INDIR_ADDR: %d, ITEM_ADDR: %d", joint_name.c_str(), indirect_addr, dxl->ctrl_table[dxl->bulk_read_items[i]->item_name]->address + _l); + write2Byte(joint_name, indirect_addr, dxl->ctrl_table_[dxl->bulk_read_items_[i]->item_name_]->address_ + l); + indirect_addr += 2; + } + } + } + } + else // INDIRECT_ADDRESS_1 NOT exist + { + if (dxl->bulk_read_items_.size() != 0) + { + bulkread_start_addr = dxl->bulk_read_items_[0]->address_; + bulkread_data_length = 0; + + ControlTableItem *last_item = dxl->bulk_read_items_[0]; + + for (int i = 0; i < dxl->bulk_read_items_.size(); i++) + { + int addr = dxl->bulk_read_items_[i]->address_; + if (addr < bulkread_start_addr) + bulkread_start_addr = addr; + else if (last_item->address_ < addr) + last_item = dxl->bulk_read_items_[i]; + } + + bulkread_data_length = last_item->address_ - bulkread_start_addr + last_item->data_length_; + } + } + +// ROS_WARN("[%12s] start_addr: %d, data_length: %d", joint_name.c_str(), bulkread_start_addr, bulkread_data_length); + if (bulkread_start_addr != 0) + port_to_bulk_read_[dxl->port_name_]->addParam(dxl->id_, bulkread_start_addr, bulkread_data_length); + + // Torque ON + if (writeCtrlItem(joint_name, dxl->torque_enable_item_->item_name_, 1) != COMM_SUCCESS) + writeCtrlItem(joint_name, dxl->torque_enable_item_->item_name_, 1); + } + + for (auto& it : robot_->sensors_) + { + std::string sensor_name = it.first; + Sensor *sensor = it.second; + + if (sensor == NULL) + continue; + + int bulkread_start_addr = 0; + int bulkread_data_length = 0; + + // calculate bulk read start address & data length + auto indirect_addr_it = sensor->ctrl_table_.find(INDIRECT_ADDRESS_1); + if (indirect_addr_it != sensor->ctrl_table_.end()) // INDIRECT_ADDRESS_1 exist + { + if (sensor->bulk_read_items_.size() != 0) + { + bulkread_start_addr = sensor->bulk_read_items_[0]->address_; + bulkread_data_length = 0; + + // set indirect address + int indirect_addr = indirect_addr_it->second->address_; + for (int i = 0; i < sensor->bulk_read_items_.size(); i++) + { + int addr_leng = sensor->bulk_read_items_[i]->data_length_; + + bulkread_data_length += addr_leng; + for (int l = 0; l < addr_leng; l++) + { +// ROS_WARN("[%12s] INDIR_ADDR: %d, ITEM_ADDR: %d", sensor_name.c_str(), indirect_addr, sensor->ctrl_table[sensor->bulk_read_items[i]->item_name]->address + _l); + write2Byte(sensor_name, + indirect_addr, + sensor->ctrl_table_[sensor->bulk_read_items_[i]->item_name_]->address_ + l); + indirect_addr += 2; + } + } + } + } + else // INDIRECT_ADDRESS_1 NOT exist + { + if (sensor->bulk_read_items_.size() != 0) + { + bulkread_start_addr = sensor->bulk_read_items_[0]->address_; + bulkread_data_length = 0; + + ControlTableItem *last_item = sensor->bulk_read_items_[0]; + + for (int i = 0; i < sensor->bulk_read_items_.size(); i++) + { + int addr = sensor->bulk_read_items_[i]->address_; + if (addr < bulkread_start_addr) + bulkread_start_addr = addr; + else if (last_item->address_ < addr) + last_item = sensor->bulk_read_items_[i]; + } + + bulkread_data_length = last_item->address_ - bulkread_start_addr + last_item->data_length_; + } + } + + //ROS_WARN("[%12s] start_addr: %d, data_length: %d", sensor_name.c_str(), bulkread_start_addr, bulkread_data_length); + if (bulkread_start_addr != 0) + port_to_bulk_read_[sensor->port_name_]->addParam(sensor->id_, bulkread_start_addr, bulkread_data_length); + } + + queue_thread_ = boost::thread(boost::bind(&RobotisController::msgQueueThread, this)); + return true; +} + +void RobotisController::initializeDevice(const std::string init_file_path) +{ + // device initialize + if (DEBUG_PRINT) + ROS_WARN("INIT FILE LOAD"); + + YAML::Node doc; + try + { + doc = YAML::LoadFile(init_file_path.c_str()); + + for (YAML::const_iterator it_doc = doc.begin(); it_doc != doc.end(); it_doc++) + { + std::string joint_name = it_doc->first.as(); + + YAML::Node joint_node = doc[joint_name]; + if (joint_node.size() == 0) + continue; + + Dynamixel *dxl = NULL; + auto dxl_it = robot_->dxls_.find(joint_name); + if (dxl_it != robot_->dxls_.end()) + dxl = dxl_it->second; + + if (dxl == NULL) + { + ROS_WARN("Joint [%s] was not found.", joint_name.c_str()); + continue; + } + if (DEBUG_PRINT) + ROS_INFO("JOINT_NAME: %s", joint_name.c_str()); + + for (YAML::const_iterator it_joint = joint_node.begin(); it_joint != joint_node.end(); it_joint++) + { + std::string item_name = it_joint->first.as(); + + if (DEBUG_PRINT) + ROS_INFO(" ITEM_NAME: %s", item_name.c_str()); + + uint32_t value = it_joint->second.as(); + + ControlTableItem *item = dxl->ctrl_table_[item_name]; + if (item == NULL) + { + ROS_WARN("Control Item [%s] was not found.", item_name.c_str()); + continue; + } + + if (item->memory_type_ == EEPROM) + { + uint8_t data8 = 0; + uint16_t data16 = 0; + uint32_t data32 = 0; + + switch (item->data_length_) + { + case 1: + read1Byte(joint_name, item->address_, &data8); + if (data8 == value) + continue; + break; + case 2: + read2Byte(joint_name, item->address_, &data16); + if (data16 == value) + continue; + break; + case 4: + read4Byte(joint_name, item->address_, &data32); + if (data32 == value) + continue; + break; + default: + break; + } + } + + switch (item->data_length_) + { + case 1: + write1Byte(joint_name, item->address_, (uint8_t) value); + break; + case 2: + write2Byte(joint_name, item->address_, (uint16_t) value); + break; + case 4: + write4Byte(joint_name, item->address_, value); + break; + default: + break; + } + + if (item->memory_type_ == EEPROM) + { + // Write to EEPROM -> delay is required (max delay: 55 msec per byte) + usleep(item->data_length_ * 55 * 1000); + } + } + } + } catch (const std::exception& e) + { + ROS_INFO("Dynamixel Init file not found."); + } +} + +void RobotisController::gazeboTimerThread() +{ + ros::Rate gazebo_rate(1000 / CONTROL_CYCLE_MSEC); + + while (!stop_timer_) + { + if (init_pose_loaded_ == true) + process(); + gazebo_rate.sleep(); + } +} + +void RobotisController::msgQueueThread() +{ + ros::NodeHandle ros_node; + ros::CallbackQueue callback_queue; + + ros_node.setCallbackQueue(&callback_queue); + + /* subscriber */ + ros::Subscriber sync_write_item_sub = ros_node.subscribe("/robotis/sync_write_item", 10, + &RobotisController::syncWriteItemCallback, this); + ros::Subscriber joint_ctrl_modules_sub = ros_node.subscribe("/robotis/set_joint_ctrl_modules", 10, + &RobotisController::setJointCtrlModuleCallback, this); + ros::Subscriber enable_ctrl_module_sub = ros_node.subscribe("/robotis/enable_ctrl_module", 10, + &RobotisController::setCtrlModuleCallback, this); + ros::Subscriber control_mode_sub = ros_node.subscribe("/robotis/set_control_mode", 10, + &RobotisController::setControllerModeCallback, this); + ros::Subscriber joint_states_sub = ros_node.subscribe("/robotis/set_joint_states", 10, + &RobotisController::setJointStatesCallback, this); + + ros::Subscriber gazebo_joint_states_sub; + if (gazebo_mode_ == true) + gazebo_joint_states_sub = ros_node.subscribe("/" + gazebo_robot_name_ + "/joint_states", 10, + &RobotisController::gazeboJointStatesCallback, this); + + /* publisher */ + goal_joint_state_pub_ = ros_node.advertise("/robotis/goal_joint_states", 10); + present_joint_state_pub_ = ros_node.advertise("/robotis/present_joint_states", 10); + current_module_pub_ = ros_node.advertise( + "/robotis/present_joint_ctrl_modules", 10); + + if (gazebo_mode_ == true) + { + for (auto& it : robot_->dxls_) + { + gazebo_joint_position_pub_[it.first] = ros_node.advertise( + "/" + gazebo_robot_name_ + "/" + it.first + "_position/command", 1); + gazebo_joint_velocity_pub_[it.first] = ros_node.advertise( + "/" + gazebo_robot_name_ + "/" + it.first + "_velocity/command", 1); + gazebo_joint_effort_pub_[it.first] = ros_node.advertise( + "/" + gazebo_robot_name_ + "/" + it.first + "_effort/command", 1); + } + } + + /* service */ + ros::ServiceServer joint_module_server = ros_node.advertiseService("/robotis/get_present_joint_ctrl_modules", + &RobotisController::getCtrlModuleCallback, this); + + while (ros_node.ok()) + { + callback_queue.callAvailable(); + usleep(100); + } +} + +void *RobotisController::timerThread(void *param) +{ + RobotisController *controller = (RobotisController *) param; + static struct timespec next_time; + static struct timespec curr_time; + + ROS_INFO("controller::thread_proc"); + + clock_gettime(CLOCK_MONOTONIC, &next_time); + + while (!controller->stop_timer_) + { + next_time.tv_sec += (next_time.tv_nsec + CONTROL_CYCLE_MSEC * 1000000) / 1000000000; + next_time.tv_nsec = (next_time.tv_nsec + CONTROL_CYCLE_MSEC * 1000000) % 1000000000; + + controller->process(); + + clock_gettime(CLOCK_MONOTONIC, &curr_time); + long delta_nsec = (next_time.tv_sec - curr_time.tv_sec) * 1000000000 + (next_time.tv_nsec - curr_time.tv_nsec); + if (delta_nsec < -100000) + { + if (controller->DEBUG_PRINT == true) + { + fprintf(stderr, "[RobotisController::ThreadProc] NEXT TIME < CURR TIME.. (%f)[%ld.%09ld / %ld.%09ld]", + delta_nsec / 1000000.0, (long )next_time.tv_sec, (long )next_time.tv_nsec, + (long )curr_time.tv_sec, (long )curr_time.tv_nsec); + } + + // next_time = curr_time + 3 msec + next_time.tv_sec = curr_time.tv_sec + (curr_time.tv_nsec + 3000000) / 1000000000; + next_time.tv_nsec = (curr_time.tv_nsec + 3000000) % 1000000000; + } + + clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &next_time, NULL); + } + return 0; +} + +void RobotisController::startTimer() +{ + if (this->is_timer_running_ == true) + return; + + if (this->gazebo_mode_ == true) + { + // create and start the thread + gazebo_thread_ = boost::thread(boost::bind(&RobotisController::gazeboTimerThread, this)); + } + else + { + initializeSyncWrite(); + + for (auto& it : port_to_bulk_read_) + { + it.second->txPacket(); + } + + usleep(8 * 1000); + + int error; + struct sched_param param; + pthread_attr_t attr; + + pthread_attr_init(&attr); + + error = pthread_attr_setschedpolicy(&attr, SCHED_RR); + if (error != 0) + ROS_ERROR("pthread_attr_setschedpolicy error = %d\n", error); + error = pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED); + if (error != 0) + ROS_ERROR("pthread_attr_setinheritsched error = %d\n", error); + + memset(¶m, 0, sizeof(param)); + param.sched_priority = 31; // RT + error = pthread_attr_setschedparam(&attr, ¶m); + if (error != 0) + ROS_ERROR("pthread_attr_setschedparam error = %d\n", error); + + // create and start the thread + if ((error = pthread_create(&this->timer_thread_, &attr, this->timerThread, this)) != 0) + { + ROS_ERROR("timer thread create fail!!"); + exit(-1); + } + } + + this->is_timer_running_ = true; +} + +void RobotisController::stopTimer() +{ + int error = 0; + + // set the flag to stop the thread + if (this->is_timer_running_) + { + this->stop_timer_ = true; + + if (this->gazebo_mode_ == false) + { + // wait until the thread is stopped. + if ((error = pthread_join(this->timer_thread_, NULL)) != 0) + exit(-1); + + for (auto& it : port_to_bulk_read_) + { + if (it.second != NULL) + it.second->rxPacket(); + } + + for (auto& it : port_to_sync_write_position_) + { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto& it : port_to_sync_write_position_p_gain_) + { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto& it : port_to_sync_write_position_i_gain_) + { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto& it : port_to_sync_write_position_d_gain_) + { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto& it : port_to_sync_write_velocity_) + { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto& it : port_to_sync_write_velocity_p_gain_) + { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto& it : port_to_sync_write_velocity_i_gain_) + { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto& it : port_to_sync_write_velocity_d_gain_) + { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto& it : port_to_sync_write_current_) + { + if (it.second != NULL) + it.second->clearParam(); + } + } + else + { + // wait until the thread is stopped. + gazebo_thread_.join(); + } + + this->stop_timer_ = false; + this->is_timer_running_ = false; + } +} + +bool RobotisController::isTimerRunning() +{ + return this->is_timer_running_; +} + +void RobotisController::loadOffset(const std::string path) +{ + YAML::Node doc; + try + { + doc = YAML::LoadFile(path.c_str()); + } catch (const std::exception& e) + { + ROS_ERROR("Fail to load offset yaml."); + return; + } + + YAML::Node offset_node = doc["offset"]; + if (offset_node.size() == 0) + return; + + ROS_INFO("Load offsets..."); + for (YAML::const_iterator it = offset_node.begin(); it != offset_node.end(); it++) + { + std::string joint_name = it->first.as(); + double offset = it->second.as(); + + auto dxl_it = robot_->dxls_.find(joint_name); + if (dxl_it != robot_->dxls_.end()) + dxl_it->second->dxl_state_->position_offset_ = offset; + } +} + +void RobotisController::process() +{ + // avoid duplicated function call + static bool is_process_running = false; + if (is_process_running == true) + return; + is_process_running = true; + + // ROS_INFO("Controller::Process()"); + bool do_sync_write = false; + + ros::Time start_time; + ros::Duration time_duration; + + if (DEBUG_PRINT) + start_time = ros::Time::now(); + + sensor_msgs::JointState goal_state; + sensor_msgs::JointState present_state; + + present_state.header.stamp = ros::Time::now(); + goal_state.header.stamp = present_state.header.stamp; + + // BulkRead Rx + // -> save to Robot->dxls[]->dynamixel_state.present_position + if (gazebo_mode_ == false) + { + // BulkRead Rx + for (auto& it : port_to_bulk_read_) + { + robot_->ports_[it.first]->setPacketTimeout(0.0); + it.second->rxPacket(); + } + + if (robot_->dxls_.size() > 0) + { + for (auto& dxl_it : robot_->dxls_) + { + Dynamixel *dxl = dxl_it.second; + std::string port_name = dxl_it.second->port_name_; + std::string joint_name = dxl_it.first; + + if (dxl->bulk_read_items_.size() > 0) + { + uint32_t data = 0; + for (int i = 0; i < dxl->bulk_read_items_.size(); i++) + { + ControlTableItem *item = dxl->bulk_read_items_[i]; + if (port_to_bulk_read_[port_name]->isAvailable(dxl->id_, item->address_, item->data_length_) == true) + { + data = port_to_bulk_read_[port_name]->getData(dxl->id_, item->address_, item->data_length_); + + // change dxl_state + if (dxl->present_position_item_ != 0 && item->item_name_ == dxl->present_position_item_->item_name_) + dxl->dxl_state_->present_position_ = dxl->convertValue2Radian(data) - dxl->dxl_state_->position_offset_; // remove offset + else if (dxl->present_velocity_item_ != 0 && item->item_name_ == dxl->present_velocity_item_->item_name_) + dxl->dxl_state_->present_velocity_ = dxl->convertValue2Velocity(data); + else if (dxl->present_current_item_ != 0 && item->item_name_ == dxl->present_current_item_->item_name_) + dxl->dxl_state_->present_torque_ = dxl->convertValue2Torque(data); + else if (dxl->goal_position_item_ != 0 && item->item_name_ == dxl->goal_position_item_->item_name_) + dxl->dxl_state_->goal_position_ = dxl->convertValue2Radian(data) - dxl->dxl_state_->position_offset_; // remove offset + else if (dxl->goal_velocity_item_ != 0 && item->item_name_ == dxl->goal_velocity_item_->item_name_) + dxl->dxl_state_->goal_velocity_ = dxl->convertValue2Velocity(data); + else if (dxl->goal_current_item_ != 0 && item->item_name_ == dxl->goal_current_item_->item_name_) + dxl->dxl_state_->goal_torque_ = dxl->convertValue2Torque(data); + else + dxl->dxl_state_->bulk_read_table_[item->item_name_] = data; + } + } + + // -> update time stamp to Robot->dxls[]->dynamixel_state.update_time_stamp + dxl->dxl_state_->update_time_stamp_ = TimeStamp(present_state.header.stamp.sec, present_state.header.stamp.nsec); + } + } + } + + if (robot_->sensors_.size() > 0) + { + for (auto& sensor_it : robot_->sensors_) + { + Sensor *sensor = sensor_it.second; + std::string port_name = sensor_it.second->port_name_; + std::string sensor_name = sensor_it.first; + + if (sensor->bulk_read_items_.size() > 0) + { + uint32_t data = 0; + for (int i = 0; i < sensor->bulk_read_items_.size(); i++) + { + ControlTableItem *item = sensor->bulk_read_items_[i]; + if (port_to_bulk_read_[port_name]->isAvailable(sensor->id_, item->address_, item->data_length_) == true) + { + data = port_to_bulk_read_[port_name]->getData(sensor->id_, item->address_, item->data_length_); + + // change sensor_state + sensor->sensor_state_->bulk_read_table_[item->item_name_] = data; + } + } + + // -> update time stamp to Robot->dxls[]->dynamixel_state.update_time_stamp + sensor->sensor_state_->update_time_stamp_ = TimeStamp(present_state.header.stamp.sec, present_state.header.stamp.nsec); + } + } + } + } + + if (DEBUG_PRINT) + { + time_duration = ros::Time::now() - start_time; + fprintf(stderr, "(%2.6f) BulkRead Rx & update state \n", time_duration.nsec * 0.000001); + } + + // Call SensorModule Process() + // -> for loop : call SensorModule list -> Process() + if (sensor_modules_.size() > 0) + { + for (auto module_it = sensor_modules_.begin(); module_it != sensor_modules_.end(); module_it++) + { + (*module_it)->process(robot_->dxls_, robot_->sensors_); + + for (auto& it : (*module_it)->result_) + sensor_result_[it.first] = it.second; + } + } + + if (DEBUG_PRINT) + { + time_duration = ros::Time::now() - start_time; + fprintf(stderr, "(%2.6f) SensorModule Process() & save result \n", time_duration.nsec * 0.000001); + } + + if (controller_mode_ == MotionModuleMode) + { + // Call MotionModule Process() + // -> for loop : call MotionModule list -> Process() + if (motion_modules_.size() > 0) + { + queue_mutex_.lock(); + + for (auto module_it = motion_modules_.begin(); module_it != motion_modules_.end(); module_it++) + { + if ((*module_it)->getModuleEnable() == false) + continue; + + (*module_it)->process(robot_->dxls_, sensor_result_); + + // for loop : joint list + for (auto& dxl_it : robot_->dxls_) + { + std::string joint_name = dxl_it.first; + Dynamixel *dxl = dxl_it.second; + DynamixelState *dxl_state = dxl_it.second->dxl_state_; + + if (dxl->ctrl_module_name_ == (*module_it)->getModuleName()) + { + do_sync_write = true; + DynamixelState *result_state = (*module_it)->result_[joint_name]; + + if (result_state == NULL) + { + fprintf(stderr, "[%s] %s", (*module_it)->getModuleName().c_str(), joint_name.c_str()); + continue; + } + + // TODO: check update time stamp ? + + if ((*module_it)->getControlMode() == PositionControl) + { +// if(result_state->goal_position == 0 && dxl->id == 3) +// ROS_INFO("[MODULE:%s][ID:%2d] goal position = %f", (*module_it)->GetModuleName().c_str(), dxl->id, dxl_state->goal_position); + dxl_state->goal_position_ = result_state->goal_position_; + + if (gazebo_mode_ == false) + { + // add offset + uint32_t pos_data = dxl->convertRadian2Value(dxl_state->goal_position_ + dxl_state->position_offset_); + uint8_t sync_write_data[4] = { 0 }; + sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(pos_data)); + sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(pos_data)); + sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(pos_data)); + sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(pos_data)); + + if (abs(pos_data) > 151800) + { + printf("goal_pos : %f | position_offset : %f | pos_data : %d\n", + dxl_state->goal_position_, dxl_state->position_offset_, pos_data); + } + + if (port_to_sync_write_position_[dxl->port_name_] != NULL) + port_to_sync_write_position_[dxl->port_name_]->changeParam(dxl->id_, sync_write_data); + + // if position p gain value is changed -> sync write + if (dxl_state->position_p_gain_ != result_state->position_p_gain_) + { + dxl_state->position_p_gain_ = result_state->position_p_gain_; + uint8_t sync_write_data[4] = { 0 }; + sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(dxl_state->position_p_gain_)); + sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(dxl_state->position_p_gain_)); + sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(dxl_state->position_p_gain_)); + sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(dxl_state->position_p_gain_)); + + if (port_to_sync_write_position_p_gain_[dxl->port_name_] != NULL) + port_to_sync_write_position_p_gain_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); + } + + // if position i gain value is changed -> sync write + if (dxl_state->position_i_gain_ != result_state->position_i_gain_) + { + dxl_state->position_i_gain_ = result_state->position_i_gain_; + uint8_t sync_write_data[4] = { 0 }; + sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(dxl_state->position_i_gain_)); + sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(dxl_state->position_i_gain_)); + sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(dxl_state->position_i_gain_)); + sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(dxl_state->position_i_gain_)); + + if (port_to_sync_write_position_i_gain_[dxl->port_name_] != NULL) + port_to_sync_write_position_i_gain_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); + } + + // if position d gain value is changed -> sync write + if (dxl_state->position_d_gain_ != result_state->position_d_gain_) + { + dxl_state->position_d_gain_ = result_state->position_d_gain_; + uint8_t sync_write_data[4] = { 0 }; + sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(dxl_state->position_d_gain_)); + sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(dxl_state->position_d_gain_)); + sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(dxl_state->position_d_gain_)); + sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(dxl_state->position_d_gain_)); + + if (port_to_sync_write_position_d_gain_[dxl->port_name_] != NULL) + port_to_sync_write_position_d_gain_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); + } + } + } + else if ((*module_it)->getControlMode() == VelocityControl) + { + dxl_state->goal_velocity_ = result_state->goal_velocity_; + + if (gazebo_mode_ == false) + { + uint32_t vel_data = dxl->convertVelocity2Value(dxl_state->goal_velocity_); + uint8_t sync_write_data[4] = { 0 }; + sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(vel_data)); + sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(vel_data)); + sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(vel_data)); + sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(vel_data)); + + if (port_to_sync_write_velocity_[dxl->port_name_] != NULL) + port_to_sync_write_velocity_[dxl->port_name_]->changeParam(dxl->id_, sync_write_data); + + // if velocity p gain gain value is changed -> sync write + if (dxl_state->velocity_p_gain_ != result_state->velocity_p_gain_) + { + dxl_state->velocity_p_gain_ = result_state->velocity_p_gain_; + uint8_t sync_write_data[4] = { 0 }; + sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(dxl_state->velocity_p_gain_)); + sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(dxl_state->velocity_p_gain_)); + sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(dxl_state->velocity_p_gain_)); + sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(dxl_state->velocity_p_gain_)); + + if (port_to_sync_write_velocity_p_gain_[dxl->port_name_] != NULL) + port_to_sync_write_velocity_p_gain_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); + } + + // if velocity i gain value is changed -> sync write + if (dxl_state->velocity_i_gain_ != result_state->velocity_i_gain_) + { + dxl_state->velocity_i_gain_ = result_state->velocity_i_gain_; + uint8_t sync_write_data[4] = { 0 }; + sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(dxl_state->velocity_i_gain_)); + sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(dxl_state->velocity_i_gain_)); + sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(dxl_state->velocity_i_gain_)); + sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(dxl_state->velocity_i_gain_)); + + if (port_to_sync_write_velocity_i_gain_[dxl->port_name_] != NULL) + port_to_sync_write_velocity_i_gain_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); + } + + // if velocity d gain value is changed -> sync write + if (dxl_state->velocity_d_gain_ != result_state->velocity_d_gain_) + { + dxl_state->velocity_d_gain_ = result_state->velocity_d_gain_; + uint8_t sync_write_data[4] = { 0 }; + sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(dxl_state->velocity_d_gain_)); + sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(dxl_state->velocity_d_gain_)); + sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(dxl_state->velocity_d_gain_)); + sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(dxl_state->velocity_d_gain_)); + + if (port_to_sync_write_velocity_d_gain_[dxl->port_name_] != NULL) + port_to_sync_write_velocity_d_gain_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); + } + } + } + else if ((*module_it)->getControlMode() == TorqueControl) + { + dxl_state->goal_torque_ = result_state->goal_torque_; + + if (gazebo_mode_ == false) + { + uint32_t curr_data = dxl->convertTorque2Value(dxl_state->goal_torque_); + uint8_t sync_write_data[2] = { 0 }; + sync_write_data[0] = DXL_LOBYTE(curr_data); + sync_write_data[1] = DXL_HIBYTE(curr_data); + + if (port_to_sync_write_current_[dxl->port_name_] != NULL) + port_to_sync_write_current_[dxl->port_name_]->changeParam(dxl->id_, sync_write_data); + } + } + } + } + } + + queue_mutex_.unlock(); + } + + if (DEBUG_PRINT) + { + time_duration = ros::Time::now() - start_time; + fprintf(stderr, "(%2.6f) MotionModule Process() & save result \n", time_duration.nsec * 0.000001); + } + + // SyncWrite + if (gazebo_mode_ == false && do_sync_write) + { + if (port_to_sync_write_position_p_gain_.size() > 0) + { + for (auto& it : port_to_sync_write_position_p_gain_) + { + it.second->txPacket(); + it.second->clearParam(); + } + } + if (port_to_sync_write_position_i_gain_.size() > 0) + { + for (auto& it : port_to_sync_write_position_i_gain_) + { + it.second->txPacket(); + it.second->clearParam(); + } + } + if (port_to_sync_write_position_d_gain_.size() > 0) + { + for (auto& it : port_to_sync_write_position_d_gain_) + { + it.second->txPacket(); + it.second->clearParam(); + } + } + if (port_to_sync_write_velocity_p_gain_.size() > 0) + { + for (auto& it : port_to_sync_write_velocity_p_gain_) + { + it.second->txPacket(); + it.second->clearParam(); + } + } + if (port_to_sync_write_velocity_i_gain_.size() > 0) + { + for (auto& it : port_to_sync_write_velocity_i_gain_) + { + it.second->txPacket(); + it.second->clearParam(); + } + } + if (port_to_sync_write_velocity_d_gain_.size() > 0) + { + for (auto& it : port_to_sync_write_velocity_d_gain_) + { + it.second->txPacket(); + it.second->clearParam(); + } + } + for (auto& it : port_to_sync_write_position_) + { + if (it.second != NULL) + it.second->txPacket(); + } + for (auto& it : port_to_sync_write_velocity_) + { + if (it.second != NULL) + it.second->txPacket(); + } + for (auto& it : port_to_sync_write_current_) + { + if (it.second != NULL) + it.second->txPacket(); + } + } + else if (gazebo_mode_ == true) + { + for (auto module_it = motion_modules_.begin(); module_it != motion_modules_.end(); module_it++) + { + if ((*module_it)->getModuleEnable() == false) + continue; + + std_msgs::Float64 joint_msg; + + for (auto& dxl_it : robot_->dxls_) + { + std::string joint_name = dxl_it.first; + Dynamixel *dxl = dxl_it.second; + DynamixelState *dxl_state = dxl_it.second->dxl_state_; + + if (dxl->ctrl_module_name_ == (*module_it)->getModuleName()) + { + if ((*module_it)->getControlMode() == PositionControl) + { + joint_msg.data = dxl_state->goal_position_; + gazebo_joint_position_pub_[joint_name].publish(joint_msg); + } + else if ((*module_it)->getControlMode() == VelocityControl) + { + joint_msg.data = dxl_state->goal_velocity_; + gazebo_joint_velocity_pub_[joint_name].publish(joint_msg); + } + else if ((*module_it)->getControlMode() == TorqueControl) + { + joint_msg.data = dxl_state->goal_torque_; + gazebo_joint_effort_pub_[joint_name].publish(joint_msg); + } + } + } + } + } + } + else if (controller_mode_ == DirectControlMode) + { + queue_mutex_.lock(); + + for (auto& it : port_to_sync_write_position_) + { + it.second->txPacket(); + it.second->clearParam(); + } + + if (direct_sync_write_.size() > 0) + { + for (int i = 0; i < direct_sync_write_.size(); i++) + { + direct_sync_write_[i]->txPacket(); + direct_sync_write_[i]->clearParam(); + } + direct_sync_write_.clear(); + } + + queue_mutex_.unlock(); + } + + // TODO: User Read/Write + + // BulkRead Tx + if (gazebo_mode_ == false) + { + for (auto& it : port_to_bulk_read_) + it.second->txPacket(); + } + + if (DEBUG_PRINT) + { + time_duration = ros::Time::now() - start_time; + fprintf(stderr, "(%2.6f) SyncWrite & BulkRead Tx \n", time_duration.nsec * 0.000001); + } + + // publish present & goal position + for (auto& dxl_it : robot_->dxls_) + { + std::string joint_name = dxl_it.first; + Dynamixel *dxl = dxl_it.second; + + present_state.name.push_back(joint_name); + present_state.position.push_back(dxl->dxl_state_->present_position_); + present_state.velocity.push_back(dxl->dxl_state_->present_velocity_); + present_state.effort.push_back(dxl->dxl_state_->present_torque_); + + goal_state.name.push_back(joint_name); + goal_state.position.push_back(dxl->dxl_state_->goal_position_); + goal_state.velocity.push_back(dxl->dxl_state_->goal_velocity_); + goal_state.effort.push_back(dxl->dxl_state_->goal_torque_); + } + + // -> publish present joint_states & goal joint states topic + present_joint_state_pub_.publish(present_state); + goal_joint_state_pub_.publish(goal_state); + + if (DEBUG_PRINT) + { + time_duration = ros::Time::now() - start_time; + fprintf(stderr, "(%2.6f) Process() DONE \n", time_duration.nsec * 0.000001); + } + + is_process_running = false; +} + +void RobotisController::addMotionModule(MotionModule *module) +{ + // check whether the module name already exists + for (auto m_it = motion_modules_.begin(); m_it != motion_modules_.end(); m_it++) + { + if ((*m_it)->getModuleName() == module->getModuleName()) + { + ROS_ERROR("Motion Module Name [%s] already exist !!", module->getModuleName().c_str()); + return; + } + } + + module->initialize(CONTROL_CYCLE_MSEC, robot_); + motion_modules_.push_back(module); + motion_modules_.unique(); +} + +void RobotisController::removeMotionModule(MotionModule *module) +{ + motion_modules_.remove(module); +} + +void RobotisController::addSensorModule(SensorModule *module) +{ + // check whether the module name already exists + for (auto m_it = sensor_modules_.begin(); m_it != sensor_modules_.end(); m_it++) + { + if ((*m_it)->getModuleName() == module->getModuleName()) + { + ROS_ERROR("Sensor Module Name [%s] already exist !!", module->getModuleName().c_str()); + return; + } + } + + module->initialize(CONTROL_CYCLE_MSEC, robot_); + sensor_modules_.push_back(module); + sensor_modules_.unique(); +} + +void RobotisController::removeSensorModule(SensorModule *module) +{ + sensor_modules_.remove(module); +} + +void RobotisController::syncWriteItemCallback(const robotis_controller_msgs::SyncWriteItem::ConstPtr &msg) +{ + for (int i = 0; i < msg->joint_name.size(); i++) + { + Device *device; + + auto d_it1 = robot_->dxls_.find(msg->joint_name[i]); + if (d_it1 != robot_->dxls_.end()) + { + device = d_it1->second; + } + else + { + auto d_it2 = robot_->sensors_.find(msg->joint_name[i]); + if (d_it2 != robot_->sensors_.end()) + { + device = d_it2->second; + } + else + { + // could not find the device + continue; + } + } + + ControlTableItem *item = device->ctrl_table_[msg->item_name]; + + dynamixel::PortHandler *port = robot_->ports_[device->port_name_]; + dynamixel::PacketHandler *packet_handler = dynamixel::PacketHandler::getPacketHandler(device->protocol_version_); + + if (item->access_type_ == Read) + continue; + + int idx = 0; + if (direct_sync_write_.size() == 0) + { + direct_sync_write_.push_back(new dynamixel::GroupSyncWrite(port, packet_handler, item->address_, item->data_length_)); + idx = 0; + } + else + { + for (idx = 0; idx < direct_sync_write_.size(); idx++) + { + if (direct_sync_write_[idx]->getPortHandler() == port && direct_sync_write_[idx]->getPacketHandler() == packet_handler) + break; + } + + if (idx == direct_sync_write_.size()) + direct_sync_write_.push_back(new dynamixel::GroupSyncWrite(port, packet_handler, item->address_, item->data_length_)); + } + + uint8_t *data = new uint8_t[item->data_length_]; + if (item->data_length_ == 1) + data[0] = (uint8_t) msg->value[i]; + else if (item->data_length_ == 2) + { + data[0] = DXL_LOBYTE((uint16_t )msg->value[i]); + data[1] = DXL_HIBYTE((uint16_t )msg->value[i]); + } + else if (item->data_length_ == 4) + { + data[0] = DXL_LOBYTE(DXL_LOWORD((uint32_t)msg->value[i])); + data[1] = DXL_HIBYTE(DXL_LOWORD((uint32_t)msg->value[i])); + data[2] = DXL_LOBYTE(DXL_HIWORD((uint32_t)msg->value[i])); + data[3] = DXL_HIBYTE(DXL_HIWORD((uint32_t)msg->value[i])); + } + direct_sync_write_[idx]->addParam(device->id_, data); + delete[] data; + } +} + +void RobotisController::setControllerModeCallback(const std_msgs::String::ConstPtr &msg) +{ + if (msg->data == "DirectControlMode") + controller_mode_ = DirectControlMode; + else if (msg->data == "MotionModuleMode") + controller_mode_ = MotionModuleMode; +} + +void RobotisController::setJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg) +{ + if (controller_mode_ != DirectControlMode) + return; + + queue_mutex_.lock(); + + for (int i = 0; i < msg->name.size(); i++) + { + int32_t pos = 0; + + Dynamixel *dxl = robot_->dxls_[msg->name[i]]; + if (dxl == NULL) + continue; + + dxl->dxl_state_->goal_position_ = msg->position[i]; + pos = dxl->convertRadian2Value((double) msg->position[i]); + + uint8_t sync_write_data[4]; + sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(pos)); + sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(pos)); + sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(pos)); + sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(pos)); + + if (port_to_sync_write_position_[dxl->port_name_] != NULL) + port_to_sync_write_position_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); + } + + queue_mutex_.unlock(); +} + +void RobotisController::setCtrlModuleCallback(const std_msgs::String::ConstPtr &msg) +{ + std::string _module_name_to_set = msg->data; + + set_module_thread_ = boost::thread(boost::bind(&RobotisController::setCtrlModuleThread, this, _module_name_to_set)); +} + +void RobotisController::setJointCtrlModuleCallback(const robotis_controller_msgs::JointCtrlModule::ConstPtr &msg) +{ + if (msg->joint_name.size() != msg->module_name.size()) + return; + + queue_mutex_.lock(); + + for (unsigned int idx = 0; idx < msg->joint_name.size(); idx++) + { + Dynamixel *dxl = NULL; + auto dxl_it = robot_->dxls_.find((std::string) (msg->joint_name[idx])); + if (dxl_it != robot_->dxls_.end()) + dxl = dxl_it->second; + else + continue; + + // none + if (msg->module_name[idx] == "" || msg->module_name[idx] == "none") + { + dxl->ctrl_module_name_ = msg->module_name[idx]; + continue; + } + + // check whether the module is using this joint + for (auto m_it = motion_modules_.begin(); m_it != motion_modules_.end(); m_it++) + { + if ((*m_it)->getModuleName() == msg->module_name[idx]) + { + if ((*m_it)->result_.find(msg->joint_name[idx]) != (*m_it)->result_.end()) + { + dxl->ctrl_module_name_ = msg->module_name[idx]; + break; + } + } + } + } + + for (auto m_it = motion_modules_.begin(); m_it != motion_modules_.end(); m_it++) + { + // set all modules -> disable + (*m_it)->setModuleEnable(false); + + // set all used modules -> enable + for (auto& d_it : robot_->dxls_) + { + if (d_it.second->ctrl_module_name_ == (*m_it)->getModuleName()) + { + (*m_it)->setModuleEnable(true); + break; + } + } + } + + // TODO: set indirect address + // -> check module's control_mode + + queue_mutex_.unlock(); + + robotis_controller_msgs::JointCtrlModule current_module_msg; + for (auto& dxl_iter : robot_->dxls_) + { + current_module_msg.joint_name.push_back(dxl_iter.first); + current_module_msg.module_name.push_back(dxl_iter.second->ctrl_module_name_); + } + + if (current_module_msg.joint_name.size() == current_module_msg.module_name.size()) + current_module_pub_.publish(current_module_msg); +} + +bool RobotisController::getCtrlModuleCallback(robotis_controller_msgs::GetJointModule::Request &req, + robotis_controller_msgs::GetJointModule::Response &res) +{ + for (unsigned int idx = 0; idx < req.joint_name.size(); idx++) + { + auto d_it = robot_->dxls_.find((std::string) (req.joint_name[idx])); + if (d_it != robot_->dxls_.end()) + { + res.joint_name.push_back(req.joint_name[idx]); + res.module_name.push_back(d_it->second->ctrl_module_name_); + } + } + + if (res.joint_name.size() == 0) + return false; + + return true; +} + +void RobotisController::setCtrlModuleThread(std::string ctrl_module) +{ + // stop module + std::list stop_modules; + + if (ctrl_module == "" || ctrl_module == "none") + { + // enqueue all modules in order to stop + for (auto m_it = motion_modules_.begin(); m_it != motion_modules_.end(); m_it++) + { + if ((*m_it)->getModuleEnable() == true) + stop_modules.push_back(*m_it); + } + } + else + { + for (auto m_it = motion_modules_.begin(); m_it != motion_modules_.end(); m_it++) + { + // if it exist + if ((*m_it)->getModuleName() == ctrl_module) + { + // enqueue the module which lost control of joint in order to stop + for (auto& result_it : (*m_it)->result_) + { + auto d_it = robot_->dxls_.find(result_it.first); + + if (d_it != robot_->dxls_.end()) + { + // enqueue + if (d_it->second->ctrl_module_name_ != ctrl_module) + { + for (auto stop_m_it = motion_modules_.begin(); stop_m_it != motion_modules_.end(); stop_m_it++) + { + if (((*stop_m_it)->getModuleName() == d_it->second->ctrl_module_name_) && + ((*stop_m_it)->getModuleEnable() == true)) + { + stop_modules.push_back(*stop_m_it); + } + } + } + } + } + + break; + } + } + } + + // stop the module + stop_modules.unique(); + for (auto stop_m_it = stop_modules.begin(); stop_m_it != stop_modules.end(); stop_m_it++) + { + (*stop_m_it)->stop(); + } + + // wait to stop + for (auto stop_m_it = stop_modules.begin(); stop_m_it != stop_modules.end(); stop_m_it++) + { + while ((*stop_m_it)->isRunning()) + usleep(CONTROL_CYCLE_MSEC * 1000); + } + + // set ctrl module + queue_mutex_.lock(); + + if (DEBUG_PRINT) + ROS_INFO_STREAM("set module : " << ctrl_module); + + // none + if ((ctrl_module == "") || (ctrl_module == "none")) + { + // set all modules -> disable + for (auto m_it = motion_modules_.begin(); m_it != motion_modules_.end(); m_it++) + { + (*m_it)->setModuleEnable(false); + } + + // set dxl's control module to "none" + for (auto& d_it : robot_->dxls_) + { + Dynamixel *dxl = d_it.second; + dxl->ctrl_module_name_ = "none"; + + if (gazebo_mode_ == true) + continue; + + uint32_t pos_data = dxl->convertRadian2Value(dxl->dxl_state_->goal_position_ + dxl->dxl_state_->position_offset_); + uint8_t sync_write_data[4] = { 0 }; + sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(pos_data)); + sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(pos_data)); + sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(pos_data)); + sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(pos_data)); + + if (port_to_sync_write_position_[dxl->port_name_] != NULL) + port_to_sync_write_position_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); + + if (port_to_sync_write_current_[dxl->port_name_] != NULL) + port_to_sync_write_current_[dxl->port_name_]->removeParam(dxl->id_); + if (port_to_sync_write_velocity_[dxl->port_name_] != NULL) + port_to_sync_write_velocity_[dxl->port_name_]->removeParam(dxl->id_); + } + } + else + { + // check whether the module exist + for (auto m_it = motion_modules_.begin(); m_it != motion_modules_.end(); m_it++) + { + // if it exist + if ((*m_it)->getModuleName() == ctrl_module) + { + ControlMode mode = (*m_it)->getControlMode(); + for (auto& result_it : (*m_it)->result_) + { + auto d_it = robot_->dxls_.find(result_it.first); + if (d_it != robot_->dxls_.end()) + { + Dynamixel *dxl = d_it->second; + dxl->ctrl_module_name_ = ctrl_module; + + if (gazebo_mode_ == true) + continue; + + if (mode == PositionControl) + { + uint32_t pos_data = dxl->convertRadian2Value(dxl->dxl_state_->goal_position_ + dxl->dxl_state_->position_offset_); + uint8_t sync_write_data[4] = { 0 }; + sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(pos_data)); + sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(pos_data)); + sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(pos_data)); + sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(pos_data)); + + if (port_to_sync_write_position_[dxl->port_name_] != NULL) + port_to_sync_write_position_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); + + if (port_to_sync_write_current_[dxl->port_name_] != NULL) + port_to_sync_write_current_[dxl->port_name_]->removeParam(dxl->id_); + if (port_to_sync_write_velocity_[dxl->port_name_] != NULL) + port_to_sync_write_velocity_[dxl->port_name_]->removeParam(dxl->id_); + } + else if (mode == VelocityControl) + { + uint32_t vel_data = dxl->convertVelocity2Value(dxl->dxl_state_->goal_velocity_); + uint8_t sync_write_data[4] = { 0 }; + sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(vel_data)); + sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(vel_data)); + sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(vel_data)); + sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(vel_data)); + + if (port_to_sync_write_velocity_[dxl->port_name_] != NULL) + port_to_sync_write_velocity_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); + + if (port_to_sync_write_current_[dxl->port_name_] != NULL) + port_to_sync_write_current_[dxl->port_name_]->removeParam(dxl->id_); + if (port_to_sync_write_position_[dxl->port_name_] != NULL) + port_to_sync_write_position_[dxl->port_name_]->removeParam(dxl->id_); + } + else if (mode == TorqueControl) + { + uint32_t curr_data = dxl->convertTorque2Value(dxl->dxl_state_->goal_torque_); + uint8_t sync_write_data[4] = { 0 }; + sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(curr_data)); + sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(curr_data)); + sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(curr_data)); + sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(curr_data)); + + if (port_to_sync_write_current_[dxl->port_name_] != NULL) + port_to_sync_write_current_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); + + if (port_to_sync_write_velocity_[dxl->port_name_] != NULL) + port_to_sync_write_velocity_[dxl->port_name_]->removeParam(dxl->id_); + if (port_to_sync_write_position_[dxl->port_name_] != NULL) + port_to_sync_write_position_[dxl->port_name_]->removeParam(dxl->id_); + } + } + } + + break; + } + } + } + + for (auto m_it = motion_modules_.begin(); m_it != motion_modules_.end(); m_it++) + { + // set all modules -> disable + (*m_it)->setModuleEnable(false); + + // set all used modules -> enable + for (auto& d_it : robot_->dxls_) + { + if (d_it.second->ctrl_module_name_ == (*m_it)->getModuleName()) + { + (*m_it)->setModuleEnable(true); + break; + } + } + } + + // TODO: set indirect address + // -> check module's control_mode + + queue_mutex_.unlock(); + + // publish current module + robotis_controller_msgs::JointCtrlModule current_module_msg; + for (auto& dxl_iter : robot_->dxls_) + { + current_module_msg.joint_name.push_back(dxl_iter.first); + current_module_msg.module_name.push_back(dxl_iter.second->ctrl_module_name_); + } + + if (current_module_msg.joint_name.size() == current_module_msg.module_name.size()) + current_module_pub_.publish(current_module_msg); +} + +void RobotisController::gazeboJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg) +{ + queue_mutex_.lock(); + + for (unsigned int i = 0; i < msg->name.size(); i++) + { + auto d_it = robot_->dxls_.find((std::string) msg->name[i]); + if (d_it != robot_->dxls_.end()) + { + d_it->second->dxl_state_->present_position_ = msg->position[i]; + d_it->second->dxl_state_->present_velocity_ = msg->velocity[i]; + d_it->second->dxl_state_->present_torque_ = msg->effort[i]; + } + } + + if (init_pose_loaded_ == false) + { + for (auto& it : robot_->dxls_) + it.second->dxl_state_->goal_position_ = it.second->dxl_state_->present_position_; + init_pose_loaded_ = true; + } + + queue_mutex_.unlock(); +} + +bool RobotisController::isTimerStopped() +{ + if (this->is_timer_running_) + { + if (DEBUG_PRINT == true) + ROS_WARN("Process Timer is running.. STOP the timer first."); + return false; + } + return true; +} + +int RobotisController::ping(const std::string joint_name, uint8_t *error) +{ + return ping(joint_name, 0, error); +} +int RobotisController::ping(const std::string joint_name, uint16_t* model_number, uint8_t *error) +{ + if (isTimerStopped() == false) + return COMM_PORT_BUSY; + + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; + + dynamixel::PacketHandler *pkt_handler = dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; + + return pkt_handler->ping(port_handler, dxl->id_, model_number, error); +} + +int RobotisController::action(const std::string joint_name) +{ + if (isTimerStopped() == false) + return COMM_PORT_BUSY; + + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; + + dynamixel::PacketHandler *pkt_handler = dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; + + return pkt_handler->action(port_handler, dxl->id_); +} +int RobotisController::reboot(const std::string joint_name, uint8_t *error) +{ + if (isTimerStopped() == false) + return COMM_PORT_BUSY; + + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; + + dynamixel::PacketHandler *pkt_handler = dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; + + return pkt_handler->reboot(port_handler, dxl->id_, error); +} +int RobotisController::factoryReset(const std::string joint_name, uint8_t option, uint8_t *error) +{ + if (isTimerStopped() == false) + return COMM_PORT_BUSY; + + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; + + dynamixel::PacketHandler *pkt_handler = dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; + + return pkt_handler->factoryReset(port_handler, dxl->id_, option, error); +} + +int RobotisController::read(const std::string joint_name, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error) +{ + if (isTimerStopped() == false) + return COMM_PORT_BUSY; + + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; + + dynamixel::PacketHandler *pkt_handler = dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; + + return pkt_handler->readTxRx(port_handler, dxl->id_, address, length, data, error); +} + +int RobotisController::readCtrlItem(const std::string joint_name, const std::string item_name, uint32_t *data, uint8_t *error) +{ + if (isTimerStopped() == false) + return COMM_PORT_BUSY; + + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; + + ControlTableItem *item = dxl->ctrl_table_[item_name]; + if (item == NULL) + return COMM_NOT_AVAILABLE; + + dynamixel::PacketHandler *pkt_handler = dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; + + int result = COMM_NOT_AVAILABLE; + switch (item->data_length_) + { + case 1: + { + uint8_t read_data = 0; + result = pkt_handler->read1ByteTxRx(port_handler, dxl->id_, item->address_, &read_data, error); + if (result == COMM_SUCCESS) + *data = read_data; + break; + } + case 2: + { + uint16_t read_data = 0; + result = pkt_handler->read2ByteTxRx(port_handler, dxl->id_, item->address_, &read_data, error); + if (result == COMM_SUCCESS) + *data = read_data; + break; + } + case 4: + { + uint32_t read_data = 0; + result = pkt_handler->read4ByteTxRx(port_handler, dxl->id_, item->address_, &read_data, error); + if (result == COMM_SUCCESS) + *data = read_data; + break; + } + default: + break; + } + return result; +} + +int RobotisController::read1Byte(const std::string joint_name, uint16_t address, uint8_t *data, uint8_t *error) +{ + if (isTimerStopped() == false) + return COMM_PORT_BUSY; + + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; + + dynamixel::PacketHandler *pkt_handler = dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; + + return pkt_handler->read1ByteTxRx(port_handler, dxl->id_, address, data, error); +} + +int RobotisController::read2Byte(const std::string joint_name, uint16_t address, uint16_t *data, uint8_t *error) +{ + if (isTimerStopped() == false) + return COMM_PORT_BUSY; + + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; + + dynamixel::PacketHandler *pkt_handler = dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; + + return pkt_handler->read2ByteTxRx(port_handler, dxl->id_, address, data, error); +} + +int RobotisController::read4Byte(const std::string joint_name, uint16_t address, uint32_t *data, uint8_t *error) +{ + if (isTimerStopped() == false) + return COMM_PORT_BUSY; + + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; + + dynamixel::PacketHandler *pkt_handler = dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; + + return pkt_handler->read4ByteTxRx(port_handler, dxl->id_, address, data, error); +} + +int RobotisController::write(const std::string joint_name, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error) +{ + if (isTimerStopped() == false) + return COMM_PORT_BUSY; + + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; + + dynamixel::PacketHandler *pkt_handler = dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; + + return pkt_handler->writeTxRx(port_handler, dxl->id_, address, length, data, error); +} + +int RobotisController::writeCtrlItem(const std::string joint_name, const std::string item_name, uint32_t data, uint8_t *error) +{ + if (isTimerStopped() == false) + return COMM_PORT_BUSY; + + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; + + ControlTableItem *item = dxl->ctrl_table_[item_name]; + if (item == NULL) + return COMM_NOT_AVAILABLE; + + dynamixel::PacketHandler *pkt_handler = dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; + + int result = COMM_NOT_AVAILABLE; + uint8_t *write_data = new uint8_t[item->data_length_]; + if (item->data_length_ == 1) + { + write_data[0] = (uint8_t) data; + result = pkt_handler->write1ByteTxRx(port_handler, dxl->id_, item->address_, data, error); + } + else if (item->data_length_ == 2) + { + write_data[0] = DXL_LOBYTE((uint16_t )data); + write_data[1] = DXL_HIBYTE((uint16_t )data); + result = pkt_handler->write2ByteTxRx(port_handler, dxl->id_, item->address_, data, error); + } + else if (item->data_length_ == 4) + { + write_data[0] = DXL_LOBYTE(DXL_LOWORD((uint32_t)data)); + write_data[1] = DXL_HIBYTE(DXL_LOWORD((uint32_t)data)); + write_data[2] = DXL_LOBYTE(DXL_HIWORD((uint32_t)data)); + write_data[3] = DXL_HIBYTE(DXL_HIWORD((uint32_t)data)); + result = pkt_handler->write4ByteTxRx(port_handler, dxl->id_, item->address_, data, error); + } + delete[] write_data; + return result; +} + +int RobotisController::write1Byte(const std::string joint_name, uint16_t address, uint8_t data, uint8_t *error) +{ + if (isTimerStopped() == false) + return COMM_PORT_BUSY; + + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; + + dynamixel::PacketHandler *pkt_handler = dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; + + return pkt_handler->write1ByteTxRx(port_handler, dxl->id_, address, data, error); +} + +int RobotisController::write2Byte(const std::string joint_name, uint16_t address, uint16_t data, uint8_t *error) +{ + if (isTimerStopped() == false) + return COMM_PORT_BUSY; + + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; + + dynamixel::PacketHandler *pkt_handler = dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; + + return pkt_handler->write2ByteTxRx(port_handler, dxl->id_, address, data, error); +} + +int RobotisController::write4Byte(const std::string joint_name, uint16_t address, uint32_t data, uint8_t *error) +{ + if (isTimerStopped() == false) + return COMM_PORT_BUSY; + + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; + + dynamixel::PacketHandler *pkt_handler = dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; + + return pkt_handler->write4ByteTxRx(port_handler, dxl->id_, address, data, error); +} + +int RobotisController::regWrite(const std::string joint_name, uint16_t address, uint16_t length, uint8_t *data, + uint8_t *error) +{ + if (isTimerStopped() == false) + return COMM_PORT_BUSY; + + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; + + dynamixel::PacketHandler *pkt_handler = dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; + + return pkt_handler->regWriteTxRx(port_handler, dxl->id_, address, length, data, error); +} + diff --git a/robotis_controller_msgs/CMakeLists.txt b/robotis_controller_msgs/CMakeLists.txt deleted file mode 100644 index d9f015c..0000000 --- a/robotis_controller_msgs/CMakeLists.txt +++ /dev/null @@ -1,48 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(robotis_controller_msgs) - -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS - sensor_msgs - std_msgs - message_generation -) - -## Generate messages in the 'msg' folder -add_message_files( - FILES - SyncWriteItem.msg - JointCtrlModule.msg - StatusMsg.msg -) - -## Generate services in the 'srv' folder -add_service_files( - FILES - GetJointModule.srv -) - # Service1.srv -# Service2.srv -# ) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - -## Generate added messages and services with any dependencies listed here -generate_messages( - DEPENDENCIES - std_msgs - sensor_msgs -) - -catkin_package( -# INCLUDE_DIRS include - CATKIN_DEPENDS sensor_msgs std_msgs -) - diff --git a/robotis_controller_msgs/msg/JointCtrlModule.msg b/robotis_controller_msgs/msg/JointCtrlModule.msg deleted file mode 100644 index b91eb4d..0000000 --- a/robotis_controller_msgs/msg/JointCtrlModule.msg +++ /dev/null @@ -1,2 +0,0 @@ -string[] joint_name -string[] module_name \ No newline at end of file diff --git a/robotis_controller_msgs/msg/StatusMsg.msg b/robotis_controller_msgs/msg/StatusMsg.msg deleted file mode 100644 index 47b706c..0000000 --- a/robotis_controller_msgs/msg/StatusMsg.msg +++ /dev/null @@ -1,10 +0,0 @@ -# Status Constants -uint8 STATUS_UNKNOWN = 0 -uint8 STATUS_INFO = 1 -uint8 STATUS_WARN = 2 -uint8 STATUS_ERROR = 3 - -std_msgs/Header header -uint8 type -string module_name -string status_msg \ No newline at end of file diff --git a/robotis_controller_msgs/msg/SyncWriteItem.msg b/robotis_controller_msgs/msg/SyncWriteItem.msg deleted file mode 100644 index 4d602b6..0000000 --- a/robotis_controller_msgs/msg/SyncWriteItem.msg +++ /dev/null @@ -1,3 +0,0 @@ -string item_name -string[] joint_name -uint32[] value \ No newline at end of file diff --git a/robotis_controller_msgs/package.xml b/robotis_controller_msgs/package.xml deleted file mode 100644 index 3ce5db7..0000000 --- a/robotis_controller_msgs/package.xml +++ /dev/null @@ -1,30 +0,0 @@ - - - robotis_controller_msgs - 0.1.0 - The robotis_controller_msgs package - robotis - - BSD - - - - robotis --> - - catkin - - sensor_msgs - std_msgs - message_generation - - sensor_msgs - std_msgs - message_runtime - - - - - - - - \ No newline at end of file diff --git a/robotis_controller_msgs/srv/GetJointModule.srv b/robotis_controller_msgs/srv/GetJointModule.srv deleted file mode 100644 index bedde91..0000000 --- a/robotis_controller_msgs/srv/GetJointModule.srv +++ /dev/null @@ -1,4 +0,0 @@ -string[] joint_name ---- -string[] joint_name -string[] module_name \ No newline at end of file diff --git a/robotis_device/CMakeLists.txt b/robotis_device/CMakeLists.txt index 8bd7be5..e492450 100644 --- a/robotis_device/CMakeLists.txt +++ b/robotis_device/CMakeLists.txt @@ -5,18 +5,8 @@ find_package(catkin REQUIRED COMPONENTS roscpp rospy dynamixel_sdk - robotis_framework_common ) -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if you package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need catkin_package( INCLUDE_DIRS include LIBRARIES robotis_device @@ -24,38 +14,19 @@ catkin_package( # DEPENDS system_lib ) -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -# include_directories(include) include_directories( include ${catkin_INCLUDE_DIRS} ) -## Declare a C++ library add_library(robotis_device - src/robotis_device/Robot.cpp - src/robotis_device/Sensor.cpp - src/robotis_device/Dynamixel.cpp + src/robotis_device/robot.cpp + src/robotis_device/sensor.cpp + src/robotis_device/dynamixel.cpp ) -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure add_dependencies(robotis_device ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -## Declare a C++ executable -# add_executable(robotis_device_node src/robotis_device_node.cpp) - -## Add cmake target dependencies of the executable -## same as for the library above -# add_dependencies(robotis_device_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Specify libraries to link a library or executable target against target_link_libraries(robotis_device ${catkin_LIBRARIES} ) diff --git a/robotis_device/devices/dynamixel/GRIPPER.device b/robotis_device/devices/dynamixel/GRIPPER.device index e95ff17..4267c99 100644 --- a/robotis_device/devices/dynamixel/GRIPPER.device +++ b/robotis_device/devices/dynamixel/GRIPPER.device @@ -19,6 +19,9 @@ goal_current_item_name = goal_torque position_d_gain_item_name = position_i_gain_item_name = position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain [control table] # addr | item name | length | access | memory | min value | max value | signed diff --git a/robotis_device/devices/dynamixel/H42-20-S300-R.device b/robotis_device/devices/dynamixel/H42-20-S300-R.device index 1ed385f..95686ff 100644 --- a/robotis_device/devices/dynamixel/H42-20-S300-R.device +++ b/robotis_device/devices/dynamixel/H42-20-S300-R.device @@ -3,7 +3,7 @@ model_name = H42-20-S300-R device_type = dynamixel [type info] -current_ratio = 4.0283203125 +torque_to_current_value_ratio = 27.15146 value_of_0_radian_position = 0 value_of_min_radian_position = -151900 @@ -21,6 +21,9 @@ goal_current_item_name = goal_torque position_d_gain_item_name = position_i_gain_item_name = position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain [control table] # addr | item name | length | access | memory | min value | max value | signed diff --git a/robotis_device/devices/dynamixel/H54-100-S500-R.device b/robotis_device/devices/dynamixel/H54-100-S500-R.device index 09ef828..a251ddd 100644 --- a/robotis_device/devices/dynamixel/H54-100-S500-R.device +++ b/robotis_device/devices/dynamixel/H54-100-S500-R.device @@ -3,7 +3,7 @@ model_name = H54-100-S500-R device_type = dynamixel [type info] -current_ratio = 16.11328125 +torque_to_current_value_ratio = 9.66026 value_of_0_radian_position = 0 value_of_min_radian_position = -250950 @@ -21,6 +21,9 @@ goal_current_item_name = goal_torque position_d_gain_item_name = position_i_gain_item_name = position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain [control table] # addr | item name | length | access | memory | min value | max value | signed diff --git a/robotis_device/devices/dynamixel/H54-200-B500-R.device b/robotis_device/devices/dynamixel/H54-200-B500-R.device index 8791d7c..20a280a 100644 --- a/robotis_device/devices/dynamixel/H54-200-B500-R.device +++ b/robotis_device/devices/dynamixel/H54-200-B500-R.device @@ -3,7 +3,7 @@ model_name = H54-200-B500-R device_type = dynamixel [type info] -current_ratio = 16.11328125 +torque_to_current_value_ratio = 9.09201 value_of_0_radian_position = 0 value_of_min_radian_position = -250950 @@ -21,6 +21,9 @@ goal_current_item_name = goal_torque position_d_gain_item_name = position_i_gain_item_name = position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain [control table] # addr | item name | length | access | memory | min value | max value | signed diff --git a/robotis_device/devices/dynamixel/H54-200-S500-R.device b/robotis_device/devices/dynamixel/H54-200-S500-R.device index dba268c..5b7dc76 100644 --- a/robotis_device/devices/dynamixel/H54-200-S500-R.device +++ b/robotis_device/devices/dynamixel/H54-200-S500-R.device @@ -3,7 +3,7 @@ model_name = H54-200-S500-R device_type = dynamixel [type info] -current_ratio = 16.11328125 +torque_to_current_value_ratio = 9.09201 value_of_0_radian_position = 0 value_of_min_radian_position = -250950 @@ -21,6 +21,9 @@ goal_current_item_name = goal_torque position_d_gain_item_name = position_i_gain_item_name = position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain [control table] # addr | item name | length | access | memory | min value | max value | signed diff --git a/robotis_device/devices/dynamixel/L42-10-S300-R.device b/robotis_device/devices/dynamixel/L42-10-S300-R.device index 8a4ca5a..84e556d 100644 --- a/robotis_device/devices/dynamixel/L42-10-S300-R.device +++ b/robotis_device/devices/dynamixel/L42-10-S300-R.device @@ -19,6 +19,9 @@ goal_current_item_name = goal_torque position_d_gain_item_name = position_i_gain_item_name = position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain [control table] # addr | item name | length | access | memory | min value | max value | signed diff --git a/robotis_device/devices/dynamixel/L54-30-S400-R.device b/robotis_device/devices/dynamixel/L54-30-S400-R.device index 8cdcff3..9dc5d36 100644 --- a/robotis_device/devices/dynamixel/L54-30-S400-R.device +++ b/robotis_device/devices/dynamixel/L54-30-S400-R.device @@ -3,8 +3,6 @@ model_name = L54-30-S400-R device_type = dynamixel [type info] -current_ratio = 16.11328125 - value_of_0_radian_position = 0 value_of_min_radian_position = -144198 value_of_max_radian_position = 144198 @@ -21,6 +19,9 @@ goal_current_item_name = goal_torque position_d_gain_item_name = position_i_gain_item_name = position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain [control table] # addr | item name | length | access | memory | min value | max value | signed diff --git a/robotis_device/devices/dynamixel/L54-30-S500-R.device b/robotis_device/devices/dynamixel/L54-30-S500-R.device index db5a0b4..fa15941 100644 --- a/robotis_device/devices/dynamixel/L54-30-S500-R.device +++ b/robotis_device/devices/dynamixel/L54-30-S500-R.device @@ -3,8 +3,6 @@ model_name = L54-30-S500-R device_type = dynamixel [type info] -current_ratio = 16.11328125 - value_of_0_radian_position = 0 value_of_min_radian_position = -180684 value_of_max_radian_position = 180684 @@ -21,6 +19,9 @@ goal_current_item_name = goal_torque position_d_gain_item_name = position_i_gain_item_name = position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain [control table] # addr | item name | length | access | memory | min value | max value | signed diff --git a/robotis_device/devices/dynamixel/L54-50-S290-R.device b/robotis_device/devices/dynamixel/L54-50-S290-R.device index cbd0453..8c2f1a2 100644 --- a/robotis_device/devices/dynamixel/L54-50-S290-R.device +++ b/robotis_device/devices/dynamixel/L54-50-S290-R.device @@ -3,8 +3,6 @@ model_name = L54-50-S290-R device_type = dynamixel [type info] -current_ratio = 16.11328125 - value_of_0_radian_position = 0 value_of_min_radian_position = -103860 value_of_max_radian_position = 103860 @@ -21,6 +19,9 @@ goal_current_item_name = goal_torque position_d_gain_item_name = position_i_gain_item_name = position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain [control table] # addr | item name | length | access | memory | min value | max value | signed diff --git a/robotis_device/devices/dynamixel/L54-50-S500-R.device b/robotis_device/devices/dynamixel/L54-50-S500-R.device index d72f2ef..991e159 100644 --- a/robotis_device/devices/dynamixel/L54-50-S500-R.device +++ b/robotis_device/devices/dynamixel/L54-50-S500-R.device @@ -3,8 +3,6 @@ model_name = L54-50-S500-R device_type = dynamixel [type info] -current_ratio = 16.11328125 - value_of_0_radian_position = 0 value_of_min_radian_position = -180684 value_of_max_radian_position = 180684 @@ -21,6 +19,9 @@ goal_current_item_name = goal_torque position_d_gain_item_name = position_i_gain_item_name = position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain [control table] # addr | item name | length | access | memory | min value | max value | signed diff --git a/robotis_device/devices/dynamixel/M42-10-S260-R.device b/robotis_device/devices/dynamixel/M42-10-S260-R.device index 8c96988..4c25631 100644 --- a/robotis_device/devices/dynamixel/M42-10-S260-R.device +++ b/robotis_device/devices/dynamixel/M42-10-S260-R.device @@ -3,8 +3,6 @@ model_name = M42-10-S260-R device_type = dynamixel [type info] -current_ratio = 4.0283203125 - value_of_0_radian_position = 0 value_of_min_radian_position = -131584 value_of_max_radian_position = 131584 @@ -21,6 +19,9 @@ goal_current_item_name = goal_torque position_d_gain_item_name = position_i_gain_item_name = position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain [control table] # addr | item name | length | access | memory | min value | max value | signed diff --git a/robotis_device/devices/dynamixel/M54-40-S250-R.device b/robotis_device/devices/dynamixel/M54-40-S250-R.device index 2bc7c1c..76796b1 100644 --- a/robotis_device/devices/dynamixel/M54-40-S250-R.device +++ b/robotis_device/devices/dynamixel/M54-40-S250-R.device @@ -3,8 +3,6 @@ model_name = M54-40-S250-R device_type = dynamixel [type info] -current_ratio = 16.11328125 - value_of_0_radian_position = 0 value_of_min_radian_position = -125700 value_of_max_radian_position = 125700 @@ -21,6 +19,9 @@ goal_current_item_name = goal_torque position_d_gain_item_name = position_i_gain_item_name = position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain [control table] # addr | item name | length | access | memory | min value | max value | signed diff --git a/robotis_device/devices/dynamixel/M54-60-S250-R.device b/robotis_device/devices/dynamixel/M54-60-S250-R.device index 72382de..a6cfa2a 100644 --- a/robotis_device/devices/dynamixel/M54-60-S250-R.device +++ b/robotis_device/devices/dynamixel/M54-60-S250-R.device @@ -3,8 +3,6 @@ model_name = M54-60-S250-R device_type = dynamixel [type info] -current_ratio = 16.11328125 - value_of_0_radian_position = 0 value_of_min_radian_position = -125700 value_of_max_radian_position = 125700 @@ -21,6 +19,9 @@ goal_current_item_name = goal_torque position_d_gain_item_name = position_i_gain_item_name = position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain [control table] # addr | item name | length | access | memory | min value | max value | signed diff --git a/robotis_device/devices/dynamixel/MX-106.device b/robotis_device/devices/dynamixel/MX-106.device new file mode 100644 index 0000000..4b8c63d --- /dev/null +++ b/robotis_device/devices/dynamixel/MX-106.device @@ -0,0 +1,66 @@ +[device info] +model_name = MX-106 +device_type = dynamixel + +[type info] +value_of_0_radian_position = 2048 +value_of_min_radian_position = 0 +value_of_max_radian_position = 4095 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = +position_d_gain_item_name = position_d_gain +position_i_gain_item_name = position_i_gain +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = +velocity_p_gain_item_name = + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 2 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 3 | ID | 1 | RW | EEPROM | 0 | 252 | N + 4 | baudrate | 1 | RW | EEPROM | 0 | 252 | N + 5 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 6 | CW_angle_limit | 2 | RW | EEPROM | 0 | 4095 | N + 8 | CCW_angle_limit | 2 | RW | EEPROM | 0 | 4095 | N + 10 | drive_mode | 1 | RW | EEPROM | 0 | 3 | N + 11 | max_temperature_limit | 1 | RW | EEPROM | 0 | 99 | N + 12 | min_voltage_limit | 1 | RW | EEPROM | 0 | 250 | N + 13 | max_voltage_limit | 1 | RW | EEPROM | 0 | 250 | N + 14 | max_torque | 2 | RW | EEPROM | 0 | 1023 | N + 16 | status_return_level | 1 | RW | EEPROM | 0 | 2 | N + 17 | alarm_LED | 1 | RW | EEPROM | 0 | 127 | N + 18 | alarm_shutdown | 1 | RW | EEPROM | 0 | 127 | N + 20 | multi_turn_offset | 2 | RW | EEPROM | -26624 | 26624 | Y + 22 | resolution_dividor | 1 | RW | EEPROM | 1 | 255 | N + 24 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 25 | LED | 1 | RW | RAM | 0 | 1 | N + 26 | position_d_gain | 1 | RW | RAM | 0 | 254 | N + 27 | position_i_gain | 1 | RW | RAM | 0 | 254 | N + 28 | position_p_gain | 1 | RW | RAM | 0 | 254 | N + 30 | goal_position | 2 | RW | RAM | -28672 | 28672 | Y + 32 | goal_velocity | 2 | RW | RAM | 0 | 1023 | N + 34 | goal_torque | 2 | RW | RAM | 0 | 1023 | N + 36 | present_position | 2 | R | RAM | -32768 | 32767 | Y + 38 | present_velocity | 2 | R | RAM | 0 | 2048 | N + 40 | present_load | 2 | R | RAM | 0 | 2048 | N + 42 | present_voltage | 1 | R | RAM | 50 | 250 | N + 43 | present_temperature | 1 | R | RAM | 0 | 99 | N + 44 | registered_instruction | 1 | R | RAM | 0 | 1 | N + 46 | is_moving | 1 | R | RAM | 0 | 1 | N + 47 | EEPROM_lock | 1 | RW | RAM | 0 | 1 | N + 48 | punch | 2 | RW | RAM | 0 | 1023 | N + 68 | current_consumption | 2 | RW | RAM | 0 | 4095 | N + 70 | torque_control_mode | 1 | RW | RAM | 0 | 1 | N + 71 | torque_control_goal | 2 | RW | RAM | 0 | 2047 | N + 73 | goal_acceleration | 1 | RW | RAM | 0 | 254 | N + diff --git a/robotis_device/devices/dynamixel/MX-28.device b/robotis_device/devices/dynamixel/MX-28.device index f1cfc21..ff67f2a 100644 --- a/robotis_device/devices/dynamixel/MX-28.device +++ b/robotis_device/devices/dynamixel/MX-28.device @@ -19,6 +19,9 @@ goal_current_item_name = position_d_gain_item_name = position_d_gain position_i_gain_item_name = position_i_gain position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = +velocity_p_gain_item_name = [control table] # addr | item name | length | access | memory | min value | max value | signed @@ -47,7 +50,7 @@ position_p_gain_item_name = position_p_gain 30 | goal_position | 2 | RW | RAM | -28672 | 28672 | Y 32 | goal_velocity | 2 | RW | RAM | 0 | 1023 | N 34 | goal_torque | 2 | RW | RAM | 0 | 1023 | N - 36 | present_position | 2 | R | RAM | 0 | 4095 | N + 36 | present_position | 2 | R | RAM | -32768 | 32767 | Y 38 | present_velocity | 2 | R | RAM | 0 | 2048 | N 40 | present_load | 2 | R | RAM | 0 | 2048 | N 42 | present_voltage | 1 | R | RAM | 50 | 250 | N diff --git a/robotis_device/devices/dynamixel/MX-64.device b/robotis_device/devices/dynamixel/MX-64.device new file mode 100644 index 0000000..06c0a4a --- /dev/null +++ b/robotis_device/devices/dynamixel/MX-64.device @@ -0,0 +1,66 @@ +[device info] +model_name = MX-64 +device_type = dynamixel + +[type info] +value_of_0_radian_position = 2048 +value_of_min_radian_position = 0 +value_of_max_radian_position = 4095 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = +position_d_gain_item_name = position_d_gain +position_i_gain_item_name = position_i_gain +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = +velocity_p_gain_item_name = + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 2 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 3 | ID | 1 | RW | EEPROM | 0 | 252 | N + 4 | baudrate | 1 | RW | EEPROM | 0 | 252 | N + 5 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 6 | CW_angle_limit | 2 | RW | EEPROM | 0 | 4095 | N + 8 | CCW_angle_limit | 2 | RW | EEPROM | 0 | 4095 | N + 10 | drive_mode | 1 | RW | EEPROM | 0 | 3 | N + 11 | max_temperature_limit | 1 | RW | EEPROM | 0 | 99 | N + 12 | min_voltage_limit | 1 | RW | EEPROM | 0 | 250 | N + 13 | max_voltage_limit | 1 | RW | EEPROM | 0 | 250 | N + 14 | max_torque | 2 | RW | EEPROM | 0 | 1023 | N + 16 | status_return_level | 1 | RW | EEPROM | 0 | 2 | N + 17 | alarm_LED | 1 | RW | EEPROM | 0 | 127 | N + 18 | alarm_shutdown | 1 | RW | EEPROM | 0 | 127 | N + 20 | multi_turn_offset | 2 | RW | EEPROM | -26624 | 26624 | Y + 22 | resolution_dividor | 1 | RW | EEPROM | 1 | 255 | N + 24 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 25 | LED | 1 | RW | RAM | 0 | 1 | N + 26 | position_d_gain | 1 | RW | RAM | 0 | 254 | N + 27 | position_i_gain | 1 | RW | RAM | 0 | 254 | N + 28 | position_p_gain | 1 | RW | RAM | 0 | 254 | N + 30 | goal_position | 2 | RW | RAM | -28672 | 28672 | Y + 32 | goal_velocity | 2 | RW | RAM | 0 | 1023 | N + 34 | goal_torque | 2 | RW | RAM | 0 | 1023 | N + 36 | present_position | 2 | R | RAM | -32768 | 32767 | Y + 38 | present_velocity | 2 | R | RAM | 0 | 2048 | N + 40 | present_load | 2 | R | RAM | 0 | 2048 | N + 42 | present_voltage | 1 | R | RAM | 50 | 250 | N + 43 | present_temperature | 1 | R | RAM | 0 | 99 | N + 44 | registered_instruction | 1 | R | RAM | 0 | 1 | N + 46 | is_moving | 1 | R | RAM | 0 | 1 | N + 47 | EEPROM_lock | 1 | RW | RAM | 0 | 1 | N + 48 | punch | 2 | RW | RAM | 0 | 1023 | N + 68 | current_consumption | 2 | RW | RAM | 0 | 4095 | N + 70 | torque_control_mode | 1 | RW | RAM | 0 | 1 | N + 71 | torque_control_goal | 2 | RW | RAM | 0 | 2047 | N + 73 | goal_acceleration | 1 | RW | RAM | 0 | 254 | N + diff --git a/robotis_device/devices/dynamixel/XM-430-W210.device b/robotis_device/devices/dynamixel/XM-430-W210.device new file mode 100644 index 0000000..f42ae57 --- /dev/null +++ b/robotis_device/devices/dynamixel/XM-430-W210.device @@ -0,0 +1,81 @@ +[device info] +model_name = XM-430-W210 +device_type = dynamixel + +[type info] +torque_to_current_value_ratio = 235.53647082 +velocity_to_value_ratio = 41.707853 + +value_of_0_radian_position = 2048 +value_of_min_radian_position = 0 +value_of_max_radian_position = 4095 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_current +position_d_gain_item_name = position_d_gain +position_i_gain_item_name = position_i_gain +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 7 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 16 | N + 20 | homing_offset | 4 | RW | EEPROM | -1044479 | 1044479 | Y + 24 | moving_threshold | 4 | RW | EEPROM | 0 | 1023 | N + 31 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 32 | max_voltage_limit | 2 | RW | EEPROM | 95 | 160 | N + 34 | min_voltage_limit | 2 | RW | EEPROM | 95 | 160 | N + 36 | pwm_limit | 2 | RW | EEPROM | 0 | 885 | N + 38 | current_limit | 2 | RW | EEPROM | 0 | 1193 | N + 40 | acceleration_limit | 4 | RW | EEPROM | 0 | 32767 | N + 44 | velocity_limit | 4 | RW | EEPROM | 0 | 1023 | N + 48 | max_position_limit | 4 | RW | EEPROM | 0 | 4095 | N + 52 | min_position_limit | 4 | RW | EEPROM | 0 | 4095 | N + 63 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 64 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 65 | LED | 1 | RW | RAM | 0 | 1 | N + 68 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 69 | registered_instruction | 1 | R | RAM | 0 | 1 | N + 70 | hardware_error_status | 1 | R | RAM | 0 | 255 | N + 76 | velocity_i_gain | 2 | RW | RAM | 0 | 32767 | N + 78 | velocity_p_gain | 2 | RW | RAM | 0 | 32767 | N + 80 | position_d_gain | 2 | RW | RAM | 0 | 32767 | N + 82 | position_i_gain | 2 | RW | RAM | 0 | 32767 | N + 84 | position_p_gain | 2 | RW | RAM | 0 | 32767 | N + 88 | feedforward_2nd_gain | 2 | RW | RAM | 0 | 32767 | N + 90 | feedforward_1st_gain | 2 | RW | RAM | 0 | 32767 | N + 100 | goal_pwm | 2 | RW | RAM | 0 | 885 | N + 102 | goal_current | 2 | RW | RAM | 0 | 1193 | N + 104 | goal_velocity | 4 | RW | RAM | 0 | 1023 | N + 108 | profile_acceleration | 4 | RW | RAM | 0 | 32767 | N + 112 | profile_velocity | 4 | RW | RAM | 0 | 1023 | N + 116 | goal_position | 4 | RW | RAM | -1048575 | 1048575 | Y + 120 | realtime_tick | 2 | R | RAM | 0 | 32767 | N + 122 | moving | 1 | R | RAM | 0 | 1 | N + 123 | moving_status | 1 | R | RAM | 0 | 255 | N + 124 | present_pwm | 2 | R | RAM | 0 | 885 | N + 126 | present_current | 2 | R | RAM | 0 | 1193 | N + 128 | present_velocity | 4 | R | RAM | 0 | 1023 | N + 132 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 136 | velocity_trajectory | 4 | R | RAM | 0 | 1023 | N + 140 | position_trajectory | 4 | R | RAM | 0 | 4095 | N + 144 | present_input_voltage | 2 | R | RAM | 95 | 160 | N + 146 | present_temperature | 1 | R | RAM | 0 | 100 | N + 168 | indirect_address_1 | 2 | RW | RAM | 0 | 65535 | N + 224 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 578 | indirect_address_29 | 2 | RW | RAM | 0 | 65535 | N + 634 | indirect_data_29 | 1 | RW | RAM | 0 | 255 | N diff --git a/robotis_device/devices/dynamixel/XM-430-W350.device b/robotis_device/devices/dynamixel/XM-430-W350.device new file mode 100644 index 0000000..ee14157 --- /dev/null +++ b/robotis_device/devices/dynamixel/XM-430-W350.device @@ -0,0 +1,81 @@ +[device info] +model_name = XM-430-W350 +device_type = dynamixel + +[type info] +torque_to_current_value_ratio = 149.795386991 +velocity_to_value_ratio = 41.707853 + +value_of_0_radian_position = 2048 +value_of_min_radian_position = 0 +value_of_max_radian_position = 4095 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_current +position_d_gain_item_name = position_d_gain +position_i_gain_item_name = position_i_gain +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 7 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 16 | N + 20 | homing_offset | 4 | RW | EEPROM | -1044479 | 1044479 | Y + 24 | moving_threshold | 4 | RW | EEPROM | 0 | 1023 | N + 31 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 32 | max_voltage_limit | 2 | RW | EEPROM | 95 | 160 | N + 34 | min_voltage_limit | 2 | RW | EEPROM | 95 | 160 | N + 36 | pwm_limit | 2 | RW | EEPROM | 0 | 885 | N + 38 | current_limit | 2 | RW | EEPROM | 0 | 1193 | N + 40 | acceleration_limit | 4 | RW | EEPROM | 0 | 32767 | N + 44 | velocity_limit | 4 | RW | EEPROM | 0 | 1023 | N + 48 | max_position_limit | 4 | RW | EEPROM | 0 | 4095 | N + 52 | min_position_limit | 4 | RW | EEPROM | 0 | 4095 | N + 63 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 64 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 65 | LED | 1 | RW | RAM | 0 | 1 | N + 68 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 69 | registered_instruction | 1 | R | RAM | 0 | 1 | N + 70 | hardware_error_status | 1 | R | RAM | 0 | 255 | N + 76 | velocity_i_gain | 2 | RW | RAM | 0 | 32767 | N + 78 | velocity_p_gain | 2 | RW | RAM | 0 | 32767 | N + 80 | position_d_gain | 2 | RW | RAM | 0 | 32767 | N + 82 | position_i_gain | 2 | RW | RAM | 0 | 32767 | N + 84 | position_p_gain | 2 | RW | RAM | 0 | 32767 | N + 88 | feedforward_2nd_gain | 2 | RW | RAM | 0 | 32767 | N + 90 | feedforward_1st_gain | 2 | RW | RAM | 0 | 32767 | N + 100 | goal_pwm | 2 | RW | RAM | 0 | 885 | N + 102 | goal_current | 2 | RW | RAM | 0 | 1193 | N + 104 | goal_velocity | 4 | RW | RAM | 0 | 1023 | N + 108 | profile_acceleration | 4 | RW | RAM | 0 | 32767 | N + 112 | profile_velocity | 4 | RW | RAM | 0 | 1023 | N + 116 | goal_position | 4 | RW | RAM | -1048575 | 1048575 | Y + 120 | realtime_tick | 2 | R | RAM | 0 | 32767 | N + 122 | moving | 1 | R | RAM | 0 | 1 | N + 123 | moving_status | 1 | R | RAM | 0 | 255 | N + 124 | present_pwm | 2 | R | RAM | 0 | 885 | N + 126 | present_current | 2 | R | RAM | 0 | 1193 | N + 128 | present_velocity | 4 | R | RAM | 0 | 1023 | N + 132 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 136 | velocity_trajectory | 4 | R | RAM | 0 | 1023 | N + 140 | position_trajectory | 4 | R | RAM | 0 | 4095 | N + 144 | present_input_voltage | 2 | R | RAM | 95 | 160 | N + 146 | present_temperature | 1 | R | RAM | 0 | 100 | N + 168 | indirect_address_1 | 2 | RW | RAM | 0 | 65535 | N + 224 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 578 | indirect_address_29 | 2 | RW | RAM | 0 | 65535 | N + 634 | indirect_data_29 | 1 | RW | RAM | 0 | 255 | N diff --git a/robotis_device/devices/dynamixel/XM-430.device b/robotis_device/devices/dynamixel/XM-430.device index 9f17d37..c3a7bad 100644 --- a/robotis_device/devices/dynamixel/XM-430.device +++ b/robotis_device/devices/dynamixel/XM-430.device @@ -3,7 +3,7 @@ model_name = XM-430 device_type = dynamixel [type info] -current_ratio = 2.69 +torque_to_current_value_ratio = 149.795386991 value_of_0_radian_position = 2048 value_of_min_radian_position = 0 @@ -21,6 +21,9 @@ goal_current_item_name = goal_current position_d_gain_item_name = position_d_gain position_i_gain_item_name = position_i_gain position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain [control table] # addr | item name | length | access | memory | min value | max value | signed @@ -59,14 +62,14 @@ position_p_gain_item_name = position_p_gain 104 | goal_velocity | 4 | RW | RAM | 0 | 1023 | N 108 | profile_acceleration | 4 | RW | RAM | 0 | 32767 | N 112 | profile_velocity | 4 | RW | RAM | 0 | 1023 | N - 116 | goal_position | 4 | RW | RAM | 0 | 4095 | N + 116 | goal_position | 4 | RW | RAM | -1048575 | 1048575 | Y 120 | realtime_tick | 2 | R | RAM | 0 | 32767 | N 122 | moving | 1 | R | RAM | 0 | 1 | N 123 | moving_status | 1 | R | RAM | 0 | 255 | N 124 | present_pwm | 2 | R | RAM | 0 | 885 | N 126 | present_current | 2 | R | RAM | 0 | 1193 | N 128 | present_velocity | 4 | R | RAM | 0 | 1023 | N - 132 | present_position | 4 | R | RAM | 0 | 4095 | N + 132 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y 136 | velocity_trajectory | 4 | R | RAM | 0 | 1023 | N 140 | position_trajectory | 4 | R | RAM | 0 | 4095 | N 144 | present_input_voltage | 2 | R | RAM | 95 | 160 | N diff --git a/robotis_device/include/robotis_device/ControlTableItem.h b/robotis_device/include/robotis_device/ControlTableItem.h deleted file mode 100644 index e7b75fd..0000000 --- a/robotis_device/include/robotis_device/ControlTableItem.h +++ /dev/null @@ -1,54 +0,0 @@ -/* - * ControlTableItem.h - * - * Created on: 2015. 12. 16. - * Author: zerom - */ - -#ifndef ROBOTIS_FRAMEWORK_ROBOTIS_DEVICE_INCLUDE_DEVICE_CONTROLTABLEITEM_H_ -#define ROBOTIS_FRAMEWORK_ROBOTIS_DEVICE_INCLUDE_DEVICE_CONTROLTABLEITEM_H_ - - -#include - -namespace ROBOTIS -{ - -enum ACCESS_TYPE { - READ, - READ_WRITE -}; - -enum MEMORY_TYPE { - EEPROM, - RAM -}; - -class ControlTableItem -{ -public: - std::string item_name; - UINT16_T address; - ACCESS_TYPE access_type; - MEMORY_TYPE memory_type; - UINT8_T data_length; - INT32_T data_min_value; - INT32_T data_max_value; - bool is_signed; - - ControlTableItem() - : item_name(""), - address(0), - access_type(READ), - memory_type(RAM), - data_length(0), - data_min_value(0), - data_max_value(0), - is_signed(false) - { } -}; - -} - - -#endif /* ROBOTIS_FRAMEWORK_ROBOTIS_DEVICE_INCLUDE_DEVICE_CONTROLTABLEITEM_H_ */ diff --git a/robotis_device/include/robotis_device/Device.h b/robotis_device/include/robotis_device/Device.h deleted file mode 100644 index d9fd0bf..0000000 --- a/robotis_device/include/robotis_device/Device.h +++ /dev/null @@ -1,37 +0,0 @@ -/* - * Device.h - * - * Created on: 2016. 5. 12. - * Author: zerom - */ - -#ifndef ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_DEVICE_H_ -#define ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_DEVICE_H_ - - -#include -#include -#include -#include "ControlTableItem.h" - -namespace ROBOTIS -{ - -class Device -{ -public: - UINT8_T id; - float protocol_version; - std::string model_name; - std::string port_name; - - std::map ctrl_table; - std::vector bulk_read_items; - - virtual ~Device() { } -}; - -} - - -#endif /* ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_DEVICE_H_ */ diff --git a/robotis_device/include/robotis_device/Dynamixel.h b/robotis_device/include/robotis_device/Dynamixel.h deleted file mode 100644 index 64d4a4a..0000000 --- a/robotis_device/include/robotis_device/Dynamixel.h +++ /dev/null @@ -1,60 +0,0 @@ -/* - * Dynamixel.h - * - * Created on: 2015. 12. 8. - * Author: zerom - */ - -#ifndef ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_DYNAMIXEL_H_ -#define ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_DYNAMIXEL_H_ - - -#include -#include -#include -#include "Device.h" -#include "DynamixelState.h" -#include "ControlTableItem.h" - -namespace ROBOTIS -{ - -class Dynamixel : public Device -{ -public: - std::string ctrl_module_name; - DynamixelState *dxl_state; - - double current_ratio; - double velocity_ratio; - - INT32_T value_of_0_radian_position; - INT32_T value_of_min_radian_position; - INT32_T value_of_max_radian_position; - double min_radian; - double max_radian; - - ControlTableItem *torque_enable_item; - ControlTableItem *present_position_item; - ControlTableItem *present_velocity_item; - ControlTableItem *present_current_item; - ControlTableItem *goal_position_item; - ControlTableItem *goal_velocity_item; - ControlTableItem *goal_current_item; - - Dynamixel(int id, std::string model_name, float protocol_version); - - double ConvertValue2Radian(INT32_T value); - INT32_T ConvertRadian2Value(double radian); - - double ConvertValue2Velocity(INT32_T value); - INT32_T ConvertVelocity2Value(double velocity); - - double ConvertValue2Current(INT16_T value); - INT16_T ConvertCurrent2Value(double torque); -}; - -} - - -#endif /* ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_DYNAMIXEL_H_ */ diff --git a/robotis_device/include/robotis_device/DynamixelState.h b/robotis_device/include/robotis_device/DynamixelState.h deleted file mode 100644 index 29212c4..0000000 --- a/robotis_device/include/robotis_device/DynamixelState.h +++ /dev/null @@ -1,55 +0,0 @@ -/* - * DynamixelState.h - * - * Created on: 2015. 12. 8. - * Author: zerom - */ - -#ifndef ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_DYNAMIXELSTATE_H_ -#define ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_DYNAMIXELSTATE_H_ - -#include - -#include "robotis_device/TimeStamp.h" -#include "robotis_framework_common/RobotisDef.h" - -#define INDIRECT_DATA_1 "indirect_data_1" -#define INDIRECT_ADDRESS_1 "indirect_address_1" - -namespace ROBOTIS -{ - -class DynamixelState -{ -public: - TimeStamp update_time_stamp; - - double present_position; - double present_velocity; - double present_current; - double goal_position; - double goal_velocity; - double goal_current; - - std::map bulk_read_table; - - double position_offset; - - DynamixelState() - : update_time_stamp(0, 0), - present_position(0.0), - present_velocity(0.0), - present_current(0.0), - goal_position(0.0), - goal_velocity(0.0), - goal_current(0.0), - position_offset(0.0) - { - bulk_read_table.clear(); - } -}; - -} - - -#endif /* ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_DYNAMIXELSTATE_H_ */ diff --git a/robotis_device/include/robotis_device/Robot.h b/robotis_device/include/robotis_device/Robot.h deleted file mode 100644 index 2473e7c..0000000 --- a/robotis_device/include/robotis_device/Robot.h +++ /dev/null @@ -1,38 +0,0 @@ -/* - * Robot.h - * - * Created on: 2015. 12. 11. - * Author: zerom - */ - -#ifndef ROBOTIS_FRAMEWORK_ROBOTIS_DEVICE_INCLUDE_DEVICE_ROBOT_H_ -#define ROBOTIS_FRAMEWORK_ROBOTIS_DEVICE_INCLUDE_DEVICE_ROBOT_H_ - - -#include -#include "Sensor.h" -#include "Dynamixel.h" -#include "dynamixel_sdk/PortHandler.h" - -namespace ROBOTIS -{ - -class Robot -{ -public: - std::map ports; // string: port name - std::map port_default_device; // port name, default device name - - std::map dxls; // string: joint name - std::map sensors; // string: sensor name - - Robot(std::string robot_file_path, std::string dev_desc_dir_path); - - Sensor *getSensor(std::string path, int id, std::string port, float protocol_version); - Dynamixel *getDynamixel(std::string path, int id, std::string port, float protocol_version); -}; - -} - - -#endif /* ROBOTIS_FRAMEWORK_ROBOTIS_DEVICE_INCLUDE_DEVICE_ROBOT_H_ */ diff --git a/robotis_device/include/robotis_device/Sensor.h b/robotis_device/include/robotis_device/Sensor.h deleted file mode 100644 index 54df44f..0000000 --- a/robotis_device/include/robotis_device/Sensor.h +++ /dev/null @@ -1,32 +0,0 @@ -/* - * Sensor.h - * - * Created on: 2015. 12. 16. - * Author: zerom - */ - -#ifndef ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_SENSOR_H_ -#define ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_SENSOR_H_ - -#include -#include -#include -#include "Device.h" -#include "SensorState.h" -#include "ControlTableItem.h" - -namespace ROBOTIS -{ - -class Sensor : public Device -{ -public: - SensorState *sensor_state; - - Sensor(int id, std::string model_name, float protocol_version); -}; - -} - - -#endif /* ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_SENSOR_H_ */ diff --git a/robotis_device/include/robotis_device/SensorState.h b/robotis_device/include/robotis_device/SensorState.h deleted file mode 100644 index 4137b14..0000000 --- a/robotis_device/include/robotis_device/SensorState.h +++ /dev/null @@ -1,35 +0,0 @@ -/* - * SensorState.h - * - * Created on: 2016. 5. 16. - * Author: zerom - */ - -#ifndef ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_SENSORSTATE_H_ -#define ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_SENSORSTATE_H_ - - -#include "robotis_device/TimeStamp.h" -#include "robotis_framework_common/RobotisDef.h" - -namespace ROBOTIS -{ - -class SensorState -{ -public: - TimeStamp update_time_stamp; - - std::map bulk_read_table; - - SensorState() - : update_time_stamp(0, 0) - { - bulk_read_table.clear(); - } -}; - -} - - -#endif /* ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_SENSORSTATE_H_ */ diff --git a/robotis_device/include/robotis_device/TimeStamp.h b/robotis_device/include/robotis_device/TimeStamp.h deleted file mode 100644 index 17b749a..0000000 --- a/robotis_device/include/robotis_device/TimeStamp.h +++ /dev/null @@ -1,25 +0,0 @@ -/* - * TimeStamp.h - * - * Created on: 2016. 5. 16. - * Author: zerom - */ - -#ifndef ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_TIMESTAMP_H_ -#define ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_TIMESTAMP_H_ - - -namespace ROBOTIS -{ - -class TimeStamp { -public: - long sec; - long nsec; - TimeStamp(long sec, long nsec) : sec(sec), nsec(nsec) { } -}; - -} - - -#endif /* ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_TIMESTAMP_H_ */ diff --git a/robotis_device/include/robotis_device/control_table_item.h b/robotis_device/include/robotis_device/control_table_item.h new file mode 100644 index 0000000..28a840a --- /dev/null +++ b/robotis_device/include/robotis_device/control_table_item.h @@ -0,0 +1,84 @@ +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * 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 ROBOTIS 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. + *******************************************************************************/ + +/* + * control_table_item.h + * + * Created on: 2015. 12. 16. + * Author: zerom + */ + +#ifndef ROBOTIS_DEVICE_CONTROL_TABLE_ITEM_H_ +#define ROBOTIS_DEVICE_CONTROL_TABLE_ITEM_H_ + + +#include + +namespace robotis_framework +{ + +enum AccessType { + Read, + ReadWrite +}; + +enum MemoryType { + EEPROM, + RAM +}; + +class ControlTableItem +{ +public: + std::string item_name_; + uint16_t address_; + AccessType access_type_; + MemoryType memory_type_; + uint8_t data_length_; + int32_t data_min_value_; + int32_t data_max_value_; + bool is_signed_; + + ControlTableItem() + : item_name_(""), + address_(0), + access_type_(Read), + memory_type_(RAM), + data_length_(0), + data_min_value_(0), + data_max_value_(0), + is_signed_(false) + { } +}; + +} + + +#endif /* ROBOTIS_DEVICE_CONTROL_TABLE_ITEM_H_ */ diff --git a/robotis_device/include/robotis_device/device.h b/robotis_device/include/robotis_device/device.h new file mode 100644 index 0000000..0a5556e --- /dev/null +++ b/robotis_device/include/robotis_device/device.h @@ -0,0 +1,68 @@ +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * 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 ROBOTIS 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. + *******************************************************************************/ + +/* + * device.h + * + * Created on: 2016. 5. 12. + * Author: zerom + */ + +#ifndef ROBOTIS_DEVICE_DEVICE_H_ +#define ROBOTIS_DEVICE_DEVICE_H_ + + +#include +#include +#include + +#include "control_table_item.h" + +namespace robotis_framework +{ + +class Device +{ +public: + uint8_t id_; + float protocol_version_; + std::string model_name_; + std::string port_name_; + + std::map ctrl_table_; + std::vector bulk_read_items_; + + virtual ~Device() { } +}; + +} + + +#endif /* ROBOTIS_DEVICE_DEVICE_H_ */ diff --git a/robotis_device/include/robotis_device/dynamixel.h b/robotis_device/include/robotis_device/dynamixel.h new file mode 100644 index 0000000..adff01b --- /dev/null +++ b/robotis_device/include/robotis_device/dynamixel.h @@ -0,0 +1,97 @@ +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * 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 ROBOTIS 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. + *******************************************************************************/ + +/* + * dynamixel.h + * + * Created on: 2015. 12. 8. + * Author: zerom + */ + +#ifndef ROBOTIS_DEVICE_DYNAMIXEL_H_ +#define ROBOTIS_DEVICE_DYNAMIXEL_H_ + + +#include +#include +#include + +#include "control_table_item.h" +#include "device.h" +#include "dynamixel_state.h" + +namespace robotis_framework +{ + +class Dynamixel : public Device +{ +public: + std::string ctrl_module_name_; + DynamixelState *dxl_state_; + + double velocity_to_value_ratio_; + double torque_to_current_value_ratio_; + + int32_t value_of_0_radian_position_; + int32_t value_of_min_radian_position_; + int32_t value_of_max_radian_position_; + double min_radian_; + double max_radian_; + + ControlTableItem *torque_enable_item_; + ControlTableItem *present_position_item_; + ControlTableItem *present_velocity_item_; + ControlTableItem *present_current_item_; + ControlTableItem *goal_position_item_; + ControlTableItem *goal_velocity_item_; + ControlTableItem *goal_current_item_; + ControlTableItem *position_p_gain_item_; + ControlTableItem *position_i_gain_item_; + ControlTableItem *position_d_gain_item_; + ControlTableItem *velocity_p_gain_item_; + ControlTableItem *velocity_i_gain_item_; + ControlTableItem *velocity_d_gain_item_; + + Dynamixel(int id, std::string model_name, float protocol_version); + + double convertValue2Radian(int32_t value); + int32_t convertRadian2Value(double radian); + + double convertValue2Velocity(int32_t value); + int32_t convertVelocity2Value(double velocity); + + double convertValue2Torque(int16_t value); + int16_t convertTorque2Value(double torque); +}; + +} + + +#endif /* ROBOTIS_DEVICE_DYNAMIXEL_H_ */ diff --git a/robotis_device/include/robotis_device/dynamixel_state.h b/robotis_device/include/robotis_device/dynamixel_state.h new file mode 100644 index 0000000..5f4ae36 --- /dev/null +++ b/robotis_device/include/robotis_device/dynamixel_state.h @@ -0,0 +1,96 @@ +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * 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 ROBOTIS 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. + *******************************************************************************/ + +/* + * dynamixel_state.h + * + * Created on: 2015. 12. 8. + * Author: zerom + */ + +#ifndef ROBOTIS_DEVICE_DYNAMIXEL_STATE_H_ +#define ROBOTIS_DEVICE_DYNAMIXEL_STATE_H_ + +#include + +#include "time_stamp.h" + +#define INDIRECT_DATA_1 "indirect_data_1" +#define INDIRECT_ADDRESS_1 "indirect_address_1" + +namespace robotis_framework +{ + +class DynamixelState +{ +public: + TimeStamp update_time_stamp_; + + double present_position_; + double present_velocity_; + double present_torque_; + double goal_position_; + double goal_velocity_; + double goal_torque_; + double position_p_gain_; + double position_i_gain_; + double position_d_gain_; + double velocity_p_gain_; + double velocity_i_gain_; + double velocity_d_gain_; + + std::map bulk_read_table_; + + double position_offset_; + + DynamixelState() + : update_time_stamp_(0, 0), + present_position_(0.0), + present_velocity_(0.0), + present_torque_(0.0), + goal_position_(0.0), + goal_velocity_(0.0), + goal_torque_(0.0), + position_p_gain_(0.0), + position_i_gain_(0.0), + position_d_gain_(0.0), + velocity_p_gain_(0.0), + velocity_i_gain_(0.0), + velocity_d_gain_(0.0), + position_offset_(0.0) + { + bulk_read_table_.clear(); + } +}; + +} + + +#endif /* ROBOTIS_DEVICE_DYNAMIXEL_STATE_H_ */ diff --git a/robotis_device/include/robotis_device/robot.h b/robotis_device/include/robotis_device/robot.h new file mode 100644 index 0000000..4dfe5bc --- /dev/null +++ b/robotis_device/include/robotis_device/robot.h @@ -0,0 +1,78 @@ +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * 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 ROBOTIS 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. + *******************************************************************************/ + +/* + * robot.h + * + * Created on: 2015. 12. 11. + * Author: zerom + */ + +#ifndef ROBOTIS_DEVICE_ROBOT_H_ +#define ROBOTIS_DEVICE_ROBOT_H_ + + +#include + +#include "sensor.h" +#include "dynamixel.h" +#include "dynamixel_sdk/port_handler.h" + +#define DYNAMIXEL "dynamixel" +#define SENSOR "sensor" + +#define SESSION_PORT_INFO "port info" +#define SESSION_DEVICE_INFO "device info" + +#define SESSION_TYPE_INFO "type info" +#define SESSION_CONTROL_TABLE "control table" + +namespace robotis_framework +{ + +class Robot +{ +public: + std::map ports_; // string: port name + std::map port_default_device_; // port name, default device name + + std::map dxls_; // string: joint name + std::map sensors_; // string: sensor name + + Robot(std::string robot_file_path, std::string dev_desc_dir_path); + + Sensor *getSensor(std::string path, int id, std::string port, float protocol_version); + Dynamixel *getDynamixel(std::string path, int id, std::string port, float protocol_version); +}; + +} + + +#endif /* ROBOTIS_DEVICE_ROBOT_H_ */ diff --git a/robotis_device/include/robotis_device/sensor.h b/robotis_device/include/robotis_device/sensor.h new file mode 100644 index 0000000..2d1b757 --- /dev/null +++ b/robotis_device/include/robotis_device/sensor.h @@ -0,0 +1,63 @@ +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * 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 ROBOTIS 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. + *******************************************************************************/ + +/* + * sensor.h + * + * Created on: 2015. 12. 16. + * Author: zerom + */ + +#ifndef ROBOTIS_DEVICE_SENSOR_H_ +#define ROBOTIS_DEVICE_SENSOR_H_ + +#include +#include +#include + +#include "device.h" +#include "sensor_state.h" +#include "control_table_item.h" + +namespace robotis_framework +{ + +class Sensor : public Device +{ +public: + SensorState *sensor_state_; + + Sensor(int id, std::string model_name, float protocol_version); +}; + +} + + +#endif /* ROBOTIS_DEVICE_SENSOR_H_ */ diff --git a/robotis_device/include/robotis_device/sensor_state.h b/robotis_device/include/robotis_device/sensor_state.h new file mode 100644 index 0000000..f40afa4 --- /dev/null +++ b/robotis_device/include/robotis_device/sensor_state.h @@ -0,0 +1,64 @@ +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * 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 ROBOTIS 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. + *******************************************************************************/ + +/* + * sensor_state.h + * + * Created on: 2016. 5. 16. + * Author: zerom + */ + +#ifndef ROBOTIS_DEVICE_SENSOR_STATE_H_ +#define ROBOTIS_DEVICE_SENSOR_STATE_H_ + + +#include "time_stamp.h" + +namespace robotis_framework +{ + +class SensorState +{ +public: + TimeStamp update_time_stamp_; + + std::map bulk_read_table_; + + SensorState() + : update_time_stamp_(0, 0) + { + bulk_read_table_.clear(); + } +}; + +} + + +#endif /* ROBOTIS_DEVICE_SENSOR_STATE_H_ */ diff --git a/robotis_device/include/robotis_device/time_stamp.h b/robotis_device/include/robotis_device/time_stamp.h new file mode 100644 index 0000000..b0de1e5 --- /dev/null +++ b/robotis_device/include/robotis_device/time_stamp.h @@ -0,0 +1,59 @@ +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * 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 ROBOTIS 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. + *******************************************************************************/ + +/* + * time_stamp.h + * + * Created on: 2016. 5. 16. + * Author: zerom + */ + +#ifndef ROBOTIS_DEVICE_TIME_STAMP_H_ +#define ROBOTIS_DEVICE_TIME_STAMP_H_ + + +namespace robotis_framework +{ + +class TimeStamp { +public: + long sec_; + long nsec_; + + TimeStamp(long sec, long nsec) + : sec_(sec), + nsec_(nsec) + { } +}; + +} + + +#endif /* ROBOTIS_DEVICE_TIME_STAMP_H_ */ diff --git a/robotis_device/package.xml b/robotis_device/package.xml index be72665..6f7294a 100644 --- a/robotis_device/package.xml +++ b/robotis_device/package.xml @@ -6,29 +6,19 @@ robotis - BSD - - robotis - catkin roscpp rospy dynamixel_sdk - robotis_framework_common roscpp rospy dynamixel_sdk - - - - - \ No newline at end of file diff --git a/robotis_device/src/robotis_device/Dynamixel.cpp b/robotis_device/src/robotis_device/Dynamixel.cpp deleted file mode 100644 index daf454d..0000000 --- a/robotis_device/src/robotis_device/Dynamixel.cpp +++ /dev/null @@ -1,110 +0,0 @@ -/* - * Dynamixel.cpp - * - * Created on: 2015. 12. 8. - * Author: zerom - */ - -#include "robotis_device/Dynamixel.h" - -using namespace ROBOTIS; - -Dynamixel::Dynamixel(int id, std::string model_name, float protocol_version) - : ctrl_module_name("none"), - current_ratio(1.0), - velocity_ratio(1.0), - value_of_0_radian_position(0), - value_of_min_radian_position(0), - value_of_max_radian_position(0), - min_radian(-3.14159265), - max_radian(3.14159265), - torque_enable_item(0), - present_position_item(0), - present_velocity_item(0), - present_current_item(0), - goal_position_item(0), - goal_velocity_item(0), - goal_current_item(0) -{ - this->id = id; - this->model_name = model_name; - this->port_name = ""; - this->protocol_version = protocol_version; - - ctrl_table.clear(); - dxl_state = new DynamixelState(); - - bulk_read_items.clear(); -} - -double Dynamixel::ConvertValue2Radian(INT32_T value) -{ - double _radian = 0.0; - if(value > value_of_0_radian_position) - { - if(max_radian <= 0) - return max_radian; - _radian = (double)(value - value_of_0_radian_position) * max_radian / (double)(value_of_max_radian_position - value_of_0_radian_position); - } - else if(value < value_of_0_radian_position) - { - if(min_radian >= 0) - return min_radian; - _radian = (double)(value - value_of_0_radian_position) * min_radian / (double)(value_of_min_radian_position - value_of_0_radian_position); - } - - if(_radian > max_radian) - return max_radian; - else if(_radian < min_radian) - return min_radian; - - return _radian; -} - -INT32_T Dynamixel::ConvertRadian2Value(double radian) -{ - //return radian * value_of_max_radian_position / max_radian; - - INT32_T _value = 0; - if(radian > 0) - { - if(value_of_max_radian_position <= value_of_0_radian_position) - return value_of_max_radian_position; - _value = (radian * (value_of_max_radian_position - value_of_0_radian_position) / max_radian) + value_of_0_radian_position; - } - else if(radian < 0) - { - if(value_of_min_radian_position >= value_of_0_radian_position) - return value_of_min_radian_position; - _value = (radian * (value_of_min_radian_position - value_of_0_radian_position) / min_radian) + value_of_0_radian_position; - } - else - _value = value_of_0_radian_position; - - if(_value > value_of_max_radian_position) - return value_of_max_radian_position; - else if(_value < value_of_min_radian_position) - return value_of_min_radian_position; - - return _value; -} - -double Dynamixel::ConvertValue2Velocity(INT32_T value) -{ - return (double)value * velocity_ratio; -} - -INT32_T Dynamixel::ConvertVelocity2Value(double velocity) -{ - return (INT32_T)(velocity / velocity_ratio);; -} - -double Dynamixel::ConvertValue2Current(INT16_T value) -{ - return (double)value * current_ratio; -} - -INT16_T Dynamixel::ConvertCurrent2Value(double torque) -{ - return (INT16_T)(torque / current_ratio); -} diff --git a/robotis_device/src/robotis_device/Robot.cpp b/robotis_device/src/robotis_device/Robot.cpp deleted file mode 100644 index dc1b50e..0000000 --- a/robotis_device/src/robotis_device/Robot.cpp +++ /dev/null @@ -1,408 +0,0 @@ -/* - * Robot.cpp - * - * Created on: 2015. 12. 11. - * Author: zerom - */ - -#include -#include -#include -#include "../include/robotis_device/Robot.h" - -using namespace ROBOTIS; - -static inline std::string <rim(std::string &s) { - s.erase(s.begin(), std::find_if(s.begin(), s.end(), std::not1(std::ptr_fun(std::isspace)))); - return s; -} -static inline std::string &rtrim(std::string &s) { - s.erase(std::find_if(s.rbegin(), s.rend(), std::not1(std::ptr_fun(std::isspace))).base(), s.end()); - return s; -} -static inline std::string &trim(std::string &s) { - return ltrim(rtrim(s)); -} - -static inline std::vector split(const std::string &text, char sep) { - std::vector tokens; - std::size_t start = 0, end = 0; - while((end = text.find(sep, start)) != (std::string::npos)) { - tokens.push_back(text.substr(start, end - start)); - trim(tokens.back()); - start = end + 1; - } - tokens.push_back(text.substr(start)); - trim(tokens.back()); - return tokens; -} - -Robot::Robot(std::string robot_file_path, std::string dev_desc_dir_path) -{ - if(dev_desc_dir_path.compare(dev_desc_dir_path.size()-1, 1, "/") != 0) - dev_desc_dir_path += "/"; - - std::ifstream file(robot_file_path.c_str()); - if(file.is_open()) - { - std::string session = ""; - std::string input_str; - while(!file.eof()) - { - std::getline(file, input_str); - - // remove comment ( # ) - std::size_t pos = input_str.find("#"); - if(pos != std::string::npos) - input_str = input_str.substr(0, pos); - - // trim - input_str = trim(input_str); - - // find session - if(!input_str.compare(0, 1, "[") && !input_str.compare(input_str.size()-1, 1, "]")) - { - input_str = input_str.substr(1, input_str.size()-2); - std::transform(input_str.begin(), input_str.end(), input_str.begin(), ::tolower); - session = trim(input_str); - continue; - } - - if(session == "port info") - { - std::vector tokens = split(input_str, '|'); - if(tokens.size() != 3) - continue; - - std::cout << tokens[0] << " added. (baudrate: " << tokens[1] << ")" << std::endl; - - ports[tokens[0]] = (PortHandler*)PortHandler::GetPortHandler(tokens[0].c_str()); - ports[tokens[0]]->SetBaudRate(std::atoi(tokens[1].c_str())); - port_default_device[tokens[0]] = tokens[2]; - } - else if(session == "device info") - { - std::vector tokens = split(input_str, '|'); - if(tokens.size() != 7) - continue; - - if(tokens[0] == "dynamixel") - { - std::string _file_path = dev_desc_dir_path + tokens[0] + "/" + tokens[3] + ".device"; - int _id = std::atoi(tokens[2].c_str()); - std::string _port = tokens[1]; - float _protocol = std::atof(tokens[4].c_str()); - std::string _dev_name = tokens[5]; - - dxls[_dev_name] = getDynamixel(_file_path, _id, _port, _protocol); - - Dynamixel *_dxl = dxls[_dev_name]; - std::vector sub_tokens = split(tokens[6], ','); - if(sub_tokens.size() > 0) - { - std::map::iterator _indirect_it = _dxl->ctrl_table.find(INDIRECT_ADDRESS_1); - if(_indirect_it != _dxl->ctrl_table.end()) // INDIRECT_ADDRESS_1 exist - { - UINT16_T _indirect_data_addr = _dxl->ctrl_table[INDIRECT_DATA_1]->address; - for(int _i = 0; _i < sub_tokens.size(); _i++) - { - _dxl->bulk_read_items.push_back(new ControlTableItem()); - ControlTableItem *_dest_item = _dxl->bulk_read_items[_i]; - ControlTableItem *_src_item = _dxl->ctrl_table[sub_tokens[_i]]; - - _dest_item->item_name = _src_item->item_name; - _dest_item->address = _indirect_data_addr; - _dest_item->access_type = _src_item->access_type; - _dest_item->memory_type = _src_item->memory_type; - _dest_item->data_length = _src_item->data_length; - _dest_item->data_min_value = _src_item->data_min_value; - _dest_item->data_max_value = _src_item->data_max_value; - _dest_item->is_signed = _src_item->is_signed; - - _indirect_data_addr += _dest_item->data_length; - } - } - else // INDIRECT_ADDRESS_1 exist - { - for(int _i = 0; _i < sub_tokens.size(); _i++) - _dxl->bulk_read_items.push_back(_dxl->ctrl_table[sub_tokens[_i]]); - } - } - } - else if(tokens[0] == "sensor") - { - std::string _file_path = dev_desc_dir_path + tokens[0] + "/" + tokens[3] + ".device"; - int _id = std::atoi(tokens[2].c_str()); - std::string _port = tokens[1]; - float _protocol = std::atof(tokens[4].c_str()); - std::string _dev_name = tokens[5]; - - sensors[_dev_name] = getSensor(_file_path, _id, _port, _protocol); - - Sensor *_sensor = sensors[_dev_name]; - std::vector sub_tokens = split(tokens[6], ','); - if(sub_tokens.size() > 0) - { - std::map::iterator _indirect_it = _sensor->ctrl_table.find(INDIRECT_ADDRESS_1); - if(_indirect_it != _sensor->ctrl_table.end()) // INDIRECT_ADDRESS_1 exist - { - UINT16_T _indirect_data_addr = _sensor->ctrl_table[INDIRECT_DATA_1]->address; - for(int _i = 0; _i < sub_tokens.size(); _i++) - { - _sensor->bulk_read_items.push_back(new ControlTableItem()); - ControlTableItem *_dest_item = _sensor->bulk_read_items[_i]; - ControlTableItem *_src_item = _sensor->ctrl_table[sub_tokens[_i]]; - - _dest_item->item_name = _src_item->item_name; - _dest_item->address = _indirect_data_addr; - _dest_item->access_type = _src_item->access_type; - _dest_item->memory_type = _src_item->memory_type; - _dest_item->data_length = _src_item->data_length; - _dest_item->data_min_value = _src_item->data_min_value; - _dest_item->data_max_value = _src_item->data_max_value; - _dest_item->is_signed = _src_item->is_signed; - - _indirect_data_addr += _dest_item->data_length; - } - } - else // INDIRECT_ADDRESS_1 exist - { - for(int _i = 0; _i < sub_tokens.size(); _i++) - _sensor->bulk_read_items.push_back(_sensor->ctrl_table[sub_tokens[_i]]); - } - } - } - } - } - file.close(); - } - else - { - std::cout << "Unable to open file : " + robot_file_path << std::endl; - } -} - -Sensor *Robot::getSensor(std::string path, int id, std::string port, float protocol_version) -{ - Sensor *_sensor; - - // load file model_name.device - std::ifstream file(path.c_str()); - if(file.is_open()) - { - std::string _session = ""; - std::string _input_str; - - while(!file.eof()) - { - std::getline(file, _input_str); - - // remove comment ( # ) - std::size_t pos = _input_str.find("#"); - if(pos != std::string::npos) - _input_str = _input_str.substr(0, pos); - - // trim - _input_str = trim(_input_str); - if(_input_str == "") - continue; - - // find _session - if(!_input_str.compare(0, 1, "[") && !_input_str.compare(_input_str.size()-1, 1, "]")) - { - _input_str = _input_str.substr(1, _input_str.size()-2); - std::transform(_input_str.begin(), _input_str.end(), _input_str.begin(), ::tolower); - _session = trim(_input_str); - continue; - } - - if(_session == "device info") - { - std::vector tokens = split(_input_str, '='); - if(tokens.size() != 2) - continue; - - if(tokens[0] == "model_name") - _sensor = new Sensor(id, tokens[1], protocol_version); - } - else if(_session == "control table") - { - std::vector tokens = split(_input_str, '|'); - if(tokens.size() != 8) - continue; - - ControlTableItem *item = new ControlTableItem(); - item->item_name = tokens[1]; - item->address = std::atoi(tokens[0].c_str()); - item->data_length = std::atoi(tokens[2].c_str()); - if(tokens[3] == "R") - item->access_type = READ; - else if(tokens[3] == "RW") - item->access_type = READ_WRITE; - if(tokens[4] == "EEPROM") - item->memory_type = EEPROM; - else if(tokens[4] == "RAM") - item->memory_type = RAM; - item->data_min_value = std::atol(tokens[5].c_str()); - item->data_max_value = std::atol(tokens[6].c_str()); - if(tokens[7] == "Y") - item->is_signed = true; - else if(tokens[7] == "N") - item->is_signed = false; - _sensor->ctrl_table[tokens[1]] = item; - } - } - _sensor->port_name = port; - - fprintf(stderr, "(%s) [ID:%3d] %14s added. \n", port.c_str(), _sensor->id, _sensor->model_name.c_str()); - //std::cout << "[ID:" << (int)(_sensor->id) << "] " << _sensor->model_name << " added. (" << port << ")" << std::endl; - file.close(); - } - else - std::cout << "Unable to open file : " + path << std::endl; - - return _sensor; -} - -Dynamixel *Robot::getDynamixel(std::string path, int id, std::string port, float protocol_version) -{ - Dynamixel *_dxl; - - // load file model_name.device - std::ifstream file(path.c_str()); - if(file.is_open()) - { - std::string _session = ""; - std::string _input_str; - - std::string _torque_enable_item_name = ""; - std::string _present_position_item_name = ""; - std::string _present_velocity_item_name = ""; - std::string _present_current_item_name = ""; - std::string _goal_position_item_name = ""; - std::string _goal_velocity_item_name = ""; - std::string _goal_current_item_name = ""; - - while(!file.eof()) - { - std::getline(file, _input_str); - - // remove comment ( # ) - std::size_t pos = _input_str.find("#"); - if(pos != std::string::npos) - _input_str = _input_str.substr(0, pos); - - // trim - _input_str = trim(_input_str); - if(_input_str == "") - continue; - - // find _session - if(!_input_str.compare(0, 1, "[") && !_input_str.compare(_input_str.size()-1, 1, "]")) - { - _input_str = _input_str.substr(1, _input_str.size()-2); - std::transform(_input_str.begin(), _input_str.end(), _input_str.begin(), ::tolower); - _session = trim(_input_str); - continue; - } - - if(_session == "device info") - { - std::vector tokens = split(_input_str, '='); - if(tokens.size() != 2) - continue; - - if(tokens[0] == "model_name") - _dxl = new Dynamixel(id, tokens[1], protocol_version); - } - else if(_session == "type info") - { - std::vector tokens = split(_input_str, '='); - if(tokens.size() != 2) - continue; - - if(tokens[0] == "current_ratio") - _dxl->current_ratio = std::atof(tokens[1].c_str()); - else if(tokens[0] == "velocity_ratio") - _dxl->velocity_ratio = std::atof(tokens[1].c_str()); - - else if(tokens[0] == "value_of_0_radian_position") - _dxl->value_of_0_radian_position = std::atoi(tokens[1].c_str()); - else if(tokens[0] == "value_of_min_radian_position") - _dxl->value_of_min_radian_position = std::atoi(tokens[1].c_str()); - else if(tokens[0] == "value_of_max_radian_position") - _dxl->value_of_max_radian_position = std::atoi(tokens[1].c_str()); - else if(tokens[0] == "min_radian") - _dxl->min_radian = std::atof(tokens[1].c_str()); - else if(tokens[0] == "max_radian") - _dxl->max_radian = std::atof(tokens[1].c_str()); - - else if(tokens[0] == "torque_enable_item_name") - _torque_enable_item_name = tokens[1]; - else if(tokens[0] == "present_position_item_name") - _present_position_item_name = tokens[1]; - else if(tokens[0] == "present_velocity_item_name") - _present_velocity_item_name = tokens[1]; - else if(tokens[0] == "present_current_item_name") - _present_current_item_name = tokens[1]; - else if(tokens[0] == "goal_position_item_name") - _goal_position_item_name = tokens[1]; - else if(tokens[0] == "goal_velocity_item_name") - _goal_velocity_item_name = tokens[1]; - else if(tokens[0] == "goal_current_item_name") - _goal_current_item_name = tokens[1]; - } - else if(_session == "control table") - { - std::vector tokens = split(_input_str, '|'); - if(tokens.size() != 8) - continue; - - ControlTableItem *item = new ControlTableItem(); - item->item_name = tokens[1]; - item->address = std::atoi(tokens[0].c_str()); - item->data_length = std::atoi(tokens[2].c_str()); - if(tokens[3] == "R") - item->access_type = READ; - else if(tokens[3] == "RW") - item->access_type = READ_WRITE; - if(tokens[4] == "EEPROM") - item->memory_type = EEPROM; - else if(tokens[4] == "RAM") - item->memory_type = RAM; - item->data_min_value = std::atol(tokens[5].c_str()); - item->data_max_value = std::atol(tokens[6].c_str()); - if(tokens[7] == "Y") - item->is_signed = true; - else if(tokens[7] == "N") - item->is_signed = false; - _dxl->ctrl_table[tokens[1]] = item; - } - } - _dxl->port_name = port; - - if(_dxl->ctrl_table[_torque_enable_item_name] != NULL) - _dxl->torque_enable_item = _dxl->ctrl_table[_torque_enable_item_name]; - if(_dxl->ctrl_table[_present_position_item_name] != NULL) - _dxl->present_position_item = _dxl->ctrl_table[_present_position_item_name]; - if(_dxl->ctrl_table[_present_velocity_item_name] != NULL) - _dxl->present_velocity_item = _dxl->ctrl_table[_present_velocity_item_name]; - if(_dxl->ctrl_table[_present_current_item_name] != NULL) - _dxl->present_current_item = _dxl->ctrl_table[_present_current_item_name]; - if(_dxl->ctrl_table[_goal_position_item_name] != NULL) - _dxl->goal_position_item = _dxl->ctrl_table[_goal_position_item_name]; - if(_dxl->ctrl_table[_goal_velocity_item_name] != NULL) - _dxl->goal_velocity_item = _dxl->ctrl_table[_goal_velocity_item_name]; - if(_dxl->ctrl_table[_goal_current_item_name] != NULL) - _dxl->goal_current_item = _dxl->ctrl_table[_goal_current_item_name]; - - fprintf(stderr, "(%s) [ID:%3d] %14s added. \n", port.c_str(), _dxl->id, _dxl->model_name.c_str()); - //std::cout << "[ID:" << (int)(_dxl->id) << "] " << _dxl->model_name << " added. (" << port << ")" << std::endl; - file.close(); - } - else - std::cout << "Unable to open file : " + path << std::endl; - - return _dxl; -} - diff --git a/robotis_device/src/robotis_device/Sensor.cpp b/robotis_device/src/robotis_device/Sensor.cpp deleted file mode 100644 index db95494..0000000 --- a/robotis_device/src/robotis_device/Sensor.cpp +++ /dev/null @@ -1,23 +0,0 @@ -/* - * Sensor.cpp - * - * Created on: 2016. 5. 11. - * Author: zerom - */ - -#include "robotis_device/Sensor.h" - -using namespace ROBOTIS; - -Sensor::Sensor(int id, std::string model_name, float protocol_version) -{ - this->id = id; - this->model_name = model_name; - this->port_name = ""; - this->protocol_version = protocol_version; - ctrl_table.clear(); - - sensor_state = new SensorState(); - - bulk_read_items.clear(); -} diff --git a/robotis_device/src/robotis_device/dynamixel.cpp b/robotis_device/src/robotis_device/dynamixel.cpp new file mode 100644 index 0000000..cdd5470 --- /dev/null +++ b/robotis_device/src/robotis_device/dynamixel.cpp @@ -0,0 +1,152 @@ +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * 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 ROBOTIS 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. + *******************************************************************************/ + +/* + * dynamixel.cpp + * + * Created on: 2015. 12. 8. + * Author: zerom + */ + +#include "robotis_device/dynamixel.h" + +using namespace robotis_framework; + +Dynamixel::Dynamixel(int id, std::string model_name, float protocol_version) + : ctrl_module_name_("none"), + torque_to_current_value_ratio_(1.0), + velocity_to_value_ratio_(1.0), + value_of_0_radian_position_(0), + value_of_min_radian_position_(0), + value_of_max_radian_position_(0), + min_radian_(-3.14159265), + max_radian_(3.14159265), + torque_enable_item_(0), + present_position_item_(0), + present_velocity_item_(0), + present_current_item_(0), + goal_position_item_(0), + goal_velocity_item_(0), + goal_current_item_(0), + position_p_gain_item_(0), + position_i_gain_item_(0), + position_d_gain_item_(0), + velocity_p_gain_item_(0), + velocity_i_gain_item_(0), + velocity_d_gain_item_(0) +{ + this->id_ = id; + this->model_name_ = model_name; + this->port_name_ = ""; + this->protocol_version_ = protocol_version; + + ctrl_table_.clear(); + dxl_state_ = new DynamixelState(); + + bulk_read_items_.clear(); +} + +double Dynamixel::convertValue2Radian(int32_t value) +{ + double radian = 0.0; + if (value > value_of_0_radian_position_) + { + if (max_radian_ <= 0) + return max_radian_; + + radian = (double) (value - value_of_0_radian_position_) * max_radian_ + / (double) (value_of_max_radian_position_ - value_of_0_radian_position_); + } + else if (value < value_of_0_radian_position_) + { + if (min_radian_ >= 0) + return min_radian_; + + radian = (double) (value - value_of_0_radian_position_) * min_radian_ + / (double) (value_of_min_radian_position_ - value_of_0_radian_position_); + } + + if (radian > max_radian_) + return max_radian_; + else if (radian < min_radian_) + return min_radian_; + + return radian; +} + +int32_t Dynamixel::convertRadian2Value(double radian) +{ + int32_t value = 0; + if (radian > 0) + { + if (value_of_max_radian_position_ <= value_of_0_radian_position_) + return value_of_max_radian_position_; + + value = (radian * (value_of_max_radian_position_ - value_of_0_radian_position_) / max_radian_) + + value_of_0_radian_position_; + } + else if (radian < 0) + { + if (value_of_min_radian_position_ >= value_of_0_radian_position_) + return value_of_min_radian_position_; + + value = (radian * (value_of_min_radian_position_ - value_of_0_radian_position_) / min_radian_) + + value_of_0_radian_position_; + } + else + value = value_of_0_radian_position_; + + if (value > value_of_max_radian_position_) + return value_of_max_radian_position_; + else if (value < value_of_min_radian_position_) + return value_of_min_radian_position_; + + return value; +} + +double Dynamixel::convertValue2Velocity(int32_t value) +{ + return (double) value / velocity_to_value_ratio_; +} + +int32_t Dynamixel::convertVelocity2Value(double velocity) +{ + return (int32_t) (velocity * velocity_to_value_ratio_);; +} + +double Dynamixel::convertValue2Torque(int16_t value) +{ + return (double) value / torque_to_current_value_ratio_; +} + +int16_t Dynamixel::convertTorque2Value(double torque) +{ + return (int16_t) (torque * torque_to_current_value_ratio_); +} diff --git a/robotis_device/src/robotis_device/robot.cpp b/robotis_device/src/robotis_device/robot.cpp new file mode 100644 index 0000000..900a9b9 --- /dev/null +++ b/robotis_device/src/robotis_device/robot.cpp @@ -0,0 +1,457 @@ +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * 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 ROBOTIS 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. + *******************************************************************************/ + +/* + * robot.cpp + * + * Created on: 2015. 12. 11. + * Author: zerom + */ + +#include +#include +#include + +#include "robotis_device/robot.h" + +using namespace robotis_framework; + +static inline std::string <rim(std::string &s) +{ + s.erase(s.begin(), std::find_if(s.begin(), s.end(), std::not1(std::ptr_fun(std::isspace)))); + return s; +} +static inline std::string &rtrim(std::string &s) +{ + s.erase(std::find_if(s.rbegin(), s.rend(), std::not1(std::ptr_fun(std::isspace))).base(), s.end()); + return s; +} +static inline std::string &trim(std::string &s) +{ + return ltrim(rtrim(s)); +} + +static inline std::vector split(const std::string &text, char sep) +{ + std::vector tokens; + std::size_t start = 0, end = 0; + + while ((end = text.find(sep, start)) != (std::string::npos)) + { + tokens.push_back(text.substr(start, end - start)); + trim(tokens.back()); + start = end + 1; + } + tokens.push_back(text.substr(start)); + trim(tokens.back()); + + return tokens; +} + +Robot::Robot(std::string robot_file_path, std::string dev_desc_dir_path) +{ + if (dev_desc_dir_path.compare(dev_desc_dir_path.size() - 1, 1, "/") != 0) + dev_desc_dir_path += "/"; + + std::ifstream file(robot_file_path.c_str()); + if (file.is_open()) + { + std::string session = ""; + std::string input_str; + while (!file.eof()) + { + std::getline(file, input_str); + + // remove comment ( # ) + std::size_t pos = input_str.find("#"); + if (pos != std::string::npos) + input_str = input_str.substr(0, pos); + + // trim + input_str = trim(input_str); + + // find session + if (!input_str.compare(0, 1, "[") && !input_str.compare(input_str.size() - 1, 1, "]")) + { + input_str = input_str.substr(1, input_str.size() - 2); + std::transform(input_str.begin(), input_str.end(), input_str.begin(), ::tolower); + session = trim(input_str); + continue; + } + + if (session == SESSION_PORT_INFO) + { + std::vector tokens = split(input_str, '|'); + if (tokens.size() != 3) + continue; + + std::cout << tokens[0] << " added. (baudrate: " << tokens[1] << ")" << std::endl; + + ports_[tokens[0]] = dynamixel::PortHandler::getPortHandler(tokens[0].c_str()); + ports_[tokens[0]]->setBaudRate(std::atoi(tokens[1].c_str())); + port_default_device_[tokens[0]] = tokens[2]; + } + else if (session == SESSION_DEVICE_INFO) + { + std::vector tokens = split(input_str, '|'); + if (tokens.size() != 7) + continue; + + if (tokens[0] == DYNAMIXEL) + { + std::string file_path = dev_desc_dir_path + tokens[0] + "/" + tokens[3] + ".device"; + int id = std::atoi(tokens[2].c_str()); + std::string port = tokens[1]; + float protocol = std::atof(tokens[4].c_str()); + std::string dev_name = tokens[5]; + + dxls_[dev_name] = getDynamixel(file_path, id, port, protocol); + + Dynamixel *dxl = dxls_[dev_name]; + std::vector sub_tokens = split(tokens[6], ','); + if (sub_tokens.size() > 0) + { + std::map::iterator indirect_it = dxl->ctrl_table_.find(INDIRECT_ADDRESS_1); + if (indirect_it != dxl->ctrl_table_.end()) // INDIRECT_ADDRESS_1 exist + { + uint16_t indirect_data_addr = dxl->ctrl_table_[INDIRECT_DATA_1]->address_; + for (int _i = 0; _i < sub_tokens.size(); _i++) + { + dxl->bulk_read_items_.push_back(new ControlTableItem()); + ControlTableItem *dest_item = dxl->bulk_read_items_[_i]; + ControlTableItem *src_item = dxl->ctrl_table_[sub_tokens[_i]]; + + dest_item->item_name_ = src_item->item_name_; + dest_item->address_ = indirect_data_addr; + dest_item->access_type_ = src_item->access_type_; + dest_item->memory_type_ = src_item->memory_type_; + dest_item->data_length_ = src_item->data_length_; + dest_item->data_min_value_ = src_item->data_min_value_; + dest_item->data_max_value_ = src_item->data_max_value_; + dest_item->is_signed_ = src_item->is_signed_; + + indirect_data_addr += dest_item->data_length_; + } + } + else // INDIRECT_ADDRESS_1 not exist + { + for (int i = 0; i < sub_tokens.size(); i++) + { + if (dxl->ctrl_table_[sub_tokens[i]] != NULL) + dxl->bulk_read_items_.push_back(dxl->ctrl_table_[sub_tokens[i]]); + } + } + } + } + else if (tokens[0] == SENSOR) + { + std::string file_path = dev_desc_dir_path + tokens[0] + "/" + tokens[3] + ".device"; + int id = std::atoi(tokens[2].c_str()); + std::string port = tokens[1]; + float protocol = std::atof(tokens[4].c_str()); + std::string dev_name = tokens[5]; + + sensors_[dev_name] = getSensor(file_path, id, port, protocol); + + Sensor *sensor = sensors_[dev_name]; + std::vector sub_tokens = split(tokens[6], ','); + if (sub_tokens.size() > 0) + { + std::map::iterator indirect_it = sensor->ctrl_table_.find(INDIRECT_ADDRESS_1); + if (indirect_it != sensor->ctrl_table_.end()) // INDIRECT_ADDRESS_1 exist + { + uint16_t indirect_data_addr = sensor->ctrl_table_[INDIRECT_DATA_1]->address_; + for (int i = 0; i < sub_tokens.size(); i++) + { + sensor->bulk_read_items_.push_back(new ControlTableItem()); + ControlTableItem *dest_item = sensor->bulk_read_items_[i]; + ControlTableItem *src_item = sensor->ctrl_table_[sub_tokens[i]]; + + dest_item->item_name_ = src_item->item_name_; + dest_item->address_ = indirect_data_addr; + dest_item->access_type_ = src_item->access_type_; + dest_item->memory_type_ = src_item->memory_type_; + dest_item->data_length_ = src_item->data_length_; + dest_item->data_min_value_ = src_item->data_min_value_; + dest_item->data_max_value_ = src_item->data_max_value_; + dest_item->is_signed_ = src_item->is_signed_; + + indirect_data_addr += dest_item->data_length_; + } + } + else // INDIRECT_ADDRESS_1 exist + { + for (int i = 0; i < sub_tokens.size(); i++) + sensor->bulk_read_items_.push_back(sensor->ctrl_table_[sub_tokens[i]]); + } + } + } + } + } + file.close(); + } + else + { + std::cout << "Unable to open file : " + robot_file_path << std::endl; + } +} + +Sensor *Robot::getSensor(std::string path, int id, std::string port, float protocol_version) +{ + Sensor *sensor; + + // load file model_name.device + std::ifstream file(path.c_str()); + if (file.is_open()) + { + std::string session = ""; + std::string input_str; + + while (!file.eof()) + { + std::getline(file, input_str); + + // remove comment ( # ) + std::size_t pos = input_str.find("#"); + if (pos != std::string::npos) + input_str = input_str.substr(0, pos); + + // trim + input_str = trim(input_str); + if (input_str == "") + continue; + + // find _session + if (!input_str.compare(0, 1, "[") && !input_str.compare(input_str.size() - 1, 1, "]")) + { + input_str = input_str.substr(1, input_str.size() - 2); + std::transform(input_str.begin(), input_str.end(), input_str.begin(), ::tolower); + session = trim(input_str); + continue; + } + + if (session == SESSION_DEVICE_INFO) + { + std::vector tokens = split(input_str, '='); + if (tokens.size() != 2) + continue; + + if (tokens[0] == "model_name") + sensor = new Sensor(id, tokens[1], protocol_version); + } + else if (session == SESSION_CONTROL_TABLE) + { + std::vector tokens = split(input_str, '|'); + if (tokens.size() != 8) + continue; + + ControlTableItem *item = new ControlTableItem(); + item->item_name_ = tokens[1]; + item->address_ = std::atoi(tokens[0].c_str()); + item->data_length_ = std::atoi(tokens[2].c_str()); + + if (tokens[3] == "R") + item->access_type_ = Read; + else if (tokens[3] == "RW") + item->access_type_ = ReadWrite; + + if (tokens[4] == "EEPROM") + item->memory_type_ = EEPROM; + else if (tokens[4] == "RAM") + item->memory_type_ = RAM; + + item->data_min_value_ = std::atol(tokens[5].c_str()); + item->data_max_value_ = std::atol(tokens[6].c_str()); + + if (tokens[7] == "Y") + item->is_signed_ = true; + else if (tokens[7] == "N") + item->is_signed_ = false; + sensor->ctrl_table_[tokens[1]] = item; + } + } + sensor->port_name_ = port; + + fprintf(stderr, "(%s) [ID:%3d] %14s added. \n", port.c_str(), sensor->id_, sensor->model_name_.c_str()); + //std::cout << "[ID:" << (int)(_sensor->id) << "] " << _sensor->model_name << " added. (" << port << ")" << std::endl; + file.close(); + } + else + std::cout << "Unable to open file : " + path << std::endl; + + return sensor; +} + +Dynamixel *Robot::getDynamixel(std::string path, int id, std::string port, float protocol_version) +{ + Dynamixel *dxl; + + // load file model_name.device + std::ifstream file(path.c_str()); + if (file.is_open()) + { + std::string session = ""; + std::string input_str; + + std::string torque_enable_item_name = ""; + std::string present_position_item_name = ""; + std::string present_velocity_item_name = ""; + std::string present_current_item_name = ""; + std::string goal_position_item_name = ""; + std::string goal_velocity_item_name = ""; + std::string goal_current_item_name = ""; + + while (!file.eof()) + { + std::getline(file, input_str); + + // remove comment ( # ) + std::size_t pos = input_str.find("#"); + if (pos != std::string::npos) + input_str = input_str.substr(0, pos); + + // trim + input_str = trim(input_str); + if (input_str == "") + continue; + + // find session + if (!input_str.compare(0, 1, "[") && !input_str.compare(input_str.size() - 1, 1, "]")) + { + input_str = input_str.substr(1, input_str.size() - 2); + std::transform(input_str.begin(), input_str.end(), input_str.begin(), ::tolower); + session = trim(input_str); + continue; + } + + if (session == SESSION_DEVICE_INFO) + { + std::vector tokens = split(input_str, '='); + if (tokens.size() != 2) + continue; + + if (tokens[0] == "model_name") + dxl = new Dynamixel(id, tokens[1], protocol_version); + } + else if (session == SESSION_TYPE_INFO) + { + std::vector tokens = split(input_str, '='); + if (tokens.size() != 2) + continue; + + if (tokens[0] == "torque_to_current_value_ratio") + dxl->torque_to_current_value_ratio_ = std::atof(tokens[1].c_str()); + else if (tokens[0] == "velocity_to_value_ratio") + dxl->velocity_to_value_ratio_ = std::atof(tokens[1].c_str()); + + else if (tokens[0] == "value_of_0_radian_position") + dxl->value_of_0_radian_position_ = std::atoi(tokens[1].c_str()); + else if (tokens[0] == "value_of_min_radian_position") + dxl->value_of_min_radian_position_ = std::atoi(tokens[1].c_str()); + else if (tokens[0] == "value_of_max_radian_position") + dxl->value_of_max_radian_position_ = std::atoi(tokens[1].c_str()); + else if (tokens[0] == "min_radian") + dxl->min_radian_ = std::atof(tokens[1].c_str()); + else if (tokens[0] == "max_radian") + dxl->max_radian_ = std::atof(tokens[1].c_str()); + + else if (tokens[0] == "torque_enable_item_name") + torque_enable_item_name = tokens[1]; + else if (tokens[0] == "present_position_item_name") + present_position_item_name = tokens[1]; + else if (tokens[0] == "present_velocity_item_name") + present_velocity_item_name = tokens[1]; + else if (tokens[0] == "present_current_item_name") + present_current_item_name = tokens[1]; + else if (tokens[0] == "goal_position_item_name") + goal_position_item_name = tokens[1]; + else if (tokens[0] == "goal_velocity_item_name") + goal_velocity_item_name = tokens[1]; + else if (tokens[0] == "goal_current_item_name") + goal_current_item_name = tokens[1]; + } + else if (session == SESSION_CONTROL_TABLE) + { + std::vector tokens = split(input_str, '|'); + if (tokens.size() != 8) + continue; + + ControlTableItem *item = new ControlTableItem(); + item->item_name_ = tokens[1]; + item->address_ = std::atoi(tokens[0].c_str()); + item->data_length_ = std::atoi(tokens[2].c_str()); + + if (tokens[3] == "R") + item->access_type_ = Read; + else if (tokens[3] == "RW") + item->access_type_ = ReadWrite; + + if (tokens[4] == "EEPROM") + item->memory_type_ = EEPROM; + else if (tokens[4] == "RAM") + item->memory_type_ = RAM; + + item->data_min_value_ = std::atol(tokens[5].c_str()); + item->data_max_value_ = std::atol(tokens[6].c_str()); + + if (tokens[7] == "Y") + item->is_signed_ = true; + else if (tokens[7] == "N") + item->is_signed_ = false; + dxl->ctrl_table_[tokens[1]] = item; + } + } + dxl->port_name_ = port; + + if (dxl->ctrl_table_[torque_enable_item_name] != NULL) + dxl->torque_enable_item_ = dxl->ctrl_table_[torque_enable_item_name]; + if (dxl->ctrl_table_[present_position_item_name] != NULL) + dxl->present_position_item_ = dxl->ctrl_table_[present_position_item_name]; + if (dxl->ctrl_table_[present_velocity_item_name] != NULL) + dxl->present_velocity_item_ = dxl->ctrl_table_[present_velocity_item_name]; + if (dxl->ctrl_table_[present_current_item_name] != NULL) + dxl->present_current_item_ = dxl->ctrl_table_[present_current_item_name]; + if (dxl->ctrl_table_[goal_position_item_name] != NULL) + dxl->goal_position_item_ = dxl->ctrl_table_[goal_position_item_name]; + if (dxl->ctrl_table_[goal_velocity_item_name] != NULL) + dxl->goal_velocity_item_ = dxl->ctrl_table_[goal_velocity_item_name]; + if (dxl->ctrl_table_[goal_current_item_name] != NULL) + dxl->goal_current_item_ = dxl->ctrl_table_[goal_current_item_name]; + + fprintf(stderr, "(%s) [ID:%3d] %14s added. \n", port.c_str(), dxl->id_, dxl->model_name_.c_str()); + //std::cout << "[ID:" << (int)(_dxl->id) << "] " << _dxl->model_name << " added. (" << port << ")" << std::endl; + file.close(); + } + else + std::cout << "Unable to open file : " + path << std::endl; + + return dxl; +} + diff --git a/robotis_device/src/robotis_device/sensor.cpp b/robotis_device/src/robotis_device/sensor.cpp new file mode 100644 index 0000000..537989d --- /dev/null +++ b/robotis_device/src/robotis_device/sensor.cpp @@ -0,0 +1,53 @@ +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * 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 ROBOTIS 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. + *******************************************************************************/ + +/* + * sensor.cpp + * + * Created on: 2016. 5. 11. + * Author: zerom + */ + +#include "robotis_device/sensor.h" + +using namespace robotis_framework; + +Sensor::Sensor(int id, std::string model_name, float protocol_version) +{ + this->id_ = id; + this->model_name_ = model_name; + this->port_name_ = ""; + this->protocol_version_ = protocol_version; + ctrl_table_.clear(); + + sensor_state_ = new SensorState(); + + bulk_read_items_.clear(); +} diff --git a/robotis_framework_common/CMakeLists.txt b/robotis_framework_common/CMakeLists.txt index 17cd2cc..51f0f3f 100644 --- a/robotis_framework_common/CMakeLists.txt +++ b/robotis_framework_common/CMakeLists.txt @@ -5,15 +5,6 @@ find_package(catkin REQUIRED COMPONENTS roscpp ) -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if you package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need catkin_package( INCLUDE_DIRS include # LIBRARIES robotis_framework_common @@ -21,36 +12,7 @@ catkin_package( # DEPENDS system_lib ) -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -# include_directories(include) include_directories( ${catkin_INCLUDE_DIRS} ) -## Declare a C++ library -# add_library(robotis_framework_common -# src/${PROJECT_NAME}/robotis_framework_common.cpp -# ) - -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure -# add_dependencies(robotis_framework_common ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Declare a C++ executable -# add_executable(robotis_framework_common_node src/robotis_framework_common_node.cpp) - -## Add cmake target dependencies of the executable -## same as for the library above -# add_dependencies(robotis_framework_common_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Specify libraries to link a library or executable target against -# target_link_libraries(robotis_framework_common_node -# ${catkin_LIBRARIES} -# ) - diff --git a/robotis_framework_common/include/robotis_framework_common/MotionModule.h b/robotis_framework_common/include/robotis_framework_common/MotionModule.h deleted file mode 100644 index a305a6a..0000000 --- a/robotis_framework_common/include/robotis_framework_common/MotionModule.h +++ /dev/null @@ -1,51 +0,0 @@ -/* - * MotionModule.h - * - * Created on: 2016. 1. 15. - * Author: zerom - */ - -#ifndef ROBOTIS_FRAMEWORK_COMMON_INCLUDE_ROBOTIS_FRAMEWORK_COMMON_MOTIONMODULE_H_ -#define ROBOTIS_FRAMEWORK_COMMON_INCLUDE_ROBOTIS_FRAMEWORK_COMMON_MOTIONMODULE_H_ - - -#include -#include - -#include "robotis_device/Robot.h" -#include "robotis_device/Dynamixel.h" -#include "robotis_framework_common/Singleton.h" - -namespace ROBOTIS -{ - -enum CONTROL_MODE -{ - POSITION_CONTROL, - VELOCITY_CONTROL, - CURRENT_CONTROL -}; - -class MotionModule -{ -public: - bool enable; - std::string module_name; - CONTROL_MODE control_mode; - - std::map result; - - virtual ~MotionModule() { } - - virtual void Initialize(const int control_cycle_msec, Robot *robot) = 0; - virtual void Process(std::map dxls, std::map sensors) = 0; - - virtual void Stop() = 0; - virtual bool IsRunning() = 0; -}; - - -} - - -#endif /* ROBOTIS_FRAMEWORK_COMMON_INCLUDE_ROBOTIS_FRAMEWORK_COMMON_MOTIONMODULE_H_ */ diff --git a/robotis_framework_common/include/robotis_framework_common/RobotisDef.h b/robotis_framework_common/include/robotis_framework_common/RobotisDef.h deleted file mode 100644 index a860e9d..0000000 --- a/robotis_framework_common/include/robotis_framework_common/RobotisDef.h +++ /dev/null @@ -1,21 +0,0 @@ -/* - * RobotisDef.h - * - * Created on: 2016. 1. 27. - * Author: zerom - */ - -#ifndef ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_ROBOTISDEF_H_ -#define ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_ROBOTISDEF_H_ - - -typedef char INT8_T; -typedef short int INT16_T; -typedef int INT32_T; - -typedef unsigned char UINT8_T; -typedef unsigned short int UINT16_T; -typedef unsigned int UINT32_T; - - -#endif /* ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_ROBOTISDEF_H_ */ diff --git a/robotis_framework_common/include/robotis_framework_common/SensorModule.h b/robotis_framework_common/include/robotis_framework_common/SensorModule.h deleted file mode 100644 index 6111391..0000000 --- a/robotis_framework_common/include/robotis_framework_common/SensorModule.h +++ /dev/null @@ -1,38 +0,0 @@ -/* - * SensorModule.h - * - * Created on: 2016. 3. 30. - * Author: zerom - */ - -#ifndef ROBOTIS_FRAMEWORK_COMMON_INCLUDE_ROBOTIS_FRAMEWORK_COMMON_SENSORMODULE_H_ -#define ROBOTIS_FRAMEWORK_COMMON_INCLUDE_ROBOTIS_FRAMEWORK_COMMON_SENSORMODULE_H_ - - -#include -#include - -#include "robotis_device/Robot.h" -#include "robotis_device/Dynamixel.h" -#include "robotis_framework_common/Singleton.h" - -namespace ROBOTIS -{ - -class SensorModule -{ -public: - std::string module_name; - - std::map result; - - virtual ~SensorModule() { } - - virtual void Initialize(const int control_cycle_msec, Robot *robot) = 0; - virtual void Process(std::map dxls, std::map sensors) = 0; -}; - -} - - -#endif /* ROBOTIS_FRAMEWORK_COMMON_INCLUDE_ROBOTIS_FRAMEWORK_COMMON_SENSORMODULE_H_ */ diff --git a/robotis_framework_common/include/robotis_framework_common/Singleton.h b/robotis_framework_common/include/robotis_framework_common/Singleton.h deleted file mode 100644 index 2b4c7d8..0000000 --- a/robotis_framework_common/include/robotis_framework_common/Singleton.h +++ /dev/null @@ -1,49 +0,0 @@ -/* - * Singleton.h - * - * Created on: 2016. 5. 17. - * Author: zerom - */ - -#ifndef ROBOTIS_FRAMEWORK_COMMON_INCLUDE_ROBOTIS_FRAMEWORK_COMMON_SINGLETON_H_ -#define ROBOTIS_FRAMEWORK_COMMON_INCLUDE_ROBOTIS_FRAMEWORK_COMMON_SINGLETON_H_ - - -namespace ROBOTIS -{ - -template -class Singleton -{ -private: - static T *unique_instance_; - -protected: - Singleton() { } - Singleton(Singleton const&) { } - Singleton& operator=(Singleton const&) { return *this; } - -public: - static T* GetInstance() - { - if(unique_instance_ == NULL) - unique_instance_ = new T; - return unique_instance_; - } - - static void DestroyInstance() - { - if(unique_instance_) - { - delete unique_instance_; - unique_instance_ = NULL; - } - } -}; - -template T* Singleton::unique_instance_ = NULL; - -} - - -#endif /* ROBOTIS_FRAMEWORK_COMMON_INCLUDE_ROBOTIS_FRAMEWORK_COMMON_SINGLETON_H_ */ diff --git a/robotis_framework_common/include/robotis_framework_common/motion_module.h b/robotis_framework_common/include/robotis_framework_common/motion_module.h new file mode 100644 index 0000000..e85d938 --- /dev/null +++ b/robotis_framework_common/include/robotis_framework_common/motion_module.h @@ -0,0 +1,101 @@ +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * 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 ROBOTIS 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. + *******************************************************************************/ + +/* + * motion_module.h + * + * Created on: 2016. 1. 15. + * Author: zerom + */ + +#ifndef ROBOTIS_FRAMEWORK_COMMON_MOTION_MODULE_H_ +#define ROBOTIS_FRAMEWORK_COMMON_MOTION_MODULE_H_ + + +#include +#include + +#include "singleton.h" +#include "robotis_device/robot.h" +#include "robotis_device/dynamixel.h" + +namespace robotis_framework +{ + +enum ControlMode +{ + PositionControl, + VelocityControl, + TorqueControl +}; + +class MotionModule +{ +protected: + bool enable_; + std::string module_name_; + ControlMode control_mode_; + +public: + std::map result_; + + virtual ~MotionModule() { } + + std::string getModuleName() { return module_name_; } + ControlMode getControlMode() { return control_mode_; } + + void setModuleEnable(bool enable) + { + if(this->enable_ == enable) + return; + + this->enable_ = enable; + if(enable) + onModuleEnable(); + else + onModuleDisable(); + } + bool getModuleEnable() { return enable_; } + + virtual void onModuleEnable() { } + virtual void onModuleDisable() { } + + virtual void initialize(const int control_cycle_msec, Robot *robot) = 0; + virtual void process(std::map dxls, std::map sensors) = 0; + + virtual void stop() = 0; + virtual bool isRunning() = 0; +}; + + +} + + +#endif /* ROBOTIS_FRAMEWORK_COMMON_MOTION_MODULE_H_ */ diff --git a/robotis_framework_common/include/robotis_framework_common/sensor_module.h b/robotis_framework_common/include/robotis_framework_common/sensor_module.h new file mode 100644 index 0000000..81699e2 --- /dev/null +++ b/robotis_framework_common/include/robotis_framework_common/sensor_module.h @@ -0,0 +1,71 @@ +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * 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 ROBOTIS 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. + *******************************************************************************/ + +/* + * sensor_module.h + * + * Created on: 2016. 3. 30. + * Author: zerom + */ + +#ifndef ROBOTIS_FRAMEWORK_COMMON_SENSOR_MODULE_H_ +#define ROBOTIS_FRAMEWORK_COMMON_SENSOR_MODULE_H_ + + +#include +#include + +#include "singleton.h" +#include "robotis_device/robot.h" +#include "robotis_device/dynamixel.h" + +namespace robotis_framework +{ + +class SensorModule +{ +protected: + std::string module_name_; + +public: + std::map result_; + + virtual ~SensorModule() { } + + std::string getModuleName() { return module_name_; } + + virtual void initialize(const int control_cycle_msec, Robot *robot) = 0; + virtual void process(std::map dxls, std::map sensors) = 0; +}; + +} + + +#endif /* ROBOTIS_FRAMEWORK_COMMON_SENSOR_MODULE_H_ */ diff --git a/robotis_framework_common/include/robotis_framework_common/singleton.h b/robotis_framework_common/include/robotis_framework_common/singleton.h new file mode 100644 index 0000000..837b80b --- /dev/null +++ b/robotis_framework_common/include/robotis_framework_common/singleton.h @@ -0,0 +1,79 @@ +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * 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 ROBOTIS 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. + *******************************************************************************/ + +/* + * singleton.h + * + * Created on: 2016. 5. 17. + * Author: zerom + */ + +#ifndef ROBOTIS_FRAMEWORK_COMMON_SINGLETON_H_ +#define ROBOTIS_FRAMEWORK_COMMON_SINGLETON_H_ + + +namespace robotis_framework +{ + +template +class Singleton +{ +private: + static T *unique_instance_; + +protected: + Singleton() { } + Singleton(Singleton const&) { } + Singleton& operator=(Singleton const&) { return *this; } + +public: + static T* getInstance() + { + if(unique_instance_ == NULL) + unique_instance_ = new T; + return unique_instance_; + } + + static void destroyInstance() + { + if(unique_instance_) + { + delete unique_instance_; + unique_instance_ = NULL; + } + } +}; + +template T* Singleton::unique_instance_ = NULL; + +} + + +#endif /* ROBOTIS_FRAMEWORK_COMMON_SINGLETON_H_ */ diff --git a/robotis_framework_common/package.xml b/robotis_framework_common/package.xml index feec7f8..820f4a2 100644 --- a/robotis_framework_common/package.xml +++ b/robotis_framework_common/package.xml @@ -1,52 +1,15 @@ robotis_framework_common - 0.0.0 + 0.1.0 The robotis_framework_common package + robotis - - - - robotis - - - - - BSD - - - - - - - - - - - - - - - - - - - - - - - - + ROBOTIS + catkin roscpp roscpp - - - - - - - \ No newline at end of file diff --git a/robotis_math/CMakeLists.txt b/robotis_math/CMakeLists.txt deleted file mode 100644 index 147809f..0000000 --- a/robotis_math/CMakeLists.txt +++ /dev/null @@ -1,65 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(robotis_math) - -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS - roscpp - cmake_modules -) - -find_package(Eigen REQUIRED) - - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if you package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( - INCLUDE_DIRS include - LIBRARIES robotis_math - CATKIN_DEPENDS roscpp -# DEPENDS system_lib -) - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -include_directories( - include - ${catkin_INCLUDE_DIRS} - ${Eigen_INCLUDE_DIRS} -) - -## Declare a C++ library -add_library(robotis_math - src/RobotisMathBase.cpp - src/RobotisTrajectoryCalculator.cpp - src/RobotisLinearAlgebra.cpp -) - -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure -add_dependencies(robotis_math ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Declare a C++ executable -# add_executable(robotis_math_node src/robotis_math_node.cpp) - -## Add cmake target dependencies of the executable -## same as for the library above -#add_dependencies(robotis_math_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Specify libraries to link a library or executable target against -target_link_libraries(robotis_math - ${catkin_LIBRARIES} -) diff --git a/robotis_math/include/robotis_math/RobotisLinearAlgebra.h b/robotis_math/include/robotis_math/RobotisLinearAlgebra.h deleted file mode 100644 index 6188c50..0000000 --- a/robotis_math/include/robotis_math/RobotisLinearAlgebra.h +++ /dev/null @@ -1,52 +0,0 @@ -/* - * RobotisLinearAlgebra.h - * - * Created on: Mar 18, 2016 - * Author: jay - */ - -#ifndef ROBOTIS_LINEAR_ALGEBRA_H_ -#define ROBOTIS_LINEAR_ALGEBRA_H_ - -#include - -#define EIGEN_NO_DEBUG -#define EIGEN_NO_STATIC_ASSERT - -#include - -namespace ROBOTIS -{ - -Eigen::MatrixXd transitionXYZ ( double position_x, double position_y, double position_z ); -Eigen::MatrixXd transformationXYZRPY ( double position_x, double position_y, double position_z , double roll , double pitch , double yaw ); -Eigen::MatrixXd InverseTransformation(Eigen::MatrixXd transform); - -Eigen::MatrixXd inertiaXYZ( double ixx, double ixy, double ixz , double iyy , double iyz, double izz ); - -Eigen::MatrixXd rotationX( double angle ); -Eigen::MatrixXd rotationY( double angle ); -Eigen::MatrixXd rotationZ( double angle ); - -Eigen::MatrixXd rotation2rpy( Eigen::MatrixXd rotation ); -Eigen::MatrixXd rpy2rotation( double roll, double pitch, double yaw ); - -Eigen::Quaterniond rpy2quaternion( double roll, double pitch, double yaw ); -Eigen::Quaterniond rotation2quaternion( Eigen::MatrixXd rotation ); - -Eigen::MatrixXd quaternion2rpy( Eigen::Quaterniond quaternion ); -Eigen::MatrixXd quaternion2rotation( Eigen::Quaterniond quaternion ); - -Eigen::MatrixXd rotation4d( double roll, double pitch, double yaw ); - -Eigen::MatrixXd hatto( Eigen::MatrixXd matrix3d ); -Eigen::MatrixXd Rodrigues( Eigen::MatrixXd hat_matrix , double angle ); -Eigen::MatrixXd rot2omega(Eigen::MatrixXd rotation ); -Eigen::MatrixXd cross(Eigen::MatrixXd vector3d_a, Eigen::MatrixXd vector3d_b ); -double dot(Eigen::MatrixXd vector3d_a, Eigen::MatrixXd vector3d_b ); - -} - - - -#endif /* ROBOTIS_LINEAR_ALGEBRA_H_ */ diff --git a/robotis_math/include/robotis_math/RobotisMath.h b/robotis_math/include/robotis_math/RobotisMath.h deleted file mode 100644 index 752038d..0000000 --- a/robotis_math/include/robotis_math/RobotisMath.h +++ /dev/null @@ -1,13 +0,0 @@ -/* - * robotis_math.h - * - * Created on: Mar 18, 2016 - * Author: jay - */ - -#ifndef ROBOTIS_MATH_H_ -#define ROBOTIS_MATH_H_ - -#include "RobotisTrajectoryCalculator.h" - -#endif /* ROBOTIS_MATH_H_ */ diff --git a/robotis_math/include/robotis_math/RobotisMathBase.h b/robotis_math/include/robotis_math/RobotisMathBase.h deleted file mode 100644 index 7b7c713..0000000 --- a/robotis_math/include/robotis_math/RobotisMathBase.h +++ /dev/null @@ -1,33 +0,0 @@ -/* - * RobotisMathBase.h - * - * Created on: 2016. 3. 28. - * Author: JaySong - */ - -#ifndef ROBOTIS_MATH_BASE_H_ -#define ROBOTIS_MATH_BASE_H_ - -#include - -namespace ROBOTIS -{ - -#define PRINT_VAR(X) std::cout << #X << " : " << X << std::endl -#define PRINT_MAT(X) std::cout << #X << ":\n" << X << std::endl << std::endl - -#define deg2rad (M_PI / 180.0) -#define rad2deg (180.0 / M_PI) - -inline double powDI(double a, int b) -{ - return (b == 0 ? 1 : (b > 0 ? a * powDI(a, b - 1) : 1 / powDI(a, -b))); -} - -double sign( double x ); - -} - - - -#endif /* ROBOTIS_MATH_BASE_H_ */ diff --git a/robotis_math/include/robotis_math/RobotisTrajectoryCalculator.h b/robotis_math/include/robotis_math/RobotisTrajectoryCalculator.h deleted file mode 100644 index 3d1ff70..0000000 --- a/robotis_math/include/robotis_math/RobotisTrajectoryCalculator.h +++ /dev/null @@ -1,37 +0,0 @@ -/* - * RobotisTrajectoryCalculator.h - * - * Created on: Mar 18, 2016 - * Author: jay - */ - -#ifndef ROBOTIS_TRAJECTORY_CALCULATOR_H_ -#define ROBOTIS_TRAJECTORY_CALCULATOR_H_ - - -#include "RobotisMathBase.h" -#include "RobotisLinearAlgebra.h" - -// minimum jerk trajectory - -namespace ROBOTIS -{ - -Eigen::MatrixXd minimum_jerk_tra( double pos_start , double vel_start , double accel_start, - double pos_end , double vel_end , double accel_end, - double smp_time , double mov_time ); - -Eigen::MatrixXd minimum_jerk_tra_via_n_qdqddq( int via_num, - double pos_start , double vel_start , double accel_start , - Eigen::MatrixXd pos_via, Eigen::MatrixXd vel_via, Eigen::MatrixXd accel_via, - double pos_end, double vel_end, double accel_end, - double smp_time, Eigen::MatrixXd via_time, double mov_time ); - -Eigen::MatrixXd arc3d_tra( double smp_time, double mov_time, - Eigen::MatrixXd center_point, Eigen::MatrixXd normal_vector, Eigen::MatrixXd start_point, - double rotation_angle, double cross_ratio ); - -} - - -#endif /* ROBOTIS_TRAJECTORY_CALCULATOR_H_ */ diff --git a/robotis_math/package.xml b/robotis_math/package.xml deleted file mode 100644 index 506f261..0000000 --- a/robotis_math/package.xml +++ /dev/null @@ -1,37 +0,0 @@ - - - robotis_math - 0.0.0 - The robotis_math package - - - - - jay - - - - - - BSD - - - - - - - - - - - - - - catkin - roscpp - cmake_modules - - roscpp - cmake_modules - - \ No newline at end of file diff --git a/robotis_math/src/RobotisLinearAlgebra.cpp b/robotis_math/src/RobotisLinearAlgebra.cpp deleted file mode 100644 index 30bb5ce..0000000 --- a/robotis_math/src/RobotisLinearAlgebra.cpp +++ /dev/null @@ -1,291 +0,0 @@ -/* - * RobotisLinearAlgebra.cpp - * - * Created on: Mar 18, 2016 - * Author: jay - */ - - -#include "robotis_math/RobotisLinearAlgebra.h" - -namespace ROBOTIS -{ - -Eigen::MatrixXd transitionXYZ ( double position_x, double position_y, double position_z ) -{ - Eigen::MatrixXd _position(3,1); - - _position << position_x, - position_y, - position_z; - - return _position; -} - -Eigen::MatrixXd transformationXYZRPY ( double position_x, double position_y, double position_z , double roll , double pitch , double yaw ) -{ -// Eigen::MatrixXd _position(3,1); -// -// _position << position_x, -// position_y, -// position_z; -// -// Eigen::MatrixXd _rotation = rpy2rotation( roll , pitch , yaw ); -// -// Eigen::MatrixXd _transformation(4,4); -// -// _transformation << _rotation , _position, -// 0 , 0 , 0 , 1; - - Eigen::MatrixXd _transformation = rotation4d(roll, pitch, yaw); - _transformation.coeffRef(0,3) = position_x; - _transformation.coeffRef(1,3) = position_y; - _transformation.coeffRef(2,3) = position_z; - - return _transformation; -} - -Eigen::MatrixXd InverseTransformation(Eigen::MatrixXd transform) -{ - Eigen::Vector3d vecBOA; //If T is Transform Matrix A from B, the BOA is translation component coordi. B to coordi. A - Eigen::Vector3d vec_x, vec_y, vec_z; - Eigen::MatrixXd _invT(4,4); - - vecBOA.coeffRef(0) = -transform(0,3); vecBOA(1) = -transform(1,3); vecBOA(2) = -transform(2,3); - vec_x(0) = transform(0,0); vec_x(1) = transform(1,0); vec_x(2) = transform(2,0); - vec_y(0) = transform(0,1); vec_y(1) = transform(1,1); vec_y(2) = transform(2,1); - vec_z(0) = transform(0,2); vec_z(1) = transform(1,2); vec_z(2) = transform(2,2); -// -// -// // inv = [ x' | -AtoB??x ] -// // [ y' | -AtoB??y ] -// // [ z' | -AtoB??z ] -// // [ 0 0 0 | 1 ] -// - _invT << vec_x(0), vec_x(1), vec_x(2), vecBOA.dot(vec_x), - vec_y(0), vec_y(1), vec_y(2), vecBOA.dot(vec_y), - vec_z(0), vec_z(1), vec_z(2), vecBOA.dot(vec_z), - 0, 0, 0, 1; - - return _invT; -} - -Eigen::MatrixXd inertiaXYZ( double ixx, double ixy, double ixz , double iyy , double iyz, double izz ) -{ - Eigen::MatrixXd _inertia(3,3); - - _inertia << ixx , ixy , ixz, - ixy , iyy , iyz, - ixz , iyz , izz ; - - return _inertia; -} - -Eigen::MatrixXd rotationX( double angle ) -{ - Eigen::MatrixXd _rotation( 3 , 3 ); - - _rotation << 1.0, 0.0, 0.0, - 0.0, cos( angle ), -sin( angle ), - 0.0, sin( angle ), cos( angle ); - - return _rotation; -} - -Eigen::MatrixXd rotationY( double angle ) -{ - Eigen::MatrixXd _rotation( 3 , 3 ); - - _rotation << cos( angle ), 0.0, sin( angle ), - 0.0, 1.0, 0.0, - -sin( angle ), 0.0, cos( angle ); - - return _rotation; -} - -Eigen::MatrixXd rotationZ( double angle ) -{ - Eigen::MatrixXd _rotation(3,3); - - _rotation << cos( angle ), -sin( angle ), 0.0, - sin( angle ), cos( angle ), 0.0, - 0.0, 0.0, 1.0; - - return _rotation; -} - -Eigen::MatrixXd rotation2rpy( Eigen::MatrixXd rotation ) -{ - Eigen::MatrixXd _rpy = Eigen::MatrixXd::Zero( 3 , 1 ); - - _rpy.coeffRef( 0 , 0 ) = atan2( rotation.coeff( 2 , 1 ), rotation.coeff( 2 , 2 ) ); - _rpy.coeffRef( 1 , 0 ) = atan2( -rotation.coeff( 2 , 0 ), sqrt( pow( rotation.coeff( 2 , 1 ) , 2 ) + pow( rotation.coeff( 2 , 2 ) , 2 ) ) ); - _rpy.coeffRef( 2 , 0 ) = atan2 ( rotation.coeff( 1 , 0 ) , rotation.coeff( 0 , 0 ) ); - - return _rpy; -} - -Eigen::MatrixXd rpy2rotation( double roll, double pitch, double yaw ) -{ - Eigen::MatrixXd _rotation = rotationZ( yaw ) * rotationY( pitch ) * rotationX( roll ); - - return _rotation; -} - -Eigen::Quaterniond rpy2quaternion( double roll, double pitch, double yaw ) -{ - Eigen::MatrixXd _rotation = rpy2rotation( roll, pitch, yaw ); - - Eigen::Matrix3d _rotation3d; - _rotation3d = _rotation.block<3,3>( 0 , 0); - - Eigen::Quaterniond _quaternion; - - _quaternion = _rotation3d; - - return _quaternion; -} - -Eigen::Quaterniond rotation2quaternion( Eigen::MatrixXd rotation ) -{ - Eigen::Matrix3d _rotation3d; - - _rotation3d = rotation.block<3,3>( 0 , 0); - - Eigen::Quaterniond _quaternion; - _quaternion = _rotation3d; - - return _quaternion; -} - -Eigen::MatrixXd quaternion2rpy( Eigen::Quaterniond quaternion ) -{ - Eigen::MatrixXd _rpy = rotation2rpy( quaternion.toRotationMatrix() ); - - return _rpy; -} - -Eigen::MatrixXd quaternion2rotation( Eigen::Quaterniond quaternion ) -{ - Eigen::MatrixXd _rotation = quaternion.toRotationMatrix(); - - return _rotation; -} - -Eigen::MatrixXd rotation4d( double roll, double pitch, double yaw ) -{ -// Eigen::MatrixXd _rotation4d = Eigen::MatrixXd::Zero( 4 , 4 ); -// _rotation4d.coeffRef( 3 , 3 ) = 1.0; -// -// Eigen::MatrixXd _rotation = rotationZ( yaw ) * rotationY( pitch ) * rotationX( roll ); -// -//// _rotation4d.block<3,3>(0,0) = _rotation; -// _rotation4d.coeffRef(0,0) = _rotation.coeff(0,0); -// _rotation4d.coeffRef(0,1) = _rotation.coeff(0,1); -// _rotation4d.coeffRef(0,2) = _rotation.coeff(0,2); -// _rotation4d.coeffRef(1,0) = _rotation.coeff(1,0); -// _rotation4d.coeffRef(1,1) = _rotation.coeff(1,1); -// _rotation4d.coeffRef(1,2) = _rotation.coeff(1,2); -// _rotation4d.coeffRef(2,0) = _rotation.coeff(2,0); -// _rotation4d.coeffRef(2,1) = _rotation.coeff(2,1); -// _rotation4d.coeffRef(2,2) = _rotation.coeff(2,2); - -// return _rotation4d; - double sr = sin(roll), cr = cos(roll); - double sp = sin(pitch), cp = cos(pitch); - double sy = sin(yaw), cy = cos(yaw); - - Eigen::MatrixXd matRoll(4,4); - Eigen::MatrixXd matPitch(4,4); - Eigen::MatrixXd matYaw(4,4); - - matRoll << 1, 0, 0, 0, - 0, cr, -sr, 0, - 0, sr, cr, 0, - 0, 0, 0, 1; - - matPitch << cp, 0, sp, 0, - 0, 1, 0, 0, - -sp, 0, cp, 0, - 0, 0, 0, 1; - - matYaw << cy, -sy, 0, 0, - sy, cy, 0, 0, - 0, 0, 1, 0, - 0, 0, 0, 1; - - - return (matYaw*matPitch)*matRoll; -} - - - -Eigen::MatrixXd hatto( Eigen::MatrixXd matrix3d ) -{ - Eigen::MatrixXd _hatto(3,3); - - _hatto << 0.0, -matrix3d.coeff(2,0), matrix3d.coeff(1,0), - matrix3d.coeff(2,0), 0.0, -matrix3d.coeff(0,0), - -matrix3d.coeff(1,0), matrix3d.coeff(0,0), 0.0; - - return _hatto; -} - -Eigen::MatrixXd Rodrigues( Eigen::MatrixXd hat_matrix , double angle ) -{ - Eigen::MatrixXd _E = Eigen::MatrixXd::Identity( 3 , 3 ); - Eigen::MatrixXd _Rodrigues = _E + hat_matrix * sin( angle ) + hat_matrix * hat_matrix * ( 1 - cos( angle ) ); - - return _Rodrigues; -} - -Eigen::MatrixXd rot2omega( Eigen::MatrixXd rotation ) -{ - double _eps = 1e-12; - -// Eigen::MatrixXd _E = Eigen::MatrixXd::Identity( 3 , 3 ); - - double _alpha = ( rotation.coeff( 0 , 0 ) + rotation.coeff( 1 , 1 ) + rotation.coeff( 2 , 2 ) - 1.0 ) / 2.0 ; - - double _alpha_plus = fabs( _alpha - 1.0 ); - - Eigen::MatrixXd _rot2omega( 3 , 1 ); - - if( _alpha_plus < _eps ) - { - _rot2omega << 0.0, - 0.0, - 0.0; - } - else - { - double _theta = acos( _alpha ); - - _rot2omega << rotation.coeff( 2 , 1 ) - rotation.coeff( 1 , 2 ), - rotation.coeff( 0 , 2 ) - rotation.coeff( 2 , 0 ), - rotation.coeff( 1 , 0 ) - rotation.coeff( 0 , 1 ); - - _rot2omega = 0.5 * ( _theta / sin( _theta ) ) * _rot2omega; - } - - return _rot2omega; -} - -Eigen::MatrixXd cross(Eigen::MatrixXd vector3d_a, Eigen::MatrixXd vector3d_b ) -{ - Eigen::MatrixXd _cross( 3 , 1 ); - - _cross << vector3d_a.coeff( 1 , 0 ) * vector3d_b.coeff( 2 , 0 ) - vector3d_a.coeff( 2 , 0 ) * vector3d_b.coeff( 1 , 0 ), - vector3d_a.coeff( 2 , 0 ) * vector3d_b.coeff( 0 , 0 ) - vector3d_a.coeff( 0 , 0 ) * vector3d_b.coeff( 2 , 0 ), - vector3d_a.coeff( 0 , 0 ) * vector3d_b.coeff( 1 , 0 ) - vector3d_a.coeff( 1 , 0 ) * vector3d_b.coeff( 0 , 0 ); - - return _cross; -} - -double dot(Eigen::MatrixXd vector3d_a, Eigen::MatrixXd vector3d_b ) -{ - return vector3d_a.dot(vector3d_b); - //return vector3d_a.coeff(0,0)*vector3d_b.coeff(0,0) + vector3d_a.coeff(1,0)*vector3d_b.coeff(1,0) + vector3d_a.coeff(2,0)*vector3d_b.coeff(2,0); -} - -} diff --git a/robotis_math/src/RobotisMathBase.cpp b/robotis_math/src/RobotisMathBase.cpp deleted file mode 100644 index 0afb845..0000000 --- a/robotis_math/src/RobotisMathBase.cpp +++ /dev/null @@ -1,26 +0,0 @@ -/* - * RobotisMathBase.cpp - * - * Created on: Mar 18, 2016 - * Author: jay - */ - -#include "robotis_math/RobotisMathBase.h" - - - - -namespace ROBOTIS -{ - -double sign( double x ) -{ - if ( x < 0.0 ) - return -1.0; - else if ( x > 0.0) - return 1.0; - else - return 0.0; -} - -} diff --git a/robotis_math/src/RobotisTrajectoryCalculator.cpp b/robotis_math/src/RobotisTrajectoryCalculator.cpp deleted file mode 100644 index 665015b..0000000 --- a/robotis_math/src/RobotisTrajectoryCalculator.cpp +++ /dev/null @@ -1,325 +0,0 @@ -/* - * RobotisTrajectoryCalculator.cpp - * - * Created on: Mar 18, 2016 - * Author: jay - */ - - - -#include "robotis_math/RobotisTrajectoryCalculator.h" - - -/* - * trajectory.cpp - * - * Created on: Jul 13, 2015 - * Author: sch - */ - - - -namespace ROBOTIS -{ - -Eigen::MatrixXd minimum_jerk_tra( double pos_start , double vel_start , double accel_start, - double pos_end , double vel_end , double accel_end, - double smp_time , double mov_time ) -/* - simple minimum jerk trajectory - - pos_start : position at initial state - vel_start : velocity at initial state - accel_start : acceleration at initial state - - pos_end : position at final state - vel_end : velocity at final state - accel_end : acceleration at final state - - smp_time : sampling time - - mov_time : movement time -*/ - -{ - Eigen::MatrixXd ___C( 3 , 3 ); - Eigen::MatrixXd __C( 3 , 1 ); - - ___C << pow( mov_time , 3 ) , pow( mov_time , 4 ) , pow( mov_time , 5 ), - 3 * pow( mov_time , 2 ) , 4 * pow( mov_time , 3 ) , 5 * pow( mov_time , 4 ), - 6 * mov_time , 12 * pow( mov_time , 2 ) , 20 * pow( mov_time , 3 ); - - __C << pos_end - pos_start - vel_start * mov_time - accel_start * pow( mov_time , 2 ) / 2, - vel_end - vel_start - accel_start * mov_time, - accel_end - accel_start ; - - Eigen::Matrix _A = ___C.inverse() * __C; - - double _time_steps; - - _time_steps = mov_time / smp_time; - int __time_steps = round( _time_steps + 1 ); - - Eigen::MatrixXd _time = Eigen::MatrixXd::Zero( __time_steps , 1 ); - Eigen::MatrixXd _tra = Eigen::MatrixXd::Zero( __time_steps , 1 ); - - for ( int step = 0; step < __time_steps; step++ ) - _time.coeffRef( step , 0 ) = step * smp_time; - - for ( int step = 0; step < __time_steps; step++ ) - { - _tra.coeffRef( step , 0 ) = - pos_start + - vel_start * _time.coeff( step , 0 ) + - 0.5 * accel_start * pow( _time.coeff( step , 0 ) , 2 ) + - _A.coeff( 0 , 0 ) * pow( _time.coeff( step , 0 ) , 3 ) + - _A.coeff( 1 , 0 ) * pow( _time.coeff( step , 0 ) , 4 ) + - _A.coeff( 2 , 0 ) * pow( _time.coeff( step , 0 ) , 5 ); - } - - return _tra; -} - -Eigen::MatrixXd minimum_jerk_tra_via_n_qdqddq( int via_num, - double pos_start , double vel_start , double accel_start , - Eigen::MatrixXd pos_via, Eigen::MatrixXd vel_via, Eigen::MatrixXd accel_via, - double pos_end, double vel_end, double accel_end, - double smp_time, Eigen::MatrixXd via_time, double mov_time ) -/* - minimum jerk trajectory with via-points - (via-point constraints: position and velocity at each point) - - n : the number of via-points - - x0 : position at initial state - v0 : velocity at initial state - a0 : acceleration at initial state - - x : position matrix at via-points state ( size : n x 1 ) - dx : velocity matrix at via-points state ( size : n x 1 ) - ddx : acceleration matrix at via-points state ( size : n x 1 ) - - xf : position at final state - vf : velocity at final state - af : acceleration at final state - - smp : sampling time - - t : time matrix passing through via-points state ( size : n x 1 ) - tf : movement time -*/ - -{ - int i,j,k ; - - /* Calculation Matrix B */ - - Eigen::MatrixXd B = Eigen::MatrixXd::Zero(3*via_num+3,1); - - for (i=1; i<=via_num; i++){ - B.coeffRef(3*i-3,0) = - pos_via.coeff(i-1,0) - - pos_start - - vel_start*via_time.coeff(i-1,0) - - (accel_start/2)*pow(via_time.coeff(i-1,0),2) ; - - B.coeffRef(3*i-2,0) = - vel_via.coeff(i-1,0) - - vel_start - - accel_start*via_time.coeff(i-1,0) ; - - B.coeffRef(3*i-1,0) = - accel_via.coeff(i-1,0) - - accel_start ; - } - - B.coeffRef(3*via_num,0) = - pos_end - - pos_start - - vel_start*mov_time - - (accel_start/2)*pow(mov_time,2) ; - - B.coeffRef(3*via_num+1,0) = - vel_end - - vel_start - - accel_start*mov_time ; - - B.coeffRef(3*via_num+2,0) = - accel_end - - accel_start ; - - - /* Calculation Matrix A */ - - Eigen::MatrixXd A1 = Eigen::MatrixXd::Zero(3*via_num,3); - - for (i=1; i<=via_num; i++){ - A1.coeffRef(3*i-3,0) = pow(via_time.coeff(i-1,0),3) ; - A1.coeffRef(3*i-3,1) = pow(via_time.coeff(i-1,0),4) ; - A1.coeffRef(3*i-3,2) = pow(via_time.coeff(i-1,0),5) ; - - A1.coeffRef(3*i-2,0) = 3*pow(via_time.coeff(i-1,0),2) ; - A1.coeffRef(3*i-2,1) = 4*pow(via_time.coeff(i-1,0),3) ; - A1.coeffRef(3*i-2,2) = 5*pow(via_time.coeff(i-1,0),4) ; - - A1.coeffRef(3*i-1,0) = 6*via_time.coeff(i-1,0) ; - A1.coeffRef(3*i-1,1) = 12*pow(via_time.coeff(i-1,0),2) ; - A1.coeffRef(3*i-1,2) = 20*pow(via_time.coeff(i-1,0),3) ; - } - - - Eigen::MatrixXd A2 = Eigen::MatrixXd::Zero(3*via_num,3*via_num); - - for (i=1; i<=via_num; i++){ - for (j=1; j<=via_num; j++){ - if (i > j){ - k = i ; - }else{ - k = j ; - } - A2.coeffRef(3*j-3,3*i-3) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),3)/6 ; - A2.coeffRef(3*j-3,3*i-2) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),4)/24 ; - A2.coeffRef(3*j-3,3*i-1) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),5)/120 ; - - A2.coeffRef(3*j-2,3*i-3) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),2)/2 ; - A2.coeffRef(3*j-2,3*i-2) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),3)/6 ; - A2.coeffRef(3*j-2,3*i-1) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),4)/24 ; - - A2.coeffRef(3*j-1,3*i-3) = via_time.coeff(k-1,0)-via_time.coeff(i-1,0) ; - A2.coeffRef(3*j-1,3*i-2) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),2)/2 ; - A2.coeffRef(3*j-1,3*i-1) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),3)/6 ; - } - } - - - Eigen::MatrixXd A3 = Eigen::MatrixXd::Zero(3,3*via_num+3); - - A3.coeffRef(0,0) = pow(mov_time,3); - A3.coeffRef(0,1) = pow(mov_time,4); - A3.coeffRef(0,2) = pow(mov_time,5); - - A3.coeffRef(1,0) = 3*pow(mov_time,2); - A3.coeffRef(1,1) = 4*pow(mov_time,3); - A3.coeffRef(1,2) = 5*pow(mov_time,4); - - A3.coeffRef(2,0) = 6*mov_time; - A3.coeffRef(2,1) = 12*pow(mov_time,2); - A3.coeffRef(2,2) = 20*pow(mov_time,3); - - for (i=1; i<=via_num; i++){ - A3.coeffRef(0,3*i) = pow(mov_time-via_time.coeff(i-1,0),3)/6 ; - A3.coeffRef(1,3*i) = pow(mov_time-via_time.coeff(i-1,0),2)/2 ; - A3.coeffRef(2,3*i) = mov_time-via_time.coeff(i-1,0) ; - - A3.coeffRef(0,3*i+1) = pow(mov_time-via_time.coeff(i-1,0),4)/24 ; - A3.coeffRef(1,3*i+1) = pow(mov_time-via_time.coeff(i-1,0),3)/6 ; - A3.coeffRef(2,3*i+1) = pow(mov_time-via_time.coeff(i-1,0),2)/2 ; - - A3.coeffRef(0,3*i+2) = pow(mov_time-via_time.coeff(i-1,0),5)/120 ; - A3.coeffRef(1,3*i+2) = pow(mov_time-via_time.coeff(i-1,0),4)/24 ; - A3.coeffRef(2,3*i+2) = pow(mov_time-via_time.coeff(i-1,0),3)/6 ; - } - - Eigen::MatrixXd A = Eigen::MatrixXd::Zero(3*via_num+3,3*via_num+3); - - A.block(0,0,3*via_num,3) = A1 ; - A.block(0,3,3*via_num,3*via_num) = A2 ; - A.block(3*via_num,0,3,3*via_num+3) = A3 ; - - /* Calculation Matrix C (coefficient of polynomial function) */ - - Eigen::MatrixXd C(3*via_num+3,1); - //C = A.inverse()*B; - C = A.colPivHouseholderQr().solve(B); - - /* Time */ - - int NN; - double N; - - N = mov_time/smp_time ; - NN = round(N) ; - - Eigen::MatrixXd Time = Eigen::MatrixXd::Zero(NN+1,1); - - for (i=1; i<=NN+1; i++){ - Time.coeffRef(i-1,0) = (i-1)*smp_time; - } - - /* Time_via */ - - Eigen::MatrixXd Time_via = Eigen::MatrixXd::Zero(via_num,1); - - for (i=1; i<=via_num; i++){ - Time_via.coeffRef(i-1,0) = round(via_time.coeff(i-1,0)/smp_time)+2; - } - - /* Minimum Jerk Trajectory with Via-points */ - - Eigen::MatrixXd _tra_jerk_via = Eigen::MatrixXd::Zero(NN+1,1); - - for (i=1; i<=NN+1; i++){ - _tra_jerk_via.coeffRef(i-1,0) = - pos_start + - vel_start*Time.coeff(i-1,0) + - 0.5*accel_start*pow(Time.coeff(i-1,0),2) + - C.coeff(0,0)*pow(Time.coeff(i-1,0),3) + - C.coeff(1,0)*pow(Time.coeff(i-1,0),4) + - C.coeff(2,0)*pow(Time.coeff(i-1,0),5) ; - } - - for (i=1; i<=via_num; i++){ - for (j=Time_via.coeff(i-1,0); j<=NN+1; j++){ - _tra_jerk_via.coeffRef(j-1,0) = - _tra_jerk_via.coeff(j-1,0) + - C.coeff(3*i,0)*pow((Time.coeff(j-1,0)-via_time.coeff(i-1,0)),3)/6 + - C.coeff(3*i+1,0)*pow((Time.coeff(j-1,0)-via_time.coeff(i-1,0)),4)/24 + - C.coeff(3*i+2,0)*pow((Time.coeff(j-1,0)-via_time.coeff(i-1,0)),5)/120 ; - - } - } - - return _tra_jerk_via; - -} - -Eigen::MatrixXd arc3d_tra( double smp_time, double mov_time, - Eigen::MatrixXd center_point, Eigen::MatrixXd normal_vector, Eigen::MatrixXd start_point, - double rotation_angle, double cross_ratio ) -{ - int _all_time_steps = int ( round( mov_time / smp_time ) ) + 1 ; - - Eigen::MatrixXd _angle_tra = minimum_jerk_tra( 0.0 , 0.0 , 0.0 , - rotation_angle , 0.0 , 0.0 , - smp_time , mov_time ); - - Eigen::MatrixXd _pt = Eigen::MatrixXd::Zero( 3 , _all_time_steps ); - - for (int i = 0; i < _all_time_steps; i++ ) - { - double _t = ( ( double ) i ) * smp_time ; - - double _th = _angle_tra.coeff( i , 0 );//( _t / mov_time ) * rotation_angle; - - Eigen::MatrixXd _w_wedge( 3 , 3 ); - - _w_wedge << 0.0 , -normal_vector.coeff(2,0), normal_vector.coeff(1,0), - normal_vector.coeff(2,0), 0.0 , -normal_vector.coeff(0,0), - -normal_vector.coeff(1,0), normal_vector.coeff(0,0), 0.0 ; - - Eigen::MatrixXd _E = Eigen::MatrixXd::Identity( 3 , 3 ); - - Eigen::MatrixXd _R = _E + _w_wedge * sin( _th ) + _w_wedge * _w_wedge * ( 1 - cos( _th ) ); - - double _cross = cross_ratio * ( 1.0 - 2.0 * ( abs ( 0.5 - _t/mov_time ) ) ); - - _pt.block( 0 , i , 3 , 1 ) = ( 1 + _cross ) * ( _R * ( start_point - center_point ) ) + center_point; - } - - Eigen::MatrixXd _pt_trans = _pt.transpose(); - - return _pt_trans; -} - - -} From 6f188fc1ffa86c643b24435159e3e8ac4e081b94 Mon Sep 17 00:00:00 2001 From: pyo Date: Fri, 12 Aug 2016 23:21:40 +0900 Subject: [PATCH 055/238] added a meta-package --- robotis_framework/CMakeLists.txt | 4 ++++ robotis_framework/package.xml | 17 +++++++++++++++++ 2 files changed, 21 insertions(+) create mode 100644 robotis_framework/CMakeLists.txt create mode 100644 robotis_framework/package.xml diff --git a/robotis_framework/CMakeLists.txt b/robotis_framework/CMakeLists.txt new file mode 100644 index 0000000..d336fe9 --- /dev/null +++ b/robotis_framework/CMakeLists.txt @@ -0,0 +1,4 @@ +cmake_minimum_required(VERSION 2.8.3) +project(robotis_framework) +find_package(catkin REQUIRED) +catkin_metapackage() diff --git a/robotis_framework/package.xml b/robotis_framework/package.xml new file mode 100644 index 0000000..170f107 --- /dev/null +++ b/robotis_framework/package.xml @@ -0,0 +1,17 @@ + + + robotis_framework + 0.1.0 + ROS packages for the robotis_framework (meta package) + BSD + zerom + pyo + https://github.com/ROBOTIS-GIT/ROBOTIS-Framework/issues + https://github.com/ROBOTIS-GIT/ROBOTIS-Framework + http://wiki.ros.org/robotis_framework + catkin + robotis_framework_common + robotis_device + robotis_controller + + From caf4884f0517d41db2189b089fa70032c9045e0e Mon Sep 17 00:00:00 2001 From: pyo Date: Fri, 12 Aug 2016 23:31:04 +0900 Subject: [PATCH 056/238] modified the package information for release --- robotis_controller/CMakeLists.txt | 38 ++++++++++++++++++++- robotis_controller/package.xml | 17 ++++------ robotis_device/CMakeLists.txt | 44 ++++++++++++++++++++++--- robotis_framework_common/CMakeLists.txt | 33 +++++++++++++++++-- robotis_framework_common/package.xml | 9 ++--- 5 files changed, 116 insertions(+), 25 deletions(-) diff --git a/robotis_controller/CMakeLists.txt b/robotis_controller/CMakeLists.txt index 97c4a6d..1d94330 100644 --- a/robotis_controller/CMakeLists.txt +++ b/robotis_controller/CMakeLists.txt @@ -1,8 +1,14 @@ +################################################################################ +# CMake +################################################################################ cmake_minimum_required(VERSION 2.8.3) project(robotis_controller) set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}") +################################################################################ +# Packages +################################################################################ find_package(catkin REQUIRED COMPONENTS roscpp roslib @@ -15,13 +21,26 @@ find_package(catkin REQUIRED COMPONENTS dynamixel_sdk ) +################################################################################ +# Declare ROS messages, services and actions +################################################################################ + +################################################################################ +# Declare ROS dynamic reconfigure parameters +################################################################################ + +################################################################################ +# Catkin specific configuration +################################################################################ catkin_package( INCLUDE_DIRS include LIBRARIES robotis_controller CATKIN_DEPENDS roscpp roslib sensor_msgs std_msgs -# DEPENDS system_lib ) +################################################################################ +# Build +################################################################################ include_directories( include ${catkin_INCLUDE_DIRS} @@ -35,3 +54,20 @@ target_link_libraries(robotis_controller yaml-cpp ${catkin_LIBRARIES} ) + +################################################################################ +# Install +################################################################################ +install(TARGETS robotis_controller + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) + +################################################################################ +# Test +################################################################################ diff --git a/robotis_controller/package.xml b/robotis_controller/package.xml index 6dcf6d2..1320cf6 100644 --- a/robotis_controller/package.xml +++ b/robotis_controller/package.xml @@ -3,17 +3,13 @@ robotis_controller 0.1.1 The robotis_controller package - - ROBOTIS - BSD - - - - ROBOTIS - + zerom + pyo + https://github.com/ROBOTIS-GIT/ROBOTIS-Framework/issues + https://github.com/ROBOTIS-GIT/ROBOTIS-Framework + http://wiki.ros.org/robotis_controller catkin - roscpp roslib std_msgs @@ -22,7 +18,6 @@ robotis_device robotis_controller_msgs robotis_framework_common - roscpp roslib std_msgs @@ -30,5 +25,5 @@ dynamixel_sdk robotis_device robotis_controller_msgs - + diff --git a/robotis_device/CMakeLists.txt b/robotis_device/CMakeLists.txt index e492450..abc1965 100644 --- a/robotis_device/CMakeLists.txt +++ b/robotis_device/CMakeLists.txt @@ -1,19 +1,38 @@ +################################################################################ +# CMake +################################################################################ cmake_minimum_required(VERSION 2.8.3) project(robotis_device) +################################################################################ +# Packages +################################################################################ find_package(catkin REQUIRED COMPONENTS roscpp rospy dynamixel_sdk ) +################################################################################ +# Declare ROS messages, services and actions +################################################################################ + +################################################################################ +# Declare ROS dynamic reconfigure parameters +################################################################################ + +################################################################################ +# Catkin specific configuration +################################################################################ catkin_package( INCLUDE_DIRS include LIBRARIES robotis_device CATKIN_DEPENDS roscpp rospy -# DEPENDS system_lib ) +################################################################################ +# Build +################################################################################ include_directories( include ${catkin_INCLUDE_DIRS} @@ -24,9 +43,26 @@ add_library(robotis_device src/robotis_device/sensor.cpp src/robotis_device/dynamixel.cpp ) - add_dependencies(robotis_device ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(robotis_device ${catkin_LIBRARIES}) -target_link_libraries(robotis_device - ${catkin_LIBRARIES} +################################################################################ +# Install +################################################################################ +install(TARGETS robotis_device + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) + +install(DIRECTORY devices/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +################################################################################ +# Test +################################################################################ diff --git a/robotis_framework_common/CMakeLists.txt b/robotis_framework_common/CMakeLists.txt index 51f0f3f..20c330f 100644 --- a/robotis_framework_common/CMakeLists.txt +++ b/robotis_framework_common/CMakeLists.txt @@ -1,18 +1,45 @@ +################################################################################ +# CMake +################################################################################ cmake_minimum_required(VERSION 2.8.3) project(robotis_framework_common) +################################################################################ +# Packages +################################################################################ find_package(catkin REQUIRED COMPONENTS roscpp ) +################################################################################ +# Declare ROS messages, services and actions +################################################################################ + +################################################################################ +# Declare ROS dynamic reconfigure parameters +################################################################################ + +################################################################################ +# Catkin specific configuration +################################################################################ catkin_package( INCLUDE_DIRS include -# LIBRARIES robotis_framework_common -# CATKIN_DEPENDS roscpp -# DEPENDS system_lib ) +################################################################################ +# Build +################################################################################ include_directories( ${catkin_INCLUDE_DIRS} ) +################################################################################ +# Install +################################################################################ +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) + +################################################################################ +# Test +################################################################################ diff --git a/robotis_framework_common/package.xml b/robotis_framework_common/package.xml index 820f4a2..aa42d57 100644 --- a/robotis_framework_common/package.xml +++ b/robotis_framework_common/package.xml @@ -3,13 +3,10 @@ robotis_framework_common 0.1.0 The robotis_framework_common package - robotis - BSD - - ROBOTIS - + zerom + pyo catkin roscpp roscpp - \ No newline at end of file + From b6fd3fc96ab820932a33bd4ba5e2d421c033500c Mon Sep 17 00:00:00 2001 From: pyo Date: Fri, 12 Aug 2016 23:37:57 +0900 Subject: [PATCH 057/238] made and modified the CHANGELOG.rst for release --- robotis_controller/CHANGELOG.rst | 21 +++++++++++++++++++++ robotis_device/CHANGELOG.rst | 20 ++++++++++++++++++++ robotis_framework/CHANGELOG.rst | 8 ++++++++ robotis_framework_common/CHANGELOG.rst | 12 ++++++++++++ 4 files changed, 61 insertions(+) create mode 100644 robotis_controller/CHANGELOG.rst create mode 100644 robotis_device/CHANGELOG.rst create mode 100644 robotis_framework/CHANGELOG.rst create mode 100644 robotis_framework_common/CHANGELOG.rst diff --git a/robotis_controller/CHANGELOG.rst b/robotis_controller/CHANGELOG.rst new file mode 100644 index 0000000..74299e5 --- /dev/null +++ b/robotis_controller/CHANGELOG.rst @@ -0,0 +1,21 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package robotis_controller +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.1.1 (2016-08-12) +----------- +* first public release for Kinetic +* modified the package information for release +* develop branch -> master branch +* Merge pull request `#4 `_ from ROBOTIS-GIT/add_sensor_device + Add sensor device +* function name changed : DeviceInit() -> InitDevice() +* Fixed high CPU consumption due to busy waits +* add SensorState + add Singleton template +* XM-430 / CM-740 device file added. + Sensor device added. +* added code to support the gazebo simulator +* added first bulk read failure protection code +* renewal +* Contributors: Alexander Stumpf, ROBOTIS, ROBOTIS-zerom, pyo diff --git a/robotis_device/CHANGELOG.rst b/robotis_device/CHANGELOG.rst new file mode 100644 index 0000000..50c29fd --- /dev/null +++ b/robotis_device/CHANGELOG.rst @@ -0,0 +1,20 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package robotis_device +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.1.0 (2016-08-12) +----------- +* first public release for Kinetic +* modified the package information for release +* develop branch -> master branch +* Setting the license to BSD. +* add SensorState + add Singleton template +* XM-430 / CM-740 device file added. + Sensor device added. +* modified. +* variable name changed. + ConvertRadian2Value / ConvertValue2Radian function bug fixed. +* added code to support the gazebo simulator +* renewal +* Contributors: ROBOTIS, ROBOTIS-zerom, pyo diff --git a/robotis_framework/CHANGELOG.rst b/robotis_framework/CHANGELOG.rst new file mode 100644 index 0000000..353baf6 --- /dev/null +++ b/robotis_framework/CHANGELOG.rst @@ -0,0 +1,8 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package robotis_framework +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.1.0 (2016-08-12) +----------- +* make a meta-package +* Contributors: pyo diff --git a/robotis_framework_common/CHANGELOG.rst b/robotis_framework_common/CHANGELOG.rst new file mode 100644 index 0000000..7aacc38 --- /dev/null +++ b/robotis_framework_common/CHANGELOG.rst @@ -0,0 +1,12 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package robotis_framework_common +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.1.0 (2016-08-12) +----------- +* modified the package information for release +* develop branch -> master branch +* Setting the license to BSD. +* add SensorState + add Singleton template +* Contributors: ROBOTIS, ROBOTIS-zerom, pyo From a6c71b147905bea806273a5bea6fe60e3fd8b910 Mon Sep 17 00:00:00 2001 From: pyo Date: Fri, 12 Aug 2016 23:46:41 +0900 Subject: [PATCH 058/238] made and modified the CHANGELOG.rst for release --- robotis_controller/CHANGELOG.rst | 2 +- robotis_controller/package.xml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/robotis_controller/CHANGELOG.rst b/robotis_controller/CHANGELOG.rst index 74299e5..c81a9f1 100644 --- a/robotis_controller/CHANGELOG.rst +++ b/robotis_controller/CHANGELOG.rst @@ -2,7 +2,7 @@ Changelog for package robotis_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -0.1.1 (2016-08-12) +0.1.0 (2016-08-12) ----------- * first public release for Kinetic * modified the package information for release diff --git a/robotis_controller/package.xml b/robotis_controller/package.xml index 1320cf6..b80a81f 100644 --- a/robotis_controller/package.xml +++ b/robotis_controller/package.xml @@ -1,7 +1,7 @@ robotis_controller - 0.1.1 + 0.1.0 The robotis_controller package BSD zerom From 79126bfdc4f35a91cd481194228f438c534d2933 Mon Sep 17 00:00:00 2001 From: ROBOTIS-zerom Date: Tue, 16 Aug 2016 13:28:06 +0900 Subject: [PATCH 059/238] - package description modified. --- robotis_controller/package.xml | 4 +++- robotis_device/package.xml | 6 +++++- robotis_framework_common/package.xml | 4 +++- 3 files changed, 11 insertions(+), 3 deletions(-) diff --git a/robotis_controller/package.xml b/robotis_controller/package.xml index b80a81f..9fb1356 100644 --- a/robotis_controller/package.xml +++ b/robotis_controller/package.xml @@ -2,7 +2,9 @@ robotis_controller 0.1.0 - The robotis_controller package + + The main package that controls THORMANG3. + BSD zerom pyo diff --git a/robotis_device/package.xml b/robotis_device/package.xml index 6f7294a..3ea40fe 100644 --- a/robotis_device/package.xml +++ b/robotis_device/package.xml @@ -2,7 +2,11 @@ robotis_device 0.1.0 - The robotis_device package + + The package that manages device information of ROBOTIS robots. + This package is used when reading device information with the robot information file + from the robotis_controller package. + robotis diff --git a/robotis_framework_common/package.xml b/robotis_framework_common/package.xml index aa42d57..30c4bbb 100644 --- a/robotis_framework_common/package.xml +++ b/robotis_framework_common/package.xml @@ -2,7 +2,9 @@ robotis_framework_common 0.1.0 - The robotis_framework_common package + + The package contains commonly used Headers for the ROBOTIS Framework. + BSD zerom pyo From 71aa71ec187cc0ed0ca8294b8d194fd06d83afe7 Mon Sep 17 00:00:00 2001 From: ROBOTIS-zerom Date: Tue, 16 Aug 2016 15:57:47 +0900 Subject: [PATCH 060/238] - added direct_sync_write to MotionModuleMode - moved indirect address setting code from initialize() to initializeDevice() function. --- .../robotis_controller/robotis_controller.cpp | 229 ++++++++++-------- 1 file changed, 122 insertions(+), 107 deletions(-) diff --git a/robotis_controller/src/robotis_controller/robotis_controller.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp index 35c81b1..1bea26e 100644 --- a/robotis_controller/src/robotis_controller/robotis_controller.cpp +++ b/robotis_controller/src/robotis_controller/robotis_controller.cpp @@ -348,7 +348,119 @@ bool RobotisController::initialize(const std::string robot_file_path, const std: initializeDevice(init_file_path); + queue_thread_ = boost::thread(boost::bind(&RobotisController::msgQueueThread, this)); + return true; +} + +void RobotisController::initializeDevice(const std::string init_file_path) +{ + // device initialize + if (DEBUG_PRINT) + ROS_WARN("INIT FILE LOAD"); + + YAML::Node doc; + try + { + doc = YAML::LoadFile(init_file_path.c_str()); + + for (YAML::const_iterator it_doc = doc.begin(); it_doc != doc.end(); it_doc++) + { + std::string joint_name = it_doc->first.as(); + + YAML::Node joint_node = doc[joint_name]; + if (joint_node.size() == 0) + continue; + + Dynamixel *dxl = NULL; + auto dxl_it = robot_->dxls_.find(joint_name); + if (dxl_it != robot_->dxls_.end()) + dxl = dxl_it->second; + + if (dxl == NULL) + { + ROS_WARN("Joint [%s] was not found.", joint_name.c_str()); + continue; + } + if (DEBUG_PRINT) + ROS_INFO("JOINT_NAME: %s", joint_name.c_str()); + + for (YAML::const_iterator it_joint = joint_node.begin(); it_joint != joint_node.end(); it_joint++) + { + std::string item_name = it_joint->first.as(); + + if (DEBUG_PRINT) + ROS_INFO(" ITEM_NAME: %s", item_name.c_str()); + + uint32_t value = it_joint->second.as(); + + ControlTableItem *item = dxl->ctrl_table_[item_name]; + if (item == NULL) + { + ROS_WARN("Control Item [%s] was not found.", item_name.c_str()); + continue; + } + + if (item->memory_type_ == EEPROM) + { + uint8_t data8 = 0; + uint16_t data16 = 0; + uint32_t data32 = 0; + + switch (item->data_length_) + { + case 1: + read1Byte(joint_name, item->address_, &data8); + if (data8 == value) + continue; + break; + case 2: + read2Byte(joint_name, item->address_, &data16); + if (data16 == value) + continue; + break; + case 4: + read4Byte(joint_name, item->address_, &data32); + if (data32 == value) + continue; + break; + default: + break; + } + } + + switch (item->data_length_) + { + case 1: + write1Byte(joint_name, item->address_, (uint8_t) value); + break; + case 2: + write2Byte(joint_name, item->address_, (uint16_t) value); + break; + case 4: + write4Byte(joint_name, item->address_, value); + break; + default: + break; + } + + if (item->memory_type_ == EEPROM) + { + // Write to EEPROM -> delay is required (max delay: 55 msec per byte) + usleep(item->data_length_ * 55 * 1000); + } + } + } + } catch (const std::exception& e) + { + ROS_INFO("Dynamixel Init file not found."); + } + // [ BulkRead ] StartAddress : Present Position , Length : 10 ( Position/Velocity/Current ) + for (auto& it : robot_->ports_) + { + if (port_to_bulk_read_[it.first] != 0) + port_to_bulk_read_[it.first]->clearParam(); + } for (auto& it : robot_->dxls_) { std::string joint_name = it.first; @@ -487,113 +599,6 @@ bool RobotisController::initialize(const std::string robot_file_path, const std: if (bulkread_start_addr != 0) port_to_bulk_read_[sensor->port_name_]->addParam(sensor->id_, bulkread_start_addr, bulkread_data_length); } - - queue_thread_ = boost::thread(boost::bind(&RobotisController::msgQueueThread, this)); - return true; -} - -void RobotisController::initializeDevice(const std::string init_file_path) -{ - // device initialize - if (DEBUG_PRINT) - ROS_WARN("INIT FILE LOAD"); - - YAML::Node doc; - try - { - doc = YAML::LoadFile(init_file_path.c_str()); - - for (YAML::const_iterator it_doc = doc.begin(); it_doc != doc.end(); it_doc++) - { - std::string joint_name = it_doc->first.as(); - - YAML::Node joint_node = doc[joint_name]; - if (joint_node.size() == 0) - continue; - - Dynamixel *dxl = NULL; - auto dxl_it = robot_->dxls_.find(joint_name); - if (dxl_it != robot_->dxls_.end()) - dxl = dxl_it->second; - - if (dxl == NULL) - { - ROS_WARN("Joint [%s] was not found.", joint_name.c_str()); - continue; - } - if (DEBUG_PRINT) - ROS_INFO("JOINT_NAME: %s", joint_name.c_str()); - - for (YAML::const_iterator it_joint = joint_node.begin(); it_joint != joint_node.end(); it_joint++) - { - std::string item_name = it_joint->first.as(); - - if (DEBUG_PRINT) - ROS_INFO(" ITEM_NAME: %s", item_name.c_str()); - - uint32_t value = it_joint->second.as(); - - ControlTableItem *item = dxl->ctrl_table_[item_name]; - if (item == NULL) - { - ROS_WARN("Control Item [%s] was not found.", item_name.c_str()); - continue; - } - - if (item->memory_type_ == EEPROM) - { - uint8_t data8 = 0; - uint16_t data16 = 0; - uint32_t data32 = 0; - - switch (item->data_length_) - { - case 1: - read1Byte(joint_name, item->address_, &data8); - if (data8 == value) - continue; - break; - case 2: - read2Byte(joint_name, item->address_, &data16); - if (data16 == value) - continue; - break; - case 4: - read4Byte(joint_name, item->address_, &data32); - if (data32 == value) - continue; - break; - default: - break; - } - } - - switch (item->data_length_) - { - case 1: - write1Byte(joint_name, item->address_, (uint8_t) value); - break; - case 2: - write2Byte(joint_name, item->address_, (uint16_t) value); - break; - case 4: - write4Byte(joint_name, item->address_, value); - break; - default: - break; - } - - if (item->memory_type_ == EEPROM) - { - // Write to EEPROM -> delay is required (max delay: 55 msec per byte) - usleep(item->data_length_ * 55 * 1000); - } - } - } - } catch (const std::exception& e) - { - ROS_INFO("Dynamixel Init file not found."); - } } void RobotisController::gazeboTimerThread() @@ -1186,6 +1191,16 @@ void RobotisController::process() // SyncWrite if (gazebo_mode_ == false && do_sync_write) { + if (direct_sync_write_.size() > 0) + { + for (int i = 0; i < direct_sync_write_.size(); i++) + { + direct_sync_write_[i]->txPacket(); + direct_sync_write_[i]->clearParam(); + } + direct_sync_write_.clear(); + } + if (port_to_sync_write_position_p_gain_.size() > 0) { for (auto& it : port_to_sync_write_position_p_gain_) From ede458ace61208ecd56856721cfe05b3c982fab2 Mon Sep 17 00:00:00 2001 From: pyo Date: Wed, 17 Aug 2016 07:39:23 +0900 Subject: [PATCH 061/238] added dependency option --- robotis_controller/CMakeLists.txt | 11 +++-------- 1 file changed, 3 insertions(+), 8 deletions(-) diff --git a/robotis_controller/CMakeLists.txt b/robotis_controller/CMakeLists.txt index 1d94330..8f36296 100644 --- a/robotis_controller/CMakeLists.txt +++ b/robotis_controller/CMakeLists.txt @@ -46,14 +46,9 @@ include_directories( ${catkin_INCLUDE_DIRS} ) -add_library(robotis_controller - src/robotis_controller/robotis_controller.cpp -) - -target_link_libraries(robotis_controller - yaml-cpp - ${catkin_LIBRARIES} -) +add_library(robotis_controller src/robotis_controller/robotis_controller.cpp) +add_dependencies(robotis_controller ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(robotis_controller yaml-cpp ${catkin_LIBRARIES}) ################################################################################ # Install From 2a41c315c87cff158d7d21096eaebfa1745ad33a Mon Sep 17 00:00:00 2001 From: pyo Date: Thu, 18 Aug 2016 08:10:20 +0900 Subject: [PATCH 062/238] updated the package information --- robotis_controller/package.xml | 4 ++-- robotis_device/package.xml | 17 ++++++----------- robotis_framework/package.xml | 4 ++-- robotis_framework_common/package.xml | 7 +++++-- 4 files changed, 15 insertions(+), 17 deletions(-) diff --git a/robotis_controller/package.xml b/robotis_controller/package.xml index 9fb1356..fd27552 100644 --- a/robotis_controller/package.xml +++ b/robotis_controller/package.xml @@ -6,8 +6,8 @@ The main package that controls THORMANG3. BSD - zerom - pyo + Zerom + Pyo https://github.com/ROBOTIS-GIT/ROBOTIS-Framework/issues https://github.com/ROBOTIS-GIT/ROBOTIS-Framework http://wiki.ros.org/robotis_controller diff --git a/robotis_device/package.xml b/robotis_device/package.xml index 3ea40fe..a7f8605 100644 --- a/robotis_device/package.xml +++ b/robotis_device/package.xml @@ -7,22 +7,17 @@ This package is used when reading device information with the robot information file from the robotis_controller package. - - robotis - BSD - - - - robotis - + Zerom + Pyo + https://github.com/ROBOTIS-GIT/ROBOTIS-Framework/issues + https://github.com/ROBOTIS-GIT/ROBOTIS-Framework + http://wiki.ros.org/robotis_device catkin - roscpp rospy dynamixel_sdk - roscpp rospy dynamixel_sdk - \ No newline at end of file + diff --git a/robotis_framework/package.xml b/robotis_framework/package.xml index 170f107..f6bb99f 100644 --- a/robotis_framework/package.xml +++ b/robotis_framework/package.xml @@ -4,8 +4,8 @@ 0.1.0 ROS packages for the robotis_framework (meta package) BSD - zerom - pyo + Zerom + Pyo https://github.com/ROBOTIS-GIT/ROBOTIS-Framework/issues https://github.com/ROBOTIS-GIT/ROBOTIS-Framework http://wiki.ros.org/robotis_framework diff --git a/robotis_framework_common/package.xml b/robotis_framework_common/package.xml index 30c4bbb..b3db87b 100644 --- a/robotis_framework_common/package.xml +++ b/robotis_framework_common/package.xml @@ -6,8 +6,11 @@ The package contains commonly used Headers for the ROBOTIS Framework. BSD - zerom - pyo + Zerom + Pyo + https://github.com/ROBOTIS-GIT/ROBOTIS-Framework/issues + https://github.com/ROBOTIS-GIT/ROBOTIS-Framework + http://wiki.ros.org/robotis_framework_common catkin roscpp roscpp From 079aa4e993f88491a02bd8bc16d44242ac163771 Mon Sep 17 00:00:00 2001 From: pyo Date: Thu, 18 Aug 2016 08:10:51 +0900 Subject: [PATCH 063/238] 0.1.1 --- robotis_controller/package.xml | 2 +- robotis_device/package.xml | 2 +- robotis_framework/package.xml | 2 +- robotis_framework_common/package.xml | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/robotis_controller/package.xml b/robotis_controller/package.xml index fd27552..b86d7bf 100644 --- a/robotis_controller/package.xml +++ b/robotis_controller/package.xml @@ -1,7 +1,7 @@ robotis_controller - 0.1.0 + 0.1.1 The main package that controls THORMANG3. diff --git a/robotis_device/package.xml b/robotis_device/package.xml index a7f8605..e5caa92 100644 --- a/robotis_device/package.xml +++ b/robotis_device/package.xml @@ -1,7 +1,7 @@ robotis_device - 0.1.0 + 0.1.1 The package that manages device information of ROBOTIS robots. This package is used when reading device information with the robot information file diff --git a/robotis_framework/package.xml b/robotis_framework/package.xml index f6bb99f..809e65e 100644 --- a/robotis_framework/package.xml +++ b/robotis_framework/package.xml @@ -1,7 +1,7 @@ robotis_framework - 0.1.0 + 0.1.1 ROS packages for the robotis_framework (meta package) BSD Zerom diff --git a/robotis_framework_common/package.xml b/robotis_framework_common/package.xml index b3db87b..f01e4d5 100644 --- a/robotis_framework_common/package.xml +++ b/robotis_framework_common/package.xml @@ -1,7 +1,7 @@ robotis_framework_common - 0.1.0 + 0.1.1 The package contains commonly used Headers for the ROBOTIS Framework. From 532e746302f255439503991b159ebd3a28ae02d3 Mon Sep 17 00:00:00 2001 From: pyo Date: Thu, 18 Aug 2016 08:15:19 +0900 Subject: [PATCH 064/238] updated CHANGLOG.rst for minor release --- robotis_controller/CHANGELOG.rst | 8 +++++--- robotis_device/CHANGELOG.rst | 4 ++++ robotis_framework/CHANGELOG.rst | 6 +++++- robotis_framework_common/CHANGELOG.rst | 7 +++++-- 4 files changed, 19 insertions(+), 6 deletions(-) diff --git a/robotis_controller/CHANGELOG.rst b/robotis_controller/CHANGELOG.rst index c81a9f1..2a476ea 100644 --- a/robotis_controller/CHANGELOG.rst +++ b/robotis_controller/CHANGELOG.rst @@ -2,13 +2,15 @@ Changelog for package robotis_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.1.1 (2016-08-18) +----------- +* updated the package information + 0.1.0 (2016-08-12) ----------- * first public release for Kinetic * modified the package information for release * develop branch -> master branch -* Merge pull request `#4 `_ from ROBOTIS-GIT/add_sensor_device - Add sensor device * function name changed : DeviceInit() -> InitDevice() * Fixed high CPU consumption due to busy waits * add SensorState @@ -18,4 +20,4 @@ Changelog for package robotis_controller * added code to support the gazebo simulator * added first bulk read failure protection code * renewal -* Contributors: Alexander Stumpf, ROBOTIS, ROBOTIS-zerom, pyo +* Contributors: Alexander Stumpf, Jay Song, Zerom, Pyo diff --git a/robotis_device/CHANGELOG.rst b/robotis_device/CHANGELOG.rst index 50c29fd..9a5117c 100644 --- a/robotis_device/CHANGELOG.rst +++ b/robotis_device/CHANGELOG.rst @@ -2,6 +2,10 @@ Changelog for package robotis_device ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.1.1 (2016-08-18) +----------- +* updated the package information + 0.1.0 (2016-08-12) ----------- * first public release for Kinetic diff --git a/robotis_framework/CHANGELOG.rst b/robotis_framework/CHANGELOG.rst index 353baf6..8f61ce3 100644 --- a/robotis_framework/CHANGELOG.rst +++ b/robotis_framework/CHANGELOG.rst @@ -2,7 +2,11 @@ Changelog for package robotis_framework ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.1.1 (2016-08-18) +----------- +* updated the package information + 0.1.0 (2016-08-12) ----------- * make a meta-package -* Contributors: pyo +* Contributors: Zerom, Pyo diff --git a/robotis_framework_common/CHANGELOG.rst b/robotis_framework_common/CHANGELOG.rst index 7aacc38..bc63720 100644 --- a/robotis_framework_common/CHANGELOG.rst +++ b/robotis_framework_common/CHANGELOG.rst @@ -2,11 +2,14 @@ Changelog for package robotis_framework_common ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.1.1 (2016-08-18) +----------- +* updated the package information + 0.1.0 (2016-08-12) ----------- * modified the package information for release -* develop branch -> master branch * Setting the license to BSD. * add SensorState add Singleton template -* Contributors: ROBOTIS, ROBOTIS-zerom, pyo +* Contributors: Jay Song, Zerom, Pyo From fbddaf2df8ad82251529971acdb9d2b8373f5095 Mon Sep 17 00:00:00 2001 From: ROBOTIS-zerom Date: Mon, 22 Aug 2016 10:35:06 +0900 Subject: [PATCH 065/238] changed some debug messages. --- .../src/robotis_controller/robotis_controller.cpp | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/robotis_controller/src/robotis_controller/robotis_controller.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp index 1bea26e..2017293 100644 --- a/robotis_controller/src/robotis_controller/robotis_controller.cpp +++ b/robotis_controller/src/robotis_controller/robotis_controller.cpp @@ -673,7 +673,7 @@ void *RobotisController::timerThread(void *param) static struct timespec next_time; static struct timespec curr_time; - ROS_INFO("controller::thread_proc"); + ROS_DEBUG("controller::thread_proc started"); clock_gettime(CLOCK_MONOTONIC, &next_time); @@ -748,7 +748,7 @@ void RobotisController::startTimer() // create and start the thread if ((error = pthread_create(&this->timer_thread_, &attr, this->timerThread, this)) != 0) { - ROS_ERROR("timer thread create fail!!"); + ROS_ERROR("Creating timer thread failed!!"); exit(-1); } } @@ -1035,8 +1035,6 @@ void RobotisController::process() if ((*module_it)->getControlMode() == PositionControl) { -// if(result_state->goal_position == 0 && dxl->id == 3) -// ROS_INFO("[MODULE:%s][ID:%2d] goal position = %f", (*module_it)->GetModuleName().c_str(), dxl->id, dxl_state->goal_position); dxl_state->goal_position_ = result_state->goal_position_; if (gazebo_mode_ == false) @@ -1434,7 +1432,7 @@ void RobotisController::syncWriteItemCallback(const robotis_controller_msgs::Syn } else { - // could not find the device + ROS_WARN("[SyncWriteItem] Unknown device : %s", msg->joint_name[i].c_str()); continue; } } From adaaaea42b5cfb885d9b5412f7e0c5bc155701d8 Mon Sep 17 00:00:00 2001 From: ROBOTIS-zerom Date: Mon, 22 Aug 2016 15:49:38 +0900 Subject: [PATCH 066/238] - bug fixed (position pid gain & velocity pid gain sync write). - added velocity_to_value_ratio to DXL Pro-H series. --- .../robotis_controller/robotis_controller.cpp | 10 +++---- .../devices/dynamixel/H42-20-S300-R.device | 2 +- .../devices/dynamixel/H54-100-S500-R.device | 2 +- .../devices/dynamixel/H54-200-B500-R.device | 2 +- .../devices/dynamixel/H54-200-S500-R.device | 2 +- .../include/robotis_device/dynamixel_state.h | 26 ++++++++-------- robotis_device/src/robotis_device/robot.cpp | 30 +++++++++++++++++++ 7 files changed, 52 insertions(+), 22 deletions(-) diff --git a/robotis_controller/src/robotis_controller/robotis_controller.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp index 2017293..acbe656 100644 --- a/robotis_controller/src/robotis_controller/robotis_controller.cpp +++ b/robotis_controller/src/robotis_controller/robotis_controller.cpp @@ -1047,11 +1047,11 @@ void RobotisController::process() sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(pos_data)); sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(pos_data)); - if (abs(pos_data) > 151800) - { - printf("goal_pos : %f | position_offset : %f | pos_data : %d\n", - dxl_state->goal_position_, dxl_state->position_offset_, pos_data); - } +// if (abs(pos_data) > 151800) +// { +// printf("goal_pos : %f | position_offset : %f | pos_data : %d\n", +// dxl_state->goal_position_, dxl_state->position_offset_, pos_data); +// } if (port_to_sync_write_position_[dxl->port_name_] != NULL) port_to_sync_write_position_[dxl->port_name_]->changeParam(dxl->id_, sync_write_data); diff --git a/robotis_device/devices/dynamixel/H42-20-S300-R.device b/robotis_device/devices/dynamixel/H42-20-S300-R.device index 95686ff..f669d8d 100644 --- a/robotis_device/devices/dynamixel/H42-20-S300-R.device +++ b/robotis_device/devices/dynamixel/H42-20-S300-R.device @@ -4,7 +4,7 @@ device_type = dynamixel [type info] torque_to_current_value_ratio = 27.15146 - +velocity_to_value_ratio = 2900.59884 value_of_0_radian_position = 0 value_of_min_radian_position = -151900 value_of_max_radian_position = 151900 diff --git a/robotis_device/devices/dynamixel/H54-100-S500-R.device b/robotis_device/devices/dynamixel/H54-100-S500-R.device index a251ddd..96fffa3 100644 --- a/robotis_device/devices/dynamixel/H54-100-S500-R.device +++ b/robotis_device/devices/dynamixel/H54-100-S500-R.device @@ -4,7 +4,7 @@ device_type = dynamixel [type info] torque_to_current_value_ratio = 9.66026 - +velocity_to_value_ratio = 4793.01226 value_of_0_radian_position = 0 value_of_min_radian_position = -250950 value_of_max_radian_position = 250950 diff --git a/robotis_device/devices/dynamixel/H54-200-B500-R.device b/robotis_device/devices/dynamixel/H54-200-B500-R.device index 20a280a..aa79b4b 100644 --- a/robotis_device/devices/dynamixel/H54-200-B500-R.device +++ b/robotis_device/devices/dynamixel/H54-200-B500-R.device @@ -4,7 +4,7 @@ device_type = dynamixel [type info] torque_to_current_value_ratio = 9.09201 - +velocity_to_value_ratio = 4793.01226 value_of_0_radian_position = 0 value_of_min_radian_position = -250950 value_of_max_radian_position = 250950 diff --git a/robotis_device/devices/dynamixel/H54-200-S500-R.device b/robotis_device/devices/dynamixel/H54-200-S500-R.device index 5b7dc76..76f6300 100644 --- a/robotis_device/devices/dynamixel/H54-200-S500-R.device +++ b/robotis_device/devices/dynamixel/H54-200-S500-R.device @@ -4,7 +4,7 @@ device_type = dynamixel [type info] torque_to_current_value_ratio = 9.09201 - +velocity_to_value_ratio = 4793.01226 value_of_0_radian_position = 0 value_of_min_radian_position = -250950 value_of_max_radian_position = 250950 diff --git a/robotis_device/include/robotis_device/dynamixel_state.h b/robotis_device/include/robotis_device/dynamixel_state.h index 5f4ae36..57de9f6 100644 --- a/robotis_device/include/robotis_device/dynamixel_state.h +++ b/robotis_device/include/robotis_device/dynamixel_state.h @@ -59,12 +59,12 @@ public: double goal_position_; double goal_velocity_; double goal_torque_; - double position_p_gain_; - double position_i_gain_; - double position_d_gain_; - double velocity_p_gain_; - double velocity_i_gain_; - double velocity_d_gain_; + int position_p_gain_; + int position_i_gain_; + int position_d_gain_; + int velocity_p_gain_; + int velocity_i_gain_; + int velocity_d_gain_; std::map bulk_read_table_; @@ -78,13 +78,13 @@ public: goal_position_(0.0), goal_velocity_(0.0), goal_torque_(0.0), - position_p_gain_(0.0), - position_i_gain_(0.0), - position_d_gain_(0.0), - velocity_p_gain_(0.0), - velocity_i_gain_(0.0), - velocity_d_gain_(0.0), - position_offset_(0.0) + position_p_gain_(0), + position_i_gain_(0), + position_d_gain_(0), + velocity_p_gain_(0), + velocity_i_gain_(0), + velocity_d_gain_(0), + position_offset_(0) { bulk_read_table_.clear(); } diff --git a/robotis_device/src/robotis_device/robot.cpp b/robotis_device/src/robotis_device/robot.cpp index 900a9b9..3fc9b8b 100644 --- a/robotis_device/src/robotis_device/robot.cpp +++ b/robotis_device/src/robotis_device/robot.cpp @@ -327,6 +327,12 @@ Dynamixel *Robot::getDynamixel(std::string path, int id, std::string port, float std::string goal_position_item_name = ""; std::string goal_velocity_item_name = ""; std::string goal_current_item_name = ""; + std::string position_d_gain_item_name = ""; + std::string position_i_gain_item_name = ""; + std::string position_p_gain_item_name = ""; + std::string velocity_d_gain_item_name = ""; + std::string velocity_i_gain_item_name = ""; + std::string velocity_p_gain_item_name = ""; while (!file.eof()) { @@ -396,6 +402,18 @@ Dynamixel *Robot::getDynamixel(std::string path, int id, std::string port, float goal_velocity_item_name = tokens[1]; else if (tokens[0] == "goal_current_item_name") goal_current_item_name = tokens[1]; + else if (tokens[0] == "position_d_gain_item_name") + position_d_gain_item_name = tokens[1]; + else if (tokens[0] == "position_i_gain_item_name") + position_i_gain_item_name = tokens[1]; + else if (tokens[0] == "position_p_gain_item_name") + position_p_gain_item_name = tokens[1]; + else if (tokens[0] == "velocity_d_gain_item_name") + velocity_d_gain_item_name = tokens[1]; + else if (tokens[0] == "velocity_i_gain_item_name") + velocity_i_gain_item_name = tokens[1]; + else if (tokens[0] == "velocity_p_gain_item_name") + velocity_p_gain_item_name = tokens[1]; } else if (session == SESSION_CONTROL_TABLE) { @@ -444,6 +462,18 @@ Dynamixel *Robot::getDynamixel(std::string path, int id, std::string port, float dxl->goal_velocity_item_ = dxl->ctrl_table_[goal_velocity_item_name]; if (dxl->ctrl_table_[goal_current_item_name] != NULL) dxl->goal_current_item_ = dxl->ctrl_table_[goal_current_item_name]; + if (dxl->ctrl_table_[position_d_gain_item_name] != NULL) + dxl->position_d_gain_item_ = dxl->ctrl_table_[position_d_gain_item_name]; + if (dxl->ctrl_table_[position_i_gain_item_name] != NULL) + dxl->position_i_gain_item_ = dxl->ctrl_table_[position_i_gain_item_name]; + if (dxl->ctrl_table_[position_p_gain_item_name] != NULL) + dxl->position_p_gain_item_ = dxl->ctrl_table_[position_p_gain_item_name]; + if (dxl->ctrl_table_[velocity_d_gain_item_name] != NULL) + dxl->velocity_d_gain_item_ = dxl->ctrl_table_[velocity_d_gain_item_name]; + if (dxl->ctrl_table_[velocity_i_gain_item_name] != NULL) + dxl->velocity_i_gain_item_ = dxl->ctrl_table_[velocity_i_gain_item_name]; + if (dxl->ctrl_table_[velocity_p_gain_item_name] != NULL) + dxl->velocity_p_gain_item_ = dxl->ctrl_table_[velocity_p_gain_item_name]; fprintf(stderr, "(%s) [ID:%3d] %14s added. \n", port.c_str(), dxl->id_, dxl->model_name_.c_str()); //std::cout << "[ID:" << (int)(_dxl->id) << "] " << _dxl->model_name << " added. (" << port << ")" << std::endl; From ed8b0b54d6de35ce20d0d4d56ae9c616c892fdb6 Mon Sep 17 00:00:00 2001 From: ROBOTIS-zerom Date: Fri, 26 Aug 2016 18:36:00 +0900 Subject: [PATCH 067/238] - reduce CPU consumption --- .../src/robotis_controller/robotis_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/robotis_controller/src/robotis_controller/robotis_controller.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp index acbe656..1cb160f 100644 --- a/robotis_controller/src/robotis_controller/robotis_controller.cpp +++ b/robotis_controller/src/robotis_controller/robotis_controller.cpp @@ -663,7 +663,7 @@ void RobotisController::msgQueueThread() while (ros_node.ok()) { callback_queue.callAvailable(); - usleep(100); + usleep(1000); } } From 9947dbb6743781b4532da3503de5cb1a9ab0d227 Mon Sep 17 00:00:00 2001 From: pyo Date: Wed, 31 Aug 2016 08:08:18 +0900 Subject: [PATCH 068/238] modified the package information for new release --- robotis_controller/CHANGELOG.rst | 23 +++++++++++++++++++++++ robotis_controller/package.xml | 2 +- robotis_device/CHANGELOG.rst | 13 +++++++++++++ robotis_device/package.xml | 2 +- robotis_framework/CHANGELOG.rst | 5 ++++- robotis_framework/package.xml | 2 +- robotis_framework_common/CHANGELOG.rst | 7 +++++++ robotis_framework_common/package.xml | 2 +- 8 files changed, 51 insertions(+), 5 deletions(-) diff --git a/robotis_controller/CHANGELOG.rst b/robotis_controller/CHANGELOG.rst index 2a476ea..dd4e11c 100644 --- a/robotis_controller/CHANGELOG.rst +++ b/robotis_controller/CHANGELOG.rst @@ -2,6 +2,29 @@ Changelog for package robotis_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.2.0 (2016-08-31) +----------- +* bug fixed (position pid gain & velocity pid gain sync write). +* added velocity_to_value_ratio to DXL Pro-H series. +* changed some debug messages. +* added velocity p/i/d gain and position i/d gain sync_write code. +* SyncWriteItem bug fixed. +* add function / modified the code simple (using auto / range based for loop) +* added XM-430-W210 / XM-430-W350 device file. +* rename ControlMode(CurrentControl -> TorqueControl) +* rename (port_to_sync_write_torque\_ -> port_to_sync_write_current\_) +* rename (present_current\_ -> present_torque\_) +* modified torque control code +* fixed typos / changed ROS_INFO -> fprintf (for processing speed) +* startTimer() : after bulkread txpacket(), need some sleep() +* changed the order of processing in the Process() function. +* added missing mutex for gazebo +* fixed crash when running in gazebo simulation +* sync write bug fix. +* added position_p_gain sync write +* MotionModule/SensorModule member variable access changed (public -> protected). +* Contributors: Jay Song, Zerom, Pyo, SCH + 0.1.1 (2016-08-18) ----------- * updated the package information diff --git a/robotis_controller/package.xml b/robotis_controller/package.xml index b86d7bf..f1be9e2 100644 --- a/robotis_controller/package.xml +++ b/robotis_controller/package.xml @@ -1,7 +1,7 @@ robotis_controller - 0.1.1 + 0.2.0 The main package that controls THORMANG3. diff --git a/robotis_device/CHANGELOG.rst b/robotis_device/CHANGELOG.rst index 9a5117c..ac3e9d9 100644 --- a/robotis_device/CHANGELOG.rst +++ b/robotis_device/CHANGELOG.rst @@ -2,6 +2,19 @@ Changelog for package robotis_device ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.2.0 (2016-08-31) +----------- +* bug fixed (position pid gain & velocity pid gain sync write). +* added velocity_to_value_ratio to DXL Pro-H series. +* added velocity p/i/d gain and position i/d gain sync_write code. +* fixed robotis_device build_depend. +* added XM-430-W210 / XM-430-W350 device file. +* rename (present_current\_ -> present_torque\_) +* modified torque control code +* added device file for MX-64 / MX-106 +* adjusted position min/max value. (MX-28, XM-430) +* Contributors: Zerom, Pyo + 0.1.1 (2016-08-18) ----------- * updated the package information diff --git a/robotis_device/package.xml b/robotis_device/package.xml index e5caa92..f889c3f 100644 --- a/robotis_device/package.xml +++ b/robotis_device/package.xml @@ -1,7 +1,7 @@ robotis_device - 0.1.1 + 0.2.0 The package that manages device information of ROBOTIS robots. This package is used when reading device information with the robot information file diff --git a/robotis_framework/CHANGELOG.rst b/robotis_framework/CHANGELOG.rst index 8f61ce3..513131b 100644 --- a/robotis_framework/CHANGELOG.rst +++ b/robotis_framework/CHANGELOG.rst @@ -2,6 +2,10 @@ Changelog for package robotis_framework ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.2.0 (2016-08-31) +----------- +* updated CHANGLOG.rst for minor release + 0.1.1 (2016-08-18) ----------- * updated the package information @@ -9,4 +13,3 @@ Changelog for package robotis_framework 0.1.0 (2016-08-12) ----------- * make a meta-package -* Contributors: Zerom, Pyo diff --git a/robotis_framework/package.xml b/robotis_framework/package.xml index 809e65e..09a449a 100644 --- a/robotis_framework/package.xml +++ b/robotis_framework/package.xml @@ -1,7 +1,7 @@ robotis_framework - 0.1.1 + 0.2.0 ROS packages for the robotis_framework (meta package) BSD Zerom diff --git a/robotis_framework_common/CHANGELOG.rst b/robotis_framework_common/CHANGELOG.rst index bc63720..e5a510b 100644 --- a/robotis_framework_common/CHANGELOG.rst +++ b/robotis_framework_common/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package robotis_framework_common ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.2.0 (2016-08-31) +----------- +* updated CHANGLOG.rst for minor release +* rename ControlMode(CurrentControl -> TorqueControl) +* rename (port_to_sync_write_torque\_ -> port_to_sync_write_current\_) +* Contributors: Zerom, Pyo + 0.1.1 (2016-08-18) ----------- * updated the package information diff --git a/robotis_framework_common/package.xml b/robotis_framework_common/package.xml index f01e4d5..7cce1b6 100644 --- a/robotis_framework_common/package.xml +++ b/robotis_framework_common/package.xml @@ -1,7 +1,7 @@ robotis_framework_common - 0.1.1 + 0.2.0 The package contains commonly used Headers for the ROBOTIS Framework. From 7f15d5639ebea3a1f3eab3b9580aebfe095395d0 Mon Sep 17 00:00:00 2001 From: ROBOTIS-zerom Date: Wed, 31 Aug 2016 09:46:36 +0900 Subject: [PATCH 069/238] - modified dependency problem. --- robotis_controller/CMakeLists.txt | 2 +- robotis_device/CMakeLists.txt | 4 ++-- robotis_framework_common/CMakeLists.txt | 4 ++++ robotis_framework_common/package.xml | 2 ++ 4 files changed, 9 insertions(+), 3 deletions(-) diff --git a/robotis_controller/CMakeLists.txt b/robotis_controller/CMakeLists.txt index 8f36296..f7f73b3 100644 --- a/robotis_controller/CMakeLists.txt +++ b/robotis_controller/CMakeLists.txt @@ -47,7 +47,7 @@ include_directories( ) add_library(robotis_controller src/robotis_controller/robotis_controller.cpp) -add_dependencies(robotis_controller ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +add_dependencies(robotis_controller ${catkin_EXPORTED_TARGETS}) target_link_libraries(robotis_controller yaml-cpp ${catkin_LIBRARIES}) ################################################################################ diff --git a/robotis_device/CMakeLists.txt b/robotis_device/CMakeLists.txt index abc1965..1fac3f4 100644 --- a/robotis_device/CMakeLists.txt +++ b/robotis_device/CMakeLists.txt @@ -27,7 +27,7 @@ find_package(catkin REQUIRED COMPONENTS catkin_package( INCLUDE_DIRS include LIBRARIES robotis_device - CATKIN_DEPENDS roscpp rospy + CATKIN_DEPENDS roscpp rospy dynamixel_sdk ) ################################################################################ @@ -43,7 +43,7 @@ add_library(robotis_device src/robotis_device/sensor.cpp src/robotis_device/dynamixel.cpp ) -add_dependencies(robotis_device ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +add_dependencies(robotis_device ${catkin_EXPORTED_TARGETS}) target_link_libraries(robotis_device ${catkin_LIBRARIES}) ################################################################################ diff --git a/robotis_framework_common/CMakeLists.txt b/robotis_framework_common/CMakeLists.txt index 20c330f..430fbee 100644 --- a/robotis_framework_common/CMakeLists.txt +++ b/robotis_framework_common/CMakeLists.txt @@ -9,6 +9,7 @@ project(robotis_framework_common) ################################################################################ find_package(catkin REQUIRED COMPONENTS roscpp + robotis_device ) ################################################################################ @@ -24,12 +25,15 @@ find_package(catkin REQUIRED COMPONENTS ################################################################################ catkin_package( INCLUDE_DIRS include +# LIBRARIES robotis_framework_common + CATKIN_DEPENDS roscpp robotis_device ) ################################################################################ # Build ################################################################################ include_directories( + include ${catkin_INCLUDE_DIRS} ) diff --git a/robotis_framework_common/package.xml b/robotis_framework_common/package.xml index f01e4d5..2e66361 100644 --- a/robotis_framework_common/package.xml +++ b/robotis_framework_common/package.xml @@ -13,5 +13,7 @@ http://wiki.ros.org/robotis_framework_common catkin roscpp + robotis_device roscpp + robotis_device From 09d54945f74af783d0969944b495aadcba6c919d Mon Sep 17 00:00:00 2001 From: ROBOTIS-zerom Date: Fri, 2 Sep 2016 11:29:37 +0900 Subject: [PATCH 070/238] - make setJointCtrlModuleCallback() to the thread function & improved. --- .../robotis_controller/robotis_controller.h | 2 + .../robotis_controller/robotis_controller.cpp | 276 +++++++++++++----- 2 files changed, 207 insertions(+), 71 deletions(-) diff --git a/robotis_controller/include/robotis_controller/robotis_controller.h b/robotis_controller/include/robotis_controller/robotis_controller.h index 65d7821..4014b29 100644 --- a/robotis_controller/include/robotis_controller/robotis_controller.h +++ b/robotis_controller/include/robotis_controller/robotis_controller.h @@ -88,6 +88,7 @@ private: void gazeboTimerThread(); void msgQueueThread(); void setCtrlModuleThread(std::string ctrl_module); + void setJointCtrlModuleThread(const robotis_controller_msgs::JointCtrlModule::ConstPtr &msg); bool isTimerStopped(); void initializeSyncWrite(); @@ -141,6 +142,7 @@ public: void stopTimer(); bool isTimerRunning(); + void setCtrlModule(std::string module_name); void loadOffset(const std::string path); /* ROS Topic Callback Functions */ diff --git a/robotis_controller/src/robotis_controller/robotis_controller.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp index 1cb160f..a22c6ae 100644 --- a/robotis_controller/src/robotis_controller/robotis_controller.cpp +++ b/robotis_controller/src/robotis_controller/robotis_controller.cpp @@ -1529,75 +1529,18 @@ void RobotisController::setCtrlModuleCallback(const std_msgs::String::ConstPtr & set_module_thread_ = boost::thread(boost::bind(&RobotisController::setCtrlModuleThread, this, _module_name_to_set)); } +void RobotisController::setCtrlModule(std::string module_name) +{ + set_module_thread_ = boost::thread(boost::bind(&RobotisController::setCtrlModuleThread, this, module_name)); +} void RobotisController::setJointCtrlModuleCallback(const robotis_controller_msgs::JointCtrlModule::ConstPtr &msg) { if (msg->joint_name.size() != msg->module_name.size()) return; - queue_mutex_.lock(); - - for (unsigned int idx = 0; idx < msg->joint_name.size(); idx++) - { - Dynamixel *dxl = NULL; - auto dxl_it = robot_->dxls_.find((std::string) (msg->joint_name[idx])); - if (dxl_it != robot_->dxls_.end()) - dxl = dxl_it->second; - else - continue; - - // none - if (msg->module_name[idx] == "" || msg->module_name[idx] == "none") - { - dxl->ctrl_module_name_ = msg->module_name[idx]; - continue; - } - - // check whether the module is using this joint - for (auto m_it = motion_modules_.begin(); m_it != motion_modules_.end(); m_it++) - { - if ((*m_it)->getModuleName() == msg->module_name[idx]) - { - if ((*m_it)->result_.find(msg->joint_name[idx]) != (*m_it)->result_.end()) - { - dxl->ctrl_module_name_ = msg->module_name[idx]; - break; - } - } - } + set_module_thread_ = boost::thread(boost::bind(&RobotisController::setJointCtrlModuleThread, this, msg)); } - for (auto m_it = motion_modules_.begin(); m_it != motion_modules_.end(); m_it++) - { - // set all modules -> disable - (*m_it)->setModuleEnable(false); - - // set all used modules -> enable - for (auto& d_it : robot_->dxls_) - { - if (d_it.second->ctrl_module_name_ == (*m_it)->getModuleName()) - { - (*m_it)->setModuleEnable(true); - break; - } - } - } - - // TODO: set indirect address - // -> check module's control_mode - - queue_mutex_.unlock(); - - robotis_controller_msgs::JointCtrlModule current_module_msg; - for (auto& dxl_iter : robot_->dxls_) - { - current_module_msg.joint_name.push_back(dxl_iter.first); - current_module_msg.module_name.push_back(dxl_iter.second->ctrl_module_name_); - } - - if (current_module_msg.joint_name.size() == current_module_msg.module_name.size()) - current_module_pub_.publish(current_module_msg); -} - bool RobotisController::getCtrlModuleCallback(robotis_controller_msgs::GetJointModule::Request &req, robotis_controller_msgs::GetJointModule::Response &res) { @@ -1617,6 +1560,199 @@ bool RobotisController::getCtrlModuleCallback(robotis_controller_msgs::GetJointM return true; } +void RobotisController::setJointCtrlModuleThread(const robotis_controller_msgs::JointCtrlModule::ConstPtr &msg) +{ + // stop module list + std::list _stop_modules; + std::list _enable_modules; + + for(unsigned int idx = 0; idx < msg->joint_name.size(); idx++) + { + Dynamixel *_dxl = NULL; + std::map::iterator _dxl_it = robot_->dxls_.find((std::string)(msg->joint_name[idx])); + if(_dxl_it != robot_->dxls_.end()) + _dxl = _dxl_it->second; + else + continue; + + // enqueue + if(_dxl->ctrl_module_name_ != msg->module_name[idx]) + { + for(std::list::iterator _stop_m_it = motion_modules_.begin(); _stop_m_it != motion_modules_.end(); _stop_m_it++) + { + if((*_stop_m_it)->getModuleName() == _dxl->ctrl_module_name_ && (*_stop_m_it)->getModuleEnable() == true) + _stop_modules.push_back(*_stop_m_it); + } + } + } + + // stop the module + _stop_modules.unique(); + for(std::list::iterator _stop_m_it = _stop_modules.begin(); _stop_m_it != _stop_modules.end(); _stop_m_it++) + { + (*_stop_m_it)->stop(); + } + + // wait to stop + for(std::list::iterator _stop_m_it = _stop_modules.begin(); _stop_m_it != _stop_modules.end(); _stop_m_it++) + { + while((*_stop_m_it)->isRunning()) + usleep(CONTROL_CYCLE_MSEC * 1000); + } + + // disable module(s) + for(std::list::iterator _stop_m_it = _stop_modules.begin(); _stop_m_it != _stop_modules.end(); _stop_m_it++) + { + (*_stop_m_it)->setModuleEnable(false); + } + + // set ctrl module + queue_mutex_.lock(); + + for(unsigned int idx = 0; idx < msg->joint_name.size(); idx++) + { + std::string ctrl_module = msg->module_name[idx]; + std::string joint_name = msg->joint_name[idx]; + + Dynamixel *_dxl = NULL; + std::map::iterator _dxl_it = robot_->dxls_.find(joint_name); + if(_dxl_it != robot_->dxls_.end()) + _dxl = _dxl_it->second; + else + continue; + + // none + if(ctrl_module == "" || ctrl_module == "none") + { + _dxl->ctrl_module_name_ = "none"; + + if(gazebo_mode_ == true) + continue; + + uint32_t _pos_data = _dxl->convertRadian2Value(_dxl->dxl_state_->goal_position_ + _dxl->dxl_state_->position_offset_); + uint8_t _sync_write_data[4]; + _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_pos_data)); + _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_pos_data)); + _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_pos_data)); + _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_pos_data)); + + if(port_to_sync_write_position_[_dxl->port_name_] != NULL) + port_to_sync_write_position_[_dxl->port_name_]->addParam(_dxl->id_, _sync_write_data); + + if(port_to_sync_write_current_[_dxl->port_name_] != NULL) + port_to_sync_write_current_[_dxl->port_name_]->removeParam(_dxl->id_); + if(port_to_sync_write_velocity_[_dxl->port_name_] != NULL) + port_to_sync_write_velocity_[_dxl->port_name_]->removeParam(_dxl->id_); + } + else + { + // check whether the module exist + for(std::list::iterator _m_it = motion_modules_.begin(); _m_it != motion_modules_.end(); _m_it++) + { + // if it exist + if((*_m_it)->getModuleName() == ctrl_module) + { + std::map::iterator _result_it = (*_m_it)->result_.find(joint_name); + if(_result_it == (*_m_it)->result_.end()) + break; + + _dxl->ctrl_module_name_ = ctrl_module; + + // enqueue enable module list + _enable_modules.push_back(*_m_it); + ControlMode _mode = (*_m_it)->getControlMode(); + + if(gazebo_mode_ == true) + break; + + if(_mode == PositionControl) + { + uint32_t _pos_data = _dxl->convertRadian2Value(_dxl->dxl_state_->goal_position_ + _dxl->dxl_state_->position_offset_); + uint8_t _sync_write_data[4]; + _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_pos_data)); + _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_pos_data)); + _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_pos_data)); + _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_pos_data)); + + if(port_to_sync_write_position_[_dxl->port_name_] != NULL) + port_to_sync_write_position_[_dxl->port_name_]->addParam(_dxl->id_, _sync_write_data); + + if(port_to_sync_write_current_[_dxl->port_name_] != NULL) + port_to_sync_write_current_[_dxl->port_name_]->removeParam(_dxl->id_); + if(port_to_sync_write_velocity_[_dxl->port_name_] != NULL) + port_to_sync_write_velocity_[_dxl->port_name_]->removeParam(_dxl->id_); + } + else if(_mode == VelocityControl) + { + uint32_t _vel_data = _dxl->convertVelocity2Value(_dxl->dxl_state_->goal_velocity_); + uint8_t _sync_write_data[4]; + _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_vel_data)); + _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_vel_data)); + _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_vel_data)); + _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_vel_data)); + + if(port_to_sync_write_velocity_[_dxl->port_name_] != NULL) + port_to_sync_write_velocity_[_dxl->port_name_]->addParam(_dxl->id_, _sync_write_data); + + if(port_to_sync_write_current_[_dxl->port_name_] != NULL) + port_to_sync_write_current_[_dxl->port_name_]->removeParam(_dxl->id_); + if(port_to_sync_write_position_[_dxl->port_name_] != NULL) + port_to_sync_write_position_[_dxl->port_name_]->removeParam(_dxl->id_); + } + else if(_mode == TorqueControl) + { + uint32_t _curr_data = _dxl->convertTorque2Value(_dxl->dxl_state_->goal_torque_); + uint8_t _sync_write_data[4]; + _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_curr_data)); + _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_curr_data)); + _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_curr_data)); + _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_curr_data)); + + if(port_to_sync_write_current_[_dxl->port_name_] != NULL) + port_to_sync_write_current_[_dxl->port_name_]->addParam(_dxl->id_, _sync_write_data); + + if(port_to_sync_write_velocity_[_dxl->port_name_] != NULL) + port_to_sync_write_velocity_[_dxl->port_name_]->removeParam(_dxl->id_); + if(port_to_sync_write_position_[_dxl->port_name_] != NULL) + port_to_sync_write_position_[_dxl->port_name_]->removeParam(_dxl->id_); + } + break; + } + } + } + } + + // enable module(s) + _enable_modules.unique(); + for(std::list::iterator _m_it = _enable_modules.begin(); _m_it != _enable_modules.end(); _m_it++) + { + (*_m_it)->setModuleEnable(true); + } + + // TODO: set indirect address + // -> check module's control_mode + + queue_mutex_.unlock(); + + // log +// std::cout << "Enable Joint Ctrl Module : " << std::endl; +// for(std::list::iterator _m_it = motion_modules_.begin(); _m_it != motion_modules_.end(); _m_it++) +// { +// if((*_m_it)->GetModuleEnable() == true) +// std::cout << " - " << (*_m_it)->GetModuleName() << std::endl; +// } + + // publish current module + robotis_controller_msgs::JointCtrlModule _current_module_msg; + for(std::map::iterator _dxl_iter = robot_->dxls_.begin(); _dxl_iter != robot_->dxls_.end(); ++_dxl_iter) + { + _current_module_msg.joint_name.push_back(_dxl_iter->first); + _current_module_msg.module_name.push_back(_dxl_iter->second->ctrl_module_name_); + } + + if(_current_module_msg.joint_name.size() == _current_module_msg.module_name.size()) + current_module_pub_.publish(_current_module_msg); +} void RobotisController::setCtrlModuleThread(std::string ctrl_module) { // stop module @@ -1679,6 +1815,13 @@ void RobotisController::setCtrlModuleThread(std::string ctrl_module) usleep(CONTROL_CYCLE_MSEC * 1000); } + // disable module(s) + for(std::list::iterator _stop_m_it = stop_modules.begin(); _stop_m_it != stop_modules.end(); _stop_m_it++) + { + (*_stop_m_it)->setModuleEnable(false); + } + + // set ctrl module queue_mutex_.lock(); @@ -1688,12 +1831,6 @@ void RobotisController::setCtrlModuleThread(std::string ctrl_module) // none if ((ctrl_module == "") || (ctrl_module == "none")) { - // set all modules -> disable - for (auto m_it = motion_modules_.begin(); m_it != motion_modules_.end(); m_it++) - { - (*m_it)->setModuleEnable(false); - } - // set dxl's control module to "none" for (auto& d_it : robot_->dxls_) { @@ -1800,9 +1937,6 @@ void RobotisController::setCtrlModuleThread(std::string ctrl_module) for (auto m_it = motion_modules_.begin(); m_it != motion_modules_.end(); m_it++) { - // set all modules -> disable - (*m_it)->setModuleEnable(false); - // set all used modules -> enable for (auto& d_it : robot_->dxls_) { From 219b64db57acc20be79e4103b93b8c6699b01e37 Mon Sep 17 00:00:00 2001 From: ROBOTIS-zerom Date: Fri, 2 Sep 2016 15:19:22 +0900 Subject: [PATCH 071/238] - dependencies fixed. (Pull requests #26) --- robotis_controller/CMakeLists.txt | 4 +--- robotis_controller/package.xml | 7 ++----- robotis_framework_common/CMakeLists.txt | 8 +++++++- 3 files changed, 10 insertions(+), 9 deletions(-) diff --git a/robotis_controller/CMakeLists.txt b/robotis_controller/CMakeLists.txt index f7f73b3..4830cf7 100644 --- a/robotis_controller/CMakeLists.txt +++ b/robotis_controller/CMakeLists.txt @@ -14,11 +14,9 @@ find_package(catkin REQUIRED COMPONENTS roslib sensor_msgs std_msgs - robotis_device robotis_controller_msgs robotis_framework_common cmake_modules - dynamixel_sdk ) ################################################################################ @@ -35,7 +33,7 @@ find_package(catkin REQUIRED COMPONENTS catkin_package( INCLUDE_DIRS include LIBRARIES robotis_controller - CATKIN_DEPENDS roscpp roslib sensor_msgs std_msgs + CATKIN_DEPENDS roscpp roslib sensor_msgs std_msgs robotis_framework_common ) ################################################################################ diff --git a/robotis_controller/package.xml b/robotis_controller/package.xml index b86d7bf..9085521 100644 --- a/robotis_controller/package.xml +++ b/robotis_controller/package.xml @@ -16,16 +16,13 @@ roslib std_msgs sensor_msgs - dynamixel_sdk - robotis_device robotis_controller_msgs robotis_framework_common roscpp roslib std_msgs sensor_msgs - dynamixel_sdk - robotis_device robotis_controller_msgs - + robotis_framework_common + diff --git a/robotis_framework_common/CMakeLists.txt b/robotis_framework_common/CMakeLists.txt index 430fbee..c0505b1 100644 --- a/robotis_framework_common/CMakeLists.txt +++ b/robotis_framework_common/CMakeLists.txt @@ -25,7 +25,7 @@ find_package(catkin REQUIRED COMPONENTS ################################################################################ catkin_package( INCLUDE_DIRS include -# LIBRARIES robotis_framework_common + LIBRARIES robotis_framework_common CATKIN_DEPENDS roscpp robotis_device ) @@ -36,6 +36,12 @@ include_directories( include ${catkin_INCLUDE_DIRS} ) +add_library(${PROJECT_NAME} + include/${PROJECT_NAME}/motion_module.h + include/${PROJECT_NAME}/sensor_module.h + include/${PROJECT_NAME}/singleton.h +) +set_target_properties(${PROJECT_NAME} PROPERTIES LINKER_LANGUAGE CXX) ################################################################################ # Install From c653a07722929fff9259cb68540a5b00271c4157 Mon Sep 17 00:00:00 2001 From: ROBOTIS-zerom Date: Tue, 20 Sep 2016 11:55:29 +0900 Subject: [PATCH 072/238] - convertRadian2Value / convertValue2Radian : commented out the code that limits the maximum/minimum value. --- robotis_device/src/robotis_device/dynamixel.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/robotis_device/src/robotis_device/dynamixel.cpp b/robotis_device/src/robotis_device/dynamixel.cpp index cdd5470..16ef12e 100644 --- a/robotis_device/src/robotis_device/dynamixel.cpp +++ b/robotis_device/src/robotis_device/dynamixel.cpp @@ -93,10 +93,10 @@ double Dynamixel::convertValue2Radian(int32_t value) / (double) (value_of_min_radian_position_ - value_of_0_radian_position_); } - if (radian > max_radian_) - return max_radian_; - else if (radian < min_radian_) - return min_radian_; +// if (radian > max_radian_) +// return max_radian_; +// else if (radian < min_radian_) +// return min_radian_; return radian; } @@ -123,10 +123,10 @@ int32_t Dynamixel::convertRadian2Value(double radian) else value = value_of_0_radian_position_; - if (value > value_of_max_radian_position_) - return value_of_max_radian_position_; - else if (value < value_of_min_radian_position_) - return value_of_min_radian_position_; +// if (value > value_of_max_radian_position_) +// return value_of_max_radian_position_; +// else if (value < value_of_min_radian_position_) +// return value_of_min_radian_position_; return value; } From 176c64ec6766247a61271323274e0e102f62363d Mon Sep 17 00:00:00 2001 From: ROBOTIS-zerom Date: Tue, 20 Sep 2016 14:25:10 +0900 Subject: [PATCH 073/238] - robotis_controller process() : processing order changed. * 1st : packet communication * 2nd : processing modules --- .../robotis_controller/robotis_controller.cpp | 441 +++++++++--------- 1 file changed, 218 insertions(+), 223 deletions(-) diff --git a/robotis_controller/src/robotis_controller/robotis_controller.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp index a22c6ae..ccbdec0 100644 --- a/robotis_controller/src/robotis_controller/robotis_controller.cpp +++ b/robotis_controller/src/robotis_controller/robotis_controller.cpp @@ -876,7 +876,6 @@ void RobotisController::process() is_process_running = true; // ROS_INFO("Controller::Process()"); - bool do_sync_write = false; ros::Time start_time; ros::Duration time_duration; @@ -890,93 +889,246 @@ void RobotisController::process() present_state.header.stamp = ros::Time::now(); goal_state.header.stamp = present_state.header.stamp; - // BulkRead Rx - // -> save to Robot->dxls[]->dynamixel_state.present_position - if (gazebo_mode_ == false) + if (controller_mode_ == MotionModuleMode) { - // BulkRead Rx - for (auto& it : port_to_bulk_read_) + if (gazebo_mode_ == false) { - robot_->ports_[it.first]->setPacketTimeout(0.0); - it.second->rxPacket(); - } - - if (robot_->dxls_.size() > 0) - { - for (auto& dxl_it : robot_->dxls_) + // BulkRead Rx + for (auto& it : port_to_bulk_read_) { - Dynamixel *dxl = dxl_it.second; - std::string port_name = dxl_it.second->port_name_; - std::string joint_name = dxl_it.first; + robot_->ports_[it.first]->setPacketTimeout(0.0); + it.second->rxPacket(); + } - if (dxl->bulk_read_items_.size() > 0) + // -> save to robot->dxls_[]->dxl_state_ + if (robot_->dxls_.size() > 0) + { + for (auto& dxl_it : robot_->dxls_) { - uint32_t data = 0; - for (int i = 0; i < dxl->bulk_read_items_.size(); i++) + Dynamixel *dxl = dxl_it.second; + std::string port_name = dxl_it.second->port_name_; + std::string joint_name = dxl_it.first; + + if (dxl->bulk_read_items_.size() > 0) { - ControlTableItem *item = dxl->bulk_read_items_[i]; - if (port_to_bulk_read_[port_name]->isAvailable(dxl->id_, item->address_, item->data_length_) == true) + uint32_t data = 0; + for (int i = 0; i < dxl->bulk_read_items_.size(); i++) { - data = port_to_bulk_read_[port_name]->getData(dxl->id_, item->address_, item->data_length_); + ControlTableItem *item = dxl->bulk_read_items_[i]; + if (port_to_bulk_read_[port_name]->isAvailable(dxl->id_, item->address_, item->data_length_) == true) + { + data = port_to_bulk_read_[port_name]->getData(dxl->id_, item->address_, item->data_length_); - // change dxl_state - if (dxl->present_position_item_ != 0 && item->item_name_ == dxl->present_position_item_->item_name_) - dxl->dxl_state_->present_position_ = dxl->convertValue2Radian(data) - dxl->dxl_state_->position_offset_; // remove offset - else if (dxl->present_velocity_item_ != 0 && item->item_name_ == dxl->present_velocity_item_->item_name_) - dxl->dxl_state_->present_velocity_ = dxl->convertValue2Velocity(data); - else if (dxl->present_current_item_ != 0 && item->item_name_ == dxl->present_current_item_->item_name_) - dxl->dxl_state_->present_torque_ = dxl->convertValue2Torque(data); - else if (dxl->goal_position_item_ != 0 && item->item_name_ == dxl->goal_position_item_->item_name_) - dxl->dxl_state_->goal_position_ = dxl->convertValue2Radian(data) - dxl->dxl_state_->position_offset_; // remove offset - else if (dxl->goal_velocity_item_ != 0 && item->item_name_ == dxl->goal_velocity_item_->item_name_) - dxl->dxl_state_->goal_velocity_ = dxl->convertValue2Velocity(data); - else if (dxl->goal_current_item_ != 0 && item->item_name_ == dxl->goal_current_item_->item_name_) - dxl->dxl_state_->goal_torque_ = dxl->convertValue2Torque(data); - else - dxl->dxl_state_->bulk_read_table_[item->item_name_] = data; + // change dxl_state + if (dxl->present_position_item_ != 0 && item->item_name_ == dxl->present_position_item_->item_name_) + dxl->dxl_state_->present_position_ = dxl->convertValue2Radian(data) - dxl->dxl_state_->position_offset_; // remove offset + else if (dxl->present_velocity_item_ != 0 && item->item_name_ == dxl->present_velocity_item_->item_name_) + dxl->dxl_state_->present_velocity_ = dxl->convertValue2Velocity(data); + else if (dxl->present_current_item_ != 0 && item->item_name_ == dxl->present_current_item_->item_name_) + dxl->dxl_state_->present_torque_ = dxl->convertValue2Torque(data); + else if (dxl->goal_position_item_ != 0 && item->item_name_ == dxl->goal_position_item_->item_name_) + dxl->dxl_state_->goal_position_ = dxl->convertValue2Radian(data) - dxl->dxl_state_->position_offset_; // remove offset + else if (dxl->goal_velocity_item_ != 0 && item->item_name_ == dxl->goal_velocity_item_->item_name_) + dxl->dxl_state_->goal_velocity_ = dxl->convertValue2Velocity(data); + else if (dxl->goal_current_item_ != 0 && item->item_name_ == dxl->goal_current_item_->item_name_) + dxl->dxl_state_->goal_torque_ = dxl->convertValue2Torque(data); + else + dxl->dxl_state_->bulk_read_table_[item->item_name_] = data; + } } - } - // -> update time stamp to Robot->dxls[]->dynamixel_state.update_time_stamp - dxl->dxl_state_->update_time_stamp_ = TimeStamp(present_state.header.stamp.sec, present_state.header.stamp.nsec); + // -> update time stamp to Robot->dxls[]->dynamixel_state.update_time_stamp + dxl->dxl_state_->update_time_stamp_ = TimeStamp(present_state.header.stamp.sec, present_state.header.stamp.nsec); + } } } - } - if (robot_->sensors_.size() > 0) - { - for (auto& sensor_it : robot_->sensors_) + // -> save to robot->sensors_[]->sensor_state_ + if (robot_->sensors_.size() > 0) { - Sensor *sensor = sensor_it.second; - std::string port_name = sensor_it.second->port_name_; - std::string sensor_name = sensor_it.first; - - if (sensor->bulk_read_items_.size() > 0) + for (auto& sensor_it : robot_->sensors_) { - uint32_t data = 0; - for (int i = 0; i < sensor->bulk_read_items_.size(); i++) - { - ControlTableItem *item = sensor->bulk_read_items_[i]; - if (port_to_bulk_read_[port_name]->isAvailable(sensor->id_, item->address_, item->data_length_) == true) - { - data = port_to_bulk_read_[port_name]->getData(sensor->id_, item->address_, item->data_length_); + Sensor *sensor = sensor_it.second; + std::string port_name = sensor_it.second->port_name_; + std::string sensor_name = sensor_it.first; - // change sensor_state - sensor->sensor_state_->bulk_read_table_[item->item_name_] = data; + if (sensor->bulk_read_items_.size() > 0) + { + uint32_t data = 0; + for (int i = 0; i < sensor->bulk_read_items_.size(); i++) + { + ControlTableItem *item = sensor->bulk_read_items_[i]; + if (port_to_bulk_read_[port_name]->isAvailable(sensor->id_, item->address_, item->data_length_) == true) + { + data = port_to_bulk_read_[port_name]->getData(sensor->id_, item->address_, item->data_length_); + + // change sensor_state + sensor->sensor_state_->bulk_read_table_[item->item_name_] = data; + } + } + + // -> update time stamp to Robot->dxls[]->dynamixel_state.update_time_stamp + sensor->sensor_state_->update_time_stamp_ = TimeStamp(present_state.header.stamp.sec, present_state.header.stamp.nsec); + } + } + } + + if (DEBUG_PRINT) + { + time_duration = ros::Time::now() - start_time; + fprintf(stderr, "(%2.6f) BulkRead Rx & update state \n", time_duration.nsec * 0.000001); + } + + // SyncWrite + queue_mutex_.lock(); + + if (direct_sync_write_.size() > 0) + { + for (int i = 0; i < direct_sync_write_.size(); i++) + { + direct_sync_write_[i]->txPacket(); + direct_sync_write_[i]->clearParam(); + } + direct_sync_write_.clear(); + } + + if (port_to_sync_write_position_p_gain_.size() > 0) + { + for (auto& it : port_to_sync_write_position_p_gain_) + { + it.second->txPacket(); + it.second->clearParam(); + } + } + if (port_to_sync_write_position_i_gain_.size() > 0) + { + for (auto& it : port_to_sync_write_position_i_gain_) + { + it.second->txPacket(); + it.second->clearParam(); + } + } + if (port_to_sync_write_position_d_gain_.size() > 0) + { + for (auto& it : port_to_sync_write_position_d_gain_) + { + it.second->txPacket(); + it.second->clearParam(); + } + } + if (port_to_sync_write_velocity_p_gain_.size() > 0) + { + for (auto& it : port_to_sync_write_velocity_p_gain_) + { + it.second->txPacket(); + it.second->clearParam(); + } + } + if (port_to_sync_write_velocity_i_gain_.size() > 0) + { + for (auto& it : port_to_sync_write_velocity_i_gain_) + { + it.second->txPacket(); + it.second->clearParam(); + } + } + if (port_to_sync_write_velocity_d_gain_.size() > 0) + { + for (auto& it : port_to_sync_write_velocity_d_gain_) + { + it.second->txPacket(); + it.second->clearParam(); + } + } + for (auto& it : port_to_sync_write_position_) + { + if (it.second != NULL) + it.second->txPacket(); + } + for (auto& it : port_to_sync_write_velocity_) + { + if (it.second != NULL) + it.second->txPacket(); + } + for (auto& it : port_to_sync_write_current_) + { + if (it.second != NULL) + it.second->txPacket(); + } + + queue_mutex_.unlock(); + + // BulkRead Tx + for (auto& it : port_to_bulk_read_) + it.second->txPacket(); + + if (DEBUG_PRINT) + { + time_duration = ros::Time::now() - start_time; + fprintf(stderr, "(%2.6f) SyncWrite & BulkRead Tx \n", time_duration.nsec * 0.000001); + } + } + else if (gazebo_mode_ == true) + { + for (auto module_it = motion_modules_.begin(); module_it != motion_modules_.end(); module_it++) + { + if ((*module_it)->getModuleEnable() == false) + continue; + + std_msgs::Float64 joint_msg; + + for (auto& dxl_it : robot_->dxls_) + { + std::string joint_name = dxl_it.first; + Dynamixel *dxl = dxl_it.second; + DynamixelState *dxl_state = dxl_it.second->dxl_state_; + + if (dxl->ctrl_module_name_ == (*module_it)->getModuleName()) + { + if ((*module_it)->getControlMode() == PositionControl) + { + joint_msg.data = dxl_state->goal_position_; + gazebo_joint_position_pub_[joint_name].publish(joint_msg); + } + else if ((*module_it)->getControlMode() == VelocityControl) + { + joint_msg.data = dxl_state->goal_velocity_; + gazebo_joint_velocity_pub_[joint_name].publish(joint_msg); + } + else if ((*module_it)->getControlMode() == TorqueControl) + { + joint_msg.data = dxl_state->goal_torque_; + gazebo_joint_effort_pub_[joint_name].publish(joint_msg); } } - - // -> update time stamp to Robot->dxls[]->dynamixel_state.update_time_stamp - sensor->sensor_state_->update_time_stamp_ = TimeStamp(present_state.header.stamp.sec, present_state.header.stamp.nsec); } } } } - - if (DEBUG_PRINT) + else if (controller_mode_ == DirectControlMode) { - time_duration = ros::Time::now() - start_time; - fprintf(stderr, "(%2.6f) BulkRead Rx & update state \n", time_duration.nsec * 0.000001); + if(gazebo_mode_ == false) + { + queue_mutex_.lock(); + + for (auto& it : port_to_sync_write_position_) + { + it.second->txPacket(); + it.second->clearParam(); + } + + if (direct_sync_write_.size() > 0) + { + for (int i = 0; i < direct_sync_write_.size(); i++) + { + direct_sync_write_[i]->txPacket(); + direct_sync_write_[i]->clearParam(); + } + direct_sync_write_.clear(); + } + + queue_mutex_.unlock(); + } } // Call SensorModule Process() @@ -1022,7 +1174,7 @@ void RobotisController::process() if (dxl->ctrl_module_name_ == (*module_it)->getModuleName()) { - do_sync_write = true; + //do_sync_write = true; DynamixelState *result_state = (*module_it)->result_[joint_name]; if (result_state == NULL) @@ -1047,12 +1199,6 @@ void RobotisController::process() sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(pos_data)); sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(pos_data)); -// if (abs(pos_data) > 151800) -// { -// printf("goal_pos : %f | position_offset : %f | pos_data : %d\n", -// dxl_state->goal_position_, dxl_state->position_offset_, pos_data); -// } - if (port_to_sync_write_position_[dxl->port_name_] != NULL) port_to_sync_write_position_[dxl->port_name_]->changeParam(dxl->id_, sync_write_data); @@ -1185,157 +1331,6 @@ void RobotisController::process() time_duration = ros::Time::now() - start_time; fprintf(stderr, "(%2.6f) MotionModule Process() & save result \n", time_duration.nsec * 0.000001); } - - // SyncWrite - if (gazebo_mode_ == false && do_sync_write) - { - if (direct_sync_write_.size() > 0) - { - for (int i = 0; i < direct_sync_write_.size(); i++) - { - direct_sync_write_[i]->txPacket(); - direct_sync_write_[i]->clearParam(); - } - direct_sync_write_.clear(); - } - - if (port_to_sync_write_position_p_gain_.size() > 0) - { - for (auto& it : port_to_sync_write_position_p_gain_) - { - it.second->txPacket(); - it.second->clearParam(); - } - } - if (port_to_sync_write_position_i_gain_.size() > 0) - { - for (auto& it : port_to_sync_write_position_i_gain_) - { - it.second->txPacket(); - it.second->clearParam(); - } - } - if (port_to_sync_write_position_d_gain_.size() > 0) - { - for (auto& it : port_to_sync_write_position_d_gain_) - { - it.second->txPacket(); - it.second->clearParam(); - } - } - if (port_to_sync_write_velocity_p_gain_.size() > 0) - { - for (auto& it : port_to_sync_write_velocity_p_gain_) - { - it.second->txPacket(); - it.second->clearParam(); - } - } - if (port_to_sync_write_velocity_i_gain_.size() > 0) - { - for (auto& it : port_to_sync_write_velocity_i_gain_) - { - it.second->txPacket(); - it.second->clearParam(); - } - } - if (port_to_sync_write_velocity_d_gain_.size() > 0) - { - for (auto& it : port_to_sync_write_velocity_d_gain_) - { - it.second->txPacket(); - it.second->clearParam(); - } - } - for (auto& it : port_to_sync_write_position_) - { - if (it.second != NULL) - it.second->txPacket(); - } - for (auto& it : port_to_sync_write_velocity_) - { - if (it.second != NULL) - it.second->txPacket(); - } - for (auto& it : port_to_sync_write_current_) - { - if (it.second != NULL) - it.second->txPacket(); - } - } - else if (gazebo_mode_ == true) - { - for (auto module_it = motion_modules_.begin(); module_it != motion_modules_.end(); module_it++) - { - if ((*module_it)->getModuleEnable() == false) - continue; - - std_msgs::Float64 joint_msg; - - for (auto& dxl_it : robot_->dxls_) - { - std::string joint_name = dxl_it.first; - Dynamixel *dxl = dxl_it.second; - DynamixelState *dxl_state = dxl_it.second->dxl_state_; - - if (dxl->ctrl_module_name_ == (*module_it)->getModuleName()) - { - if ((*module_it)->getControlMode() == PositionControl) - { - joint_msg.data = dxl_state->goal_position_; - gazebo_joint_position_pub_[joint_name].publish(joint_msg); - } - else if ((*module_it)->getControlMode() == VelocityControl) - { - joint_msg.data = dxl_state->goal_velocity_; - gazebo_joint_velocity_pub_[joint_name].publish(joint_msg); - } - else if ((*module_it)->getControlMode() == TorqueControl) - { - joint_msg.data = dxl_state->goal_torque_; - gazebo_joint_effort_pub_[joint_name].publish(joint_msg); - } - } - } - } - } - } - else if (controller_mode_ == DirectControlMode) - { - queue_mutex_.lock(); - - for (auto& it : port_to_sync_write_position_) - { - it.second->txPacket(); - it.second->clearParam(); - } - - if (direct_sync_write_.size() > 0) - { - for (int i = 0; i < direct_sync_write_.size(); i++) - { - direct_sync_write_[i]->txPacket(); - direct_sync_write_[i]->clearParam(); - } - direct_sync_write_.clear(); - } - - queue_mutex_.unlock(); - } - - // TODO: User Read/Write - - // BulkRead Tx - if (gazebo_mode_ == false) - { - for (auto& it : port_to_bulk_read_) - it.second->txPacket(); - } - - if (DEBUG_PRINT) - { - time_duration = ros::Time::now() - start_time; - fprintf(stderr, "(%2.6f) SyncWrite & BulkRead Tx \n", time_duration.nsec * 0.000001); } // publish present & goal position From d0de3171d0122f190eeae9506b4cf21729496123 Mon Sep 17 00:00:00 2001 From: ROBOTIS-zerom Date: Wed, 21 Sep 2016 15:05:16 +0900 Subject: [PATCH 074/238] - optimized cpu usage by spin loop (by astumpf) --- .../src/robotis_controller/robotis_controller.cpp | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/robotis_controller/src/robotis_controller/robotis_controller.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp index ccbdec0..89a4751 100644 --- a/robotis_controller/src/robotis_controller/robotis_controller.cpp +++ b/robotis_controller/src/robotis_controller/robotis_controller.cpp @@ -660,11 +660,9 @@ void RobotisController::msgQueueThread() ros::ServiceServer joint_module_server = ros_node.advertiseService("/robotis/get_present_joint_ctrl_modules", &RobotisController::getCtrlModuleCallback, this); - while (ros_node.ok()) - { - callback_queue.callAvailable(); - usleep(1000); - } + ros::WallDuration duration(CONTROL_CYCLE_MSEC / 1000.0); + while(ros_node.ok()) + callback_queue.callAvailable(duration); } void *RobotisController::timerThread(void *param) From 260c03b3aeedc1879664b5dddafd4adcbfb57fc6 Mon Sep 17 00:00:00 2001 From: s-changhyun Date: Wed, 28 Sep 2016 18:59:49 +0900 Subject: [PATCH 075/238] mode change debugging --- .../robotis_controller/robotis_controller.cpp | 32 +++++++- .../dynamixel/H54-100-B210-R-NR.device | 76 +++++++++++++++++++ 2 files changed, 107 insertions(+), 1 deletion(-) create mode 100644 robotis_device/devices/dynamixel/H54-100-B210-R-NR.device diff --git a/robotis_controller/src/robotis_controller/robotis_controller.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp index 89a4751..fc45481 100644 --- a/robotis_controller/src/robotis_controller/robotis_controller.cpp +++ b/robotis_controller/src/robotis_controller/robotis_controller.cpp @@ -1519,6 +1519,11 @@ void RobotisController::setCtrlModuleCallback(const std_msgs::String::ConstPtr & { std::string _module_name_to_set = msg->data; + + + + + set_module_thread_ = boost::thread(boost::bind(&RobotisController::setCtrlModuleThread, this, _module_name_to_set)); } @@ -1532,7 +1537,7 @@ void RobotisController::setJointCtrlModuleCallback(const robotis_controller_msgs return; set_module_thread_ = boost::thread(boost::bind(&RobotisController::setJointCtrlModuleThread, this, msg)); - } +} bool RobotisController::getCtrlModuleCallback(robotis_controller_msgs::GetJointModule::Request &req, robotis_controller_msgs::GetJointModule::Response &res) @@ -1751,6 +1756,18 @@ void RobotisController::setCtrlModuleThread(std::string ctrl_module) // stop module std::list stop_modules; + ROS_INFO("----- Before -----"); + for (auto& d_it : robot_->dxls_) + { + std::string joint_name = d_it.first; + + Dynamixel *dxl = d_it.second; + double goal_position = dxl->dxl_state_->goal_position_; + + fprintf(stderr, "%s : %f \n", joint_name.c_str(), goal_position); + } + ROS_INFO("----- ----- -----"); + if (ctrl_module == "" || ctrl_module == "none") { // enqueue all modules in order to stop @@ -1956,6 +1973,19 @@ void RobotisController::setCtrlModuleThread(std::string ctrl_module) if (current_module_msg.joint_name.size() == current_module_msg.module_name.size()) current_module_pub_.publish(current_module_msg); + + + ROS_INFO("----- After -----"); + for (auto& d_it : robot_->dxls_) + { + std::string joint_name = d_it.first; + + Dynamixel *dxl = d_it.second; + double goal_position = dxl->dxl_state_->goal_position_; + + fprintf(stderr, "%s : %f \n", joint_name.c_str(), goal_position); + } + ROS_INFO("----- ----- -----"); } void RobotisController::gazeboJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg) diff --git a/robotis_device/devices/dynamixel/H54-100-B210-R-NR.device b/robotis_device/devices/dynamixel/H54-100-B210-R-NR.device new file mode 100644 index 0000000..4cb137a --- /dev/null +++ b/robotis_device/devices/dynamixel/H54-100-B210-R-NR.device @@ -0,0 +1,76 @@ +[device info] +model_name = H54-100-B210-R-NR +device_type = dynamixel + +[type info] +torque_to_current_value_ratio = 9.09201 +velocity_to_value_ratio = 2046.2777 +value_of_0_radian_position = 0 +value_of_min_radian_position = -250950 +value_of_max_radian_position = 250950 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_torque +position_d_gain_item_name = +position_i_gain_item_name = +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 13 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 30 | torque_limit | 2 | RW | EEPROM | 0 | 32767 | N + 32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 562 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 586 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N + 588 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N + 594 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N + 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y + 606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N + 610 | is_moving | 1 | R | RAM | 0 | 1 | N + 611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 621 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 623 | present_voltage | 2 | R | RAM | 0 | 500 | N + 625 | present_temperature | 1 | R | RAM | 0 | 200 | N + 626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 891 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N From 303e24c55e707db2a7a747512cceb061acd0d3b2 Mon Sep 17 00:00:00 2001 From: "zerom@robotis.com" Date: Mon, 10 Oct 2016 10:16:03 +0900 Subject: [PATCH 076/238] - added WriteControlTable msg callback --- .../robotis_controller/robotis_controller.cpp | 91 ++++++++++++------- 1 file changed, 60 insertions(+), 31 deletions(-) diff --git a/robotis_controller/src/robotis_controller/robotis_controller.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp index fc45481..1dad69e 100644 --- a/robotis_controller/src/robotis_controller/robotis_controller.cpp +++ b/robotis_controller/src/robotis_controller/robotis_controller.cpp @@ -621,6 +621,8 @@ void RobotisController::msgQueueThread() ros_node.setCallbackQueue(&callback_queue); /* subscriber */ + ros::Subscriber write_control_table_sub = ros_node.subscribe("/robotis/write_control_table", 5, + &RobotisController::writeControlTableCallback, this); ros::Subscriber sync_write_item_sub = ros_node.subscribe("/robotis/sync_write_item", 10, &RobotisController::syncWriteItemCallback, this); ros::Subscriber joint_ctrl_modules_sub = ros_node.subscribe("/robotis/set_joint_ctrl_modules", 10, @@ -1405,6 +1407,52 @@ void RobotisController::removeSensorModule(SensorModule *module) sensor_modules_.remove(module); } +void RobotisController::writeControlTableCallback(const robotis_controller_msgs::WriteControlTable::ConstPtr &msg) +{ + Device *device = NULL; + + auto dev_it1 = robot_->dxls_.find(msg->joint_name); + if(dev_it1 != robot_->dxls_.end()) + { + device = dev_it1->second; + } + else + { + auto dev_it2 = robot_->sensors_.find(msg->joint_name); + if(dev_it2 != robot_->sensors_.end()) + { + device = dev_it2->second; + } + else + { + ROS_WARN("[WriteControlTable] Unknown device : %s", msg->joint_name.c_str()); + return; + } + } + + ControlTableItem *item = NULL; + auto item_it = device->ctrl_table_.find(msg->start_item_name); + if(item_it != device->ctrl_table_.end()) + { + item = item_it->second; + } + else + { + ROS_WARN("WriteControlTable] Unknown item : %s", msg->start_item_name.c_str()); + return; + } + + dynamixel::PortHandler *port = robot_->ports_[device->port_name_]; + dynamixel::PacketHandler *packet_handler = dynamixel::PacketHandler::getPacketHandler(device->protocol_version_); + + if (item->access_type_ == Read) + return; + + direct_sync_write_.push_back(new dynamixel::GroupSyncWrite(port, packet_handler, item->address_, msg->data_length)); + + direct_sync_write_[direct_sync_write_.size() - 1]->addParam(device->id_, (uint8_t *)(msg->data.data())); +} + void RobotisController::syncWriteItemCallback(const robotis_controller_msgs::SyncWriteItem::ConstPtr &msg) { for (int i = 0; i < msg->joint_name.size(); i++) @@ -1430,7 +1478,18 @@ void RobotisController::syncWriteItemCallback(const robotis_controller_msgs::Syn } } - ControlTableItem *item = device->ctrl_table_[msg->item_name]; +// ControlTableItem *item = device->ctrl_table_[msg->item_name]; + ControlTableItem *item = NULL; + auto item_it = device->ctrl_table_.find(msg->item_name); + if(item_it != device->ctrl_table_.end()) + { + item = item_it->second; + } + else + { + ROS_WARN("SyncWriteItem] Unknown item : %s", msg->item_name.c_str()); + continue; + } dynamixel::PortHandler *port = robot_->ports_[device->port_name_]; dynamixel::PacketHandler *packet_handler = dynamixel::PacketHandler::getPacketHandler(device->protocol_version_); @@ -1519,11 +1578,6 @@ void RobotisController::setCtrlModuleCallback(const std_msgs::String::ConstPtr & { std::string _module_name_to_set = msg->data; - - - - - set_module_thread_ = boost::thread(boost::bind(&RobotisController::setCtrlModuleThread, this, _module_name_to_set)); } @@ -1756,18 +1810,6 @@ void RobotisController::setCtrlModuleThread(std::string ctrl_module) // stop module std::list stop_modules; - ROS_INFO("----- Before -----"); - for (auto& d_it : robot_->dxls_) - { - std::string joint_name = d_it.first; - - Dynamixel *dxl = d_it.second; - double goal_position = dxl->dxl_state_->goal_position_; - - fprintf(stderr, "%s : %f \n", joint_name.c_str(), goal_position); - } - ROS_INFO("----- ----- -----"); - if (ctrl_module == "" || ctrl_module == "none") { // enqueue all modules in order to stop @@ -1973,19 +2015,6 @@ void RobotisController::setCtrlModuleThread(std::string ctrl_module) if (current_module_msg.joint_name.size() == current_module_msg.module_name.size()) current_module_pub_.publish(current_module_msg); - - - ROS_INFO("----- After -----"); - for (auto& d_it : robot_->dxls_) - { - std::string joint_name = d_it.first; - - Dynamixel *dxl = d_it.second; - double goal_position = dxl->dxl_state_->goal_position_; - - fprintf(stderr, "%s : %f \n", joint_name.c_str(), goal_position); - } - ROS_INFO("----- ----- -----"); } void RobotisController::gazeboJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg) From 49ed1c9cbc17041a8355058a76489fd31e485f66 Mon Sep 17 00:00:00 2001 From: "zerom@robotis.com" Date: Wed, 12 Oct 2016 10:25:55 +0900 Subject: [PATCH 077/238] - added writeControlTableCallback --- .../robotis_controller/robotis_controller.h | 372 +++++++++--------- 1 file changed, 187 insertions(+), 185 deletions(-) diff --git a/robotis_controller/include/robotis_controller/robotis_controller.h b/robotis_controller/include/robotis_controller/robotis_controller.h index 4014b29..8c1ae27 100644 --- a/robotis_controller/include/robotis_controller/robotis_controller.h +++ b/robotis_controller/include/robotis_controller/robotis_controller.h @@ -1,185 +1,187 @@ -/******************************************************************************* - * Copyright (c) 2016, ROBOTIS CO., LTD. - * 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 ROBOTIS 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. - *******************************************************************************/ - -/* - * robotis_controller.h - * - * Created on: 2016. 1. 15. - * Author: zerom - */ - -#ifndef ROBOTIS_CONTROLLER_ROBOTIS_CONTROLLER_H_ -#define ROBOTIS_CONTROLLER_ROBOTIS_CONTROLLER_H_ - - -#include -#include -#include -#include -#include -#include - -#include "robotis_controller_msgs/SyncWriteItem.h" -#include "robotis_controller_msgs/JointCtrlModule.h" -#include "robotis_controller_msgs/GetJointModule.h" - -#include "robotis_device/robot.h" -#include "robotis_framework_common/motion_module.h" -#include "robotis_framework_common/sensor_module.h" -#include "dynamixel_sdk/group_bulk_read.h" -#include "dynamixel_sdk/group_sync_write.h" - -namespace robotis_framework -{ - -enum ControllerMode -{ - MotionModuleMode, - DirectControlMode -}; - -class RobotisController : public Singleton -{ -private: - boost::thread queue_thread_; - boost::thread gazebo_thread_; - boost::thread set_module_thread_; - boost::mutex queue_mutex_; - - bool init_pose_loaded_; - bool is_timer_running_; - bool stop_timer_; - pthread_t timer_thread_; - ControllerMode controller_mode_; - - std::list motion_modules_; - std::list sensor_modules_; - std::vector direct_sync_write_; - - std::map sensor_result_; - - void gazeboTimerThread(); - void msgQueueThread(); - void setCtrlModuleThread(std::string ctrl_module); - void setJointCtrlModuleThread(const robotis_controller_msgs::JointCtrlModule::ConstPtr &msg); - - bool isTimerStopped(); - void initializeSyncWrite(); - -public: - static const int CONTROL_CYCLE_MSEC = 8; // 8 msec - - bool DEBUG_PRINT; - Robot *robot_; - - bool gazebo_mode_; - std::string gazebo_robot_name_; - - /* bulk read */ - std::map port_to_bulk_read_; - - /* sync write */ - std::map port_to_sync_write_position_; - std::map port_to_sync_write_velocity_; - std::map port_to_sync_write_current_; - std::map port_to_sync_write_position_p_gain_; - std::map port_to_sync_write_position_i_gain_; - std::map port_to_sync_write_position_d_gain_; - std::map port_to_sync_write_velocity_p_gain_; - std::map port_to_sync_write_velocity_i_gain_; - std::map port_to_sync_write_velocity_d_gain_; - - /* publisher */ - ros::Publisher goal_joint_state_pub_; - ros::Publisher present_joint_state_pub_; - ros::Publisher current_module_pub_; - - std::map gazebo_joint_position_pub_; - std::map gazebo_joint_velocity_pub_; - std::map gazebo_joint_effort_pub_; - - static void *timerThread(void *param); - - RobotisController(); - - bool initialize(const std::string robot_file_path, const std::string init_file_path); - void initializeDevice(const std::string init_file_path); - void process(); - - void addMotionModule(MotionModule *module); - void removeMotionModule(MotionModule *module); - void addSensorModule(SensorModule *module); - void removeSensorModule(SensorModule *module); - - void startTimer(); - void stopTimer(); - bool isTimerRunning(); - - void setCtrlModule(std::string module_name); - void loadOffset(const std::string path); - - /* ROS Topic Callback Functions */ - void syncWriteItemCallback(const robotis_controller_msgs::SyncWriteItem::ConstPtr &msg); - void setControllerModeCallback(const std_msgs::String::ConstPtr &msg); - void setJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg); - void setJointCtrlModuleCallback(const robotis_controller_msgs::JointCtrlModule::ConstPtr &msg); - void setCtrlModuleCallback(const std_msgs::String::ConstPtr &msg); - bool getCtrlModuleCallback(robotis_controller_msgs::GetJointModule::Request &req, robotis_controller_msgs::GetJointModule::Response &res); - - void gazeboJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg); - - int ping (const std::string joint_name, uint8_t *error = 0); - int ping (const std::string joint_name, uint16_t* model_number, uint8_t *error = 0); - - int action (const std::string joint_name); - int reboot (const std::string joint_name, uint8_t *error = 0); - int factoryReset(const std::string joint_name, uint8_t option = 0, uint8_t *error = 0); - - int read (const std::string joint_name, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0); - int readCtrlItem(const std::string joint_name, const std::string item_name, uint32_t *data, uint8_t *error = 0); - - int read1Byte (const std::string joint_name, uint16_t address, uint8_t *data, uint8_t *error = 0); - int read2Byte (const std::string joint_name, uint16_t address, uint16_t *data, uint8_t *error = 0); - int read4Byte (const std::string joint_name, uint16_t address, uint32_t *data, uint8_t *error = 0); - - int write (const std::string joint_name, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0); - int writeCtrlItem(const std::string joint_name, const std::string item_name, uint32_t data, uint8_t *error = 0); - - int write1Byte (const std::string joint_name, uint16_t address, uint8_t data, uint8_t *error = 0); - int write2Byte (const std::string joint_name, uint16_t address, uint16_t data, uint8_t *error = 0); - int write4Byte (const std::string joint_name, uint16_t address, uint32_t data, uint8_t *error = 0); - - int regWrite (const std::string joint_name, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0); -}; - -} - - -#endif /* ROBOTIS_CONTROLLER_ROBOTIS_CONTROLLER_H_ */ +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * 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 ROBOTIS 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. + *******************************************************************************/ + +/* + * robotis_controller.h + * + * Created on: 2016. 1. 15. + * Author: zerom + */ + +#ifndef ROBOTIS_CONTROLLER_ROBOTIS_CONTROLLER_H_ +#define ROBOTIS_CONTROLLER_ROBOTIS_CONTROLLER_H_ + + +#include +#include +#include +#include +#include +#include + +#include "robotis_controller_msgs/WriteControlTable.h" +#include "robotis_controller_msgs/SyncWriteItem.h" +#include "robotis_controller_msgs/JointCtrlModule.h" +#include "robotis_controller_msgs/GetJointModule.h" + +#include "robotis_device/robot.h" +#include "robotis_framework_common/motion_module.h" +#include "robotis_framework_common/sensor_module.h" +#include "dynamixel_sdk/group_bulk_read.h" +#include "dynamixel_sdk/group_sync_write.h" + +namespace robotis_framework +{ + +enum ControllerMode +{ + MotionModuleMode, + DirectControlMode +}; + +class RobotisController : public Singleton +{ +private: + boost::thread queue_thread_; + boost::thread gazebo_thread_; + boost::thread set_module_thread_; + boost::mutex queue_mutex_; + + bool init_pose_loaded_; + bool is_timer_running_; + bool stop_timer_; + pthread_t timer_thread_; + ControllerMode controller_mode_; + + std::list motion_modules_; + std::list sensor_modules_; + std::vector direct_sync_write_; + + std::map sensor_result_; + + void gazeboTimerThread(); + void msgQueueThread(); + void setCtrlModuleThread(std::string ctrl_module); + void setJointCtrlModuleThread(const robotis_controller_msgs::JointCtrlModule::ConstPtr &msg); + + bool isTimerStopped(); + void initializeSyncWrite(); + +public: + static const int CONTROL_CYCLE_MSEC = 8; // 8 msec + + bool DEBUG_PRINT; + Robot *robot_; + + bool gazebo_mode_; + std::string gazebo_robot_name_; + + /* bulk read */ + std::map port_to_bulk_read_; + + /* sync write */ + std::map port_to_sync_write_position_; + std::map port_to_sync_write_velocity_; + std::map port_to_sync_write_current_; + std::map port_to_sync_write_position_p_gain_; + std::map port_to_sync_write_position_i_gain_; + std::map port_to_sync_write_position_d_gain_; + std::map port_to_sync_write_velocity_p_gain_; + std::map port_to_sync_write_velocity_i_gain_; + std::map port_to_sync_write_velocity_d_gain_; + + /* publisher */ + ros::Publisher goal_joint_state_pub_; + ros::Publisher present_joint_state_pub_; + ros::Publisher current_module_pub_; + + std::map gazebo_joint_position_pub_; + std::map gazebo_joint_velocity_pub_; + std::map gazebo_joint_effort_pub_; + + static void *timerThread(void *param); + + RobotisController(); + + bool initialize(const std::string robot_file_path, const std::string init_file_path); + void initializeDevice(const std::string init_file_path); + void process(); + + void addMotionModule(MotionModule *module); + void removeMotionModule(MotionModule *module); + void addSensorModule(SensorModule *module); + void removeSensorModule(SensorModule *module); + + void startTimer(); + void stopTimer(); + bool isTimerRunning(); + + void setCtrlModule(std::string module_name); + void loadOffset(const std::string path); + + /* ROS Topic Callback Functions */ + void writeControlTableCallback(const robotis_controller_msgs::WriteControlTable::ConstPtr &msg); + void syncWriteItemCallback(const robotis_controller_msgs::SyncWriteItem::ConstPtr &msg); + void setControllerModeCallback(const std_msgs::String::ConstPtr &msg); + void setJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg); + void setJointCtrlModuleCallback(const robotis_controller_msgs::JointCtrlModule::ConstPtr &msg); + void setCtrlModuleCallback(const std_msgs::String::ConstPtr &msg); + bool getCtrlModuleCallback(robotis_controller_msgs::GetJointModule::Request &req, robotis_controller_msgs::GetJointModule::Response &res); + + void gazeboJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg); + + int ping (const std::string joint_name, uint8_t *error = 0); + int ping (const std::string joint_name, uint16_t* model_number, uint8_t *error = 0); + + int action (const std::string joint_name); + int reboot (const std::string joint_name, uint8_t *error = 0); + int factoryReset(const std::string joint_name, uint8_t option = 0, uint8_t *error = 0); + + int read (const std::string joint_name, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0); + int readCtrlItem(const std::string joint_name, const std::string item_name, uint32_t *data, uint8_t *error = 0); + + int read1Byte (const std::string joint_name, uint16_t address, uint8_t *data, uint8_t *error = 0); + int read2Byte (const std::string joint_name, uint16_t address, uint16_t *data, uint8_t *error = 0); + int read4Byte (const std::string joint_name, uint16_t address, uint32_t *data, uint8_t *error = 0); + + int write (const std::string joint_name, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0); + int writeCtrlItem(const std::string joint_name, const std::string item_name, uint32_t data, uint8_t *error = 0); + + int write1Byte (const std::string joint_name, uint16_t address, uint8_t data, uint8_t *error = 0); + int write2Byte (const std::string joint_name, uint16_t address, uint16_t data, uint8_t *error = 0); + int write4Byte (const std::string joint_name, uint16_t address, uint32_t data, uint8_t *error = 0); + + int regWrite (const std::string joint_name, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0); +}; + +} + + +#endif /* ROBOTIS_CONTROLLER_ROBOTIS_CONTROLLER_H_ */ From c8abeff06387ee11c79b2546fd68e6bc88c69c8c Mon Sep 17 00:00:00 2001 From: Jay-Song Date: Fri, 28 Oct 2016 10:29:30 +0900 Subject: [PATCH 078/238] update --- .gitignore | 1 + LICENSE | 52 +- README.md | 8 +- robotis_controller/CHANGELOG.rst | 46 +- robotis_controller/CMakeLists.txt | 132 +-- .../robotis_controller/robotis_controller.h | 0 robotis_controller/package.xml | 56 +- .../robotis_controller/robotis_controller.cpp | 17 +- robotis_device/CHANGELOG.rst | 48 +- robotis_device/CMakeLists.txt | 136 +-- .../devices/dynamixel/GRIPPER.device | 146 +-- .../devices/dynamixel/GRIPPER_TORQUE.device | 74 ++ .../devices/dynamixel/H42-20-S300-R.device | 152 +-- .../dynamixel/H54-100-B210-R-NR.device | 152 +-- .../devices/dynamixel/H54-100-S500-R.device | 152 +-- .../devices/dynamixel/H54-200-B500-R.device | 152 +-- .../devices/dynamixel/H54-200-S500-R.device | 152 +-- .../devices/dynamixel/L42-10-S300-R.device | 146 +-- .../devices/dynamixel/L54-30-S400-R.device | 148 +-- .../devices/dynamixel/L54-30-S500-R.device | 148 +-- .../devices/dynamixel/L54-50-S290-R.device | 148 +-- .../devices/dynamixel/L54-50-S500-R.device | 148 +-- .../devices/dynamixel/M42-10-S260-R.device | 148 +-- .../devices/dynamixel/M54-40-S250-R.device | 148 +-- .../devices/dynamixel/M54-60-S250-R.device | 148 +-- .../devices/dynamixel/MX-106.device | 132 +-- robotis_device/devices/dynamixel/MX-28.device | 124 +-- robotis_device/devices/dynamixel/MX-64.device | 132 +-- .../devices/dynamixel/XM-430-W210.device | 162 +-- .../devices/dynamixel/XM-430-W350.device | 162 +-- .../devices/dynamixel/XM-430.device | 160 +-- robotis_device/devices/sensor/CM-740.device | 48 +- .../robotis_device/control_table_item.h | 168 +-- .../include/robotis_device/device.h | 136 +-- .../include/robotis_device/dynamixel.h | 194 ++-- .../include/robotis_device/dynamixel_state.h | 192 ++-- robotis_device/include/robotis_device/robot.h | 156 +-- .../include/robotis_device/sensor.h | 126 +-- .../include/robotis_device/sensor_state.h | 128 +-- .../include/robotis_device/time_stamp.h | 118 +-- robotis_device/package.xml | 46 +- .../src/robotis_device/dynamixel.cpp | 0 robotis_device/src/robotis_device/robot.cpp | 974 +++++++++--------- robotis_device/src/robotis_device/sensor.cpp | 106 +- robotis_framework/CHANGELOG.rst | 24 +- robotis_framework/CMakeLists.txt | 8 +- robotis_framework/package.xml | 34 +- robotis_framework_common/CHANGELOG.rst | 30 +- robotis_framework_common/CMakeLists.txt | 110 +- .../robotis_framework_common/motion_module.h | 202 ++-- .../robotis_framework_common/sensor_module.h | 142 +-- .../robotis_framework_common/singleton.h | 158 +-- robotis_framework_common/package.xml | 38 +- 53 files changed, 3379 insertions(+), 3289 deletions(-) create mode 100755 .gitignore mode change 100644 => 100755 LICENSE mode change 100644 => 100755 README.md mode change 100644 => 100755 robotis_controller/CHANGELOG.rst mode change 100644 => 100755 robotis_controller/CMakeLists.txt mode change 100644 => 100755 robotis_controller/include/robotis_controller/robotis_controller.h mode change 100644 => 100755 robotis_controller/package.xml mode change 100644 => 100755 robotis_controller/src/robotis_controller/robotis_controller.cpp mode change 100644 => 100755 robotis_device/CHANGELOG.rst mode change 100644 => 100755 robotis_device/CMakeLists.txt mode change 100644 => 100755 robotis_device/devices/dynamixel/GRIPPER.device create mode 100755 robotis_device/devices/dynamixel/GRIPPER_TORQUE.device mode change 100644 => 100755 robotis_device/devices/dynamixel/H42-20-S300-R.device mode change 100644 => 100755 robotis_device/devices/dynamixel/H54-100-B210-R-NR.device mode change 100644 => 100755 robotis_device/devices/dynamixel/H54-100-S500-R.device mode change 100644 => 100755 robotis_device/devices/dynamixel/H54-200-B500-R.device mode change 100644 => 100755 robotis_device/devices/dynamixel/H54-200-S500-R.device mode change 100644 => 100755 robotis_device/devices/dynamixel/L42-10-S300-R.device mode change 100644 => 100755 robotis_device/devices/dynamixel/L54-30-S400-R.device mode change 100644 => 100755 robotis_device/devices/dynamixel/L54-30-S500-R.device mode change 100644 => 100755 robotis_device/devices/dynamixel/L54-50-S290-R.device mode change 100644 => 100755 robotis_device/devices/dynamixel/L54-50-S500-R.device mode change 100644 => 100755 robotis_device/devices/dynamixel/M42-10-S260-R.device mode change 100644 => 100755 robotis_device/devices/dynamixel/M54-40-S250-R.device mode change 100644 => 100755 robotis_device/devices/dynamixel/M54-60-S250-R.device mode change 100644 => 100755 robotis_device/devices/dynamixel/MX-106.device mode change 100644 => 100755 robotis_device/devices/dynamixel/MX-28.device mode change 100644 => 100755 robotis_device/devices/dynamixel/MX-64.device mode change 100644 => 100755 robotis_device/devices/dynamixel/XM-430-W210.device mode change 100644 => 100755 robotis_device/devices/dynamixel/XM-430-W350.device mode change 100644 => 100755 robotis_device/devices/dynamixel/XM-430.device mode change 100644 => 100755 robotis_device/devices/sensor/CM-740.device mode change 100644 => 100755 robotis_device/include/robotis_device/control_table_item.h mode change 100644 => 100755 robotis_device/include/robotis_device/device.h mode change 100644 => 100755 robotis_device/include/robotis_device/dynamixel.h mode change 100644 => 100755 robotis_device/include/robotis_device/dynamixel_state.h mode change 100644 => 100755 robotis_device/include/robotis_device/robot.h mode change 100644 => 100755 robotis_device/include/robotis_device/sensor.h mode change 100644 => 100755 robotis_device/include/robotis_device/sensor_state.h mode change 100644 => 100755 robotis_device/include/robotis_device/time_stamp.h mode change 100644 => 100755 robotis_device/package.xml mode change 100644 => 100755 robotis_device/src/robotis_device/dynamixel.cpp mode change 100644 => 100755 robotis_device/src/robotis_device/robot.cpp mode change 100644 => 100755 robotis_device/src/robotis_device/sensor.cpp mode change 100644 => 100755 robotis_framework/CHANGELOG.rst mode change 100644 => 100755 robotis_framework/CMakeLists.txt mode change 100644 => 100755 robotis_framework/package.xml mode change 100644 => 100755 robotis_framework_common/CHANGELOG.rst mode change 100644 => 100755 robotis_framework_common/CMakeLists.txt mode change 100644 => 100755 robotis_framework_common/include/robotis_framework_common/motion_module.h mode change 100644 => 100755 robotis_framework_common/include/robotis_framework_common/sensor_module.h mode change 100644 => 100755 robotis_framework_common/include/robotis_framework_common/singleton.h mode change 100644 => 100755 robotis_framework_common/package.xml diff --git a/.gitignore b/.gitignore new file mode 100755 index 0000000..6702710 --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +/robotis_device/devices/dynamixel/H54-100-B210-R-NR.device diff --git a/LICENSE b/LICENSE old mode 100644 new mode 100755 index 1d93559..5298325 --- a/LICENSE +++ b/LICENSE @@ -1,26 +1,26 @@ -Software License Agreement (BSD License) - -Copyright (c) 2014, ROBOTIS Inc. -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 ROBOTIS 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 ROBOTIS "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 ROBOTIS 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. +Software License Agreement (BSD License) + +Copyright (c) 2014, ROBOTIS Inc. +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 ROBOTIS 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 ROBOTIS "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 ROBOTIS 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. diff --git a/README.md b/README.md old mode 100644 new mode 100755 index a1ba62c..02d6b48 --- a/README.md +++ b/README.md @@ -1,4 +1,4 @@ -# ROBOTIS-Framework -ROBOTIS Framework GIT REP MAIN - -Documents : [ROBOTIS Framework WIKI](https://github.com/ROBOTIS-GIT/ROBOTIS-Documents/wiki/ROBOTIS-ROS-Framework-Documents) +# ROBOTIS-Framework +ROBOTIS Framework GIT REP MAIN + +Documents : [ROBOTIS Framework WIKI](https://github.com/ROBOTIS-GIT/ROBOTIS-Documents/wiki/ROBOTIS-ROS-Framework-Documents) diff --git a/robotis_controller/CHANGELOG.rst b/robotis_controller/CHANGELOG.rst old mode 100644 new mode 100755 index 2a476ea..c310553 --- a/robotis_controller/CHANGELOG.rst +++ b/robotis_controller/CHANGELOG.rst @@ -1,23 +1,23 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package robotis_controller -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.1.1 (2016-08-18) ------------ -* updated the package information - -0.1.0 (2016-08-12) ------------ -* first public release for Kinetic -* modified the package information for release -* develop branch -> master branch -* function name changed : DeviceInit() -> InitDevice() -* Fixed high CPU consumption due to busy waits -* add SensorState - add Singleton template -* XM-430 / CM-740 device file added. - Sensor device added. -* added code to support the gazebo simulator -* added first bulk read failure protection code -* renewal -* Contributors: Alexander Stumpf, Jay Song, Zerom, Pyo +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package robotis_controller +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.1.1 (2016-08-18) +----------- +* updated the package information + +0.1.0 (2016-08-12) +----------- +* first public release for Kinetic +* modified the package information for release +* develop branch -> master branch +* function name changed : DeviceInit() -> InitDevice() +* Fixed high CPU consumption due to busy waits +* add SensorState + add Singleton template +* XM-430 / CM-740 device file added. + Sensor device added. +* added code to support the gazebo simulator +* added first bulk read failure protection code +* renewal +* Contributors: Alexander Stumpf, Jay Song, Zerom, Pyo diff --git a/robotis_controller/CMakeLists.txt b/robotis_controller/CMakeLists.txt old mode 100644 new mode 100755 index 4830cf7..6e1bbb3 --- a/robotis_controller/CMakeLists.txt +++ b/robotis_controller/CMakeLists.txt @@ -1,66 +1,66 @@ -################################################################################ -# CMake -################################################################################ -cmake_minimum_required(VERSION 2.8.3) -project(robotis_controller) - -set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}") - -################################################################################ -# Packages -################################################################################ -find_package(catkin REQUIRED COMPONENTS - roscpp - roslib - sensor_msgs - std_msgs - robotis_controller_msgs - robotis_framework_common - cmake_modules -) - -################################################################################ -# Declare ROS messages, services and actions -################################################################################ - -################################################################################ -# Declare ROS dynamic reconfigure parameters -################################################################################ - -################################################################################ -# Catkin specific configuration -################################################################################ -catkin_package( - INCLUDE_DIRS include - LIBRARIES robotis_controller - CATKIN_DEPENDS roscpp roslib sensor_msgs std_msgs robotis_framework_common -) - -################################################################################ -# Build -################################################################################ -include_directories( - include - ${catkin_INCLUDE_DIRS} -) - -add_library(robotis_controller src/robotis_controller/robotis_controller.cpp) -add_dependencies(robotis_controller ${catkin_EXPORTED_TARGETS}) -target_link_libraries(robotis_controller yaml-cpp ${catkin_LIBRARIES}) - -################################################################################ -# Install -################################################################################ -install(TARGETS robotis_controller - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -) - -################################################################################ -# Test -################################################################################ +################################################################################ +# CMake +################################################################################ +cmake_minimum_required(VERSION 2.8.3) +project(robotis_controller) + +set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}") + +################################################################################ +# Packages +################################################################################ +find_package(catkin REQUIRED COMPONENTS + roscpp + roslib + sensor_msgs + std_msgs + robotis_controller_msgs + robotis_framework_common + cmake_modules +) + +################################################################################ +# Declare ROS messages, services and actions +################################################################################ + +################################################################################ +# Declare ROS dynamic reconfigure parameters +################################################################################ + +################################################################################ +# Catkin specific configuration +################################################################################ +catkin_package( + INCLUDE_DIRS include + LIBRARIES robotis_controller + CATKIN_DEPENDS roscpp roslib sensor_msgs std_msgs robotis_framework_common +) + +################################################################################ +# Build +################################################################################ +include_directories( + include + ${catkin_INCLUDE_DIRS} +) + +add_library(robotis_controller src/robotis_controller/robotis_controller.cpp) +add_dependencies(robotis_controller ${catkin_EXPORTED_TARGETS}) +target_link_libraries(robotis_controller yaml-cpp ${catkin_LIBRARIES}) + +################################################################################ +# Install +################################################################################ +install(TARGETS robotis_controller + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) + +################################################################################ +# Test +################################################################################ diff --git a/robotis_controller/include/robotis_controller/robotis_controller.h b/robotis_controller/include/robotis_controller/robotis_controller.h old mode 100644 new mode 100755 diff --git a/robotis_controller/package.xml b/robotis_controller/package.xml old mode 100644 new mode 100755 index 9085521..fd96c43 --- a/robotis_controller/package.xml +++ b/robotis_controller/package.xml @@ -1,28 +1,28 @@ - - - robotis_controller - 0.1.1 - - The main package that controls THORMANG3. - - BSD - Zerom - Pyo - https://github.com/ROBOTIS-GIT/ROBOTIS-Framework/issues - https://github.com/ROBOTIS-GIT/ROBOTIS-Framework - http://wiki.ros.org/robotis_controller - catkin - roscpp - roslib - std_msgs - sensor_msgs - robotis_controller_msgs - robotis_framework_common - roscpp - roslib - std_msgs - sensor_msgs - robotis_controller_msgs - robotis_framework_common - - + + + robotis_controller + 0.1.1 + + The main package that controls THORMANG3. + + BSD + Zerom + Pyo + https://github.com/ROBOTIS-GIT/ROBOTIS-Framework/issues + https://github.com/ROBOTIS-GIT/ROBOTIS-Framework + http://wiki.ros.org/robotis_controller + catkin + roscpp + roslib + std_msgs + sensor_msgs + robotis_controller_msgs + robotis_framework_common + roscpp + roslib + std_msgs + sensor_msgs + robotis_controller_msgs + robotis_framework_common + + diff --git a/robotis_controller/src/robotis_controller/robotis_controller.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp old mode 100644 new mode 100755 index 1dad69e..5a0c1ec --- a/robotis_controller/src/robotis_controller/robotis_controller.cpp +++ b/robotis_controller/src/robotis_controller/robotis_controller.cpp @@ -1409,6 +1409,7 @@ void RobotisController::removeSensorModule(SensorModule *module) void RobotisController::writeControlTableCallback(const robotis_controller_msgs::WriteControlTable::ConstPtr &msg) { + fprintf(stderr, "[WriteControlTable] led control msg received\n"); Device *device = NULL; auto dev_it1 = robot_->dxls_.find(msg->joint_name); @@ -1429,7 +1430,7 @@ void RobotisController::writeControlTableCallback(const robotis_controller_msgs: return; } } - + fprintf(stderr, " #1 "); ControlTableItem *item = NULL; auto item_it = device->ctrl_table_.find(msg->start_item_name); if(item_it != device->ctrl_table_.end()) @@ -1442,15 +1443,29 @@ void RobotisController::writeControlTableCallback(const robotis_controller_msgs: return; } + fprintf(stderr, " #2 "); dynamixel::PortHandler *port = robot_->ports_[device->port_name_]; dynamixel::PacketHandler *packet_handler = dynamixel::PacketHandler::getPacketHandler(device->protocol_version_); if (item->access_type_ == Read) return; + queue_mutex_.lock(); + + direct_sync_write_.push_back(new dynamixel::GroupSyncWrite(port, packet_handler, item->address_, msg->data_length)); + fprintf(stderr, " #3 \n"); + direct_sync_write_[direct_sync_write_.size() - 1]->addParam(device->id_, (uint8_t *)(msg->data.data())); + + fprintf(stderr, "[WriteControlTable] %s -> %s : ", msg->joint_name.c_str(), msg->start_item_name.c_str()); + for (auto &dt : msg->data) + fprintf(stderr, "%02X ", dt); + fprintf(stderr, "\n"); + + queue_mutex_.unlock(); + } void RobotisController::syncWriteItemCallback(const robotis_controller_msgs::SyncWriteItem::ConstPtr &msg) diff --git a/robotis_device/CHANGELOG.rst b/robotis_device/CHANGELOG.rst old mode 100644 new mode 100755 index 9a5117c..91b9800 --- a/robotis_device/CHANGELOG.rst +++ b/robotis_device/CHANGELOG.rst @@ -1,24 +1,24 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package robotis_device -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.1.1 (2016-08-18) ------------ -* updated the package information - -0.1.0 (2016-08-12) ------------ -* first public release for Kinetic -* modified the package information for release -* develop branch -> master branch -* Setting the license to BSD. -* add SensorState - add Singleton template -* XM-430 / CM-740 device file added. - Sensor device added. -* modified. -* variable name changed. - ConvertRadian2Value / ConvertValue2Radian function bug fixed. -* added code to support the gazebo simulator -* renewal -* Contributors: ROBOTIS, ROBOTIS-zerom, pyo +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package robotis_device +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.1.1 (2016-08-18) +----------- +* updated the package information + +0.1.0 (2016-08-12) +----------- +* first public release for Kinetic +* modified the package information for release +* develop branch -> master branch +* Setting the license to BSD. +* add SensorState + add Singleton template +* XM-430 / CM-740 device file added. + Sensor device added. +* modified. +* variable name changed. + ConvertRadian2Value / ConvertValue2Radian function bug fixed. +* added code to support the gazebo simulator +* renewal +* Contributors: ROBOTIS, ROBOTIS-zerom, pyo diff --git a/robotis_device/CMakeLists.txt b/robotis_device/CMakeLists.txt old mode 100644 new mode 100755 index 1fac3f4..45a1fc8 --- a/robotis_device/CMakeLists.txt +++ b/robotis_device/CMakeLists.txt @@ -1,68 +1,68 @@ -################################################################################ -# CMake -################################################################################ -cmake_minimum_required(VERSION 2.8.3) -project(robotis_device) - -################################################################################ -# Packages -################################################################################ -find_package(catkin REQUIRED COMPONENTS - roscpp - rospy - dynamixel_sdk -) - -################################################################################ -# Declare ROS messages, services and actions -################################################################################ - -################################################################################ -# Declare ROS dynamic reconfigure parameters -################################################################################ - -################################################################################ -# Catkin specific configuration -################################################################################ -catkin_package( - INCLUDE_DIRS include - LIBRARIES robotis_device - CATKIN_DEPENDS roscpp rospy dynamixel_sdk -) - -################################################################################ -# Build -################################################################################ -include_directories( - include - ${catkin_INCLUDE_DIRS} -) - -add_library(robotis_device - src/robotis_device/robot.cpp - src/robotis_device/sensor.cpp - src/robotis_device/dynamixel.cpp -) -add_dependencies(robotis_device ${catkin_EXPORTED_TARGETS}) -target_link_libraries(robotis_device ${catkin_LIBRARIES}) - -################################################################################ -# Install -################################################################################ -install(TARGETS robotis_device - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -) - -install(DIRECTORY devices/ - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -################################################################################ -# Test -################################################################################ +################################################################################ +# CMake +################################################################################ +cmake_minimum_required(VERSION 2.8.3) +project(robotis_device) + +################################################################################ +# Packages +################################################################################ +find_package(catkin REQUIRED COMPONENTS + roscpp + rospy + dynamixel_sdk +) + +################################################################################ +# Declare ROS messages, services and actions +################################################################################ + +################################################################################ +# Declare ROS dynamic reconfigure parameters +################################################################################ + +################################################################################ +# Catkin specific configuration +################################################################################ +catkin_package( + INCLUDE_DIRS include + LIBRARIES robotis_device + CATKIN_DEPENDS roscpp rospy dynamixel_sdk +) + +################################################################################ +# Build +################################################################################ +include_directories( + include + ${catkin_INCLUDE_DIRS} +) + +add_library(robotis_device + src/robotis_device/robot.cpp + src/robotis_device/sensor.cpp + src/robotis_device/dynamixel.cpp +) +add_dependencies(robotis_device ${catkin_EXPORTED_TARGETS}) +target_link_libraries(robotis_device ${catkin_LIBRARIES}) + +################################################################################ +# Install +################################################################################ +install(TARGETS robotis_device + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) + +install(DIRECTORY devices/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +################################################################################ +# Test +################################################################################ diff --git a/robotis_device/devices/dynamixel/GRIPPER.device b/robotis_device/devices/dynamixel/GRIPPER.device old mode 100644 new mode 100755 index 4267c99..7e0846d --- a/robotis_device/devices/dynamixel/GRIPPER.device +++ b/robotis_device/devices/dynamixel/GRIPPER.device @@ -1,73 +1,73 @@ -[device info] -model_name = GRIPPER -device_type = dynamixel - -[type info] -value_of_0_radian_position = 0 -value_of_min_radian_position = 0 -value_of_max_radian_position = 750 -min_radian = 0 -max_radian = 1.150767 - -torque_enable_item_name = torque_enable -present_position_item_name = present_position -present_velocity_item_name = present_velocity -present_current_item_name = present_current -goal_position_item_name = goal_position -goal_velocity_item_name = goal_velocity -goal_current_item_name = goal_torque -position_d_gain_item_name = -position_i_gain_item_name = -position_p_gain_item_name = position_p_gain -velocity_d_gain_item_name = -velocity_i_gain_item_name = velocity_i_gain -velocity_p_gain_item_name = velocity_p_gain - -[control table] -# addr | item name | length | access | memory | min value | max value | signed - 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N - 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N - 7 | ID | 1 | RW | EEPROM | 0 | 252 | N - 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N - 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N - 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N - 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N - 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N - 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N - 24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N - 26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N - 30 | torque_limit | 2 | RW | EEPROM | 0 | 32767 | N - 32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N - 36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y - 40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y - 44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N - 45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N - 46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N - 47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N - 48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N - 49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N - 562 | torque_enable | 1 | RW | RAM | 0 | 1 | N - 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N - 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N - 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N - 586 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N - 588 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N - 594 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N - 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y - 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y - 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y - 606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N - 610 | is_moving | 1 | R | RAM | 0 | 1 | N - 611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y - 615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y - 621 | present_current | 2 | R | RAM | -32768 | 32767 | Y - 623 | present_voltage | 2 | R | RAM | 0 | 500 | N - 625 | present_temperature | 1 | R | RAM | 0 | 200 | N - 626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N - 628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N - 630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N - 632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N - 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N - 890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N - 891 | status_return_level | 1 | RW | RAM | 0 | 2 | N - 892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N +[device info] +model_name = GRIPPER +device_type = dynamixel + +[type info] +value_of_0_radian_position = 0 +value_of_min_radian_position = 0 +value_of_max_radian_position = 750 +min_radian = 0 +max_radian = 1.150767 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_torque +position_d_gain_item_name = +position_i_gain_item_name = +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 30 | torque_limit | 2 | RW | EEPROM | 0 | 32767 | N + 32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 562 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 586 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N + 588 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N + 594 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N + 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y + 606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N + 610 | is_moving | 1 | R | RAM | 0 | 1 | N + 611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 621 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 623 | present_voltage | 2 | R | RAM | 0 | 500 | N + 625 | present_temperature | 1 | R | RAM | 0 | 200 | N + 626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 891 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N diff --git a/robotis_device/devices/dynamixel/GRIPPER_TORQUE.device b/robotis_device/devices/dynamixel/GRIPPER_TORQUE.device new file mode 100755 index 0000000..208b7cb --- /dev/null +++ b/robotis_device/devices/dynamixel/GRIPPER_TORQUE.device @@ -0,0 +1,74 @@ +[device info] +model_name = GRIPPER +device_type = dynamixel + +[type info] +value_of_0_radian_position = 0 +value_of_min_radian_position = 0 +value_of_max_radian_position = 750 +min_radian = 0 +max_radian = 1.150767 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_torque +position_d_gain_item_name = +position_i_gain_item_name = +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 30 | torque_limit | 2 | RW | EEPROM | 0 | 32767 | N + 32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 562 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 586 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N + 588 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N + 590 | position_d_gain | 2 | RW | RAM | 0 | 2047 | N + 594 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N + 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y + 606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N + 610 | is_moving | 1 | R | RAM | 0 | 1 | N + 611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 621 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 623 | present_voltage | 2 | R | RAM | 0 | 500 | N + 625 | present_temperature | 1 | R | RAM | 0 | 200 | N + 626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 891 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N diff --git a/robotis_device/devices/dynamixel/H42-20-S300-R.device b/robotis_device/devices/dynamixel/H42-20-S300-R.device old mode 100644 new mode 100755 index f669d8d..ab7aecb --- a/robotis_device/devices/dynamixel/H42-20-S300-R.device +++ b/robotis_device/devices/dynamixel/H42-20-S300-R.device @@ -1,76 +1,76 @@ -[device info] -model_name = H42-20-S300-R -device_type = dynamixel - -[type info] -torque_to_current_value_ratio = 27.15146 -velocity_to_value_ratio = 2900.59884 -value_of_0_radian_position = 0 -value_of_min_radian_position = -151900 -value_of_max_radian_position = 151900 -min_radian = -3.14159265 -max_radian = 3.14159265 - -torque_enable_item_name = torque_enable -present_position_item_name = present_position -present_velocity_item_name = present_velocity -present_current_item_name = present_current -goal_position_item_name = goal_position -goal_velocity_item_name = goal_velocity -goal_current_item_name = goal_torque -position_d_gain_item_name = -position_i_gain_item_name = -position_p_gain_item_name = position_p_gain -velocity_d_gain_item_name = -velocity_i_gain_item_name = velocity_i_gain -velocity_p_gain_item_name = velocity_p_gain - -[control table] -# addr | item name | length | access | memory | min value | max value | signed - 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N - 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N - 7 | ID | 1 | RW | EEPROM | 0 | 252 | N - 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N - 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N - 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N - 13 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y - 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N - 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N - 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N - 24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N - 26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N - 30 | torque_limit | 2 | RW | EEPROM | 0 | 32767 | N - 32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N - 36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y - 40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y - 44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N - 45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N - 46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N - 47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N - 48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N - 49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N - 562 | torque_enable | 1 | RW | RAM | 0 | 1 | N - 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N - 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N - 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N - 586 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N - 588 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N - 594 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N - 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y - 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y - 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y - 606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N - 610 | is_moving | 1 | R | RAM | 0 | 1 | N - 611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y - 615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y - 621 | present_current | 2 | R | RAM | -32768 | 32767 | Y - 623 | present_voltage | 2 | R | RAM | 0 | 500 | N - 625 | present_temperature | 1 | R | RAM | 0 | 200 | N - 626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N - 628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N - 630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N - 632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N - 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N - 890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N - 891 | status_return_level | 1 | RW | RAM | 0 | 2 | N - 892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N +[device info] +model_name = H42-20-S300-R +device_type = dynamixel + +[type info] +torque_to_current_value_ratio = 27.15146 +velocity_to_value_ratio = 2900.59884 +value_of_0_radian_position = 0 +value_of_min_radian_position = -151900 +value_of_max_radian_position = 151900 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_torque +position_d_gain_item_name = +position_i_gain_item_name = +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 13 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 30 | torque_limit | 2 | RW | EEPROM | 0 | 32767 | N + 32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 562 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 586 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N + 588 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N + 594 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N + 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y + 606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N + 610 | is_moving | 1 | R | RAM | 0 | 1 | N + 611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 621 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 623 | present_voltage | 2 | R | RAM | 0 | 500 | N + 625 | present_temperature | 1 | R | RAM | 0 | 200 | N + 626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 891 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N diff --git a/robotis_device/devices/dynamixel/H54-100-B210-R-NR.device b/robotis_device/devices/dynamixel/H54-100-B210-R-NR.device old mode 100644 new mode 100755 index 4cb137a..42342ae --- a/robotis_device/devices/dynamixel/H54-100-B210-R-NR.device +++ b/robotis_device/devices/dynamixel/H54-100-B210-R-NR.device @@ -1,76 +1,76 @@ -[device info] -model_name = H54-100-B210-R-NR -device_type = dynamixel - -[type info] -torque_to_current_value_ratio = 9.09201 -velocity_to_value_ratio = 2046.2777 -value_of_0_radian_position = 0 -value_of_min_radian_position = -250950 -value_of_max_radian_position = 250950 -min_radian = -3.14159265 -max_radian = 3.14159265 - -torque_enable_item_name = torque_enable -present_position_item_name = present_position -present_velocity_item_name = present_velocity -present_current_item_name = present_current -goal_position_item_name = goal_position -goal_velocity_item_name = goal_velocity -goal_current_item_name = goal_torque -position_d_gain_item_name = -position_i_gain_item_name = -position_p_gain_item_name = position_p_gain -velocity_d_gain_item_name = -velocity_i_gain_item_name = velocity_i_gain -velocity_p_gain_item_name = velocity_p_gain - -[control table] -# addr | item name | length | access | memory | min value | max value | signed - 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N - 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N - 7 | ID | 1 | RW | EEPROM | 0 | 252 | N - 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N - 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N - 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N - 13 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y - 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N - 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N - 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N - 24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N - 26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N - 30 | torque_limit | 2 | RW | EEPROM | 0 | 32767 | N - 32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N - 36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y - 40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y - 44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N - 45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N - 46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N - 47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N - 48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N - 49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N - 562 | torque_enable | 1 | RW | RAM | 0 | 1 | N - 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N - 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N - 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N - 586 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N - 588 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N - 594 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N - 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y - 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y - 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y - 606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N - 610 | is_moving | 1 | R | RAM | 0 | 1 | N - 611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y - 615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y - 621 | present_current | 2 | R | RAM | -32768 | 32767 | Y - 623 | present_voltage | 2 | R | RAM | 0 | 500 | N - 625 | present_temperature | 1 | R | RAM | 0 | 200 | N - 626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N - 628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N - 630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N - 632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N - 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N - 890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N - 891 | status_return_level | 1 | RW | RAM | 0 | 2 | N - 892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N +[device info] +model_name = H54-100-B210-R-NR +device_type = dynamixel + +[type info] +torque_to_current_value_ratio = 9.09201 +velocity_to_value_ratio = 2046.2777 +value_of_0_radian_position = 0 +value_of_min_radian_position = -250950 +value_of_max_radian_position = 250950 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_torque +position_d_gain_item_name = +position_i_gain_item_name = +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 13 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 30 | torque_limit | 2 | RW | EEPROM | 0 | 32767 | N + 32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 562 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 586 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N + 588 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N + 594 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N + 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y + 606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N + 610 | is_moving | 1 | R | RAM | 0 | 1 | N + 611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 621 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 623 | present_voltage | 2 | R | RAM | 0 | 500 | N + 625 | present_temperature | 1 | R | RAM | 0 | 200 | N + 626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 891 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N diff --git a/robotis_device/devices/dynamixel/H54-100-S500-R.device b/robotis_device/devices/dynamixel/H54-100-S500-R.device old mode 100644 new mode 100755 index 96fffa3..b042f27 --- a/robotis_device/devices/dynamixel/H54-100-S500-R.device +++ b/robotis_device/devices/dynamixel/H54-100-S500-R.device @@ -1,76 +1,76 @@ -[device info] -model_name = H54-100-S500-R -device_type = dynamixel - -[type info] -torque_to_current_value_ratio = 9.66026 -velocity_to_value_ratio = 4793.01226 -value_of_0_radian_position = 0 -value_of_min_radian_position = -250950 -value_of_max_radian_position = 250950 -min_radian = -3.14159265 -max_radian = 3.14159265 - -torque_enable_item_name = torque_enable -present_position_item_name = present_position -present_velocity_item_name = present_velocity -present_current_item_name = present_current -goal_position_item_name = goal_position -goal_velocity_item_name = goal_velocity -goal_current_item_name = goal_torque -position_d_gain_item_name = -position_i_gain_item_name = -position_p_gain_item_name = position_p_gain -velocity_d_gain_item_name = -velocity_i_gain_item_name = velocity_i_gain -velocity_p_gain_item_name = velocity_p_gain - -[control table] -# addr | item name | length | access | memory | min value | max value | signed - 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N - 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N - 7 | ID | 1 | RW | EEPROM | 0 | 252 | N - 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N - 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N - 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N - 13 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y - 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N - 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N - 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N - 24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N - 26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N - 30 | torque_limit | 2 | RW | EEPROM | 0 | 32767 | N - 32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N - 36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y - 40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y - 44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N - 45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N - 46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N - 47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N - 48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N - 49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N - 562 | torque_enable | 1 | RW | RAM | 0 | 1 | N - 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N - 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N - 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N - 586 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N - 588 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N - 594 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N - 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y - 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y - 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y - 606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N - 610 | is_moving | 1 | R | RAM | 0 | 1 | N - 611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y - 615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y - 621 | present_current | 2 | R | RAM | -32768 | 32767 | Y - 623 | present_voltage | 2 | R | RAM | 0 | 500 | N - 625 | present_temperature | 1 | R | RAM | 0 | 200 | N - 626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N - 628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N - 630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N - 632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N - 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N - 890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N - 891 | status_return_level | 1 | RW | RAM | 0 | 2 | N - 892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N +[device info] +model_name = H54-100-S500-R +device_type = dynamixel + +[type info] +torque_to_current_value_ratio = 9.66026 +velocity_to_value_ratio = 4793.01226 +value_of_0_radian_position = 0 +value_of_min_radian_position = -250950 +value_of_max_radian_position = 250950 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_torque +position_d_gain_item_name = +position_i_gain_item_name = +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 13 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 30 | torque_limit | 2 | RW | EEPROM | 0 | 32767 | N + 32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 562 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 586 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N + 588 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N + 594 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N + 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y + 606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N + 610 | is_moving | 1 | R | RAM | 0 | 1 | N + 611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 621 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 623 | present_voltage | 2 | R | RAM | 0 | 500 | N + 625 | present_temperature | 1 | R | RAM | 0 | 200 | N + 626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 891 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N diff --git a/robotis_device/devices/dynamixel/H54-200-B500-R.device b/robotis_device/devices/dynamixel/H54-200-B500-R.device old mode 100644 new mode 100755 index aa79b4b..ca52162 --- a/robotis_device/devices/dynamixel/H54-200-B500-R.device +++ b/robotis_device/devices/dynamixel/H54-200-B500-R.device @@ -1,76 +1,76 @@ -[device info] -model_name = H54-200-B500-R -device_type = dynamixel - -[type info] -torque_to_current_value_ratio = 9.09201 -velocity_to_value_ratio = 4793.01226 -value_of_0_radian_position = 0 -value_of_min_radian_position = -250950 -value_of_max_radian_position = 250950 -min_radian = -3.14159265 -max_radian = 3.14159265 - -torque_enable_item_name = torque_enable -present_position_item_name = present_position -present_velocity_item_name = present_velocity -present_current_item_name = present_current -goal_position_item_name = goal_position -goal_velocity_item_name = goal_velocity -goal_current_item_name = goal_torque -position_d_gain_item_name = -position_i_gain_item_name = -position_p_gain_item_name = position_p_gain -velocity_d_gain_item_name = -velocity_i_gain_item_name = velocity_i_gain -velocity_p_gain_item_name = velocity_p_gain - -[control table] -# addr | item name | length | access | memory | min value | max value | signed - 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N - 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N - 7 | ID | 1 | RW | EEPROM | 0 | 252 | N - 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N - 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N - 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N - 13 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y - 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N - 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N - 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N - 24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N - 26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N - 30 | torque_limit | 2 | RW | EEPROM | 0 | 32767 | N - 32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N - 36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y - 40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y - 44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N - 45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N - 46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N - 47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N - 48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N - 49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N - 562 | torque_enable | 1 | RW | RAM | 0 | 1 | N - 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N - 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N - 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N - 586 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N - 588 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N - 594 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N - 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y - 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y - 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y - 606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N - 610 | is_moving | 1 | R | RAM | 0 | 1 | N - 611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y - 615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y - 621 | present_current | 2 | R | RAM | -32768 | 32767 | Y - 623 | present_voltage | 2 | R | RAM | 0 | 500 | N - 625 | present_temperature | 1 | R | RAM | 0 | 200 | N - 626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N - 628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N - 630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N - 632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N - 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N - 890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N - 891 | status_return_level | 1 | RW | RAM | 0 | 2 | N - 892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N +[device info] +model_name = H54-200-B500-R +device_type = dynamixel + +[type info] +torque_to_current_value_ratio = 9.09201 +velocity_to_value_ratio = 4793.01226 +value_of_0_radian_position = 0 +value_of_min_radian_position = -250950 +value_of_max_radian_position = 250950 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_torque +position_d_gain_item_name = +position_i_gain_item_name = +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 13 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 30 | torque_limit | 2 | RW | EEPROM | 0 | 32767 | N + 32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 562 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 586 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N + 588 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N + 594 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N + 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y + 606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N + 610 | is_moving | 1 | R | RAM | 0 | 1 | N + 611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 621 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 623 | present_voltage | 2 | R | RAM | 0 | 500 | N + 625 | present_temperature | 1 | R | RAM | 0 | 200 | N + 626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 891 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N diff --git a/robotis_device/devices/dynamixel/H54-200-S500-R.device b/robotis_device/devices/dynamixel/H54-200-S500-R.device old mode 100644 new mode 100755 index 76f6300..4f81222 --- a/robotis_device/devices/dynamixel/H54-200-S500-R.device +++ b/robotis_device/devices/dynamixel/H54-200-S500-R.device @@ -1,76 +1,76 @@ -[device info] -model_name = H54-200-S500-R -device_type = dynamixel - -[type info] -torque_to_current_value_ratio = 9.09201 -velocity_to_value_ratio = 4793.01226 -value_of_0_radian_position = 0 -value_of_min_radian_position = -250950 -value_of_max_radian_position = 250950 -min_radian = -3.14159265 -max_radian = 3.14159265 - -torque_enable_item_name = torque_enable -present_position_item_name = present_position -present_velocity_item_name = present_velocity -present_current_item_name = present_current -goal_position_item_name = goal_position -goal_velocity_item_name = goal_velocity -goal_current_item_name = goal_torque -position_d_gain_item_name = -position_i_gain_item_name = -position_p_gain_item_name = position_p_gain -velocity_d_gain_item_name = -velocity_i_gain_item_name = velocity_i_gain -velocity_p_gain_item_name = velocity_p_gain - -[control table] -# addr | item name | length | access | memory | min value | max value | signed - 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N - 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N - 7 | ID | 1 | RW | EEPROM | 0 | 252 | N - 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N - 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N - 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N - 13 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y - 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N - 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N - 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N - 24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N - 26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N - 30 | torque_limit | 2 | RW | EEPROM | 0 | 32767 | N - 32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N - 36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y - 40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y - 44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N - 45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N - 46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N - 47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N - 48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N - 49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N - 562 | torque_enable | 1 | RW | RAM | 0 | 1 | N - 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N - 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N - 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N - 586 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N - 588 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N - 594 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N - 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y - 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y - 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y - 606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N - 610 | is_moving | 1 | R | RAM | 0 | 1 | N - 611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y - 615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y - 621 | present_current | 2 | R | RAM | -32768 | 32767 | Y - 623 | present_voltage | 2 | R | RAM | 0 | 500 | N - 625 | present_temperature | 1 | R | RAM | 0 | 200 | N - 626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N - 628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N - 630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N - 632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N - 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N - 890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N - 891 | status_return_level | 1 | RW | RAM | 0 | 2 | N - 892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N +[device info] +model_name = H54-200-S500-R +device_type = dynamixel + +[type info] +torque_to_current_value_ratio = 9.09201 +velocity_to_value_ratio = 4793.01226 +value_of_0_radian_position = 0 +value_of_min_radian_position = -250950 +value_of_max_radian_position = 250950 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_torque +position_d_gain_item_name = +position_i_gain_item_name = +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 13 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 30 | torque_limit | 2 | RW | EEPROM | 0 | 32767 | N + 32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 562 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 586 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N + 588 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N + 594 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N + 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y + 606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N + 610 | is_moving | 1 | R | RAM | 0 | 1 | N + 611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 621 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 623 | present_voltage | 2 | R | RAM | 0 | 500 | N + 625 | present_temperature | 1 | R | RAM | 0 | 200 | N + 626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 891 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N diff --git a/robotis_device/devices/dynamixel/L42-10-S300-R.device b/robotis_device/devices/dynamixel/L42-10-S300-R.device old mode 100644 new mode 100755 index 84e556d..8d861f1 --- a/robotis_device/devices/dynamixel/L42-10-S300-R.device +++ b/robotis_device/devices/dynamixel/L42-10-S300-R.device @@ -1,73 +1,73 @@ -[device info] -model_name = L42-10-S300-R -device_type = dynamixel - -[type info] -value_of_0_radian_position = 0 -value_of_min_radian_position = -2047 -value_of_max_radian_position = 2048 -min_radian = -3.14159265 -max_radian = 3.14159265 - -torque_enable_item_name = torque_enable -present_position_item_name = present_position -present_velocity_item_name = present_velocity -present_current_item_name = present_current -goal_position_item_name = goal_position -goal_velocity_item_name = goal_velocity -goal_current_item_name = goal_torque -position_d_gain_item_name = -position_i_gain_item_name = -position_p_gain_item_name = position_p_gain -velocity_d_gain_item_name = -velocity_i_gain_item_name = velocity_i_gain -velocity_p_gain_item_name = velocity_p_gain - -[control table] -# addr | item name | length | access | memory | min value | max value | signed - 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N - 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N - 7 | ID | 1 | RW | EEPROM | 0 | 252 | N - 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N - 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N - 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N - 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N - 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N - 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N - 24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N - 26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N - 30 | torque_limit | 2 | RW | EEPROM | 0 | 32767 | N - 32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N - 36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y - 40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y - 44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N - 45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N - 46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N - 47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N - 48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N - 49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N - 562 | torque_enable | 1 | RW | RAM | 0 | 1 | N - 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N - 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N - 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N - 586 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N - 588 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N - 594 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N - 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y - 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y - 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y - 606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N - 610 | is_moving | 1 | R | RAM | 0 | 1 | N - 611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y - 615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y - 621 | present_current | 2 | R | RAM | -32768 | 32767 | Y - 623 | present_voltage | 2 | R | RAM | 0 | 500 | N - 625 | present_temperature | 1 | R | RAM | 0 | 200 | N - 626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N - 628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N - 630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N - 632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N - 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N - 890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N - 891 | status_return_level | 1 | RW | RAM | 0 | 2 | N - 892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N +[device info] +model_name = L42-10-S300-R +device_type = dynamixel + +[type info] +value_of_0_radian_position = 0 +value_of_min_radian_position = -2047 +value_of_max_radian_position = 2048 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_torque +position_d_gain_item_name = +position_i_gain_item_name = +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 30 | torque_limit | 2 | RW | EEPROM | 0 | 32767 | N + 32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 562 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 586 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N + 588 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N + 594 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N + 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y + 606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N + 610 | is_moving | 1 | R | RAM | 0 | 1 | N + 611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 621 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 623 | present_voltage | 2 | R | RAM | 0 | 500 | N + 625 | present_temperature | 1 | R | RAM | 0 | 200 | N + 626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 891 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N diff --git a/robotis_device/devices/dynamixel/L54-30-S400-R.device b/robotis_device/devices/dynamixel/L54-30-S400-R.device old mode 100644 new mode 100755 index 9dc5d36..71fa09d --- a/robotis_device/devices/dynamixel/L54-30-S400-R.device +++ b/robotis_device/devices/dynamixel/L54-30-S400-R.device @@ -1,74 +1,74 @@ -[device info] -model_name = L54-30-S400-R -device_type = dynamixel - -[type info] -value_of_0_radian_position = 0 -value_of_min_radian_position = -144198 -value_of_max_radian_position = 144198 -min_radian = -3.14159265 -max_radian = 3.14159265 - -torque_enable_item_name = torque_enable -present_position_item_name = present_position -present_velocity_item_name = present_velocity -present_current_item_name = present_current -goal_position_item_name = goal_position -goal_velocity_item_name = goal_velocity -goal_current_item_name = goal_torque -position_d_gain_item_name = -position_i_gain_item_name = -position_p_gain_item_name = position_p_gain -velocity_d_gain_item_name = -velocity_i_gain_item_name = velocity_i_gain -velocity_p_gain_item_name = velocity_p_gain - -[control table] -# addr | item name | length | access | memory | min value | max value | signed - 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N - 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N - 7 | ID | 1 | RW | EEPROM | 0 | 252 | N - 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N - 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N - 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N - 13 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y - 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N - 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N - 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N - 24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N - 26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N - 30 | torque_limit | 2 | RW | EEPROM | 0 | 32767 | N - 32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N - 36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y - 40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y - 44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N - 45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N - 46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N - 47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N - 48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N - 49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N - 562 | torque_enable | 1 | RW | RAM | 0 | 1 | N - 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N - 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N - 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N - 586 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N - 588 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N - 594 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N - 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y - 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y - 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y - 606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N - 610 | is_moving | 1 | R | RAM | 0 | 1 | N - 611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y - 615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y - 621 | present_current | 2 | R | RAM | -32768 | 32767 | Y - 623 | present_voltage | 2 | R | RAM | 0 | 500 | N - 625 | present_temperature | 1 | R | RAM | 0 | 200 | N - 626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N - 628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N - 630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N - 632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N - 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N - 890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N - 891 | status_return_level | 1 | RW | RAM | 0 | 2 | N - 892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N +[device info] +model_name = L54-30-S400-R +device_type = dynamixel + +[type info] +value_of_0_radian_position = 0 +value_of_min_radian_position = -144198 +value_of_max_radian_position = 144198 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_torque +position_d_gain_item_name = +position_i_gain_item_name = +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 13 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 30 | torque_limit | 2 | RW | EEPROM | 0 | 32767 | N + 32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 562 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 586 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N + 588 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N + 594 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N + 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y + 606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N + 610 | is_moving | 1 | R | RAM | 0 | 1 | N + 611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 621 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 623 | present_voltage | 2 | R | RAM | 0 | 500 | N + 625 | present_temperature | 1 | R | RAM | 0 | 200 | N + 626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 891 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N diff --git a/robotis_device/devices/dynamixel/L54-30-S500-R.device b/robotis_device/devices/dynamixel/L54-30-S500-R.device old mode 100644 new mode 100755 index fa15941..5a8add7 --- a/robotis_device/devices/dynamixel/L54-30-S500-R.device +++ b/robotis_device/devices/dynamixel/L54-30-S500-R.device @@ -1,74 +1,74 @@ -[device info] -model_name = L54-30-S500-R -device_type = dynamixel - -[type info] -value_of_0_radian_position = 0 -value_of_min_radian_position = -180684 -value_of_max_radian_position = 180684 -min_radian = -3.14159265 -max_radian = 3.14159265 - -torque_enable_item_name = torque_enable -present_position_item_name = present_position -present_velocity_item_name = present_velocity -present_current_item_name = present_current -goal_position_item_name = goal_position -goal_velocity_item_name = goal_velocity -goal_current_item_name = goal_torque -position_d_gain_item_name = -position_i_gain_item_name = -position_p_gain_item_name = position_p_gain -velocity_d_gain_item_name = -velocity_i_gain_item_name = velocity_i_gain -velocity_p_gain_item_name = velocity_p_gain - -[control table] -# addr | item name | length | access | memory | min value | max value | signed - 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N - 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N - 7 | ID | 1 | RW | EEPROM | 0 | 252 | N - 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N - 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N - 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N - 13 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y - 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N - 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N - 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N - 24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N - 26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N - 30 | torque_limit | 2 | RW | EEPROM | 0 | 32767 | N - 32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N - 36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y - 40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y - 44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N - 45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N - 46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N - 47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N - 48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N - 49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N - 562 | torque_enable | 1 | RW | RAM | 0 | 1 | N - 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N - 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N - 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N - 586 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N - 588 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N - 594 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N - 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y - 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y - 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y - 606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N - 610 | is_moving | 1 | R | RAM | 0 | 1 | N - 611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y - 615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y - 621 | present_current | 2 | R | RAM | -32768 | 32767 | Y - 623 | present_voltage | 2 | R | RAM | 0 | 500 | N - 625 | present_temperature | 1 | R | RAM | 0 | 200 | N - 626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N - 628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N - 630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N - 632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N - 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N - 890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N - 891 | status_return_level | 1 | RW | RAM | 0 | 2 | N - 892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N +[device info] +model_name = L54-30-S500-R +device_type = dynamixel + +[type info] +value_of_0_radian_position = 0 +value_of_min_radian_position = -180684 +value_of_max_radian_position = 180684 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_torque +position_d_gain_item_name = +position_i_gain_item_name = +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 13 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 30 | torque_limit | 2 | RW | EEPROM | 0 | 32767 | N + 32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 562 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 586 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N + 588 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N + 594 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N + 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y + 606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N + 610 | is_moving | 1 | R | RAM | 0 | 1 | N + 611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 621 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 623 | present_voltage | 2 | R | RAM | 0 | 500 | N + 625 | present_temperature | 1 | R | RAM | 0 | 200 | N + 626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 891 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N diff --git a/robotis_device/devices/dynamixel/L54-50-S290-R.device b/robotis_device/devices/dynamixel/L54-50-S290-R.device old mode 100644 new mode 100755 index 8c2f1a2..7af9c22 --- a/robotis_device/devices/dynamixel/L54-50-S290-R.device +++ b/robotis_device/devices/dynamixel/L54-50-S290-R.device @@ -1,74 +1,74 @@ -[device info] -model_name = L54-50-S290-R -device_type = dynamixel - -[type info] -value_of_0_radian_position = 0 -value_of_min_radian_position = -103860 -value_of_max_radian_position = 103860 -min_radian = -3.14159265 -max_radian = 3.14159265 - -torque_enable_item_name = torque_enable -present_position_item_name = present_position -present_velocity_item_name = present_velocity -present_current_item_name = present_current -goal_position_item_name = goal_position -goal_velocity_item_name = goal_velocity -goal_current_item_name = goal_torque -position_d_gain_item_name = -position_i_gain_item_name = -position_p_gain_item_name = position_p_gain -velocity_d_gain_item_name = -velocity_i_gain_item_name = velocity_i_gain -velocity_p_gain_item_name = velocity_p_gain - -[control table] -# addr | item name | length | access | memory | min value | max value | signed - 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N - 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N - 7 | ID | 1 | RW | EEPROM | 0 | 252 | N - 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N - 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N - 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N - 13 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y - 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N - 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N - 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N - 24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N - 26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N - 30 | torque_limit | 2 | RW | EEPROM | 0 | 32767 | N - 32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N - 36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y - 40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y - 44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N - 45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N - 46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N - 47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N - 48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N - 49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N - 562 | torque_enable | 1 | RW | RAM | 0 | 1 | N - 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N - 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N - 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N - 586 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N - 588 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N - 594 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N - 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y - 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y - 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y - 606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N - 610 | is_moving | 1 | R | RAM | 0 | 1 | N - 611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y - 615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y - 621 | present_current | 2 | R | RAM | -32768 | 32767 | Y - 623 | present_voltage | 2 | R | RAM | 0 | 500 | N - 625 | present_temperature | 1 | R | RAM | 0 | 200 | N - 626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N - 628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N - 630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N - 632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N - 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N - 890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N - 891 | status_return_level | 1 | RW | RAM | 0 | 2 | N - 892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N +[device info] +model_name = L54-50-S290-R +device_type = dynamixel + +[type info] +value_of_0_radian_position = 0 +value_of_min_radian_position = -103860 +value_of_max_radian_position = 103860 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_torque +position_d_gain_item_name = +position_i_gain_item_name = +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 13 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 30 | torque_limit | 2 | RW | EEPROM | 0 | 32767 | N + 32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 562 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 586 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N + 588 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N + 594 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N + 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y + 606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N + 610 | is_moving | 1 | R | RAM | 0 | 1 | N + 611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 621 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 623 | present_voltage | 2 | R | RAM | 0 | 500 | N + 625 | present_temperature | 1 | R | RAM | 0 | 200 | N + 626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 891 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N diff --git a/robotis_device/devices/dynamixel/L54-50-S500-R.device b/robotis_device/devices/dynamixel/L54-50-S500-R.device old mode 100644 new mode 100755 index 991e159..11d63b3 --- a/robotis_device/devices/dynamixel/L54-50-S500-R.device +++ b/robotis_device/devices/dynamixel/L54-50-S500-R.device @@ -1,74 +1,74 @@ -[device info] -model_name = L54-50-S500-R -device_type = dynamixel - -[type info] -value_of_0_radian_position = 0 -value_of_min_radian_position = -180684 -value_of_max_radian_position = 180684 -min_radian = -3.14159265 -max_radian = 3.14159265 - -torque_enable_item_name = torque_enable -present_position_item_name = present_position -present_velocity_item_name = present_velocity -present_current_item_name = present_current -goal_position_item_name = goal_position -goal_velocity_item_name = goal_velocity -goal_current_item_name = goal_torque -position_d_gain_item_name = -position_i_gain_item_name = -position_p_gain_item_name = position_p_gain -velocity_d_gain_item_name = -velocity_i_gain_item_name = velocity_i_gain -velocity_p_gain_item_name = velocity_p_gain - -[control table] -# addr | item name | length | access | memory | min value | max value | signed - 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N - 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N - 7 | ID | 1 | RW | EEPROM | 0 | 252 | N - 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N - 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N - 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N - 13 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y - 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N - 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N - 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N - 24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N - 26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N - 30 | torque_limit | 2 | RW | EEPROM | 0 | 32767 | N - 32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N - 36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y - 40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y - 44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N - 45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N - 46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N - 47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N - 48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N - 49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N - 562 | torque_enable | 1 | RW | RAM | 0 | 1 | N - 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N - 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N - 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N - 586 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N - 588 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N - 594 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N - 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y - 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y - 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y - 606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N - 610 | is_moving | 1 | R | RAM | 0 | 1 | N - 611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y - 615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y - 621 | present_current | 2 | R | RAM | -32768 | 32767 | Y - 623 | present_voltage | 2 | R | RAM | 0 | 500 | N - 625 | present_temperature | 1 | R | RAM | 0 | 200 | N - 626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N - 628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N - 630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N - 632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N - 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N - 890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N - 891 | status_return_level | 1 | RW | RAM | 0 | 2 | N - 892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N +[device info] +model_name = L54-50-S500-R +device_type = dynamixel + +[type info] +value_of_0_radian_position = 0 +value_of_min_radian_position = -180684 +value_of_max_radian_position = 180684 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_torque +position_d_gain_item_name = +position_i_gain_item_name = +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 13 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 30 | torque_limit | 2 | RW | EEPROM | 0 | 32767 | N + 32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 562 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 586 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N + 588 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N + 594 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N + 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y + 606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N + 610 | is_moving | 1 | R | RAM | 0 | 1 | N + 611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 621 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 623 | present_voltage | 2 | R | RAM | 0 | 500 | N + 625 | present_temperature | 1 | R | RAM | 0 | 200 | N + 626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 891 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N diff --git a/robotis_device/devices/dynamixel/M42-10-S260-R.device b/robotis_device/devices/dynamixel/M42-10-S260-R.device old mode 100644 new mode 100755 index 4c25631..ba205bb --- a/robotis_device/devices/dynamixel/M42-10-S260-R.device +++ b/robotis_device/devices/dynamixel/M42-10-S260-R.device @@ -1,74 +1,74 @@ -[device info] -model_name = M42-10-S260-R -device_type = dynamixel - -[type info] -value_of_0_radian_position = 0 -value_of_min_radian_position = -131584 -value_of_max_radian_position = 131584 -min_radian = -3.14159265 -max_radian = 3.14159265 - -torque_enable_item_name = torque_enable -present_position_item_name = present_position -present_velocity_item_name = present_velocity -present_current_item_name = present_current -goal_position_item_name = goal_position -goal_velocity_item_name = goal_velocity -goal_current_item_name = goal_torque -position_d_gain_item_name = -position_i_gain_item_name = -position_p_gain_item_name = position_p_gain -velocity_d_gain_item_name = -velocity_i_gain_item_name = velocity_i_gain -velocity_p_gain_item_name = velocity_p_gain - -[control table] -# addr | item name | length | access | memory | min value | max value | signed - 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N - 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N - 7 | ID | 1 | RW | EEPROM | 0 | 252 | N - 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N - 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N - 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N - 13 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y - 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N - 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N - 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N - 24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N - 26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N - 30 | torque_limit | 2 | RW | EEPROM | 0 | 32767 | N - 32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N - 36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y - 40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y - 44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N - 45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N - 46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N - 47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N - 48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N - 49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N - 562 | torque_enable | 1 | RW | RAM | 0 | 1 | N - 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N - 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N - 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N - 586 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N - 588 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N - 594 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N - 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y - 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y - 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y - 606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N - 610 | is_moving | 1 | R | RAM | 0 | 1 | N - 611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y - 615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y - 621 | present_current | 2 | R | RAM | -32768 | 32767 | Y - 623 | present_voltage | 2 | R | RAM | 0 | 500 | N - 625 | present_temperature | 1 | R | RAM | 0 | 200 | N - 626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N - 628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N - 630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N - 632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N - 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N - 890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N - 891 | status_return_level | 1 | RW | RAM | 0 | 2 | N - 892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N +[device info] +model_name = M42-10-S260-R +device_type = dynamixel + +[type info] +value_of_0_radian_position = 0 +value_of_min_radian_position = -131584 +value_of_max_radian_position = 131584 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_torque +position_d_gain_item_name = +position_i_gain_item_name = +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 13 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 30 | torque_limit | 2 | RW | EEPROM | 0 | 32767 | N + 32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 562 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 586 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N + 588 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N + 594 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N + 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y + 606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N + 610 | is_moving | 1 | R | RAM | 0 | 1 | N + 611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 621 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 623 | present_voltage | 2 | R | RAM | 0 | 500 | N + 625 | present_temperature | 1 | R | RAM | 0 | 200 | N + 626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 891 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N diff --git a/robotis_device/devices/dynamixel/M54-40-S250-R.device b/robotis_device/devices/dynamixel/M54-40-S250-R.device old mode 100644 new mode 100755 index 76796b1..62f9990 --- a/robotis_device/devices/dynamixel/M54-40-S250-R.device +++ b/robotis_device/devices/dynamixel/M54-40-S250-R.device @@ -1,74 +1,74 @@ -[device info] -model_name = M54-40-S250-R -device_type = dynamixel - -[type info] -value_of_0_radian_position = 0 -value_of_min_radian_position = -125700 -value_of_max_radian_position = 125700 -min_radian = -3.14159265 -max_radian = 3.14159265 - -torque_enable_item_name = torque_enable -present_position_item_name = present_position -present_velocity_item_name = present_velocity -present_current_item_name = present_current -goal_position_item_name = goal_position -goal_velocity_item_name = goal_velocity -goal_current_item_name = goal_torque -position_d_gain_item_name = -position_i_gain_item_name = -position_p_gain_item_name = position_p_gain -velocity_d_gain_item_name = -velocity_i_gain_item_name = velocity_i_gain -velocity_p_gain_item_name = velocity_p_gain - -[control table] -# addr | item name | length | access | memory | min value | max value | signed - 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N - 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N - 7 | ID | 1 | RW | EEPROM | 0 | 252 | N - 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N - 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N - 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N - 13 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y - 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N - 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N - 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N - 24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N - 26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N - 30 | torque_limit | 2 | RW | EEPROM | 0 | 32767 | N - 32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N - 36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y - 40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y - 44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N - 45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N - 46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N - 47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N - 48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N - 49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N - 562 | torque_enable | 1 | RW | RAM | 0 | 1 | N - 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N - 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N - 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N - 586 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N - 588 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N - 594 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N - 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y - 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y - 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y - 606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N - 610 | is_moving | 1 | R | RAM | 0 | 1 | N - 611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y - 615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y - 621 | present_current | 2 | R | RAM | -32768 | 32767 | Y - 623 | present_voltage | 2 | R | RAM | 0 | 500 | N - 625 | present_temperature | 1 | R | RAM | 0 | 200 | N - 626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N - 628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N - 630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N - 632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N - 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N - 890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N - 891 | status_return_level | 1 | RW | RAM | 0 | 2 | N - 892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N +[device info] +model_name = M54-40-S250-R +device_type = dynamixel + +[type info] +value_of_0_radian_position = 0 +value_of_min_radian_position = -125700 +value_of_max_radian_position = 125700 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_torque +position_d_gain_item_name = +position_i_gain_item_name = +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 13 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 30 | torque_limit | 2 | RW | EEPROM | 0 | 32767 | N + 32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 562 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 586 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N + 588 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N + 594 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N + 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y + 606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N + 610 | is_moving | 1 | R | RAM | 0 | 1 | N + 611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 621 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 623 | present_voltage | 2 | R | RAM | 0 | 500 | N + 625 | present_temperature | 1 | R | RAM | 0 | 200 | N + 626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 891 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N diff --git a/robotis_device/devices/dynamixel/M54-60-S250-R.device b/robotis_device/devices/dynamixel/M54-60-S250-R.device old mode 100644 new mode 100755 index a6cfa2a..dd2efde --- a/robotis_device/devices/dynamixel/M54-60-S250-R.device +++ b/robotis_device/devices/dynamixel/M54-60-S250-R.device @@ -1,74 +1,74 @@ -[device info] -model_name = M54-60-S250-R -device_type = dynamixel - -[type info] -value_of_0_radian_position = 0 -value_of_min_radian_position = -125700 -value_of_max_radian_position = 125700 -min_radian = -3.14159265 -max_radian = 3.14159265 - -torque_enable_item_name = torque_enable -present_position_item_name = present_position -present_velocity_item_name = present_velocity -present_current_item_name = present_current -goal_position_item_name = goal_position -goal_velocity_item_name = goal_velocity -goal_current_item_name = goal_torque -position_d_gain_item_name = -position_i_gain_item_name = -position_p_gain_item_name = position_p_gain -velocity_d_gain_item_name = -velocity_i_gain_item_name = velocity_i_gain -velocity_p_gain_item_name = velocity_p_gain - -[control table] -# addr | item name | length | access | memory | min value | max value | signed - 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N - 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N - 7 | ID | 1 | RW | EEPROM | 0 | 252 | N - 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N - 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N - 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N - 13 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y - 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N - 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N - 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N - 24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N - 26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N - 30 | torque_limit | 2 | RW | EEPROM | 0 | 32767 | N - 32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N - 36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y - 40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y - 44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N - 45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N - 46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N - 47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N - 48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N - 49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N - 562 | torque_enable | 1 | RW | RAM | 0 | 1 | N - 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N - 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N - 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N - 586 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N - 588 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N - 594 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N - 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y - 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y - 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y - 606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N - 610 | is_moving | 1 | R | RAM | 0 | 1 | N - 611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y - 615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y - 621 | present_current | 2 | R | RAM | -32768 | 32767 | Y - 623 | present_voltage | 2 | R | RAM | 0 | 500 | N - 625 | present_temperature | 1 | R | RAM | 0 | 200 | N - 626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N - 628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N - 630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N - 632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N - 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N - 890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N - 891 | status_return_level | 1 | RW | RAM | 0 | 2 | N - 892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N +[device info] +model_name = M54-60-S250-R +device_type = dynamixel + +[type info] +value_of_0_radian_position = 0 +value_of_min_radian_position = -125700 +value_of_max_radian_position = 125700 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_torque +position_d_gain_item_name = +position_i_gain_item_name = +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 13 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 30 | torque_limit | 2 | RW | EEPROM | 0 | 32767 | N + 32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 562 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 586 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N + 588 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N + 594 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N + 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y + 606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N + 610 | is_moving | 1 | R | RAM | 0 | 1 | N + 611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 621 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 623 | present_voltage | 2 | R | RAM | 0 | 500 | N + 625 | present_temperature | 1 | R | RAM | 0 | 200 | N + 626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 891 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N diff --git a/robotis_device/devices/dynamixel/MX-106.device b/robotis_device/devices/dynamixel/MX-106.device old mode 100644 new mode 100755 index 4b8c63d..a122831 --- a/robotis_device/devices/dynamixel/MX-106.device +++ b/robotis_device/devices/dynamixel/MX-106.device @@ -1,66 +1,66 @@ -[device info] -model_name = MX-106 -device_type = dynamixel - -[type info] -value_of_0_radian_position = 2048 -value_of_min_radian_position = 0 -value_of_max_radian_position = 4095 -min_radian = -3.14159265 -max_radian = 3.14159265 - -torque_enable_item_name = torque_enable -present_position_item_name = present_position -present_velocity_item_name = present_velocity -present_current_item_name = -goal_position_item_name = goal_position -goal_velocity_item_name = goal_velocity -goal_current_item_name = -position_d_gain_item_name = position_d_gain -position_i_gain_item_name = position_i_gain -position_p_gain_item_name = position_p_gain -velocity_d_gain_item_name = -velocity_i_gain_item_name = -velocity_p_gain_item_name = - -[control table] -# addr | item name | length | access | memory | min value | max value | signed - 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N - 2 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N - 3 | ID | 1 | RW | EEPROM | 0 | 252 | N - 4 | baudrate | 1 | RW | EEPROM | 0 | 252 | N - 5 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N - 6 | CW_angle_limit | 2 | RW | EEPROM | 0 | 4095 | N - 8 | CCW_angle_limit | 2 | RW | EEPROM | 0 | 4095 | N - 10 | drive_mode | 1 | RW | EEPROM | 0 | 3 | N - 11 | max_temperature_limit | 1 | RW | EEPROM | 0 | 99 | N - 12 | min_voltage_limit | 1 | RW | EEPROM | 0 | 250 | N - 13 | max_voltage_limit | 1 | RW | EEPROM | 0 | 250 | N - 14 | max_torque | 2 | RW | EEPROM | 0 | 1023 | N - 16 | status_return_level | 1 | RW | EEPROM | 0 | 2 | N - 17 | alarm_LED | 1 | RW | EEPROM | 0 | 127 | N - 18 | alarm_shutdown | 1 | RW | EEPROM | 0 | 127 | N - 20 | multi_turn_offset | 2 | RW | EEPROM | -26624 | 26624 | Y - 22 | resolution_dividor | 1 | RW | EEPROM | 1 | 255 | N - 24 | torque_enable | 1 | RW | RAM | 0 | 1 | N - 25 | LED | 1 | RW | RAM | 0 | 1 | N - 26 | position_d_gain | 1 | RW | RAM | 0 | 254 | N - 27 | position_i_gain | 1 | RW | RAM | 0 | 254 | N - 28 | position_p_gain | 1 | RW | RAM | 0 | 254 | N - 30 | goal_position | 2 | RW | RAM | -28672 | 28672 | Y - 32 | goal_velocity | 2 | RW | RAM | 0 | 1023 | N - 34 | goal_torque | 2 | RW | RAM | 0 | 1023 | N - 36 | present_position | 2 | R | RAM | -32768 | 32767 | Y - 38 | present_velocity | 2 | R | RAM | 0 | 2048 | N - 40 | present_load | 2 | R | RAM | 0 | 2048 | N - 42 | present_voltage | 1 | R | RAM | 50 | 250 | N - 43 | present_temperature | 1 | R | RAM | 0 | 99 | N - 44 | registered_instruction | 1 | R | RAM | 0 | 1 | N - 46 | is_moving | 1 | R | RAM | 0 | 1 | N - 47 | EEPROM_lock | 1 | RW | RAM | 0 | 1 | N - 48 | punch | 2 | RW | RAM | 0 | 1023 | N - 68 | current_consumption | 2 | RW | RAM | 0 | 4095 | N - 70 | torque_control_mode | 1 | RW | RAM | 0 | 1 | N - 71 | torque_control_goal | 2 | RW | RAM | 0 | 2047 | N - 73 | goal_acceleration | 1 | RW | RAM | 0 | 254 | N - +[device info] +model_name = MX-106 +device_type = dynamixel + +[type info] +value_of_0_radian_position = 2048 +value_of_min_radian_position = 0 +value_of_max_radian_position = 4095 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = +position_d_gain_item_name = position_d_gain +position_i_gain_item_name = position_i_gain +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = +velocity_p_gain_item_name = + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 2 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 3 | ID | 1 | RW | EEPROM | 0 | 252 | N + 4 | baudrate | 1 | RW | EEPROM | 0 | 252 | N + 5 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 6 | CW_angle_limit | 2 | RW | EEPROM | 0 | 4095 | N + 8 | CCW_angle_limit | 2 | RW | EEPROM | 0 | 4095 | N + 10 | drive_mode | 1 | RW | EEPROM | 0 | 3 | N + 11 | max_temperature_limit | 1 | RW | EEPROM | 0 | 99 | N + 12 | min_voltage_limit | 1 | RW | EEPROM | 0 | 250 | N + 13 | max_voltage_limit | 1 | RW | EEPROM | 0 | 250 | N + 14 | max_torque | 2 | RW | EEPROM | 0 | 1023 | N + 16 | status_return_level | 1 | RW | EEPROM | 0 | 2 | N + 17 | alarm_LED | 1 | RW | EEPROM | 0 | 127 | N + 18 | alarm_shutdown | 1 | RW | EEPROM | 0 | 127 | N + 20 | multi_turn_offset | 2 | RW | EEPROM | -26624 | 26624 | Y + 22 | resolution_dividor | 1 | RW | EEPROM | 1 | 255 | N + 24 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 25 | LED | 1 | RW | RAM | 0 | 1 | N + 26 | position_d_gain | 1 | RW | RAM | 0 | 254 | N + 27 | position_i_gain | 1 | RW | RAM | 0 | 254 | N + 28 | position_p_gain | 1 | RW | RAM | 0 | 254 | N + 30 | goal_position | 2 | RW | RAM | -28672 | 28672 | Y + 32 | goal_velocity | 2 | RW | RAM | 0 | 1023 | N + 34 | goal_torque | 2 | RW | RAM | 0 | 1023 | N + 36 | present_position | 2 | R | RAM | -32768 | 32767 | Y + 38 | present_velocity | 2 | R | RAM | 0 | 2048 | N + 40 | present_load | 2 | R | RAM | 0 | 2048 | N + 42 | present_voltage | 1 | R | RAM | 50 | 250 | N + 43 | present_temperature | 1 | R | RAM | 0 | 99 | N + 44 | registered_instruction | 1 | R | RAM | 0 | 1 | N + 46 | is_moving | 1 | R | RAM | 0 | 1 | N + 47 | EEPROM_lock | 1 | RW | RAM | 0 | 1 | N + 48 | punch | 2 | RW | RAM | 0 | 1023 | N + 68 | current_consumption | 2 | RW | RAM | 0 | 4095 | N + 70 | torque_control_mode | 1 | RW | RAM | 0 | 1 | N + 71 | torque_control_goal | 2 | RW | RAM | 0 | 2047 | N + 73 | goal_acceleration | 1 | RW | RAM | 0 | 254 | N + diff --git a/robotis_device/devices/dynamixel/MX-28.device b/robotis_device/devices/dynamixel/MX-28.device old mode 100644 new mode 100755 index ff67f2a..ae1f877 --- a/robotis_device/devices/dynamixel/MX-28.device +++ b/robotis_device/devices/dynamixel/MX-28.device @@ -1,62 +1,62 @@ -[device info] -model_name = MX-28 -device_type = dynamixel - -[type info] -value_of_0_radian_position = 2048 -value_of_min_radian_position = 0 -value_of_max_radian_position = 4095 -min_radian = -3.14159265 -max_radian = 3.14159265 - -torque_enable_item_name = torque_enable -present_position_item_name = present_position -present_velocity_item_name = present_velocity -present_current_item_name = -goal_position_item_name = goal_position -goal_velocity_item_name = goal_velocity -goal_current_item_name = -position_d_gain_item_name = position_d_gain -position_i_gain_item_name = position_i_gain -position_p_gain_item_name = position_p_gain -velocity_d_gain_item_name = -velocity_i_gain_item_name = -velocity_p_gain_item_name = - -[control table] -# addr | item name | length | access | memory | min value | max value | signed - 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N - 2 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N - 3 | ID | 1 | RW | EEPROM | 0 | 252 | N - 4 | baudrate | 1 | RW | EEPROM | 0 | 252 | N - 5 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N - 6 | CW_angle_limit | 2 | RW | EEPROM | 0 | 4095 | N - 8 | CCW_angle_limit | 2 | RW | EEPROM | 0 | 4095 | N - 10 | drive_mode | 1 | RW | EEPROM | 0 | 3 | N - 11 | max_temperature_limit | 1 | RW | EEPROM | 0 | 99 | N - 12 | min_voltage_limit | 1 | RW | EEPROM | 0 | 250 | N - 13 | max_voltage_limit | 1 | RW | EEPROM | 0 | 250 | N - 14 | max_torque | 2 | RW | EEPROM | 0 | 1023 | N - 16 | status_return_level | 1 | RW | EEPROM | 0 | 2 | N - 17 | alarm_LED | 1 | RW | EEPROM | 0 | 127 | N - 18 | alarm_shutdown | 1 | RW | EEPROM | 0 | 127 | N - 20 | multi_turn_offset | 2 | RW | EEPROM | -26624 | 26624 | Y - 22 | resolution_dividor | 1 | RW | EEPROM | 1 | 255 | N - 24 | torque_enable | 1 | RW | RAM | 0 | 1 | N - 25 | LED | 1 | RW | RAM | 0 | 1 | N - 26 | position_d_gain | 1 | RW | RAM | 0 | 254 | N - 27 | position_i_gain | 1 | RW | RAM | 0 | 254 | N - 28 | position_p_gain | 1 | RW | RAM | 0 | 254 | N - 30 | goal_position | 2 | RW | RAM | -28672 | 28672 | Y - 32 | goal_velocity | 2 | RW | RAM | 0 | 1023 | N - 34 | goal_torque | 2 | RW | RAM | 0 | 1023 | N - 36 | present_position | 2 | R | RAM | -32768 | 32767 | Y - 38 | present_velocity | 2 | R | RAM | 0 | 2048 | N - 40 | present_load | 2 | R | RAM | 0 | 2048 | N - 42 | present_voltage | 1 | R | RAM | 50 | 250 | N - 43 | present_temperature | 1 | R | RAM | 0 | 99 | N - 44 | registered_instruction | 1 | R | RAM | 0 | 1 | N - 46 | is_moving | 1 | R | RAM | 0 | 1 | N - 47 | EEPROM_lock | 1 | RW | RAM | 0 | 1 | N - 48 | punch | 2 | RW | RAM | 0 | 1023 | N - 73 | goal_acceleration | 1 | RW | RAM | 0 | 254 | N +[device info] +model_name = MX-28 +device_type = dynamixel + +[type info] +value_of_0_radian_position = 2048 +value_of_min_radian_position = 0 +value_of_max_radian_position = 4095 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = +position_d_gain_item_name = position_d_gain +position_i_gain_item_name = position_i_gain +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = +velocity_p_gain_item_name = + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 2 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 3 | ID | 1 | RW | EEPROM | 0 | 252 | N + 4 | baudrate | 1 | RW | EEPROM | 0 | 252 | N + 5 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 6 | CW_angle_limit | 2 | RW | EEPROM | 0 | 4095 | N + 8 | CCW_angle_limit | 2 | RW | EEPROM | 0 | 4095 | N + 10 | drive_mode | 1 | RW | EEPROM | 0 | 3 | N + 11 | max_temperature_limit | 1 | RW | EEPROM | 0 | 99 | N + 12 | min_voltage_limit | 1 | RW | EEPROM | 0 | 250 | N + 13 | max_voltage_limit | 1 | RW | EEPROM | 0 | 250 | N + 14 | max_torque | 2 | RW | EEPROM | 0 | 1023 | N + 16 | status_return_level | 1 | RW | EEPROM | 0 | 2 | N + 17 | alarm_LED | 1 | RW | EEPROM | 0 | 127 | N + 18 | alarm_shutdown | 1 | RW | EEPROM | 0 | 127 | N + 20 | multi_turn_offset | 2 | RW | EEPROM | -26624 | 26624 | Y + 22 | resolution_dividor | 1 | RW | EEPROM | 1 | 255 | N + 24 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 25 | LED | 1 | RW | RAM | 0 | 1 | N + 26 | position_d_gain | 1 | RW | RAM | 0 | 254 | N + 27 | position_i_gain | 1 | RW | RAM | 0 | 254 | N + 28 | position_p_gain | 1 | RW | RAM | 0 | 254 | N + 30 | goal_position | 2 | RW | RAM | -28672 | 28672 | Y + 32 | goal_velocity | 2 | RW | RAM | 0 | 1023 | N + 34 | goal_torque | 2 | RW | RAM | 0 | 1023 | N + 36 | present_position | 2 | R | RAM | -32768 | 32767 | Y + 38 | present_velocity | 2 | R | RAM | 0 | 2048 | N + 40 | present_load | 2 | R | RAM | 0 | 2048 | N + 42 | present_voltage | 1 | R | RAM | 50 | 250 | N + 43 | present_temperature | 1 | R | RAM | 0 | 99 | N + 44 | registered_instruction | 1 | R | RAM | 0 | 1 | N + 46 | is_moving | 1 | R | RAM | 0 | 1 | N + 47 | EEPROM_lock | 1 | RW | RAM | 0 | 1 | N + 48 | punch | 2 | RW | RAM | 0 | 1023 | N + 73 | goal_acceleration | 1 | RW | RAM | 0 | 254 | N diff --git a/robotis_device/devices/dynamixel/MX-64.device b/robotis_device/devices/dynamixel/MX-64.device old mode 100644 new mode 100755 index 06c0a4a..c6a50de --- a/robotis_device/devices/dynamixel/MX-64.device +++ b/robotis_device/devices/dynamixel/MX-64.device @@ -1,66 +1,66 @@ -[device info] -model_name = MX-64 -device_type = dynamixel - -[type info] -value_of_0_radian_position = 2048 -value_of_min_radian_position = 0 -value_of_max_radian_position = 4095 -min_radian = -3.14159265 -max_radian = 3.14159265 - -torque_enable_item_name = torque_enable -present_position_item_name = present_position -present_velocity_item_name = present_velocity -present_current_item_name = -goal_position_item_name = goal_position -goal_velocity_item_name = goal_velocity -goal_current_item_name = -position_d_gain_item_name = position_d_gain -position_i_gain_item_name = position_i_gain -position_p_gain_item_name = position_p_gain -velocity_d_gain_item_name = -velocity_i_gain_item_name = -velocity_p_gain_item_name = - -[control table] -# addr | item name | length | access | memory | min value | max value | signed - 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N - 2 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N - 3 | ID | 1 | RW | EEPROM | 0 | 252 | N - 4 | baudrate | 1 | RW | EEPROM | 0 | 252 | N - 5 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N - 6 | CW_angle_limit | 2 | RW | EEPROM | 0 | 4095 | N - 8 | CCW_angle_limit | 2 | RW | EEPROM | 0 | 4095 | N - 10 | drive_mode | 1 | RW | EEPROM | 0 | 3 | N - 11 | max_temperature_limit | 1 | RW | EEPROM | 0 | 99 | N - 12 | min_voltage_limit | 1 | RW | EEPROM | 0 | 250 | N - 13 | max_voltage_limit | 1 | RW | EEPROM | 0 | 250 | N - 14 | max_torque | 2 | RW | EEPROM | 0 | 1023 | N - 16 | status_return_level | 1 | RW | EEPROM | 0 | 2 | N - 17 | alarm_LED | 1 | RW | EEPROM | 0 | 127 | N - 18 | alarm_shutdown | 1 | RW | EEPROM | 0 | 127 | N - 20 | multi_turn_offset | 2 | RW | EEPROM | -26624 | 26624 | Y - 22 | resolution_dividor | 1 | RW | EEPROM | 1 | 255 | N - 24 | torque_enable | 1 | RW | RAM | 0 | 1 | N - 25 | LED | 1 | RW | RAM | 0 | 1 | N - 26 | position_d_gain | 1 | RW | RAM | 0 | 254 | N - 27 | position_i_gain | 1 | RW | RAM | 0 | 254 | N - 28 | position_p_gain | 1 | RW | RAM | 0 | 254 | N - 30 | goal_position | 2 | RW | RAM | -28672 | 28672 | Y - 32 | goal_velocity | 2 | RW | RAM | 0 | 1023 | N - 34 | goal_torque | 2 | RW | RAM | 0 | 1023 | N - 36 | present_position | 2 | R | RAM | -32768 | 32767 | Y - 38 | present_velocity | 2 | R | RAM | 0 | 2048 | N - 40 | present_load | 2 | R | RAM | 0 | 2048 | N - 42 | present_voltage | 1 | R | RAM | 50 | 250 | N - 43 | present_temperature | 1 | R | RAM | 0 | 99 | N - 44 | registered_instruction | 1 | R | RAM | 0 | 1 | N - 46 | is_moving | 1 | R | RAM | 0 | 1 | N - 47 | EEPROM_lock | 1 | RW | RAM | 0 | 1 | N - 48 | punch | 2 | RW | RAM | 0 | 1023 | N - 68 | current_consumption | 2 | RW | RAM | 0 | 4095 | N - 70 | torque_control_mode | 1 | RW | RAM | 0 | 1 | N - 71 | torque_control_goal | 2 | RW | RAM | 0 | 2047 | N - 73 | goal_acceleration | 1 | RW | RAM | 0 | 254 | N - +[device info] +model_name = MX-64 +device_type = dynamixel + +[type info] +value_of_0_radian_position = 2048 +value_of_min_radian_position = 0 +value_of_max_radian_position = 4095 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = +position_d_gain_item_name = position_d_gain +position_i_gain_item_name = position_i_gain +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = +velocity_p_gain_item_name = + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 2 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 3 | ID | 1 | RW | EEPROM | 0 | 252 | N + 4 | baudrate | 1 | RW | EEPROM | 0 | 252 | N + 5 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 6 | CW_angle_limit | 2 | RW | EEPROM | 0 | 4095 | N + 8 | CCW_angle_limit | 2 | RW | EEPROM | 0 | 4095 | N + 10 | drive_mode | 1 | RW | EEPROM | 0 | 3 | N + 11 | max_temperature_limit | 1 | RW | EEPROM | 0 | 99 | N + 12 | min_voltage_limit | 1 | RW | EEPROM | 0 | 250 | N + 13 | max_voltage_limit | 1 | RW | EEPROM | 0 | 250 | N + 14 | max_torque | 2 | RW | EEPROM | 0 | 1023 | N + 16 | status_return_level | 1 | RW | EEPROM | 0 | 2 | N + 17 | alarm_LED | 1 | RW | EEPROM | 0 | 127 | N + 18 | alarm_shutdown | 1 | RW | EEPROM | 0 | 127 | N + 20 | multi_turn_offset | 2 | RW | EEPROM | -26624 | 26624 | Y + 22 | resolution_dividor | 1 | RW | EEPROM | 1 | 255 | N + 24 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 25 | LED | 1 | RW | RAM | 0 | 1 | N + 26 | position_d_gain | 1 | RW | RAM | 0 | 254 | N + 27 | position_i_gain | 1 | RW | RAM | 0 | 254 | N + 28 | position_p_gain | 1 | RW | RAM | 0 | 254 | N + 30 | goal_position | 2 | RW | RAM | -28672 | 28672 | Y + 32 | goal_velocity | 2 | RW | RAM | 0 | 1023 | N + 34 | goal_torque | 2 | RW | RAM | 0 | 1023 | N + 36 | present_position | 2 | R | RAM | -32768 | 32767 | Y + 38 | present_velocity | 2 | R | RAM | 0 | 2048 | N + 40 | present_load | 2 | R | RAM | 0 | 2048 | N + 42 | present_voltage | 1 | R | RAM | 50 | 250 | N + 43 | present_temperature | 1 | R | RAM | 0 | 99 | N + 44 | registered_instruction | 1 | R | RAM | 0 | 1 | N + 46 | is_moving | 1 | R | RAM | 0 | 1 | N + 47 | EEPROM_lock | 1 | RW | RAM | 0 | 1 | N + 48 | punch | 2 | RW | RAM | 0 | 1023 | N + 68 | current_consumption | 2 | RW | RAM | 0 | 4095 | N + 70 | torque_control_mode | 1 | RW | RAM | 0 | 1 | N + 71 | torque_control_goal | 2 | RW | RAM | 0 | 2047 | N + 73 | goal_acceleration | 1 | RW | RAM | 0 | 254 | N + diff --git a/robotis_device/devices/dynamixel/XM-430-W210.device b/robotis_device/devices/dynamixel/XM-430-W210.device old mode 100644 new mode 100755 index f42ae57..df823c8 --- a/robotis_device/devices/dynamixel/XM-430-W210.device +++ b/robotis_device/devices/dynamixel/XM-430-W210.device @@ -1,81 +1,81 @@ -[device info] -model_name = XM-430-W210 -device_type = dynamixel - -[type info] -torque_to_current_value_ratio = 235.53647082 -velocity_to_value_ratio = 41.707853 - -value_of_0_radian_position = 2048 -value_of_min_radian_position = 0 -value_of_max_radian_position = 4095 -min_radian = -3.14159265 -max_radian = 3.14159265 - -torque_enable_item_name = torque_enable -present_position_item_name = present_position -present_velocity_item_name = present_velocity -present_current_item_name = present_current -goal_position_item_name = goal_position -goal_velocity_item_name = goal_velocity -goal_current_item_name = goal_current -position_d_gain_item_name = position_d_gain -position_i_gain_item_name = position_i_gain -position_p_gain_item_name = position_p_gain -velocity_d_gain_item_name = -velocity_i_gain_item_name = velocity_i_gain -velocity_p_gain_item_name = velocity_p_gain - -[control table] -# addr | item name | length | access | memory | min value | max value | signed - 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N - 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N - 7 | ID | 1 | RW | EEPROM | 0 | 252 | N - 8 | baudrate | 1 | RW | EEPROM | 0 | 7 | N - 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N - 11 | operating_mode | 1 | RW | EEPROM | 0 | 16 | N - 20 | homing_offset | 4 | RW | EEPROM | -1044479 | 1044479 | Y - 24 | moving_threshold | 4 | RW | EEPROM | 0 | 1023 | N - 31 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N - 32 | max_voltage_limit | 2 | RW | EEPROM | 95 | 160 | N - 34 | min_voltage_limit | 2 | RW | EEPROM | 95 | 160 | N - 36 | pwm_limit | 2 | RW | EEPROM | 0 | 885 | N - 38 | current_limit | 2 | RW | EEPROM | 0 | 1193 | N - 40 | acceleration_limit | 4 | RW | EEPROM | 0 | 32767 | N - 44 | velocity_limit | 4 | RW | EEPROM | 0 | 1023 | N - 48 | max_position_limit | 4 | RW | EEPROM | 0 | 4095 | N - 52 | min_position_limit | 4 | RW | EEPROM | 0 | 4095 | N - 63 | shutdown | 1 | RW | EEPROM | 0 | 255 | N - 64 | torque_enable | 1 | RW | RAM | 0 | 1 | N - 65 | LED | 1 | RW | RAM | 0 | 1 | N - 68 | status_return_level | 1 | RW | RAM | 0 | 2 | N - 69 | registered_instruction | 1 | R | RAM | 0 | 1 | N - 70 | hardware_error_status | 1 | R | RAM | 0 | 255 | N - 76 | velocity_i_gain | 2 | RW | RAM | 0 | 32767 | N - 78 | velocity_p_gain | 2 | RW | RAM | 0 | 32767 | N - 80 | position_d_gain | 2 | RW | RAM | 0 | 32767 | N - 82 | position_i_gain | 2 | RW | RAM | 0 | 32767 | N - 84 | position_p_gain | 2 | RW | RAM | 0 | 32767 | N - 88 | feedforward_2nd_gain | 2 | RW | RAM | 0 | 32767 | N - 90 | feedforward_1st_gain | 2 | RW | RAM | 0 | 32767 | N - 100 | goal_pwm | 2 | RW | RAM | 0 | 885 | N - 102 | goal_current | 2 | RW | RAM | 0 | 1193 | N - 104 | goal_velocity | 4 | RW | RAM | 0 | 1023 | N - 108 | profile_acceleration | 4 | RW | RAM | 0 | 32767 | N - 112 | profile_velocity | 4 | RW | RAM | 0 | 1023 | N - 116 | goal_position | 4 | RW | RAM | -1048575 | 1048575 | Y - 120 | realtime_tick | 2 | R | RAM | 0 | 32767 | N - 122 | moving | 1 | R | RAM | 0 | 1 | N - 123 | moving_status | 1 | R | RAM | 0 | 255 | N - 124 | present_pwm | 2 | R | RAM | 0 | 885 | N - 126 | present_current | 2 | R | RAM | 0 | 1193 | N - 128 | present_velocity | 4 | R | RAM | 0 | 1023 | N - 132 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y - 136 | velocity_trajectory | 4 | R | RAM | 0 | 1023 | N - 140 | position_trajectory | 4 | R | RAM | 0 | 4095 | N - 144 | present_input_voltage | 2 | R | RAM | 95 | 160 | N - 146 | present_temperature | 1 | R | RAM | 0 | 100 | N - 168 | indirect_address_1 | 2 | RW | RAM | 0 | 65535 | N - 224 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N - 578 | indirect_address_29 | 2 | RW | RAM | 0 | 65535 | N - 634 | indirect_data_29 | 1 | RW | RAM | 0 | 255 | N +[device info] +model_name = XM-430-W210 +device_type = dynamixel + +[type info] +torque_to_current_value_ratio = 235.53647082 +velocity_to_value_ratio = 41.707853 + +value_of_0_radian_position = 2048 +value_of_min_radian_position = 0 +value_of_max_radian_position = 4095 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_current +position_d_gain_item_name = position_d_gain +position_i_gain_item_name = position_i_gain +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 7 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 16 | N + 20 | homing_offset | 4 | RW | EEPROM | -1044479 | 1044479 | Y + 24 | moving_threshold | 4 | RW | EEPROM | 0 | 1023 | N + 31 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 32 | max_voltage_limit | 2 | RW | EEPROM | 95 | 160 | N + 34 | min_voltage_limit | 2 | RW | EEPROM | 95 | 160 | N + 36 | pwm_limit | 2 | RW | EEPROM | 0 | 885 | N + 38 | current_limit | 2 | RW | EEPROM | 0 | 1193 | N + 40 | acceleration_limit | 4 | RW | EEPROM | 0 | 32767 | N + 44 | velocity_limit | 4 | RW | EEPROM | 0 | 1023 | N + 48 | max_position_limit | 4 | RW | EEPROM | 0 | 4095 | N + 52 | min_position_limit | 4 | RW | EEPROM | 0 | 4095 | N + 63 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 64 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 65 | LED | 1 | RW | RAM | 0 | 1 | N + 68 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 69 | registered_instruction | 1 | R | RAM | 0 | 1 | N + 70 | hardware_error_status | 1 | R | RAM | 0 | 255 | N + 76 | velocity_i_gain | 2 | RW | RAM | 0 | 32767 | N + 78 | velocity_p_gain | 2 | RW | RAM | 0 | 32767 | N + 80 | position_d_gain | 2 | RW | RAM | 0 | 32767 | N + 82 | position_i_gain | 2 | RW | RAM | 0 | 32767 | N + 84 | position_p_gain | 2 | RW | RAM | 0 | 32767 | N + 88 | feedforward_2nd_gain | 2 | RW | RAM | 0 | 32767 | N + 90 | feedforward_1st_gain | 2 | RW | RAM | 0 | 32767 | N + 100 | goal_pwm | 2 | RW | RAM | 0 | 885 | N + 102 | goal_current | 2 | RW | RAM | 0 | 1193 | N + 104 | goal_velocity | 4 | RW | RAM | 0 | 1023 | N + 108 | profile_acceleration | 4 | RW | RAM | 0 | 32767 | N + 112 | profile_velocity | 4 | RW | RAM | 0 | 1023 | N + 116 | goal_position | 4 | RW | RAM | -1048575 | 1048575 | Y + 120 | realtime_tick | 2 | R | RAM | 0 | 32767 | N + 122 | moving | 1 | R | RAM | 0 | 1 | N + 123 | moving_status | 1 | R | RAM | 0 | 255 | N + 124 | present_pwm | 2 | R | RAM | 0 | 885 | N + 126 | present_current | 2 | R | RAM | 0 | 1193 | N + 128 | present_velocity | 4 | R | RAM | 0 | 1023 | N + 132 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 136 | velocity_trajectory | 4 | R | RAM | 0 | 1023 | N + 140 | position_trajectory | 4 | R | RAM | 0 | 4095 | N + 144 | present_input_voltage | 2 | R | RAM | 95 | 160 | N + 146 | present_temperature | 1 | R | RAM | 0 | 100 | N + 168 | indirect_address_1 | 2 | RW | RAM | 0 | 65535 | N + 224 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 578 | indirect_address_29 | 2 | RW | RAM | 0 | 65535 | N + 634 | indirect_data_29 | 1 | RW | RAM | 0 | 255 | N diff --git a/robotis_device/devices/dynamixel/XM-430-W350.device b/robotis_device/devices/dynamixel/XM-430-W350.device old mode 100644 new mode 100755 index ee14157..ce6e895 --- a/robotis_device/devices/dynamixel/XM-430-W350.device +++ b/robotis_device/devices/dynamixel/XM-430-W350.device @@ -1,81 +1,81 @@ -[device info] -model_name = XM-430-W350 -device_type = dynamixel - -[type info] -torque_to_current_value_ratio = 149.795386991 -velocity_to_value_ratio = 41.707853 - -value_of_0_radian_position = 2048 -value_of_min_radian_position = 0 -value_of_max_radian_position = 4095 -min_radian = -3.14159265 -max_radian = 3.14159265 - -torque_enable_item_name = torque_enable -present_position_item_name = present_position -present_velocity_item_name = present_velocity -present_current_item_name = present_current -goal_position_item_name = goal_position -goal_velocity_item_name = goal_velocity -goal_current_item_name = goal_current -position_d_gain_item_name = position_d_gain -position_i_gain_item_name = position_i_gain -position_p_gain_item_name = position_p_gain -velocity_d_gain_item_name = -velocity_i_gain_item_name = velocity_i_gain -velocity_p_gain_item_name = velocity_p_gain - -[control table] -# addr | item name | length | access | memory | min value | max value | signed - 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N - 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N - 7 | ID | 1 | RW | EEPROM | 0 | 252 | N - 8 | baudrate | 1 | RW | EEPROM | 0 | 7 | N - 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N - 11 | operating_mode | 1 | RW | EEPROM | 0 | 16 | N - 20 | homing_offset | 4 | RW | EEPROM | -1044479 | 1044479 | Y - 24 | moving_threshold | 4 | RW | EEPROM | 0 | 1023 | N - 31 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N - 32 | max_voltage_limit | 2 | RW | EEPROM | 95 | 160 | N - 34 | min_voltage_limit | 2 | RW | EEPROM | 95 | 160 | N - 36 | pwm_limit | 2 | RW | EEPROM | 0 | 885 | N - 38 | current_limit | 2 | RW | EEPROM | 0 | 1193 | N - 40 | acceleration_limit | 4 | RW | EEPROM | 0 | 32767 | N - 44 | velocity_limit | 4 | RW | EEPROM | 0 | 1023 | N - 48 | max_position_limit | 4 | RW | EEPROM | 0 | 4095 | N - 52 | min_position_limit | 4 | RW | EEPROM | 0 | 4095 | N - 63 | shutdown | 1 | RW | EEPROM | 0 | 255 | N - 64 | torque_enable | 1 | RW | RAM | 0 | 1 | N - 65 | LED | 1 | RW | RAM | 0 | 1 | N - 68 | status_return_level | 1 | RW | RAM | 0 | 2 | N - 69 | registered_instruction | 1 | R | RAM | 0 | 1 | N - 70 | hardware_error_status | 1 | R | RAM | 0 | 255 | N - 76 | velocity_i_gain | 2 | RW | RAM | 0 | 32767 | N - 78 | velocity_p_gain | 2 | RW | RAM | 0 | 32767 | N - 80 | position_d_gain | 2 | RW | RAM | 0 | 32767 | N - 82 | position_i_gain | 2 | RW | RAM | 0 | 32767 | N - 84 | position_p_gain | 2 | RW | RAM | 0 | 32767 | N - 88 | feedforward_2nd_gain | 2 | RW | RAM | 0 | 32767 | N - 90 | feedforward_1st_gain | 2 | RW | RAM | 0 | 32767 | N - 100 | goal_pwm | 2 | RW | RAM | 0 | 885 | N - 102 | goal_current | 2 | RW | RAM | 0 | 1193 | N - 104 | goal_velocity | 4 | RW | RAM | 0 | 1023 | N - 108 | profile_acceleration | 4 | RW | RAM | 0 | 32767 | N - 112 | profile_velocity | 4 | RW | RAM | 0 | 1023 | N - 116 | goal_position | 4 | RW | RAM | -1048575 | 1048575 | Y - 120 | realtime_tick | 2 | R | RAM | 0 | 32767 | N - 122 | moving | 1 | R | RAM | 0 | 1 | N - 123 | moving_status | 1 | R | RAM | 0 | 255 | N - 124 | present_pwm | 2 | R | RAM | 0 | 885 | N - 126 | present_current | 2 | R | RAM | 0 | 1193 | N - 128 | present_velocity | 4 | R | RAM | 0 | 1023 | N - 132 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y - 136 | velocity_trajectory | 4 | R | RAM | 0 | 1023 | N - 140 | position_trajectory | 4 | R | RAM | 0 | 4095 | N - 144 | present_input_voltage | 2 | R | RAM | 95 | 160 | N - 146 | present_temperature | 1 | R | RAM | 0 | 100 | N - 168 | indirect_address_1 | 2 | RW | RAM | 0 | 65535 | N - 224 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N - 578 | indirect_address_29 | 2 | RW | RAM | 0 | 65535 | N - 634 | indirect_data_29 | 1 | RW | RAM | 0 | 255 | N +[device info] +model_name = XM-430-W350 +device_type = dynamixel + +[type info] +torque_to_current_value_ratio = 149.795386991 +velocity_to_value_ratio = 41.707853 + +value_of_0_radian_position = 2048 +value_of_min_radian_position = 0 +value_of_max_radian_position = 4095 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_current +position_d_gain_item_name = position_d_gain +position_i_gain_item_name = position_i_gain +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 7 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 16 | N + 20 | homing_offset | 4 | RW | EEPROM | -1044479 | 1044479 | Y + 24 | moving_threshold | 4 | RW | EEPROM | 0 | 1023 | N + 31 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 32 | max_voltage_limit | 2 | RW | EEPROM | 95 | 160 | N + 34 | min_voltage_limit | 2 | RW | EEPROM | 95 | 160 | N + 36 | pwm_limit | 2 | RW | EEPROM | 0 | 885 | N + 38 | current_limit | 2 | RW | EEPROM | 0 | 1193 | N + 40 | acceleration_limit | 4 | RW | EEPROM | 0 | 32767 | N + 44 | velocity_limit | 4 | RW | EEPROM | 0 | 1023 | N + 48 | max_position_limit | 4 | RW | EEPROM | 0 | 4095 | N + 52 | min_position_limit | 4 | RW | EEPROM | 0 | 4095 | N + 63 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 64 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 65 | LED | 1 | RW | RAM | 0 | 1 | N + 68 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 69 | registered_instruction | 1 | R | RAM | 0 | 1 | N + 70 | hardware_error_status | 1 | R | RAM | 0 | 255 | N + 76 | velocity_i_gain | 2 | RW | RAM | 0 | 32767 | N + 78 | velocity_p_gain | 2 | RW | RAM | 0 | 32767 | N + 80 | position_d_gain | 2 | RW | RAM | 0 | 32767 | N + 82 | position_i_gain | 2 | RW | RAM | 0 | 32767 | N + 84 | position_p_gain | 2 | RW | RAM | 0 | 32767 | N + 88 | feedforward_2nd_gain | 2 | RW | RAM | 0 | 32767 | N + 90 | feedforward_1st_gain | 2 | RW | RAM | 0 | 32767 | N + 100 | goal_pwm | 2 | RW | RAM | 0 | 885 | N + 102 | goal_current | 2 | RW | RAM | 0 | 1193 | N + 104 | goal_velocity | 4 | RW | RAM | 0 | 1023 | N + 108 | profile_acceleration | 4 | RW | RAM | 0 | 32767 | N + 112 | profile_velocity | 4 | RW | RAM | 0 | 1023 | N + 116 | goal_position | 4 | RW | RAM | -1048575 | 1048575 | Y + 120 | realtime_tick | 2 | R | RAM | 0 | 32767 | N + 122 | moving | 1 | R | RAM | 0 | 1 | N + 123 | moving_status | 1 | R | RAM | 0 | 255 | N + 124 | present_pwm | 2 | R | RAM | 0 | 885 | N + 126 | present_current | 2 | R | RAM | 0 | 1193 | N + 128 | present_velocity | 4 | R | RAM | 0 | 1023 | N + 132 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 136 | velocity_trajectory | 4 | R | RAM | 0 | 1023 | N + 140 | position_trajectory | 4 | R | RAM | 0 | 4095 | N + 144 | present_input_voltage | 2 | R | RAM | 95 | 160 | N + 146 | present_temperature | 1 | R | RAM | 0 | 100 | N + 168 | indirect_address_1 | 2 | RW | RAM | 0 | 65535 | N + 224 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 578 | indirect_address_29 | 2 | RW | RAM | 0 | 65535 | N + 634 | indirect_data_29 | 1 | RW | RAM | 0 | 255 | N diff --git a/robotis_device/devices/dynamixel/XM-430.device b/robotis_device/devices/dynamixel/XM-430.device old mode 100644 new mode 100755 index c3a7bad..fadcfaf --- a/robotis_device/devices/dynamixel/XM-430.device +++ b/robotis_device/devices/dynamixel/XM-430.device @@ -1,80 +1,80 @@ -[device info] -model_name = XM-430 -device_type = dynamixel - -[type info] -torque_to_current_value_ratio = 149.795386991 - -value_of_0_radian_position = 2048 -value_of_min_radian_position = 0 -value_of_max_radian_position = 4095 -min_radian = -3.14159265 -max_radian = 3.14159265 - -torque_enable_item_name = torque_enable -present_position_item_name = present_position -present_velocity_item_name = present_velocity -present_current_item_name = present_current -goal_position_item_name = goal_position -goal_velocity_item_name = goal_velocity -goal_current_item_name = goal_current -position_d_gain_item_name = position_d_gain -position_i_gain_item_name = position_i_gain -position_p_gain_item_name = position_p_gain -velocity_d_gain_item_name = -velocity_i_gain_item_name = velocity_i_gain -velocity_p_gain_item_name = velocity_p_gain - -[control table] -# addr | item name | length | access | memory | min value | max value | signed - 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N - 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N - 7 | ID | 1 | RW | EEPROM | 0 | 252 | N - 8 | baudrate | 1 | RW | EEPROM | 0 | 7 | N - 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N - 11 | operating_mode | 1 | RW | EEPROM | 0 | 16 | N - 20 | homing_offset | 4 | RW | EEPROM | -1044479 | 1044479 | Y - 24 | moving_threshold | 4 | RW | EEPROM | 0 | 1023 | N - 31 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N - 32 | max_voltage_limit | 2 | RW | EEPROM | 95 | 160 | N - 34 | min_voltage_limit | 2 | RW | EEPROM | 95 | 160 | N - 36 | pwm_limit | 2 | RW | EEPROM | 0 | 885 | N - 38 | current_limit | 2 | RW | EEPROM | 0 | 1193 | N - 40 | acceleration_limit | 4 | RW | EEPROM | 0 | 32767 | N - 44 | velocity_limit | 4 | RW | EEPROM | 0 | 1023 | N - 48 | max_position_limit | 4 | RW | EEPROM | 0 | 4095 | N - 52 | min_position_limit | 4 | RW | EEPROM | 0 | 4095 | N - 63 | shutdown | 1 | RW | EEPROM | 0 | 255 | N - 64 | torque_enable | 1 | RW | RAM | 0 | 1 | N - 65 | LED | 1 | RW | RAM | 0 | 1 | N - 68 | status_return_level | 1 | RW | RAM | 0 | 2 | N - 69 | registered_instruction | 1 | R | RAM | 0 | 1 | N - 70 | hardware_error_status | 1 | R | RAM | 0 | 255 | N - 76 | velocity_i_gain | 2 | RW | RAM | 0 | 32767 | N - 78 | velocity_p_gain | 2 | RW | RAM | 0 | 32767 | N - 80 | position_d_gain | 2 | RW | RAM | 0 | 32767 | N - 82 | position_i_gain | 2 | RW | RAM | 0 | 32767 | N - 84 | position_p_gain | 2 | RW | RAM | 0 | 32767 | N - 88 | feedforward_2nd_gain | 2 | RW | RAM | 0 | 32767 | N - 90 | feedforward_1st_gain | 2 | RW | RAM | 0 | 32767 | N - 100 | goal_pwm | 2 | RW | RAM | 0 | 885 | N - 102 | goal_current | 2 | RW | RAM | 0 | 1193 | N - 104 | goal_velocity | 4 | RW | RAM | 0 | 1023 | N - 108 | profile_acceleration | 4 | RW | RAM | 0 | 32767 | N - 112 | profile_velocity | 4 | RW | RAM | 0 | 1023 | N - 116 | goal_position | 4 | RW | RAM | -1048575 | 1048575 | Y - 120 | realtime_tick | 2 | R | RAM | 0 | 32767 | N - 122 | moving | 1 | R | RAM | 0 | 1 | N - 123 | moving_status | 1 | R | RAM | 0 | 255 | N - 124 | present_pwm | 2 | R | RAM | 0 | 885 | N - 126 | present_current | 2 | R | RAM | 0 | 1193 | N - 128 | present_velocity | 4 | R | RAM | 0 | 1023 | N - 132 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y - 136 | velocity_trajectory | 4 | R | RAM | 0 | 1023 | N - 140 | position_trajectory | 4 | R | RAM | 0 | 4095 | N - 144 | present_input_voltage | 2 | R | RAM | 95 | 160 | N - 146 | present_temperature | 1 | R | RAM | 0 | 100 | N - 168 | indirect_address_1 | 2 | RW | RAM | 0 | 65535 | N - 224 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N - 578 | indirect_address_29 | 2 | RW | RAM | 0 | 65535 | N - 634 | indirect_data_29 | 1 | RW | RAM | 0 | 255 | N +[device info] +model_name = XM-430 +device_type = dynamixel + +[type info] +torque_to_current_value_ratio = 149.795386991 + +value_of_0_radian_position = 2048 +value_of_min_radian_position = 0 +value_of_max_radian_position = 4095 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_current +position_d_gain_item_name = position_d_gain +position_i_gain_item_name = position_i_gain +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 7 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 16 | N + 20 | homing_offset | 4 | RW | EEPROM | -1044479 | 1044479 | Y + 24 | moving_threshold | 4 | RW | EEPROM | 0 | 1023 | N + 31 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 32 | max_voltage_limit | 2 | RW | EEPROM | 95 | 160 | N + 34 | min_voltage_limit | 2 | RW | EEPROM | 95 | 160 | N + 36 | pwm_limit | 2 | RW | EEPROM | 0 | 885 | N + 38 | current_limit | 2 | RW | EEPROM | 0 | 1193 | N + 40 | acceleration_limit | 4 | RW | EEPROM | 0 | 32767 | N + 44 | velocity_limit | 4 | RW | EEPROM | 0 | 1023 | N + 48 | max_position_limit | 4 | RW | EEPROM | 0 | 4095 | N + 52 | min_position_limit | 4 | RW | EEPROM | 0 | 4095 | N + 63 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 64 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 65 | LED | 1 | RW | RAM | 0 | 1 | N + 68 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 69 | registered_instruction | 1 | R | RAM | 0 | 1 | N + 70 | hardware_error_status | 1 | R | RAM | 0 | 255 | N + 76 | velocity_i_gain | 2 | RW | RAM | 0 | 32767 | N + 78 | velocity_p_gain | 2 | RW | RAM | 0 | 32767 | N + 80 | position_d_gain | 2 | RW | RAM | 0 | 32767 | N + 82 | position_i_gain | 2 | RW | RAM | 0 | 32767 | N + 84 | position_p_gain | 2 | RW | RAM | 0 | 32767 | N + 88 | feedforward_2nd_gain | 2 | RW | RAM | 0 | 32767 | N + 90 | feedforward_1st_gain | 2 | RW | RAM | 0 | 32767 | N + 100 | goal_pwm | 2 | RW | RAM | 0 | 885 | N + 102 | goal_current | 2 | RW | RAM | 0 | 1193 | N + 104 | goal_velocity | 4 | RW | RAM | 0 | 1023 | N + 108 | profile_acceleration | 4 | RW | RAM | 0 | 32767 | N + 112 | profile_velocity | 4 | RW | RAM | 0 | 1023 | N + 116 | goal_position | 4 | RW | RAM | -1048575 | 1048575 | Y + 120 | realtime_tick | 2 | R | RAM | 0 | 32767 | N + 122 | moving | 1 | R | RAM | 0 | 1 | N + 123 | moving_status | 1 | R | RAM | 0 | 255 | N + 124 | present_pwm | 2 | R | RAM | 0 | 885 | N + 126 | present_current | 2 | R | RAM | 0 | 1193 | N + 128 | present_velocity | 4 | R | RAM | 0 | 1023 | N + 132 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 136 | velocity_trajectory | 4 | R | RAM | 0 | 1023 | N + 140 | position_trajectory | 4 | R | RAM | 0 | 4095 | N + 144 | present_input_voltage | 2 | R | RAM | 95 | 160 | N + 146 | present_temperature | 1 | R | RAM | 0 | 100 | N + 168 | indirect_address_1 | 2 | RW | RAM | 0 | 65535 | N + 224 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 578 | indirect_address_29 | 2 | RW | RAM | 0 | 65535 | N + 634 | indirect_data_29 | 1 | RW | RAM | 0 | 255 | N diff --git a/robotis_device/devices/sensor/CM-740.device b/robotis_device/devices/sensor/CM-740.device old mode 100644 new mode 100755 index 700d43a..5cdf45f --- a/robotis_device/devices/sensor/CM-740.device +++ b/robotis_device/devices/sensor/CM-740.device @@ -1,25 +1,25 @@ -[device info] -model_name = CM-740 -device_type = sensor - -[control table] -# addr | item name | length | access | memory | min value | max value | signed - 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N - 2 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N - 3 | ID | 1 | RW | EEPROM | 0 | 252 | N - 4 | baudrate | 1 | RW | EEPROM | 0 | 252 | N - 5 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N - 16 | status_return_level | 1 | RW | EEPROM | 0 | 2 | N - 24 | torque_enable | 1 | RW | RAM | 0 | 1 | N - 25 | LED | 1 | RW | RAM | 0 | 7 | N - 26 | LED_5 | 2 | RW | RAM | 0 | 32767 | N - 28 | LED_6 | 2 | RW | RAM | 0 | 32767 | N - 30 | button | 1 | RW | RAM | 0 | 3 | N - 38 | gyro_z | 2 | R | RAM | 0 | 1023 | N - 40 | gyro_y | 2 | R | RAM | 0 | 1023 | N - 42 | gyro_x | 2 | R | RAM | 0 | 1023 | N - 44 | acc_x | 2 | R | RAM | 0 | 1023 | N - 46 | acc_y | 2 | R | RAM | 0 | 1023 | N - 48 | acc_z | 2 | R | RAM | 0 | 1023 | N - 50 | present_voltage | 1 | R | RAM | 50 | 250 | N +[device info] +model_name = CM-740 +device_type = sensor + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 2 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 3 | ID | 1 | RW | EEPROM | 0 | 252 | N + 4 | baudrate | 1 | RW | EEPROM | 0 | 252 | N + 5 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 16 | status_return_level | 1 | RW | EEPROM | 0 | 2 | N + 24 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 25 | LED | 1 | RW | RAM | 0 | 7 | N + 26 | LED_5 | 2 | RW | RAM | 0 | 32767 | N + 28 | LED_6 | 2 | RW | RAM | 0 | 32767 | N + 30 | button | 1 | RW | RAM | 0 | 3 | N + 38 | gyro_z | 2 | R | RAM | 0 | 1023 | N + 40 | gyro_y | 2 | R | RAM | 0 | 1023 | N + 42 | gyro_x | 2 | R | RAM | 0 | 1023 | N + 44 | acc_x | 2 | R | RAM | 0 | 1023 | N + 46 | acc_y | 2 | R | RAM | 0 | 1023 | N + 48 | acc_z | 2 | R | RAM | 0 | 1023 | N + 50 | present_voltage | 1 | R | RAM | 50 | 250 | N \ No newline at end of file diff --git a/robotis_device/include/robotis_device/control_table_item.h b/robotis_device/include/robotis_device/control_table_item.h old mode 100644 new mode 100755 index 28a840a..aa09131 --- a/robotis_device/include/robotis_device/control_table_item.h +++ b/robotis_device/include/robotis_device/control_table_item.h @@ -1,84 +1,84 @@ -/******************************************************************************* - * Copyright (c) 2016, ROBOTIS CO., LTD. - * 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 ROBOTIS 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. - *******************************************************************************/ - -/* - * control_table_item.h - * - * Created on: 2015. 12. 16. - * Author: zerom - */ - -#ifndef ROBOTIS_DEVICE_CONTROL_TABLE_ITEM_H_ -#define ROBOTIS_DEVICE_CONTROL_TABLE_ITEM_H_ - - -#include - -namespace robotis_framework -{ - -enum AccessType { - Read, - ReadWrite -}; - -enum MemoryType { - EEPROM, - RAM -}; - -class ControlTableItem -{ -public: - std::string item_name_; - uint16_t address_; - AccessType access_type_; - MemoryType memory_type_; - uint8_t data_length_; - int32_t data_min_value_; - int32_t data_max_value_; - bool is_signed_; - - ControlTableItem() - : item_name_(""), - address_(0), - access_type_(Read), - memory_type_(RAM), - data_length_(0), - data_min_value_(0), - data_max_value_(0), - is_signed_(false) - { } -}; - -} - - -#endif /* ROBOTIS_DEVICE_CONTROL_TABLE_ITEM_H_ */ +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * 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 ROBOTIS 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. + *******************************************************************************/ + +/* + * control_table_item.h + * + * Created on: 2015. 12. 16. + * Author: zerom + */ + +#ifndef ROBOTIS_DEVICE_CONTROL_TABLE_ITEM_H_ +#define ROBOTIS_DEVICE_CONTROL_TABLE_ITEM_H_ + + +#include + +namespace robotis_framework +{ + +enum AccessType { + Read, + ReadWrite +}; + +enum MemoryType { + EEPROM, + RAM +}; + +class ControlTableItem +{ +public: + std::string item_name_; + uint16_t address_; + AccessType access_type_; + MemoryType memory_type_; + uint8_t data_length_; + int32_t data_min_value_; + int32_t data_max_value_; + bool is_signed_; + + ControlTableItem() + : item_name_(""), + address_(0), + access_type_(Read), + memory_type_(RAM), + data_length_(0), + data_min_value_(0), + data_max_value_(0), + is_signed_(false) + { } +}; + +} + + +#endif /* ROBOTIS_DEVICE_CONTROL_TABLE_ITEM_H_ */ diff --git a/robotis_device/include/robotis_device/device.h b/robotis_device/include/robotis_device/device.h old mode 100644 new mode 100755 index 0a5556e..a2e5a1c --- a/robotis_device/include/robotis_device/device.h +++ b/robotis_device/include/robotis_device/device.h @@ -1,68 +1,68 @@ -/******************************************************************************* - * Copyright (c) 2016, ROBOTIS CO., LTD. - * 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 ROBOTIS 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. - *******************************************************************************/ - -/* - * device.h - * - * Created on: 2016. 5. 12. - * Author: zerom - */ - -#ifndef ROBOTIS_DEVICE_DEVICE_H_ -#define ROBOTIS_DEVICE_DEVICE_H_ - - -#include -#include -#include - -#include "control_table_item.h" - -namespace robotis_framework -{ - -class Device -{ -public: - uint8_t id_; - float protocol_version_; - std::string model_name_; - std::string port_name_; - - std::map ctrl_table_; - std::vector bulk_read_items_; - - virtual ~Device() { } -}; - -} - - -#endif /* ROBOTIS_DEVICE_DEVICE_H_ */ +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * 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 ROBOTIS 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. + *******************************************************************************/ + +/* + * device.h + * + * Created on: 2016. 5. 12. + * Author: zerom + */ + +#ifndef ROBOTIS_DEVICE_DEVICE_H_ +#define ROBOTIS_DEVICE_DEVICE_H_ + + +#include +#include +#include + +#include "control_table_item.h" + +namespace robotis_framework +{ + +class Device +{ +public: + uint8_t id_; + float protocol_version_; + std::string model_name_; + std::string port_name_; + + std::map ctrl_table_; + std::vector bulk_read_items_; + + virtual ~Device() { } +}; + +} + + +#endif /* ROBOTIS_DEVICE_DEVICE_H_ */ diff --git a/robotis_device/include/robotis_device/dynamixel.h b/robotis_device/include/robotis_device/dynamixel.h old mode 100644 new mode 100755 index adff01b..f9b60f7 --- a/robotis_device/include/robotis_device/dynamixel.h +++ b/robotis_device/include/robotis_device/dynamixel.h @@ -1,97 +1,97 @@ -/******************************************************************************* - * Copyright (c) 2016, ROBOTIS CO., LTD. - * 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 ROBOTIS 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. - *******************************************************************************/ - -/* - * dynamixel.h - * - * Created on: 2015. 12. 8. - * Author: zerom - */ - -#ifndef ROBOTIS_DEVICE_DYNAMIXEL_H_ -#define ROBOTIS_DEVICE_DYNAMIXEL_H_ - - -#include -#include -#include - -#include "control_table_item.h" -#include "device.h" -#include "dynamixel_state.h" - -namespace robotis_framework -{ - -class Dynamixel : public Device -{ -public: - std::string ctrl_module_name_; - DynamixelState *dxl_state_; - - double velocity_to_value_ratio_; - double torque_to_current_value_ratio_; - - int32_t value_of_0_radian_position_; - int32_t value_of_min_radian_position_; - int32_t value_of_max_radian_position_; - double min_radian_; - double max_radian_; - - ControlTableItem *torque_enable_item_; - ControlTableItem *present_position_item_; - ControlTableItem *present_velocity_item_; - ControlTableItem *present_current_item_; - ControlTableItem *goal_position_item_; - ControlTableItem *goal_velocity_item_; - ControlTableItem *goal_current_item_; - ControlTableItem *position_p_gain_item_; - ControlTableItem *position_i_gain_item_; - ControlTableItem *position_d_gain_item_; - ControlTableItem *velocity_p_gain_item_; - ControlTableItem *velocity_i_gain_item_; - ControlTableItem *velocity_d_gain_item_; - - Dynamixel(int id, std::string model_name, float protocol_version); - - double convertValue2Radian(int32_t value); - int32_t convertRadian2Value(double radian); - - double convertValue2Velocity(int32_t value); - int32_t convertVelocity2Value(double velocity); - - double convertValue2Torque(int16_t value); - int16_t convertTorque2Value(double torque); -}; - -} - - -#endif /* ROBOTIS_DEVICE_DYNAMIXEL_H_ */ +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * 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 ROBOTIS 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. + *******************************************************************************/ + +/* + * dynamixel.h + * + * Created on: 2015. 12. 8. + * Author: zerom + */ + +#ifndef ROBOTIS_DEVICE_DYNAMIXEL_H_ +#define ROBOTIS_DEVICE_DYNAMIXEL_H_ + + +#include +#include +#include + +#include "control_table_item.h" +#include "device.h" +#include "dynamixel_state.h" + +namespace robotis_framework +{ + +class Dynamixel : public Device +{ +public: + std::string ctrl_module_name_; + DynamixelState *dxl_state_; + + double velocity_to_value_ratio_; + double torque_to_current_value_ratio_; + + int32_t value_of_0_radian_position_; + int32_t value_of_min_radian_position_; + int32_t value_of_max_radian_position_; + double min_radian_; + double max_radian_; + + ControlTableItem *torque_enable_item_; + ControlTableItem *present_position_item_; + ControlTableItem *present_velocity_item_; + ControlTableItem *present_current_item_; + ControlTableItem *goal_position_item_; + ControlTableItem *goal_velocity_item_; + ControlTableItem *goal_current_item_; + ControlTableItem *position_p_gain_item_; + ControlTableItem *position_i_gain_item_; + ControlTableItem *position_d_gain_item_; + ControlTableItem *velocity_p_gain_item_; + ControlTableItem *velocity_i_gain_item_; + ControlTableItem *velocity_d_gain_item_; + + Dynamixel(int id, std::string model_name, float protocol_version); + + double convertValue2Radian(int32_t value); + int32_t convertRadian2Value(double radian); + + double convertValue2Velocity(int32_t value); + int32_t convertVelocity2Value(double velocity); + + double convertValue2Torque(int16_t value); + int16_t convertTorque2Value(double torque); +}; + +} + + +#endif /* ROBOTIS_DEVICE_DYNAMIXEL_H_ */ diff --git a/robotis_device/include/robotis_device/dynamixel_state.h b/robotis_device/include/robotis_device/dynamixel_state.h old mode 100644 new mode 100755 index 57de9f6..1651eb2 --- a/robotis_device/include/robotis_device/dynamixel_state.h +++ b/robotis_device/include/robotis_device/dynamixel_state.h @@ -1,96 +1,96 @@ -/******************************************************************************* - * Copyright (c) 2016, ROBOTIS CO., LTD. - * 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 ROBOTIS 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. - *******************************************************************************/ - -/* - * dynamixel_state.h - * - * Created on: 2015. 12. 8. - * Author: zerom - */ - -#ifndef ROBOTIS_DEVICE_DYNAMIXEL_STATE_H_ -#define ROBOTIS_DEVICE_DYNAMIXEL_STATE_H_ - -#include - -#include "time_stamp.h" - -#define INDIRECT_DATA_1 "indirect_data_1" -#define INDIRECT_ADDRESS_1 "indirect_address_1" - -namespace robotis_framework -{ - -class DynamixelState -{ -public: - TimeStamp update_time_stamp_; - - double present_position_; - double present_velocity_; - double present_torque_; - double goal_position_; - double goal_velocity_; - double goal_torque_; - int position_p_gain_; - int position_i_gain_; - int position_d_gain_; - int velocity_p_gain_; - int velocity_i_gain_; - int velocity_d_gain_; - - std::map bulk_read_table_; - - double position_offset_; - - DynamixelState() - : update_time_stamp_(0, 0), - present_position_(0.0), - present_velocity_(0.0), - present_torque_(0.0), - goal_position_(0.0), - goal_velocity_(0.0), - goal_torque_(0.0), - position_p_gain_(0), - position_i_gain_(0), - position_d_gain_(0), - velocity_p_gain_(0), - velocity_i_gain_(0), - velocity_d_gain_(0), - position_offset_(0) - { - bulk_read_table_.clear(); - } -}; - -} - - -#endif /* ROBOTIS_DEVICE_DYNAMIXEL_STATE_H_ */ +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * 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 ROBOTIS 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. + *******************************************************************************/ + +/* + * dynamixel_state.h + * + * Created on: 2015. 12. 8. + * Author: zerom + */ + +#ifndef ROBOTIS_DEVICE_DYNAMIXEL_STATE_H_ +#define ROBOTIS_DEVICE_DYNAMIXEL_STATE_H_ + +#include + +#include "time_stamp.h" + +#define INDIRECT_DATA_1 "indirect_data_1" +#define INDIRECT_ADDRESS_1 "indirect_address_1" + +namespace robotis_framework +{ + +class DynamixelState +{ +public: + TimeStamp update_time_stamp_; + + double present_position_; + double present_velocity_; + double present_torque_; + double goal_position_; + double goal_velocity_; + double goal_torque_; + int position_p_gain_; + int position_i_gain_; + int position_d_gain_; + int velocity_p_gain_; + int velocity_i_gain_; + int velocity_d_gain_; + + std::map bulk_read_table_; + + double position_offset_; + + DynamixelState() + : update_time_stamp_(0, 0), + present_position_(0.0), + present_velocity_(0.0), + present_torque_(0.0), + goal_position_(0.0), + goal_velocity_(0.0), + goal_torque_(0.0), + position_p_gain_(0), + position_i_gain_(0), + position_d_gain_(0), + velocity_p_gain_(0), + velocity_i_gain_(0), + velocity_d_gain_(0), + position_offset_(0) + { + bulk_read_table_.clear(); + } +}; + +} + + +#endif /* ROBOTIS_DEVICE_DYNAMIXEL_STATE_H_ */ diff --git a/robotis_device/include/robotis_device/robot.h b/robotis_device/include/robotis_device/robot.h old mode 100644 new mode 100755 index 4dfe5bc..91b30f1 --- a/robotis_device/include/robotis_device/robot.h +++ b/robotis_device/include/robotis_device/robot.h @@ -1,78 +1,78 @@ -/******************************************************************************* - * Copyright (c) 2016, ROBOTIS CO., LTD. - * 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 ROBOTIS 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. - *******************************************************************************/ - -/* - * robot.h - * - * Created on: 2015. 12. 11. - * Author: zerom - */ - -#ifndef ROBOTIS_DEVICE_ROBOT_H_ -#define ROBOTIS_DEVICE_ROBOT_H_ - - -#include - -#include "sensor.h" -#include "dynamixel.h" -#include "dynamixel_sdk/port_handler.h" - -#define DYNAMIXEL "dynamixel" -#define SENSOR "sensor" - -#define SESSION_PORT_INFO "port info" -#define SESSION_DEVICE_INFO "device info" - -#define SESSION_TYPE_INFO "type info" -#define SESSION_CONTROL_TABLE "control table" - -namespace robotis_framework -{ - -class Robot -{ -public: - std::map ports_; // string: port name - std::map port_default_device_; // port name, default device name - - std::map dxls_; // string: joint name - std::map sensors_; // string: sensor name - - Robot(std::string robot_file_path, std::string dev_desc_dir_path); - - Sensor *getSensor(std::string path, int id, std::string port, float protocol_version); - Dynamixel *getDynamixel(std::string path, int id, std::string port, float protocol_version); -}; - -} - - -#endif /* ROBOTIS_DEVICE_ROBOT_H_ */ +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * 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 ROBOTIS 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. + *******************************************************************************/ + +/* + * robot.h + * + * Created on: 2015. 12. 11. + * Author: zerom + */ + +#ifndef ROBOTIS_DEVICE_ROBOT_H_ +#define ROBOTIS_DEVICE_ROBOT_H_ + + +#include + +#include "sensor.h" +#include "dynamixel.h" +#include "dynamixel_sdk/port_handler.h" + +#define DYNAMIXEL "dynamixel" +#define SENSOR "sensor" + +#define SESSION_PORT_INFO "port info" +#define SESSION_DEVICE_INFO "device info" + +#define SESSION_TYPE_INFO "type info" +#define SESSION_CONTROL_TABLE "control table" + +namespace robotis_framework +{ + +class Robot +{ +public: + std::map ports_; // string: port name + std::map port_default_device_; // port name, default device name + + std::map dxls_; // string: joint name + std::map sensors_; // string: sensor name + + Robot(std::string robot_file_path, std::string dev_desc_dir_path); + + Sensor *getSensor(std::string path, int id, std::string port, float protocol_version); + Dynamixel *getDynamixel(std::string path, int id, std::string port, float protocol_version); +}; + +} + + +#endif /* ROBOTIS_DEVICE_ROBOT_H_ */ diff --git a/robotis_device/include/robotis_device/sensor.h b/robotis_device/include/robotis_device/sensor.h old mode 100644 new mode 100755 index 2d1b757..dda755f --- a/robotis_device/include/robotis_device/sensor.h +++ b/robotis_device/include/robotis_device/sensor.h @@ -1,63 +1,63 @@ -/******************************************************************************* - * Copyright (c) 2016, ROBOTIS CO., LTD. - * 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 ROBOTIS 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. - *******************************************************************************/ - -/* - * sensor.h - * - * Created on: 2015. 12. 16. - * Author: zerom - */ - -#ifndef ROBOTIS_DEVICE_SENSOR_H_ -#define ROBOTIS_DEVICE_SENSOR_H_ - -#include -#include -#include - -#include "device.h" -#include "sensor_state.h" -#include "control_table_item.h" - -namespace robotis_framework -{ - -class Sensor : public Device -{ -public: - SensorState *sensor_state_; - - Sensor(int id, std::string model_name, float protocol_version); -}; - -} - - -#endif /* ROBOTIS_DEVICE_SENSOR_H_ */ +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * 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 ROBOTIS 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. + *******************************************************************************/ + +/* + * sensor.h + * + * Created on: 2015. 12. 16. + * Author: zerom + */ + +#ifndef ROBOTIS_DEVICE_SENSOR_H_ +#define ROBOTIS_DEVICE_SENSOR_H_ + +#include +#include +#include + +#include "device.h" +#include "sensor_state.h" +#include "control_table_item.h" + +namespace robotis_framework +{ + +class Sensor : public Device +{ +public: + SensorState *sensor_state_; + + Sensor(int id, std::string model_name, float protocol_version); +}; + +} + + +#endif /* ROBOTIS_DEVICE_SENSOR_H_ */ diff --git a/robotis_device/include/robotis_device/sensor_state.h b/robotis_device/include/robotis_device/sensor_state.h old mode 100644 new mode 100755 index f40afa4..01ec952 --- a/robotis_device/include/robotis_device/sensor_state.h +++ b/robotis_device/include/robotis_device/sensor_state.h @@ -1,64 +1,64 @@ -/******************************************************************************* - * Copyright (c) 2016, ROBOTIS CO., LTD. - * 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 ROBOTIS 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. - *******************************************************************************/ - -/* - * sensor_state.h - * - * Created on: 2016. 5. 16. - * Author: zerom - */ - -#ifndef ROBOTIS_DEVICE_SENSOR_STATE_H_ -#define ROBOTIS_DEVICE_SENSOR_STATE_H_ - - -#include "time_stamp.h" - -namespace robotis_framework -{ - -class SensorState -{ -public: - TimeStamp update_time_stamp_; - - std::map bulk_read_table_; - - SensorState() - : update_time_stamp_(0, 0) - { - bulk_read_table_.clear(); - } -}; - -} - - -#endif /* ROBOTIS_DEVICE_SENSOR_STATE_H_ */ +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * 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 ROBOTIS 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. + *******************************************************************************/ + +/* + * sensor_state.h + * + * Created on: 2016. 5. 16. + * Author: zerom + */ + +#ifndef ROBOTIS_DEVICE_SENSOR_STATE_H_ +#define ROBOTIS_DEVICE_SENSOR_STATE_H_ + + +#include "time_stamp.h" + +namespace robotis_framework +{ + +class SensorState +{ +public: + TimeStamp update_time_stamp_; + + std::map bulk_read_table_; + + SensorState() + : update_time_stamp_(0, 0) + { + bulk_read_table_.clear(); + } +}; + +} + + +#endif /* ROBOTIS_DEVICE_SENSOR_STATE_H_ */ diff --git a/robotis_device/include/robotis_device/time_stamp.h b/robotis_device/include/robotis_device/time_stamp.h old mode 100644 new mode 100755 index b0de1e5..1dc7510 --- a/robotis_device/include/robotis_device/time_stamp.h +++ b/robotis_device/include/robotis_device/time_stamp.h @@ -1,59 +1,59 @@ -/******************************************************************************* - * Copyright (c) 2016, ROBOTIS CO., LTD. - * 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 ROBOTIS 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. - *******************************************************************************/ - -/* - * time_stamp.h - * - * Created on: 2016. 5. 16. - * Author: zerom - */ - -#ifndef ROBOTIS_DEVICE_TIME_STAMP_H_ -#define ROBOTIS_DEVICE_TIME_STAMP_H_ - - -namespace robotis_framework -{ - -class TimeStamp { -public: - long sec_; - long nsec_; - - TimeStamp(long sec, long nsec) - : sec_(sec), - nsec_(nsec) - { } -}; - -} - - -#endif /* ROBOTIS_DEVICE_TIME_STAMP_H_ */ +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * 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 ROBOTIS 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. + *******************************************************************************/ + +/* + * time_stamp.h + * + * Created on: 2016. 5. 16. + * Author: zerom + */ + +#ifndef ROBOTIS_DEVICE_TIME_STAMP_H_ +#define ROBOTIS_DEVICE_TIME_STAMP_H_ + + +namespace robotis_framework +{ + +class TimeStamp { +public: + long sec_; + long nsec_; + + TimeStamp(long sec, long nsec) + : sec_(sec), + nsec_(nsec) + { } +}; + +} + + +#endif /* ROBOTIS_DEVICE_TIME_STAMP_H_ */ diff --git a/robotis_device/package.xml b/robotis_device/package.xml old mode 100644 new mode 100755 index e5caa92..1b0d2f3 --- a/robotis_device/package.xml +++ b/robotis_device/package.xml @@ -1,23 +1,23 @@ - - - robotis_device - 0.1.1 - - The package that manages device information of ROBOTIS robots. - This package is used when reading device information with the robot information file - from the robotis_controller package. - - BSD - Zerom - Pyo - https://github.com/ROBOTIS-GIT/ROBOTIS-Framework/issues - https://github.com/ROBOTIS-GIT/ROBOTIS-Framework - http://wiki.ros.org/robotis_device - catkin - roscpp - rospy - dynamixel_sdk - roscpp - rospy - dynamixel_sdk - + + + robotis_device + 0.1.1 + + The package that manages device information of ROBOTIS robots. + This package is used when reading device information with the robot information file + from the robotis_controller package. + + BSD + Zerom + Pyo + https://github.com/ROBOTIS-GIT/ROBOTIS-Framework/issues + https://github.com/ROBOTIS-GIT/ROBOTIS-Framework + http://wiki.ros.org/robotis_device + catkin + roscpp + rospy + dynamixel_sdk + roscpp + rospy + dynamixel_sdk + diff --git a/robotis_device/src/robotis_device/dynamixel.cpp b/robotis_device/src/robotis_device/dynamixel.cpp old mode 100644 new mode 100755 diff --git a/robotis_device/src/robotis_device/robot.cpp b/robotis_device/src/robotis_device/robot.cpp old mode 100644 new mode 100755 index 3fc9b8b..fcc8fa0 --- a/robotis_device/src/robotis_device/robot.cpp +++ b/robotis_device/src/robotis_device/robot.cpp @@ -1,487 +1,487 @@ -/******************************************************************************* - * Copyright (c) 2016, ROBOTIS CO., LTD. - * 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 ROBOTIS 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. - *******************************************************************************/ - -/* - * robot.cpp - * - * Created on: 2015. 12. 11. - * Author: zerom - */ - -#include -#include -#include - -#include "robotis_device/robot.h" - -using namespace robotis_framework; - -static inline std::string <rim(std::string &s) -{ - s.erase(s.begin(), std::find_if(s.begin(), s.end(), std::not1(std::ptr_fun(std::isspace)))); - return s; -} -static inline std::string &rtrim(std::string &s) -{ - s.erase(std::find_if(s.rbegin(), s.rend(), std::not1(std::ptr_fun(std::isspace))).base(), s.end()); - return s; -} -static inline std::string &trim(std::string &s) -{ - return ltrim(rtrim(s)); -} - -static inline std::vector split(const std::string &text, char sep) -{ - std::vector tokens; - std::size_t start = 0, end = 0; - - while ((end = text.find(sep, start)) != (std::string::npos)) - { - tokens.push_back(text.substr(start, end - start)); - trim(tokens.back()); - start = end + 1; - } - tokens.push_back(text.substr(start)); - trim(tokens.back()); - - return tokens; -} - -Robot::Robot(std::string robot_file_path, std::string dev_desc_dir_path) -{ - if (dev_desc_dir_path.compare(dev_desc_dir_path.size() - 1, 1, "/") != 0) - dev_desc_dir_path += "/"; - - std::ifstream file(robot_file_path.c_str()); - if (file.is_open()) - { - std::string session = ""; - std::string input_str; - while (!file.eof()) - { - std::getline(file, input_str); - - // remove comment ( # ) - std::size_t pos = input_str.find("#"); - if (pos != std::string::npos) - input_str = input_str.substr(0, pos); - - // trim - input_str = trim(input_str); - - // find session - if (!input_str.compare(0, 1, "[") && !input_str.compare(input_str.size() - 1, 1, "]")) - { - input_str = input_str.substr(1, input_str.size() - 2); - std::transform(input_str.begin(), input_str.end(), input_str.begin(), ::tolower); - session = trim(input_str); - continue; - } - - if (session == SESSION_PORT_INFO) - { - std::vector tokens = split(input_str, '|'); - if (tokens.size() != 3) - continue; - - std::cout << tokens[0] << " added. (baudrate: " << tokens[1] << ")" << std::endl; - - ports_[tokens[0]] = dynamixel::PortHandler::getPortHandler(tokens[0].c_str()); - ports_[tokens[0]]->setBaudRate(std::atoi(tokens[1].c_str())); - port_default_device_[tokens[0]] = tokens[2]; - } - else if (session == SESSION_DEVICE_INFO) - { - std::vector tokens = split(input_str, '|'); - if (tokens.size() != 7) - continue; - - if (tokens[0] == DYNAMIXEL) - { - std::string file_path = dev_desc_dir_path + tokens[0] + "/" + tokens[3] + ".device"; - int id = std::atoi(tokens[2].c_str()); - std::string port = tokens[1]; - float protocol = std::atof(tokens[4].c_str()); - std::string dev_name = tokens[5]; - - dxls_[dev_name] = getDynamixel(file_path, id, port, protocol); - - Dynamixel *dxl = dxls_[dev_name]; - std::vector sub_tokens = split(tokens[6], ','); - if (sub_tokens.size() > 0) - { - std::map::iterator indirect_it = dxl->ctrl_table_.find(INDIRECT_ADDRESS_1); - if (indirect_it != dxl->ctrl_table_.end()) // INDIRECT_ADDRESS_1 exist - { - uint16_t indirect_data_addr = dxl->ctrl_table_[INDIRECT_DATA_1]->address_; - for (int _i = 0; _i < sub_tokens.size(); _i++) - { - dxl->bulk_read_items_.push_back(new ControlTableItem()); - ControlTableItem *dest_item = dxl->bulk_read_items_[_i]; - ControlTableItem *src_item = dxl->ctrl_table_[sub_tokens[_i]]; - - dest_item->item_name_ = src_item->item_name_; - dest_item->address_ = indirect_data_addr; - dest_item->access_type_ = src_item->access_type_; - dest_item->memory_type_ = src_item->memory_type_; - dest_item->data_length_ = src_item->data_length_; - dest_item->data_min_value_ = src_item->data_min_value_; - dest_item->data_max_value_ = src_item->data_max_value_; - dest_item->is_signed_ = src_item->is_signed_; - - indirect_data_addr += dest_item->data_length_; - } - } - else // INDIRECT_ADDRESS_1 not exist - { - for (int i = 0; i < sub_tokens.size(); i++) - { - if (dxl->ctrl_table_[sub_tokens[i]] != NULL) - dxl->bulk_read_items_.push_back(dxl->ctrl_table_[sub_tokens[i]]); - } - } - } - } - else if (tokens[0] == SENSOR) - { - std::string file_path = dev_desc_dir_path + tokens[0] + "/" + tokens[3] + ".device"; - int id = std::atoi(tokens[2].c_str()); - std::string port = tokens[1]; - float protocol = std::atof(tokens[4].c_str()); - std::string dev_name = tokens[5]; - - sensors_[dev_name] = getSensor(file_path, id, port, protocol); - - Sensor *sensor = sensors_[dev_name]; - std::vector sub_tokens = split(tokens[6], ','); - if (sub_tokens.size() > 0) - { - std::map::iterator indirect_it = sensor->ctrl_table_.find(INDIRECT_ADDRESS_1); - if (indirect_it != sensor->ctrl_table_.end()) // INDIRECT_ADDRESS_1 exist - { - uint16_t indirect_data_addr = sensor->ctrl_table_[INDIRECT_DATA_1]->address_; - for (int i = 0; i < sub_tokens.size(); i++) - { - sensor->bulk_read_items_.push_back(new ControlTableItem()); - ControlTableItem *dest_item = sensor->bulk_read_items_[i]; - ControlTableItem *src_item = sensor->ctrl_table_[sub_tokens[i]]; - - dest_item->item_name_ = src_item->item_name_; - dest_item->address_ = indirect_data_addr; - dest_item->access_type_ = src_item->access_type_; - dest_item->memory_type_ = src_item->memory_type_; - dest_item->data_length_ = src_item->data_length_; - dest_item->data_min_value_ = src_item->data_min_value_; - dest_item->data_max_value_ = src_item->data_max_value_; - dest_item->is_signed_ = src_item->is_signed_; - - indirect_data_addr += dest_item->data_length_; - } - } - else // INDIRECT_ADDRESS_1 exist - { - for (int i = 0; i < sub_tokens.size(); i++) - sensor->bulk_read_items_.push_back(sensor->ctrl_table_[sub_tokens[i]]); - } - } - } - } - } - file.close(); - } - else - { - std::cout << "Unable to open file : " + robot_file_path << std::endl; - } -} - -Sensor *Robot::getSensor(std::string path, int id, std::string port, float protocol_version) -{ - Sensor *sensor; - - // load file model_name.device - std::ifstream file(path.c_str()); - if (file.is_open()) - { - std::string session = ""; - std::string input_str; - - while (!file.eof()) - { - std::getline(file, input_str); - - // remove comment ( # ) - std::size_t pos = input_str.find("#"); - if (pos != std::string::npos) - input_str = input_str.substr(0, pos); - - // trim - input_str = trim(input_str); - if (input_str == "") - continue; - - // find _session - if (!input_str.compare(0, 1, "[") && !input_str.compare(input_str.size() - 1, 1, "]")) - { - input_str = input_str.substr(1, input_str.size() - 2); - std::transform(input_str.begin(), input_str.end(), input_str.begin(), ::tolower); - session = trim(input_str); - continue; - } - - if (session == SESSION_DEVICE_INFO) - { - std::vector tokens = split(input_str, '='); - if (tokens.size() != 2) - continue; - - if (tokens[0] == "model_name") - sensor = new Sensor(id, tokens[1], protocol_version); - } - else if (session == SESSION_CONTROL_TABLE) - { - std::vector tokens = split(input_str, '|'); - if (tokens.size() != 8) - continue; - - ControlTableItem *item = new ControlTableItem(); - item->item_name_ = tokens[1]; - item->address_ = std::atoi(tokens[0].c_str()); - item->data_length_ = std::atoi(tokens[2].c_str()); - - if (tokens[3] == "R") - item->access_type_ = Read; - else if (tokens[3] == "RW") - item->access_type_ = ReadWrite; - - if (tokens[4] == "EEPROM") - item->memory_type_ = EEPROM; - else if (tokens[4] == "RAM") - item->memory_type_ = RAM; - - item->data_min_value_ = std::atol(tokens[5].c_str()); - item->data_max_value_ = std::atol(tokens[6].c_str()); - - if (tokens[7] == "Y") - item->is_signed_ = true; - else if (tokens[7] == "N") - item->is_signed_ = false; - sensor->ctrl_table_[tokens[1]] = item; - } - } - sensor->port_name_ = port; - - fprintf(stderr, "(%s) [ID:%3d] %14s added. \n", port.c_str(), sensor->id_, sensor->model_name_.c_str()); - //std::cout << "[ID:" << (int)(_sensor->id) << "] " << _sensor->model_name << " added. (" << port << ")" << std::endl; - file.close(); - } - else - std::cout << "Unable to open file : " + path << std::endl; - - return sensor; -} - -Dynamixel *Robot::getDynamixel(std::string path, int id, std::string port, float protocol_version) -{ - Dynamixel *dxl; - - // load file model_name.device - std::ifstream file(path.c_str()); - if (file.is_open()) - { - std::string session = ""; - std::string input_str; - - std::string torque_enable_item_name = ""; - std::string present_position_item_name = ""; - std::string present_velocity_item_name = ""; - std::string present_current_item_name = ""; - std::string goal_position_item_name = ""; - std::string goal_velocity_item_name = ""; - std::string goal_current_item_name = ""; - std::string position_d_gain_item_name = ""; - std::string position_i_gain_item_name = ""; - std::string position_p_gain_item_name = ""; - std::string velocity_d_gain_item_name = ""; - std::string velocity_i_gain_item_name = ""; - std::string velocity_p_gain_item_name = ""; - - while (!file.eof()) - { - std::getline(file, input_str); - - // remove comment ( # ) - std::size_t pos = input_str.find("#"); - if (pos != std::string::npos) - input_str = input_str.substr(0, pos); - - // trim - input_str = trim(input_str); - if (input_str == "") - continue; - - // find session - if (!input_str.compare(0, 1, "[") && !input_str.compare(input_str.size() - 1, 1, "]")) - { - input_str = input_str.substr(1, input_str.size() - 2); - std::transform(input_str.begin(), input_str.end(), input_str.begin(), ::tolower); - session = trim(input_str); - continue; - } - - if (session == SESSION_DEVICE_INFO) - { - std::vector tokens = split(input_str, '='); - if (tokens.size() != 2) - continue; - - if (tokens[0] == "model_name") - dxl = new Dynamixel(id, tokens[1], protocol_version); - } - else if (session == SESSION_TYPE_INFO) - { - std::vector tokens = split(input_str, '='); - if (tokens.size() != 2) - continue; - - if (tokens[0] == "torque_to_current_value_ratio") - dxl->torque_to_current_value_ratio_ = std::atof(tokens[1].c_str()); - else if (tokens[0] == "velocity_to_value_ratio") - dxl->velocity_to_value_ratio_ = std::atof(tokens[1].c_str()); - - else if (tokens[0] == "value_of_0_radian_position") - dxl->value_of_0_radian_position_ = std::atoi(tokens[1].c_str()); - else if (tokens[0] == "value_of_min_radian_position") - dxl->value_of_min_radian_position_ = std::atoi(tokens[1].c_str()); - else if (tokens[0] == "value_of_max_radian_position") - dxl->value_of_max_radian_position_ = std::atoi(tokens[1].c_str()); - else if (tokens[0] == "min_radian") - dxl->min_radian_ = std::atof(tokens[1].c_str()); - else if (tokens[0] == "max_radian") - dxl->max_radian_ = std::atof(tokens[1].c_str()); - - else if (tokens[0] == "torque_enable_item_name") - torque_enable_item_name = tokens[1]; - else if (tokens[0] == "present_position_item_name") - present_position_item_name = tokens[1]; - else if (tokens[0] == "present_velocity_item_name") - present_velocity_item_name = tokens[1]; - else if (tokens[0] == "present_current_item_name") - present_current_item_name = tokens[1]; - else if (tokens[0] == "goal_position_item_name") - goal_position_item_name = tokens[1]; - else if (tokens[0] == "goal_velocity_item_name") - goal_velocity_item_name = tokens[1]; - else if (tokens[0] == "goal_current_item_name") - goal_current_item_name = tokens[1]; - else if (tokens[0] == "position_d_gain_item_name") - position_d_gain_item_name = tokens[1]; - else if (tokens[0] == "position_i_gain_item_name") - position_i_gain_item_name = tokens[1]; - else if (tokens[0] == "position_p_gain_item_name") - position_p_gain_item_name = tokens[1]; - else if (tokens[0] == "velocity_d_gain_item_name") - velocity_d_gain_item_name = tokens[1]; - else if (tokens[0] == "velocity_i_gain_item_name") - velocity_i_gain_item_name = tokens[1]; - else if (tokens[0] == "velocity_p_gain_item_name") - velocity_p_gain_item_name = tokens[1]; - } - else if (session == SESSION_CONTROL_TABLE) - { - std::vector tokens = split(input_str, '|'); - if (tokens.size() != 8) - continue; - - ControlTableItem *item = new ControlTableItem(); - item->item_name_ = tokens[1]; - item->address_ = std::atoi(tokens[0].c_str()); - item->data_length_ = std::atoi(tokens[2].c_str()); - - if (tokens[3] == "R") - item->access_type_ = Read; - else if (tokens[3] == "RW") - item->access_type_ = ReadWrite; - - if (tokens[4] == "EEPROM") - item->memory_type_ = EEPROM; - else if (tokens[4] == "RAM") - item->memory_type_ = RAM; - - item->data_min_value_ = std::atol(tokens[5].c_str()); - item->data_max_value_ = std::atol(tokens[6].c_str()); - - if (tokens[7] == "Y") - item->is_signed_ = true; - else if (tokens[7] == "N") - item->is_signed_ = false; - dxl->ctrl_table_[tokens[1]] = item; - } - } - dxl->port_name_ = port; - - if (dxl->ctrl_table_[torque_enable_item_name] != NULL) - dxl->torque_enable_item_ = dxl->ctrl_table_[torque_enable_item_name]; - if (dxl->ctrl_table_[present_position_item_name] != NULL) - dxl->present_position_item_ = dxl->ctrl_table_[present_position_item_name]; - if (dxl->ctrl_table_[present_velocity_item_name] != NULL) - dxl->present_velocity_item_ = dxl->ctrl_table_[present_velocity_item_name]; - if (dxl->ctrl_table_[present_current_item_name] != NULL) - dxl->present_current_item_ = dxl->ctrl_table_[present_current_item_name]; - if (dxl->ctrl_table_[goal_position_item_name] != NULL) - dxl->goal_position_item_ = dxl->ctrl_table_[goal_position_item_name]; - if (dxl->ctrl_table_[goal_velocity_item_name] != NULL) - dxl->goal_velocity_item_ = dxl->ctrl_table_[goal_velocity_item_name]; - if (dxl->ctrl_table_[goal_current_item_name] != NULL) - dxl->goal_current_item_ = dxl->ctrl_table_[goal_current_item_name]; - if (dxl->ctrl_table_[position_d_gain_item_name] != NULL) - dxl->position_d_gain_item_ = dxl->ctrl_table_[position_d_gain_item_name]; - if (dxl->ctrl_table_[position_i_gain_item_name] != NULL) - dxl->position_i_gain_item_ = dxl->ctrl_table_[position_i_gain_item_name]; - if (dxl->ctrl_table_[position_p_gain_item_name] != NULL) - dxl->position_p_gain_item_ = dxl->ctrl_table_[position_p_gain_item_name]; - if (dxl->ctrl_table_[velocity_d_gain_item_name] != NULL) - dxl->velocity_d_gain_item_ = dxl->ctrl_table_[velocity_d_gain_item_name]; - if (dxl->ctrl_table_[velocity_i_gain_item_name] != NULL) - dxl->velocity_i_gain_item_ = dxl->ctrl_table_[velocity_i_gain_item_name]; - if (dxl->ctrl_table_[velocity_p_gain_item_name] != NULL) - dxl->velocity_p_gain_item_ = dxl->ctrl_table_[velocity_p_gain_item_name]; - - fprintf(stderr, "(%s) [ID:%3d] %14s added. \n", port.c_str(), dxl->id_, dxl->model_name_.c_str()); - //std::cout << "[ID:" << (int)(_dxl->id) << "] " << _dxl->model_name << " added. (" << port << ")" << std::endl; - file.close(); - } - else - std::cout << "Unable to open file : " + path << std::endl; - - return dxl; -} - +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * 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 ROBOTIS 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. + *******************************************************************************/ + +/* + * robot.cpp + * + * Created on: 2015. 12. 11. + * Author: zerom + */ + +#include +#include +#include + +#include "robotis_device/robot.h" + +using namespace robotis_framework; + +static inline std::string <rim(std::string &s) +{ + s.erase(s.begin(), std::find_if(s.begin(), s.end(), std::not1(std::ptr_fun(std::isspace)))); + return s; +} +static inline std::string &rtrim(std::string &s) +{ + s.erase(std::find_if(s.rbegin(), s.rend(), std::not1(std::ptr_fun(std::isspace))).base(), s.end()); + return s; +} +static inline std::string &trim(std::string &s) +{ + return ltrim(rtrim(s)); +} + +static inline std::vector split(const std::string &text, char sep) +{ + std::vector tokens; + std::size_t start = 0, end = 0; + + while ((end = text.find(sep, start)) != (std::string::npos)) + { + tokens.push_back(text.substr(start, end - start)); + trim(tokens.back()); + start = end + 1; + } + tokens.push_back(text.substr(start)); + trim(tokens.back()); + + return tokens; +} + +Robot::Robot(std::string robot_file_path, std::string dev_desc_dir_path) +{ + if (dev_desc_dir_path.compare(dev_desc_dir_path.size() - 1, 1, "/") != 0) + dev_desc_dir_path += "/"; + + std::ifstream file(robot_file_path.c_str()); + if (file.is_open()) + { + std::string session = ""; + std::string input_str; + while (!file.eof()) + { + std::getline(file, input_str); + + // remove comment ( # ) + std::size_t pos = input_str.find("#"); + if (pos != std::string::npos) + input_str = input_str.substr(0, pos); + + // trim + input_str = trim(input_str); + + // find session + if (!input_str.compare(0, 1, "[") && !input_str.compare(input_str.size() - 1, 1, "]")) + { + input_str = input_str.substr(1, input_str.size() - 2); + std::transform(input_str.begin(), input_str.end(), input_str.begin(), ::tolower); + session = trim(input_str); + continue; + } + + if (session == SESSION_PORT_INFO) + { + std::vector tokens = split(input_str, '|'); + if (tokens.size() != 3) + continue; + + std::cout << tokens[0] << " added. (baudrate: " << tokens[1] << ")" << std::endl; + + ports_[tokens[0]] = dynamixel::PortHandler::getPortHandler(tokens[0].c_str()); + ports_[tokens[0]]->setBaudRate(std::atoi(tokens[1].c_str())); + port_default_device_[tokens[0]] = tokens[2]; + } + else if (session == SESSION_DEVICE_INFO) + { + std::vector tokens = split(input_str, '|'); + if (tokens.size() != 7) + continue; + + if (tokens[0] == DYNAMIXEL) + { + std::string file_path = dev_desc_dir_path + tokens[0] + "/" + tokens[3] + ".device"; + int id = std::atoi(tokens[2].c_str()); + std::string port = tokens[1]; + float protocol = std::atof(tokens[4].c_str()); + std::string dev_name = tokens[5]; + + dxls_[dev_name] = getDynamixel(file_path, id, port, protocol); + + Dynamixel *dxl = dxls_[dev_name]; + std::vector sub_tokens = split(tokens[6], ','); + if (sub_tokens.size() > 0) + { + std::map::iterator indirect_it = dxl->ctrl_table_.find(INDIRECT_ADDRESS_1); + if (indirect_it != dxl->ctrl_table_.end()) // INDIRECT_ADDRESS_1 exist + { + uint16_t indirect_data_addr = dxl->ctrl_table_[INDIRECT_DATA_1]->address_; + for (int _i = 0; _i < sub_tokens.size(); _i++) + { + dxl->bulk_read_items_.push_back(new ControlTableItem()); + ControlTableItem *dest_item = dxl->bulk_read_items_[_i]; + ControlTableItem *src_item = dxl->ctrl_table_[sub_tokens[_i]]; + + dest_item->item_name_ = src_item->item_name_; + dest_item->address_ = indirect_data_addr; + dest_item->access_type_ = src_item->access_type_; + dest_item->memory_type_ = src_item->memory_type_; + dest_item->data_length_ = src_item->data_length_; + dest_item->data_min_value_ = src_item->data_min_value_; + dest_item->data_max_value_ = src_item->data_max_value_; + dest_item->is_signed_ = src_item->is_signed_; + + indirect_data_addr += dest_item->data_length_; + } + } + else // INDIRECT_ADDRESS_1 not exist + { + for (int i = 0; i < sub_tokens.size(); i++) + { + if (dxl->ctrl_table_[sub_tokens[i]] != NULL) + dxl->bulk_read_items_.push_back(dxl->ctrl_table_[sub_tokens[i]]); + } + } + } + } + else if (tokens[0] == SENSOR) + { + std::string file_path = dev_desc_dir_path + tokens[0] + "/" + tokens[3] + ".device"; + int id = std::atoi(tokens[2].c_str()); + std::string port = tokens[1]; + float protocol = std::atof(tokens[4].c_str()); + std::string dev_name = tokens[5]; + + sensors_[dev_name] = getSensor(file_path, id, port, protocol); + + Sensor *sensor = sensors_[dev_name]; + std::vector sub_tokens = split(tokens[6], ','); + if (sub_tokens.size() > 0) + { + std::map::iterator indirect_it = sensor->ctrl_table_.find(INDIRECT_ADDRESS_1); + if (indirect_it != sensor->ctrl_table_.end()) // INDIRECT_ADDRESS_1 exist + { + uint16_t indirect_data_addr = sensor->ctrl_table_[INDIRECT_DATA_1]->address_; + for (int i = 0; i < sub_tokens.size(); i++) + { + sensor->bulk_read_items_.push_back(new ControlTableItem()); + ControlTableItem *dest_item = sensor->bulk_read_items_[i]; + ControlTableItem *src_item = sensor->ctrl_table_[sub_tokens[i]]; + + dest_item->item_name_ = src_item->item_name_; + dest_item->address_ = indirect_data_addr; + dest_item->access_type_ = src_item->access_type_; + dest_item->memory_type_ = src_item->memory_type_; + dest_item->data_length_ = src_item->data_length_; + dest_item->data_min_value_ = src_item->data_min_value_; + dest_item->data_max_value_ = src_item->data_max_value_; + dest_item->is_signed_ = src_item->is_signed_; + + indirect_data_addr += dest_item->data_length_; + } + } + else // INDIRECT_ADDRESS_1 exist + { + for (int i = 0; i < sub_tokens.size(); i++) + sensor->bulk_read_items_.push_back(sensor->ctrl_table_[sub_tokens[i]]); + } + } + } + } + } + file.close(); + } + else + { + std::cout << "Unable to open file : " + robot_file_path << std::endl; + } +} + +Sensor *Robot::getSensor(std::string path, int id, std::string port, float protocol_version) +{ + Sensor *sensor; + + // load file model_name.device + std::ifstream file(path.c_str()); + if (file.is_open()) + { + std::string session = ""; + std::string input_str; + + while (!file.eof()) + { + std::getline(file, input_str); + + // remove comment ( # ) + std::size_t pos = input_str.find("#"); + if (pos != std::string::npos) + input_str = input_str.substr(0, pos); + + // trim + input_str = trim(input_str); + if (input_str == "") + continue; + + // find _session + if (!input_str.compare(0, 1, "[") && !input_str.compare(input_str.size() - 1, 1, "]")) + { + input_str = input_str.substr(1, input_str.size() - 2); + std::transform(input_str.begin(), input_str.end(), input_str.begin(), ::tolower); + session = trim(input_str); + continue; + } + + if (session == SESSION_DEVICE_INFO) + { + std::vector tokens = split(input_str, '='); + if (tokens.size() != 2) + continue; + + if (tokens[0] == "model_name") + sensor = new Sensor(id, tokens[1], protocol_version); + } + else if (session == SESSION_CONTROL_TABLE) + { + std::vector tokens = split(input_str, '|'); + if (tokens.size() != 8) + continue; + + ControlTableItem *item = new ControlTableItem(); + item->item_name_ = tokens[1]; + item->address_ = std::atoi(tokens[0].c_str()); + item->data_length_ = std::atoi(tokens[2].c_str()); + + if (tokens[3] == "R") + item->access_type_ = Read; + else if (tokens[3] == "RW") + item->access_type_ = ReadWrite; + + if (tokens[4] == "EEPROM") + item->memory_type_ = EEPROM; + else if (tokens[4] == "RAM") + item->memory_type_ = RAM; + + item->data_min_value_ = std::atol(tokens[5].c_str()); + item->data_max_value_ = std::atol(tokens[6].c_str()); + + if (tokens[7] == "Y") + item->is_signed_ = true; + else if (tokens[7] == "N") + item->is_signed_ = false; + sensor->ctrl_table_[tokens[1]] = item; + } + } + sensor->port_name_ = port; + + fprintf(stderr, "(%s) [ID:%3d] %14s added. \n", port.c_str(), sensor->id_, sensor->model_name_.c_str()); + //std::cout << "[ID:" << (int)(_sensor->id) << "] " << _sensor->model_name << " added. (" << port << ")" << std::endl; + file.close(); + } + else + std::cout << "Unable to open file : " + path << std::endl; + + return sensor; +} + +Dynamixel *Robot::getDynamixel(std::string path, int id, std::string port, float protocol_version) +{ + Dynamixel *dxl; + + // load file model_name.device + std::ifstream file(path.c_str()); + if (file.is_open()) + { + std::string session = ""; + std::string input_str; + + std::string torque_enable_item_name = ""; + std::string present_position_item_name = ""; + std::string present_velocity_item_name = ""; + std::string present_current_item_name = ""; + std::string goal_position_item_name = ""; + std::string goal_velocity_item_name = ""; + std::string goal_current_item_name = ""; + std::string position_d_gain_item_name = ""; + std::string position_i_gain_item_name = ""; + std::string position_p_gain_item_name = ""; + std::string velocity_d_gain_item_name = ""; + std::string velocity_i_gain_item_name = ""; + std::string velocity_p_gain_item_name = ""; + + while (!file.eof()) + { + std::getline(file, input_str); + + // remove comment ( # ) + std::size_t pos = input_str.find("#"); + if (pos != std::string::npos) + input_str = input_str.substr(0, pos); + + // trim + input_str = trim(input_str); + if (input_str == "") + continue; + + // find session + if (!input_str.compare(0, 1, "[") && !input_str.compare(input_str.size() - 1, 1, "]")) + { + input_str = input_str.substr(1, input_str.size() - 2); + std::transform(input_str.begin(), input_str.end(), input_str.begin(), ::tolower); + session = trim(input_str); + continue; + } + + if (session == SESSION_DEVICE_INFO) + { + std::vector tokens = split(input_str, '='); + if (tokens.size() != 2) + continue; + + if (tokens[0] == "model_name") + dxl = new Dynamixel(id, tokens[1], protocol_version); + } + else if (session == SESSION_TYPE_INFO) + { + std::vector tokens = split(input_str, '='); + if (tokens.size() != 2) + continue; + + if (tokens[0] == "torque_to_current_value_ratio") + dxl->torque_to_current_value_ratio_ = std::atof(tokens[1].c_str()); + else if (tokens[0] == "velocity_to_value_ratio") + dxl->velocity_to_value_ratio_ = std::atof(tokens[1].c_str()); + + else if (tokens[0] == "value_of_0_radian_position") + dxl->value_of_0_radian_position_ = std::atoi(tokens[1].c_str()); + else if (tokens[0] == "value_of_min_radian_position") + dxl->value_of_min_radian_position_ = std::atoi(tokens[1].c_str()); + else if (tokens[0] == "value_of_max_radian_position") + dxl->value_of_max_radian_position_ = std::atoi(tokens[1].c_str()); + else if (tokens[0] == "min_radian") + dxl->min_radian_ = std::atof(tokens[1].c_str()); + else if (tokens[0] == "max_radian") + dxl->max_radian_ = std::atof(tokens[1].c_str()); + + else if (tokens[0] == "torque_enable_item_name") + torque_enable_item_name = tokens[1]; + else if (tokens[0] == "present_position_item_name") + present_position_item_name = tokens[1]; + else if (tokens[0] == "present_velocity_item_name") + present_velocity_item_name = tokens[1]; + else if (tokens[0] == "present_current_item_name") + present_current_item_name = tokens[1]; + else if (tokens[0] == "goal_position_item_name") + goal_position_item_name = tokens[1]; + else if (tokens[0] == "goal_velocity_item_name") + goal_velocity_item_name = tokens[1]; + else if (tokens[0] == "goal_current_item_name") + goal_current_item_name = tokens[1]; + else if (tokens[0] == "position_d_gain_item_name") + position_d_gain_item_name = tokens[1]; + else if (tokens[0] == "position_i_gain_item_name") + position_i_gain_item_name = tokens[1]; + else if (tokens[0] == "position_p_gain_item_name") + position_p_gain_item_name = tokens[1]; + else if (tokens[0] == "velocity_d_gain_item_name") + velocity_d_gain_item_name = tokens[1]; + else if (tokens[0] == "velocity_i_gain_item_name") + velocity_i_gain_item_name = tokens[1]; + else if (tokens[0] == "velocity_p_gain_item_name") + velocity_p_gain_item_name = tokens[1]; + } + else if (session == SESSION_CONTROL_TABLE) + { + std::vector tokens = split(input_str, '|'); + if (tokens.size() != 8) + continue; + + ControlTableItem *item = new ControlTableItem(); + item->item_name_ = tokens[1]; + item->address_ = std::atoi(tokens[0].c_str()); + item->data_length_ = std::atoi(tokens[2].c_str()); + + if (tokens[3] == "R") + item->access_type_ = Read; + else if (tokens[3] == "RW") + item->access_type_ = ReadWrite; + + if (tokens[4] == "EEPROM") + item->memory_type_ = EEPROM; + else if (tokens[4] == "RAM") + item->memory_type_ = RAM; + + item->data_min_value_ = std::atol(tokens[5].c_str()); + item->data_max_value_ = std::atol(tokens[6].c_str()); + + if (tokens[7] == "Y") + item->is_signed_ = true; + else if (tokens[7] == "N") + item->is_signed_ = false; + dxl->ctrl_table_[tokens[1]] = item; + } + } + dxl->port_name_ = port; + + if (dxl->ctrl_table_[torque_enable_item_name] != NULL) + dxl->torque_enable_item_ = dxl->ctrl_table_[torque_enable_item_name]; + if (dxl->ctrl_table_[present_position_item_name] != NULL) + dxl->present_position_item_ = dxl->ctrl_table_[present_position_item_name]; + if (dxl->ctrl_table_[present_velocity_item_name] != NULL) + dxl->present_velocity_item_ = dxl->ctrl_table_[present_velocity_item_name]; + if (dxl->ctrl_table_[present_current_item_name] != NULL) + dxl->present_current_item_ = dxl->ctrl_table_[present_current_item_name]; + if (dxl->ctrl_table_[goal_position_item_name] != NULL) + dxl->goal_position_item_ = dxl->ctrl_table_[goal_position_item_name]; + if (dxl->ctrl_table_[goal_velocity_item_name] != NULL) + dxl->goal_velocity_item_ = dxl->ctrl_table_[goal_velocity_item_name]; + if (dxl->ctrl_table_[goal_current_item_name] != NULL) + dxl->goal_current_item_ = dxl->ctrl_table_[goal_current_item_name]; + if (dxl->ctrl_table_[position_d_gain_item_name] != NULL) + dxl->position_d_gain_item_ = dxl->ctrl_table_[position_d_gain_item_name]; + if (dxl->ctrl_table_[position_i_gain_item_name] != NULL) + dxl->position_i_gain_item_ = dxl->ctrl_table_[position_i_gain_item_name]; + if (dxl->ctrl_table_[position_p_gain_item_name] != NULL) + dxl->position_p_gain_item_ = dxl->ctrl_table_[position_p_gain_item_name]; + if (dxl->ctrl_table_[velocity_d_gain_item_name] != NULL) + dxl->velocity_d_gain_item_ = dxl->ctrl_table_[velocity_d_gain_item_name]; + if (dxl->ctrl_table_[velocity_i_gain_item_name] != NULL) + dxl->velocity_i_gain_item_ = dxl->ctrl_table_[velocity_i_gain_item_name]; + if (dxl->ctrl_table_[velocity_p_gain_item_name] != NULL) + dxl->velocity_p_gain_item_ = dxl->ctrl_table_[velocity_p_gain_item_name]; + + fprintf(stderr, "(%s) [ID:%3d] %14s added. \n", port.c_str(), dxl->id_, dxl->model_name_.c_str()); + //std::cout << "[ID:" << (int)(_dxl->id) << "] " << _dxl->model_name << " added. (" << port << ")" << std::endl; + file.close(); + } + else + std::cout << "Unable to open file : " + path << std::endl; + + return dxl; +} + diff --git a/robotis_device/src/robotis_device/sensor.cpp b/robotis_device/src/robotis_device/sensor.cpp old mode 100644 new mode 100755 index 537989d..3507342 --- a/robotis_device/src/robotis_device/sensor.cpp +++ b/robotis_device/src/robotis_device/sensor.cpp @@ -1,53 +1,53 @@ -/******************************************************************************* - * Copyright (c) 2016, ROBOTIS CO., LTD. - * 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 ROBOTIS 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. - *******************************************************************************/ - -/* - * sensor.cpp - * - * Created on: 2016. 5. 11. - * Author: zerom - */ - -#include "robotis_device/sensor.h" - -using namespace robotis_framework; - -Sensor::Sensor(int id, std::string model_name, float protocol_version) -{ - this->id_ = id; - this->model_name_ = model_name; - this->port_name_ = ""; - this->protocol_version_ = protocol_version; - ctrl_table_.clear(); - - sensor_state_ = new SensorState(); - - bulk_read_items_.clear(); -} +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * 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 ROBOTIS 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. + *******************************************************************************/ + +/* + * sensor.cpp + * + * Created on: 2016. 5. 11. + * Author: zerom + */ + +#include "robotis_device/sensor.h" + +using namespace robotis_framework; + +Sensor::Sensor(int id, std::string model_name, float protocol_version) +{ + this->id_ = id; + this->model_name_ = model_name; + this->port_name_ = ""; + this->protocol_version_ = protocol_version; + ctrl_table_.clear(); + + sensor_state_ = new SensorState(); + + bulk_read_items_.clear(); +} diff --git a/robotis_framework/CHANGELOG.rst b/robotis_framework/CHANGELOG.rst old mode 100644 new mode 100755 index 8f61ce3..2a746ef --- a/robotis_framework/CHANGELOG.rst +++ b/robotis_framework/CHANGELOG.rst @@ -1,12 +1,12 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package robotis_framework -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.1.1 (2016-08-18) ------------ -* updated the package information - -0.1.0 (2016-08-12) ------------ -* make a meta-package -* Contributors: Zerom, Pyo +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package robotis_framework +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.1.1 (2016-08-18) +----------- +* updated the package information + +0.1.0 (2016-08-12) +----------- +* make a meta-package +* Contributors: Zerom, Pyo diff --git a/robotis_framework/CMakeLists.txt b/robotis_framework/CMakeLists.txt old mode 100644 new mode 100755 index d336fe9..c5278ee --- a/robotis_framework/CMakeLists.txt +++ b/robotis_framework/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8.3) -project(robotis_framework) -find_package(catkin REQUIRED) -catkin_metapackage() +cmake_minimum_required(VERSION 2.8.3) +project(robotis_framework) +find_package(catkin REQUIRED) +catkin_metapackage() diff --git a/robotis_framework/package.xml b/robotis_framework/package.xml old mode 100644 new mode 100755 index 809e65e..87628dd --- a/robotis_framework/package.xml +++ b/robotis_framework/package.xml @@ -1,17 +1,17 @@ - - - robotis_framework - 0.1.1 - ROS packages for the robotis_framework (meta package) - BSD - Zerom - Pyo - https://github.com/ROBOTIS-GIT/ROBOTIS-Framework/issues - https://github.com/ROBOTIS-GIT/ROBOTIS-Framework - http://wiki.ros.org/robotis_framework - catkin - robotis_framework_common - robotis_device - robotis_controller - - + + + robotis_framework + 0.1.1 + ROS packages for the robotis_framework (meta package) + BSD + Zerom + Pyo + https://github.com/ROBOTIS-GIT/ROBOTIS-Framework/issues + https://github.com/ROBOTIS-GIT/ROBOTIS-Framework + http://wiki.ros.org/robotis_framework + catkin + robotis_framework_common + robotis_device + robotis_controller + + diff --git a/robotis_framework_common/CHANGELOG.rst b/robotis_framework_common/CHANGELOG.rst old mode 100644 new mode 100755 index bc63720..1adc0e1 --- a/robotis_framework_common/CHANGELOG.rst +++ b/robotis_framework_common/CHANGELOG.rst @@ -1,15 +1,15 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package robotis_framework_common -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.1.1 (2016-08-18) ------------ -* updated the package information - -0.1.0 (2016-08-12) ------------ -* modified the package information for release -* Setting the license to BSD. -* add SensorState - add Singleton template -* Contributors: Jay Song, Zerom, Pyo +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package robotis_framework_common +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.1.1 (2016-08-18) +----------- +* updated the package information + +0.1.0 (2016-08-12) +----------- +* modified the package information for release +* Setting the license to BSD. +* add SensorState + add Singleton template +* Contributors: Jay Song, Zerom, Pyo diff --git a/robotis_framework_common/CMakeLists.txt b/robotis_framework_common/CMakeLists.txt old mode 100644 new mode 100755 index c0505b1..d6ebb1c --- a/robotis_framework_common/CMakeLists.txt +++ b/robotis_framework_common/CMakeLists.txt @@ -1,55 +1,55 @@ -################################################################################ -# CMake -################################################################################ -cmake_minimum_required(VERSION 2.8.3) -project(robotis_framework_common) - -################################################################################ -# Packages -################################################################################ -find_package(catkin REQUIRED COMPONENTS - roscpp - robotis_device -) - -################################################################################ -# Declare ROS messages, services and actions -################################################################################ - -################################################################################ -# Declare ROS dynamic reconfigure parameters -################################################################################ - -################################################################################ -# Catkin specific configuration -################################################################################ -catkin_package( - INCLUDE_DIRS include - LIBRARIES robotis_framework_common - CATKIN_DEPENDS roscpp robotis_device -) - -################################################################################ -# Build -################################################################################ -include_directories( - include - ${catkin_INCLUDE_DIRS} -) -add_library(${PROJECT_NAME} - include/${PROJECT_NAME}/motion_module.h - include/${PROJECT_NAME}/sensor_module.h - include/${PROJECT_NAME}/singleton.h -) -set_target_properties(${PROJECT_NAME} PROPERTIES LINKER_LANGUAGE CXX) - -################################################################################ -# Install -################################################################################ -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -) - -################################################################################ -# Test -################################################################################ +################################################################################ +# CMake +################################################################################ +cmake_minimum_required(VERSION 2.8.3) +project(robotis_framework_common) + +################################################################################ +# Packages +################################################################################ +find_package(catkin REQUIRED COMPONENTS + roscpp + robotis_device +) + +################################################################################ +# Declare ROS messages, services and actions +################################################################################ + +################################################################################ +# Declare ROS dynamic reconfigure parameters +################################################################################ + +################################################################################ +# Catkin specific configuration +################################################################################ +catkin_package( + INCLUDE_DIRS include + LIBRARIES robotis_framework_common + CATKIN_DEPENDS roscpp robotis_device +) + +################################################################################ +# Build +################################################################################ +include_directories( + include + ${catkin_INCLUDE_DIRS} +) +add_library(${PROJECT_NAME} + include/${PROJECT_NAME}/motion_module.h + include/${PROJECT_NAME}/sensor_module.h + include/${PROJECT_NAME}/singleton.h +) +set_target_properties(${PROJECT_NAME} PROPERTIES LINKER_LANGUAGE CXX) + +################################################################################ +# Install +################################################################################ +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) + +################################################################################ +# Test +################################################################################ diff --git a/robotis_framework_common/include/robotis_framework_common/motion_module.h b/robotis_framework_common/include/robotis_framework_common/motion_module.h old mode 100644 new mode 100755 index e85d938..e81bbc8 --- a/robotis_framework_common/include/robotis_framework_common/motion_module.h +++ b/robotis_framework_common/include/robotis_framework_common/motion_module.h @@ -1,101 +1,101 @@ -/******************************************************************************* - * Copyright (c) 2016, ROBOTIS CO., LTD. - * 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 ROBOTIS 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. - *******************************************************************************/ - -/* - * motion_module.h - * - * Created on: 2016. 1. 15. - * Author: zerom - */ - -#ifndef ROBOTIS_FRAMEWORK_COMMON_MOTION_MODULE_H_ -#define ROBOTIS_FRAMEWORK_COMMON_MOTION_MODULE_H_ - - -#include -#include - -#include "singleton.h" -#include "robotis_device/robot.h" -#include "robotis_device/dynamixel.h" - -namespace robotis_framework -{ - -enum ControlMode -{ - PositionControl, - VelocityControl, - TorqueControl -}; - -class MotionModule -{ -protected: - bool enable_; - std::string module_name_; - ControlMode control_mode_; - -public: - std::map result_; - - virtual ~MotionModule() { } - - std::string getModuleName() { return module_name_; } - ControlMode getControlMode() { return control_mode_; } - - void setModuleEnable(bool enable) - { - if(this->enable_ == enable) - return; - - this->enable_ = enable; - if(enable) - onModuleEnable(); - else - onModuleDisable(); - } - bool getModuleEnable() { return enable_; } - - virtual void onModuleEnable() { } - virtual void onModuleDisable() { } - - virtual void initialize(const int control_cycle_msec, Robot *robot) = 0; - virtual void process(std::map dxls, std::map sensors) = 0; - - virtual void stop() = 0; - virtual bool isRunning() = 0; -}; - - -} - - -#endif /* ROBOTIS_FRAMEWORK_COMMON_MOTION_MODULE_H_ */ +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * 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 ROBOTIS 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. + *******************************************************************************/ + +/* + * motion_module.h + * + * Created on: 2016. 1. 15. + * Author: zerom + */ + +#ifndef ROBOTIS_FRAMEWORK_COMMON_MOTION_MODULE_H_ +#define ROBOTIS_FRAMEWORK_COMMON_MOTION_MODULE_H_ + + +#include +#include + +#include "singleton.h" +#include "robotis_device/robot.h" +#include "robotis_device/dynamixel.h" + +namespace robotis_framework +{ + +enum ControlMode +{ + PositionControl, + VelocityControl, + TorqueControl +}; + +class MotionModule +{ +protected: + bool enable_; + std::string module_name_; + ControlMode control_mode_; + +public: + std::map result_; + + virtual ~MotionModule() { } + + std::string getModuleName() { return module_name_; } + ControlMode getControlMode() { return control_mode_; } + + void setModuleEnable(bool enable) + { + if(this->enable_ == enable) + return; + + this->enable_ = enable; + if(enable) + onModuleEnable(); + else + onModuleDisable(); + } + bool getModuleEnable() { return enable_; } + + virtual void onModuleEnable() { } + virtual void onModuleDisable() { } + + virtual void initialize(const int control_cycle_msec, Robot *robot) = 0; + virtual void process(std::map dxls, std::map sensors) = 0; + + virtual void stop() = 0; + virtual bool isRunning() = 0; +}; + + +} + + +#endif /* ROBOTIS_FRAMEWORK_COMMON_MOTION_MODULE_H_ */ diff --git a/robotis_framework_common/include/robotis_framework_common/sensor_module.h b/robotis_framework_common/include/robotis_framework_common/sensor_module.h old mode 100644 new mode 100755 index 81699e2..775acfd --- a/robotis_framework_common/include/robotis_framework_common/sensor_module.h +++ b/robotis_framework_common/include/robotis_framework_common/sensor_module.h @@ -1,71 +1,71 @@ -/******************************************************************************* - * Copyright (c) 2016, ROBOTIS CO., LTD. - * 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 ROBOTIS 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. - *******************************************************************************/ - -/* - * sensor_module.h - * - * Created on: 2016. 3. 30. - * Author: zerom - */ - -#ifndef ROBOTIS_FRAMEWORK_COMMON_SENSOR_MODULE_H_ -#define ROBOTIS_FRAMEWORK_COMMON_SENSOR_MODULE_H_ - - -#include -#include - -#include "singleton.h" -#include "robotis_device/robot.h" -#include "robotis_device/dynamixel.h" - -namespace robotis_framework -{ - -class SensorModule -{ -protected: - std::string module_name_; - -public: - std::map result_; - - virtual ~SensorModule() { } - - std::string getModuleName() { return module_name_; } - - virtual void initialize(const int control_cycle_msec, Robot *robot) = 0; - virtual void process(std::map dxls, std::map sensors) = 0; -}; - -} - - -#endif /* ROBOTIS_FRAMEWORK_COMMON_SENSOR_MODULE_H_ */ +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * 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 ROBOTIS 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. + *******************************************************************************/ + +/* + * sensor_module.h + * + * Created on: 2016. 3. 30. + * Author: zerom + */ + +#ifndef ROBOTIS_FRAMEWORK_COMMON_SENSOR_MODULE_H_ +#define ROBOTIS_FRAMEWORK_COMMON_SENSOR_MODULE_H_ + + +#include +#include + +#include "singleton.h" +#include "robotis_device/robot.h" +#include "robotis_device/dynamixel.h" + +namespace robotis_framework +{ + +class SensorModule +{ +protected: + std::string module_name_; + +public: + std::map result_; + + virtual ~SensorModule() { } + + std::string getModuleName() { return module_name_; } + + virtual void initialize(const int control_cycle_msec, Robot *robot) = 0; + virtual void process(std::map dxls, std::map sensors) = 0; +}; + +} + + +#endif /* ROBOTIS_FRAMEWORK_COMMON_SENSOR_MODULE_H_ */ diff --git a/robotis_framework_common/include/robotis_framework_common/singleton.h b/robotis_framework_common/include/robotis_framework_common/singleton.h old mode 100644 new mode 100755 index 837b80b..b6338b2 --- a/robotis_framework_common/include/robotis_framework_common/singleton.h +++ b/robotis_framework_common/include/robotis_framework_common/singleton.h @@ -1,79 +1,79 @@ -/******************************************************************************* - * Copyright (c) 2016, ROBOTIS CO., LTD. - * 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 ROBOTIS 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. - *******************************************************************************/ - -/* - * singleton.h - * - * Created on: 2016. 5. 17. - * Author: zerom - */ - -#ifndef ROBOTIS_FRAMEWORK_COMMON_SINGLETON_H_ -#define ROBOTIS_FRAMEWORK_COMMON_SINGLETON_H_ - - -namespace robotis_framework -{ - -template -class Singleton -{ -private: - static T *unique_instance_; - -protected: - Singleton() { } - Singleton(Singleton const&) { } - Singleton& operator=(Singleton const&) { return *this; } - -public: - static T* getInstance() - { - if(unique_instance_ == NULL) - unique_instance_ = new T; - return unique_instance_; - } - - static void destroyInstance() - { - if(unique_instance_) - { - delete unique_instance_; - unique_instance_ = NULL; - } - } -}; - -template T* Singleton::unique_instance_ = NULL; - -} - - -#endif /* ROBOTIS_FRAMEWORK_COMMON_SINGLETON_H_ */ +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * 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 ROBOTIS 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. + *******************************************************************************/ + +/* + * singleton.h + * + * Created on: 2016. 5. 17. + * Author: zerom + */ + +#ifndef ROBOTIS_FRAMEWORK_COMMON_SINGLETON_H_ +#define ROBOTIS_FRAMEWORK_COMMON_SINGLETON_H_ + + +namespace robotis_framework +{ + +template +class Singleton +{ +private: + static T *unique_instance_; + +protected: + Singleton() { } + Singleton(Singleton const&) { } + Singleton& operator=(Singleton const&) { return *this; } + +public: + static T* getInstance() + { + if(unique_instance_ == NULL) + unique_instance_ = new T; + return unique_instance_; + } + + static void destroyInstance() + { + if(unique_instance_) + { + delete unique_instance_; + unique_instance_ = NULL; + } + } +}; + +template T* Singleton::unique_instance_ = NULL; + +} + + +#endif /* ROBOTIS_FRAMEWORK_COMMON_SINGLETON_H_ */ diff --git a/robotis_framework_common/package.xml b/robotis_framework_common/package.xml old mode 100644 new mode 100755 index 2e66361..0957d69 --- a/robotis_framework_common/package.xml +++ b/robotis_framework_common/package.xml @@ -1,19 +1,19 @@ - - - robotis_framework_common - 0.1.1 - - The package contains commonly used Headers for the ROBOTIS Framework. - - BSD - Zerom - Pyo - https://github.com/ROBOTIS-GIT/ROBOTIS-Framework/issues - https://github.com/ROBOTIS-GIT/ROBOTIS-Framework - http://wiki.ros.org/robotis_framework_common - catkin - roscpp - robotis_device - roscpp - robotis_device - + + + robotis_framework_common + 0.1.1 + + The package contains commonly used Headers for the ROBOTIS Framework. + + BSD + Zerom + Pyo + https://github.com/ROBOTIS-GIT/ROBOTIS-Framework/issues + https://github.com/ROBOTIS-GIT/ROBOTIS-Framework + http://wiki.ros.org/robotis_framework_common + catkin + roscpp + robotis_device + roscpp + robotis_device + From abbbec3fe39de67a99581c28212ed152d79e0c7a Mon Sep 17 00:00:00 2001 From: ROBOTIS-zerom Date: Thu, 10 Nov 2016 10:16:37 +0900 Subject: [PATCH 079/238] - Direct Control Mode bug fixed. --- .../robotis_controller/robotis_controller.cpp | 77 +++++++++++++++++-- 1 file changed, 72 insertions(+), 5 deletions(-) diff --git a/robotis_controller/src/robotis_controller/robotis_controller.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp index 5a0c1ec..2be5d6a 100755 --- a/robotis_controller/src/robotis_controller/robotis_controller.cpp +++ b/robotis_controller/src/robotis_controller/robotis_controller.cpp @@ -1109,13 +1109,63 @@ void RobotisController::process() { if(gazebo_mode_ == false) { + // BulkRead Rx + for (auto& it : port_to_bulk_read_) + { + robot_->ports_[it.first]->setPacketTimeout(0.0); + it.second->rxPacket(); + } + + // -> save to robot->dxls_[]->dxl_state_ + if (robot_->dxls_.size() > 0) + { + for (auto& dxl_it : robot_->dxls_) + { + Dynamixel *dxl = dxl_it.second; + std::string port_name = dxl_it.second->port_name_; + std::string joint_name = dxl_it.first; + + if (dxl->bulk_read_items_.size() > 0) + { + uint32_t data = 0; + for (int i = 0; i < dxl->bulk_read_items_.size(); i++) + { + ControlTableItem *item = dxl->bulk_read_items_[i]; + if (port_to_bulk_read_[port_name]->isAvailable(dxl->id_, item->address_, item->data_length_) == true) + { + data = port_to_bulk_read_[port_name]->getData(dxl->id_, item->address_, item->data_length_); + + // change dxl_state + if (dxl->present_position_item_ != 0 && item->item_name_ == dxl->present_position_item_->item_name_) + dxl->dxl_state_->present_position_ = dxl->convertValue2Radian(data) - dxl->dxl_state_->position_offset_; // remove offset + else if (dxl->present_velocity_item_ != 0 && item->item_name_ == dxl->present_velocity_item_->item_name_) + dxl->dxl_state_->present_velocity_ = dxl->convertValue2Velocity(data); + else if (dxl->present_current_item_ != 0 && item->item_name_ == dxl->present_current_item_->item_name_) + dxl->dxl_state_->present_torque_ = dxl->convertValue2Torque(data); + else if (dxl->goal_position_item_ != 0 && item->item_name_ == dxl->goal_position_item_->item_name_) + dxl->dxl_state_->goal_position_ = dxl->convertValue2Radian(data) - dxl->dxl_state_->position_offset_; // remove offset + else if (dxl->goal_velocity_item_ != 0 && item->item_name_ == dxl->goal_velocity_item_->item_name_) + dxl->dxl_state_->goal_velocity_ = dxl->convertValue2Velocity(data); + else if (dxl->goal_current_item_ != 0 && item->item_name_ == dxl->goal_current_item_->item_name_) + dxl->dxl_state_->goal_torque_ = dxl->convertValue2Torque(data); + else + dxl->dxl_state_->bulk_read_table_[item->item_name_] = data; + } + } + + // -> update time stamp to Robot->dxls[]->dynamixel_state.update_time_stamp + dxl->dxl_state_->update_time_stamp_ = TimeStamp(present_state.header.stamp.sec, present_state.header.stamp.nsec); + } + } + } + queue_mutex_.lock(); - for (auto& it : port_to_sync_write_position_) - { - it.second->txPacket(); - it.second->clearParam(); - } +// for (auto& it : port_to_sync_write_position_) +// { +// it.second->txPacket(); +// it.second->clearParam(); +// } if (direct_sync_write_.size() > 0) { @@ -1128,6 +1178,10 @@ void RobotisController::process() } queue_mutex_.unlock(); + + // BulkRead Tx + for (auto& it : port_to_bulk_read_) + it.second->txPacket(); } } @@ -1553,9 +1607,22 @@ void RobotisController::syncWriteItemCallback(const robotis_controller_msgs::Syn void RobotisController::setControllerModeCallback(const std_msgs::String::ConstPtr &msg) { if (msg->data == "DirectControlMode") + { + for (auto& it : port_to_bulk_read_) + { + robot_->ports_[it.first]->setPacketTimeout(0.0); + it.second->rxPacket(); + } controller_mode_ = DirectControlMode; + } else if (msg->data == "MotionModuleMode") + { + for (auto& it : port_to_bulk_read_) + { + it.second->txPacket(); + } controller_mode_ = MotionModuleMode; + } } void RobotisController::setJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg) From d1d39d3c82710cafe6e68b5be545b8f6ffbe21b3 Mon Sep 17 00:00:00 2001 From: Pyo Date: Wed, 23 Nov 2016 09:13:50 +0900 Subject: [PATCH 080/238] update 0.2.0 > 0.2.1 --- robotis_controller/CHANGELOG.rst | 18 ++++++++++++++++++ robotis_controller/package.xml | 2 +- robotis_device/CHANGELOG.rst | 8 ++++++++ robotis_device/package.xml | 2 +- robotis_framework/CHANGELOG.rst | 4 ++++ robotis_framework/package.xml | 2 +- robotis_framework_common/CHANGELOG.rst | 7 +++++++ robotis_framework_common/package.xml | 2 +- 8 files changed, 41 insertions(+), 4 deletions(-) diff --git a/robotis_controller/CHANGELOG.rst b/robotis_controller/CHANGELOG.rst index dd4e11c..002d663 100755 --- a/robotis_controller/CHANGELOG.rst +++ b/robotis_controller/CHANGELOG.rst @@ -2,6 +2,24 @@ Changelog for package robotis_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.2.1 (2016-11-23) +----------- +* Merge the changes and update +* - Direct Control Mode bug fixed. +* update +* - added writeControlTableCallback +* - added WriteControlTable msg callback +* mode change debugging +* - optimized cpu usage by spin loop (by astumpf) +* - robotis_controller process() : processing order changed. + * 1st : packet communication + * 2nd : processing modules +* - dependencies fixed. (Pull requests `#26 `_) +* - make setJointCtrlModuleCallback() to the thread function & improved. +* - modified dependency problem. +* - reduce CPU consumption +* Contributors: Jay Song, Pyo, Zerom, SCH + 0.2.0 (2016-08-31) ----------- * bug fixed (position pid gain & velocity pid gain sync write). diff --git a/robotis_controller/package.xml b/robotis_controller/package.xml index f1be9e2..2b14099 100755 --- a/robotis_controller/package.xml +++ b/robotis_controller/package.xml @@ -1,7 +1,7 @@ robotis_controller - 0.2.0 + 0.2.1 The main package that controls THORMANG3. diff --git a/robotis_device/CHANGELOG.rst b/robotis_device/CHANGELOG.rst index ac3e9d9..20b2638 100755 --- a/robotis_device/CHANGELOG.rst +++ b/robotis_device/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package robotis_device ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.2.1 (2016-11-23) +----------- +* Merge the changes and update +* mode change debugging +* - convertRadian2Value / convertValue2Radian : commented out the code that limits the maximum/minimum value. +* - modified dependency problem. +* Contributors: Jay Song, Pyo, Zerom, SCH + 0.2.0 (2016-08-31) ----------- * bug fixed (position pid gain & velocity pid gain sync write). diff --git a/robotis_device/package.xml b/robotis_device/package.xml index f889c3f..516770d 100755 --- a/robotis_device/package.xml +++ b/robotis_device/package.xml @@ -1,7 +1,7 @@ robotis_device - 0.2.0 + 0.2.1 The package that manages device information of ROBOTIS robots. This package is used when reading device information with the robot information file diff --git a/robotis_framework/CHANGELOG.rst b/robotis_framework/CHANGELOG.rst index 513131b..87f4509 100755 --- a/robotis_framework/CHANGELOG.rst +++ b/robotis_framework/CHANGELOG.rst @@ -2,6 +2,10 @@ Changelog for package robotis_framework ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.2.1 (2016-11-23) +----------- +* Merge the changes and update + 0.2.0 (2016-08-31) ----------- * updated CHANGLOG.rst for minor release diff --git a/robotis_framework/package.xml b/robotis_framework/package.xml index 09a449a..44a7089 100755 --- a/robotis_framework/package.xml +++ b/robotis_framework/package.xml @@ -1,7 +1,7 @@ robotis_framework - 0.2.0 + 0.2.1 ROS packages for the robotis_framework (meta package) BSD Zerom diff --git a/robotis_framework_common/CHANGELOG.rst b/robotis_framework_common/CHANGELOG.rst index e5a510b..fb68bac 100755 --- a/robotis_framework_common/CHANGELOG.rst +++ b/robotis_framework_common/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package robotis_framework_common ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.2.1 (2016-11-23) +----------- +* Merge the changes and update +* - dependencies fixed. (Pull requests `#26 `_) +* - modified dependency problem. +* Contributors: Jay Song, Pyo, Zerom + 0.2.0 (2016-08-31) ----------- * updated CHANGLOG.rst for minor release diff --git a/robotis_framework_common/package.xml b/robotis_framework_common/package.xml index 7cce1b6..0084962 100755 --- a/robotis_framework_common/package.xml +++ b/robotis_framework_common/package.xml @@ -1,7 +1,7 @@ robotis_framework_common - 0.2.0 + 0.2.1 The package contains commonly used Headers for the ROBOTIS Framework. From a5bcabbac687c323d3167086dd66ddd3ae9a4243 Mon Sep 17 00:00:00 2001 From: Pyo Date: Wed, 23 Nov 2016 10:06:45 +0900 Subject: [PATCH 081/238] update 0.2.0 > 0.2.1 --- robotis_controller/package.xml | 1 + robotis_framework_common/package.xml | 2 ++ 2 files changed, 3 insertions(+) diff --git a/robotis_controller/package.xml b/robotis_controller/package.xml index 2b14099..e2368e3 100755 --- a/robotis_controller/package.xml +++ b/robotis_controller/package.xml @@ -27,5 +27,6 @@ dynamixel_sdk robotis_device robotis_controller_msgs + robotis_framework_common diff --git a/robotis_framework_common/package.xml b/robotis_framework_common/package.xml index 0084962..8f0a6c7 100755 --- a/robotis_framework_common/package.xml +++ b/robotis_framework_common/package.xml @@ -13,5 +13,7 @@ http://wiki.ros.org/robotis_framework_common catkin roscpp + robotis_device roscpp + robotis_device From 3f7c86f6118afbdba81760fd6aeb446c0faada22 Mon Sep 17 00:00:00 2001 From: Pyo Date: Wed, 23 Nov 2016 10:52:37 +0900 Subject: [PATCH 082/238] check the bloom error --- robotis_framework/CMakeLists.txt | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/robotis_framework/CMakeLists.txt b/robotis_framework/CMakeLists.txt index c5278ee..c0d90b3 100755 --- a/robotis_framework/CMakeLists.txt +++ b/robotis_framework/CMakeLists.txt @@ -2,3 +2,18 @@ cmake_minimum_required(VERSION 2.8.3) project(robotis_framework) find_package(catkin REQUIRED) catkin_metapackage() + +Invalid metapackage: + Metapackage 'robotis_framework': Invalid CMakeLists.txt +Expected: +<<>> +Got: +<<>> From f14a1fc54979e142da124f1d50c4309ee00626b7 Mon Sep 17 00:00:00 2001 From: Pyo Date: Wed, 23 Nov 2016 10:53:48 +0900 Subject: [PATCH 083/238] check the bloom error --- robotis_framework/CMakeLists.txt | 23 ++++------------------- 1 file changed, 4 insertions(+), 19 deletions(-) mode change 100755 => 100644 robotis_framework/CMakeLists.txt diff --git a/robotis_framework/CMakeLists.txt b/robotis_framework/CMakeLists.txt old mode 100755 new mode 100644 index c0d90b3..d336fe9 --- a/robotis_framework/CMakeLists.txt +++ b/robotis_framework/CMakeLists.txt @@ -1,19 +1,4 @@ -cmake_minimum_required(VERSION 2.8.3) -project(robotis_framework) -find_package(catkin REQUIRED) -catkin_metapackage() - -Invalid metapackage: - Metapackage 'robotis_framework': Invalid CMakeLists.txt -Expected: -<<>> -Got: -<<>> +cmake_minimum_required(VERSION 2.8.3) +project(robotis_framework) +find_package(catkin REQUIRED) +catkin_metapackage() From 0afd62afb38c82c3b1828c27d7b49dfeb89ada10 Mon Sep 17 00:00:00 2001 From: robotis Date: Fri, 9 Dec 2016 14:50:59 +0900 Subject: [PATCH 084/238] Now, control cycle can be changed. --- .../robotis_controller/robotis_controller.h | 3 ++- .../robotis_controller/robotis_controller.cpp | 19 ++++++++++--------- 2 files changed, 12 insertions(+), 10 deletions(-) diff --git a/robotis_controller/include/robotis_controller/robotis_controller.h b/robotis_controller/include/robotis_controller/robotis_controller.h index 8c1ae27..f59dbd5 100755 --- a/robotis_controller/include/robotis_controller/robotis_controller.h +++ b/robotis_controller/include/robotis_controller/robotis_controller.h @@ -95,13 +95,14 @@ private: void initializeSyncWrite(); public: - static const int CONTROL_CYCLE_MSEC = 8; // 8 msec + static const int DEFAULT_CONTROL_CYCLE_MSEC = 8; // 8 msec bool DEBUG_PRINT; Robot *robot_; bool gazebo_mode_; std::string gazebo_robot_name_; + int control_cycle_msec_; /* bulk read */ std::map port_to_bulk_read_; diff --git a/robotis_controller/src/robotis_controller/robotis_controller.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp index 2be5d6a..d75684e 100755 --- a/robotis_controller/src/robotis_controller/robotis_controller.cpp +++ b/robotis_controller/src/robotis_controller/robotis_controller.cpp @@ -51,7 +51,8 @@ RobotisController::RobotisController() DEBUG_PRINT(false), robot_(0), gazebo_mode_(false), - gazebo_robot_name_("robotis") + gazebo_robot_name_("robotis"), + control_cycle_msec_(DEFAULT_CONTROL_CYCLE_MSEC) { direct_sync_write_.clear(); } @@ -603,7 +604,7 @@ void RobotisController::initializeDevice(const std::string init_file_path) void RobotisController::gazeboTimerThread() { - ros::Rate gazebo_rate(1000 / CONTROL_CYCLE_MSEC); + ros::Rate gazebo_rate(1000 / control_cycle_msec_); while (!stop_timer_) { @@ -662,7 +663,7 @@ void RobotisController::msgQueueThread() ros::ServiceServer joint_module_server = ros_node.advertiseService("/robotis/get_present_joint_ctrl_modules", &RobotisController::getCtrlModuleCallback, this); - ros::WallDuration duration(CONTROL_CYCLE_MSEC / 1000.0); + ros::WallDuration duration(control_cycle_msec_ / 1000.0); while(ros_node.ok()) callback_queue.callAvailable(duration); } @@ -679,8 +680,8 @@ void *RobotisController::timerThread(void *param) while (!controller->stop_timer_) { - next_time.tv_sec += (next_time.tv_nsec + CONTROL_CYCLE_MSEC * 1000000) / 1000000000; - next_time.tv_nsec = (next_time.tv_nsec + CONTROL_CYCLE_MSEC * 1000000) % 1000000000; + next_time.tv_sec += (next_time.tv_nsec + controller->control_cycle_msec_ * 1000000) / 1000000000; + next_time.tv_nsec = (next_time.tv_nsec + controller->control_cycle_msec_ * 1000000) % 1000000000; controller->process(); @@ -1429,7 +1430,7 @@ void RobotisController::addMotionModule(MotionModule *module) } } - module->initialize(CONTROL_CYCLE_MSEC, robot_); + module->initialize(control_cycle_msec_, robot_); motion_modules_.push_back(module); motion_modules_.unique(); } @@ -1451,7 +1452,7 @@ void RobotisController::addSensorModule(SensorModule *module) } } - module->initialize(CONTROL_CYCLE_MSEC, robot_); + module->initialize(control_cycle_msec_, robot_); sensor_modules_.push_back(module); sensor_modules_.unique(); } @@ -1731,7 +1732,7 @@ void RobotisController::setJointCtrlModuleThread(const robotis_controller_msgs:: for(std::list::iterator _stop_m_it = _stop_modules.begin(); _stop_m_it != _stop_modules.end(); _stop_m_it++) { while((*_stop_m_it)->isRunning()) - usleep(CONTROL_CYCLE_MSEC * 1000); + usleep(control_cycle_msec_ * 1000); } // disable module(s) @@ -1946,7 +1947,7 @@ void RobotisController::setCtrlModuleThread(std::string ctrl_module) for (auto stop_m_it = stop_modules.begin(); stop_m_it != stop_modules.end(); stop_m_it++) { while ((*stop_m_it)->isRunning()) - usleep(CONTROL_CYCLE_MSEC * 1000); + usleep(control_cycle_msec_ * 1000); } // disable module(s) From 41a8e3073103eb4e813320273cd71be18daa48c5 Mon Sep 17 00:00:00 2001 From: ROBOTIS-zerom Date: Wed, 14 Dec 2016 17:19:52 +0900 Subject: [PATCH 085/238] read control cycle from .robot file --- .../robotis_controller/robotis_controller.h | 3 --- .../robotis_controller/robotis_controller.cpp | 19 +++++++++---------- robotis_device/include/robotis_device/robot.h | 8 ++++++++ robotis_device/src/robotis_device/robot.cpp | 16 +++++++++++++++- 4 files changed, 32 insertions(+), 14 deletions(-) diff --git a/robotis_controller/include/robotis_controller/robotis_controller.h b/robotis_controller/include/robotis_controller/robotis_controller.h index f59dbd5..f811b24 100755 --- a/robotis_controller/include/robotis_controller/robotis_controller.h +++ b/robotis_controller/include/robotis_controller/robotis_controller.h @@ -95,14 +95,11 @@ private: void initializeSyncWrite(); public: - static const int DEFAULT_CONTROL_CYCLE_MSEC = 8; // 8 msec - bool DEBUG_PRINT; Robot *robot_; bool gazebo_mode_; std::string gazebo_robot_name_; - int control_cycle_msec_; /* bulk read */ std::map port_to_bulk_read_; diff --git a/robotis_controller/src/robotis_controller/robotis_controller.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp index d75684e..966dbdc 100755 --- a/robotis_controller/src/robotis_controller/robotis_controller.cpp +++ b/robotis_controller/src/robotis_controller/robotis_controller.cpp @@ -51,8 +51,7 @@ RobotisController::RobotisController() DEBUG_PRINT(false), robot_(0), gazebo_mode_(false), - gazebo_robot_name_("robotis"), - control_cycle_msec_(DEFAULT_CONTROL_CYCLE_MSEC) + gazebo_robot_name_("robotis") { direct_sync_write_.clear(); } @@ -604,7 +603,7 @@ void RobotisController::initializeDevice(const std::string init_file_path) void RobotisController::gazeboTimerThread() { - ros::Rate gazebo_rate(1000 / control_cycle_msec_); + ros::Rate gazebo_rate(1000 / robot_->getControlCycle()); while (!stop_timer_) { @@ -663,7 +662,7 @@ void RobotisController::msgQueueThread() ros::ServiceServer joint_module_server = ros_node.advertiseService("/robotis/get_present_joint_ctrl_modules", &RobotisController::getCtrlModuleCallback, this); - ros::WallDuration duration(control_cycle_msec_ / 1000.0); + ros::WallDuration duration(robot_->getControlCycle() / 1000.0); while(ros_node.ok()) callback_queue.callAvailable(duration); } @@ -680,8 +679,8 @@ void *RobotisController::timerThread(void *param) while (!controller->stop_timer_) { - next_time.tv_sec += (next_time.tv_nsec + controller->control_cycle_msec_ * 1000000) / 1000000000; - next_time.tv_nsec = (next_time.tv_nsec + controller->control_cycle_msec_ * 1000000) % 1000000000; + next_time.tv_sec += (next_time.tv_nsec + controller->robot_->getControlCycle() * 1000000) / 1000000000; + next_time.tv_nsec = (next_time.tv_nsec + controller->robot_->getControlCycle() * 1000000) % 1000000000; controller->process(); @@ -1430,7 +1429,7 @@ void RobotisController::addMotionModule(MotionModule *module) } } - module->initialize(control_cycle_msec_, robot_); + module->initialize(robot_->getControlCycle(), robot_); motion_modules_.push_back(module); motion_modules_.unique(); } @@ -1452,7 +1451,7 @@ void RobotisController::addSensorModule(SensorModule *module) } } - module->initialize(control_cycle_msec_, robot_); + module->initialize(robot_->getControlCycle(), robot_); sensor_modules_.push_back(module); sensor_modules_.unique(); } @@ -1732,7 +1731,7 @@ void RobotisController::setJointCtrlModuleThread(const robotis_controller_msgs:: for(std::list::iterator _stop_m_it = _stop_modules.begin(); _stop_m_it != _stop_modules.end(); _stop_m_it++) { while((*_stop_m_it)->isRunning()) - usleep(control_cycle_msec_ * 1000); + usleep(robot_->getControlCycle() * 1000); } // disable module(s) @@ -1947,7 +1946,7 @@ void RobotisController::setCtrlModuleThread(std::string ctrl_module) for (auto stop_m_it = stop_modules.begin(); stop_m_it != stop_modules.end(); stop_m_it++) { while ((*stop_m_it)->isRunning()) - usleep(control_cycle_msec_ * 1000); + usleep(robot_->getControlCycle() * 1000); } // disable module(s) diff --git a/robotis_device/include/robotis_device/robot.h b/robotis_device/include/robotis_device/robot.h index 91b30f1..684fe6a 100755 --- a/robotis_device/include/robotis_device/robot.h +++ b/robotis_device/include/robotis_device/robot.h @@ -48,17 +48,23 @@ #define DYNAMIXEL "dynamixel" #define SENSOR "sensor" +#define SESSION_CONTROL_INFO "control info" #define SESSION_PORT_INFO "port info" #define SESSION_DEVICE_INFO "device info" #define SESSION_TYPE_INFO "type info" #define SESSION_CONTROL_TABLE "control table" +#define DEFAULT_CONTROL_CYCLE 8 // milliseconds + namespace robotis_framework { class Robot { +private: + int control_cycle_msec_; + public: std::map ports_; // string: port name std::map port_default_device_; // port name, default device name @@ -70,6 +76,8 @@ public: Sensor *getSensor(std::string path, int id, std::string port, float protocol_version); Dynamixel *getDynamixel(std::string path, int id, std::string port, float protocol_version); + + int getControlCycle(); }; } diff --git a/robotis_device/src/robotis_device/robot.cpp b/robotis_device/src/robotis_device/robot.cpp index fcc8fa0..94d4133 100755 --- a/robotis_device/src/robotis_device/robot.cpp +++ b/robotis_device/src/robotis_device/robot.cpp @@ -76,6 +76,7 @@ static inline std::vector split(const std::string &text, char sep) } Robot::Robot(std::string robot_file_path, std::string dev_desc_dir_path) + : control_cycle_msec_(DEFAULT_CONTROL_CYCLE) { if (dev_desc_dir_path.compare(dev_desc_dir_path.size() - 1, 1, "/") != 0) dev_desc_dir_path += "/"; @@ -106,7 +107,16 @@ Robot::Robot(std::string robot_file_path, std::string dev_desc_dir_path) continue; } - if (session == SESSION_PORT_INFO) + if (session == SESSION_CONTROL_INFO) + { + std::vector tokens = split(input_str, '='); + if (tokens.size() != 2) + continue; + + if (tokens[0] == "control_cycle") + control_cycle_msec_ = std::atoi(tokens[1].c_str()); + } + else if (session == SESSION_PORT_INFO) { std::vector tokens = split(input_str, '|'); if (tokens.size() != 3) @@ -485,3 +495,7 @@ Dynamixel *Robot::getDynamixel(std::string path, int id, std::string port, float return dxl; } +int Robot::getControlCycle() +{ + return control_cycle_msec_; +} From 60dfda6adeca4be6c5c6e98402f8dd3e60536d5f Mon Sep 17 00:00:00 2001 From: kayman Date: Tue, 28 Feb 2017 10:59:04 +0900 Subject: [PATCH 086/238] add deivce : OPEN-CR --- robotis_device/devices/sensor/OPEN-CR.device | 30 ++++++++++++++++++++ 1 file changed, 30 insertions(+) create mode 100644 robotis_device/devices/sensor/OPEN-CR.device diff --git a/robotis_device/devices/sensor/OPEN-CR.device b/robotis_device/devices/sensor/OPEN-CR.device new file mode 100644 index 0000000..94b446e --- /dev/null +++ b/robotis_device/devices/sensor/OPEN-CR.device @@ -0,0 +1,30 @@ +[device info] +model_name = OPEN-CR +device_type = sensor + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 2 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 3 | ID | 1 | RW | EEPROM | 0 | 252 | N + 4 | baudrate | 1 | RW | EEPROM | 0 | 252 | N + 5 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 16 | status_return_level | 1 | RW | EEPROM | 0 | 2 | N + 24 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 25 | LED | 1 | RW | RAM | 0 | 7 | N + 26 | LED_RGB | 2 | RW | RAM | 0 | 32767 | N + 28 | buzzer | 2 | RW | RAM | 0 | 65535 | N + 30 | button | 1 | R | RAM | 0 | 15 | N + 31 | present_voltage | 1 | R | RAM | 50 | 250 | N + 32 | gyro_x | 2 | R | RAM | -32800 | 32800 | Y + 34 | gyro_y | 2 | R | RAM | -32800 | 32800 | Y + 36 | gyro_z | 2 | R | RAM | -32800 | 32800 | Y + 38 | acc_x | 2 | R | RAM | -32768 | 32768 | Y + 40 | acc_y | 2 | R | RAM | -32768 | 32768 | Y + 42 | acc_z | 2 | R | RAM | -32768 | 32768 | Y + 44 | roll | 2 | R | RAM | 0 | 4096 | N + 46 | pitch | 2 | R | RAM | 0 | 4096 | N + 48 | yaw | 2 | R | RAM | 0 | 4096 | N + 50 | imu_control | 1 | RW | RAM | 0 | 255 | N + + \ No newline at end of file From c42dadf25f01bc7de48af9ab1d7c1ec9c5285b91 Mon Sep 17 00:00:00 2001 From: ROBOTIS-zerom Date: Fri, 21 Apr 2017 15:56:31 +0900 Subject: [PATCH 087/238] Update robotis_controller.cpp some logs are commented out. --- .../src/robotis_controller/robotis_controller.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/robotis_controller/src/robotis_controller/robotis_controller.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp index 966dbdc..ca6e374 100755 --- a/robotis_controller/src/robotis_controller/robotis_controller.cpp +++ b/robotis_controller/src/robotis_controller/robotis_controller.cpp @@ -61,7 +61,7 @@ void RobotisController::initializeSyncWrite() if (gazebo_mode_ == true) return; - ROS_INFO("FIRST BULKREAD"); + //ROS_INFO("FIRST BULKREAD"); for (auto& it : port_to_bulk_read_) it.second->txRxPacket(); for(auto& it : port_to_bulk_read_) @@ -72,7 +72,7 @@ void RobotisController::initializeSyncWrite() { if (++error_count > 10) { - ROS_ERROR("[RobotisController] bulk read fail!!"); + ROS_ERROR("[RobotisController] first bulk read fail!!"); exit(-1); } usleep(10 * 1000); @@ -80,7 +80,7 @@ void RobotisController::initializeSyncWrite() } while (result != COMM_SUCCESS); } init_pose_loaded_ = true; - ROS_INFO("FIRST BULKREAD END"); + //ROS_INFO("FIRST BULKREAD END"); // clear syncwrite param setting for (auto& it : port_to_sync_write_position_) From 40201e4a18353abf48b82b9201c458427a43246c Mon Sep 17 00:00:00 2001 From: Pyo Date: Mon, 24 Apr 2017 17:42:48 +0900 Subject: [PATCH 088/238] Version Update (0.2.2) --- robotis_controller/CHANGELOG.rst | 6 ++++++ robotis_controller/package.xml | 2 +- robotis_device/CHANGELOG.rst | 9 ++++++++- robotis_device/package.xml | 2 +- robotis_framework/CHANGELOG.rst | 7 +++++++ robotis_framework/package.xml | 2 +- robotis_framework_common/CHANGELOG.rst | 5 +++++ robotis_framework_common/package.xml | 2 +- 8 files changed, 30 insertions(+), 5 deletions(-) diff --git a/robotis_controller/CHANGELOG.rst b/robotis_controller/CHANGELOG.rst index 002d663..7868223 100755 --- a/robotis_controller/CHANGELOG.rst +++ b/robotis_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package robotis_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.2.2 (2017-04-24) +----------- +* updated robotis_controller.cpp +* changed to read control cycle from .robot file +* Contributors: Zerom + 0.2.1 (2016-11-23) ----------- * Merge the changes and update diff --git a/robotis_controller/package.xml b/robotis_controller/package.xml index e2368e3..2816dc7 100755 --- a/robotis_controller/package.xml +++ b/robotis_controller/package.xml @@ -1,7 +1,7 @@ robotis_controller - 0.2.1 + 0.2.2 The main package that controls THORMANG3. diff --git a/robotis_device/CHANGELOG.rst b/robotis_device/CHANGELOG.rst index 20b2638..c442622 100755 --- a/robotis_device/CHANGELOG.rst +++ b/robotis_device/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package robotis_device ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.2.2 (2017-04-24) +----------- +* added a deivce: OpenCR +* changed to read control cycle from .robot file +* Contributors: Zerom, Kayman + 0.2.1 (2016-11-23) ----------- * Merge the changes and update @@ -26,6 +32,7 @@ Changelog for package robotis_device 0.1.1 (2016-08-18) ----------- * updated the package information +* Contributors: Zerom 0.1.0 (2016-08-12) ----------- @@ -42,4 +49,4 @@ Changelog for package robotis_device ConvertRadian2Value / ConvertValue2Radian function bug fixed. * added code to support the gazebo simulator * renewal -* Contributors: ROBOTIS, ROBOTIS-zerom, pyo +* Contributors: Zerom diff --git a/robotis_device/package.xml b/robotis_device/package.xml index 516770d..5da3bd9 100755 --- a/robotis_device/package.xml +++ b/robotis_device/package.xml @@ -1,7 +1,7 @@ robotis_device - 0.2.1 + 0.2.2 The package that manages device information of ROBOTIS robots. This package is used when reading device information with the robot information file diff --git a/robotis_framework/CHANGELOG.rst b/robotis_framework/CHANGELOG.rst index 87f4509..dd7d008 100755 --- a/robotis_framework/CHANGELOG.rst +++ b/robotis_framework/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package robotis_framework ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.2.2 (2017-04-24) +----------- +* added a deivce: OpenCR +* updated robotis_controller.cpp +* changed to read control cycle from .robot file +* Contributors: Zerom, Kayman + 0.2.1 (2016-11-23) ----------- * Merge the changes and update diff --git a/robotis_framework/package.xml b/robotis_framework/package.xml index 44a7089..ffdf9c6 100755 --- a/robotis_framework/package.xml +++ b/robotis_framework/package.xml @@ -1,7 +1,7 @@ robotis_framework - 0.2.1 + 0.2.2 ROS packages for the robotis_framework (meta package) BSD Zerom diff --git a/robotis_framework_common/CHANGELOG.rst b/robotis_framework_common/CHANGELOG.rst index fb68bac..34d437c 100755 --- a/robotis_framework_common/CHANGELOG.rst +++ b/robotis_framework_common/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package robotis_framework_common ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.2.2 (2017-04-24) +----------- +* updated for other packages +* Contributors: Zerom, Kayman + 0.2.1 (2016-11-23) ----------- * Merge the changes and update diff --git a/robotis_framework_common/package.xml b/robotis_framework_common/package.xml index 8f0a6c7..7e80aa2 100755 --- a/robotis_framework_common/package.xml +++ b/robotis_framework_common/package.xml @@ -1,7 +1,7 @@ robotis_framework_common - 0.2.1 + 0.2.2 The package contains commonly used Headers for the ROBOTIS Framework. From 35d164614c7095b3acc9f2c8d355ea53ca128026 Mon Sep 17 00:00:00 2001 From: sch Date: Tue, 16 May 2017 14:47:26 +0900 Subject: [PATCH 089/238] cmake update for ros install --- robotis_framework_common/CMakeLists.txt | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/robotis_framework_common/CMakeLists.txt b/robotis_framework_common/CMakeLists.txt index d6ebb1c..ed263f9 100755 --- a/robotis_framework_common/CMakeLists.txt +++ b/robotis_framework_common/CMakeLists.txt @@ -46,6 +46,12 @@ set_target_properties(${PROJECT_NAME} PROPERTIES LINKER_LANGUAGE CXX) ################################################################################ # Install ################################################################################ +install(TARGETS robotis_framework_common + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} ) From 8e8e024c123f132f5ba88a4e1d05f876c15c4207 Mon Sep 17 00:00:00 2001 From: Pyo Date: Tue, 23 May 2017 19:32:46 +0900 Subject: [PATCH 090/238] version update (0.2.3) --- robotis_controller/CHANGELOG.rst | 5 +++++ robotis_controller/package.xml | 2 +- robotis_device/CHANGELOG.rst | 5 +++++ robotis_device/package.xml | 2 +- robotis_framework/CHANGELOG.rst | 5 +++++ robotis_framework/package.xml | 2 +- robotis_framework_common/CHANGELOG.rst | 5 +++++ robotis_framework_common/package.xml | 2 +- 8 files changed, 24 insertions(+), 4 deletions(-) diff --git a/robotis_controller/CHANGELOG.rst b/robotis_controller/CHANGELOG.rst index 7868223..192af5e 100755 --- a/robotis_controller/CHANGELOG.rst +++ b/robotis_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package robotis_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.2.3 (2017-05-23) +----------- +* updated the cmake file for ros install +* Contributors: SCH + 0.2.2 (2017-04-24) ----------- * updated robotis_controller.cpp diff --git a/robotis_controller/package.xml b/robotis_controller/package.xml index 2816dc7..3d17748 100755 --- a/robotis_controller/package.xml +++ b/robotis_controller/package.xml @@ -1,7 +1,7 @@ robotis_controller - 0.2.2 + 0.2.3 The main package that controls THORMANG3. diff --git a/robotis_device/CHANGELOG.rst b/robotis_device/CHANGELOG.rst index c442622..262244a 100755 --- a/robotis_device/CHANGELOG.rst +++ b/robotis_device/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package robotis_device ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.2.3 (2017-05-23) +----------- +* updated the cmake file for ros install +* Contributors: SCH + 0.2.2 (2017-04-24) ----------- * added a deivce: OpenCR diff --git a/robotis_device/package.xml b/robotis_device/package.xml index 5da3bd9..c2a76a5 100755 --- a/robotis_device/package.xml +++ b/robotis_device/package.xml @@ -1,7 +1,7 @@ robotis_device - 0.2.2 + 0.2.3 The package that manages device information of ROBOTIS robots. This package is used when reading device information with the robot information file diff --git a/robotis_framework/CHANGELOG.rst b/robotis_framework/CHANGELOG.rst index dd7d008..48b41c3 100755 --- a/robotis_framework/CHANGELOG.rst +++ b/robotis_framework/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package robotis_framework ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.2.3 (2017-05-23) +----------- +* updated the cmake file for ros install +* Contributors: SCH + 0.2.2 (2017-04-24) ----------- * added a deivce: OpenCR diff --git a/robotis_framework/package.xml b/robotis_framework/package.xml index ffdf9c6..778b3b0 100755 --- a/robotis_framework/package.xml +++ b/robotis_framework/package.xml @@ -1,7 +1,7 @@ robotis_framework - 0.2.2 + 0.2.3 ROS packages for the robotis_framework (meta package) BSD Zerom diff --git a/robotis_framework_common/CHANGELOG.rst b/robotis_framework_common/CHANGELOG.rst index 34d437c..a96464d 100755 --- a/robotis_framework_common/CHANGELOG.rst +++ b/robotis_framework_common/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package robotis_framework_common ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.2.3 (2017-05-23) +----------- +* updated the cmake file for ros install +* Contributors: SCH + 0.2.2 (2017-04-24) ----------- * updated for other packages diff --git a/robotis_framework_common/package.xml b/robotis_framework_common/package.xml index 7e80aa2..00cd197 100755 --- a/robotis_framework_common/package.xml +++ b/robotis_framework_common/package.xml @@ -1,7 +1,7 @@ robotis_framework_common - 0.2.2 + 0.2.3 The package contains commonly used Headers for the ROBOTIS Framework. From 46a5b05040a051271b7a871e7e12ac631e59a4a5 Mon Sep 17 00:00:00 2001 From: sch Date: Wed, 7 Jun 2017 10:43:04 +0900 Subject: [PATCH 091/238] cmake_modules added in package.xml --- robotis_controller/package.xml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/robotis_controller/package.xml b/robotis_controller/package.xml index 3d17748..2af74c1 100755 --- a/robotis_controller/package.xml +++ b/robotis_controller/package.xml @@ -20,6 +20,7 @@ robotis_device robotis_controller_msgs robotis_framework_common + cmake_modules roscpp roslib std_msgs @@ -28,5 +29,6 @@ robotis_device robotis_controller_msgs robotis_framework_common + cmake_modules From 052637ee28a3b359d70cb01263dcbf4049bb18d6 Mon Sep 17 00:00:00 2001 From: Pyo Date: Wed, 7 Jun 2017 13:30:51 +0900 Subject: [PATCH 092/238] updated the CHANGELOG and Version to release binary packages --- robotis_controller/CHANGELOG.rst | 5 +++++ robotis_controller/package.xml | 2 +- robotis_device/CHANGELOG.rst | 4 ++++ robotis_device/package.xml | 2 +- robotis_framework/CHANGELOG.rst | 5 +++++ robotis_framework/package.xml | 2 +- robotis_framework_common/CHANGELOG.rst | 4 ++++ robotis_framework_common/package.xml | 2 +- 8 files changed, 22 insertions(+), 4 deletions(-) diff --git a/robotis_controller/CHANGELOG.rst b/robotis_controller/CHANGELOG.rst index 192af5e..9b68a85 100755 --- a/robotis_controller/CHANGELOG.rst +++ b/robotis_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package robotis_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.2.4 (2017-06-07) +----------- +* added cmake_modules in package.xml +* Contributors: SCH + 0.2.3 (2017-05-23) ----------- * updated the cmake file for ros install diff --git a/robotis_controller/package.xml b/robotis_controller/package.xml index 2af74c1..dcff20e 100755 --- a/robotis_controller/package.xml +++ b/robotis_controller/package.xml @@ -1,7 +1,7 @@ robotis_controller - 0.2.3 + 0.2.4 The main package that controls THORMANG3. diff --git a/robotis_device/CHANGELOG.rst b/robotis_device/CHANGELOG.rst index 262244a..ca19348 100755 --- a/robotis_device/CHANGELOG.rst +++ b/robotis_device/CHANGELOG.rst @@ -2,6 +2,10 @@ Changelog for package robotis_device ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.2.4 (2017-06-07) +----------- +* none + 0.2.3 (2017-05-23) ----------- * updated the cmake file for ros install diff --git a/robotis_device/package.xml b/robotis_device/package.xml index c2a76a5..b4f747c 100755 --- a/robotis_device/package.xml +++ b/robotis_device/package.xml @@ -1,7 +1,7 @@ robotis_device - 0.2.3 + 0.2.4 The package that manages device information of ROBOTIS robots. This package is used when reading device information with the robot information file diff --git a/robotis_framework/CHANGELOG.rst b/robotis_framework/CHANGELOG.rst index 48b41c3..2af3823 100755 --- a/robotis_framework/CHANGELOG.rst +++ b/robotis_framework/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package robotis_framework ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.2.4 (2017-06-07) +----------- +* added cmake_modules in package.xml +* Contributors: SCH + 0.2.3 (2017-05-23) ----------- * updated the cmake file for ros install diff --git a/robotis_framework/package.xml b/robotis_framework/package.xml index 778b3b0..60a8095 100755 --- a/robotis_framework/package.xml +++ b/robotis_framework/package.xml @@ -1,7 +1,7 @@ robotis_framework - 0.2.3 + 0.2.4 ROS packages for the robotis_framework (meta package) BSD Zerom diff --git a/robotis_framework_common/CHANGELOG.rst b/robotis_framework_common/CHANGELOG.rst index a96464d..9ddaab0 100755 --- a/robotis_framework_common/CHANGELOG.rst +++ b/robotis_framework_common/CHANGELOG.rst @@ -2,6 +2,10 @@ Changelog for package robotis_framework_common ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.2.4 (2017-06-07) +----------- +* none + 0.2.3 (2017-05-23) ----------- * updated the cmake file for ros install diff --git a/robotis_framework_common/package.xml b/robotis_framework_common/package.xml index 00cd197..5ac50b8 100755 --- a/robotis_framework_common/package.xml +++ b/robotis_framework_common/package.xml @@ -1,7 +1,7 @@ robotis_framework_common - 0.2.3 + 0.2.4 The package contains commonly used Headers for the ROBOTIS Framework. From 1c1672d0c4c6be15e4b3030241b3999deef490c3 Mon Sep 17 00:00:00 2001 From: SCH Date: Fri, 9 Jun 2017 09:36:45 +0900 Subject: [PATCH 093/238] update for yaml-cpp dependencies --- robotis_controller/CMakeLists.txt | 7 ++++++- robotis_controller/package.xml | 2 ++ 2 files changed, 8 insertions(+), 1 deletion(-) diff --git a/robotis_controller/CMakeLists.txt b/robotis_controller/CMakeLists.txt index 6e1bbb3..491f965 100755 --- a/robotis_controller/CMakeLists.txt +++ b/robotis_controller/CMakeLists.txt @@ -19,6 +19,10 @@ find_package(catkin REQUIRED COMPONENTS cmake_modules ) +set(ENV{PKG_CONFIG_PATH} "$ENV{PKG_CONFIG_PATH}:${CATKIN_DEVEL_PREFIX}/lib/pkgconfig") +find_package(PkgConfig) +pkg_check_modules(yaml_cpp yaml-cpp REQUIRED) + ################################################################################ # Declare ROS messages, services and actions ################################################################################ @@ -42,11 +46,12 @@ catkin_package( include_directories( include ${catkin_INCLUDE_DIRS} + ${yaml_cpp_INCLUDE_DIRS} ) add_library(robotis_controller src/robotis_controller/robotis_controller.cpp) add_dependencies(robotis_controller ${catkin_EXPORTED_TARGETS}) -target_link_libraries(robotis_controller yaml-cpp ${catkin_LIBRARIES}) +target_link_libraries(robotis_controller ${yaml_cpp_LIBRARIES} ${catkin_LIBRARIES}) ################################################################################ # Install diff --git a/robotis_controller/package.xml b/robotis_controller/package.xml index dcff20e..72a1c9a 100755 --- a/robotis_controller/package.xml +++ b/robotis_controller/package.xml @@ -21,6 +21,7 @@ robotis_controller_msgs robotis_framework_common cmake_modules + yaml-cpp roscpp roslib std_msgs @@ -30,5 +31,6 @@ robotis_controller_msgs robotis_framework_common cmake_modules + yaml-cpp From f08d522e0bee32daceea23aebd82824a7c638c23 Mon Sep 17 00:00:00 2001 From: Pyo Date: Fri, 9 Jun 2017 11:45:42 +0900 Subject: [PATCH 094/238] updated the CHANGELOG and Version to release binary packages --- robotis_controller/CHANGELOG.rst | 5 +++++ robotis_controller/package.xml | 2 +- robotis_device/CHANGELOG.rst | 4 ++++ robotis_device/package.xml | 2 +- robotis_framework/CHANGELOG.rst | 5 +++++ robotis_framework/package.xml | 2 +- robotis_framework_common/CHANGELOG.rst | 4 ++++ robotis_framework_common/package.xml | 2 +- 8 files changed, 22 insertions(+), 4 deletions(-) diff --git a/robotis_controller/CHANGELOG.rst b/robotis_controller/CHANGELOG.rst index 9b68a85..5a4eb37 100755 --- a/robotis_controller/CHANGELOG.rst +++ b/robotis_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package robotis_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.2.5 (2017-06-09) +----------- +* updated for yaml-cpp dependencies +* Contributors: SCH + 0.2.4 (2017-06-07) ----------- * added cmake_modules in package.xml diff --git a/robotis_controller/package.xml b/robotis_controller/package.xml index 72a1c9a..3ba70e6 100755 --- a/robotis_controller/package.xml +++ b/robotis_controller/package.xml @@ -1,7 +1,7 @@ robotis_controller - 0.2.4 + 0.2.5 The main package that controls THORMANG3. diff --git a/robotis_device/CHANGELOG.rst b/robotis_device/CHANGELOG.rst index ca19348..8a0f671 100755 --- a/robotis_device/CHANGELOG.rst +++ b/robotis_device/CHANGELOG.rst @@ -2,6 +2,10 @@ Changelog for package robotis_device ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.2.5 (2017-06-09) +----------- +* none + 0.2.4 (2017-06-07) ----------- * none diff --git a/robotis_device/package.xml b/robotis_device/package.xml index b4f747c..00fd5c1 100755 --- a/robotis_device/package.xml +++ b/robotis_device/package.xml @@ -1,7 +1,7 @@ robotis_device - 0.2.4 + 0.2.5 The package that manages device information of ROBOTIS robots. This package is used when reading device information with the robot information file diff --git a/robotis_framework/CHANGELOG.rst b/robotis_framework/CHANGELOG.rst index 2af3823..59cfa8c 100755 --- a/robotis_framework/CHANGELOG.rst +++ b/robotis_framework/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package robotis_framework ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.2.5 (2017-06-09) +----------- +* updated for yaml-cpp dependencies (robotis_controller) +* Contributors: SCH + 0.2.4 (2017-06-07) ----------- * added cmake_modules in package.xml diff --git a/robotis_framework/package.xml b/robotis_framework/package.xml index 60a8095..5a661bc 100755 --- a/robotis_framework/package.xml +++ b/robotis_framework/package.xml @@ -1,7 +1,7 @@ robotis_framework - 0.2.4 + 0.2.5 ROS packages for the robotis_framework (meta package) BSD Zerom diff --git a/robotis_framework_common/CHANGELOG.rst b/robotis_framework_common/CHANGELOG.rst index 9ddaab0..a1adecf 100755 --- a/robotis_framework_common/CHANGELOG.rst +++ b/robotis_framework_common/CHANGELOG.rst @@ -2,6 +2,10 @@ Changelog for package robotis_framework_common ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.2.5 (2017-06-09) +----------- +* none + 0.2.4 (2017-06-07) ----------- * none diff --git a/robotis_framework_common/package.xml b/robotis_framework_common/package.xml index 5ac50b8..3b6194b 100755 --- a/robotis_framework_common/package.xml +++ b/robotis_framework_common/package.xml @@ -1,7 +1,7 @@ robotis_framework_common - 0.2.4 + 0.2.5 The package contains commonly used Headers for the ROBOTIS Framework. From ac3dc6d9eb7be820dbb9816c70c6c72ea15cfc19 Mon Sep 17 00:00:00 2001 From: zerom Date: Tue, 11 Jul 2017 13:26:10 +0900 Subject: [PATCH 095/238] - OpenCR control table item name changed. (torque_enable -> dynamixel_power) - fixed to not update update_time_stamp_ if bulk read fails. --- .../robotis_controller/robotis_controller.cpp | 20 ++++++++++++------- robotis_device/devices/sensor/OPEN-CR.device | 2 +- 2 files changed, 14 insertions(+), 8 deletions(-) mode change 100644 => 100755 robotis_device/devices/sensor/OPEN-CR.device diff --git a/robotis_controller/src/robotis_controller/robotis_controller.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp index 966dbdc..c494cd0 100755 --- a/robotis_controller/src/robotis_controller/robotis_controller.cpp +++ b/robotis_controller/src/robotis_controller/robotis_controller.cpp @@ -61,7 +61,7 @@ void RobotisController::initializeSyncWrite() if (gazebo_mode_ == true) return; - ROS_INFO("FIRST BULKREAD"); + //ROS_INFO("FIRST BULKREAD"); for (auto& it : port_to_bulk_read_) it.second->txRxPacket(); for(auto& it : port_to_bulk_read_) @@ -72,7 +72,7 @@ void RobotisController::initializeSyncWrite() { if (++error_count > 10) { - ROS_ERROR("[RobotisController] bulk read fail!!"); + ROS_ERROR("[RobotisController] first bulk read fail!!"); exit(-1); } usleep(10 * 1000); @@ -80,7 +80,7 @@ void RobotisController::initializeSyncWrite() } while (result != COMM_SUCCESS); } init_pose_loaded_ = true; - ROS_INFO("FIRST BULKREAD END"); + //ROS_INFO("FIRST BULKREAD END"); // clear syncwrite param setting for (auto& it : port_to_sync_write_position_) @@ -911,12 +911,14 @@ void RobotisController::process() if (dxl->bulk_read_items_.size() > 0) { - uint32_t data = 0; + bool updated = false; + uint32_t data = 0; for (int i = 0; i < dxl->bulk_read_items_.size(); i++) { ControlTableItem *item = dxl->bulk_read_items_[i]; if (port_to_bulk_read_[port_name]->isAvailable(dxl->id_, item->address_, item->data_length_) == true) { + updated = true; data = port_to_bulk_read_[port_name]->getData(dxl->id_, item->address_, item->data_length_); // change dxl_state @@ -938,7 +940,8 @@ void RobotisController::process() } // -> update time stamp to Robot->dxls[]->dynamixel_state.update_time_stamp - dxl->dxl_state_->update_time_stamp_ = TimeStamp(present_state.header.stamp.sec, present_state.header.stamp.nsec); + if (updated == true) + dxl->dxl_state_->update_time_stamp_ = TimeStamp(present_state.header.stamp.sec, present_state.header.stamp.nsec); } } } @@ -954,12 +957,14 @@ void RobotisController::process() if (sensor->bulk_read_items_.size() > 0) { - uint32_t data = 0; + bool updated = false; + uint32_t data = 0; for (int i = 0; i < sensor->bulk_read_items_.size(); i++) { ControlTableItem *item = sensor->bulk_read_items_[i]; if (port_to_bulk_read_[port_name]->isAvailable(sensor->id_, item->address_, item->data_length_) == true) { + updated = true; data = port_to_bulk_read_[port_name]->getData(sensor->id_, item->address_, item->data_length_); // change sensor_state @@ -968,7 +973,8 @@ void RobotisController::process() } // -> update time stamp to Robot->dxls[]->dynamixel_state.update_time_stamp - sensor->sensor_state_->update_time_stamp_ = TimeStamp(present_state.header.stamp.sec, present_state.header.stamp.nsec); + if (updated == true) + sensor->sensor_state_->update_time_stamp_ = TimeStamp(present_state.header.stamp.sec, present_state.header.stamp.nsec); } } } diff --git a/robotis_device/devices/sensor/OPEN-CR.device b/robotis_device/devices/sensor/OPEN-CR.device old mode 100644 new mode 100755 index 94b446e..71cee8e --- a/robotis_device/devices/sensor/OPEN-CR.device +++ b/robotis_device/devices/sensor/OPEN-CR.device @@ -10,7 +10,7 @@ device_type = sensor 4 | baudrate | 1 | RW | EEPROM | 0 | 252 | N 5 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N 16 | status_return_level | 1 | RW | EEPROM | 0 | 2 | N - 24 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 24 | dynamixel_power | 1 | RW | RAM | 0 | 1 | N 25 | LED | 1 | RW | RAM | 0 | 7 | N 26 | LED_RGB | 2 | RW | RAM | 0 | 32767 | N 28 | buzzer | 2 | RW | RAM | 0 | 65535 | N From ef1c16664af20654154a7711e64932cf85af6311 Mon Sep 17 00:00:00 2001 From: zerom Date: Tue, 11 Jul 2017 14:42:15 +0900 Subject: [PATCH 096/238] - unnecessary debug print removed. --- .../robotis_controller/robotis_controller.cpp | 19 ++++++++----------- 1 file changed, 8 insertions(+), 11 deletions(-) diff --git a/robotis_controller/src/robotis_controller/robotis_controller.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp index c494cd0..94f4ccb 100755 --- a/robotis_controller/src/robotis_controller/robotis_controller.cpp +++ b/robotis_controller/src/robotis_controller/robotis_controller.cpp @@ -1469,9 +1469,11 @@ void RobotisController::removeSensorModule(SensorModule *module) void RobotisController::writeControlTableCallback(const robotis_controller_msgs::WriteControlTable::ConstPtr &msg) { - fprintf(stderr, "[WriteControlTable] led control msg received\n"); Device *device = NULL; + if (DEBUG_PRINT) + fprintf(stderr, "[WriteControlTable] led control msg received\n"); + auto dev_it1 = robot_->dxls_.find(msg->joint_name); if(dev_it1 != robot_->dxls_.end()) { @@ -1490,7 +1492,7 @@ void RobotisController::writeControlTableCallback(const robotis_controller_msgs: return; } } - fprintf(stderr, " #1 "); + ControlTableItem *item = NULL; auto item_it = device->ctrl_table_.find(msg->start_item_name); if(item_it != device->ctrl_table_.end()) @@ -1503,7 +1505,6 @@ void RobotisController::writeControlTableCallback(const robotis_controller_msgs: return; } - fprintf(stderr, " #2 "); dynamixel::PortHandler *port = robot_->ports_[device->port_name_]; dynamixel::PacketHandler *packet_handler = dynamixel::PacketHandler::getPacketHandler(device->protocol_version_); @@ -1512,17 +1513,13 @@ void RobotisController::writeControlTableCallback(const robotis_controller_msgs: queue_mutex_.lock(); - direct_sync_write_.push_back(new dynamixel::GroupSyncWrite(port, packet_handler, item->address_, msg->data_length)); - - fprintf(stderr, " #3 \n"); - direct_sync_write_[direct_sync_write_.size() - 1]->addParam(device->id_, (uint8_t *)(msg->data.data())); - fprintf(stderr, "[WriteControlTable] %s -> %s : ", msg->joint_name.c_str(), msg->start_item_name.c_str()); - for (auto &dt : msg->data) - fprintf(stderr, "%02X ", dt); - fprintf(stderr, "\n"); +// fprintf(stderr, "[WriteControlTable] %s -> %s : ", msg->joint_name.c_str(), msg->start_item_name.c_str()); +// for (auto &dt : msg->data) +// fprintf(stderr, "%02X ", dt); +// fprintf(stderr, "\n"); queue_mutex_.unlock(); From 05f4a5e51c7fac1659fb0eaecf50de29bda8051e Mon Sep 17 00:00:00 2001 From: zerom Date: Wed, 9 Aug 2017 14:27:40 +0900 Subject: [PATCH 097/238] multi thread bug fixed. --- .../src/robotis_controller/robotis_controller.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/robotis_controller/src/robotis_controller/robotis_controller.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp index 94f4ccb..be70a4e 100755 --- a/robotis_controller/src/robotis_controller/robotis_controller.cpp +++ b/robotis_controller/src/robotis_controller/robotis_controller.cpp @@ -1501,7 +1501,7 @@ void RobotisController::writeControlTableCallback(const robotis_controller_msgs: } else { - ROS_WARN("WriteControlTable] Unknown item : %s", msg->start_item_name.c_str()); + ROS_WARN("[WriteControlTable] Unknown item : %s", msg->start_item_name.c_str()); return; } @@ -1569,6 +1569,8 @@ void RobotisController::syncWriteItemCallback(const robotis_controller_msgs::Syn if (item->access_type_ == Read) continue; + queue_mutex_.lock(); + int idx = 0; if (direct_sync_write_.size() == 0) { @@ -1604,6 +1606,8 @@ void RobotisController::syncWriteItemCallback(const robotis_controller_msgs::Syn } direct_sync_write_[idx]->addParam(device->id_, data); delete[] data; + + queue_mutex_.unlock(); } } From 27885eb900087ae675a46da018d31d33b570fc3e Mon Sep 17 00:00:00 2001 From: Pyo Date: Wed, 9 Aug 2017 15:31:48 +0900 Subject: [PATCH 098/238] updated the CHANGELOG and version to release binary packages --- LICENSE | 8 ++++---- robotis_controller/CHANGELOG.rst | 8 ++++++++ robotis_controller/package.xml | 2 +- robotis_device/CHANGELOG.rst | 6 ++++++ robotis_device/package.xml | 2 +- robotis_framework/CHANGELOG.rst | 8 ++++++++ robotis_framework/package.xml | 2 +- robotis_framework_common/CHANGELOG.rst | 4 ++++ robotis_framework_common/package.xml | 2 +- 9 files changed, 34 insertions(+), 8 deletions(-) diff --git a/LICENSE b/LICENSE index 5298325..8954985 100755 --- a/LICENSE +++ b/LICENSE @@ -1,8 +1,8 @@ Software License Agreement (BSD License) - -Copyright (c) 2014, ROBOTIS Inc. + +Copyright (c) 2014-2017, ROBOTIS CO., LTD. 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 @@ -13,7 +13,7 @@ modification, are permitted provided that the following conditions are met: * Neither the name of ROBOTIS 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 ROBOTIS "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. diff --git a/robotis_controller/CHANGELOG.rst b/robotis_controller/CHANGELOG.rst index 5a4eb37..9b4a8db 100755 --- a/robotis_controller/CHANGELOG.rst +++ b/robotis_controller/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package robotis_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.2.6 (2017-08-09) +----------- +* multi thread bug fixed. +* unnecessary debug print removed. +* OpenCR control table item name changed. (torque_enable -> dynamixel_power) +* fixed to not update update_time_stamp\_ if bulk read fails. +* Contributors: Zerom + 0.2.5 (2017-06-09) ----------- * updated for yaml-cpp dependencies diff --git a/robotis_controller/package.xml b/robotis_controller/package.xml index 3ba70e6..09a7d65 100755 --- a/robotis_controller/package.xml +++ b/robotis_controller/package.xml @@ -1,7 +1,7 @@ robotis_controller - 0.2.5 + 0.2.6 The main package that controls THORMANG3. diff --git a/robotis_device/CHANGELOG.rst b/robotis_device/CHANGELOG.rst index 8a0f671..eda74db 100755 --- a/robotis_device/CHANGELOG.rst +++ b/robotis_device/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package robotis_device ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.2.6 (2017-08-09) +----------- +* OpenCR control table item name changed. (torque_enable -> dynamixel_power) +* fixed to not update update_time_stamp\_ if bulk read fails. +* Contributors: Zerom + 0.2.5 (2017-06-09) ----------- * none diff --git a/robotis_device/package.xml b/robotis_device/package.xml index 00fd5c1..6ad1fa0 100755 --- a/robotis_device/package.xml +++ b/robotis_device/package.xml @@ -1,7 +1,7 @@ robotis_device - 0.2.5 + 0.2.6 The package that manages device information of ROBOTIS robots. This package is used when reading device information with the robot information file diff --git a/robotis_framework/CHANGELOG.rst b/robotis_framework/CHANGELOG.rst index 59cfa8c..173b194 100755 --- a/robotis_framework/CHANGELOG.rst +++ b/robotis_framework/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package robotis_framework ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.2.6 (2017-08-09) +----------- +* multi thread bug fixed. +* unnecessary debug print removed. +* OpenCR control table item name changed. (torque_enable -> dynamixel_power) +* fixed to not update update_time_stamp\_ if bulk read fails. +* Contributors: Zerom + 0.2.5 (2017-06-09) ----------- * updated for yaml-cpp dependencies (robotis_controller) diff --git a/robotis_framework/package.xml b/robotis_framework/package.xml index 5a661bc..e5fae21 100755 --- a/robotis_framework/package.xml +++ b/robotis_framework/package.xml @@ -1,7 +1,7 @@ robotis_framework - 0.2.5 + 0.2.6 ROS packages for the robotis_framework (meta package) BSD Zerom diff --git a/robotis_framework_common/CHANGELOG.rst b/robotis_framework_common/CHANGELOG.rst index a1adecf..af8081d 100755 --- a/robotis_framework_common/CHANGELOG.rst +++ b/robotis_framework_common/CHANGELOG.rst @@ -2,6 +2,10 @@ Changelog for package robotis_framework_common ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.2.6 (2017-08-09) +----------- +* none + 0.2.5 (2017-06-09) ----------- * none diff --git a/robotis_framework_common/package.xml b/robotis_framework_common/package.xml index 3b6194b..86e845d 100755 --- a/robotis_framework_common/package.xml +++ b/robotis_framework_common/package.xml @@ -1,7 +1,7 @@ robotis_framework_common - 0.2.5 + 0.2.6 The package contains commonly used Headers for the ROBOTIS Framework. From 21e9a81af0d61ea6fd5d7df675934939b9cafd38 Mon Sep 17 00:00:00 2001 From: SCH Date: Wed, 29 Nov 2017 13:51:25 +0900 Subject: [PATCH 099/238] - Modified to prevent duplicate indirect address write --- .../robotis_controller/robotis_controller.cpp | 20 +++++++++++++++---- 1 file changed, 16 insertions(+), 4 deletions(-) diff --git a/robotis_controller/src/robotis_controller/robotis_controller.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp index be70a4e..691df50 100755 --- a/robotis_controller/src/robotis_controller/robotis_controller.cpp +++ b/robotis_controller/src/robotis_controller/robotis_controller.cpp @@ -485,6 +485,8 @@ void RobotisController::initializeDevice(const std::string init_file_path) { if (dxl->bulk_read_items_.size() != 0) { + uint16_t data16 = 0; + bulkread_start_addr = dxl->bulk_read_items_[0]->address_; bulkread_data_length = 0; @@ -498,7 +500,11 @@ void RobotisController::initializeDevice(const std::string init_file_path) for (int l = 0; l < addr_leng; l++) { // ROS_WARN("[%12s] INDIR_ADDR: %d, ITEM_ADDR: %d", joint_name.c_str(), indirect_addr, dxl->ctrl_table[dxl->bulk_read_items[i]->item_name]->address + _l); - write2Byte(joint_name, indirect_addr, dxl->ctrl_table_[dxl->bulk_read_items_[i]->item_name_]->address_ + l); + read2Byte(joint_name, indirect_addr, &data16); + if (data16 != dxl->ctrl_table_[dxl->bulk_read_items_[i]->item_name_]->address_ + l) + { + write2Byte(joint_name, indirect_addr, dxl->ctrl_table_[dxl->bulk_read_items_[i]->item_name_]->address_ + l); + } indirect_addr += 2; } } @@ -552,6 +558,8 @@ void RobotisController::initializeDevice(const std::string init_file_path) { if (sensor->bulk_read_items_.size() != 0) { + uint16_t data16 = 0; + bulkread_start_addr = sensor->bulk_read_items_[0]->address_; bulkread_data_length = 0; @@ -565,9 +573,13 @@ void RobotisController::initializeDevice(const std::string init_file_path) for (int l = 0; l < addr_leng; l++) { // ROS_WARN("[%12s] INDIR_ADDR: %d, ITEM_ADDR: %d", sensor_name.c_str(), indirect_addr, sensor->ctrl_table[sensor->bulk_read_items[i]->item_name]->address + _l); - write2Byte(sensor_name, - indirect_addr, - sensor->ctrl_table_[sensor->bulk_read_items_[i]->item_name_]->address_ + l); + read2Byte(sensor_name, indirect_addr, &data16); + if (data16 != sensor->ctrl_table_[sensor->bulk_read_items_[i]->item_name_]->address_ + l) + { + write2Byte(sensor_name, + indirect_addr, + sensor->ctrl_table_[sensor->bulk_read_items_[i]->item_name_]->address_ + l); + } indirect_addr += 2; } } From 91d4452a71f128af1165075363fd5a4e5e2cd2ce Mon Sep 17 00:00:00 2001 From: SCH Date: Wed, 29 Nov 2017 15:34:20 +0900 Subject: [PATCH 100/238] - fixed a bug that occur when handling bulk read item that does not exist. --- robotis_device/src/robotis_device/robot.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/robotis_device/src/robotis_device/robot.cpp b/robotis_device/src/robotis_device/robot.cpp index 94d4133..bcbda3a 100755 --- a/robotis_device/src/robotis_device/robot.cpp +++ b/robotis_device/src/robotis_device/robot.cpp @@ -154,6 +154,13 @@ Robot::Robot(std::string robot_file_path, std::string dev_desc_dir_path) uint16_t indirect_data_addr = dxl->ctrl_table_[INDIRECT_DATA_1]->address_; for (int _i = 0; _i < sub_tokens.size(); _i++) { + std::map::iterator bulkread_it = dxl->ctrl_table_.find(sub_tokens[_i]); + if(bulkread_it == dxl->ctrl_table_.end()) + { + fprintf(stderr, "\n ##### BULK READ ITEM [ %s ] NOT FOUND!! #####\n\n", sub_tokens[_i].c_str()); + continue; + } + dxl->bulk_read_items_.push_back(new ControlTableItem()); ControlTableItem *dest_item = dxl->bulk_read_items_[_i]; ControlTableItem *src_item = dxl->ctrl_table_[sub_tokens[_i]]; From 0076da8a98737272f8a8147cc8a493f6b145eb12 Mon Sep 17 00:00:00 2001 From: ROBOTIS-zerom Date: Thu, 30 Nov 2017 17:14:28 +0900 Subject: [PATCH 101/238] - modified to prevent duplicate indirect address write - fixed a bug that occure when handling bulk read item that does not exist. --- .../robotis_controller/robotis_controller.cpp | 20 +++++++++++++++---- robotis_device/src/robotis_device/robot.cpp | 7 +++++++ 2 files changed, 23 insertions(+), 4 deletions(-) diff --git a/robotis_controller/src/robotis_controller/robotis_controller.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp index be70a4e..691df50 100755 --- a/robotis_controller/src/robotis_controller/robotis_controller.cpp +++ b/robotis_controller/src/robotis_controller/robotis_controller.cpp @@ -485,6 +485,8 @@ void RobotisController::initializeDevice(const std::string init_file_path) { if (dxl->bulk_read_items_.size() != 0) { + uint16_t data16 = 0; + bulkread_start_addr = dxl->bulk_read_items_[0]->address_; bulkread_data_length = 0; @@ -498,7 +500,11 @@ void RobotisController::initializeDevice(const std::string init_file_path) for (int l = 0; l < addr_leng; l++) { // ROS_WARN("[%12s] INDIR_ADDR: %d, ITEM_ADDR: %d", joint_name.c_str(), indirect_addr, dxl->ctrl_table[dxl->bulk_read_items[i]->item_name]->address + _l); - write2Byte(joint_name, indirect_addr, dxl->ctrl_table_[dxl->bulk_read_items_[i]->item_name_]->address_ + l); + read2Byte(joint_name, indirect_addr, &data16); + if (data16 != dxl->ctrl_table_[dxl->bulk_read_items_[i]->item_name_]->address_ + l) + { + write2Byte(joint_name, indirect_addr, dxl->ctrl_table_[dxl->bulk_read_items_[i]->item_name_]->address_ + l); + } indirect_addr += 2; } } @@ -552,6 +558,8 @@ void RobotisController::initializeDevice(const std::string init_file_path) { if (sensor->bulk_read_items_.size() != 0) { + uint16_t data16 = 0; + bulkread_start_addr = sensor->bulk_read_items_[0]->address_; bulkread_data_length = 0; @@ -565,9 +573,13 @@ void RobotisController::initializeDevice(const std::string init_file_path) for (int l = 0; l < addr_leng; l++) { // ROS_WARN("[%12s] INDIR_ADDR: %d, ITEM_ADDR: %d", sensor_name.c_str(), indirect_addr, sensor->ctrl_table[sensor->bulk_read_items[i]->item_name]->address + _l); - write2Byte(sensor_name, - indirect_addr, - sensor->ctrl_table_[sensor->bulk_read_items_[i]->item_name_]->address_ + l); + read2Byte(sensor_name, indirect_addr, &data16); + if (data16 != sensor->ctrl_table_[sensor->bulk_read_items_[i]->item_name_]->address_ + l) + { + write2Byte(sensor_name, + indirect_addr, + sensor->ctrl_table_[sensor->bulk_read_items_[i]->item_name_]->address_ + l); + } indirect_addr += 2; } } diff --git a/robotis_device/src/robotis_device/robot.cpp b/robotis_device/src/robotis_device/robot.cpp index 94d4133..bcbda3a 100755 --- a/robotis_device/src/robotis_device/robot.cpp +++ b/robotis_device/src/robotis_device/robot.cpp @@ -154,6 +154,13 @@ Robot::Robot(std::string robot_file_path, std::string dev_desc_dir_path) uint16_t indirect_data_addr = dxl->ctrl_table_[INDIRECT_DATA_1]->address_; for (int _i = 0; _i < sub_tokens.size(); _i++) { + std::map::iterator bulkread_it = dxl->ctrl_table_.find(sub_tokens[_i]); + if(bulkread_it == dxl->ctrl_table_.end()) + { + fprintf(stderr, "\n ##### BULK READ ITEM [ %s ] NOT FOUND!! #####\n\n", sub_tokens[_i].c_str()); + continue; + } + dxl->bulk_read_items_.push_back(new ControlTableItem()); ControlTableItem *dest_item = dxl->bulk_read_items_[_i]; ControlTableItem *src_item = dxl->ctrl_table_[sub_tokens[_i]]; From 6ad9f4da3f805e8f463a5587c269b5fe82cfd7cd Mon Sep 17 00:00:00 2001 From: Yoonseok Pyo Date: Fri, 2 Mar 2018 16:39:43 +0900 Subject: [PATCH 102/238] Create .travis.yml --- .travis.yml | 35 +++++++++++++++++++++++++++++++++++ 1 file changed, 35 insertions(+) create mode 100644 .travis.yml diff --git a/.travis.yml b/.travis.yml new file mode 100644 index 0000000..ee670e4 --- /dev/null +++ b/.travis.yml @@ -0,0 +1,35 @@ +# This config file for Travis CI utilizes ros-industrial/industrial_ci package. +# For more info for the package, see https://github.com/ros-industrial/industrial_ci/blob/master/README.rst + +dist: trusty +sudo: required +services: + - docker +language: generic +python: + - "2.7" +compiler: + - gcc +notifications: + email: + on_success: always + on_failure: always + recipients: + - pyo@robotis.com +env: + matrix: + - ROS_DISTRO="kinetic" ROS_REPOSITORY_PATH=http://packages.ros.org/ros/ubuntu USE_DEB=true + - ROS_DISTRO="kinetic" PRERELEASE=true PRERELEASE_DOWNSTREAM_DEPTH=0 +matrix: + allow_failures: + - env: ROS_DISTRO="kinetic" PRERELEASE=true PRERELEASE_DOWNSTREAM_DEPTH=0 +branches: + only: + - master + - develop + - kinetic-devel +install: + - git clone https://github.com/ros-industrial/industrial_ci.git .ci_config +script: + - source .ci_config/travis.sh + From 3c275dd05a60c3fdef4d1ea4c862b1dd4165d145 Mon Sep 17 00:00:00 2001 From: Yoonseok Pyo Date: Fri, 2 Mar 2018 16:39:43 +0900 Subject: [PATCH 103/238] Create .travis.yml --- .travis.yml | 35 +++++++++++++++++++++++++++++++++++ 1 file changed, 35 insertions(+) create mode 100644 .travis.yml diff --git a/.travis.yml b/.travis.yml new file mode 100644 index 0000000..ee670e4 --- /dev/null +++ b/.travis.yml @@ -0,0 +1,35 @@ +# This config file for Travis CI utilizes ros-industrial/industrial_ci package. +# For more info for the package, see https://github.com/ros-industrial/industrial_ci/blob/master/README.rst + +dist: trusty +sudo: required +services: + - docker +language: generic +python: + - "2.7" +compiler: + - gcc +notifications: + email: + on_success: always + on_failure: always + recipients: + - pyo@robotis.com +env: + matrix: + - ROS_DISTRO="kinetic" ROS_REPOSITORY_PATH=http://packages.ros.org/ros/ubuntu USE_DEB=true + - ROS_DISTRO="kinetic" PRERELEASE=true PRERELEASE_DOWNSTREAM_DEPTH=0 +matrix: + allow_failures: + - env: ROS_DISTRO="kinetic" PRERELEASE=true PRERELEASE_DOWNSTREAM_DEPTH=0 +branches: + only: + - master + - develop + - kinetic-devel +install: + - git clone https://github.com/ros-industrial/industrial_ci.git .ci_config +script: + - source .ci_config/travis.sh + From ee6f240f900bcaf99ad5d5cde8fc0407fae83b1f Mon Sep 17 00:00:00 2001 From: robotis Date: Wed, 7 Mar 2018 18:42:28 +0900 Subject: [PATCH 104/238] - changed all values read by bulk read are saved to dxl_state_->bulk_read_table_. --- .../src/robotis_controller/robotis_controller.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/robotis_controller/src/robotis_controller/robotis_controller.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp index 691df50..7a66038 100755 --- a/robotis_controller/src/robotis_controller/robotis_controller.cpp +++ b/robotis_controller/src/robotis_controller/robotis_controller.cpp @@ -499,7 +499,7 @@ void RobotisController::initializeDevice(const std::string init_file_path) bulkread_data_length += addr_leng; for (int l = 0; l < addr_leng; l++) { -// ROS_WARN("[%12s] INDIR_ADDR: %d, ITEM_ADDR: %d", joint_name.c_str(), indirect_addr, dxl->ctrl_table[dxl->bulk_read_items[i]->item_name]->address + _l); + //ROS_WARN("[%12s] INDIR_ADDR: %d, ITEM_ADDR: %d", joint_name.c_str(), indirect_addr, dxl->ctrl_table_[dxl->bulk_read_items_[i]->item_name_]->address_ + l); read2Byte(joint_name, indirect_addr, &data16); if (data16 != dxl->ctrl_table_[dxl->bulk_read_items_[i]->item_name_]->address_ + l) { @@ -946,8 +946,8 @@ void RobotisController::process() dxl->dxl_state_->goal_velocity_ = dxl->convertValue2Velocity(data); else if (dxl->goal_current_item_ != 0 && item->item_name_ == dxl->goal_current_item_->item_name_) dxl->dxl_state_->goal_torque_ = dxl->convertValue2Torque(data); - else - dxl->dxl_state_->bulk_read_table_[item->item_name_] = data; + + dxl->dxl_state_->bulk_read_table_[item->item_name_] = data; } } @@ -1166,8 +1166,8 @@ void RobotisController::process() dxl->dxl_state_->goal_velocity_ = dxl->convertValue2Velocity(data); else if (dxl->goal_current_item_ != 0 && item->item_name_ == dxl->goal_current_item_->item_name_) dxl->dxl_state_->goal_torque_ = dxl->convertValue2Torque(data); - else - dxl->dxl_state_->bulk_read_table_[item->item_name_] = data; + + dxl->dxl_state_->bulk_read_table_[item->item_name_] = data; } } @@ -1251,7 +1251,7 @@ void RobotisController::process() if (result_state == NULL) { - fprintf(stderr, "[%s] %s", (*module_it)->getModuleName().c_str(), joint_name.c_str()); + ROS_ERROR("[%s] %s ", (*module_it)->getModuleName().c_str(), joint_name.c_str()); continue; } From 5e9278ee51dc325e1be1659f2576ec5d07780b6d Mon Sep 17 00:00:00 2001 From: Kayman Date: Wed, 14 Mar 2018 15:16:16 +0900 Subject: [PATCH 105/238] added serivce for setting module --- .../robotis_controller/robotis_controller.h | 6 +- .../robotis_controller/robotis_controller.cpp | 55 ++++++++++++++++++- 2 files changed, 57 insertions(+), 4 deletions(-) diff --git a/robotis_controller/include/robotis_controller/robotis_controller.h b/robotis_controller/include/robotis_controller/robotis_controller.h index f811b24..4653aac 100755 --- a/robotis_controller/include/robotis_controller/robotis_controller.h +++ b/robotis_controller/include/robotis_controller/robotis_controller.h @@ -50,6 +50,8 @@ #include "robotis_controller_msgs/SyncWriteItem.h" #include "robotis_controller_msgs/JointCtrlModule.h" #include "robotis_controller_msgs/GetJointModule.h" +#include "robotis_controller_msgs/SetJointModule.h" +#include "robotis_controller_msgs/SetModule.h" #include "robotis_device/robot.h" #include "robotis_framework_common/motion_module.h" @@ -151,7 +153,9 @@ public: void setJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg); void setJointCtrlModuleCallback(const robotis_controller_msgs::JointCtrlModule::ConstPtr &msg); void setCtrlModuleCallback(const std_msgs::String::ConstPtr &msg); - bool getCtrlModuleCallback(robotis_controller_msgs::GetJointModule::Request &req, robotis_controller_msgs::GetJointModule::Response &res); + bool getJointCtrlModuleService(robotis_controller_msgs::GetJointModule::Request &req, robotis_controller_msgs::GetJointModule::Response &res); + bool setJointCtrlModuleService(robotis_controller_msgs::SetJointModule::Request &req, robotis_controller_msgs::SetJointModule::Response &res); + bool setCtrlModuleService(robotis_controller_msgs::SetModule::Request &req, robotis_controller_msgs::SetModule::Response &res); void gazeboJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg); diff --git a/robotis_controller/src/robotis_controller/robotis_controller.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp index 691df50..f53cb94 100755 --- a/robotis_controller/src/robotis_controller/robotis_controller.cpp +++ b/robotis_controller/src/robotis_controller/robotis_controller.cpp @@ -671,8 +671,12 @@ void RobotisController::msgQueueThread() } /* service */ - ros::ServiceServer joint_module_server = ros_node.advertiseService("/robotis/get_present_joint_ctrl_modules", - &RobotisController::getCtrlModuleCallback, this); + ros::ServiceServer get_joint_module_server = ros_node.advertiseService("/robotis/get_present_joint_ctrl_modules", + &RobotisController::getJointCtrlModuleService, this); + ros::ServiceServer set_joint_module_server = ros_node.advertiseService("/robotis/set_present_joint_ctrl_modules", + &RobotisController::setJointCtrlModuleService, this); + ros::ServiceServer set_module_server = ros_node.advertiseService("/robotis/set_present_ctrl_modules", + &RobotisController::setCtrlModuleService, this); ros::WallDuration duration(robot_->getControlCycle() / 1000.0); while(ros_node.ok()) @@ -1677,6 +1681,9 @@ void RobotisController::setJointStatesCallback(const sensor_msgs::JointState::Co void RobotisController::setCtrlModuleCallback(const std_msgs::String::ConstPtr &msg) { + if(set_module_thread_.joinable()) + set_module_thread_.join(); + std::string _module_name_to_set = msg->data; set_module_thread_ = boost::thread(boost::bind(&RobotisController::setCtrlModuleThread, this, _module_name_to_set)); @@ -1684,6 +1691,9 @@ void RobotisController::setCtrlModuleCallback(const std_msgs::String::ConstPtr & void RobotisController::setCtrlModule(std::string module_name) { + if(set_module_thread_.joinable()) + set_module_thread_.join(); + set_module_thread_ = boost::thread(boost::bind(&RobotisController::setCtrlModuleThread, this, module_name)); } void RobotisController::setJointCtrlModuleCallback(const robotis_controller_msgs::JointCtrlModule::ConstPtr &msg) @@ -1691,10 +1701,13 @@ void RobotisController::setJointCtrlModuleCallback(const robotis_controller_msgs if (msg->joint_name.size() != msg->module_name.size()) return; + if(set_module_thread_.joinable()) + set_module_thread_.join(); + set_module_thread_ = boost::thread(boost::bind(&RobotisController::setJointCtrlModuleThread, this, msg)); } -bool RobotisController::getCtrlModuleCallback(robotis_controller_msgs::GetJointModule::Request &req, +bool RobotisController::getJointCtrlModuleService(robotis_controller_msgs::GetJointModule::Request &req, robotis_controller_msgs::GetJointModule::Response &res) { for (unsigned int idx = 0; idx < req.joint_name.size(); idx++) @@ -1713,6 +1726,41 @@ bool RobotisController::getCtrlModuleCallback(robotis_controller_msgs::GetJointM return true; } +bool RobotisController::setJointCtrlModuleService(robotis_controller_msgs::SetJointModule::Request &req, robotis_controller_msgs::SetJointModule::Response &res) +{ + if(set_module_thread_.joinable()) + set_module_thread_.join(); + + robotis_controller_msgs::JointCtrlModule modules; + modules.joint_name = req.joint_name; + modules.module_name = req.module_name; + + robotis_controller_msgs::JointCtrlModule::ConstPtr msg_ptr(new robotis_controller_msgs::JointCtrlModule(modules)); + + if (modules.joint_name.size() != modules.module_name.size()) + return false; + + set_module_thread_ = boost::thread(boost::bind(&RobotisController::setJointCtrlModuleThread, this, msg_ptr)); + + set_module_thread_.join(); + + return true; +} + +bool RobotisController::setCtrlModuleService(robotis_controller_msgs::SetModule::Request &req, robotis_controller_msgs::SetModule::Response &res) +{ + if(set_module_thread_.joinable()) + set_module_thread_.join(); + + std::string _module_name_to_set = req.module_name; + + set_module_thread_ = boost::thread(boost::bind(&RobotisController::setCtrlModuleThread, this, _module_name_to_set)); + + set_module_thread_.join(); + + return true; +} + void RobotisController::setJointCtrlModuleThread(const robotis_controller_msgs::JointCtrlModule::ConstPtr &msg) { // stop module list @@ -1906,6 +1954,7 @@ void RobotisController::setJointCtrlModuleThread(const robotis_controller_msgs:: if(_current_module_msg.joint_name.size() == _current_module_msg.module_name.size()) current_module_pub_.publish(_current_module_msg); } + void RobotisController::setCtrlModuleThread(std::string ctrl_module) { // stop module From 82765b785510e8e40125f480ddc234dd3553f449 Mon Sep 17 00:00:00 2001 From: Pyo Date: Thu, 15 Mar 2018 08:59:43 +0900 Subject: [PATCH 106/238] modified the LICENSE and package information for release --- .travis.yml | 11 +-- LICENSE | 221 ++++++++++++++++++++++++++++++++++++++++++++++------ README.md | 11 ++- 3 files changed, 210 insertions(+), 33 deletions(-) diff --git a/.travis.yml b/.travis.yml index ee670e4..1145575 100644 --- a/.travis.yml +++ b/.travis.yml @@ -1,8 +1,8 @@ # This config file for Travis CI utilizes ros-industrial/industrial_ci package. # For more info for the package, see https://github.com/ros-industrial/industrial_ci/blob/master/README.rst -dist: trusty sudo: required +dist: trusty services: - docker language: generic @@ -18,11 +18,8 @@ notifications: - pyo@robotis.com env: matrix: - - ROS_DISTRO="kinetic" ROS_REPOSITORY_PATH=http://packages.ros.org/ros/ubuntu USE_DEB=true - - ROS_DISTRO="kinetic" PRERELEASE=true PRERELEASE_DOWNSTREAM_DEPTH=0 -matrix: - allow_failures: - - env: ROS_DISTRO="kinetic" PRERELEASE=true PRERELEASE_DOWNSTREAM_DEPTH=0 + - ROS_DISTRO=kinetic ROS_REPO=ros-shadow-fixed UPSTREAM_WORKSPACE=debian +# - ROS_DISTRO=kinetic ROS_REPO=ros-shadow-fixed UPSTREAM_WORKSPACE=debian OS_NAME=debian OS_CODE_NAME=jessie branches: only: - master @@ -32,4 +29,4 @@ install: - git clone https://github.com/ros-industrial/industrial_ci.git .ci_config script: - source .ci_config/travis.sh - + \ No newline at end of file diff --git a/LICENSE b/LICENSE index 8954985..c0ee812 100755 --- a/LICENSE +++ b/LICENSE @@ -1,26 +1,201 @@ -Software License Agreement (BSD License) + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ -Copyright (c) 2014-2017, ROBOTIS CO., LTD. -All rights reserved. + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION -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 ROBOTIS nor the names of its contributors may be - used to endorse or promote products derived from this software - without specific prior written permission. + 1. Definitions. -THIS SOFTWARE IS PROVIDED BY ROBOTIS "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 ROBOTIS 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. + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "{}" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright {yyyy} {name of copyright owner} + + 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. diff --git a/README.md b/README.md index 02d6b48..8c6b748 100755 --- a/README.md +++ b/README.md @@ -1,4 +1,9 @@ -# ROBOTIS-Framework -ROBOTIS Framework GIT REP MAIN +# robotis_framework -Documents : [ROBOTIS Framework WIKI](https://github.com/ROBOTIS-GIT/ROBOTIS-Documents/wiki/ROBOTIS-ROS-Framework-Documents) +[![GitHub version](https://badge.fury.io/gh/ROBOTIS-GIT%2FROBOTIS-Framework.svg)](https://badge.fury.io/gh/ROBOTIS-GIT%2FROBOTIS-Framework) [![Build Status](https://travis-ci.org/ROBOTIS-GIT/ROBOTIS-Framework.svg?branch=master)](https://travis-ci.org/ROBOTIS-GIT/ROBOTIS-Framework) + +ROS packages for the robotis_framework (meta package) + +# Documents +- ROS Wiki: http://wiki.ros.org/robotis_framework +- ROBOTIS e-Manual: http://emanual.robotis.com/docs/en/software/robotis_framework_packages/ From c5e9cf9414279ae1d0cc5b52ee11d427e4a1fb85 Mon Sep 17 00:00:00 2001 From: Pyo Date: Thu, 15 Mar 2018 08:59:43 +0900 Subject: [PATCH 107/238] modified the LICENSE and package information for release --- .travis.yml | 11 +-- LICENSE | 221 ++++++++++++++++++++++++++++++++++++++++++++++------ README.md | 11 ++- 3 files changed, 210 insertions(+), 33 deletions(-) diff --git a/.travis.yml b/.travis.yml index ee670e4..1145575 100644 --- a/.travis.yml +++ b/.travis.yml @@ -1,8 +1,8 @@ # This config file for Travis CI utilizes ros-industrial/industrial_ci package. # For more info for the package, see https://github.com/ros-industrial/industrial_ci/blob/master/README.rst -dist: trusty sudo: required +dist: trusty services: - docker language: generic @@ -18,11 +18,8 @@ notifications: - pyo@robotis.com env: matrix: - - ROS_DISTRO="kinetic" ROS_REPOSITORY_PATH=http://packages.ros.org/ros/ubuntu USE_DEB=true - - ROS_DISTRO="kinetic" PRERELEASE=true PRERELEASE_DOWNSTREAM_DEPTH=0 -matrix: - allow_failures: - - env: ROS_DISTRO="kinetic" PRERELEASE=true PRERELEASE_DOWNSTREAM_DEPTH=0 + - ROS_DISTRO=kinetic ROS_REPO=ros-shadow-fixed UPSTREAM_WORKSPACE=debian +# - ROS_DISTRO=kinetic ROS_REPO=ros-shadow-fixed UPSTREAM_WORKSPACE=debian OS_NAME=debian OS_CODE_NAME=jessie branches: only: - master @@ -32,4 +29,4 @@ install: - git clone https://github.com/ros-industrial/industrial_ci.git .ci_config script: - source .ci_config/travis.sh - + \ No newline at end of file diff --git a/LICENSE b/LICENSE index 8954985..c0ee812 100755 --- a/LICENSE +++ b/LICENSE @@ -1,26 +1,201 @@ -Software License Agreement (BSD License) + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ -Copyright (c) 2014-2017, ROBOTIS CO., LTD. -All rights reserved. + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION -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 ROBOTIS nor the names of its contributors may be - used to endorse or promote products derived from this software - without specific prior written permission. + 1. Definitions. -THIS SOFTWARE IS PROVIDED BY ROBOTIS "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 ROBOTIS 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. + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "{}" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright {yyyy} {name of copyright owner} + + 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. diff --git a/README.md b/README.md index 02d6b48..8c6b748 100755 --- a/README.md +++ b/README.md @@ -1,4 +1,9 @@ -# ROBOTIS-Framework -ROBOTIS Framework GIT REP MAIN +# robotis_framework -Documents : [ROBOTIS Framework WIKI](https://github.com/ROBOTIS-GIT/ROBOTIS-Documents/wiki/ROBOTIS-ROS-Framework-Documents) +[![GitHub version](https://badge.fury.io/gh/ROBOTIS-GIT%2FROBOTIS-Framework.svg)](https://badge.fury.io/gh/ROBOTIS-GIT%2FROBOTIS-Framework) [![Build Status](https://travis-ci.org/ROBOTIS-GIT/ROBOTIS-Framework.svg?branch=master)](https://travis-ci.org/ROBOTIS-GIT/ROBOTIS-Framework) + +ROS packages for the robotis_framework (meta package) + +# Documents +- ROS Wiki: http://wiki.ros.org/robotis_framework +- ROBOTIS e-Manual: http://emanual.robotis.com/docs/en/software/robotis_framework_packages/ From 74fd344c67105654f979bc6b68ed28d2484d2a6d Mon Sep 17 00:00:00 2001 From: Pyo Date: Thu, 15 Mar 2018 09:22:17 +0900 Subject: [PATCH 108/238] change the License and package format to version 2 --- robotis_controller/CMakeLists.txt | 11 ++--- .../robotis_controller/robotis_controller.h | 42 +++++++------------ robotis_controller/package.xml | 37 ++++++---------- .../robotis_controller/robotis_controller.cpp | 42 +++++++------------ robotis_device/CMakeLists.txt | 7 ++-- .../robotis_device/control_table_item.h | 42 +++++++------------ .../include/robotis_device/device.h | 42 +++++++------------ .../include/robotis_device/dynamixel.h | 42 +++++++------------ .../include/robotis_device/dynamixel_state.h | 42 +++++++------------ robotis_device/include/robotis_device/robot.h | 42 +++++++------------ .../include/robotis_device/sensor.h | 42 +++++++------------ .../include/robotis_device/sensor_state.h | 42 +++++++------------ .../include/robotis_device/time_stamp.h | 42 +++++++------------ robotis_device/package.xml | 12 ++---- .../src/robotis_device/dynamixel.cpp | 42 +++++++------------ robotis_device/src/robotis_device/robot.cpp | 42 +++++++------------ robotis_device/src/robotis_device/sensor.cpp | 42 +++++++------------ robotis_framework/package.xml | 14 ++++--- robotis_framework_common/CMakeLists.txt | 2 + .../robotis_framework_common/motion_module.h | 42 +++++++------------ .../robotis_framework_common/sensor_module.h | 42 +++++++------------ .../robotis_framework_common/singleton.h | 42 +++++++------------ robotis_framework_common/package.xml | 10 ++--- 23 files changed, 264 insertions(+), 501 deletions(-) diff --git a/robotis_controller/CMakeLists.txt b/robotis_controller/CMakeLists.txt index 491f965..9ffde36 100755 --- a/robotis_controller/CMakeLists.txt +++ b/robotis_controller/CMakeLists.txt @@ -12,16 +12,17 @@ set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}") find_package(catkin REQUIRED COMPONENTS roscpp roslib - sensor_msgs std_msgs + sensor_msgs robotis_controller_msgs + dynamixel_sdk + robotis_device robotis_framework_common cmake_modules ) -set(ENV{PKG_CONFIG_PATH} "$ENV{PKG_CONFIG_PATH}:${CATKIN_DEVEL_PREFIX}/lib/pkgconfig") -find_package(PkgConfig) -pkg_check_modules(yaml_cpp yaml-cpp REQUIRED) +find_package(PkgConfig REQUIRED) +pkg_check_modules(YAML_CPP REQUIRED yaml-cpp) ################################################################################ # Declare ROS messages, services and actions @@ -37,7 +38,7 @@ pkg_check_modules(yaml_cpp yaml-cpp REQUIRED) catkin_package( INCLUDE_DIRS include LIBRARIES robotis_controller - CATKIN_DEPENDS roscpp roslib sensor_msgs std_msgs robotis_framework_common + CATKIN_DEPENDS roscpp roslib std_msgs sensor_msgs robotis_controller_msgs dynamixel_sdk robotis_device robotis_framework_common cmake_modules ) ################################################################################ diff --git a/robotis_controller/include/robotis_controller/robotis_controller.h b/robotis_controller/include/robotis_controller/robotis_controller.h index f811b24..1f16099 100755 --- a/robotis_controller/include/robotis_controller/robotis_controller.h +++ b/robotis_controller/include/robotis_controller/robotis_controller.h @@ -1,32 +1,18 @@ /******************************************************************************* - * Copyright (c) 2016, ROBOTIS CO., LTD. - * 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 ROBOTIS 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. - *******************************************************************************/ +* Copyright 2018 ROBOTIS CO., LTD. +* +* 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. +*******************************************************************************/ /* * robotis_controller.h diff --git a/robotis_controller/package.xml b/robotis_controller/package.xml index 09a7d65..9960d78 100755 --- a/robotis_controller/package.xml +++ b/robotis_controller/package.xml @@ -1,36 +1,25 @@ - + robotis_controller 0.2.6 - The main package that controls THORMANG3. + robotis_controller package for ROBOTIS's platform like Manipulator-H, THORMANG and OP series - BSD + Apache 2.0 Zerom Pyo https://github.com/ROBOTIS-GIT/ROBOTIS-Framework/issues https://github.com/ROBOTIS-GIT/ROBOTIS-Framework http://wiki.ros.org/robotis_controller catkin - roscpp - roslib - std_msgs - sensor_msgs - dynamixel_sdk - robotis_device - robotis_controller_msgs - robotis_framework_common - cmake_modules - yaml-cpp - roscpp - roslib - std_msgs - sensor_msgs - dynamixel_sdk - robotis_device - robotis_controller_msgs - robotis_framework_common - cmake_modules - yaml-cpp - + roscpp + roslib + std_msgs + sensor_msgs + robotis_controller_msgs + dynamixel_sdk + robotis_device + robotis_framework_common + cmake_modules + yaml-cpp diff --git a/robotis_controller/src/robotis_controller/robotis_controller.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp index 7a66038..f234e29 100755 --- a/robotis_controller/src/robotis_controller/robotis_controller.cpp +++ b/robotis_controller/src/robotis_controller/robotis_controller.cpp @@ -1,32 +1,18 @@ /******************************************************************************* - * Copyright (c) 2016, ROBOTIS CO., LTD. - * 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 ROBOTIS 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. - *******************************************************************************/ +* Copyright 2018 ROBOTIS CO., LTD. +* +* 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. +*******************************************************************************/ /* * robotis_controller.cpp diff --git a/robotis_device/CMakeLists.txt b/robotis_device/CMakeLists.txt index 45a1fc8..3b981d3 100755 --- a/robotis_device/CMakeLists.txt +++ b/robotis_device/CMakeLists.txt @@ -9,7 +9,6 @@ project(robotis_device) ################################################################################ find_package(catkin REQUIRED COMPONENTS roscpp - rospy dynamixel_sdk ) @@ -27,7 +26,7 @@ find_package(catkin REQUIRED COMPONENTS catkin_package( INCLUDE_DIRS include LIBRARIES robotis_device - CATKIN_DEPENDS roscpp rospy dynamixel_sdk + CATKIN_DEPENDS roscpp dynamixel_sdk ) ################################################################################ @@ -59,8 +58,8 @@ install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} ) -install(DIRECTORY devices/ - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +install(DIRECTORY devices + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) ################################################################################ diff --git a/robotis_device/include/robotis_device/control_table_item.h b/robotis_device/include/robotis_device/control_table_item.h index aa09131..ed7b956 100755 --- a/robotis_device/include/robotis_device/control_table_item.h +++ b/robotis_device/include/robotis_device/control_table_item.h @@ -1,32 +1,18 @@ /******************************************************************************* - * Copyright (c) 2016, ROBOTIS CO., LTD. - * 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 ROBOTIS 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. - *******************************************************************************/ +* Copyright 2018 ROBOTIS CO., LTD. +* +* 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. +*******************************************************************************/ /* * control_table_item.h diff --git a/robotis_device/include/robotis_device/device.h b/robotis_device/include/robotis_device/device.h index a2e5a1c..b3b2936 100755 --- a/robotis_device/include/robotis_device/device.h +++ b/robotis_device/include/robotis_device/device.h @@ -1,32 +1,18 @@ /******************************************************************************* - * Copyright (c) 2016, ROBOTIS CO., LTD. - * 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 ROBOTIS 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. - *******************************************************************************/ +* Copyright 2018 ROBOTIS CO., LTD. +* +* 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. +*******************************************************************************/ /* * device.h diff --git a/robotis_device/include/robotis_device/dynamixel.h b/robotis_device/include/robotis_device/dynamixel.h index f9b60f7..43e8981 100755 --- a/robotis_device/include/robotis_device/dynamixel.h +++ b/robotis_device/include/robotis_device/dynamixel.h @@ -1,32 +1,18 @@ /******************************************************************************* - * Copyright (c) 2016, ROBOTIS CO., LTD. - * 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 ROBOTIS 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. - *******************************************************************************/ +* Copyright 2018 ROBOTIS CO., LTD. +* +* 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. +*******************************************************************************/ /* * dynamixel.h diff --git a/robotis_device/include/robotis_device/dynamixel_state.h b/robotis_device/include/robotis_device/dynamixel_state.h index 1651eb2..b287bb3 100755 --- a/robotis_device/include/robotis_device/dynamixel_state.h +++ b/robotis_device/include/robotis_device/dynamixel_state.h @@ -1,32 +1,18 @@ /******************************************************************************* - * Copyright (c) 2016, ROBOTIS CO., LTD. - * 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 ROBOTIS 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. - *******************************************************************************/ +* Copyright 2018 ROBOTIS CO., LTD. +* +* 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. +*******************************************************************************/ /* * dynamixel_state.h diff --git a/robotis_device/include/robotis_device/robot.h b/robotis_device/include/robotis_device/robot.h index 684fe6a..58b7a77 100755 --- a/robotis_device/include/robotis_device/robot.h +++ b/robotis_device/include/robotis_device/robot.h @@ -1,32 +1,18 @@ /******************************************************************************* - * Copyright (c) 2016, ROBOTIS CO., LTD. - * 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 ROBOTIS 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. - *******************************************************************************/ +* Copyright 2018 ROBOTIS CO., LTD. +* +* 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. +*******************************************************************************/ /* * robot.h diff --git a/robotis_device/include/robotis_device/sensor.h b/robotis_device/include/robotis_device/sensor.h index dda755f..dc98eaa 100755 --- a/robotis_device/include/robotis_device/sensor.h +++ b/robotis_device/include/robotis_device/sensor.h @@ -1,32 +1,18 @@ /******************************************************************************* - * Copyright (c) 2016, ROBOTIS CO., LTD. - * 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 ROBOTIS 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. - *******************************************************************************/ +* Copyright 2018 ROBOTIS CO., LTD. +* +* 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. +*******************************************************************************/ /* * sensor.h diff --git a/robotis_device/include/robotis_device/sensor_state.h b/robotis_device/include/robotis_device/sensor_state.h index 01ec952..2cfc98d 100755 --- a/robotis_device/include/robotis_device/sensor_state.h +++ b/robotis_device/include/robotis_device/sensor_state.h @@ -1,32 +1,18 @@ /******************************************************************************* - * Copyright (c) 2016, ROBOTIS CO., LTD. - * 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 ROBOTIS 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. - *******************************************************************************/ +* Copyright 2018 ROBOTIS CO., LTD. +* +* 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. +*******************************************************************************/ /* * sensor_state.h diff --git a/robotis_device/include/robotis_device/time_stamp.h b/robotis_device/include/robotis_device/time_stamp.h index 1dc7510..8662ad7 100755 --- a/robotis_device/include/robotis_device/time_stamp.h +++ b/robotis_device/include/robotis_device/time_stamp.h @@ -1,32 +1,18 @@ /******************************************************************************* - * Copyright (c) 2016, ROBOTIS CO., LTD. - * 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 ROBOTIS 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. - *******************************************************************************/ +* Copyright 2018 ROBOTIS CO., LTD. +* +* 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. +*******************************************************************************/ /* * time_stamp.h diff --git a/robotis_device/package.xml b/robotis_device/package.xml index 6ad1fa0..94290df 100755 --- a/robotis_device/package.xml +++ b/robotis_device/package.xml @@ -1,5 +1,5 @@ - + robotis_device 0.2.6 @@ -7,17 +7,13 @@ This package is used when reading device information with the robot information file from the robotis_controller package. - BSD + Apache 2.0 Zerom Pyo https://github.com/ROBOTIS-GIT/ROBOTIS-Framework/issues https://github.com/ROBOTIS-GIT/ROBOTIS-Framework http://wiki.ros.org/robotis_device catkin - roscpp - rospy - dynamixel_sdk - roscpp - rospy - dynamixel_sdk + roscpp + dynamixel_sdk diff --git a/robotis_device/src/robotis_device/dynamixel.cpp b/robotis_device/src/robotis_device/dynamixel.cpp index 16ef12e..500393c 100755 --- a/robotis_device/src/robotis_device/dynamixel.cpp +++ b/robotis_device/src/robotis_device/dynamixel.cpp @@ -1,32 +1,18 @@ /******************************************************************************* - * Copyright (c) 2016, ROBOTIS CO., LTD. - * 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 ROBOTIS 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. - *******************************************************************************/ +* Copyright 2018 ROBOTIS CO., LTD. +* +* 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. +*******************************************************************************/ /* * dynamixel.cpp diff --git a/robotis_device/src/robotis_device/robot.cpp b/robotis_device/src/robotis_device/robot.cpp index bcbda3a..6214d16 100755 --- a/robotis_device/src/robotis_device/robot.cpp +++ b/robotis_device/src/robotis_device/robot.cpp @@ -1,32 +1,18 @@ /******************************************************************************* - * Copyright (c) 2016, ROBOTIS CO., LTD. - * 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 ROBOTIS 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. - *******************************************************************************/ +* Copyright 2018 ROBOTIS CO., LTD. +* +* 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. +*******************************************************************************/ /* * robot.cpp diff --git a/robotis_device/src/robotis_device/sensor.cpp b/robotis_device/src/robotis_device/sensor.cpp index 3507342..ba7bf29 100755 --- a/robotis_device/src/robotis_device/sensor.cpp +++ b/robotis_device/src/robotis_device/sensor.cpp @@ -1,32 +1,18 @@ /******************************************************************************* - * Copyright (c) 2016, ROBOTIS CO., LTD. - * 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 ROBOTIS 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. - *******************************************************************************/ +* Copyright 2018 ROBOTIS CO., LTD. +* +* 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. +*******************************************************************************/ /* * sensor.cpp diff --git a/robotis_framework/package.xml b/robotis_framework/package.xml index e5fae21..0901721 100755 --- a/robotis_framework/package.xml +++ b/robotis_framework/package.xml @@ -1,17 +1,19 @@ - + robotis_framework 0.2.6 - ROS packages for the robotis_framework (meta package) - BSD + + ROS packages for the robotis_framework (meta package) + + Apache 2.0 Zerom Pyo https://github.com/ROBOTIS-GIT/ROBOTIS-Framework/issues https://github.com/ROBOTIS-GIT/ROBOTIS-Framework http://wiki.ros.org/robotis_framework catkin - robotis_framework_common - robotis_device - robotis_controller + robotis_framework_common + robotis_device + robotis_controller diff --git a/robotis_framework_common/CMakeLists.txt b/robotis_framework_common/CMakeLists.txt index ed263f9..ce7bbd4 100755 --- a/robotis_framework_common/CMakeLists.txt +++ b/robotis_framework_common/CMakeLists.txt @@ -36,11 +36,13 @@ include_directories( include ${catkin_INCLUDE_DIRS} ) + add_library(${PROJECT_NAME} include/${PROJECT_NAME}/motion_module.h include/${PROJECT_NAME}/sensor_module.h include/${PROJECT_NAME}/singleton.h ) + set_target_properties(${PROJECT_NAME} PROPERTIES LINKER_LANGUAGE CXX) ################################################################################ diff --git a/robotis_framework_common/include/robotis_framework_common/motion_module.h b/robotis_framework_common/include/robotis_framework_common/motion_module.h index e81bbc8..b18d858 100755 --- a/robotis_framework_common/include/robotis_framework_common/motion_module.h +++ b/robotis_framework_common/include/robotis_framework_common/motion_module.h @@ -1,32 +1,18 @@ /******************************************************************************* - * Copyright (c) 2016, ROBOTIS CO., LTD. - * 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 ROBOTIS 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. - *******************************************************************************/ +* Copyright 2018 ROBOTIS CO., LTD. +* +* 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. +*******************************************************************************/ /* * motion_module.h diff --git a/robotis_framework_common/include/robotis_framework_common/sensor_module.h b/robotis_framework_common/include/robotis_framework_common/sensor_module.h index 775acfd..356d14b 100755 --- a/robotis_framework_common/include/robotis_framework_common/sensor_module.h +++ b/robotis_framework_common/include/robotis_framework_common/sensor_module.h @@ -1,32 +1,18 @@ /******************************************************************************* - * Copyright (c) 2016, ROBOTIS CO., LTD. - * 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 ROBOTIS 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. - *******************************************************************************/ +* Copyright 2018 ROBOTIS CO., LTD. +* +* 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. +*******************************************************************************/ /* * sensor_module.h diff --git a/robotis_framework_common/include/robotis_framework_common/singleton.h b/robotis_framework_common/include/robotis_framework_common/singleton.h index b6338b2..992e2f3 100755 --- a/robotis_framework_common/include/robotis_framework_common/singleton.h +++ b/robotis_framework_common/include/robotis_framework_common/singleton.h @@ -1,32 +1,18 @@ /******************************************************************************* - * Copyright (c) 2016, ROBOTIS CO., LTD. - * 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 ROBOTIS 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. - *******************************************************************************/ +* Copyright 2018 ROBOTIS CO., LTD. +* +* 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. +*******************************************************************************/ /* * singleton.h diff --git a/robotis_framework_common/package.xml b/robotis_framework_common/package.xml index 86e845d..c22bffe 100755 --- a/robotis_framework_common/package.xml +++ b/robotis_framework_common/package.xml @@ -1,19 +1,17 @@ - + robotis_framework_common 0.2.6 The package contains commonly used Headers for the ROBOTIS Framework. - BSD + Apache 2.0 Zerom Pyo https://github.com/ROBOTIS-GIT/ROBOTIS-Framework/issues https://github.com/ROBOTIS-GIT/ROBOTIS-Framework http://wiki.ros.org/robotis_framework_common catkin - roscpp - robotis_device - roscpp - robotis_device + roscpp + robotis_device From 10172c2e34009f567fa0db13449f76c4f2605678 Mon Sep 17 00:00:00 2001 From: Pyo Date: Thu, 15 Mar 2018 09:35:18 +0900 Subject: [PATCH 109/238] updated the CHANGELOG and version to release binary packages --- robotis_controller/CHANGELOG.rst | 25 ++++++++++++++++--------- robotis_controller/package.xml | 3 ++- robotis_device/CHANGELOG.rst | 24 +++++++++++++++--------- robotis_device/package.xml | 3 ++- robotis_framework/CHANGELOG.rst | 26 +++++++++++++++++--------- robotis_framework/package.xml | 3 ++- robotis_framework_common/CHANGELOG.rst | 23 ++++++++++++++--------- robotis_framework_common/package.xml | 3 ++- 8 files changed, 70 insertions(+), 40 deletions(-) diff --git a/robotis_controller/CHANGELOG.rst b/robotis_controller/CHANGELOG.rst index 9b4a8db..d1aa7be 100755 --- a/robotis_controller/CHANGELOG.rst +++ b/robotis_controller/CHANGELOG.rst @@ -2,8 +2,15 @@ Changelog for package robotis_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.2.7 (2018-03-15) +------------------ +* changed the License and package format to version 2 +* changed all values read by bulk read are saved to dxl_state\_->bulk_read_table\_. +* Modified to prevent duplicate indirect address write +* Contributors: SCH, Zerom, Pyo + 0.2.6 (2017-08-09) ------------ +------------------ * multi thread bug fixed. * unnecessary debug print removed. * OpenCR control table item name changed. (torque_enable -> dynamixel_power) @@ -11,28 +18,28 @@ Changelog for package robotis_controller * Contributors: Zerom 0.2.5 (2017-06-09) ------------ +------------------ * updated for yaml-cpp dependencies * Contributors: SCH 0.2.4 (2017-06-07) ------------ +------------------ * added cmake_modules in package.xml * Contributors: SCH 0.2.3 (2017-05-23) ------------ +------------------ * updated the cmake file for ros install * Contributors: SCH 0.2.2 (2017-04-24) ------------ +------------------ * updated robotis_controller.cpp * changed to read control cycle from .robot file * Contributors: Zerom 0.2.1 (2016-11-23) ------------ +------------------ * Merge the changes and update * - Direct Control Mode bug fixed. * update @@ -50,7 +57,7 @@ Changelog for package robotis_controller * Contributors: Jay Song, Pyo, Zerom, SCH 0.2.0 (2016-08-31) ------------ +------------------ * bug fixed (position pid gain & velocity pid gain sync write). * added velocity_to_value_ratio to DXL Pro-H series. * changed some debug messages. @@ -73,11 +80,11 @@ Changelog for package robotis_controller * Contributors: Jay Song, Zerom, Pyo, SCH 0.1.1 (2016-08-18) ------------ +------------------ * updated the package information 0.1.0 (2016-08-12) ------------ +------------------ * first public release for Kinetic * modified the package information for release * develop branch -> master branch diff --git a/robotis_controller/package.xml b/robotis_controller/package.xml index 9960d78..9073883 100755 --- a/robotis_controller/package.xml +++ b/robotis_controller/package.xml @@ -1,12 +1,13 @@ robotis_controller - 0.2.6 + 0.2.7 robotis_controller package for ROBOTIS's platform like Manipulator-H, THORMANG and OP series Apache 2.0 Zerom + SCH Pyo https://github.com/ROBOTIS-GIT/ROBOTIS-Framework/issues https://github.com/ROBOTIS-GIT/ROBOTIS-Framework diff --git a/robotis_device/CHANGELOG.rst b/robotis_device/CHANGELOG.rst index eda74db..7330dc8 100755 --- a/robotis_device/CHANGELOG.rst +++ b/robotis_device/CHANGELOG.rst @@ -2,33 +2,39 @@ Changelog for package robotis_device ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.2.7 (2018-03-15) +------------------------- +* fixed a bug that occur when handling bulk read item that does not exist +* changed the License and package format to version 2 +* Contributors: SCH, Pyo + 0.2.6 (2017-08-09) ------------ +------------------ * OpenCR control table item name changed. (torque_enable -> dynamixel_power) * fixed to not update update_time_stamp\_ if bulk read fails. * Contributors: Zerom 0.2.5 (2017-06-09) ------------ +------------------ * none 0.2.4 (2017-06-07) ------------ +------------------ * none 0.2.3 (2017-05-23) ------------ +------------------ * updated the cmake file for ros install * Contributors: SCH 0.2.2 (2017-04-24) ------------ +------------------ * added a deivce: OpenCR * changed to read control cycle from .robot file * Contributors: Zerom, Kayman 0.2.1 (2016-11-23) ------------ +------------------ * Merge the changes and update * mode change debugging * - convertRadian2Value / convertValue2Radian : commented out the code that limits the maximum/minimum value. @@ -36,7 +42,7 @@ Changelog for package robotis_device * Contributors: Jay Song, Pyo, Zerom, SCH 0.2.0 (2016-08-31) ------------ +------------------ * bug fixed (position pid gain & velocity pid gain sync write). * added velocity_to_value_ratio to DXL Pro-H series. * added velocity p/i/d gain and position i/d gain sync_write code. @@ -49,12 +55,12 @@ Changelog for package robotis_device * Contributors: Zerom, Pyo 0.1.1 (2016-08-18) ------------ +------------------ * updated the package information * Contributors: Zerom 0.1.0 (2016-08-12) ------------ +------------------ * first public release for Kinetic * modified the package information for release * develop branch -> master branch diff --git a/robotis_device/package.xml b/robotis_device/package.xml index 94290df..c0d1d1e 100755 --- a/robotis_device/package.xml +++ b/robotis_device/package.xml @@ -1,7 +1,7 @@ robotis_device - 0.2.6 + 0.2.7 The package that manages device information of ROBOTIS robots. This package is used when reading device information with the robot information file @@ -9,6 +9,7 @@ Apache 2.0 Zerom + SCH Pyo https://github.com/ROBOTIS-GIT/ROBOTIS-Framework/issues https://github.com/ROBOTIS-GIT/ROBOTIS-Framework diff --git a/robotis_framework/CHANGELOG.rst b/robotis_framework/CHANGELOG.rst index 173b194..8f3d7f2 100755 --- a/robotis_framework/CHANGELOG.rst +++ b/robotis_framework/CHANGELOG.rst @@ -2,8 +2,16 @@ Changelog for package robotis_framework ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.2.7 (2018-03-15) +------------------ +* changed all values read by bulk read are saved to dxl_state\_->bulk_read_table\_. +* Modified to prevent duplicate indirect address write +* fixed a bug that occur when handling bulk read item that does not exist +* changed the License and package format to version 2 +* Contributors: SCH, Zerom, Pyo + 0.2.6 (2017-08-09) ------------ +------------------ * multi thread bug fixed. * unnecessary debug print removed. * OpenCR control table item name changed. (torque_enable -> dynamixel_power) @@ -11,39 +19,39 @@ Changelog for package robotis_framework * Contributors: Zerom 0.2.5 (2017-06-09) ------------ +------------------ * updated for yaml-cpp dependencies (robotis_controller) * Contributors: SCH 0.2.4 (2017-06-07) ------------ +------------------ * added cmake_modules in package.xml * Contributors: SCH 0.2.3 (2017-05-23) ------------ +------------------ * updated the cmake file for ros install * Contributors: SCH 0.2.2 (2017-04-24) ------------ +------------------ * added a deivce: OpenCR * updated robotis_controller.cpp * changed to read control cycle from .robot file * Contributors: Zerom, Kayman 0.2.1 (2016-11-23) ------------ +------------------ * Merge the changes and update 0.2.0 (2016-08-31) ------------ +------------------ * updated CHANGLOG.rst for minor release 0.1.1 (2016-08-18) ------------ +------------------ * updated the package information 0.1.0 (2016-08-12) ------------ +------------------ * make a meta-package diff --git a/robotis_framework/package.xml b/robotis_framework/package.xml index 0901721..4da2d0a 100755 --- a/robotis_framework/package.xml +++ b/robotis_framework/package.xml @@ -1,12 +1,13 @@ robotis_framework - 0.2.6 + 0.2.7 ROS packages for the robotis_framework (meta package) Apache 2.0 Zerom + SCH Pyo https://github.com/ROBOTIS-GIT/ROBOTIS-Framework/issues https://github.com/ROBOTIS-GIT/ROBOTIS-Framework diff --git a/robotis_framework_common/CHANGELOG.rst b/robotis_framework_common/CHANGELOG.rst index af8081d..1359706 100755 --- a/robotis_framework_common/CHANGELOG.rst +++ b/robotis_framework_common/CHANGELOG.rst @@ -2,48 +2,53 @@ Changelog for package robotis_framework_common ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.2.7 (2018-03-15) +------------------ +* change the License and package format to version 2 +* Contributors: Pyo + 0.2.6 (2017-08-09) ------------ +------------------ * none 0.2.5 (2017-06-09) ------------ +------------------ * none 0.2.4 (2017-06-07) ------------ +------------------ * none 0.2.3 (2017-05-23) ------------ +------------------ * updated the cmake file for ros install * Contributors: SCH 0.2.2 (2017-04-24) ------------ +------------------ * updated for other packages * Contributors: Zerom, Kayman 0.2.1 (2016-11-23) ------------ +------------------ * Merge the changes and update * - dependencies fixed. (Pull requests `#26 `_) * - modified dependency problem. * Contributors: Jay Song, Pyo, Zerom 0.2.0 (2016-08-31) ------------ +------------------ * updated CHANGLOG.rst for minor release * rename ControlMode(CurrentControl -> TorqueControl) * rename (port_to_sync_write_torque\_ -> port_to_sync_write_current\_) * Contributors: Zerom, Pyo 0.1.1 (2016-08-18) ------------ +------------------ * updated the package information 0.1.0 (2016-08-12) ------------ +------------------ * modified the package information for release * Setting the license to BSD. * add SensorState diff --git a/robotis_framework_common/package.xml b/robotis_framework_common/package.xml index c22bffe..7243698 100755 --- a/robotis_framework_common/package.xml +++ b/robotis_framework_common/package.xml @@ -1,12 +1,13 @@ robotis_framework_common - 0.2.6 + 0.2.7 The package contains commonly used Headers for the ROBOTIS Framework. Apache 2.0 Zerom + SCH Pyo https://github.com/ROBOTIS-GIT/ROBOTIS-Framework/issues https://github.com/ROBOTIS-GIT/ROBOTIS-Framework From 947c0ac1926bae41901f95ef1314873f1a5ac503 Mon Sep 17 00:00:00 2001 From: Zerom Date: Tue, 20 Mar 2018 14:02:14 +0900 Subject: [PATCH 110/238] - added RH-P12-RN.device file. - modified robotis_controller/CMakeLists.txt (yaml-cpp bug). --- .../devices/dynamixel/RH-P12-RN.device | 73 +++++++++++++++++++ 1 file changed, 73 insertions(+) create mode 100755 robotis_device/devices/dynamixel/RH-P12-RN.device diff --git a/robotis_device/devices/dynamixel/RH-P12-RN.device b/robotis_device/devices/dynamixel/RH-P12-RN.device new file mode 100755 index 0000000..aec9b67 --- /dev/null +++ b/robotis_device/devices/dynamixel/RH-P12-RN.device @@ -0,0 +1,73 @@ +[device info] +model_name = RH-P12-RN +device_type = dynamixel + +[type info] +value_of_0_radian_position = 0 +value_of_min_radian_position = 0 +value_of_max_radian_position = 740 +min_radian = 0 +max_radian = 1.150767 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_current +position_d_gain_item_name = position_d_gain +position_i_gain_item_name = position_i_gain +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = +velocity_p_gain_item_name = + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 30 | current_limit | 2 | RW | EEPROM | 0 | 32767 | N + 32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 562 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 590 | position_d_gain | 2 | RW | RAM | 0 | 2047 | N + 592 | position_i_gain | 2 | RW | RAM | 0 | 2047 | N + 594 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N + 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 604 | goal_current | 2 | RW | RAM | -32768 | 32767 | Y + 606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N + 610 | is_moving | 1 | R | RAM | 0 | 1 | N + 611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 621 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 623 | present_voltage | 2 | R | RAM | 0 | 500 | N + 625 | present_temperature | 1 | R | RAM | 0 | 200 | N + 626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 891 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N From d7161f1033266ef10be4493302f787bab631a6c1 Mon Sep 17 00:00:00 2001 From: Zerom Date: Tue, 20 Mar 2018 14:14:34 +0900 Subject: [PATCH 111/238] - modified robotis_controller/CMakeLists.txt (yaml-cpp bug). --- robotis_controller/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/robotis_controller/CMakeLists.txt b/robotis_controller/CMakeLists.txt index 9ffde36..db8b1f5 100755 --- a/robotis_controller/CMakeLists.txt +++ b/robotis_controller/CMakeLists.txt @@ -22,7 +22,7 @@ find_package(catkin REQUIRED COMPONENTS ) find_package(PkgConfig REQUIRED) -pkg_check_modules(YAML_CPP REQUIRED yaml-cpp) +pkg_check_modules(yaml_cpp yaml-cpp REQUIRED) ################################################################################ # Declare ROS messages, services and actions From 5aa587d22878e8dfaf594a6bc3eebd1a17c19e0d Mon Sep 17 00:00:00 2001 From: Pyo Date: Tue, 20 Mar 2018 15:22:52 +0900 Subject: [PATCH 112/238] tested for system dependencies --- robotis_controller/CMakeLists.txt | 21 +++++++++++++++------ 1 file changed, 15 insertions(+), 6 deletions(-) diff --git a/robotis_controller/CMakeLists.txt b/robotis_controller/CMakeLists.txt index db8b1f5..44ab833 100755 --- a/robotis_controller/CMakeLists.txt +++ b/robotis_controller/CMakeLists.txt @@ -1,13 +1,14 @@ ################################################################################ -# CMake +# Set minimum required version of cmake, project name and compile options ################################################################################ cmake_minimum_required(VERSION 2.8.3) project(robotis_controller) -set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}") +## Compile as C++11, supported in ROS Kinetic and newer +add_compile_options(-std=c++11) ################################################################################ -# Packages +# Find catkin packages and libraries for catkin and system dependencies ################################################################################ find_package(catkin REQUIRED COMPONENTS roscpp @@ -21,8 +22,15 @@ find_package(catkin REQUIRED COMPONENTS cmake_modules ) -find_package(PkgConfig REQUIRED) -pkg_check_modules(yaml_cpp yaml-cpp REQUIRED) +find_package(yaml-cpp REQUIRED) + +# find_package(PkgConfig REQUIRED) +# pkg_check_modules(YAML_CPP REQUIRED yaml-cpp) +# pkg_check_modules(yaml_cpp yaml-cpp REQUIRED) + +################################################################################ +# Setup for python modules and scripts +################################################################################ ################################################################################ # Declare ROS messages, services and actions @@ -33,12 +41,13 @@ pkg_check_modules(yaml_cpp yaml-cpp REQUIRED) ################################################################################ ################################################################################ -# Catkin specific configuration +# Declare catkin specific configuration to be passed to dependent projects ################################################################################ catkin_package( INCLUDE_DIRS include LIBRARIES robotis_controller CATKIN_DEPENDS roscpp roslib std_msgs sensor_msgs robotis_controller_msgs dynamixel_sdk robotis_device robotis_framework_common cmake_modules + DEPENDS YAML_CPP ) ################################################################################ From 9b2a520810772d790786dae45919438bf8b6af2e Mon Sep 17 00:00:00 2001 From: Pyo Date: Tue, 20 Mar 2018 16:17:33 +0900 Subject: [PATCH 113/238] tested for system dependencies --- robotis_controller/CMakeLists.txt | 25 ++++++++++++++++++------- 1 file changed, 18 insertions(+), 7 deletions(-) diff --git a/robotis_controller/CMakeLists.txt b/robotis_controller/CMakeLists.txt index 44ab833..5c32abd 100755 --- a/robotis_controller/CMakeLists.txt +++ b/robotis_controller/CMakeLists.txt @@ -22,11 +22,23 @@ find_package(catkin REQUIRED COMPONENTS cmake_modules ) -find_package(yaml-cpp REQUIRED) +# Resolve system dependency on yaml-cpp, which apparently does not +# provide a CMake find_package() module. +find_package(PkgConfig REQUIRED) +pkg_check_modules(YAML_CPP REQUIRED yaml-cpp) +find_path(YAML_CPP_INCLUDE_DIR + NAMES yaml_cpp.h + PATHS ${YAML_CPP_INCLUDE_DIRS} +) +find_library(YAML_CPP_LIBRARY + NAMES YAML_CPP + PATHS ${YAML_CPP_LIBRARY_DIRS} +) +link_directories(${YAML_CPP_LIBRARY_DIRS}) -# find_package(PkgConfig REQUIRED) -# pkg_check_modules(YAML_CPP REQUIRED yaml-cpp) -# pkg_check_modules(yaml_cpp yaml-cpp REQUIRED) +if(NOT ${YAML_CPP_VERSION} VERSION_LESS "0.5") +add_definitions(-DHAVE_NEW_YAMLCPP) +endif(NOT ${YAML_CPP_VERSION} VERSION_LESS "0.5") ################################################################################ # Setup for python modules and scripts @@ -47,7 +59,6 @@ catkin_package( INCLUDE_DIRS include LIBRARIES robotis_controller CATKIN_DEPENDS roscpp roslib std_msgs sensor_msgs robotis_controller_msgs dynamixel_sdk robotis_device robotis_framework_common cmake_modules - DEPENDS YAML_CPP ) ################################################################################ @@ -56,12 +67,12 @@ catkin_package( include_directories( include ${catkin_INCLUDE_DIRS} - ${yaml_cpp_INCLUDE_DIRS} + ${YAML_CPP_INCLUDE_DIRS} ) add_library(robotis_controller src/robotis_controller/robotis_controller.cpp) add_dependencies(robotis_controller ${catkin_EXPORTED_TARGETS}) -target_link_libraries(robotis_controller ${yaml_cpp_LIBRARIES} ${catkin_LIBRARIES}) +target_link_libraries(robotis_controller ${catkin_LIBRARIES} ${YAML_CPP_LIBRARIES}) ################################################################################ # Install From c02aa74b8c6e7860d7735935044505b80c9bb7bb Mon Sep 17 00:00:00 2001 From: Pyo Date: Tue, 20 Mar 2018 16:53:01 +0900 Subject: [PATCH 114/238] tested for system dependencies --- .travis.yml | 2 +- README.md | 21 +++++++++++++++------ robotis_controller/package.xml | 6 ++++-- robotis_device/CMakeLists.txt | 10 +++++++--- robotis_device/package.xml | 6 ++++-- robotis_framework/package.xml | 10 ++++++---- robotis_framework_common/CMakeLists.txt | 13 ++++++++----- robotis_framework_common/package.xml | 6 ++++-- 8 files changed, 49 insertions(+), 25 deletions(-) diff --git a/.travis.yml b/.travis.yml index 1145575..68240e5 100644 --- a/.travis.yml +++ b/.travis.yml @@ -12,7 +12,7 @@ compiler: - gcc notifications: email: - on_success: always + on_success: change on_failure: always recipients: - pyo@robotis.com diff --git a/README.md b/README.md index 8c6b748..c40af91 100755 --- a/README.md +++ b/README.md @@ -1,9 +1,18 @@ -# robotis_framework - +# ROBOTIS Framework Metapackage [![GitHub version](https://badge.fury.io/gh/ROBOTIS-GIT%2FROBOTIS-Framework.svg)](https://badge.fury.io/gh/ROBOTIS-GIT%2FROBOTIS-Framework) [![Build Status](https://travis-ci.org/ROBOTIS-GIT/ROBOTIS-Framework.svg?branch=master)](https://travis-ci.org/ROBOTIS-GIT/ROBOTIS-Framework) -ROS packages for the robotis_framework (meta package) +# Documents for robotis_framework packages +- [ROBOTIS e-Manual](http://emanual.robotis.com/docs/en/software/robotis_framework_packages/#robotis-framework) +- http://wiki.ros.org/robotis_framework +- http://wiki.ros.org/robotis_controller +- http://wiki.ros.org/robotis_device +- http://wiki.ros.org/robotis_framework_common -# Documents -- ROS Wiki: http://wiki.ros.org/robotis_framework -- ROBOTIS e-Manual: http://emanual.robotis.com/docs/en/software/robotis_framework_packages/ +# ROS packages related to ROBOTIS Framework +- [dynamixel_sdk](https://github.com/ROBOTIS-GIT/DynamixelSDK) +- [robotis_framework](https://github.com/ROBOTIS-GIT/ROBOTIS-Framework) +- [robotis_controller_msgs](https://github.com/ROBOTIS-GIT/ROBOTIS-Framework-msgs) + +# Documents and Videos for ROBOTIS Framework +- [ROBOTIS e-Manual for Dynamixel SDK](http://emanual.robotis.com/docs/en/software/dynamixel/dynamixel_sdk/overview/) +- [ROBOTIS e-Manual for ROBOTIS Framework](http://emanual.robotis.com/docs/en/software/robotis_framework_packages/) \ No newline at end of file diff --git a/robotis_controller/package.xml b/robotis_controller/package.xml index 9073883..ff3b8ad 100755 --- a/robotis_controller/package.xml +++ b/robotis_controller/package.xml @@ -7,11 +7,13 @@ Apache 2.0 Zerom + Kayman SCH Pyo - https://github.com/ROBOTIS-GIT/ROBOTIS-Framework/issues - https://github.com/ROBOTIS-GIT/ROBOTIS-Framework http://wiki.ros.org/robotis_controller + http://emanual.robotis.com/docs/en/software/robotis_framework_packages/ + https://github.com/ROBOTIS-GIT/ROBOTIS-Framework + https://github.com/ROBOTIS-GIT/ROBOTIS-Framework/issues catkin roscpp roslib diff --git a/robotis_device/CMakeLists.txt b/robotis_device/CMakeLists.txt index 3b981d3..17bbcf6 100755 --- a/robotis_device/CMakeLists.txt +++ b/robotis_device/CMakeLists.txt @@ -1,17 +1,21 @@ ################################################################################ -# CMake +# Set minimum required version of cmake, project name and compile options ################################################################################ cmake_minimum_required(VERSION 2.8.3) project(robotis_device) ################################################################################ -# Packages +# Find catkin packages and libraries for catkin and system dependencies ################################################################################ find_package(catkin REQUIRED COMPONENTS roscpp dynamixel_sdk ) +################################################################################ +# Setup for python modules and scripts +################################################################################ + ################################################################################ # Declare ROS messages, services and actions ################################################################################ @@ -21,7 +25,7 @@ find_package(catkin REQUIRED COMPONENTS ################################################################################ ################################################################################ -# Catkin specific configuration +# Declare catkin specific configuration to be passed to dependent projects ################################################################################ catkin_package( INCLUDE_DIRS include diff --git a/robotis_device/package.xml b/robotis_device/package.xml index c0d1d1e..12a98fe 100755 --- a/robotis_device/package.xml +++ b/robotis_device/package.xml @@ -9,11 +9,13 @@ Apache 2.0 Zerom + Kayman SCH Pyo - https://github.com/ROBOTIS-GIT/ROBOTIS-Framework/issues - https://github.com/ROBOTIS-GIT/ROBOTIS-Framework http://wiki.ros.org/robotis_device + http://emanual.robotis.com/docs/en/software/robotis_framework_packages/ + https://github.com/ROBOTIS-GIT/ROBOTIS-Framework + https://github.com/ROBOTIS-GIT/ROBOTIS-Framework/issues catkin roscpp dynamixel_sdk diff --git a/robotis_framework/package.xml b/robotis_framework/package.xml index 4da2d0a..4da3659 100755 --- a/robotis_framework/package.xml +++ b/robotis_framework/package.xml @@ -7,14 +7,16 @@ Apache 2.0 Zerom + Kayman SCH Pyo - https://github.com/ROBOTIS-GIT/ROBOTIS-Framework/issues - https://github.com/ROBOTIS-GIT/ROBOTIS-Framework http://wiki.ros.org/robotis_framework + http://emanual.robotis.com/docs/en/software/robotis_framework_packages/ + https://github.com/ROBOTIS-GIT/ROBOTIS-Framework + https://github.com/ROBOTIS-GIT/ROBOTIS-Framework/issues catkin - robotis_framework_common - robotis_device robotis_controller + robotis_device + robotis_framework_common diff --git a/robotis_framework_common/CMakeLists.txt b/robotis_framework_common/CMakeLists.txt index ce7bbd4..e05df3c 100755 --- a/robotis_framework_common/CMakeLists.txt +++ b/robotis_framework_common/CMakeLists.txt @@ -1,17 +1,21 @@ ################################################################################ -# CMake +# Set minimum required version of cmake, project name and compile options ################################################################################ cmake_minimum_required(VERSION 2.8.3) project(robotis_framework_common) ################################################################################ -# Packages +# Find catkin packages and libraries for catkin and system dependencies ################################################################################ find_package(catkin REQUIRED COMPONENTS roscpp robotis_device ) +################################################################################ +# Setup for python modules and scripts +################################################################################ + ################################################################################ # Declare ROS messages, services and actions ################################################################################ @@ -21,11 +25,11 @@ find_package(catkin REQUIRED COMPONENTS ################################################################################ ################################################################################ -# Catkin specific configuration +# Declare catkin specific configuration to be passed to dependent projects ################################################################################ catkin_package( INCLUDE_DIRS include - LIBRARIES robotis_framework_common + LIBRARIES ${PROJECT_NAME} CATKIN_DEPENDS roscpp robotis_device ) @@ -42,7 +46,6 @@ add_library(${PROJECT_NAME} include/${PROJECT_NAME}/sensor_module.h include/${PROJECT_NAME}/singleton.h ) - set_target_properties(${PROJECT_NAME} PROPERTIES LINKER_LANGUAGE CXX) ################################################################################ diff --git a/robotis_framework_common/package.xml b/robotis_framework_common/package.xml index 7243698..54efc9d 100755 --- a/robotis_framework_common/package.xml +++ b/robotis_framework_common/package.xml @@ -7,11 +7,13 @@ Apache 2.0 Zerom + Kayman SCH Pyo - https://github.com/ROBOTIS-GIT/ROBOTIS-Framework/issues - https://github.com/ROBOTIS-GIT/ROBOTIS-Framework http://wiki.ros.org/robotis_framework_common + http://emanual.robotis.com/docs/en/software/robotis_framework_packages/ + https://github.com/ROBOTIS-GIT/ROBOTIS-Framework + https://github.com/ROBOTIS-GIT/ROBOTIS-Framework/issues catkin roscpp robotis_device From 2eb879eb3588b17f254dbce1cac6fe2705c940ba Mon Sep 17 00:00:00 2001 From: Pyo Date: Tue, 20 Mar 2018 16:53:01 +0900 Subject: [PATCH 115/238] tested for system dependencies --- .travis.yml | 2 +- README.md | 21 +++++++++++++++------ 2 files changed, 16 insertions(+), 7 deletions(-) diff --git a/.travis.yml b/.travis.yml index 1145575..68240e5 100644 --- a/.travis.yml +++ b/.travis.yml @@ -12,7 +12,7 @@ compiler: - gcc notifications: email: - on_success: always + on_success: change on_failure: always recipients: - pyo@robotis.com diff --git a/README.md b/README.md index 8c6b748..c40af91 100755 --- a/README.md +++ b/README.md @@ -1,9 +1,18 @@ -# robotis_framework - +# ROBOTIS Framework Metapackage [![GitHub version](https://badge.fury.io/gh/ROBOTIS-GIT%2FROBOTIS-Framework.svg)](https://badge.fury.io/gh/ROBOTIS-GIT%2FROBOTIS-Framework) [![Build Status](https://travis-ci.org/ROBOTIS-GIT/ROBOTIS-Framework.svg?branch=master)](https://travis-ci.org/ROBOTIS-GIT/ROBOTIS-Framework) -ROS packages for the robotis_framework (meta package) +# Documents for robotis_framework packages +- [ROBOTIS e-Manual](http://emanual.robotis.com/docs/en/software/robotis_framework_packages/#robotis-framework) +- http://wiki.ros.org/robotis_framework +- http://wiki.ros.org/robotis_controller +- http://wiki.ros.org/robotis_device +- http://wiki.ros.org/robotis_framework_common -# Documents -- ROS Wiki: http://wiki.ros.org/robotis_framework -- ROBOTIS e-Manual: http://emanual.robotis.com/docs/en/software/robotis_framework_packages/ +# ROS packages related to ROBOTIS Framework +- [dynamixel_sdk](https://github.com/ROBOTIS-GIT/DynamixelSDK) +- [robotis_framework](https://github.com/ROBOTIS-GIT/ROBOTIS-Framework) +- [robotis_controller_msgs](https://github.com/ROBOTIS-GIT/ROBOTIS-Framework-msgs) + +# Documents and Videos for ROBOTIS Framework +- [ROBOTIS e-Manual for Dynamixel SDK](http://emanual.robotis.com/docs/en/software/dynamixel/dynamixel_sdk/overview/) +- [ROBOTIS e-Manual for ROBOTIS Framework](http://emanual.robotis.com/docs/en/software/robotis_framework_packages/) \ No newline at end of file From f44c58a101d5d9dfdbf41bab77135256f579375d Mon Sep 17 00:00:00 2001 From: Pyo Date: Tue, 20 Mar 2018 17:02:55 +0900 Subject: [PATCH 116/238] updated the CHANGELOG and version to release binary packages --- robotis_controller/CHANGELOG.rst | 5 +++++ robotis_controller/package.xml | 2 +- robotis_device/CHANGELOG.rst | 7 ++++++- robotis_device/package.xml | 2 +- robotis_framework/CHANGELOG.rst | 6 ++++++ robotis_framework/package.xml | 2 +- robotis_framework_common/CHANGELOG.rst | 5 +++++ robotis_framework_common/package.xml | 2 +- 8 files changed, 26 insertions(+), 5 deletions(-) diff --git a/robotis_controller/CHANGELOG.rst b/robotis_controller/CHANGELOG.rst index d1aa7be..dad6244 100755 --- a/robotis_controller/CHANGELOG.rst +++ b/robotis_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package robotis_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.2.8 (2018-03-20) +------------------ +* modified CMakeLists.txt for system dependencies (yaml-cpp) +* Contributors: Zerom, Pyo + 0.2.7 (2018-03-15) ------------------ * changed the License and package format to version 2 diff --git a/robotis_controller/package.xml b/robotis_controller/package.xml index ff3b8ad..48371dc 100755 --- a/robotis_controller/package.xml +++ b/robotis_controller/package.xml @@ -1,7 +1,7 @@ robotis_controller - 0.2.7 + 0.2.8 robotis_controller package for ROBOTIS's platform like Manipulator-H, THORMANG and OP series diff --git a/robotis_device/CHANGELOG.rst b/robotis_device/CHANGELOG.rst index 7330dc8..8d62fbc 100755 --- a/robotis_device/CHANGELOG.rst +++ b/robotis_device/CHANGELOG.rst @@ -2,8 +2,13 @@ Changelog for package robotis_device ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.2.8 (2018-03-20) +------------------ +* added RH-P12-RN.device file +* Contributors: Zerom, Pyo + 0.2.7 (2018-03-15) -------------------------- +------------------ * fixed a bug that occur when handling bulk read item that does not exist * changed the License and package format to version 2 * Contributors: SCH, Pyo diff --git a/robotis_device/package.xml b/robotis_device/package.xml index 12a98fe..565f964 100755 --- a/robotis_device/package.xml +++ b/robotis_device/package.xml @@ -1,7 +1,7 @@ robotis_device - 0.2.7 + 0.2.8 The package that manages device information of ROBOTIS robots. This package is used when reading device information with the robot information file diff --git a/robotis_framework/CHANGELOG.rst b/robotis_framework/CHANGELOG.rst index 8f3d7f2..0424994 100755 --- a/robotis_framework/CHANGELOG.rst +++ b/robotis_framework/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package robotis_framework ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.2.8 (2018-03-20) +------------------ +* added RH-P12-RN.device file +* modified CMakeLists.txt for system dependencies (yaml-cpp) +* Contributors: Zerom, Pyo + 0.2.7 (2018-03-15) ------------------ * changed all values read by bulk read are saved to dxl_state\_->bulk_read_table\_. diff --git a/robotis_framework/package.xml b/robotis_framework/package.xml index 4da3659..fd9efbe 100755 --- a/robotis_framework/package.xml +++ b/robotis_framework/package.xml @@ -1,7 +1,7 @@ robotis_framework - 0.2.7 + 0.2.8 ROS packages for the robotis_framework (meta package) diff --git a/robotis_framework_common/CHANGELOG.rst b/robotis_framework_common/CHANGELOG.rst index 1359706..d4c4bd6 100755 --- a/robotis_framework_common/CHANGELOG.rst +++ b/robotis_framework_common/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package robotis_framework_common ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.2.8 (2018-03-20) +------------------ +* tested for system dependencies +* Contributors: Pyo + 0.2.7 (2018-03-15) ------------------ * change the License and package format to version 2 diff --git a/robotis_framework_common/package.xml b/robotis_framework_common/package.xml index 54efc9d..47f8a65 100755 --- a/robotis_framework_common/package.xml +++ b/robotis_framework_common/package.xml @@ -1,7 +1,7 @@ robotis_framework_common - 0.2.7 + 0.2.8 The package contains commonly used Headers for the ROBOTIS Framework. From 04f04b06433280f40a1db26fa8616c95306df598 Mon Sep 17 00:00:00 2001 From: Kayman Date: Wed, 21 Mar 2018 19:03:11 +0900 Subject: [PATCH 117/238] deleted comment for debug --- .../src/robotis_controller/robotis_controller.cpp | 8 -------- 1 file changed, 8 deletions(-) diff --git a/robotis_controller/src/robotis_controller/robotis_controller.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp index f53cb94..921189c 100755 --- a/robotis_controller/src/robotis_controller/robotis_controller.cpp +++ b/robotis_controller/src/robotis_controller/robotis_controller.cpp @@ -1935,14 +1935,6 @@ void RobotisController::setJointCtrlModuleThread(const robotis_controller_msgs:: queue_mutex_.unlock(); - // log -// std::cout << "Enable Joint Ctrl Module : " << std::endl; -// for(std::list::iterator _m_it = motion_modules_.begin(); _m_it != motion_modules_.end(); _m_it++) -// { -// if((*_m_it)->GetModuleEnable() == true) -// std::cout << " - " << (*_m_it)->GetModuleName() << std::endl; -// } - // publish current module robotis_controller_msgs::JointCtrlModule _current_module_msg; for(std::map::iterator _dxl_iter = robot_->dxls_.begin(); _dxl_iter != robot_->dxls_.end(); ++_dxl_iter) From b15d2ab7de300cc8661ccc94d1650fa829472237 Mon Sep 17 00:00:00 2001 From: Pyo Date: Thu, 22 Mar 2018 15:06:09 +0900 Subject: [PATCH 118/238] updated the CHANGELOG and version to release binary packages --- README.md | 24 ++++++++++++++---------- robotis_controller/CHANGELOG.rst | 9 +++++++++ robotis_controller/CMakeLists.txt | 6 +++++- robotis_controller/package.xml | 2 +- robotis_device/CHANGELOG.rst | 6 ++++++ robotis_device/package.xml | 2 +- robotis_framework/CHANGELOG.rst | 9 +++++++++ robotis_framework/package.xml | 2 +- robotis_framework_common/CHANGELOG.rst | 4 ++++ robotis_framework_common/package.xml | 2 +- 10 files changed, 51 insertions(+), 15 deletions(-) diff --git a/README.md b/README.md index c40af91..fc06982 100755 --- a/README.md +++ b/README.md @@ -1,18 +1,22 @@ -# ROBOTIS Framework Metapackage -[![GitHub version](https://badge.fury.io/gh/ROBOTIS-GIT%2FROBOTIS-Framework.svg)](https://badge.fury.io/gh/ROBOTIS-GIT%2FROBOTIS-Framework) [![Build Status](https://travis-ci.org/ROBOTIS-GIT/ROBOTIS-Framework.svg?branch=master)](https://travis-ci.org/ROBOTIS-GIT/ROBOTIS-Framework) +## ROS Packages for ROBOTIS Framework +|Version|Kinetic + Ubuntu Xenial|Melodic + Ubuntu Bionic| +|:---:|:---:|:---:| +|[![GitHub version](https://badge.fury.io/gh/ROBOTIS-GIT%2FROBOTIS-Framework.svg)](https://badge.fury.io/gh/ROBOTIS-GIT%2FROBOTIS-Framework)|[![Build Status](https://travis-ci.org/ROBOTIS-GIT/ROBOTIS-Framework.svg?branch=master)](https://travis-ci.org/ROBOTIS-GIT/ROBOTIS-Framework)|-| -# Documents for robotis_framework packages -- [ROBOTIS e-Manual](http://emanual.robotis.com/docs/en/software/robotis_framework_packages/#robotis-framework) -- http://wiki.ros.org/robotis_framework +## ROBOTIS e-Manual for ROBOTIS Framework +- [ROBOTIS e-Manual for ROBOTIS Framework](http://emanual.robotis.com/docs/en/software/robotis_framework_packages) + +## Wiki for robotis_framework Packages +- http://wiki.ros.org/robotis_framework (metapackage) - http://wiki.ros.org/robotis_controller - http://wiki.ros.org/robotis_device - http://wiki.ros.org/robotis_framework_common -# ROS packages related to ROBOTIS Framework -- [dynamixel_sdk](https://github.com/ROBOTIS-GIT/DynamixelSDK) +## Open Source related to ROBOTIS Framework - [robotis_framework](https://github.com/ROBOTIS-GIT/ROBOTIS-Framework) - [robotis_controller_msgs](https://github.com/ROBOTIS-GIT/ROBOTIS-Framework-msgs) +- [dynamixel_sdk](https://github.com/ROBOTIS-GIT/DynamixelSDK) -# Documents and Videos for ROBOTIS Framework -- [ROBOTIS e-Manual for Dynamixel SDK](http://emanual.robotis.com/docs/en/software/dynamixel/dynamixel_sdk/overview/) -- [ROBOTIS e-Manual for ROBOTIS Framework](http://emanual.robotis.com/docs/en/software/robotis_framework_packages/) \ No newline at end of file +## Documents and Videos related to ROBOTIS Framework +- [ROBOTIS e-Manual for ROBOTIS Framework](http://emanual.robotis.com/docs/en/software/robotis_framework_packages/) +- [ROBOTIS e-Manual for Dynamixel SDK](http://emanual.robotis.com/docs/en/software/dynamixel/dynamixel_sdk/overview/) \ No newline at end of file diff --git a/robotis_controller/CHANGELOG.rst b/robotis_controller/CHANGELOG.rst index dad6244..990b3bc 100755 --- a/robotis_controller/CHANGELOG.rst +++ b/robotis_controller/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package robotis_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.2.9 (2018-03-22) +------------------ +* added serivce for setting module +* deleted comment for debug +* modified to prevent duplicate indirect address write +* added boost system dependencies +* fixed a bug that occure when handling bulk read item that does not exist. +* Contributors: Kayman, Zerom, Pyo + 0.2.8 (2018-03-20) ------------------ * modified CMakeLists.txt for system dependencies (yaml-cpp) diff --git a/robotis_controller/CMakeLists.txt b/robotis_controller/CMakeLists.txt index 5c32abd..8e5dd9c 100755 --- a/robotis_controller/CMakeLists.txt +++ b/robotis_controller/CMakeLists.txt @@ -22,6 +22,8 @@ find_package(catkin REQUIRED COMPONENTS cmake_modules ) +find_package(Boost REQUIRED) + # Resolve system dependency on yaml-cpp, which apparently does not # provide a CMake find_package() module. find_package(PkgConfig REQUIRED) @@ -59,6 +61,7 @@ catkin_package( INCLUDE_DIRS include LIBRARIES robotis_controller CATKIN_DEPENDS roscpp roslib std_msgs sensor_msgs robotis_controller_msgs dynamixel_sdk robotis_device robotis_framework_common cmake_modules + DEPENDS Boost ) ################################################################################ @@ -67,12 +70,13 @@ catkin_package( include_directories( include ${catkin_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} ${YAML_CPP_INCLUDE_DIRS} ) add_library(robotis_controller src/robotis_controller/robotis_controller.cpp) add_dependencies(robotis_controller ${catkin_EXPORTED_TARGETS}) -target_link_libraries(robotis_controller ${catkin_LIBRARIES} ${YAML_CPP_LIBRARIES}) +target_link_libraries(robotis_controller ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${YAML_CPP_LIBRARIES}) ################################################################################ # Install diff --git a/robotis_controller/package.xml b/robotis_controller/package.xml index 48371dc..aeb4e91 100755 --- a/robotis_controller/package.xml +++ b/robotis_controller/package.xml @@ -1,7 +1,7 @@ robotis_controller - 0.2.8 + 0.2.9 robotis_controller package for ROBOTIS's platform like Manipulator-H, THORMANG and OP series diff --git a/robotis_device/CHANGELOG.rst b/robotis_device/CHANGELOG.rst index 8d62fbc..af36406 100755 --- a/robotis_device/CHANGELOG.rst +++ b/robotis_device/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package robotis_device ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.2.9 (2018-03-22) +------------------ +* modified to prevent duplicate indirect address write +* fixed a bug that occure when handling bulk read item that does not exist +* Contributors: Zerom + 0.2.8 (2018-03-20) ------------------ * added RH-P12-RN.device file diff --git a/robotis_device/package.xml b/robotis_device/package.xml index 565f964..09a81af 100755 --- a/robotis_device/package.xml +++ b/robotis_device/package.xml @@ -1,7 +1,7 @@ robotis_device - 0.2.8 + 0.2.9 The package that manages device information of ROBOTIS robots. This package is used when reading device information with the robot information file diff --git a/robotis_framework/CHANGELOG.rst b/robotis_framework/CHANGELOG.rst index 0424994..794034b 100755 --- a/robotis_framework/CHANGELOG.rst +++ b/robotis_framework/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package robotis_framework ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.2.9 (2018-03-22) +------------------ +* added serivce for setting module +* deleted comment for debug +* modified to prevent duplicate indirect address write +* added boost system dependencies +* fixed a bug that occure when handling bulk read item that does not exist +* Contributors: Kayman, Zerom, Pyo + 0.2.8 (2018-03-20) ------------------ * added RH-P12-RN.device file diff --git a/robotis_framework/package.xml b/robotis_framework/package.xml index fd9efbe..775bdd5 100755 --- a/robotis_framework/package.xml +++ b/robotis_framework/package.xml @@ -1,7 +1,7 @@ robotis_framework - 0.2.8 + 0.2.9 ROS packages for the robotis_framework (meta package) diff --git a/robotis_framework_common/CHANGELOG.rst b/robotis_framework_common/CHANGELOG.rst index d4c4bd6..7b3a4de 100755 --- a/robotis_framework_common/CHANGELOG.rst +++ b/robotis_framework_common/CHANGELOG.rst @@ -2,6 +2,10 @@ Changelog for package robotis_framework_common ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.2.9 (2018-03-22) +------------------ +* none + 0.2.8 (2018-03-20) ------------------ * tested for system dependencies diff --git a/robotis_framework_common/package.xml b/robotis_framework_common/package.xml index 47f8a65..08a423b 100755 --- a/robotis_framework_common/package.xml +++ b/robotis_framework_common/package.xml @@ -1,7 +1,7 @@ robotis_framework_common - 0.2.8 + 0.2.9 The package contains commonly used Headers for the ROBOTIS Framework. From 3ba9d9f85d98b3a4de742de03cde06fe85608424 Mon Sep 17 00:00:00 2001 From: Pyo Date: Thu, 22 Mar 2018 15:06:09 +0900 Subject: [PATCH 119/238] updated the CHANGELOG and version to release binary packages --- README.md | 24 ++++++++++++++---------- 1 file changed, 14 insertions(+), 10 deletions(-) diff --git a/README.md b/README.md index c40af91..fc06982 100755 --- a/README.md +++ b/README.md @@ -1,18 +1,22 @@ -# ROBOTIS Framework Metapackage -[![GitHub version](https://badge.fury.io/gh/ROBOTIS-GIT%2FROBOTIS-Framework.svg)](https://badge.fury.io/gh/ROBOTIS-GIT%2FROBOTIS-Framework) [![Build Status](https://travis-ci.org/ROBOTIS-GIT/ROBOTIS-Framework.svg?branch=master)](https://travis-ci.org/ROBOTIS-GIT/ROBOTIS-Framework) +## ROS Packages for ROBOTIS Framework +|Version|Kinetic + Ubuntu Xenial|Melodic + Ubuntu Bionic| +|:---:|:---:|:---:| +|[![GitHub version](https://badge.fury.io/gh/ROBOTIS-GIT%2FROBOTIS-Framework.svg)](https://badge.fury.io/gh/ROBOTIS-GIT%2FROBOTIS-Framework)|[![Build Status](https://travis-ci.org/ROBOTIS-GIT/ROBOTIS-Framework.svg?branch=master)](https://travis-ci.org/ROBOTIS-GIT/ROBOTIS-Framework)|-| -# Documents for robotis_framework packages -- [ROBOTIS e-Manual](http://emanual.robotis.com/docs/en/software/robotis_framework_packages/#robotis-framework) -- http://wiki.ros.org/robotis_framework +## ROBOTIS e-Manual for ROBOTIS Framework +- [ROBOTIS e-Manual for ROBOTIS Framework](http://emanual.robotis.com/docs/en/software/robotis_framework_packages) + +## Wiki for robotis_framework Packages +- http://wiki.ros.org/robotis_framework (metapackage) - http://wiki.ros.org/robotis_controller - http://wiki.ros.org/robotis_device - http://wiki.ros.org/robotis_framework_common -# ROS packages related to ROBOTIS Framework -- [dynamixel_sdk](https://github.com/ROBOTIS-GIT/DynamixelSDK) +## Open Source related to ROBOTIS Framework - [robotis_framework](https://github.com/ROBOTIS-GIT/ROBOTIS-Framework) - [robotis_controller_msgs](https://github.com/ROBOTIS-GIT/ROBOTIS-Framework-msgs) +- [dynamixel_sdk](https://github.com/ROBOTIS-GIT/DynamixelSDK) -# Documents and Videos for ROBOTIS Framework -- [ROBOTIS e-Manual for Dynamixel SDK](http://emanual.robotis.com/docs/en/software/dynamixel/dynamixel_sdk/overview/) -- [ROBOTIS e-Manual for ROBOTIS Framework](http://emanual.robotis.com/docs/en/software/robotis_framework_packages/) \ No newline at end of file +## Documents and Videos related to ROBOTIS Framework +- [ROBOTIS e-Manual for ROBOTIS Framework](http://emanual.robotis.com/docs/en/software/robotis_framework_packages/) +- [ROBOTIS e-Manual for Dynamixel SDK](http://emanual.robotis.com/docs/en/software/dynamixel/dynamixel_sdk/overview/) \ No newline at end of file From 91e791fa55298ea599069298d00eec1b3923c7e2 Mon Sep 17 00:00:00 2001 From: Pyo Date: Thu, 22 Mar 2018 15:50:12 +0900 Subject: [PATCH 120/238] added travis option to test using source --- .robotis_framework.rosinstall | 1 + .travis.yml | 3 ++- 2 files changed, 3 insertions(+), 1 deletion(-) create mode 100644 .robotis_framework.rosinstall diff --git a/.robotis_framework.rosinstall b/.robotis_framework.rosinstall new file mode 100644 index 0000000..9fd2d5e --- /dev/null +++ b/.robotis_framework.rosinstall @@ -0,0 +1 @@ +- git: {local-name: robotis_controller_msgs, uri: 'https://github.com/ROBOTIS-GIT/ROBOTIS-Framework-msgs.git', version: master} \ No newline at end of file diff --git a/.travis.yml b/.travis.yml index 68240e5..0532d36 100644 --- a/.travis.yml +++ b/.travis.yml @@ -18,8 +18,9 @@ notifications: - pyo@robotis.com env: matrix: - - ROS_DISTRO=kinetic ROS_REPO=ros-shadow-fixed UPSTREAM_WORKSPACE=debian +# - ROS_DISTRO=kinetic ROS_REPO=ros-shadow-fixed UPSTREAM_WORKSPACE=debian # - ROS_DISTRO=kinetic ROS_REPO=ros-shadow-fixed UPSTREAM_WORKSPACE=debian OS_NAME=debian OS_CODE_NAME=jessie + - ROS_DISTRO=kinetic ROS_REPO=ros UPSTREAM_WORKSPACE=file OS_NAME=debian OS_CODE_NAME=jessie $ROSINSTALL_FILENAME=".robotis_framework.rosinstall" branches: only: - master From 24cdd3fafd6d8a807257af394e6e21fd91cafe96 Mon Sep 17 00:00:00 2001 From: Pyo Date: Thu, 22 Mar 2018 15:50:12 +0900 Subject: [PATCH 121/238] added travis option to test using source --- .travis.yml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/.travis.yml b/.travis.yml index 68240e5..0532d36 100644 --- a/.travis.yml +++ b/.travis.yml @@ -18,8 +18,9 @@ notifications: - pyo@robotis.com env: matrix: - - ROS_DISTRO=kinetic ROS_REPO=ros-shadow-fixed UPSTREAM_WORKSPACE=debian +# - ROS_DISTRO=kinetic ROS_REPO=ros-shadow-fixed UPSTREAM_WORKSPACE=debian # - ROS_DISTRO=kinetic ROS_REPO=ros-shadow-fixed UPSTREAM_WORKSPACE=debian OS_NAME=debian OS_CODE_NAME=jessie + - ROS_DISTRO=kinetic ROS_REPO=ros UPSTREAM_WORKSPACE=file OS_NAME=debian OS_CODE_NAME=jessie $ROSINSTALL_FILENAME=".robotis_framework.rosinstall" branches: only: - master From 8aa620212ea000db750004f7e5e51aa97f0ba5c7 Mon Sep 17 00:00:00 2001 From: Pyo Date: Thu, 22 Mar 2018 16:43:00 +0900 Subject: [PATCH 122/238] Update .travis.yml --- .travis.yml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/.travis.yml b/.travis.yml index 0532d36..a101867 100644 --- a/.travis.yml +++ b/.travis.yml @@ -18,9 +18,9 @@ notifications: - pyo@robotis.com env: matrix: -# - ROS_DISTRO=kinetic ROS_REPO=ros-shadow-fixed UPSTREAM_WORKSPACE=debian + - ROS_DISTRO=kinetic ROS_REPO=ros-shadow-fixed UPSTREAM_WORKSPACE=debian # - ROS_DISTRO=kinetic ROS_REPO=ros-shadow-fixed UPSTREAM_WORKSPACE=debian OS_NAME=debian OS_CODE_NAME=jessie - - ROS_DISTRO=kinetic ROS_REPO=ros UPSTREAM_WORKSPACE=file OS_NAME=debian OS_CODE_NAME=jessie $ROSINSTALL_FILENAME=".robotis_framework.rosinstall" +# - ROS_DISTRO=kinetic ROS_REPO=ros UPSTREAM_WORKSPACE=file OS_NAME=debian OS_CODE_NAME=jessie $ROSINSTALL_FILENAME=".robotis_framework.rosinstall" branches: only: - master @@ -30,4 +30,4 @@ install: - git clone https://github.com/ros-industrial/industrial_ci.git .ci_config script: - source .ci_config/travis.sh - \ No newline at end of file + From 850c1f882c9ab89f540b19c07168e46deef4e54b Mon Sep 17 00:00:00 2001 From: Pyo Date: Thu, 22 Mar 2018 16:43:00 +0900 Subject: [PATCH 123/238] Update .travis.yml --- .travis.yml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/.travis.yml b/.travis.yml index 0532d36..a101867 100644 --- a/.travis.yml +++ b/.travis.yml @@ -18,9 +18,9 @@ notifications: - pyo@robotis.com env: matrix: -# - ROS_DISTRO=kinetic ROS_REPO=ros-shadow-fixed UPSTREAM_WORKSPACE=debian + - ROS_DISTRO=kinetic ROS_REPO=ros-shadow-fixed UPSTREAM_WORKSPACE=debian # - ROS_DISTRO=kinetic ROS_REPO=ros-shadow-fixed UPSTREAM_WORKSPACE=debian OS_NAME=debian OS_CODE_NAME=jessie - - ROS_DISTRO=kinetic ROS_REPO=ros UPSTREAM_WORKSPACE=file OS_NAME=debian OS_CODE_NAME=jessie $ROSINSTALL_FILENAME=".robotis_framework.rosinstall" +# - ROS_DISTRO=kinetic ROS_REPO=ros UPSTREAM_WORKSPACE=file OS_NAME=debian OS_CODE_NAME=jessie $ROSINSTALL_FILENAME=".robotis_framework.rosinstall" branches: only: - master @@ -30,4 +30,4 @@ install: - git clone https://github.com/ros-industrial/industrial_ci.git .ci_config script: - source .ci_config/travis.sh - \ No newline at end of file + From d56fcc01bcd87fec81fe658f0485f3f1ba199bb2 Mon Sep 17 00:00:00 2001 From: Zerom Date: Mon, 9 Apr 2018 14:09:45 +0900 Subject: [PATCH 124/238] - changed ROS_ERROR to ROS_WARN - changed the message from ROS_ERROR to ROS_WARN when the offset file fails to load. --- .../src/robotis_controller/robotis_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/robotis_controller/src/robotis_controller/robotis_controller.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp index 82c8c54..9c2278e 100755 --- a/robotis_controller/src/robotis_controller/robotis_controller.cpp +++ b/robotis_controller/src/robotis_controller/robotis_controller.cpp @@ -850,7 +850,7 @@ void RobotisController::loadOffset(const std::string path) doc = YAML::LoadFile(path.c_str()); } catch (const std::exception& e) { - ROS_ERROR("Fail to load offset yaml."); + ROS_WARN("Fail to load offset yaml."); return; } From 6547e9da274602b2f188a461d3238f388c0e5838 Mon Sep 17 00:00:00 2001 From: Zerom Date: Mon, 9 Apr 2018 15:20:41 +0900 Subject: [PATCH 125/238] added DXL Pro (A) device files. added DXL Pro (A) device files. --- .../devices/dynamixel/H42-20-S300-R(A).device | 92 +++++++++++++++++++ .../dynamixel/H54-100-S500-R(A).device | 92 +++++++++++++++++++ .../dynamixel/H54-200-S500-R(A).device | 92 +++++++++++++++++++ 3 files changed, 276 insertions(+) create mode 100644 robotis_device/devices/dynamixel/H42-20-S300-R(A).device create mode 100644 robotis_device/devices/dynamixel/H54-100-S500-R(A).device create mode 100644 robotis_device/devices/dynamixel/H54-200-S500-R(A).device diff --git a/robotis_device/devices/dynamixel/H42-20-S300-R(A).device b/robotis_device/devices/dynamixel/H42-20-S300-R(A).device new file mode 100644 index 0000000..a6f523c --- /dev/null +++ b/robotis_device/devices/dynamixel/H42-20-S300-R(A).device @@ -0,0 +1,92 @@ +[device info] +model_name = H42-20-S300-R(A) +device_type = dynamixel + +[type info] +torque_to_current_value_ratio = 109.3747778 +velocity_to_value_ratio = 954.93 +value_of_0_radian_position = 0 +value_of_min_radian_position = -303750 +value_of_max_radian_position = 303750 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_current +position_d_gain_item_name = position_d_gain +position_i_gain_item_name = position_i_gain +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 10 | drive_mode | 1 | RW | EEPROM | 0 | 3 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 12 | secondary_id | 1 | RW | EEPROM | 0 | 255 | N + 13 | protocol_version | 1 | RW | EEPROM | 1 | 2 | N + 20 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 24 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 31 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 32 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 34 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 36 | pwm_limit | 2 | RW | EEPROM | 0 | 885 | N + 38 | current_limit | 2 | RW | EEPROM | 0 | 32767 | N + 40 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 44 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 48 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 52 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 56 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 57 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 58 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 59 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 63 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 168 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 512 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 513 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 514 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 515 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 516 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 517 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 518 | hardware_error_status | 1 | R | RAM | 0 | 31 | N + 524 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N + 526 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N + 528 | position_d_gain | 2 | RW | RAM | 0 | 2047 | N + 530 | position_i_gain | 2 | RW | RAM | 0 | 2047 | N + 532 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N + 536 | feedforward_2nd_gain | 2 | RW | RAM | 0 | 32767 | N + 538 | feedforward_1st_gain | 2 | RW | RAM | 0 | 32767 | N + 546 | bus_watchdog | 1 | RW | RAM | -1 | 127 | Y + 548 | goal_pwm | 2 | RW | RAM | -2100 | 2100 | Y + 550 | goal_current | 2 | RW | RAM | -32768 | 32767 | Y + 552 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 556 | profile_acceleration | 4 | RW | RAM | 0 | 32767 | N + 560 | profile_velocity | 4 | RW | RAM | 0 | 1023 | N + 564 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 568 | realtime_tick | 2 | R | RAM | 0 | 32767 | N + 570 | moving | 1 | R | RAM | 0 | 1 | N + 571 | moving_status | 1 | R | RAM | 0 | 255 | N + 572 | present_pwm | 2 | R | RAM | -2100 | 2100 | Y + 574 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 576 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 580 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 584 | velocity_trajectory | 4 | R | RAM | 0 | 1023 | N + 588 | position_trajectory | 4 | R | RAM | 0 | 4095 | N + 592 | present_voltage | 2 | R | RAM | 0 | 500 | N + 594 | present_temperature | 1 | R | RAM | 0 | 200 | N + 600 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 602 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 604 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 606 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N diff --git a/robotis_device/devices/dynamixel/H54-100-S500-R(A).device b/robotis_device/devices/dynamixel/H54-100-S500-R(A).device new file mode 100644 index 0000000..fe706cc --- /dev/null +++ b/robotis_device/devices/dynamixel/H54-100-S500-R(A).device @@ -0,0 +1,92 @@ +[device info] +model_name = H54-100-S500-R(A) +device_type = dynamixel + +[type info] +torque_to_current_value_ratio = 155.658486 +velocity_to_value_ratio = 954.93 +value_of_0_radian_position = 0 +value_of_min_radian_position = -501923 +value_of_max_radian_position = 501923 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_current +position_d_gain_item_name = position_d_gain +position_i_gain_item_name = position_i_gain +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 10 | drive_mode | 1 | RW | EEPROM | 0 | 3 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 12 | secondary_id | 1 | RW | EEPROM | 0 | 255 | N + 13 | protocol_version | 1 | RW | EEPROM | 1 | 2 | N + 20 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 24 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 31 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 32 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 34 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 36 | pwm_limit | 2 | RW | EEPROM | 0 | 885 | N + 38 | current_limit | 2 | RW | EEPROM | 0 | 32767 | N + 40 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 44 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 48 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 52 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 56 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 57 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 58 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 59 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 63 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 168 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 512 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 513 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 514 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 515 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 516 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 517 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 518 | hardware_error_status | 1 | R | RAM | 0 | 31 | N + 524 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N + 526 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N + 528 | position_d_gain | 2 | RW | RAM | 0 | 2047 | N + 530 | position_i_gain | 2 | RW | RAM | 0 | 2047 | N + 532 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N + 536 | feedforward_2nd_gain | 2 | RW | RAM | 0 | 32767 | N + 538 | feedforward_1st_gain | 2 | RW | RAM | 0 | 32767 | N + 546 | bus_watchdog | 1 | RW | RAM | -1 | 127 | Y + 548 | goal_pwm | 2 | RW | RAM | -2100 | 2100 | Y + 550 | goal_current | 2 | RW | RAM | -32768 | 32767 | Y + 552 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 556 | profile_acceleration | 4 | RW | RAM | 0 | 32767 | N + 560 | profile_velocity | 4 | RW | RAM | 0 | 1023 | N + 564 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 568 | realtime_tick | 2 | R | RAM | 0 | 32767 | N + 570 | moving | 1 | R | RAM | 0 | 1 | N + 571 | moving_status | 1 | R | RAM | 0 | 255 | N + 572 | present_pwm | 2 | R | RAM | -2100 | 2100 | Y + 574 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 576 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 580 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 584 | velocity_trajectory | 4 | R | RAM | 0 | 1023 | N + 588 | position_trajectory | 4 | R | RAM | 0 | 4095 | N + 592 | present_voltage | 2 | R | RAM | 0 | 500 | N + 594 | present_temperature | 1 | R | RAM | 0 | 200 | N + 600 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 602 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 604 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 606 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N diff --git a/robotis_device/devices/dynamixel/H54-200-S500-R(A).device b/robotis_device/devices/dynamixel/H54-200-S500-R(A).device new file mode 100644 index 0000000..3cfb758 --- /dev/null +++ b/robotis_device/devices/dynamixel/H54-200-S500-R(A).device @@ -0,0 +1,92 @@ +[device info] +model_name = H54-200-S500-R(A) +device_type = dynamixel + +[type info] +torque_to_current_value_ratio = 146.502114 +velocity_to_value_ratio = 954.93 +value_of_0_radian_position = 0 +value_of_min_radian_position = -501923 +value_of_max_radian_position = 501923 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_current +position_d_gain_item_name = position_d_gain +position_i_gain_item_name = position_i_gain +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 10 | drive_mode | 1 | RW | EEPROM | 0 | 3 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 12 | secondary_id | 1 | RW | EEPROM | 0 | 255 | N + 13 | protocol_version | 1 | RW | EEPROM | 1 | 2 | N + 20 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 24 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 31 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 32 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 34 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 36 | pwm_limit | 2 | RW | EEPROM | 0 | 885 | N + 38 | current_limit | 2 | RW | EEPROM | 0 | 32767 | N + 40 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 44 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 48 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 52 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 56 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 57 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 58 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 59 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 63 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 168 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 512 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 513 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 514 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 515 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 516 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 517 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 518 | hardware_error_status | 1 | R | RAM | 0 | 31 | N + 524 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N + 526 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N + 528 | position_d_gain | 2 | RW | RAM | 0 | 2047 | N + 530 | position_i_gain | 2 | RW | RAM | 0 | 2047 | N + 532 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N + 536 | feedforward_2nd_gain | 2 | RW | RAM | 0 | 32767 | N + 538 | feedforward_1st_gain | 2 | RW | RAM | 0 | 32767 | N + 546 | bus_watchdog | 1 | RW | RAM | -1 | 127 | Y + 548 | goal_pwm | 2 | RW | RAM | -2100 | 2100 | Y + 550 | goal_current | 2 | RW | RAM | -32768 | 32767 | Y + 552 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 556 | profile_acceleration | 4 | RW | RAM | 0 | 32767 | N + 560 | profile_velocity | 4 | RW | RAM | 0 | 1023 | N + 564 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 568 | realtime_tick | 2 | R | RAM | 0 | 32767 | N + 570 | moving | 1 | R | RAM | 0 | 1 | N + 571 | moving_status | 1 | R | RAM | 0 | 255 | N + 572 | present_pwm | 2 | R | RAM | -2100 | 2100 | Y + 574 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 576 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 580 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 584 | velocity_trajectory | 4 | R | RAM | 0 | 1023 | N + 588 | position_trajectory | 4 | R | RAM | 0 | 4095 | N + 592 | present_voltage | 2 | R | RAM | 0 | 500 | N + 594 | present_temperature | 1 | R | RAM | 0 | 200 | N + 600 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 602 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 604 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 606 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N From 45b088e30a84d9b0f5327fd490cfa8f03f2664bd Mon Sep 17 00:00:00 2001 From: Zerom Date: Tue, 10 Apr 2018 17:51:53 +0900 Subject: [PATCH 126/238] - modified RobotisController::setJointStatesCallback function --- .../robotis_controller/robotis_controller.cpp | 35 ++++++++++--------- 1 file changed, 19 insertions(+), 16 deletions(-) diff --git a/robotis_controller/src/robotis_controller/robotis_controller.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp index 9c2278e..365a8a5 100755 --- a/robotis_controller/src/robotis_controller/robotis_controller.cpp +++ b/robotis_controller/src/robotis_controller/robotis_controller.cpp @@ -1637,30 +1637,33 @@ void RobotisController::setControllerModeCallback(const std_msgs::String::ConstP void RobotisController::setJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg) { - if (controller_mode_ != DirectControlMode) - return; - queue_mutex_.lock(); for (int i = 0; i < msg->name.size(); i++) { - int32_t pos = 0; - Dynamixel *dxl = robot_->dxls_[msg->name[i]]; if (dxl == NULL) continue; - dxl->dxl_state_->goal_position_ = msg->position[i]; - pos = dxl->convertRadian2Value((double) msg->position[i]); - - uint8_t sync_write_data[4]; - sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(pos)); - sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(pos)); - sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(pos)); - sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(pos)); - - if (port_to_sync_write_position_[dxl->port_name_] != NULL) - port_to_sync_write_position_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); + if ((controller_mode_ == DirectControlMode) || + (controller_mode_ == MotionModuleMode && dxl->ctrl_module_name_ == "none")) + { + dxl->dxl_state->goal_position_ = (double) msg->position[i]; + + if (gazebo_mode_ == false) + { + // add offset + uint32_t pos_data = dxl->convertRadian2Value(dxl->dxl_state->goal_position_ + dxl->dxl_state->position_offset_); + uint8_t sync_write_data[4] = { 0 }; + sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(pos_data)); + sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(pos_data)); + sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(pos_data)); + sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(pos_data)); + + if (port_to_sync_write_position_[dxl->port_name_] != NULL) + port_to_sync_write_position_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); + } + } } queue_mutex_.unlock(); From 6310436bb6986783d16fd3f7b011443e06536b8f Mon Sep 17 00:00:00 2001 From: Zerom Date: Tue, 10 Apr 2018 18:05:08 +0900 Subject: [PATCH 127/238] - fixed typo --- .../src/robotis_controller/robotis_controller.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/robotis_controller/src/robotis_controller/robotis_controller.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp index 365a8a5..11722ae 100755 --- a/robotis_controller/src/robotis_controller/robotis_controller.cpp +++ b/robotis_controller/src/robotis_controller/robotis_controller.cpp @@ -1648,12 +1648,12 @@ void RobotisController::setJointStatesCallback(const sensor_msgs::JointState::Co if ((controller_mode_ == DirectControlMode) || (controller_mode_ == MotionModuleMode && dxl->ctrl_module_name_ == "none")) { - dxl->dxl_state->goal_position_ = (double) msg->position[i]; + dxl->dxl_state_->goal_position_ = (double) msg->position[i]; if (gazebo_mode_ == false) { // add offset - uint32_t pos_data = dxl->convertRadian2Value(dxl->dxl_state->goal_position_ + dxl->dxl_state->position_offset_); + uint32_t pos_data = dxl->convertRadian2Value(dxl->dxl_state_->goal_position_ + dxl->dxl_state_->position_offset_); uint8_t sync_write_data[4] = { 0 }; sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(pos_data)); sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(pos_data)); From b846058fdc82da10dd960d8fc4350d59647aea50 Mon Sep 17 00:00:00 2001 From: zerom Date: Tue, 24 Apr 2018 13:26:11 +0900 Subject: [PATCH 128/238] Update robotis_controller.cpp - fixed setJointStatesCallback function (addParam -> changeParam) --- .../src/robotis_controller/robotis_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/robotis_controller/src/robotis_controller/robotis_controller.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp index 11722ae..000bd59 100755 --- a/robotis_controller/src/robotis_controller/robotis_controller.cpp +++ b/robotis_controller/src/robotis_controller/robotis_controller.cpp @@ -1661,7 +1661,7 @@ void RobotisController::setJointStatesCallback(const sensor_msgs::JointState::Co sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(pos_data)); if (port_to_sync_write_position_[dxl->port_name_] != NULL) - port_to_sync_write_position_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); + port_to_sync_write_position_[dxl->port_name_]->changeParam(dxl->id_, sync_write_data); } } } From 037b96de6a0a9680b91dbe199333671b0753a5c9 Mon Sep 17 00:00:00 2001 From: Zerom Date: Tue, 29 May 2018 17:57:11 +0900 Subject: [PATCH 129/238] - added enable offset service - added load offset service --- .../robotis_controller/robotis_controller.h | 3 + .../robotis_controller/robotis_controller.cpp | 95 ++++++++++++++++--- 2 files changed, 87 insertions(+), 11 deletions(-) diff --git a/robotis_controller/include/robotis_controller/robotis_controller.h b/robotis_controller/include/robotis_controller/robotis_controller.h index aab8da0..a381a62 100755 --- a/robotis_controller/include/robotis_controller/robotis_controller.h +++ b/robotis_controller/include/robotis_controller/robotis_controller.h @@ -64,6 +64,7 @@ private: bool init_pose_loaded_; bool is_timer_running_; + bool is_offset_enabled_; bool stop_timer_; pthread_t timer_thread_; ControllerMode controller_mode_; @@ -142,6 +143,8 @@ public: bool getJointCtrlModuleService(robotis_controller_msgs::GetJointModule::Request &req, robotis_controller_msgs::GetJointModule::Response &res); bool setJointCtrlModuleService(robotis_controller_msgs::SetJointModule::Request &req, robotis_controller_msgs::SetJointModule::Response &res); bool setCtrlModuleService(robotis_controller_msgs::SetModule::Request &req, robotis_controller_msgs::SetModule::Response &res); + bool enableOffsetService(robotis_controller_msgs::EnableOffset::Request &req, robotis_controller_msgs::EnableOffset::Response &res); + bool loadOffsetService(robotis_controller_msgs::LoadOffset::Request &req, robotis_controller_msgs::LoadOffset::Response &res); void gazeboJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg); diff --git a/robotis_controller/src/robotis_controller/robotis_controller.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp index 000bd59..eb8d74e 100755 --- a/robotis_controller/src/robotis_controller/robotis_controller.cpp +++ b/robotis_controller/src/robotis_controller/robotis_controller.cpp @@ -30,6 +30,7 @@ using namespace robotis_framework; RobotisController::RobotisController() : is_timer_running_(false), + is_offset_enabled_(true), stop_timer_(false), init_pose_loaded_(false), timer_thread_(0), @@ -142,7 +143,10 @@ void RobotisController::initializeSyncWrite() if ((dxl->present_position_item_ != 0) && (dxl->bulk_read_items_[i]->item_name_ == dxl->present_position_item_->item_name_)) { - dxl->dxl_state_->present_position_ = dxl->convertValue2Radian(read_data) - dxl->dxl_state_->position_offset_; // remove offset + if(is_offset_enabled_) + dxl->dxl_state_->present_position_ = dxl->convertValue2Radian(read_data) - dxl->dxl_state_->position_offset_; // remove offset + else + dxl->dxl_state_->present_position_ = dxl->convertValue2Radian(read_data); dxl->dxl_state_->goal_position_ = dxl->dxl_state_->present_position_; port_to_sync_write_position_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); @@ -664,6 +668,10 @@ void RobotisController::msgQueueThread() &RobotisController::setJointCtrlModuleService, this); ros::ServiceServer set_module_server = ros_node.advertiseService("/robotis/set_present_ctrl_modules", &RobotisController::setCtrlModuleService, this); + ros::ServiceServer enable_offset_server = ros_node.advertiseService("/robotis/enable_offset", + &RobotisController::enableOffsetService, this); + ros::ServiceServer load_offset_server = ros_node.advertiseService("/robotis/load_offset", + &RobotisController::loadOffsetService, this); ros::WallDuration duration(robot_->getControlCycle() / 1000.0); while(ros_node.ok()) @@ -926,13 +934,23 @@ void RobotisController::process() // change dxl_state if (dxl->present_position_item_ != 0 && item->item_name_ == dxl->present_position_item_->item_name_) - dxl->dxl_state_->present_position_ = dxl->convertValue2Radian(data) - dxl->dxl_state_->position_offset_; // remove offset + { + if(is_offset_enabled_) + dxl->dxl_state_->present_position_ = dxl->convertValue2Radian(data) - dxl->dxl_state_->position_offset_; // remove offset + else + dxl->dxl_state_->present_position_ = dxl->convertValue2Radian(data); + } else if (dxl->present_velocity_item_ != 0 && item->item_name_ == dxl->present_velocity_item_->item_name_) dxl->dxl_state_->present_velocity_ = dxl->convertValue2Velocity(data); else if (dxl->present_current_item_ != 0 && item->item_name_ == dxl->present_current_item_->item_name_) dxl->dxl_state_->present_torque_ = dxl->convertValue2Torque(data); else if (dxl->goal_position_item_ != 0 && item->item_name_ == dxl->goal_position_item_->item_name_) - dxl->dxl_state_->goal_position_ = dxl->convertValue2Radian(data) - dxl->dxl_state_->position_offset_; // remove offset + { + if(is_offset_enabled_) + dxl->dxl_state_->goal_position_ = dxl->convertValue2Radian(data) - dxl->dxl_state_->position_offset_; // remove offset + else + dxl->dxl_state_->goal_position_ = dxl->convertValue2Radian(data); + } else if (dxl->goal_velocity_item_ != 0 && item->item_name_ == dxl->goal_velocity_item_->item_name_) dxl->dxl_state_->goal_velocity_ = dxl->convertValue2Velocity(data); else if (dxl->goal_current_item_ != 0 && item->item_name_ == dxl->goal_current_item_->item_name_) @@ -1146,13 +1164,23 @@ void RobotisController::process() // change dxl_state if (dxl->present_position_item_ != 0 && item->item_name_ == dxl->present_position_item_->item_name_) - dxl->dxl_state_->present_position_ = dxl->convertValue2Radian(data) - dxl->dxl_state_->position_offset_; // remove offset + { + if(is_offset_enabled_) + dxl->dxl_state_->present_position_ = dxl->convertValue2Radian(data) - dxl->dxl_state_->position_offset_; // remove offset + else + dxl->dxl_state_->present_position_ = dxl->convertValue2Radian(data); + } else if (dxl->present_velocity_item_ != 0 && item->item_name_ == dxl->present_velocity_item_->item_name_) dxl->dxl_state_->present_velocity_ = dxl->convertValue2Velocity(data); else if (dxl->present_current_item_ != 0 && item->item_name_ == dxl->present_current_item_->item_name_) dxl->dxl_state_->present_torque_ = dxl->convertValue2Torque(data); else if (dxl->goal_position_item_ != 0 && item->item_name_ == dxl->goal_position_item_->item_name_) - dxl->dxl_state_->goal_position_ = dxl->convertValue2Radian(data) - dxl->dxl_state_->position_offset_; // remove offset + { + if(is_offset_enabled_) + dxl->dxl_state_->goal_position_ = dxl->convertValue2Radian(data) - dxl->dxl_state_->position_offset_; // remove offset + else + dxl->dxl_state_->goal_position_ = dxl->convertValue2Radian(data); + } else if (dxl->goal_velocity_item_ != 0 && item->item_name_ == dxl->goal_velocity_item_->item_name_) dxl->dxl_state_->goal_velocity_ = dxl->convertValue2Velocity(data); else if (dxl->goal_current_item_ != 0 && item->item_name_ == dxl->goal_current_item_->item_name_) @@ -1255,7 +1283,12 @@ void RobotisController::process() if (gazebo_mode_ == false) { // add offset - uint32_t pos_data = dxl->convertRadian2Value(dxl_state->goal_position_ + dxl_state->position_offset_); + uint32_t pos_data; + if(is_offset_enabled_) + pos_data= dxl->convertRadian2Value(dxl_state->goal_position_ + dxl_state->position_offset_); + else + pos_data= dxl->convertRadian2Value(dxl_state->goal_position_); + uint8_t sync_write_data[4] = { 0 }; sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(pos_data)); sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(pos_data)); @@ -1653,7 +1686,12 @@ void RobotisController::setJointStatesCallback(const sensor_msgs::JointState::Co if (gazebo_mode_ == false) { // add offset - uint32_t pos_data = dxl->convertRadian2Value(dxl->dxl_state_->goal_position_ + dxl->dxl_state_->position_offset_); + uint32_t pos_data; + if(is_offset_enabled_) + pos_data = dxl->convertRadian2Value(dxl->dxl_state_->goal_position_ + dxl->dxl_state_->position_offset_); + else + pos_data = dxl->convertRadian2Value(dxl->dxl_state_->goal_position_); + uint8_t sync_write_data[4] = { 0 }; sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(pos_data)); sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(pos_data)); @@ -1748,6 +1786,21 @@ bool RobotisController::setCtrlModuleService(robotis_controller_msgs::SetModule: set_module_thread_.join(); + res.result = true; + return true; +} + +bool RobotisController::enableOffsetService(robotis_controller_msgs::EnableOffset::Request &req, robotis_controller_msgs::EnableOffset::Response &res) +{ + is_offset_enabled_ = (bool)req.enable; + res.result = true; + return true; +} + +bool RobotisController::loadOffsetService(robotis_controller_msgs::LoadOffset::Request &req, robotis_controller_msgs::LoadOffset::Response &res) +{ + loadOffset((std::string)req.file_path); + res.result = true; return true; } @@ -1820,7 +1873,12 @@ void RobotisController::setJointCtrlModuleThread(const robotis_controller_msgs:: if(gazebo_mode_ == true) continue; - uint32_t _pos_data = _dxl->convertRadian2Value(_dxl->dxl_state_->goal_position_ + _dxl->dxl_state_->position_offset_); + uint32_t _pos_data; + if(is_offset_enabled_) + _pos_data = _dxl->convertRadian2Value(_dxl->dxl_state_->goal_position_ + _dxl->dxl_state_->position_offset_); + else + _pos_data = _dxl->convertRadian2Value(_dxl->dxl_state_->goal_position_); + uint8_t _sync_write_data[4]; _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_pos_data)); _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_pos_data)); @@ -1858,7 +1916,12 @@ void RobotisController::setJointCtrlModuleThread(const robotis_controller_msgs:: if(_mode == PositionControl) { - uint32_t _pos_data = _dxl->convertRadian2Value(_dxl->dxl_state_->goal_position_ + _dxl->dxl_state_->position_offset_); + uint32_t _pos_data; + if(is_offset_enabled_) + _pos_data = _dxl->convertRadian2Value(_dxl->dxl_state_->goal_position_ + _dxl->dxl_state_->position_offset_); + else + _pos_data = _dxl->convertRadian2Value(_dxl->dxl_state_->goal_position_); + uint8_t _sync_write_data[4]; _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_pos_data)); _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_pos_data)); @@ -2024,7 +2087,12 @@ void RobotisController::setCtrlModuleThread(std::string ctrl_module) if (gazebo_mode_ == true) continue; - uint32_t pos_data = dxl->convertRadian2Value(dxl->dxl_state_->goal_position_ + dxl->dxl_state_->position_offset_); + uint32_t pos_data; + if(is_offset_enabled_) + pos_data = dxl->convertRadian2Value(dxl->dxl_state_->goal_position_ + dxl->dxl_state_->position_offset_); + else + pos_data = dxl->convertRadian2Value(dxl->dxl_state_->goal_position_); + uint8_t sync_write_data[4] = { 0 }; sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(pos_data)); sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(pos_data)); @@ -2062,7 +2130,12 @@ void RobotisController::setCtrlModuleThread(std::string ctrl_module) if (mode == PositionControl) { - uint32_t pos_data = dxl->convertRadian2Value(dxl->dxl_state_->goal_position_ + dxl->dxl_state_->position_offset_); + uint32_t pos_data; + if(is_offset_enabled_) + pos_data = dxl->convertRadian2Value(dxl->dxl_state_->goal_position_ + dxl->dxl_state_->position_offset_); + else + pos_data = dxl->convertRadian2Value(dxl->dxl_state_->goal_position_); + uint8_t sync_write_data[4] = { 0 }; sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(pos_data)); sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(pos_data)); From bc20638cd17bda15e626139b348cadc9501e124f Mon Sep 17 00:00:00 2001 From: Zerom Date: Tue, 29 May 2018 18:38:21 +0900 Subject: [PATCH 130/238] - added include file --- .../include/robotis_controller/robotis_controller.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/robotis_controller/include/robotis_controller/robotis_controller.h b/robotis_controller/include/robotis_controller/robotis_controller.h index a381a62..c2eec4a 100755 --- a/robotis_controller/include/robotis_controller/robotis_controller.h +++ b/robotis_controller/include/robotis_controller/robotis_controller.h @@ -38,6 +38,8 @@ #include "robotis_controller_msgs/GetJointModule.h" #include "robotis_controller_msgs/SetJointModule.h" #include "robotis_controller_msgs/SetModule.h" +#include "robotis_controller_msgs/EnableOffset.h" +#include "robotis_controller_msgs/LoadOffset.h" #include "robotis_device/robot.h" #include "robotis_framework_common/motion_module.h" From b9ed3fecf286a3886dde847c72a59ebf16fccf7b Mon Sep 17 00:00:00 2001 From: zerom Date: Thu, 31 May 2018 14:21:01 +0900 Subject: [PATCH 131/238] - changed enable_offset from service to topic. --- .../robotis_controller/robotis_controller.h | 4 ++-- .../robotis_controller/robotis_controller.cpp | 16 +++++++--------- 2 files changed, 9 insertions(+), 11 deletions(-) diff --git a/robotis_controller/include/robotis_controller/robotis_controller.h b/robotis_controller/include/robotis_controller/robotis_controller.h index c2eec4a..6ee5c26 100755 --- a/robotis_controller/include/robotis_controller/robotis_controller.h +++ b/robotis_controller/include/robotis_controller/robotis_controller.h @@ -28,6 +28,7 @@ #include #include #include +#include #include #include #include @@ -38,7 +39,6 @@ #include "robotis_controller_msgs/GetJointModule.h" #include "robotis_controller_msgs/SetJointModule.h" #include "robotis_controller_msgs/SetModule.h" -#include "robotis_controller_msgs/EnableOffset.h" #include "robotis_controller_msgs/LoadOffset.h" #include "robotis_device/robot.h" @@ -142,10 +142,10 @@ public: void setJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg); void setJointCtrlModuleCallback(const robotis_controller_msgs::JointCtrlModule::ConstPtr &msg); void setCtrlModuleCallback(const std_msgs::String::ConstPtr &msg); + void enableOffsetCallback(const std_msgs::Bool::ConstPtr &msg); bool getJointCtrlModuleService(robotis_controller_msgs::GetJointModule::Request &req, robotis_controller_msgs::GetJointModule::Response &res); bool setJointCtrlModuleService(robotis_controller_msgs::SetJointModule::Request &req, robotis_controller_msgs::SetJointModule::Response &res); bool setCtrlModuleService(robotis_controller_msgs::SetModule::Request &req, robotis_controller_msgs::SetModule::Response &res); - bool enableOffsetService(robotis_controller_msgs::EnableOffset::Request &req, robotis_controller_msgs::EnableOffset::Response &res); bool loadOffsetService(robotis_controller_msgs::LoadOffset::Request &req, robotis_controller_msgs::LoadOffset::Response &res); void gazeboJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg); diff --git a/robotis_controller/src/robotis_controller/robotis_controller.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp index eb8d74e..1e0de91 100755 --- a/robotis_controller/src/robotis_controller/robotis_controller.cpp +++ b/robotis_controller/src/robotis_controller/robotis_controller.cpp @@ -636,6 +636,8 @@ void RobotisController::msgQueueThread() &RobotisController::setControllerModeCallback, this); ros::Subscriber joint_states_sub = ros_node.subscribe("/robotis/set_joint_states", 10, &RobotisController::setJointStatesCallback, this); + ros::Subscriber enable_offset_sub = ros_node.subscribe("/robotis/enable_offset", 10, + &RobotisController::enableOffsetCallback, this); ros::Subscriber gazebo_joint_states_sub; if (gazebo_mode_ == true) @@ -668,8 +670,6 @@ void RobotisController::msgQueueThread() &RobotisController::setJointCtrlModuleService, this); ros::ServiceServer set_module_server = ros_node.advertiseService("/robotis/set_present_ctrl_modules", &RobotisController::setCtrlModuleService, this); - ros::ServiceServer enable_offset_server = ros_node.advertiseService("/robotis/enable_offset", - &RobotisController::enableOffsetService, this); ros::ServiceServer load_offset_server = ros_node.advertiseService("/robotis/load_offset", &RobotisController::loadOffsetService, this); @@ -1735,6 +1735,11 @@ void RobotisController::setJointCtrlModuleCallback(const robotis_controller_msgs set_module_thread_ = boost::thread(boost::bind(&RobotisController::setJointCtrlModuleThread, this, msg)); } +void RobotisController::enableOffsetCallback(const std_msgs::Bool::ConstPtr &msg) +{ + is_offset_enabled_ = (bool)msg->data; +} + bool RobotisController::getJointCtrlModuleService(robotis_controller_msgs::GetJointModule::Request &req, robotis_controller_msgs::GetJointModule::Response &res) { @@ -1790,13 +1795,6 @@ bool RobotisController::setCtrlModuleService(robotis_controller_msgs::SetModule: return true; } -bool RobotisController::enableOffsetService(robotis_controller_msgs::EnableOffset::Request &req, robotis_controller_msgs::EnableOffset::Response &res) -{ - is_offset_enabled_ = (bool)req.enable; - res.result = true; - return true; -} - bool RobotisController::loadOffsetService(robotis_controller_msgs::LoadOffset::Request &req, robotis_controller_msgs::LoadOffset::Response &res) { loadOffset((std::string)req.file_path); From 624b3a810186f2eec906191c9b98ce2c400ea6f3 Mon Sep 17 00:00:00 2001 From: zerom Date: Thu, 31 May 2018 19:45:09 +0900 Subject: [PATCH 132/238] - added XM540 device files --- .../devices/dynamixel/XM540-W150.device | 89 +++++++++++++++++++ .../devices/dynamixel/XM540-W270.device | 89 +++++++++++++++++++ 2 files changed, 178 insertions(+) create mode 100755 robotis_device/devices/dynamixel/XM540-W150.device create mode 100755 robotis_device/devices/dynamixel/XM540-W270.device diff --git a/robotis_device/devices/dynamixel/XM540-W150.device b/robotis_device/devices/dynamixel/XM540-W150.device new file mode 100755 index 0000000..e11c017 --- /dev/null +++ b/robotis_device/devices/dynamixel/XM540-W150.device @@ -0,0 +1,89 @@ +[device info] +model_name = XM540-W150 +device_type = dynamixel + +[type info] +torque_to_current_value_ratio = 289.13672036 +velocity_to_value_ratio = 41.707853 + +value_of_0_radian_position = 2048 +value_of_min_radian_position = 0 +value_of_max_radian_position = 4095 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_current +position_d_gain_item_name = position_d_gain +position_i_gain_item_name = position_i_gain +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 7 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 16 | N + 12 | secondary_id | 1 | RW | EEPROM | 0 | 255 | N + 20 | homing_offset | 4 | RW | EEPROM | -1044479 | 1044479 | Y + 24 | moving_threshold | 4 | RW | EEPROM | 0 | 1023 | N + 31 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 32 | max_voltage_limit | 2 | RW | EEPROM | 95 | 160 | N + 34 | min_voltage_limit | 2 | RW | EEPROM | 95 | 160 | N + 36 | pwm_limit | 2 | RW | EEPROM | 0 | 885 | N + 38 | current_limit | 2 | RW | EEPROM | 0 | 1193 | N + 40 | acceleration_limit | 4 | RW | EEPROM | 0 | 32767 | N + 44 | velocity_limit | 4 | RW | EEPROM | 0 | 1023 | N + 48 | max_position_limit | 4 | RW | EEPROM | 0 | 4095 | N + 52 | min_position_limit | 4 | RW | EEPROM | 0 | 4095 | N + 56 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 57 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 58 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 63 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 64 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 65 | LED | 1 | RW | RAM | 0 | 1 | N + 68 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 69 | registered_instruction | 1 | R | RAM | 0 | 1 | N + 70 | hardware_error_status | 1 | R | RAM | 0 | 255 | N + 76 | velocity_i_gain | 2 | RW | RAM | 0 | 32767 | N + 78 | velocity_p_gain | 2 | RW | RAM | 0 | 32767 | N + 80 | position_d_gain | 2 | RW | RAM | 0 | 32767 | N + 82 | position_i_gain | 2 | RW | RAM | 0 | 32767 | N + 84 | position_p_gain | 2 | RW | RAM | 0 | 32767 | N + 88 | feedforward_2nd_gain | 2 | RW | RAM | 0 | 32767 | N + 90 | feedforward_1st_gain | 2 | RW | RAM | 0 | 32767 | N + 98 | bus_watchdog | 1 | RW | RAM | -1 | 127 | Y + 100 | goal_pwm | 2 | RW | RAM | 0 | 885 | N + 102 | goal_current | 2 | RW | RAM | 0 | 1193 | N + 104 | goal_velocity | 4 | RW | RAM | 0 | 1023 | N + 108 | profile_acceleration | 4 | RW | RAM | 0 | 32767 | N + 112 | profile_velocity | 4 | RW | RAM | 0 | 1023 | N + 116 | goal_position | 4 | RW | RAM | -1048575 | 1048575 | Y + 120 | realtime_tick | 2 | R | RAM | 0 | 32767 | N + 122 | moving | 1 | R | RAM | 0 | 1 | N + 123 | moving_status | 1 | R | RAM | 0 | 255 | N + 124 | present_pwm | 2 | R | RAM | 0 | 885 | N + 126 | present_current | 2 | R | RAM | 0 | 1193 | N + 128 | present_velocity | 4 | R | RAM | 0 | 1023 | N + 132 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 136 | velocity_trajectory | 4 | R | RAM | 0 | 1023 | N + 140 | position_trajectory | 4 | R | RAM | 0 | 4095 | N + 144 | present_input_voltage | 2 | R | RAM | 95 | 160 | N + 146 | present_temperature | 1 | R | RAM | 0 | 100 | N + 152 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 154 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 156 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 168 | indirect_address_1 | 2 | RW | RAM | 0 | 65535 | N + 224 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 578 | indirect_address_29 | 2 | RW | RAM | 0 | 65535 | N + 634 | indirect_data_29 | 1 | RW | RAM | 0 | 255 | N diff --git a/robotis_device/devices/dynamixel/XM540-W270.device b/robotis_device/devices/dynamixel/XM540-W270.device new file mode 100755 index 0000000..897f398 --- /dev/null +++ b/robotis_device/devices/dynamixel/XM540-W270.device @@ -0,0 +1,89 @@ +[device info] +model_name = XM540-W270 +device_type = dynamixel + +[type info] +torque_to_current_value_ratio = 156.133829 +velocity_to_value_ratio = 41.707853 + +value_of_0_radian_position = 2048 +value_of_min_radian_position = 0 +value_of_max_radian_position = 4095 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_current +position_d_gain_item_name = position_d_gain +position_i_gain_item_name = position_i_gain +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 7 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 16 | N + 12 | secondary_id | 1 | RW | EEPROM | 0 | 255 | N + 20 | homing_offset | 4 | RW | EEPROM | -1044479 | 1044479 | Y + 24 | moving_threshold | 4 | RW | EEPROM | 0 | 1023 | N + 31 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 32 | max_voltage_limit | 2 | RW | EEPROM | 95 | 160 | N + 34 | min_voltage_limit | 2 | RW | EEPROM | 95 | 160 | N + 36 | pwm_limit | 2 | RW | EEPROM | 0 | 885 | N + 38 | current_limit | 2 | RW | EEPROM | 0 | 1193 | N + 40 | acceleration_limit | 4 | RW | EEPROM | 0 | 32767 | N + 44 | velocity_limit | 4 | RW | EEPROM | 0 | 1023 | N + 48 | max_position_limit | 4 | RW | EEPROM | 0 | 4095 | N + 52 | min_position_limit | 4 | RW | EEPROM | 0 | 4095 | N + 56 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 57 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 58 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 63 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 64 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 65 | LED | 1 | RW | RAM | 0 | 1 | N + 68 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 69 | registered_instruction | 1 | R | RAM | 0 | 1 | N + 70 | hardware_error_status | 1 | R | RAM | 0 | 255 | N + 76 | velocity_i_gain | 2 | RW | RAM | 0 | 32767 | N + 78 | velocity_p_gain | 2 | RW | RAM | 0 | 32767 | N + 80 | position_d_gain | 2 | RW | RAM | 0 | 32767 | N + 82 | position_i_gain | 2 | RW | RAM | 0 | 32767 | N + 84 | position_p_gain | 2 | RW | RAM | 0 | 32767 | N + 88 | feedforward_2nd_gain | 2 | RW | RAM | 0 | 32767 | N + 90 | feedforward_1st_gain | 2 | RW | RAM | 0 | 32767 | N + 98 | bus_watchdog | 1 | RW | RAM | -1 | 127 | Y + 100 | goal_pwm | 2 | RW | RAM | 0 | 885 | N + 102 | goal_current | 2 | RW | RAM | 0 | 1193 | N + 104 | goal_velocity | 4 | RW | RAM | 0 | 1023 | N + 108 | profile_acceleration | 4 | RW | RAM | 0 | 32767 | N + 112 | profile_velocity | 4 | RW | RAM | 0 | 1023 | N + 116 | goal_position | 4 | RW | RAM | -1048575 | 1048575 | Y + 120 | realtime_tick | 2 | R | RAM | 0 | 32767 | N + 122 | moving | 1 | R | RAM | 0 | 1 | N + 123 | moving_status | 1 | R | RAM | 0 | 255 | N + 124 | present_pwm | 2 | R | RAM | 0 | 885 | N + 126 | present_current | 2 | R | RAM | 0 | 1193 | N + 128 | present_velocity | 4 | R | RAM | 0 | 1023 | N + 132 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 136 | velocity_trajectory | 4 | R | RAM | 0 | 1023 | N + 140 | position_trajectory | 4 | R | RAM | 0 | 4095 | N + 144 | present_input_voltage | 2 | R | RAM | 95 | 160 | N + 146 | present_temperature | 1 | R | RAM | 0 | 100 | N + 152 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 154 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 156 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 168 | indirect_address_1 | 2 | RW | RAM | 0 | 65535 | N + 224 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 578 | indirect_address_29 | 2 | RW | RAM | 0 | 65535 | N + 634 | indirect_data_29 | 1 | RW | RAM | 0 | 255 | N From 0851b50b5a600229b96a786945969aa0663ec48b Mon Sep 17 00:00:00 2001 From: zerom Date: Fri, 1 Jun 2018 13:33:01 +0900 Subject: [PATCH 133/238] - fixed motion module result gain bug --- .../src/robotis_controller/robotis_controller.cpp | 12 ++++++------ .../include/robotis_device/dynamixel_state.h | 12 ++++++------ 2 files changed, 12 insertions(+), 12 deletions(-) diff --git a/robotis_controller/src/robotis_controller/robotis_controller.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp index 000bd59..1f78056 100755 --- a/robotis_controller/src/robotis_controller/robotis_controller.cpp +++ b/robotis_controller/src/robotis_controller/robotis_controller.cpp @@ -1266,7 +1266,7 @@ void RobotisController::process() port_to_sync_write_position_[dxl->port_name_]->changeParam(dxl->id_, sync_write_data); // if position p gain value is changed -> sync write - if (dxl_state->position_p_gain_ != result_state->position_p_gain_) + if (result_state->position_p_gain_ != 65535 && dxl_state->position_p_gain_ != result_state->position_p_gain_) { dxl_state->position_p_gain_ = result_state->position_p_gain_; uint8_t sync_write_data[4] = { 0 }; @@ -1280,7 +1280,7 @@ void RobotisController::process() } // if position i gain value is changed -> sync write - if (dxl_state->position_i_gain_ != result_state->position_i_gain_) + if (result_state->position_i_gain_ != 65535 && dxl_state->position_i_gain_ != result_state->position_i_gain_) { dxl_state->position_i_gain_ = result_state->position_i_gain_; uint8_t sync_write_data[4] = { 0 }; @@ -1294,7 +1294,7 @@ void RobotisController::process() } // if position d gain value is changed -> sync write - if (dxl_state->position_d_gain_ != result_state->position_d_gain_) + if (result_state->position_d_gain_ != 65535 && dxl_state->position_d_gain_ != result_state->position_d_gain_) { dxl_state->position_d_gain_ = result_state->position_d_gain_; uint8_t sync_write_data[4] = { 0 }; @@ -1325,7 +1325,7 @@ void RobotisController::process() port_to_sync_write_velocity_[dxl->port_name_]->changeParam(dxl->id_, sync_write_data); // if velocity p gain gain value is changed -> sync write - if (dxl_state->velocity_p_gain_ != result_state->velocity_p_gain_) + if (result_state->velocity_p_gain_ != 65535 && dxl_state->velocity_p_gain_ != result_state->velocity_p_gain_) { dxl_state->velocity_p_gain_ = result_state->velocity_p_gain_; uint8_t sync_write_data[4] = { 0 }; @@ -1339,7 +1339,7 @@ void RobotisController::process() } // if velocity i gain value is changed -> sync write - if (dxl_state->velocity_i_gain_ != result_state->velocity_i_gain_) + if (result_state->velocity_i_gain_ != 65535 && dxl_state->velocity_i_gain_ != result_state->velocity_i_gain_) { dxl_state->velocity_i_gain_ = result_state->velocity_i_gain_; uint8_t sync_write_data[4] = { 0 }; @@ -1353,7 +1353,7 @@ void RobotisController::process() } // if velocity d gain value is changed -> sync write - if (dxl_state->velocity_d_gain_ != result_state->velocity_d_gain_) + if (result_state->velocity_d_gain_ != 65535 && dxl_state->velocity_d_gain_ != result_state->velocity_d_gain_) { dxl_state->velocity_d_gain_ = result_state->velocity_d_gain_; uint8_t sync_write_data[4] = { 0 }; diff --git a/robotis_device/include/robotis_device/dynamixel_state.h b/robotis_device/include/robotis_device/dynamixel_state.h index b287bb3..05116c6 100755 --- a/robotis_device/include/robotis_device/dynamixel_state.h +++ b/robotis_device/include/robotis_device/dynamixel_state.h @@ -64,12 +64,12 @@ public: goal_position_(0.0), goal_velocity_(0.0), goal_torque_(0.0), - position_p_gain_(0), - position_i_gain_(0), - position_d_gain_(0), - velocity_p_gain_(0), - velocity_i_gain_(0), - velocity_d_gain_(0), + position_p_gain_(65535), + position_i_gain_(65535), + position_d_gain_(65535), + velocity_p_gain_(65535), + velocity_i_gain_(65535), + velocity_d_gain_(65535), position_offset_(0) { bulk_read_table_.clear(); From e4f0066b77311756eb723bf23b41001e8f0bb892 Mon Sep 17 00:00:00 2001 From: Kayman Date: Thu, 19 Jul 2018 15:54:37 +0900 Subject: [PATCH 134/238] Update .travis.yml --- .travis.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.travis.yml b/.travis.yml index a101867..53f5d3e 100644 --- a/.travis.yml +++ b/.travis.yml @@ -18,9 +18,9 @@ notifications: - pyo@robotis.com env: matrix: - - ROS_DISTRO=kinetic ROS_REPO=ros-shadow-fixed UPSTREAM_WORKSPACE=debian +# - ROS_DISTRO=kinetic ROS_REPO=ros-shadow-fixed UPSTREAM_WORKSPACE=debian # - ROS_DISTRO=kinetic ROS_REPO=ros-shadow-fixed UPSTREAM_WORKSPACE=debian OS_NAME=debian OS_CODE_NAME=jessie -# - ROS_DISTRO=kinetic ROS_REPO=ros UPSTREAM_WORKSPACE=file OS_NAME=debian OS_CODE_NAME=jessie $ROSINSTALL_FILENAME=".robotis_framework.rosinstall" + - ROS_DISTRO=kinetic ROS_REPO=ros UPSTREAM_WORKSPACE=file OS_NAME=debian OS_CODE_NAME=jessie $ROSINSTALL_FILENAME=".robotis_framework.rosinstall" branches: only: - master From 8325af31124741a68e097f41f8c661b94d47e450 Mon Sep 17 00:00:00 2001 From: Kayman Date: Thu, 19 Jul 2018 15:54:37 +0900 Subject: [PATCH 135/238] Update .travis.yml --- .travis.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.travis.yml b/.travis.yml index a101867..53f5d3e 100644 --- a/.travis.yml +++ b/.travis.yml @@ -18,9 +18,9 @@ notifications: - pyo@robotis.com env: matrix: - - ROS_DISTRO=kinetic ROS_REPO=ros-shadow-fixed UPSTREAM_WORKSPACE=debian +# - ROS_DISTRO=kinetic ROS_REPO=ros-shadow-fixed UPSTREAM_WORKSPACE=debian # - ROS_DISTRO=kinetic ROS_REPO=ros-shadow-fixed UPSTREAM_WORKSPACE=debian OS_NAME=debian OS_CODE_NAME=jessie -# - ROS_DISTRO=kinetic ROS_REPO=ros UPSTREAM_WORKSPACE=file OS_NAME=debian OS_CODE_NAME=jessie $ROSINSTALL_FILENAME=".robotis_framework.rosinstall" + - ROS_DISTRO=kinetic ROS_REPO=ros UPSTREAM_WORKSPACE=file OS_NAME=debian OS_CODE_NAME=jessie $ROSINSTALL_FILENAME=".robotis_framework.rosinstall" branches: only: - master From cd58b92487b701380ff606c6f430e891f1c12ad2 Mon Sep 17 00:00:00 2001 From: Kayman Date: Thu, 19 Jul 2018 18:29:40 +0900 Subject: [PATCH 136/238] Update .travis.yml --- .travis.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.travis.yml b/.travis.yml index 53f5d3e..695ee46 100644 --- a/.travis.yml +++ b/.travis.yml @@ -20,7 +20,7 @@ env: matrix: # - ROS_DISTRO=kinetic ROS_REPO=ros-shadow-fixed UPSTREAM_WORKSPACE=debian # - ROS_DISTRO=kinetic ROS_REPO=ros-shadow-fixed UPSTREAM_WORKSPACE=debian OS_NAME=debian OS_CODE_NAME=jessie - - ROS_DISTRO=kinetic ROS_REPO=ros UPSTREAM_WORKSPACE=file OS_NAME=debian OS_CODE_NAME=jessie $ROSINSTALL_FILENAME=".robotis_framework.rosinstall" + - ROS_DISTRO=kinetic ROS_REPO=ros-shadow-fixed UPSTREAM_WORKSPACE=file OS_NAME=ubuntu OS_CODE_NAME=xenial $ROSINSTALL_FILENAME=".robotis_framework.rosinstall" branches: only: - master From 7ed03c668e1314bb516b44eb7ee5b977973b2280 Mon Sep 17 00:00:00 2001 From: Kayman Date: Thu, 19 Jul 2018 18:29:40 +0900 Subject: [PATCH 137/238] Update .travis.yml --- .travis.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.travis.yml b/.travis.yml index 53f5d3e..695ee46 100644 --- a/.travis.yml +++ b/.travis.yml @@ -20,7 +20,7 @@ env: matrix: # - ROS_DISTRO=kinetic ROS_REPO=ros-shadow-fixed UPSTREAM_WORKSPACE=debian # - ROS_DISTRO=kinetic ROS_REPO=ros-shadow-fixed UPSTREAM_WORKSPACE=debian OS_NAME=debian OS_CODE_NAME=jessie - - ROS_DISTRO=kinetic ROS_REPO=ros UPSTREAM_WORKSPACE=file OS_NAME=debian OS_CODE_NAME=jessie $ROSINSTALL_FILENAME=".robotis_framework.rosinstall" + - ROS_DISTRO=kinetic ROS_REPO=ros-shadow-fixed UPSTREAM_WORKSPACE=file OS_NAME=ubuntu OS_CODE_NAME=xenial $ROSINSTALL_FILENAME=".robotis_framework.rosinstall" branches: only: - master From 7b7250c4e55ebcb932cf4cfe55408fc6617610ba Mon Sep 17 00:00:00 2001 From: zerom Date: Mon, 5 Nov 2018 11:43:49 +0900 Subject: [PATCH 138/238] - changed X-Series control tables. --- robotis_device/devices/dynamixel/XM-430.device | 4 +++- .../dynamixel/{XM-430-W210.device => XM430-W210.device} | 6 ++++-- .../dynamixel/{XM-430-W350.device => XM430-W350.device} | 6 ++++-- robotis_device/devices/dynamixel/XM540-W150.device | 2 +- robotis_device/devices/dynamixel/XM540-W270.device | 2 +- 5 files changed, 13 insertions(+), 7 deletions(-) rename robotis_device/devices/dynamixel/{XM-430-W210.device => XM430-W210.device} (93%) mode change 100755 => 100644 rename robotis_device/devices/dynamixel/{XM-430-W350.device => XM430-W350.device} (93%) mode change 100755 => 100644 diff --git a/robotis_device/devices/dynamixel/XM-430.device b/robotis_device/devices/dynamixel/XM-430.device index fadcfaf..7dfe91d 100755 --- a/robotis_device/devices/dynamixel/XM-430.device +++ b/robotis_device/devices/dynamixel/XM-430.device @@ -32,7 +32,9 @@ velocity_p_gain_item_name = velocity_p_gain 7 | ID | 1 | RW | EEPROM | 0 | 252 | N 8 | baudrate | 1 | RW | EEPROM | 0 | 7 | N 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 10 | drive_mode | 1 | RW | EEPROM | 0 | 1 | N 11 | operating_mode | 1 | RW | EEPROM | 0 | 16 | N + 12 | secondary_id | 1 | RW | EEPROM | 0 | 255 | N 20 | homing_offset | 4 | RW | EEPROM | -1044479 | 1044479 | Y 24 | moving_threshold | 4 | RW | EEPROM | 0 | 1023 | N 31 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N @@ -40,7 +42,6 @@ velocity_p_gain_item_name = velocity_p_gain 34 | min_voltage_limit | 2 | RW | EEPROM | 95 | 160 | N 36 | pwm_limit | 2 | RW | EEPROM | 0 | 885 | N 38 | current_limit | 2 | RW | EEPROM | 0 | 1193 | N - 40 | acceleration_limit | 4 | RW | EEPROM | 0 | 32767 | N 44 | velocity_limit | 4 | RW | EEPROM | 0 | 1023 | N 48 | max_position_limit | 4 | RW | EEPROM | 0 | 4095 | N 52 | min_position_limit | 4 | RW | EEPROM | 0 | 4095 | N @@ -57,6 +58,7 @@ velocity_p_gain_item_name = velocity_p_gain 84 | position_p_gain | 2 | RW | RAM | 0 | 32767 | N 88 | feedforward_2nd_gain | 2 | RW | RAM | 0 | 32767 | N 90 | feedforward_1st_gain | 2 | RW | RAM | 0 | 32767 | N + 98 | bus_watchdog | 1 | RW | RAM | -1 | 127 | Y 100 | goal_pwm | 2 | RW | RAM | 0 | 885 | N 102 | goal_current | 2 | RW | RAM | 0 | 1193 | N 104 | goal_velocity | 4 | RW | RAM | 0 | 1023 | N diff --git a/robotis_device/devices/dynamixel/XM-430-W210.device b/robotis_device/devices/dynamixel/XM430-W210.device old mode 100755 new mode 100644 similarity index 93% rename from robotis_device/devices/dynamixel/XM-430-W210.device rename to robotis_device/devices/dynamixel/XM430-W210.device index df823c8..6113e04 --- a/robotis_device/devices/dynamixel/XM-430-W210.device +++ b/robotis_device/devices/dynamixel/XM430-W210.device @@ -1,5 +1,5 @@ [device info] -model_name = XM-430-W210 +model_name = XM430-W210 device_type = dynamixel [type info] @@ -33,7 +33,9 @@ velocity_p_gain_item_name = velocity_p_gain 7 | ID | 1 | RW | EEPROM | 0 | 252 | N 8 | baudrate | 1 | RW | EEPROM | 0 | 7 | N 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 10 | drive_mode | 1 | RW | EEPROM | 0 | 1 | N 11 | operating_mode | 1 | RW | EEPROM | 0 | 16 | N + 12 | secondary_id | 1 | RW | EEPROM | 0 | 255 | N 20 | homing_offset | 4 | RW | EEPROM | -1044479 | 1044479 | Y 24 | moving_threshold | 4 | RW | EEPROM | 0 | 1023 | N 31 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N @@ -41,7 +43,6 @@ velocity_p_gain_item_name = velocity_p_gain 34 | min_voltage_limit | 2 | RW | EEPROM | 95 | 160 | N 36 | pwm_limit | 2 | RW | EEPROM | 0 | 885 | N 38 | current_limit | 2 | RW | EEPROM | 0 | 1193 | N - 40 | acceleration_limit | 4 | RW | EEPROM | 0 | 32767 | N 44 | velocity_limit | 4 | RW | EEPROM | 0 | 1023 | N 48 | max_position_limit | 4 | RW | EEPROM | 0 | 4095 | N 52 | min_position_limit | 4 | RW | EEPROM | 0 | 4095 | N @@ -58,6 +59,7 @@ velocity_p_gain_item_name = velocity_p_gain 84 | position_p_gain | 2 | RW | RAM | 0 | 32767 | N 88 | feedforward_2nd_gain | 2 | RW | RAM | 0 | 32767 | N 90 | feedforward_1st_gain | 2 | RW | RAM | 0 | 32767 | N + 98 | bus_watchdog | 1 | RW | RAM | -1 | 127 | Y 100 | goal_pwm | 2 | RW | RAM | 0 | 885 | N 102 | goal_current | 2 | RW | RAM | 0 | 1193 | N 104 | goal_velocity | 4 | RW | RAM | 0 | 1023 | N diff --git a/robotis_device/devices/dynamixel/XM-430-W350.device b/robotis_device/devices/dynamixel/XM430-W350.device old mode 100755 new mode 100644 similarity index 93% rename from robotis_device/devices/dynamixel/XM-430-W350.device rename to robotis_device/devices/dynamixel/XM430-W350.device index ce6e895..e1be362 --- a/robotis_device/devices/dynamixel/XM-430-W350.device +++ b/robotis_device/devices/dynamixel/XM430-W350.device @@ -1,5 +1,5 @@ [device info] -model_name = XM-430-W350 +model_name = XM430-W350 device_type = dynamixel [type info] @@ -33,7 +33,9 @@ velocity_p_gain_item_name = velocity_p_gain 7 | ID | 1 | RW | EEPROM | 0 | 252 | N 8 | baudrate | 1 | RW | EEPROM | 0 | 7 | N 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 10 | drive_mode | 1 | RW | EEPROM | 0 | 1 | N 11 | operating_mode | 1 | RW | EEPROM | 0 | 16 | N + 12 | secondary_id | 1 | RW | EEPROM | 0 | 255 | N 20 | homing_offset | 4 | RW | EEPROM | -1044479 | 1044479 | Y 24 | moving_threshold | 4 | RW | EEPROM | 0 | 1023 | N 31 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N @@ -41,7 +43,6 @@ velocity_p_gain_item_name = velocity_p_gain 34 | min_voltage_limit | 2 | RW | EEPROM | 95 | 160 | N 36 | pwm_limit | 2 | RW | EEPROM | 0 | 885 | N 38 | current_limit | 2 | RW | EEPROM | 0 | 1193 | N - 40 | acceleration_limit | 4 | RW | EEPROM | 0 | 32767 | N 44 | velocity_limit | 4 | RW | EEPROM | 0 | 1023 | N 48 | max_position_limit | 4 | RW | EEPROM | 0 | 4095 | N 52 | min_position_limit | 4 | RW | EEPROM | 0 | 4095 | N @@ -58,6 +59,7 @@ velocity_p_gain_item_name = velocity_p_gain 84 | position_p_gain | 2 | RW | RAM | 0 | 32767 | N 88 | feedforward_2nd_gain | 2 | RW | RAM | 0 | 32767 | N 90 | feedforward_1st_gain | 2 | RW | RAM | 0 | 32767 | N + 98 | bus_watchdog | 1 | RW | RAM | -1 | 127 | Y 100 | goal_pwm | 2 | RW | RAM | 0 | 885 | N 102 | goal_current | 2 | RW | RAM | 0 | 1193 | N 104 | goal_velocity | 4 | RW | RAM | 0 | 1023 | N diff --git a/robotis_device/devices/dynamixel/XM540-W150.device b/robotis_device/devices/dynamixel/XM540-W150.device index e11c017..64fcf3b 100755 --- a/robotis_device/devices/dynamixel/XM540-W150.device +++ b/robotis_device/devices/dynamixel/XM540-W150.device @@ -33,6 +33,7 @@ velocity_p_gain_item_name = velocity_p_gain 7 | ID | 1 | RW | EEPROM | 0 | 252 | N 8 | baudrate | 1 | RW | EEPROM | 0 | 7 | N 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 10 | drive_mode | 1 | RW | EEPROM | 0 | 1 | N 11 | operating_mode | 1 | RW | EEPROM | 0 | 16 | N 12 | secondary_id | 1 | RW | EEPROM | 0 | 255 | N 20 | homing_offset | 4 | RW | EEPROM | -1044479 | 1044479 | Y @@ -42,7 +43,6 @@ velocity_p_gain_item_name = velocity_p_gain 34 | min_voltage_limit | 2 | RW | EEPROM | 95 | 160 | N 36 | pwm_limit | 2 | RW | EEPROM | 0 | 885 | N 38 | current_limit | 2 | RW | EEPROM | 0 | 1193 | N - 40 | acceleration_limit | 4 | RW | EEPROM | 0 | 32767 | N 44 | velocity_limit | 4 | RW | EEPROM | 0 | 1023 | N 48 | max_position_limit | 4 | RW | EEPROM | 0 | 4095 | N 52 | min_position_limit | 4 | RW | EEPROM | 0 | 4095 | N diff --git a/robotis_device/devices/dynamixel/XM540-W270.device b/robotis_device/devices/dynamixel/XM540-W270.device index 897f398..c787cba 100755 --- a/robotis_device/devices/dynamixel/XM540-W270.device +++ b/robotis_device/devices/dynamixel/XM540-W270.device @@ -33,6 +33,7 @@ velocity_p_gain_item_name = velocity_p_gain 7 | ID | 1 | RW | EEPROM | 0 | 252 | N 8 | baudrate | 1 | RW | EEPROM | 0 | 7 | N 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 10 | drive_mode | 1 | RW | EEPROM | 0 | 1 | N 11 | operating_mode | 1 | RW | EEPROM | 0 | 16 | N 12 | secondary_id | 1 | RW | EEPROM | 0 | 255 | N 20 | homing_offset | 4 | RW | EEPROM | -1044479 | 1044479 | Y @@ -42,7 +43,6 @@ velocity_p_gain_item_name = velocity_p_gain 34 | min_voltage_limit | 2 | RW | EEPROM | 95 | 160 | N 36 | pwm_limit | 2 | RW | EEPROM | 0 | 885 | N 38 | current_limit | 2 | RW | EEPROM | 0 | 1193 | N - 40 | acceleration_limit | 4 | RW | EEPROM | 0 | 32767 | N 44 | velocity_limit | 4 | RW | EEPROM | 0 | 1023 | N 48 | max_position_limit | 4 | RW | EEPROM | 0 | 4095 | N 52 | min_position_limit | 4 | RW | EEPROM | 0 | 4095 | N From edde9d26cbb0f7d30e53ac038dbfc9618ae5ec94 Mon Sep 17 00:00:00 2001 From: zerom Date: Thu, 20 Dec 2018 16:56:26 +0900 Subject: [PATCH 139/238] - gazebo bug fixed (when ctrl_module_name_ == "none") --- .../robotis_controller/robotis_controller.cpp | 17 +++++++++++++++-- 1 file changed, 15 insertions(+), 2 deletions(-) diff --git a/robotis_controller/src/robotis_controller/robotis_controller.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp index 8a7ee7d..7e59385 100755 --- a/robotis_controller/src/robotis_controller/robotis_controller.cpp +++ b/robotis_controller/src/robotis_controller/robotis_controller.cpp @@ -1097,13 +1097,26 @@ void RobotisController::process() } else if (gazebo_mode_ == true) { + std_msgs::Float64 joint_msg; + + for (auto& dxl_it : robot_->dxls_) + { + std::string joint_name = dxl_it.first; + Dynamixel *dxl = dxl_it.second; + DynamixelState *dxl_state = dxl_it.second->dxl_state_; + + if (dxl->ctrl_module_name_ == "none") + { + joint_msg.data = dxl_state->goal_position_; + gazebo_joint_position_pub_[joint_name].publish(joint_msg); + } + } + for (auto module_it = motion_modules_.begin(); module_it != motion_modules_.end(); module_it++) { if ((*module_it)->getModuleEnable() == false) continue; - std_msgs::Float64 joint_msg; - for (auto& dxl_it : robot_->dxls_) { std::string joint_name = dxl_it.first; From 01c1fb47a2b10e5ec927d00ddcc5c28c3d71f4d2 Mon Sep 17 00:00:00 2001 From: zerom Date: Thu, 3 Jan 2019 16:20:00 +0900 Subject: [PATCH 140/238] - added RH-P12-RN(A).device --- .../devices/dynamixel/RH-P12-RN(A).device | 90 +++++++++++++++++++ .../devices/dynamixel/RH-P12-RN.device | 2 +- 2 files changed, 91 insertions(+), 1 deletion(-) create mode 100644 robotis_device/devices/dynamixel/RH-P12-RN(A).device diff --git a/robotis_device/devices/dynamixel/RH-P12-RN(A).device b/robotis_device/devices/dynamixel/RH-P12-RN(A).device new file mode 100644 index 0000000..31d253b --- /dev/null +++ b/robotis_device/devices/dynamixel/RH-P12-RN(A).device @@ -0,0 +1,90 @@ +[device info] +model_name = RH-P12-RN(A) +device_type = dynamixel + +[type info] +value_of_0_radian_position = 0 +value_of_min_radian_position = 0 +value_of_max_radian_position = 740 +min_radian = 0 +max_radian = 1.150767 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_current +position_d_gain_item_name = position_d_gain +position_i_gain_item_name = position_i_gain +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 9 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 10 | drive_mode | 1 | RW | EEPROM | 0 | 1 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 12 | secondary_ID | 1 | RW | EEPROM | 0 | 255 | N + 20 | homing_offset | 4 | RW | EEPROM | -1150 | 1150 | Y + 24 | moving_threshold | 4 | RW | EEPROM | 0 | 2970 | N + 31 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 32 | max_voltage_limit | 2 | RW | EEPROM | 0 | 350 | N + 34 | min_voltage_limit | 2 | RW | EEPROM | 0 | 350 | N + 36 | pwm_limit | 2 | RW | EEPROM | 0 | 2009 | N + 38 | current_limit | 2 | RW | EEPROM | 0 | 1984 | N + 40 | acceleration_limit | 4 | RW | EEPROM | 0 | 1378788 | N + 44 | velocity_limit | 4 | RW | EEPROM | 0 | 2970 | N + 48 | max_position_limit | 4 | RW | EEPROM | 0 | 1150 | N + 52 | min_position_limit | 4 | RW | EEPROM | 0 | 1150 | N + 56 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 57 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 58 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 59 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 63 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 168 | indirect_address_1 | 2 | RW | EEPROM | 512 | 1023 | N + 512 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 513 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 514 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 515 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 516 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 517 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 518 | hardware_error_status | 2 | R | RAM | 0 | 63 | N + 524 | velocity_i_gain | 2 | RW | RAM | 0 | 32767 | N + 526 | velocity_p_gain | 2 | RW | RAM | 0 | 32767 | N + 528 | position_d_gain | 2 | RW | RAM | 0 | 32767 | N + 530 | position_i_gain | 2 | RW | RAM | 0 | 32767 | N + 532 | position_p_gain | 2 | RW | RAM | 0 | 32767 | N + 536 | feedforward_2nd_gain | 2 | RW | RAM | 0 | 32767 | N + 538 | feedforward_1st_gain | 2 | RW | RAM | 0 | 32767 | N + 546 | bus_watchdog | 1 | RW | RAM | -1 | 127 | Y + 548 | goal_pwm | 2 | RW | RAM | -2009 | 2009 | Y + 550 | goal_current | 2 | RW | RAM | -1984 | 1984 | Y + 552 | goal_velocity | 4 | RW | RAM | -2970 | 2970 | Y + 556 | profile_acceleration | 4 | RW | RAM | 0 | 1378788 | N + 560 | profile_velocity | 4 | RW | RAM | 0 | 2970 | N + 564 | goal_position | 4 | RW | RAM | 0 | 1150 | N + 568 | realtime_tick | 2 | R | RAM | 0 | 32767 | N + 570 | is_moving | 1 | R | RAM | 0 | 1 | N + 571 | moving_status | 1 | R | RAM | 0 | 255 | N + 572 | present_pwm | 2 | R | RAM | -2009 | 2009 | Y + 574 | present_current | 2 | R | RAM | -1984 | 1984 | Y + 576 | present_velocity | 4 | R | RAM | -2970 | 2970 | Y + 580 | present_position | 4 | R | RAM | -1150 | 1150 | Y + 584 | velocity_trajectory | 4 | R | RAM | -2147483648 | 2147483648 | Y + 588 | position_trajectory | 4 | R | RAM | -2147483648 | 2147483648 | Y + 592 | present_voltage | 2 | R | RAM | 0 | 350 | N + 594 | present_temperature | 1 | R | RAM | 0 | 100 | N + 595 | grip_detection | 1 | R | RAM | 0 | 1 | N + 600 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 602 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 604 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 606 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N diff --git a/robotis_device/devices/dynamixel/RH-P12-RN.device b/robotis_device/devices/dynamixel/RH-P12-RN.device index aec9b67..14ed73b 100755 --- a/robotis_device/devices/dynamixel/RH-P12-RN.device +++ b/robotis_device/devices/dynamixel/RH-P12-RN.device @@ -30,7 +30,7 @@ velocity_p_gain_item_name = 7 | ID | 1 | RW | EEPROM | 0 | 252 | N 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N - 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 5 | N 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N From c0898da3ee481f395a08ebe7b1010b60b7a75e0d Mon Sep 17 00:00:00 2001 From: zerom Date: Fri, 4 Jan 2019 17:49:51 +0900 Subject: [PATCH 141/238] - added DYNAMIXEL PRO+ device file. --- .../devices/dynamixel/H42P-020-S300-R.device | 92 +++++++++++++++++++ .../devices/dynamixel/H54P-100-S500-R.device | 92 +++++++++++++++++++ .../devices/dynamixel/H54P-200-S500-R.device | 92 +++++++++++++++++++ 3 files changed, 276 insertions(+) create mode 100644 robotis_device/devices/dynamixel/H42P-020-S300-R.device create mode 100644 robotis_device/devices/dynamixel/H54P-100-S500-R.device create mode 100644 robotis_device/devices/dynamixel/H54P-200-S500-R.device diff --git a/robotis_device/devices/dynamixel/H42P-020-S300-R.device b/robotis_device/devices/dynamixel/H42P-020-S300-R.device new file mode 100644 index 0000000..92d405a --- /dev/null +++ b/robotis_device/devices/dynamixel/H42P-020-S300-R.device @@ -0,0 +1,92 @@ +[device info] +model_name = H42P-020-S300-R +device_type = dynamixel + +[type info] +torque_to_current_value_ratio = 109.3747778 +velocity_to_value_ratio = 954.93 +value_of_0_radian_position = 0 +value_of_min_radian_position = -303750 +value_of_max_radian_position = 303750 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_current +position_d_gain_item_name = position_d_gain +position_i_gain_item_name = position_i_gain +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 10 | drive_mode | 1 | RW | EEPROM | 0 | 3 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 12 | secondary_id | 1 | RW | EEPROM | 0 | 255 | N + 13 | protocol_version | 1 | RW | EEPROM | 1 | 2 | N + 20 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 24 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 31 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 32 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 34 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 36 | pwm_limit | 2 | RW | EEPROM | 0 | 885 | N + 38 | current_limit | 2 | RW | EEPROM | 0 | 32767 | N + 40 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 44 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 48 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 52 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 56 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 57 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 58 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 59 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 63 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 168 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 512 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 513 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 514 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 515 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 516 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 517 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 518 | hardware_error_status | 1 | R | RAM | 0 | 31 | N + 524 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N + 526 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N + 528 | position_d_gain | 2 | RW | RAM | 0 | 2047 | N + 530 | position_i_gain | 2 | RW | RAM | 0 | 2047 | N + 532 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N + 536 | feedforward_2nd_gain | 2 | RW | RAM | 0 | 32767 | N + 538 | feedforward_1st_gain | 2 | RW | RAM | 0 | 32767 | N + 546 | bus_watchdog | 1 | RW | RAM | -1 | 127 | Y + 548 | goal_pwm | 2 | RW | RAM | -2100 | 2100 | Y + 550 | goal_current | 2 | RW | RAM | -32768 | 32767 | Y + 552 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 556 | profile_acceleration | 4 | RW | RAM | 0 | 32767 | N + 560 | profile_velocity | 4 | RW | RAM | 0 | 1023 | N + 564 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 568 | realtime_tick | 2 | R | RAM | 0 | 32767 | N + 570 | moving | 1 | R | RAM | 0 | 1 | N + 571 | moving_status | 1 | R | RAM | 0 | 255 | N + 572 | present_pwm | 2 | R | RAM | -2100 | 2100 | Y + 574 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 576 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 580 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 584 | velocity_trajectory | 4 | R | RAM | 0 | 1023 | N + 588 | position_trajectory | 4 | R | RAM | 0 | 4095 | N + 592 | present_voltage | 2 | R | RAM | 0 | 500 | N + 594 | present_temperature | 1 | R | RAM | 0 | 200 | N + 600 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 602 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 604 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 606 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N diff --git a/robotis_device/devices/dynamixel/H54P-100-S500-R.device b/robotis_device/devices/dynamixel/H54P-100-S500-R.device new file mode 100644 index 0000000..e9661e0 --- /dev/null +++ b/robotis_device/devices/dynamixel/H54P-100-S500-R.device @@ -0,0 +1,92 @@ +[device info] +model_name = H54P-100-S500-R +device_type = dynamixel + +[type info] +torque_to_current_value_ratio = 155.658486 +velocity_to_value_ratio = 954.93 +value_of_0_radian_position = 0 +value_of_min_radian_position = -501923 +value_of_max_radian_position = 501923 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_current +position_d_gain_item_name = position_d_gain +position_i_gain_item_name = position_i_gain +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 10 | drive_mode | 1 | RW | EEPROM | 0 | 3 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 12 | secondary_id | 1 | RW | EEPROM | 0 | 255 | N + 13 | protocol_version | 1 | RW | EEPROM | 1 | 2 | N + 20 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 24 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 31 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 32 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 34 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 36 | pwm_limit | 2 | RW | EEPROM | 0 | 885 | N + 38 | current_limit | 2 | RW | EEPROM | 0 | 32767 | N + 40 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 44 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 48 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 52 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 56 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 57 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 58 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 59 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 63 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 168 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 512 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 513 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 514 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 515 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 516 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 517 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 518 | hardware_error_status | 1 | R | RAM | 0 | 31 | N + 524 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N + 526 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N + 528 | position_d_gain | 2 | RW | RAM | 0 | 2047 | N + 530 | position_i_gain | 2 | RW | RAM | 0 | 2047 | N + 532 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N + 536 | feedforward_2nd_gain | 2 | RW | RAM | 0 | 32767 | N + 538 | feedforward_1st_gain | 2 | RW | RAM | 0 | 32767 | N + 546 | bus_watchdog | 1 | RW | RAM | -1 | 127 | Y + 548 | goal_pwm | 2 | RW | RAM | -2100 | 2100 | Y + 550 | goal_current | 2 | RW | RAM | -32768 | 32767 | Y + 552 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 556 | profile_acceleration | 4 | RW | RAM | 0 | 32767 | N + 560 | profile_velocity | 4 | RW | RAM | 0 | 1023 | N + 564 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 568 | realtime_tick | 2 | R | RAM | 0 | 32767 | N + 570 | moving | 1 | R | RAM | 0 | 1 | N + 571 | moving_status | 1 | R | RAM | 0 | 255 | N + 572 | present_pwm | 2 | R | RAM | -2100 | 2100 | Y + 574 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 576 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 580 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 584 | velocity_trajectory | 4 | R | RAM | 0 | 1023 | N + 588 | position_trajectory | 4 | R | RAM | 0 | 4095 | N + 592 | present_voltage | 2 | R | RAM | 0 | 500 | N + 594 | present_temperature | 1 | R | RAM | 0 | 200 | N + 600 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 602 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 604 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 606 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N diff --git a/robotis_device/devices/dynamixel/H54P-200-S500-R.device b/robotis_device/devices/dynamixel/H54P-200-S500-R.device new file mode 100644 index 0000000..c58460e --- /dev/null +++ b/robotis_device/devices/dynamixel/H54P-200-S500-R.device @@ -0,0 +1,92 @@ +[device info] +model_name = H54P-200-S500-R +device_type = dynamixel + +[type info] +torque_to_current_value_ratio = 146.502114 +velocity_to_value_ratio = 954.93 +value_of_0_radian_position = 0 +value_of_min_radian_position = -501923 +value_of_max_radian_position = 501923 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_current +position_d_gain_item_name = position_d_gain +position_i_gain_item_name = position_i_gain +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 10 | drive_mode | 1 | RW | EEPROM | 0 | 3 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 12 | secondary_id | 1 | RW | EEPROM | 0 | 255 | N + 13 | protocol_version | 1 | RW | EEPROM | 1 | 2 | N + 20 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 24 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 31 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 32 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 34 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 36 | pwm_limit | 2 | RW | EEPROM | 0 | 885 | N + 38 | current_limit | 2 | RW | EEPROM | 0 | 32767 | N + 40 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 44 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 48 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 52 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 56 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 57 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 58 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 59 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 63 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 168 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 512 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 513 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 514 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 515 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 516 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 517 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 518 | hardware_error_status | 1 | R | RAM | 0 | 31 | N + 524 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N + 526 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N + 528 | position_d_gain | 2 | RW | RAM | 0 | 2047 | N + 530 | position_i_gain | 2 | RW | RAM | 0 | 2047 | N + 532 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N + 536 | feedforward_2nd_gain | 2 | RW | RAM | 0 | 32767 | N + 538 | feedforward_1st_gain | 2 | RW | RAM | 0 | 32767 | N + 546 | bus_watchdog | 1 | RW | RAM | -1 | 127 | Y + 548 | goal_pwm | 2 | RW | RAM | -2100 | 2100 | Y + 550 | goal_current | 2 | RW | RAM | -32768 | 32767 | Y + 552 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 556 | profile_acceleration | 4 | RW | RAM | 0 | 32767 | N + 560 | profile_velocity | 4 | RW | RAM | 0 | 1023 | N + 564 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 568 | realtime_tick | 2 | R | RAM | 0 | 32767 | N + 570 | moving | 1 | R | RAM | 0 | 1 | N + 571 | moving_status | 1 | R | RAM | 0 | 255 | N + 572 | present_pwm | 2 | R | RAM | -2100 | 2100 | Y + 574 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 576 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 580 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 584 | velocity_trajectory | 4 | R | RAM | 0 | 1023 | N + 588 | position_trajectory | 4 | R | RAM | 0 | 4095 | N + 592 | present_voltage | 2 | R | RAM | 0 | 500 | N + 594 | present_temperature | 1 | R | RAM | 0 | 200 | N + 600 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 602 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 604 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 606 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N From 4be0cf0b65a6081fa8af987ea2b418f9b133b756 Mon Sep 17 00:00:00 2001 From: Kayman Date: Fri, 24 May 2019 13:36:48 +0900 Subject: [PATCH 142/238] added device file for H54P-200-B500-R --- .../devices/dynamixel/H54P-200-B500-R.device | 92 +++++++++++++++++++ 1 file changed, 92 insertions(+) create mode 100644 robotis_device/devices/dynamixel/H54P-200-B500-R.device diff --git a/robotis_device/devices/dynamixel/H54P-200-B500-R.device b/robotis_device/devices/dynamixel/H54P-200-B500-R.device new file mode 100644 index 0000000..97f44c5 --- /dev/null +++ b/robotis_device/devices/dynamixel/H54P-200-B500-R.device @@ -0,0 +1,92 @@ +[device info] +model_name = H54P-200-B500-R +device_type = dynamixel + +[type info] +torque_to_current_value_ratio = 146.502114 +velocity_to_value_ratio = 954.93 +value_of_0_radian_position = 0 +value_of_min_radian_position = -501923 +value_of_max_radian_position = 501923 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_current +position_d_gain_item_name = position_d_gain +position_i_gain_item_name = position_i_gain +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 10 | drive_mode | 1 | RW | EEPROM | 0 | 3 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 12 | secondary_id | 1 | RW | EEPROM | 0 | 255 | N + 13 | protocol_version | 1 | RW | EEPROM | 1 | 2 | N + 20 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 24 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 31 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 32 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 34 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 36 | pwm_limit | 2 | RW | EEPROM | 0 | 885 | N + 38 | current_limit | 2 | RW | EEPROM | 0 | 32767 | N + 40 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 44 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 48 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 52 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 56 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 57 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 58 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 59 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 63 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 168 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 512 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 513 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 514 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 515 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 516 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 517 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 518 | hardware_error_status | 1 | R | RAM | 0 | 31 | N + 524 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N + 526 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N + 528 | position_d_gain | 2 | RW | RAM | 0 | 2047 | N + 530 | position_i_gain | 2 | RW | RAM | 0 | 2047 | N + 532 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N + 536 | feedforward_2nd_gain | 2 | RW | RAM | 0 | 32767 | N + 538 | feedforward_1st_gain | 2 | RW | RAM | 0 | 32767 | N + 546 | bus_watchdog | 1 | RW | RAM | -1 | 127 | Y + 548 | goal_pwm | 2 | RW | RAM | -2100 | 2100 | Y + 550 | goal_current | 2 | RW | RAM | -32768 | 32767 | Y + 552 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 556 | profile_acceleration | 4 | RW | RAM | 0 | 32767 | N + 560 | profile_velocity | 4 | RW | RAM | 0 | 1023 | N + 564 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 568 | realtime_tick | 2 | R | RAM | 0 | 32767 | N + 570 | moving | 1 | R | RAM | 0 | 1 | N + 571 | moving_status | 1 | R | RAM | 0 | 255 | N + 572 | present_pwm | 2 | R | RAM | -2100 | 2100 | Y + 574 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 576 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 580 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 584 | velocity_trajectory | 4 | R | RAM | 0 | 1023 | N + 588 | position_trajectory | 4 | R | RAM | 0 | 4095 | N + 592 | present_voltage | 2 | R | RAM | 0 | 500 | N + 594 | present_temperature | 1 | R | RAM | 0 | 200 | N + 600 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 602 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 604 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 606 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N From af992c7d15426dcf9899075e69029796ebf6a332 Mon Sep 17 00:00:00 2001 From: Kayman Date: Tue, 4 Jun 2019 14:55:25 +0900 Subject: [PATCH 143/238] Added check to change value of EEPROM area. --- .../robotis_controller/robotis_controller.cpp | 23 ++++++++++++++++++- 1 file changed, 22 insertions(+), 1 deletion(-) diff --git a/robotis_controller/src/robotis_controller/robotis_controller.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp index 7e59385..d50bfdd 100755 --- a/robotis_controller/src/robotis_controller/robotis_controller.cpp +++ b/robotis_controller/src/robotis_controller/robotis_controller.cpp @@ -374,6 +374,9 @@ void RobotisController::initializeDevice(const std::string init_file_path) if (DEBUG_PRINT) ROS_INFO("JOINT_NAME: %s", joint_name.c_str()); + uint8_t torque_enabled = 0; + read1Byte(joint_name, dxl->torque_enable_item_->address_, &torque_enabled); + for (YAML::const_iterator it_joint = joint_node.begin(); it_joint != joint_node.end(); it_joint++) { std::string item_name = it_joint->first.as(); @@ -416,6 +419,12 @@ void RobotisController::initializeDevice(const std::string init_file_path) default: break; } + + if (torque_enabled == 1) + { + ROS_ERROR("################\nThe initial value of the EEPROM area has been changed. \nTurn off Torque Enable and try again."); + exit(-1); + } } switch (item->data_length_) @@ -469,6 +478,9 @@ void RobotisController::initializeDevice(const std::string init_file_path) // bulkread_data_length = dxl->present_position_item->data_length; // } + uint8_t torque_enabled = 0; + read1Byte(joint_name, dxl->torque_enable_item_->address_, &torque_enabled); + // calculate bulk read start address & data length auto indirect_addr_it = dxl->ctrl_table_.find(INDIRECT_ADDRESS_1); if (indirect_addr_it != dxl->ctrl_table_.end()) // INDIRECT_ADDRESS_1 exist @@ -494,6 +506,11 @@ void RobotisController::initializeDevice(const std::string init_file_path) read2Byte(joint_name, indirect_addr, &data16); if (data16 != dxl->ctrl_table_[dxl->bulk_read_items_[i]->item_name_]->address_ + l) { + if (torque_enabled == 1) + { + ROS_ERROR("################\nThe indirect address of the EEPROM area has been changed. \nTurn off Torque Enable and try again."); + exit(-1); + } write2Byte(joint_name, indirect_addr, dxl->ctrl_table_[dxl->bulk_read_items_[i]->item_name_]->address_ + l); } indirect_addr += 2; @@ -908,7 +925,11 @@ void RobotisController::process() for (auto& it : port_to_bulk_read_) { robot_->ports_[it.first]->setPacketTimeout(0.0); - it.second->rxPacket(); + //it.second->rxPacket(); + + int result = it.second->rxPacket(); + if(result != COMM_SUCCESS) + ROS_ERROR_STREAM("Bulk Read Fail : " << it.first); } // -> save to robot->dxls_[]->dxl_state_ From 1b746fbf0476d2693b0692c5c6969d4a67c18f54 Mon Sep 17 00:00:00 2001 From: Kayman Date: Wed, 5 Jun 2019 16:27:49 +0900 Subject: [PATCH 144/238] Added ability to check for bulkread success. --- .../src/robotis_controller/robotis_controller.cpp | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/robotis_controller/src/robotis_controller/robotis_controller.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp index d50bfdd..20cd766 100755 --- a/robotis_controller/src/robotis_controller/robotis_controller.cpp +++ b/robotis_controller/src/robotis_controller/robotis_controller.cpp @@ -925,11 +925,14 @@ void RobotisController::process() for (auto& it : port_to_bulk_read_) { robot_->ports_[it.first]->setPacketTimeout(0.0); - //it.second->rxPacket(); - - int result = it.second->rxPacket(); - if(result != COMM_SUCCESS) - ROS_ERROR_STREAM("Bulk Read Fail : " << it.first); + if(DEBUG_PRINT) + { + int result = it.second->rxPacket(); + if(result != COMM_SUCCESS) + ROS_ERROR_STREAM("Bulk Read Fail : " << it.first); + } + else + it.second->rxPacket(); } // -> save to robot->dxls_[]->dxl_state_ From 13457bd5dc3494de8f11ea65f78504a516e66cd1 Mon Sep 17 00:00:00 2001 From: Kayman Date: Tue, 11 Jun 2019 13:32:33 +0900 Subject: [PATCH 145/238] Modified robotis_controller for tuning_module --- .../robotis_controller/robotis_controller.h | 1 + .../robotis_controller/robotis_controller.cpp | 41 ++++++++++++++++--- 2 files changed, 36 insertions(+), 6 deletions(-) diff --git a/robotis_controller/include/robotis_controller/robotis_controller.h b/robotis_controller/include/robotis_controller/robotis_controller.h index 6ee5c26..4772f78 100755 --- a/robotis_controller/include/robotis_controller/robotis_controller.h +++ b/robotis_controller/include/robotis_controller/robotis_controller.h @@ -86,6 +86,7 @@ private: void initializeSyncWrite(); public: + const int NONE_GAIN = -1; bool DEBUG_PRINT; Robot *robot_; diff --git a/robotis_controller/src/robotis_controller/robotis_controller.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp index 20cd766..c91d696 100755 --- a/robotis_controller/src/robotis_controller/robotis_controller.cpp +++ b/robotis_controller/src/robotis_controller/robotis_controller.cpp @@ -1336,7 +1336,7 @@ void RobotisController::process() port_to_sync_write_position_[dxl->port_name_]->changeParam(dxl->id_, sync_write_data); // if position p gain value is changed -> sync write - if (result_state->position_p_gain_ != 65535 && dxl_state->position_p_gain_ != result_state->position_p_gain_) + if (result_state->position_p_gain_ != NONE_GAIN && dxl_state->position_p_gain_ != result_state->position_p_gain_) { dxl_state->position_p_gain_ = result_state->position_p_gain_; uint8_t sync_write_data[4] = { 0 }; @@ -1350,7 +1350,7 @@ void RobotisController::process() } // if position i gain value is changed -> sync write - if (result_state->position_i_gain_ != 65535 && dxl_state->position_i_gain_ != result_state->position_i_gain_) + if (result_state->position_i_gain_ != NONE_GAIN && dxl_state->position_i_gain_ != result_state->position_i_gain_) { dxl_state->position_i_gain_ = result_state->position_i_gain_; uint8_t sync_write_data[4] = { 0 }; @@ -1364,7 +1364,7 @@ void RobotisController::process() } // if position d gain value is changed -> sync write - if (result_state->position_d_gain_ != 65535 && dxl_state->position_d_gain_ != result_state->position_d_gain_) + if (result_state->position_d_gain_ != NONE_GAIN && dxl_state->position_d_gain_ != result_state->position_d_gain_) { dxl_state->position_d_gain_ = result_state->position_d_gain_; uint8_t sync_write_data[4] = { 0 }; @@ -1376,6 +1376,35 @@ void RobotisController::process() if (port_to_sync_write_position_d_gain_[dxl->port_name_] != NULL) port_to_sync_write_position_d_gain_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); } + + // if velocity p gain gain value is changed -> sync write + if (result_state->velocity_p_gain_ != NONE_GAIN && dxl_state->velocity_p_gain_ != result_state->velocity_p_gain_) + { + dxl_state->velocity_p_gain_ = result_state->velocity_p_gain_; + uint8_t sync_write_data[4] = { 0 }; + sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(dxl_state->velocity_p_gain_)); + sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(dxl_state->velocity_p_gain_)); + sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(dxl_state->velocity_p_gain_)); + sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(dxl_state->velocity_p_gain_)); + + if (port_to_sync_write_velocity_p_gain_[dxl->port_name_] != NULL) + port_to_sync_write_velocity_p_gain_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); + } + + // if velocity i gain value is changed -> sync write + if (result_state->velocity_i_gain_ != NONE_GAIN && dxl_state->velocity_i_gain_ != result_state->velocity_i_gain_) + { + dxl_state->velocity_i_gain_ = result_state->velocity_i_gain_; + uint8_t sync_write_data[4] = { 0 }; + sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(dxl_state->velocity_i_gain_)); + sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(dxl_state->velocity_i_gain_)); + sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(dxl_state->velocity_i_gain_)); + sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(dxl_state->velocity_i_gain_)); + + if (port_to_sync_write_velocity_i_gain_[dxl->port_name_] != NULL) + port_to_sync_write_velocity_i_gain_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); + } + } } else if ((*module_it)->getControlMode() == VelocityControl) @@ -1395,7 +1424,7 @@ void RobotisController::process() port_to_sync_write_velocity_[dxl->port_name_]->changeParam(dxl->id_, sync_write_data); // if velocity p gain gain value is changed -> sync write - if (result_state->velocity_p_gain_ != 65535 && dxl_state->velocity_p_gain_ != result_state->velocity_p_gain_) + if (result_state->velocity_p_gain_ != NONE_GAIN && dxl_state->velocity_p_gain_ != result_state->velocity_p_gain_) { dxl_state->velocity_p_gain_ = result_state->velocity_p_gain_; uint8_t sync_write_data[4] = { 0 }; @@ -1409,7 +1438,7 @@ void RobotisController::process() } // if velocity i gain value is changed -> sync write - if (result_state->velocity_i_gain_ != 65535 && dxl_state->velocity_i_gain_ != result_state->velocity_i_gain_) + if (result_state->velocity_i_gain_ != NONE_GAIN && dxl_state->velocity_i_gain_ != result_state->velocity_i_gain_) { dxl_state->velocity_i_gain_ = result_state->velocity_i_gain_; uint8_t sync_write_data[4] = { 0 }; @@ -1423,7 +1452,7 @@ void RobotisController::process() } // if velocity d gain value is changed -> sync write - if (result_state->velocity_d_gain_ != 65535 && dxl_state->velocity_d_gain_ != result_state->velocity_d_gain_) + if (result_state->velocity_d_gain_ != NONE_GAIN && dxl_state->velocity_d_gain_ != result_state->velocity_d_gain_) { dxl_state->velocity_d_gain_ = result_state->velocity_d_gain_; uint8_t sync_write_data[4] = { 0 }; From bd03350cc81c28078c05cd9264c3f036488e3e56 Mon Sep 17 00:00:00 2001 From: Kayman Date: Wed, 28 Aug 2019 09:42:39 +0900 Subject: [PATCH 146/238] Fixed body splashing when changing module between tuning_module and others. --- .../robotis_controller/robotis_controller.h | 3 +- .../robotis_controller/robotis_controller.cpp | 76 ++++++++----------- 2 files changed, 34 insertions(+), 45 deletions(-) diff --git a/robotis_controller/include/robotis_controller/robotis_controller.h b/robotis_controller/include/robotis_controller/robotis_controller.h index 4772f78..05ae93c 100755 --- a/robotis_controller/include/robotis_controller/robotis_controller.h +++ b/robotis_controller/include/robotis_controller/robotis_controller.h @@ -67,6 +67,7 @@ private: bool init_pose_loaded_; bool is_timer_running_; bool is_offset_enabled_; + double offset_ratio_; bool stop_timer_; pthread_t timer_thread_; ControllerMode controller_mode_; @@ -86,7 +87,7 @@ private: void initializeSyncWrite(); public: - const int NONE_GAIN = -1; + const int NONE_GAIN = 65535; bool DEBUG_PRINT; Robot *robot_; diff --git a/robotis_controller/src/robotis_controller/robotis_controller.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp index c91d696..548bfaa 100755 --- a/robotis_controller/src/robotis_controller/robotis_controller.cpp +++ b/robotis_controller/src/robotis_controller/robotis_controller.cpp @@ -31,6 +31,7 @@ using namespace robotis_framework; RobotisController::RobotisController() : is_timer_running_(false), is_offset_enabled_(true), + offset_ratio_(1.0), stop_timer_(false), init_pose_loaded_(false), timer_thread_(0), @@ -143,10 +144,7 @@ void RobotisController::initializeSyncWrite() if ((dxl->present_position_item_ != 0) && (dxl->bulk_read_items_[i]->item_name_ == dxl->present_position_item_->item_name_)) { - if(is_offset_enabled_) - dxl->dxl_state_->present_position_ = dxl->convertValue2Radian(read_data) - dxl->dxl_state_->position_offset_; // remove offset - else - dxl->dxl_state_->present_position_ = dxl->convertValue2Radian(read_data); + dxl->dxl_state_->present_position_ = dxl->convertValue2Radian(read_data) - dxl->dxl_state_->position_offset_ * offset_ratio_; dxl->dxl_state_->goal_position_ = dxl->dxl_state_->present_position_; port_to_sync_write_position_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); @@ -904,6 +902,22 @@ void RobotisController::process() is_process_running = true; // ROS_INFO("Controller::Process()"); + // offset ratio + if(is_offset_enabled_) + { + if(offset_ratio_ < 1.0) + offset_ratio_ += 0.01; + else + offset_ratio_ = 1.0; + } + else + { + if(offset_ratio_ > 0.0) + offset_ratio_ -= 0.01; + else + offset_ratio_ = 0.0; + } + ros::Time start_time; ros::Duration time_duration; @@ -959,10 +973,7 @@ void RobotisController::process() // change dxl_state if (dxl->present_position_item_ != 0 && item->item_name_ == dxl->present_position_item_->item_name_) { - if(is_offset_enabled_) - dxl->dxl_state_->present_position_ = dxl->convertValue2Radian(data) - dxl->dxl_state_->position_offset_; // remove offset - else - dxl->dxl_state_->present_position_ = dxl->convertValue2Radian(data); + dxl->dxl_state_->present_position_ = dxl->convertValue2Radian(data) - dxl->dxl_state_->position_offset_ * offset_ratio_; } else if (dxl->present_velocity_item_ != 0 && item->item_name_ == dxl->present_velocity_item_->item_name_) dxl->dxl_state_->present_velocity_ = dxl->convertValue2Velocity(data); @@ -970,10 +981,7 @@ void RobotisController::process() dxl->dxl_state_->present_torque_ = dxl->convertValue2Torque(data); else if (dxl->goal_position_item_ != 0 && item->item_name_ == dxl->goal_position_item_->item_name_) { - if(is_offset_enabled_) - dxl->dxl_state_->goal_position_ = dxl->convertValue2Radian(data) - dxl->dxl_state_->position_offset_; // remove offset - else - dxl->dxl_state_->goal_position_ = dxl->convertValue2Radian(data); + dxl->dxl_state_->goal_position_ = dxl->convertValue2Radian(data) - dxl->dxl_state_->position_offset_ * offset_ratio_; } else if (dxl->goal_velocity_item_ != 0 && item->item_name_ == dxl->goal_velocity_item_->item_name_) dxl->dxl_state_->goal_velocity_ = dxl->convertValue2Velocity(data); @@ -1202,10 +1210,7 @@ void RobotisController::process() // change dxl_state if (dxl->present_position_item_ != 0 && item->item_name_ == dxl->present_position_item_->item_name_) { - if(is_offset_enabled_) - dxl->dxl_state_->present_position_ = dxl->convertValue2Radian(data) - dxl->dxl_state_->position_offset_; // remove offset - else - dxl->dxl_state_->present_position_ = dxl->convertValue2Radian(data); + dxl->dxl_state_->present_position_ = dxl->convertValue2Radian(data) - dxl->dxl_state_->position_offset_ * offset_ratio_; } else if (dxl->present_velocity_item_ != 0 && item->item_name_ == dxl->present_velocity_item_->item_name_) dxl->dxl_state_->present_velocity_ = dxl->convertValue2Velocity(data); @@ -1213,10 +1218,7 @@ void RobotisController::process() dxl->dxl_state_->present_torque_ = dxl->convertValue2Torque(data); else if (dxl->goal_position_item_ != 0 && item->item_name_ == dxl->goal_position_item_->item_name_) { - if(is_offset_enabled_) - dxl->dxl_state_->goal_position_ = dxl->convertValue2Radian(data) - dxl->dxl_state_->position_offset_; // remove offset - else - dxl->dxl_state_->goal_position_ = dxl->convertValue2Radian(data); + dxl->dxl_state_->goal_position_ = dxl->convertValue2Radian(data) - dxl->dxl_state_->position_offset_ * offset_ratio_; } else if (dxl->goal_velocity_item_ != 0 && item->item_name_ == dxl->goal_velocity_item_->item_name_) dxl->dxl_state_->goal_velocity_ = dxl->convertValue2Velocity(data); @@ -1321,10 +1323,7 @@ void RobotisController::process() { // add offset uint32_t pos_data; - if(is_offset_enabled_) - pos_data= dxl->convertRadian2Value(dxl_state->goal_position_ + dxl_state->position_offset_); - else - pos_data= dxl->convertRadian2Value(dxl_state->goal_position_); + pos_data= dxl->convertRadian2Value(dxl_state->goal_position_ + dxl_state->position_offset_ * offset_ratio_); uint8_t sync_write_data[4] = { 0 }; sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(pos_data)); @@ -1753,10 +1752,7 @@ void RobotisController::setJointStatesCallback(const sensor_msgs::JointState::Co { // add offset uint32_t pos_data; - if(is_offset_enabled_) - pos_data = dxl->convertRadian2Value(dxl->dxl_state_->goal_position_ + dxl->dxl_state_->position_offset_); - else - pos_data = dxl->convertRadian2Value(dxl->dxl_state_->goal_position_); + pos_data = dxl->convertRadian2Value(dxl->dxl_state_->goal_position_ + dxl->dxl_state_->position_offset_ * offset_ratio_); uint8_t sync_write_data[4] = { 0 }; sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(pos_data)); @@ -1804,6 +1800,10 @@ void RobotisController::setJointCtrlModuleCallback(const robotis_controller_msgs void RobotisController::enableOffsetCallback(const std_msgs::Bool::ConstPtr &msg) { is_offset_enabled_ = (bool)msg->data; + if(is_offset_enabled_) + offset_ratio_ = 0.0; + else + offset_ratio_ = 1.0; } bool RobotisController::getJointCtrlModuleService(robotis_controller_msgs::GetJointModule::Request &req, @@ -1938,10 +1938,7 @@ void RobotisController::setJointCtrlModuleThread(const robotis_controller_msgs:: continue; uint32_t _pos_data; - if(is_offset_enabled_) - _pos_data = _dxl->convertRadian2Value(_dxl->dxl_state_->goal_position_ + _dxl->dxl_state_->position_offset_); - else - _pos_data = _dxl->convertRadian2Value(_dxl->dxl_state_->goal_position_); + _pos_data = _dxl->convertRadian2Value(_dxl->dxl_state_->goal_position_ + _dxl->dxl_state_->position_offset_ * offset_ratio_); uint8_t _sync_write_data[4]; _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_pos_data)); @@ -1981,10 +1978,7 @@ void RobotisController::setJointCtrlModuleThread(const robotis_controller_msgs:: if(_mode == PositionControl) { uint32_t _pos_data; - if(is_offset_enabled_) - _pos_data = _dxl->convertRadian2Value(_dxl->dxl_state_->goal_position_ + _dxl->dxl_state_->position_offset_); - else - _pos_data = _dxl->convertRadian2Value(_dxl->dxl_state_->goal_position_); + _pos_data = _dxl->convertRadian2Value(_dxl->dxl_state_->goal_position_ + _dxl->dxl_state_->position_offset_ * offset_ratio_); uint8_t _sync_write_data[4]; _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_pos_data)); @@ -2152,10 +2146,7 @@ void RobotisController::setCtrlModuleThread(std::string ctrl_module) continue; uint32_t pos_data; - if(is_offset_enabled_) - pos_data = dxl->convertRadian2Value(dxl->dxl_state_->goal_position_ + dxl->dxl_state_->position_offset_); - else - pos_data = dxl->convertRadian2Value(dxl->dxl_state_->goal_position_); + pos_data = dxl->convertRadian2Value(dxl->dxl_state_->goal_position_ + dxl->dxl_state_->position_offset_ * offset_ratio_); uint8_t sync_write_data[4] = { 0 }; sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(pos_data)); @@ -2195,10 +2186,7 @@ void RobotisController::setCtrlModuleThread(std::string ctrl_module) if (mode == PositionControl) { uint32_t pos_data; - if(is_offset_enabled_) - pos_data = dxl->convertRadian2Value(dxl->dxl_state_->goal_position_ + dxl->dxl_state_->position_offset_); - else - pos_data = dxl->convertRadian2Value(dxl->dxl_state_->goal_position_); + pos_data = dxl->convertRadian2Value(dxl->dxl_state_->goal_position_ + dxl->dxl_state_->position_offset_ * offset_ratio_); uint8_t sync_write_data[4] = { 0 }; sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(pos_data)); From d67d6c2ee814d4e6301579c1e9c99bb10d3e9b8a Mon Sep 17 00:00:00 2001 From: Kayman Date: Thu, 28 Nov 2019 14:29:00 +0900 Subject: [PATCH 147/238] fixed a bug that occurred when parsing the robot file with empty bulk read item. --- robotis_device/src/robotis_device/robot.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/robotis_device/src/robotis_device/robot.cpp b/robotis_device/src/robotis_device/robot.cpp index 6214d16..5cd98c6 100755 --- a/robotis_device/src/robotis_device/robot.cpp +++ b/robotis_device/src/robotis_device/robot.cpp @@ -132,7 +132,7 @@ Robot::Robot(std::string robot_file_path, std::string dev_desc_dir_path) Dynamixel *dxl = dxls_[dev_name]; std::vector sub_tokens = split(tokens[6], ','); - if (sub_tokens.size() > 0) + if (sub_tokens.size() > 0 && sub_tokens[0] != "")) { std::map::iterator indirect_it = dxl->ctrl_table_.find(INDIRECT_ADDRESS_1); if (indirect_it != dxl->ctrl_table_.end()) // INDIRECT_ADDRESS_1 exist @@ -185,7 +185,7 @@ Robot::Robot(std::string robot_file_path, std::string dev_desc_dir_path) Sensor *sensor = sensors_[dev_name]; std::vector sub_tokens = split(tokens[6], ','); - if (sub_tokens.size() > 0) + if (sub_tokens.size() > 0 && sub_tokens[0] != "") { std::map::iterator indirect_it = sensor->ctrl_table_.find(INDIRECT_ADDRESS_1); if (indirect_it != sensor->ctrl_table_.end()) // INDIRECT_ADDRESS_1 exist From 5c15224726dca5304e27d0e6969dc5629322658c Mon Sep 17 00:00:00 2001 From: Kayman Date: Thu, 28 Nov 2019 14:35:15 +0900 Subject: [PATCH 148/238] fixed typo --- robotis_device/src/robotis_device/robot.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/robotis_device/src/robotis_device/robot.cpp b/robotis_device/src/robotis_device/robot.cpp index 5cd98c6..45a97c5 100755 --- a/robotis_device/src/robotis_device/robot.cpp +++ b/robotis_device/src/robotis_device/robot.cpp @@ -132,7 +132,7 @@ Robot::Robot(std::string robot_file_path, std::string dev_desc_dir_path) Dynamixel *dxl = dxls_[dev_name]; std::vector sub_tokens = split(tokens[6], ','); - if (sub_tokens.size() > 0 && sub_tokens[0] != "")) + if (sub_tokens.size() > 0 && sub_tokens[0] != "") { std::map::iterator indirect_it = dxl->ctrl_table_.find(INDIRECT_ADDRESS_1); if (indirect_it != dxl->ctrl_table_.end()) // INDIRECT_ADDRESS_1 exist From c38a4b92a204c3c654c79047ef9543dfc66a402b Mon Sep 17 00:00:00 2001 From: zerom Date: Wed, 8 Jan 2020 15:59:54 +0900 Subject: [PATCH 149/238] - added PH54-100-S500-R, PH54-200-S500-R, PH42-020-S300-R --- .../devices/dynamixel/PH42-020-S300-R.device | 92 +++++++++++++++++++ .../devices/dynamixel/PH54-100-S500-R.device | 92 +++++++++++++++++++ .../devices/dynamixel/PH54-200-S500-R.device | 92 +++++++++++++++++++ 3 files changed, 276 insertions(+) create mode 100644 robotis_device/devices/dynamixel/PH42-020-S300-R.device create mode 100644 robotis_device/devices/dynamixel/PH54-100-S500-R.device create mode 100644 robotis_device/devices/dynamixel/PH54-200-S500-R.device diff --git a/robotis_device/devices/dynamixel/PH42-020-S300-R.device b/robotis_device/devices/dynamixel/PH42-020-S300-R.device new file mode 100644 index 0000000..92e9df6 --- /dev/null +++ b/robotis_device/devices/dynamixel/PH42-020-S300-R.device @@ -0,0 +1,92 @@ +[device info] +model_name = PH42-020-S300-R +device_type = dynamixel + +[type info] +torque_to_current_value_ratio = 109.3747778 +velocity_to_value_ratio = 954.93 +value_of_0_radian_position = 0 +value_of_min_radian_position = -303750 +value_of_max_radian_position = 303750 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_current +position_d_gain_item_name = position_d_gain +position_i_gain_item_name = position_i_gain +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 10 | drive_mode | 1 | RW | EEPROM | 0 | 3 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 12 | secondary_id | 1 | RW | EEPROM | 0 | 255 | N + 13 | protocol_version | 1 | RW | EEPROM | 1 | 2 | N + 20 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 24 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 31 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 32 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 34 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 36 | pwm_limit | 2 | RW | EEPROM | 0 | 885 | N + 38 | current_limit | 2 | RW | EEPROM | 0 | 32767 | N + 40 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 44 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 48 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 52 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 56 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 57 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 58 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 59 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 63 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 168 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 512 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 513 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 514 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 515 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 516 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 517 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 518 | hardware_error_status | 1 | R | RAM | 0 | 31 | N + 524 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N + 526 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N + 528 | position_d_gain | 2 | RW | RAM | 0 | 2047 | N + 530 | position_i_gain | 2 | RW | RAM | 0 | 2047 | N + 532 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N + 536 | feedforward_2nd_gain | 2 | RW | RAM | 0 | 32767 | N + 538 | feedforward_1st_gain | 2 | RW | RAM | 0 | 32767 | N + 546 | bus_watchdog | 1 | RW | RAM | -1 | 127 | Y + 548 | goal_pwm | 2 | RW | RAM | -2100 | 2100 | Y + 550 | goal_current | 2 | RW | RAM | -32768 | 32767 | Y + 552 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 556 | profile_acceleration | 4 | RW | RAM | 0 | 32767 | N + 560 | profile_velocity | 4 | RW | RAM | 0 | 1023 | N + 564 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 568 | realtime_tick | 2 | R | RAM | 0 | 32767 | N + 570 | moving | 1 | R | RAM | 0 | 1 | N + 571 | moving_status | 1 | R | RAM | 0 | 255 | N + 572 | present_pwm | 2 | R | RAM | -2100 | 2100 | Y + 574 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 576 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 580 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 584 | velocity_trajectory | 4 | R | RAM | 0 | 1023 | N + 588 | position_trajectory | 4 | R | RAM | 0 | 4095 | N + 592 | present_voltage | 2 | R | RAM | 0 | 500 | N + 594 | present_temperature | 1 | R | RAM | 0 | 200 | N + 600 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 602 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 604 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 606 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N diff --git a/robotis_device/devices/dynamixel/PH54-100-S500-R.device b/robotis_device/devices/dynamixel/PH54-100-S500-R.device new file mode 100644 index 0000000..6dd1ece --- /dev/null +++ b/robotis_device/devices/dynamixel/PH54-100-S500-R.device @@ -0,0 +1,92 @@ +[device info] +model_name = PH54-100-S500-R +device_type = dynamixel + +[type info] +torque_to_current_value_ratio = 155.658486 +velocity_to_value_ratio = 954.93 +value_of_0_radian_position = 0 +value_of_min_radian_position = -501923 +value_of_max_radian_position = 501923 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_current +position_d_gain_item_name = position_d_gain +position_i_gain_item_name = position_i_gain +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 10 | drive_mode | 1 | RW | EEPROM | 0 | 3 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 12 | secondary_id | 1 | RW | EEPROM | 0 | 255 | N + 13 | protocol_version | 1 | RW | EEPROM | 1 | 2 | N + 20 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 24 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 31 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 32 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 34 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 36 | pwm_limit | 2 | RW | EEPROM | 0 | 885 | N + 38 | current_limit | 2 | RW | EEPROM | 0 | 32767 | N + 40 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 44 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 48 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 52 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 56 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 57 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 58 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 59 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 63 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 168 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 512 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 513 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 514 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 515 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 516 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 517 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 518 | hardware_error_status | 1 | R | RAM | 0 | 31 | N + 524 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N + 526 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N + 528 | position_d_gain | 2 | RW | RAM | 0 | 2047 | N + 530 | position_i_gain | 2 | RW | RAM | 0 | 2047 | N + 532 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N + 536 | feedforward_2nd_gain | 2 | RW | RAM | 0 | 32767 | N + 538 | feedforward_1st_gain | 2 | RW | RAM | 0 | 32767 | N + 546 | bus_watchdog | 1 | RW | RAM | -1 | 127 | Y + 548 | goal_pwm | 2 | RW | RAM | -2100 | 2100 | Y + 550 | goal_current | 2 | RW | RAM | -32768 | 32767 | Y + 552 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 556 | profile_acceleration | 4 | RW | RAM | 0 | 32767 | N + 560 | profile_velocity | 4 | RW | RAM | 0 | 1023 | N + 564 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 568 | realtime_tick | 2 | R | RAM | 0 | 32767 | N + 570 | moving | 1 | R | RAM | 0 | 1 | N + 571 | moving_status | 1 | R | RAM | 0 | 255 | N + 572 | present_pwm | 2 | R | RAM | -2100 | 2100 | Y + 574 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 576 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 580 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 584 | velocity_trajectory | 4 | R | RAM | 0 | 1023 | N + 588 | position_trajectory | 4 | R | RAM | 0 | 4095 | N + 592 | present_voltage | 2 | R | RAM | 0 | 500 | N + 594 | present_temperature | 1 | R | RAM | 0 | 200 | N + 600 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 602 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 604 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 606 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N diff --git a/robotis_device/devices/dynamixel/PH54-200-S500-R.device b/robotis_device/devices/dynamixel/PH54-200-S500-R.device new file mode 100644 index 0000000..b758315 --- /dev/null +++ b/robotis_device/devices/dynamixel/PH54-200-S500-R.device @@ -0,0 +1,92 @@ +[device info] +model_name = PH54-200-S500-R +device_type = dynamixel + +[type info] +torque_to_current_value_ratio = 146.502114 +velocity_to_value_ratio = 954.93 +value_of_0_radian_position = 0 +value_of_min_radian_position = -501923 +value_of_max_radian_position = 501923 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_current +position_d_gain_item_name = position_d_gain +position_i_gain_item_name = position_i_gain +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 10 | drive_mode | 1 | RW | EEPROM | 0 | 3 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 12 | secondary_id | 1 | RW | EEPROM | 0 | 255 | N + 13 | protocol_version | 1 | RW | EEPROM | 1 | 2 | N + 20 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 24 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 31 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 32 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 34 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 36 | pwm_limit | 2 | RW | EEPROM | 0 | 885 | N + 38 | current_limit | 2 | RW | EEPROM | 0 | 32767 | N + 40 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 44 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 48 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 52 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 56 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 57 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 58 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 59 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 63 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 168 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 512 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 513 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 514 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 515 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 516 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 517 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 518 | hardware_error_status | 1 | R | RAM | 0 | 31 | N + 524 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N + 526 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N + 528 | position_d_gain | 2 | RW | RAM | 0 | 2047 | N + 530 | position_i_gain | 2 | RW | RAM | 0 | 2047 | N + 532 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N + 536 | feedforward_2nd_gain | 2 | RW | RAM | 0 | 32767 | N + 538 | feedforward_1st_gain | 2 | RW | RAM | 0 | 32767 | N + 546 | bus_watchdog | 1 | RW | RAM | -1 | 127 | Y + 548 | goal_pwm | 2 | RW | RAM | -2100 | 2100 | Y + 550 | goal_current | 2 | RW | RAM | -32768 | 32767 | Y + 552 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 556 | profile_acceleration | 4 | RW | RAM | 0 | 32767 | N + 560 | profile_velocity | 4 | RW | RAM | 0 | 1023 | N + 564 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 568 | realtime_tick | 2 | R | RAM | 0 | 32767 | N + 570 | moving | 1 | R | RAM | 0 | 1 | N + 571 | moving_status | 1 | R | RAM | 0 | 255 | N + 572 | present_pwm | 2 | R | RAM | -2100 | 2100 | Y + 574 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 576 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 580 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 584 | velocity_trajectory | 4 | R | RAM | 0 | 1023 | N + 588 | position_trajectory | 4 | R | RAM | 0 | 4095 | N + 592 | present_voltage | 2 | R | RAM | 0 | 500 | N + 594 | present_temperature | 1 | R | RAM | 0 | 200 | N + 600 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 602 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 604 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 606 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N From 651204dc5ae68758cc54d582b442b8042758224b Mon Sep 17 00:00:00 2001 From: RonaldsonBellande Date: Wed, 4 May 2022 12:20:56 -0400 Subject: [PATCH 150/238] noetic build --- robotis_controller/CHANGELOG.rst | 5 +++ robotis_controller/CMakeLists.txt | 13 +++++-- robotis_controller/package.xml | 54 ++++++++++++++++++++-------- robotis_device/CHANGELOG.rst | 5 +++ robotis_device/CMakeLists.txt | 6 ++-- robotis_device/package.xml | 17 ++++++--- robotis_framework/CMakeLists.txt | 2 +- robotis_framework/package.xml | 24 +++++++++---- robotis_framework_common/package.xml | 20 +++++++---- 9 files changed, 110 insertions(+), 36 deletions(-) diff --git a/robotis_controller/CHANGELOG.rst b/robotis_controller/CHANGELOG.rst index 990b3bc..ec0f902 100755 --- a/robotis_controller/CHANGELOG.rst +++ b/robotis_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package robotis_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.3.0 (2021-05-03) +------------------ +* Update package.xml and CMakeList.txt for noetic branch +* Contributors: Ronaldson Bellande + 0.2.9 (2018-03-22) ------------------ * added serivce for setting module diff --git a/robotis_controller/CMakeLists.txt b/robotis_controller/CMakeLists.txt index 8e5dd9c..fa09fd4 100755 --- a/robotis_controller/CMakeLists.txt +++ b/robotis_controller/CMakeLists.txt @@ -1,7 +1,7 @@ ################################################################################ # Set minimum required version of cmake, project name and compile options ################################################################################ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.0.2) project(robotis_controller) ## Compile as C++11, supported in ROS Kinetic and newer @@ -60,7 +60,16 @@ endif(NOT ${YAML_CPP_VERSION} VERSION_LESS "0.5") catkin_package( INCLUDE_DIRS include LIBRARIES robotis_controller - CATKIN_DEPENDS roscpp roslib std_msgs sensor_msgs robotis_controller_msgs dynamixel_sdk robotis_device robotis_framework_common cmake_modules + CATKIN_DEPENDS + roscpp + roslib + std_msgs + sensor_msgs + robotis_controller_msgs + dynamixel_sdk + robotis_device + robotis_framework_common + cmake_modules DEPENDS Boost ) diff --git a/robotis_controller/package.xml b/robotis_controller/package.xml index aeb4e91..3e123fa 100755 --- a/robotis_controller/package.xml +++ b/robotis_controller/package.xml @@ -1,28 +1,52 @@ robotis_controller - 0.2.9 - - robotis_controller package for ROBOTIS's platform like Manipulator-H, THORMANG and OP series - + 0.3.0 + robotis_controller package for ROBOTIS's platform like Manipulator-H, THORMANG and OP series Apache 2.0 Zerom Kayman SCH - Pyo + Ronaldson Bellande + http://wiki.ros.org/robotis_controller http://emanual.robotis.com/docs/en/software/robotis_framework_packages/ https://github.com/ROBOTIS-GIT/ROBOTIS-Framework https://github.com/ROBOTIS-GIT/ROBOTIS-Framework/issues + catkin - roscpp - roslib - std_msgs - sensor_msgs - robotis_controller_msgs - dynamixel_sdk - robotis_device - robotis_framework_common - cmake_modules - yaml-cpp + + roscpp + roslib + std_msgs + sensor_msgs + robotis_controller_msgs + dynamixel_sdk + robotis_device + robotis_framework_common + cmake_modules + yaml-cpp + + roscpp + roslib + std_msgs + sensor_msgs + robotis_controller_msgs + dynamixel_sdk + robotis_device + robotis_framework_common + cmake_modules + yaml-cpp + + roscpp + roslib + std_msgs + sensor_msgs + robotis_controller_msgs + dynamixel_sdk + robotis_device + robotis_framework_common + cmake_modules + yaml-cpp + diff --git a/robotis_device/CHANGELOG.rst b/robotis_device/CHANGELOG.rst index af36406..daa3c1d 100755 --- a/robotis_device/CHANGELOG.rst +++ b/robotis_device/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package robotis_device ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.3.0 (2021-05-03) +------------------ +* Update package.xml and CMakeList.txt for noetic branch +* Contributors: Ronaldson Bellande + 0.2.9 (2018-03-22) ------------------ * modified to prevent duplicate indirect address write diff --git a/robotis_device/CMakeLists.txt b/robotis_device/CMakeLists.txt index 17bbcf6..0eb6656 100755 --- a/robotis_device/CMakeLists.txt +++ b/robotis_device/CMakeLists.txt @@ -1,7 +1,7 @@ ################################################################################ # Set minimum required version of cmake, project name and compile options ################################################################################ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.0.2) project(robotis_device) ################################################################################ @@ -30,7 +30,9 @@ find_package(catkin REQUIRED COMPONENTS catkin_package( INCLUDE_DIRS include LIBRARIES robotis_device - CATKIN_DEPENDS roscpp dynamixel_sdk + CATKIN_DEPENDS + roscpp + dynamixel_sdk ) ################################################################################ diff --git a/robotis_device/package.xml b/robotis_device/package.xml index 09a81af..7100feb 100755 --- a/robotis_device/package.xml +++ b/robotis_device/package.xml @@ -1,7 +1,7 @@ robotis_device - 0.2.9 + 0.3.0 The package that manages device information of ROBOTIS robots. This package is used when reading device information with the robot information file @@ -11,12 +11,21 @@ Zerom Kayman SCH - Pyo + Ronaldson Bellande + http://wiki.ros.org/robotis_device http://emanual.robotis.com/docs/en/software/robotis_framework_packages/ https://github.com/ROBOTIS-GIT/ROBOTIS-Framework https://github.com/ROBOTIS-GIT/ROBOTIS-Framework/issues + catkin - roscpp - dynamixel_sdk + + roscpp + dynamixel_sdk + + roscpp + dynamixel_sdk + + roscpp + dynamixel_sdk diff --git a/robotis_framework/CMakeLists.txt b/robotis_framework/CMakeLists.txt index d336fe9..5777d65 100644 --- a/robotis_framework/CMakeLists.txt +++ b/robotis_framework/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.0.2) project(robotis_framework) find_package(catkin REQUIRED) catkin_metapackage() diff --git a/robotis_framework/package.xml b/robotis_framework/package.xml index 775bdd5..91ab8f5 100755 --- a/robotis_framework/package.xml +++ b/robotis_framework/package.xml @@ -1,22 +1,34 @@ robotis_framework - 0.2.9 - - ROS packages for the robotis_framework (meta package) - + 0.3.0 + ROS packages for the robotis_framework (meta package) Apache 2.0 Zerom Kayman SCH - Pyo + Ronaldson Bellande + http://wiki.ros.org/robotis_framework http://emanual.robotis.com/docs/en/software/robotis_framework_packages/ https://github.com/ROBOTIS-GIT/ROBOTIS-Framework https://github.com/ROBOTIS-GIT/ROBOTIS-Framework/issues + catkin + + robotis_controller + robotis_device + robotis_framework_common + + robotis_controller + robotis_device + robotis_framework_common + robotis_controller robotis_device robotis_framework_common - + + + + diff --git a/robotis_framework_common/package.xml b/robotis_framework_common/package.xml index 08a423b..e2fefcc 100755 --- a/robotis_framework_common/package.xml +++ b/robotis_framework_common/package.xml @@ -2,19 +2,27 @@ robotis_framework_common 0.2.9 - - The package contains commonly used Headers for the ROBOTIS Framework. - + The package contains commonly used Headers for the ROBOTIS Framework. Apache 2.0 Zerom Kayman SCH - Pyo + Ronaldson Bellande + http://wiki.ros.org/robotis_framework_common http://emanual.robotis.com/docs/en/software/robotis_framework_packages/ https://github.com/ROBOTIS-GIT/ROBOTIS-Framework https://github.com/ROBOTIS-GIT/ROBOTIS-Framework/issues + catkin - roscpp - robotis_device + + roscpp + robotis_device + + roscpp + robotis_device + + roscpp + robotis_device + From 16d64e855c05554b89761df40c97fb92f49b94ad Mon Sep 17 00:00:00 2001 From: RonaldsonBellande Date: Wed, 4 May 2022 18:50:09 -0400 Subject: [PATCH 151/238] small modification --- .../robotis_controller/robotis_controller.h | 265 ++++++++------- .../robotis_device/control_table_item.h | 84 ++--- .../src/robotis_device/dynamixel.cpp | 141 ++++---- robotis_device/src/robotis_device/robot.cpp | 316 +++++++++--------- robotis_device/src/robotis_device/sensor.cpp | 52 +-- 5 files changed, 417 insertions(+), 441 deletions(-) diff --git a/robotis_controller/include/robotis_controller/robotis_controller.h b/robotis_controller/include/robotis_controller/robotis_controller.h index 05ae93c..75a4580 100755 --- a/robotis_controller/include/robotis_controller/robotis_controller.h +++ b/robotis_controller/include/robotis_controller/robotis_controller.h @@ -1,18 +1,18 @@ -/******************************************************************************* -* Copyright 2018 ROBOTIS CO., LTD. -* -* 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. -*******************************************************************************/ +/******************************************************************************* + * Copyright 2018 ROBOTIS CO., LTD. + * + * 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. + *******************************************************************************/ /* * robotis_controller.h @@ -22,96 +22,99 @@ */ #ifndef ROBOTIS_CONTROLLER_ROBOTIS_CONTROLLER_H_ -#define ROBOTIS_CONTROLLER_ROBOTIS_CONTROLLER_H_ +#define ROBOTIS_CONTROLLER_ROBOTIS_CONTROLLER_H_ + +#include +#include +#include +#include +#include +#include +#include +#include "robotis_controller_msgs/GetJointModule.h" +#include "robotis_controller_msgs/JointCtrlModule.h" +#include "robotis_controller_msgs/LoadOffset.h" +#include "robotis_controller_msgs/SetJointModule.h" +#include "robotis_controller_msgs/SetModule.h" +#include "robotis_controller_msgs/SyncWriteItem.h" +#include "robotis_controller_msgs/WriteControlTable.h" -#include -#include -#include -#include -#include -#include -#include +#include "dynamixel_sdk/group_bulk_read.h" +#include "dynamixel_sdk/group_sync_write.h" +#include "robotis_device/robot.h" +#include "robotis_framework_common/motion_module.h" +#include "robotis_framework_common/sensor_module.h" -#include "robotis_controller_msgs/WriteControlTable.h" -#include "robotis_controller_msgs/SyncWriteItem.h" -#include "robotis_controller_msgs/JointCtrlModule.h" -#include "robotis_controller_msgs/GetJointModule.h" -#include "robotis_controller_msgs/SetJointModule.h" -#include "robotis_controller_msgs/SetModule.h" -#include "robotis_controller_msgs/LoadOffset.h" +namespace robotis_framework { -#include "robotis_device/robot.h" -#include "robotis_framework_common/motion_module.h" -#include "robotis_framework_common/sensor_module.h" -#include "dynamixel_sdk/group_bulk_read.h" -#include "dynamixel_sdk/group_sync_write.h" +enum ControllerMode { MotionModuleMode, DirectControlMode }; -namespace robotis_framework -{ - -enum ControllerMode -{ - MotionModuleMode, - DirectControlMode -}; - -class RobotisController : public Singleton -{ +class RobotisController : public Singleton { private: - boost::thread queue_thread_; - boost::thread gazebo_thread_; - boost::thread set_module_thread_; - boost::mutex queue_mutex_; + boost::thread queue_thread_; + boost::thread gazebo_thread_; + boost::thread set_module_thread_; + boost::mutex queue_mutex_; - bool init_pose_loaded_; - bool is_timer_running_; - bool is_offset_enabled_; - double offset_ratio_; - bool stop_timer_; - pthread_t timer_thread_; - ControllerMode controller_mode_; + bool init_pose_loaded_; + bool is_timer_running_; + bool is_offset_enabled_; + double offset_ratio_; + bool stop_timer_; + pthread_t timer_thread_; + ControllerMode controller_mode_; std::list motion_modules_; std::list sensor_modules_; - std::vector direct_sync_write_; + std::vector direct_sync_write_; std::map sensor_result_; void gazeboTimerThread(); void msgQueueThread(); void setCtrlModuleThread(std::string ctrl_module); - void setJointCtrlModuleThread(const robotis_controller_msgs::JointCtrlModule::ConstPtr &msg); + void setJointCtrlModuleThread( + const robotis_controller_msgs::JointCtrlModule::ConstPtr &msg); bool isTimerStopped(); void initializeSyncWrite(); public: - const int NONE_GAIN = 65535; - bool DEBUG_PRINT; - Robot *robot_; + const int NONE_GAIN = 65535; + bool DEBUG_PRINT; + Robot *robot_; - bool gazebo_mode_; - std::string gazebo_robot_name_; + bool gazebo_mode_; + std::string gazebo_robot_name_; /* bulk read */ - std::map port_to_bulk_read_; + std::map port_to_bulk_read_; /* sync write */ - std::map port_to_sync_write_position_; - std::map port_to_sync_write_velocity_; - std::map port_to_sync_write_current_; - std::map port_to_sync_write_position_p_gain_; - std::map port_to_sync_write_position_i_gain_; - std::map port_to_sync_write_position_d_gain_; - std::map port_to_sync_write_velocity_p_gain_; - std::map port_to_sync_write_velocity_i_gain_; - std::map port_to_sync_write_velocity_d_gain_; + std::map + port_to_sync_write_position_; + std::map + port_to_sync_write_velocity_; + std::map + port_to_sync_write_current_; + std::map + port_to_sync_write_position_p_gain_; + std::map + port_to_sync_write_position_i_gain_; + std::map + port_to_sync_write_position_d_gain_; + std::map + port_to_sync_write_velocity_p_gain_; + std::map + port_to_sync_write_velocity_i_gain_; + std::map + port_to_sync_write_velocity_d_gain_; /* publisher */ - ros::Publisher goal_joint_state_pub_; - ros::Publisher present_joint_state_pub_; - ros::Publisher current_module_pub_; + ros::Publisher goal_joint_state_pub_; + ros::Publisher present_joint_state_pub_; + ros::Publisher current_module_pub_; std::map gazebo_joint_position_pub_; std::map gazebo_joint_velocity_pub_; @@ -121,62 +124,84 @@ public: RobotisController(); - bool initialize(const std::string robot_file_path, const std::string init_file_path); - void initializeDevice(const std::string init_file_path); - void process(); + bool initialize(const std::string robot_file_path, + const std::string init_file_path); + void initializeDevice(const std::string init_file_path); + void process(); - void addMotionModule(MotionModule *module); - void removeMotionModule(MotionModule *module); - void addSensorModule(SensorModule *module); - void removeSensorModule(SensorModule *module); + void addMotionModule(MotionModule *module); + void removeMotionModule(MotionModule *module); + void addSensorModule(SensorModule *module); + void removeSensorModule(SensorModule *module); - void startTimer(); - void stopTimer(); - bool isTimerRunning(); + void startTimer(); + void stopTimer(); + bool isTimerRunning(); - void setCtrlModule(std::string module_name); - void loadOffset(const std::string path); + void setCtrlModule(std::string module_name); + void loadOffset(const std::string path); /* ROS Topic Callback Functions */ - void writeControlTableCallback(const robotis_controller_msgs::WriteControlTable::ConstPtr &msg); - void syncWriteItemCallback(const robotis_controller_msgs::SyncWriteItem::ConstPtr &msg); - void setControllerModeCallback(const std_msgs::String::ConstPtr &msg); - void setJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg); - void setJointCtrlModuleCallback(const robotis_controller_msgs::JointCtrlModule::ConstPtr &msg); - void setCtrlModuleCallback(const std_msgs::String::ConstPtr &msg); - void enableOffsetCallback(const std_msgs::Bool::ConstPtr &msg); - bool getJointCtrlModuleService(robotis_controller_msgs::GetJointModule::Request &req, robotis_controller_msgs::GetJointModule::Response &res); - bool setJointCtrlModuleService(robotis_controller_msgs::SetJointModule::Request &req, robotis_controller_msgs::SetJointModule::Response &res); - bool setCtrlModuleService(robotis_controller_msgs::SetModule::Request &req, robotis_controller_msgs::SetModule::Response &res); - bool loadOffsetService(robotis_controller_msgs::LoadOffset::Request &req, robotis_controller_msgs::LoadOffset::Response &res); + void writeControlTableCallback( + const robotis_controller_msgs::WriteControlTable::ConstPtr &msg); + void syncWriteItemCallback( + const robotis_controller_msgs::SyncWriteItem::ConstPtr &msg); + void setControllerModeCallback(const std_msgs::String::ConstPtr &msg); + void setJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg); + void setJointCtrlModuleCallback( + const robotis_controller_msgs::JointCtrlModule::ConstPtr &msg); + void setCtrlModuleCallback(const std_msgs::String::ConstPtr &msg); + void enableOffsetCallback(const std_msgs::Bool::ConstPtr &msg); + bool getJointCtrlModuleService( + robotis_controller_msgs::GetJointModule::Request &req, + robotis_controller_msgs::GetJointModule::Response &res); + bool setJointCtrlModuleService( + robotis_controller_msgs::SetJointModule::Request &req, + robotis_controller_msgs::SetJointModule::Response &res); + bool setCtrlModuleService(robotis_controller_msgs::SetModule::Request &req, + robotis_controller_msgs::SetModule::Response &res); + bool loadOffsetService(robotis_controller_msgs::LoadOffset::Request &req, + robotis_controller_msgs::LoadOffset::Response &res); - void gazeboJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg); + void gazeboJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg); - int ping (const std::string joint_name, uint8_t *error = 0); - int ping (const std::string joint_name, uint16_t* model_number, uint8_t *error = 0); + int ping(const std::string joint_name, uint8_t *error = 0); + int ping(const std::string joint_name, uint16_t *model_number, + uint8_t *error = 0); - int action (const std::string joint_name); - int reboot (const std::string joint_name, uint8_t *error = 0); - int factoryReset(const std::string joint_name, uint8_t option = 0, uint8_t *error = 0); + int action(const std::string joint_name); + int reboot(const std::string joint_name, uint8_t *error = 0); + int factoryReset(const std::string joint_name, uint8_t option = 0, + uint8_t *error = 0); - int read (const std::string joint_name, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0); - int readCtrlItem(const std::string joint_name, const std::string item_name, uint32_t *data, uint8_t *error = 0); + int read(const std::string joint_name, uint16_t address, uint16_t length, + uint8_t *data, uint8_t *error = 0); + int readCtrlItem(const std::string joint_name, const std::string item_name, + uint32_t *data, uint8_t *error = 0); - int read1Byte (const std::string joint_name, uint16_t address, uint8_t *data, uint8_t *error = 0); - int read2Byte (const std::string joint_name, uint16_t address, uint16_t *data, uint8_t *error = 0); - int read4Byte (const std::string joint_name, uint16_t address, uint32_t *data, uint8_t *error = 0); + int read1Byte(const std::string joint_name, uint16_t address, uint8_t *data, + uint8_t *error = 0); + int read2Byte(const std::string joint_name, uint16_t address, uint16_t *data, + uint8_t *error = 0); + int read4Byte(const std::string joint_name, uint16_t address, uint32_t *data, + uint8_t *error = 0); - int write (const std::string joint_name, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0); - int writeCtrlItem(const std::string joint_name, const std::string item_name, uint32_t data, uint8_t *error = 0); + int write(const std::string joint_name, uint16_t address, uint16_t length, + uint8_t *data, uint8_t *error = 0); + int writeCtrlItem(const std::string joint_name, const std::string item_name, + uint32_t data, uint8_t *error = 0); - int write1Byte (const std::string joint_name, uint16_t address, uint8_t data, uint8_t *error = 0); - int write2Byte (const std::string joint_name, uint16_t address, uint16_t data, uint8_t *error = 0); - int write4Byte (const std::string joint_name, uint16_t address, uint32_t data, uint8_t *error = 0); + int write1Byte(const std::string joint_name, uint16_t address, uint8_t data, + uint8_t *error = 0); + int write2Byte(const std::string joint_name, uint16_t address, uint16_t data, + uint8_t *error = 0); + int write4Byte(const std::string joint_name, uint16_t address, uint32_t data, + uint8_t *error = 0); - int regWrite (const std::string joint_name, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0); + int regWrite(const std::string joint_name, uint16_t address, uint16_t length, + uint8_t *data, uint8_t *error = 0); }; -} - - +} // namespace robotis_framework + #endif /* ROBOTIS_CONTROLLER_ROBOTIS_CONTROLLER_H_ */ diff --git a/robotis_device/include/robotis_device/control_table_item.h b/robotis_device/include/robotis_device/control_table_item.h index ed7b956..7249a90 100755 --- a/robotis_device/include/robotis_device/control_table_item.h +++ b/robotis_device/include/robotis_device/control_table_item.h @@ -1,18 +1,18 @@ -/******************************************************************************* -* Copyright 2018 ROBOTIS CO., LTD. -* -* 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. -*******************************************************************************/ +/******************************************************************************* + * Copyright 2018 ROBOTIS CO., LTD. + * + * 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. + *******************************************************************************/ /* * control_table_item.h @@ -22,49 +22,33 @@ */ #ifndef ROBOTIS_DEVICE_CONTROL_TABLE_ITEM_H_ -#define ROBOTIS_DEVICE_CONTROL_TABLE_ITEM_H_ - - +#define ROBOTIS_DEVICE_CONTROL_TABLE_ITEM_H_ + #include -namespace robotis_framework -{ +namespace robotis_framework { -enum AccessType { - Read, - ReadWrite -}; +enum AccessType { Read, ReadWrite }; -enum MemoryType { - EEPROM, - RAM -}; +enum MemoryType { EEPROM, RAM }; -class ControlTableItem -{ +class ControlTableItem { public: std::string item_name_; - uint16_t address_; - AccessType access_type_; - MemoryType memory_type_; - uint8_t data_length_; - int32_t data_min_value_; - int32_t data_max_value_; - bool is_signed_; + uint16_t address_; + AccessType access_type_; + MemoryType memory_type_; + uint8_t data_length_; + int32_t data_min_value_; + int32_t data_max_value_; + bool is_signed_; - ControlTableItem() - : item_name_(""), - address_(0), - access_type_(Read), - memory_type_(RAM), - data_length_(0), - data_min_value_(0), - data_max_value_(0), - is_signed_(false) - { } + ControlTableItem() + : item_name_(""), address_(0), access_type_(Read), memory_type_(RAM), + data_length_(0), data_min_value_(0), data_max_value_(0), + is_signed_(false) {} }; -} - - +} // namespace robotis_framework + #endif /* ROBOTIS_DEVICE_CONTROL_TABLE_ITEM_H_ */ diff --git a/robotis_device/src/robotis_device/dynamixel.cpp b/robotis_device/src/robotis_device/dynamixel.cpp index 500393c..beda733 100755 --- a/robotis_device/src/robotis_device/dynamixel.cpp +++ b/robotis_device/src/robotis_device/dynamixel.cpp @@ -1,18 +1,18 @@ /******************************************************************************* -* Copyright 2018 ROBOTIS CO., LTD. -* -* 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. -*******************************************************************************/ + * Copyright 2018 ROBOTIS CO., LTD. + * + * 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. + *******************************************************************************/ /* * dynamixel.cpp @@ -26,28 +26,15 @@ using namespace robotis_framework; Dynamixel::Dynamixel(int id, std::string model_name, float protocol_version) - : ctrl_module_name_("none"), - torque_to_current_value_ratio_(1.0), - velocity_to_value_ratio_(1.0), - value_of_0_radian_position_(0), - value_of_min_radian_position_(0), - value_of_max_radian_position_(0), - min_radian_(-3.14159265), - max_radian_(3.14159265), - torque_enable_item_(0), - present_position_item_(0), - present_velocity_item_(0), - present_current_item_(0), - goal_position_item_(0), - goal_velocity_item_(0), - goal_current_item_(0), - position_p_gain_item_(0), - position_i_gain_item_(0), - position_d_gain_item_(0), - velocity_p_gain_item_(0), - velocity_i_gain_item_(0), - velocity_d_gain_item_(0) -{ + : ctrl_module_name_("none"), torque_to_current_value_ratio_(1.0), + velocity_to_value_ratio_(1.0), value_of_0_radian_position_(0), + value_of_min_radian_position_(0), value_of_max_radian_position_(0), + min_radian_(-3.14159265), max_radian_(3.14159265), torque_enable_item_(0), + present_position_item_(0), present_velocity_item_(0), + present_current_item_(0), goal_position_item_(0), goal_velocity_item_(0), + goal_current_item_(0), position_p_gain_item_(0), position_i_gain_item_(0), + position_d_gain_item_(0), velocity_p_gain_item_(0), + velocity_i_gain_item_(0), velocity_d_gain_item_(0) { this->id_ = id; this->model_name_ = model_name; this->port_name_ = ""; @@ -59,80 +46,74 @@ Dynamixel::Dynamixel(int id, std::string model_name, float protocol_version) bulk_read_items_.clear(); } -double Dynamixel::convertValue2Radian(int32_t value) -{ +double Dynamixel::convertValue2Radian(int32_t value) { double radian = 0.0; - if (value > value_of_0_radian_position_) - { + if (value > value_of_0_radian_position_) { if (max_radian_ <= 0) return max_radian_; - radian = (double) (value - value_of_0_radian_position_) * max_radian_ - / (double) (value_of_max_radian_position_ - value_of_0_radian_position_); - } - else if (value < value_of_0_radian_position_) - { + radian = + (double)(value - value_of_0_radian_position_) * max_radian_ / + (double)(value_of_max_radian_position_ - value_of_0_radian_position_); + } else if (value < value_of_0_radian_position_) { if (min_radian_ >= 0) return min_radian_; - radian = (double) (value - value_of_0_radian_position_) * min_radian_ - / (double) (value_of_min_radian_position_ - value_of_0_radian_position_); + radian = + (double)(value - value_of_0_radian_position_) * min_radian_ / + (double)(value_of_min_radian_position_ - value_of_0_radian_position_); } -// if (radian > max_radian_) -// return max_radian_; -// else if (radian < min_radian_) -// return min_radian_; + // if (radian > max_radian_) + // return max_radian_; + // else if (radian < min_radian_) + // return min_radian_; return radian; } -int32_t Dynamixel::convertRadian2Value(double radian) -{ +int32_t Dynamixel::convertRadian2Value(double radian) { int32_t value = 0; - if (radian > 0) - { + if (radian > 0) { if (value_of_max_radian_position_ <= value_of_0_radian_position_) return value_of_max_radian_position_; - value = (radian * (value_of_max_radian_position_ - value_of_0_radian_position_) / max_radian_) - + value_of_0_radian_position_; - } - else if (radian < 0) - { + value = (radian * + (value_of_max_radian_position_ - value_of_0_radian_position_) / + max_radian_) + + value_of_0_radian_position_; + } else if (radian < 0) { if (value_of_min_radian_position_ >= value_of_0_radian_position_) return value_of_min_radian_position_; - value = (radian * (value_of_min_radian_position_ - value_of_0_radian_position_) / min_radian_) - + value_of_0_radian_position_; - } - else + value = (radian * + (value_of_min_radian_position_ - value_of_0_radian_position_) / + min_radian_) + + value_of_0_radian_position_; + } else value = value_of_0_radian_position_; -// if (value > value_of_max_radian_position_) -// return value_of_max_radian_position_; -// else if (value < value_of_min_radian_position_) -// return value_of_min_radian_position_; + // if (value > value_of_max_radian_position_) + // return value_of_max_radian_position_; + // else if (value < value_of_min_radian_position_) + // return value_of_min_radian_position_; return value; } -double Dynamixel::convertValue2Velocity(int32_t value) -{ - return (double) value / velocity_to_value_ratio_; +double Dynamixel::convertValue2Velocity(int32_t value) { + return (double)value / velocity_to_value_ratio_; } -int32_t Dynamixel::convertVelocity2Value(double velocity) -{ - return (int32_t) (velocity * velocity_to_value_ratio_);; +int32_t Dynamixel::convertVelocity2Value(double velocity) { + return (int32_t)(velocity * velocity_to_value_ratio_); + ; } -double Dynamixel::convertValue2Torque(int16_t value) -{ - return (double) value / torque_to_current_value_ratio_; +double Dynamixel::convertValue2Torque(int16_t value) { + return (double)value / torque_to_current_value_ratio_; } -int16_t Dynamixel::convertTorque2Value(double torque) -{ - return (int16_t) (torque * torque_to_current_value_ratio_); +int16_t Dynamixel::convertTorque2Value(double torque) { + return (int16_t)(torque * torque_to_current_value_ratio_); } diff --git a/robotis_device/src/robotis_device/robot.cpp b/robotis_device/src/robotis_device/robot.cpp index 45a97c5..ea738f1 100755 --- a/robotis_device/src/robotis_device/robot.cpp +++ b/robotis_device/src/robotis_device/robot.cpp @@ -1,18 +1,18 @@ -/******************************************************************************* -* Copyright 2018 ROBOTIS CO., LTD. -* -* 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. -*******************************************************************************/ +/******************************************************************************* + * Copyright 2018 ROBOTIS CO., LTD. + * + * 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. + *******************************************************************************/ /* * robot.cpp @@ -21,36 +21,35 @@ * Author: zerom */ -#include -#include -#include +#include +#include +#include #include "robotis_device/robot.h" using namespace robotis_framework; -static inline std::string <rim(std::string &s) -{ - s.erase(s.begin(), std::find_if(s.begin(), s.end(), std::not1(std::ptr_fun(std::isspace)))); +static inline std::string <rim(std::string &s) { + s.erase(s.begin(), + std::find_if(s.begin(), s.end(), + std::not1(std::ptr_fun(std::isspace)))); return s; } -static inline std::string &rtrim(std::string &s) -{ - s.erase(std::find_if(s.rbegin(), s.rend(), std::not1(std::ptr_fun(std::isspace))).base(), s.end()); +static inline std::string &rtrim(std::string &s) { + s.erase(std::find_if(s.rbegin(), s.rend(), + std::not1(std::ptr_fun(std::isspace))) + .base(), + s.end()); return s; } -static inline std::string &trim(std::string &s) -{ - return ltrim(rtrim(s)); -} +static inline std::string &trim(std::string &s) { return ltrim(rtrim(s)); } -static inline std::vector split(const std::string &text, char sep) -{ +static inline std::vector split(const std::string &text, + char sep) { std::vector tokens; std::size_t start = 0, end = 0; - while ((end = text.find(sep, start)) != (std::string::npos)) - { + while ((end = text.find(sep, start)) != (std::string::npos)) { tokens.push_back(text.substr(start, end - start)); trim(tokens.back()); start = end + 1; @@ -61,19 +60,16 @@ static inline std::vector split(const std::string &text, char sep) return tokens; } -Robot::Robot(std::string robot_file_path, std::string dev_desc_dir_path) - : control_cycle_msec_(DEFAULT_CONTROL_CYCLE) -{ +Robot::Robot(std::string robot_file_path, std::string dev_desc_dir_path) + : control_cycle_msec_(DEFAULT_CONTROL_CYCLE) { if (dev_desc_dir_path.compare(dev_desc_dir_path.size() - 1, 1, "/") != 0) dev_desc_dir_path += "/"; std::ifstream file(robot_file_path.c_str()); - if (file.is_open()) - { + if (file.is_open()) { std::string session = ""; std::string input_str; - while (!file.eof()) - { + while (!file.eof()) { std::getline(file, input_str); // remove comment ( # ) @@ -85,160 +81,157 @@ Robot::Robot(std::string robot_file_path, std::string dev_desc_dir_path) input_str = trim(input_str); // find session - if (!input_str.compare(0, 1, "[") && !input_str.compare(input_str.size() - 1, 1, "]")) - { + if (!input_str.compare(0, 1, "[") && + !input_str.compare(input_str.size() - 1, 1, "]")) { input_str = input_str.substr(1, input_str.size() - 2); - std::transform(input_str.begin(), input_str.end(), input_str.begin(), ::tolower); + std::transform(input_str.begin(), input_str.end(), input_str.begin(), + ::tolower); session = trim(input_str); continue; } - if (session == SESSION_CONTROL_INFO) - { + if (session == SESSION_CONTROL_INFO) { std::vector tokens = split(input_str, '='); if (tokens.size() != 2) continue; if (tokens[0] == "control_cycle") control_cycle_msec_ = std::atoi(tokens[1].c_str()); - } - else if (session == SESSION_PORT_INFO) - { + } else if (session == SESSION_PORT_INFO) { std::vector tokens = split(input_str, '|'); if (tokens.size() != 3) continue; - std::cout << tokens[0] << " added. (baudrate: " << tokens[1] << ")" << std::endl; + std::cout << tokens[0] << " added. (baudrate: " << tokens[1] << ")" + << std::endl; - ports_[tokens[0]] = dynamixel::PortHandler::getPortHandler(tokens[0].c_str()); + ports_[tokens[0]] = + dynamixel::PortHandler::getPortHandler(tokens[0].c_str()); ports_[tokens[0]]->setBaudRate(std::atoi(tokens[1].c_str())); port_default_device_[tokens[0]] = tokens[2]; - } - else if (session == SESSION_DEVICE_INFO) - { + } else if (session == SESSION_DEVICE_INFO) { std::vector tokens = split(input_str, '|'); if (tokens.size() != 7) continue; - if (tokens[0] == DYNAMIXEL) - { - std::string file_path = dev_desc_dir_path + tokens[0] + "/" + tokens[3] + ".device"; - int id = std::atoi(tokens[2].c_str()); - std::string port = tokens[1]; - float protocol = std::atof(tokens[4].c_str()); - std::string dev_name = tokens[5]; + if (tokens[0] == DYNAMIXEL) { + std::string file_path = + dev_desc_dir_path + tokens[0] + "/" + tokens[3] + ".device"; + int id = std::atoi(tokens[2].c_str()); + std::string port = tokens[1]; + float protocol = std::atof(tokens[4].c_str()); + std::string dev_name = tokens[5]; dxls_[dev_name] = getDynamixel(file_path, id, port, protocol); Dynamixel *dxl = dxls_[dev_name]; std::vector sub_tokens = split(tokens[6], ','); - if (sub_tokens.size() > 0 && sub_tokens[0] != "") - { - std::map::iterator indirect_it = dxl->ctrl_table_.find(INDIRECT_ADDRESS_1); - if (indirect_it != dxl->ctrl_table_.end()) // INDIRECT_ADDRESS_1 exist + if (sub_tokens.size() > 0 && sub_tokens[0] != "") { + std::map::iterator indirect_it = + dxl->ctrl_table_.find(INDIRECT_ADDRESS_1); + if (indirect_it != + dxl->ctrl_table_.end()) // INDIRECT_ADDRESS_1 exist { - uint16_t indirect_data_addr = dxl->ctrl_table_[INDIRECT_DATA_1]->address_; - for (int _i = 0; _i < sub_tokens.size(); _i++) - { - std::map::iterator bulkread_it = dxl->ctrl_table_.find(sub_tokens[_i]); - if(bulkread_it == dxl->ctrl_table_.end()) - { - fprintf(stderr, "\n ##### BULK READ ITEM [ %s ] NOT FOUND!! #####\n\n", sub_tokens[_i].c_str()); + uint16_t indirect_data_addr = + dxl->ctrl_table_[INDIRECT_DATA_1]->address_; + for (int _i = 0; _i < sub_tokens.size(); _i++) { + std::map::iterator + bulkread_it = dxl->ctrl_table_.find(sub_tokens[_i]); + if (bulkread_it == dxl->ctrl_table_.end()) { + fprintf( + stderr, + "\n ##### BULK READ ITEM [ %s ] NOT FOUND!! #####\n\n", + sub_tokens[_i].c_str()); continue; } dxl->bulk_read_items_.push_back(new ControlTableItem()); ControlTableItem *dest_item = dxl->bulk_read_items_[_i]; - ControlTableItem *src_item = dxl->ctrl_table_[sub_tokens[_i]]; + ControlTableItem *src_item = dxl->ctrl_table_[sub_tokens[_i]]; - dest_item->item_name_ = src_item->item_name_; - dest_item->address_ = indirect_data_addr; - dest_item->access_type_ = src_item->access_type_; - dest_item->memory_type_ = src_item->memory_type_; - dest_item->data_length_ = src_item->data_length_; - dest_item->data_min_value_ = src_item->data_min_value_; - dest_item->data_max_value_ = src_item->data_max_value_; - dest_item->is_signed_ = src_item->is_signed_; + dest_item->item_name_ = src_item->item_name_; + dest_item->address_ = indirect_data_addr; + dest_item->access_type_ = src_item->access_type_; + dest_item->memory_type_ = src_item->memory_type_; + dest_item->data_length_ = src_item->data_length_; + dest_item->data_min_value_ = src_item->data_min_value_; + dest_item->data_max_value_ = src_item->data_max_value_; + dest_item->is_signed_ = src_item->is_signed_; indirect_data_addr += dest_item->data_length_; } - } - else // INDIRECT_ADDRESS_1 not exist + } else // INDIRECT_ADDRESS_1 not exist { - for (int i = 0; i < sub_tokens.size(); i++) - { + for (int i = 0; i < sub_tokens.size(); i++) { if (dxl->ctrl_table_[sub_tokens[i]] != NULL) - dxl->bulk_read_items_.push_back(dxl->ctrl_table_[sub_tokens[i]]); + dxl->bulk_read_items_.push_back( + dxl->ctrl_table_[sub_tokens[i]]); } } } - } - else if (tokens[0] == SENSOR) - { - std::string file_path = dev_desc_dir_path + tokens[0] + "/" + tokens[3] + ".device"; - int id = std::atoi(tokens[2].c_str()); - std::string port = tokens[1]; - float protocol = std::atof(tokens[4].c_str()); - std::string dev_name = tokens[5]; + } else if (tokens[0] == SENSOR) { + std::string file_path = + dev_desc_dir_path + tokens[0] + "/" + tokens[3] + ".device"; + int id = std::atoi(tokens[2].c_str()); + std::string port = tokens[1]; + float protocol = std::atof(tokens[4].c_str()); + std::string dev_name = tokens[5]; sensors_[dev_name] = getSensor(file_path, id, port, protocol); Sensor *sensor = sensors_[dev_name]; std::vector sub_tokens = split(tokens[6], ','); - if (sub_tokens.size() > 0 && sub_tokens[0] != "") - { - std::map::iterator indirect_it = sensor->ctrl_table_.find(INDIRECT_ADDRESS_1); - if (indirect_it != sensor->ctrl_table_.end()) // INDIRECT_ADDRESS_1 exist + if (sub_tokens.size() > 0 && sub_tokens[0] != "") { + std::map::iterator indirect_it = + sensor->ctrl_table_.find(INDIRECT_ADDRESS_1); + if (indirect_it != + sensor->ctrl_table_.end()) // INDIRECT_ADDRESS_1 exist { - uint16_t indirect_data_addr = sensor->ctrl_table_[INDIRECT_DATA_1]->address_; - for (int i = 0; i < sub_tokens.size(); i++) - { + uint16_t indirect_data_addr = + sensor->ctrl_table_[INDIRECT_DATA_1]->address_; + for (int i = 0; i < sub_tokens.size(); i++) { sensor->bulk_read_items_.push_back(new ControlTableItem()); ControlTableItem *dest_item = sensor->bulk_read_items_[i]; - ControlTableItem *src_item = sensor->ctrl_table_[sub_tokens[i]]; + ControlTableItem *src_item = sensor->ctrl_table_[sub_tokens[i]]; - dest_item->item_name_ = src_item->item_name_; - dest_item->address_ = indirect_data_addr; - dest_item->access_type_ = src_item->access_type_; - dest_item->memory_type_ = src_item->memory_type_; - dest_item->data_length_ = src_item->data_length_; - dest_item->data_min_value_ = src_item->data_min_value_; - dest_item->data_max_value_ = src_item->data_max_value_; - dest_item->is_signed_ = src_item->is_signed_; + dest_item->item_name_ = src_item->item_name_; + dest_item->address_ = indirect_data_addr; + dest_item->access_type_ = src_item->access_type_; + dest_item->memory_type_ = src_item->memory_type_; + dest_item->data_length_ = src_item->data_length_; + dest_item->data_min_value_ = src_item->data_min_value_; + dest_item->data_max_value_ = src_item->data_max_value_; + dest_item->is_signed_ = src_item->is_signed_; indirect_data_addr += dest_item->data_length_; } - } - else // INDIRECT_ADDRESS_1 exist + } else // INDIRECT_ADDRESS_1 exist { for (int i = 0; i < sub_tokens.size(); i++) - sensor->bulk_read_items_.push_back(sensor->ctrl_table_[sub_tokens[i]]); + sensor->bulk_read_items_.push_back( + sensor->ctrl_table_[sub_tokens[i]]); } } } } } file.close(); - } - else - { + } else { std::cout << "Unable to open file : " + robot_file_path << std::endl; } } -Sensor *Robot::getSensor(std::string path, int id, std::string port, float protocol_version) -{ +Sensor *Robot::getSensor(std::string path, int id, std::string port, + float protocol_version) { Sensor *sensor; // load file model_name.device std::ifstream file(path.c_str()); - if (file.is_open()) - { + if (file.is_open()) { std::string session = ""; std::string input_str; - while (!file.eof()) - { + while (!file.eof()) { std::getline(file, input_str); // remove comment ( # ) @@ -252,33 +245,31 @@ Sensor *Robot::getSensor(std::string path, int id, std::string port, float proto continue; // find _session - if (!input_str.compare(0, 1, "[") && !input_str.compare(input_str.size() - 1, 1, "]")) - { + if (!input_str.compare(0, 1, "[") && + !input_str.compare(input_str.size() - 1, 1, "]")) { input_str = input_str.substr(1, input_str.size() - 2); - std::transform(input_str.begin(), input_str.end(), input_str.begin(), ::tolower); + std::transform(input_str.begin(), input_str.end(), input_str.begin(), + ::tolower); session = trim(input_str); continue; } - if (session == SESSION_DEVICE_INFO) - { + if (session == SESSION_DEVICE_INFO) { std::vector tokens = split(input_str, '='); if (tokens.size() != 2) continue; if (tokens[0] == "model_name") sensor = new Sensor(id, tokens[1], protocol_version); - } - else if (session == SESSION_CONTROL_TABLE) - { + } else if (session == SESSION_CONTROL_TABLE) { std::vector tokens = split(input_str, '|'); if (tokens.size() != 8) continue; ControlTableItem *item = new ControlTableItem(); - item->item_name_ = tokens[1]; - item->address_ = std::atoi(tokens[0].c_str()); - item->data_length_ = std::atoi(tokens[2].c_str()); + item->item_name_ = tokens[1]; + item->address_ = std::atoi(tokens[0].c_str()); + item->data_length_ = std::atoi(tokens[2].c_str()); if (tokens[3] == "R") item->access_type_ = Read; @@ -302,24 +293,24 @@ Sensor *Robot::getSensor(std::string path, int id, std::string port, float proto } sensor->port_name_ = port; - fprintf(stderr, "(%s) [ID:%3d] %14s added. \n", port.c_str(), sensor->id_, sensor->model_name_.c_str()); - //std::cout << "[ID:" << (int)(_sensor->id) << "] " << _sensor->model_name << " added. (" << port << ")" << std::endl; + fprintf(stderr, "(%s) [ID:%3d] %14s added. \n", port.c_str(), sensor->id_, + sensor->model_name_.c_str()); + // std::cout << "[ID:" << (int)(_sensor->id) << "] " << _sensor->model_name + // << " added. (" << port << ")" << std::endl; file.close(); - } - else + } else std::cout << "Unable to open file : " + path << std::endl; return sensor; } -Dynamixel *Robot::getDynamixel(std::string path, int id, std::string port, float protocol_version) -{ +Dynamixel *Robot::getDynamixel(std::string path, int id, std::string port, + float protocol_version) { Dynamixel *dxl; // load file model_name.device std::ifstream file(path.c_str()); - if (file.is_open()) - { + if (file.is_open()) { std::string session = ""; std::string input_str; @@ -337,8 +328,7 @@ Dynamixel *Robot::getDynamixel(std::string path, int id, std::string port, float std::string velocity_i_gain_item_name = ""; std::string velocity_p_gain_item_name = ""; - while (!file.eof()) - { + while (!file.eof()) { std::getline(file, input_str); // remove comment ( # ) @@ -352,25 +342,23 @@ Dynamixel *Robot::getDynamixel(std::string path, int id, std::string port, float continue; // find session - if (!input_str.compare(0, 1, "[") && !input_str.compare(input_str.size() - 1, 1, "]")) - { + if (!input_str.compare(0, 1, "[") && + !input_str.compare(input_str.size() - 1, 1, "]")) { input_str = input_str.substr(1, input_str.size() - 2); - std::transform(input_str.begin(), input_str.end(), input_str.begin(), ::tolower); + std::transform(input_str.begin(), input_str.end(), input_str.begin(), + ::tolower); session = trim(input_str); continue; } - if (session == SESSION_DEVICE_INFO) - { + if (session == SESSION_DEVICE_INFO) { std::vector tokens = split(input_str, '='); if (tokens.size() != 2) continue; if (tokens[0] == "model_name") dxl = new Dynamixel(id, tokens[1], protocol_version); - } - else if (session == SESSION_TYPE_INFO) - { + } else if (session == SESSION_TYPE_INFO) { std::vector tokens = split(input_str, '='); if (tokens.size() != 2) continue; @@ -417,17 +405,15 @@ Dynamixel *Robot::getDynamixel(std::string path, int id, std::string port, float velocity_i_gain_item_name = tokens[1]; else if (tokens[0] == "velocity_p_gain_item_name") velocity_p_gain_item_name = tokens[1]; - } - else if (session == SESSION_CONTROL_TABLE) - { + } else if (session == SESSION_CONTROL_TABLE) { std::vector tokens = split(input_str, '|'); if (tokens.size() != 8) continue; ControlTableItem *item = new ControlTableItem(); - item->item_name_ = tokens[1]; - item->address_ = std::atoi(tokens[0].c_str()); - item->data_length_ = std::atoi(tokens[2].c_str()); + item->item_name_ = tokens[1]; + item->address_ = std::atoi(tokens[0].c_str()); + item->data_length_ = std::atoi(tokens[2].c_str()); if (tokens[3] == "R") item->access_type_ = Read; @@ -454,9 +440,11 @@ Dynamixel *Robot::getDynamixel(std::string path, int id, std::string port, float if (dxl->ctrl_table_[torque_enable_item_name] != NULL) dxl->torque_enable_item_ = dxl->ctrl_table_[torque_enable_item_name]; if (dxl->ctrl_table_[present_position_item_name] != NULL) - dxl->present_position_item_ = dxl->ctrl_table_[present_position_item_name]; + dxl->present_position_item_ = + dxl->ctrl_table_[present_position_item_name]; if (dxl->ctrl_table_[present_velocity_item_name] != NULL) - dxl->present_velocity_item_ = dxl->ctrl_table_[present_velocity_item_name]; + dxl->present_velocity_item_ = + dxl->ctrl_table_[present_velocity_item_name]; if (dxl->ctrl_table_[present_current_item_name] != NULL) dxl->present_current_item_ = dxl->ctrl_table_[present_current_item_name]; if (dxl->ctrl_table_[goal_position_item_name] != NULL) @@ -478,17 +466,15 @@ Dynamixel *Robot::getDynamixel(std::string path, int id, std::string port, float if (dxl->ctrl_table_[velocity_p_gain_item_name] != NULL) dxl->velocity_p_gain_item_ = dxl->ctrl_table_[velocity_p_gain_item_name]; - fprintf(stderr, "(%s) [ID:%3d] %14s added. \n", port.c_str(), dxl->id_, dxl->model_name_.c_str()); - //std::cout << "[ID:" << (int)(_dxl->id) << "] " << _dxl->model_name << " added. (" << port << ")" << std::endl; + fprintf(stderr, "(%s) [ID:%3d] %14s added. \n", port.c_str(), dxl->id_, + dxl->model_name_.c_str()); + // std::cout << "[ID:" << (int)(_dxl->id) << "] " << _dxl->model_name << " + // added. (" << port << ")" << std::endl; file.close(); - } - else + } else std::cout << "Unable to open file : " + path << std::endl; return dxl; } -int Robot::getControlCycle() -{ - return control_cycle_msec_; -} +int Robot::getControlCycle() { return control_cycle_msec_; } diff --git a/robotis_device/src/robotis_device/sensor.cpp b/robotis_device/src/robotis_device/sensor.cpp index ba7bf29..096de2a 100755 --- a/robotis_device/src/robotis_device/sensor.cpp +++ b/robotis_device/src/robotis_device/sensor.cpp @@ -1,18 +1,18 @@ -/******************************************************************************* -* Copyright 2018 ROBOTIS CO., LTD. -* -* 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. -*******************************************************************************/ +/******************************************************************************* + * Copyright 2018 ROBOTIS CO., LTD. + * + * 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. + *******************************************************************************/ /* * sensor.cpp @@ -25,15 +25,15 @@ using namespace robotis_framework; -Sensor::Sensor(int id, std::string model_name, float protocol_version) -{ - this->id_ = id; - this->model_name_ = model_name; - this->port_name_ = ""; - this->protocol_version_ = protocol_version; - ctrl_table_.clear(); - - sensor_state_ = new SensorState(); - - bulk_read_items_.clear(); +Sensor::Sensor(int id, std::string model_name, float protocol_version) { + + this->id_ = id; + this->model_name_ = model_name; + this->port_name_ = ""; + this->protocol_version_ = protocol_version; + ctrl_table_.clear(); + + sensor_state_ = new SensorState(); + + bulk_read_items_.clear(); } From c10896dc88f2da02232aa4b5a359635b4aa924fb Mon Sep 17 00:00:00 2001 From: RonaldsonBellande Date: Fri, 13 May 2022 11:46:53 -0400 Subject: [PATCH 152/238] travis and rosinstall --- .robotis_framework.rosinstall | 2 +- .travis.yml | 11 ++++------- 2 files changed, 5 insertions(+), 8 deletions(-) diff --git a/.robotis_framework.rosinstall b/.robotis_framework.rosinstall index 9fd2d5e..9e8f90b 100644 --- a/.robotis_framework.rosinstall +++ b/.robotis_framework.rosinstall @@ -1 +1 @@ -- git: {local-name: robotis_controller_msgs, uri: 'https://github.com/ROBOTIS-GIT/ROBOTIS-Framework-msgs.git', version: master} \ No newline at end of file +- git: {local-name: robotis_controller_msgs, uri: 'https://github.com/Robotics-Sensors/ROBOTIS-Framework-msgs.git', version: noetic} diff --git a/.travis.yml b/.travis.yml index 695ee46..9696b6b 100644 --- a/.travis.yml +++ b/.travis.yml @@ -7,7 +7,7 @@ services: - docker language: generic python: - - "2.7" + - "3.8" compiler: - gcc notifications: @@ -15,17 +15,14 @@ notifications: on_success: change on_failure: always recipients: - - pyo@robotis.com + - ronaldsonbellande@gmail.com env: matrix: -# - ROS_DISTRO=kinetic ROS_REPO=ros-shadow-fixed UPSTREAM_WORKSPACE=debian -# - ROS_DISTRO=kinetic ROS_REPO=ros-shadow-fixed UPSTREAM_WORKSPACE=debian OS_NAME=debian OS_CODE_NAME=jessie - - ROS_DISTRO=kinetic ROS_REPO=ros-shadow-fixed UPSTREAM_WORKSPACE=file OS_NAME=ubuntu OS_CODE_NAME=xenial $ROSINSTALL_FILENAME=".robotis_framework.rosinstall" + - ROS_DISTRO=noetic ROS_REPO=ros-shadow-fixed UPSTREAM_WORKSPACE=file OS_NAME=ubuntu OS_CODE_NAME=focal $ROSINSTALL_FILENAME=".robotis_framework.rosinstall" branches: only: - master - - develop - - kinetic-devel + - noetic install: - git clone https://github.com/ros-industrial/industrial_ci.git .ci_config script: From 95c35503afbbd0f8dad266801683f6f8f495fa57 Mon Sep 17 00:00:00 2001 From: RonaldsonBellande Date: Fri, 13 May 2022 11:46:53 -0400 Subject: [PATCH 153/238] travis and rosinstall --- .travis.yml | 11 ++++------- 1 file changed, 4 insertions(+), 7 deletions(-) diff --git a/.travis.yml b/.travis.yml index 695ee46..9696b6b 100644 --- a/.travis.yml +++ b/.travis.yml @@ -7,7 +7,7 @@ services: - docker language: generic python: - - "2.7" + - "3.8" compiler: - gcc notifications: @@ -15,17 +15,14 @@ notifications: on_success: change on_failure: always recipients: - - pyo@robotis.com + - ronaldsonbellande@gmail.com env: matrix: -# - ROS_DISTRO=kinetic ROS_REPO=ros-shadow-fixed UPSTREAM_WORKSPACE=debian -# - ROS_DISTRO=kinetic ROS_REPO=ros-shadow-fixed UPSTREAM_WORKSPACE=debian OS_NAME=debian OS_CODE_NAME=jessie - - ROS_DISTRO=kinetic ROS_REPO=ros-shadow-fixed UPSTREAM_WORKSPACE=file OS_NAME=ubuntu OS_CODE_NAME=xenial $ROSINSTALL_FILENAME=".robotis_framework.rosinstall" + - ROS_DISTRO=noetic ROS_REPO=ros-shadow-fixed UPSTREAM_WORKSPACE=file OS_NAME=ubuntu OS_CODE_NAME=focal $ROSINSTALL_FILENAME=".robotis_framework.rosinstall" branches: only: - master - - develop - - kinetic-devel + - noetic install: - git clone https://github.com/ros-industrial/industrial_ci.git .ci_config script: From 2f013d372954380b2745f08dd5f9dd7561127450 Mon Sep 17 00:00:00 2001 From: Ronaldson Bellande <47253433+RonaldsonBellande@users.noreply.github.com> Date: Sun, 15 May 2022 16:41:00 -0400 Subject: [PATCH 154/238] Create cmake.yml --- .github/workflows/cmake.yml | 37 +++++++++++++++++++++++++++++++++++++ 1 file changed, 37 insertions(+) create mode 100644 .github/workflows/cmake.yml diff --git a/.github/workflows/cmake.yml b/.github/workflows/cmake.yml new file mode 100644 index 0000000..97f7fe7 --- /dev/null +++ b/.github/workflows/cmake.yml @@ -0,0 +1,37 @@ +name: CMake + +on: + push: + branches: [ noetic ] + pull_request: + branches: [ noetic ] + +env: + # Customize the CMake build type here (Release, Debug, RelWithDebInfo, etc.) + BUILD_TYPE: Release + +jobs: + build: + # The CMake configure and build commands are platform agnostic and should work equally well on Windows or Mac. + # You can convert this to a matrix build if you need cross-platform coverage. + # See: https://docs.github.com/en/free-pro-team@latest/actions/learn-github-actions/managing-complex-workflows#using-a-build-matrix + runs-on: ubuntu-latest + + steps: + - uses: actions/checkout@v3 + + - name: Configure CMake + # Configure CMake in a 'build' subdirectory. `CMAKE_BUILD_TYPE` is only required if you are using a single-configuration generator such as make. + # See https://cmake.org/cmake/help/latest/variable/CMAKE_BUILD_TYPE.html?highlight=cmake_build_type + run: cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} + + - name: Build + # Build your program with the given configuration + run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}} + + - name: Test + working-directory: ${{github.workspace}}/build + # Execute tests defined by the CMake configuration. + # See https://cmake.org/cmake/help/latest/manual/ctest.1.html for more detail + run: ctest -C ${{env.BUILD_TYPE}} + From bbf8ea871e0d7c704bf98801d16d7ff023b5888d Mon Sep 17 00:00:00 2001 From: Ronaldson Bellande <47253433+RonaldsonBellande@users.noreply.github.com> Date: Sun, 15 May 2022 16:41:00 -0400 Subject: [PATCH 155/238] Create cmake.yml --- .github/workflows/cmake.yml | 37 +++++++++++++++++++++++++++++++++++++ 1 file changed, 37 insertions(+) create mode 100644 .github/workflows/cmake.yml diff --git a/.github/workflows/cmake.yml b/.github/workflows/cmake.yml new file mode 100644 index 0000000..97f7fe7 --- /dev/null +++ b/.github/workflows/cmake.yml @@ -0,0 +1,37 @@ +name: CMake + +on: + push: + branches: [ noetic ] + pull_request: + branches: [ noetic ] + +env: + # Customize the CMake build type here (Release, Debug, RelWithDebInfo, etc.) + BUILD_TYPE: Release + +jobs: + build: + # The CMake configure and build commands are platform agnostic and should work equally well on Windows or Mac. + # You can convert this to a matrix build if you need cross-platform coverage. + # See: https://docs.github.com/en/free-pro-team@latest/actions/learn-github-actions/managing-complex-workflows#using-a-build-matrix + runs-on: ubuntu-latest + + steps: + - uses: actions/checkout@v3 + + - name: Configure CMake + # Configure CMake in a 'build' subdirectory. `CMAKE_BUILD_TYPE` is only required if you are using a single-configuration generator such as make. + # See https://cmake.org/cmake/help/latest/variable/CMAKE_BUILD_TYPE.html?highlight=cmake_build_type + run: cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} + + - name: Build + # Build your program with the given configuration + run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}} + + - name: Test + working-directory: ${{github.workspace}}/build + # Execute tests defined by the CMake configuration. + # See https://cmake.org/cmake/help/latest/manual/ctest.1.html for more detail + run: ctest -C ${{env.BUILD_TYPE}} + From 68f6535adb2b5272a4b9d24318a1abbe7acca734 Mon Sep 17 00:00:00 2001 From: Ronaldson Bellande <47253433+RonaldsonBellande@users.noreply.github.com> Date: Sun, 15 May 2022 16:41:34 -0400 Subject: [PATCH 156/238] Create docker-publish.yml --- .github/workflows/docker-publish.yml | 93 ++++++++++++++++++++++++++++ 1 file changed, 93 insertions(+) create mode 100644 .github/workflows/docker-publish.yml diff --git a/.github/workflows/docker-publish.yml b/.github/workflows/docker-publish.yml new file mode 100644 index 0000000..b33bae0 --- /dev/null +++ b/.github/workflows/docker-publish.yml @@ -0,0 +1,93 @@ +name: Docker + +# This workflow uses actions that are not certified by GitHub. +# They are provided by a third-party and are governed by +# separate terms of service, privacy policy, and support +# documentation. + +on: + schedule: + - cron: '15 20 * * *' + push: + branches: [ noetic ] + # Publish semver tags as releases. + tags: [ 'v*.*.*' ] + pull_request: + branches: [ noetic ] + +env: + # Use docker.io for Docker Hub if empty + REGISTRY: ghcr.io + # github.repository as / + IMAGE_NAME: ${{ github.repository }} + + +jobs: + build: + + runs-on: ubuntu-latest + permissions: + contents: read + packages: write + # This is used to complete the identity challenge + # with sigstore/fulcio when running outside of PRs. + id-token: write + + steps: + - name: Checkout repository + uses: actions/checkout@v3 + + # Install the cosign tool except on PR + # https://github.com/sigstore/cosign-installer + - name: Install cosign + if: github.event_name != 'pull_request' + uses: sigstore/cosign-installer@d6a3abf1bdea83574e28d40543793018b6035605 + with: + cosign-release: 'v1.7.1' + + + # Workaround: https://github.com/docker/build-push-action/issues/461 + - name: Setup Docker buildx + uses: docker/setup-buildx-action@79abd3f86f79a9d68a23c75a09a9a85889262adf + + # Login against a Docker registry except on PR + # https://github.com/docker/login-action + - name: Log into registry ${{ env.REGISTRY }} + if: github.event_name != 'pull_request' + uses: docker/login-action@28218f9b04b4f3f62068d7b6ce6ca5b26e35336c + with: + registry: ${{ env.REGISTRY }} + username: ${{ github.actor }} + password: ${{ secrets.GITHUB_TOKEN }} + + # Extract metadata (tags, labels) for Docker + # https://github.com/docker/metadata-action + - name: Extract Docker metadata + id: meta + uses: docker/metadata-action@98669ae865ea3cffbcbaa878cf57c20bbf1c6c38 + with: + images: ${{ env.REGISTRY }}/${{ env.IMAGE_NAME }} + + # Build and push Docker image with Buildx (don't push on PR) + # https://github.com/docker/build-push-action + - name: Build and push Docker image + id: build-and-push + uses: docker/build-push-action@ac9327eae2b366085ac7f6a2d02df8aa8ead720a + with: + context: . + push: ${{ github.event_name != 'pull_request' }} + tags: ${{ steps.meta.outputs.tags }} + labels: ${{ steps.meta.outputs.labels }} + + # Sign the resulting Docker image digest except on PRs. + # This will only write to the public Rekor transparency log when the Docker + # repository is public to avoid leaking data. If you would like to publish + # transparency data even for private images, pass --force to cosign below. + # https://github.com/sigstore/cosign + - name: Sign the published Docker image + if: ${{ github.event_name != 'pull_request' }} + env: + COSIGN_EXPERIMENTAL: "true" + # This step uses the identity token to provision an ephemeral certificate + # against the sigstore community Fulcio instance. + run: cosign sign ${{ steps.meta.outputs.tags }}@${{ steps.build-and-push.outputs.digest }} From b2ca15d8fe31f91c6d0493894600bc4ad1302a3a Mon Sep 17 00:00:00 2001 From: Ronaldson Bellande <47253433+RonaldsonBellande@users.noreply.github.com> Date: Sun, 15 May 2022 16:41:34 -0400 Subject: [PATCH 157/238] Create docker-publish.yml --- .github/workflows/docker-publish.yml | 93 ++++++++++++++++++++++++++++ 1 file changed, 93 insertions(+) create mode 100644 .github/workflows/docker-publish.yml diff --git a/.github/workflows/docker-publish.yml b/.github/workflows/docker-publish.yml new file mode 100644 index 0000000..b33bae0 --- /dev/null +++ b/.github/workflows/docker-publish.yml @@ -0,0 +1,93 @@ +name: Docker + +# This workflow uses actions that are not certified by GitHub. +# They are provided by a third-party and are governed by +# separate terms of service, privacy policy, and support +# documentation. + +on: + schedule: + - cron: '15 20 * * *' + push: + branches: [ noetic ] + # Publish semver tags as releases. + tags: [ 'v*.*.*' ] + pull_request: + branches: [ noetic ] + +env: + # Use docker.io for Docker Hub if empty + REGISTRY: ghcr.io + # github.repository as / + IMAGE_NAME: ${{ github.repository }} + + +jobs: + build: + + runs-on: ubuntu-latest + permissions: + contents: read + packages: write + # This is used to complete the identity challenge + # with sigstore/fulcio when running outside of PRs. + id-token: write + + steps: + - name: Checkout repository + uses: actions/checkout@v3 + + # Install the cosign tool except on PR + # https://github.com/sigstore/cosign-installer + - name: Install cosign + if: github.event_name != 'pull_request' + uses: sigstore/cosign-installer@d6a3abf1bdea83574e28d40543793018b6035605 + with: + cosign-release: 'v1.7.1' + + + # Workaround: https://github.com/docker/build-push-action/issues/461 + - name: Setup Docker buildx + uses: docker/setup-buildx-action@79abd3f86f79a9d68a23c75a09a9a85889262adf + + # Login against a Docker registry except on PR + # https://github.com/docker/login-action + - name: Log into registry ${{ env.REGISTRY }} + if: github.event_name != 'pull_request' + uses: docker/login-action@28218f9b04b4f3f62068d7b6ce6ca5b26e35336c + with: + registry: ${{ env.REGISTRY }} + username: ${{ github.actor }} + password: ${{ secrets.GITHUB_TOKEN }} + + # Extract metadata (tags, labels) for Docker + # https://github.com/docker/metadata-action + - name: Extract Docker metadata + id: meta + uses: docker/metadata-action@98669ae865ea3cffbcbaa878cf57c20bbf1c6c38 + with: + images: ${{ env.REGISTRY }}/${{ env.IMAGE_NAME }} + + # Build and push Docker image with Buildx (don't push on PR) + # https://github.com/docker/build-push-action + - name: Build and push Docker image + id: build-and-push + uses: docker/build-push-action@ac9327eae2b366085ac7f6a2d02df8aa8ead720a + with: + context: . + push: ${{ github.event_name != 'pull_request' }} + tags: ${{ steps.meta.outputs.tags }} + labels: ${{ steps.meta.outputs.labels }} + + # Sign the resulting Docker image digest except on PRs. + # This will only write to the public Rekor transparency log when the Docker + # repository is public to avoid leaking data. If you would like to publish + # transparency data even for private images, pass --force to cosign below. + # https://github.com/sigstore/cosign + - name: Sign the published Docker image + if: ${{ github.event_name != 'pull_request' }} + env: + COSIGN_EXPERIMENTAL: "true" + # This step uses the identity token to provision an ephemeral certificate + # against the sigstore community Fulcio instance. + run: cosign sign ${{ steps.meta.outputs.tags }}@${{ steps.build-and-push.outputs.digest }} From 8e73cb0abf54983dc6fd1d6f46f73c352f194c3c Mon Sep 17 00:00:00 2001 From: Ronaldson Bellande <47253433+RonaldsonBellande@users.noreply.github.com> Date: Tue, 14 Jun 2022 03:34:16 -0400 Subject: [PATCH 158/238] Create docker-image.yml --- .github/workflows/docker-image.yml | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) create mode 100644 .github/workflows/docker-image.yml diff --git a/.github/workflows/docker-image.yml b/.github/workflows/docker-image.yml new file mode 100644 index 0000000..acdd5fa --- /dev/null +++ b/.github/workflows/docker-image.yml @@ -0,0 +1,18 @@ +name: Docker Image CI + +on: + push: + branches: [ "noetic" ] + pull_request: + branches: [ "noetic" ] + +jobs: + + build: + + runs-on: ubuntu-latest + + steps: + - uses: actions/checkout@v3 + - name: Build the Docker image + run: docker build . --file Dockerfile --tag my-image-name:$(date +%s) From 61bc7ceb3b17489ed981a55d290cf9c8c8df15b9 Mon Sep 17 00:00:00 2001 From: Ronaldson Bellande <47253433+RonaldsonBellande@users.noreply.github.com> Date: Tue, 14 Jun 2022 03:34:16 -0400 Subject: [PATCH 159/238] Create docker-image.yml --- .github/workflows/docker-image.yml | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) create mode 100644 .github/workflows/docker-image.yml diff --git a/.github/workflows/docker-image.yml b/.github/workflows/docker-image.yml new file mode 100644 index 0000000..acdd5fa --- /dev/null +++ b/.github/workflows/docker-image.yml @@ -0,0 +1,18 @@ +name: Docker Image CI + +on: + push: + branches: [ "noetic" ] + pull_request: + branches: [ "noetic" ] + +jobs: + + build: + + runs-on: ubuntu-latest + + steps: + - uses: actions/checkout@v3 + - name: Build the Docker image + run: docker build . --file Dockerfile --tag my-image-name:$(date +%s) From a4af03ef5e63b5361f3053703c54e6e10b226076 Mon Sep 17 00:00:00 2001 From: RonaldsonBellande Date: Tue, 2 Aug 2022 23:48:15 -0400 Subject: [PATCH 160/238] docker --- Dockerfile | 73 ++++++++++++++++++++++++++ catkin_setup.sh | 5 ++ gpu.Dockerfile | 90 +++++++++++++++++++++++++++++++++ requirements.txt | 39 ++++++++++++++ ros_repository_requirements.txt | 9 ++++ ros_requirements.txt | 15 ++++++ system_requirements.txt | 29 +++++++++++ 7 files changed, 260 insertions(+) create mode 100644 Dockerfile create mode 100644 catkin_setup.sh create mode 100644 gpu.Dockerfile create mode 100644 requirements.txt create mode 100644 ros_repository_requirements.txt create mode 100644 ros_requirements.txt create mode 100644 system_requirements.txt diff --git a/Dockerfile b/Dockerfile new file mode 100644 index 0000000..ccda94f --- /dev/null +++ b/Dockerfile @@ -0,0 +1,73 @@ +ARG ROS_ARCHITECTURE_VERSION=latest + +FROM ubuntu:20.04 as base_build +SHELL [ "/bin/bash" , "-c" ] + +ENV DEBIAN_FRONTEND noninteractive +ENV PYTHON_VERSION="3.8" + +ARG ROS_ARCHITECTURE_VERSION_GIT_BRANCH=master +ARG ROS_ARCHITECTURE_VERSION_GIT_COMMIT=HEAD + +LABEL maintainer=ronaldsonbellande@gmail.com +LABEL ROS_architecture_github_branchtag=${ROS_ARCHITECTURE_VERSION_GIT_BRANCH} +LABEL ROS_architecture_github_commit=${ROS_ARCHITECTURE_VERSION_GIT_COMMIT} + +# Ubuntu setup +RUN apt-get update -y +RUN apt-get upgrade -y + +# RUN workspace and sourcing +WORKDIR ./ +COPY requirements.txt . +COPY system_requirements.txt . +COPY ros_requirements.txt . +COPY ros_repository_requirements.txt . + +# Install dependencies for system +RUN apt-get update && apt-get install -y --no-install-recommends > ~/.bashrc +RUN source ~/.bashrc + +RUN cd $CATKIN_WS \ + && rosdep init \ + && rosdep update \ + && rosdep update --rosdistro noetic \ + && rosdep fix-permissions \ + && rosdep install -y --from-paths . --ignore-src --rosdistro noetic + +# Always source catkin_setup.sh when launching bash +RUN echo "source /usr/local/bin/catkin_setup.sh" >> /root/.bashrc +COPY catkin_setup.sh /usr/local/bin/catkin_setup.sh +RUN chmod +x /usr/local/bin/catkin_setup.sh + +ENTRYPOINT ["/usr/local/bin/catkin_setup.sh"] +CMD ["bash"] diff --git a/catkin_setup.sh b/catkin_setup.sh new file mode 100644 index 0000000..6ec332f --- /dev/null +++ b/catkin_setup.sh @@ -0,0 +1,5 @@ +#!/bin/bash + +catkin build +source "/opt/ros/noetic/setup.bash" +source "$CATKIN_WS/devel/setup.bash" diff --git a/gpu.Dockerfile b/gpu.Dockerfile new file mode 100644 index 0000000..df15614 --- /dev/null +++ b/gpu.Dockerfile @@ -0,0 +1,90 @@ +ARG ROS_ARCHITECTURE_VERSION=latest + +FROM ubuntu:20.04 as base_build +FROM nvidia/cuda:11.2.1-base-ubuntu20.04 + +ENV DEBIAN_FRONTEND noninteractive +ENV PYTHON_VERSION="3.8" +ENV CUDNN_VERSION=8.1.0.77 +ENV TF_TENSORRT_VERSION=7.2.2 +ENV CUDA=11.2 +ENV LD_LIBRARY_PATH /usr/local/cuda/extras/CUPTI/lib64:$LD_LIBRARY_PATH + +ARG ROS_ARCHITECTURE_VERSION_GIT_BRANCH=master +ARG ROS_ARCHITECTURE_VERSION_GIT_COMMIT=HEAD + +LABEL maintainer=ronaldsonbellande@gmail.com +LABEL ROS_architecture_github_branchtag=${ROS_ARCHITECTURE_VERSION_GIT_BRANCH} +LABEL ROS_architecture_github_commit=${ROS_ARCHITECTURE_VERSION_GIT_COMMIT} + +# Ubuntu setup +RUN apt-get update -y +RUN apt-get upgrade -y + +# RUN workspace and sourcing +WORKDIR ./ +COPY requirements.txt . +COPY system_requirements.txt . +COPY ros_requirements.txt . +COPY ros_repository_requirements.txt . + +# Install dependencies for system +RUN apt-get update && apt-get install -y --no-install-recommends > ~/.bashrc +RUN source ~/.bashrc + +RUN cd $CATKIN_WS \ + && rosdep init \ + && rosdep update \ + && rosdep update --rosdistro noetic \ + && rosdep fix-permissions \ + && rosdep install -y --from-paths . --ignore-src --rosdistro noetic + +# Always source catkin_setup.sh when launching bash +RUN echo "source /usr/local/bin/catkin_setup.sh" >> /root/.bashrc +COPY catkin_setup.sh /usr/local/bin/catkin_setup.sh +RUN chmod +x /usr/local/bin/catkin_setup.sh + +ENTRYPOINT ["/usr/local/bin/catkin_setup.sh"] +CMD ["bash"] + +RUN apt-key adv --fetch-keys https://developer.download.nvidia.com/compute/cuda/repos/ubuntu2004/x86_64/3bf863cc.pub && \ + apt-get update && apt-get install -y --no-install-recommends \ + cuda-nvrtc-${CUDA/./-} \ + libcudnn8=${CUDNN_VERSION}-1+cuda${CUDA} \ + -r cuda_requirements.txt + +# We don't install libnvinfer-dev since we don't need to build against TensorRT +RUN apt-key adv --fetch-keys https://developer.download.nvidia.com/compute/machine-learning/repos/ubuntu2004/x86_64/7fa2af80.pub && \ + echo "deb https://developer.download.nvidia.com/compute/machine-learning/repos/ubuntu2004/x86_64 /" > /etc/apt/sources.list.d/tensorRT.list && \ + apt-get update && \ + apt-get install -y --no-install-recommends libnvinfer7=${TF_TENSORRT_VERSION}-1+cuda11.0 \ + libnvinfer-plugin7=${TF_TENSORRT_VERSION}-1+cuda11.0 \ + && apt-get clean && \ + rm -rf /var/lib/apt/lists/*; + diff --git a/requirements.txt b/requirements.txt new file mode 100644 index 0000000..d08c640 --- /dev/null +++ b/requirements.txt @@ -0,0 +1,39 @@ +# Recomandation for library to install in python + +setuptools +pandas +scipy +sklearn +future +grpcio +h5py +requests +opencv-python +python-math +random2 +pytest-warnings +os.path2 +pydicom +glob2 +pytest-shutil +DateTime +zipfile36 +urllib3 +python-time +trimesh +librosa +gym +matplotlib +image-slicer +nvidia-ml-py3 +imgaug +tqdm +rosdep + +protobuf==3.15.2 +numpy==1.19.2 +numba==0.55.2 +imageio==2.9.0 +pillow==7.2.0 +tensorflow==2.7.0 +keras==2.7.0 diff --git a/ros_repository_requirements.txt b/ros_repository_requirements.txt new file mode 100644 index 0000000..09c4578 --- /dev/null +++ b/ros_repository_requirements.txt @@ -0,0 +1,9 @@ +git clone -b https://github.com/Robotics-Sensors/ROBOTIS-OP3-msgs +git clone -b https://github.com/Robotics-Sensors/ROBOTIS-OP3 +git clone -b https://github.com/Robotics-Sensors/ROBOTIS-OP3-Tools +git clone -b https://github.com/Robotics-Sensors/ROBOTIS-Framework-msgs +git clone -b https://github.com/Robotics-Sensors/ROBOTIS-Framework +git clone -b https://github.com/Robotics-Sensors/ROBOTIS-OP3-Common +git clone -b https://github.com/Robotics-Sensors/ROBOTIS-Utility +git clone -b https://github.com/Robotics-Sensors/ROBOTIS-OP3-Demo +git clone -b https://github.com/Robotics-Sensors/ROBOTIS-Math diff --git a/ros_requirements.txt b/ros_requirements.txt new file mode 100644 index 0000000..7b80df2 --- /dev/null +++ b/ros_requirements.txt @@ -0,0 +1,15 @@ +# Recomandation for dependencies for ros system + +ros-noetic-desktop-full +build-essential +ros-noetic-catkin +python-rosdep +python-rosinstall +python-rosinstall-generator +python-wstool +python-catkin-tools +ros-noetic-pcl-ros +ros-noetic-flexbe-behavior-engine +ros-noetic-moveit +ros-noetic-gazebo-ros-pkgs +ros-noetic-gazebo-ros-control diff --git a/system_requirements.txt b/system_requirements.txt new file mode 100644 index 0000000..46eea93 --- /dev/null +++ b/system_requirements.txt @@ -0,0 +1,29 @@ +# Recomandation for dependencies for linux system + +software-properties-common +python3.8 +neovim +apt-utils +automake +build-essential +ca-certificates +pycurl +git +python3-pip +libcurl3-dev +libfreetype6-dev +libpng-dev +libtool +libzmq3-dev +mlocate +openjdk-8-jdk +openjdk-8-jre-headless +pkg-config +python-dev +software-properties-common +swig +unzip +wget +zip +zlib1g-dev +python3-distutils From 3576dcd5c46297a9199334303fa531d01a69441d Mon Sep 17 00:00:00 2001 From: Ronaldson Bellande <47253433+RonaldsonBellande@users.noreply.github.com> Date: Thu, 4 Aug 2022 05:38:18 -0400 Subject: [PATCH 161/238] Update cmake.yml --- .github/workflows/cmake.yml | 71 ++++++++++++++++++++++++++----------- 1 file changed, 50 insertions(+), 21 deletions(-) diff --git a/.github/workflows/cmake.yml b/.github/workflows/cmake.yml index 97f7fe7..31edac6 100644 --- a/.github/workflows/cmake.yml +++ b/.github/workflows/cmake.yml @@ -2,36 +2,65 @@ name: CMake on: push: - branches: [ noetic ] + branches: [ master ] pull_request: - branches: [ noetic ] + branches: [ master ] env: - # Customize the CMake build type here (Release, Debug, RelWithDebInfo, etc.) BUILD_TYPE: Release jobs: build: - # The CMake configure and build commands are platform agnostic and should work equally well on Windows or Mac. - # You can convert this to a matrix build if you need cross-platform coverage. - # See: https://docs.github.com/en/free-pro-team@latest/actions/learn-github-actions/managing-complex-workflows#using-a-build-matrix runs-on: ubuntu-latest - steps: - uses: actions/checkout@v3 - - name: Configure CMake - # Configure CMake in a 'build' subdirectory. `CMAKE_BUILD_TYPE` is only required if you are using a single-configuration generator such as make. - # See https://cmake.org/cmake/help/latest/variable/CMAKE_BUILD_TYPE.html?highlight=cmake_build_type - run: cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} - - - name: Build - # Build your program with the given configuration - run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}} - - - name: Test - working-directory: ${{github.workspace}}/build - # Execute tests defined by the CMake configuration. - # See https://cmake.org/cmake/help/latest/manual/ctest.1.html for more detail - run: ctest -C ${{env.BUILD_TYPE}} + - name: Configure CMake 1 + run: cmake -E make_directory ${{runner.workspace}}/robotis_controller/build + - name: Configure CMake 2 + run: cmake -E make_directory ${{runner.workspace}}/robotis_device/build + + - name: Configure CMake 3 + run: cmake -E make_directory ${{runner.workspace}}/robotis_framework/build + + - name: Configure CMake 4 + run: cmake -E make_directory ${{runner.workspace}}/robotis_framework_common/build + + + + - name: Install dependencies + shell: bash + run: sudo apt-get update ; sudo apt-get install Date: Thu, 4 Aug 2022 05:38:18 -0400 Subject: [PATCH 162/238] Update cmake.yml --- .github/workflows/cmake.yml | 71 ++++++++++++++++++++++++++----------- 1 file changed, 50 insertions(+), 21 deletions(-) diff --git a/.github/workflows/cmake.yml b/.github/workflows/cmake.yml index 97f7fe7..31edac6 100644 --- a/.github/workflows/cmake.yml +++ b/.github/workflows/cmake.yml @@ -2,36 +2,65 @@ name: CMake on: push: - branches: [ noetic ] + branches: [ master ] pull_request: - branches: [ noetic ] + branches: [ master ] env: - # Customize the CMake build type here (Release, Debug, RelWithDebInfo, etc.) BUILD_TYPE: Release jobs: build: - # The CMake configure and build commands are platform agnostic and should work equally well on Windows or Mac. - # You can convert this to a matrix build if you need cross-platform coverage. - # See: https://docs.github.com/en/free-pro-team@latest/actions/learn-github-actions/managing-complex-workflows#using-a-build-matrix runs-on: ubuntu-latest - steps: - uses: actions/checkout@v3 - - name: Configure CMake - # Configure CMake in a 'build' subdirectory. `CMAKE_BUILD_TYPE` is only required if you are using a single-configuration generator such as make. - # See https://cmake.org/cmake/help/latest/variable/CMAKE_BUILD_TYPE.html?highlight=cmake_build_type - run: cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} - - - name: Build - # Build your program with the given configuration - run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}} - - - name: Test - working-directory: ${{github.workspace}}/build - # Execute tests defined by the CMake configuration. - # See https://cmake.org/cmake/help/latest/manual/ctest.1.html for more detail - run: ctest -C ${{env.BUILD_TYPE}} + - name: Configure CMake 1 + run: cmake -E make_directory ${{runner.workspace}}/robotis_controller/build + - name: Configure CMake 2 + run: cmake -E make_directory ${{runner.workspace}}/robotis_device/build + + - name: Configure CMake 3 + run: cmake -E make_directory ${{runner.workspace}}/robotis_framework/build + + - name: Configure CMake 4 + run: cmake -E make_directory ${{runner.workspace}}/robotis_framework_common/build + + + + - name: Install dependencies + shell: bash + run: sudo apt-get update ; sudo apt-get install Date: Wed, 27 Sep 2023 09:45:50 -0400 Subject: [PATCH 163/238] latest pushes --- .github/workflows/cmake.yml | 67 ++++++------------- Dockerfile => docker/Dockerfile | 0 gpu.Dockerfile => docker/gpu.Dockerfile | 0 push.sh | 4 ++ .../requirements.txt | 0 .../ros_repository_requirements.txt | 0 .../ros_requirements.txt | 0 .../system_requirements.txt | 0 8 files changed, 23 insertions(+), 48 deletions(-) rename Dockerfile => docker/Dockerfile (100%) rename gpu.Dockerfile => docker/gpu.Dockerfile (100%) create mode 100755 push.sh rename requirements.txt => requirements/requirements.txt (100%) rename ros_repository_requirements.txt => requirements/ros_repository_requirements.txt (100%) rename ros_requirements.txt => requirements/ros_requirements.txt (100%) rename system_requirements.txt => requirements/system_requirements.txt (100%) diff --git a/.github/workflows/cmake.yml b/.github/workflows/cmake.yml index 31edac6..97f7fe7 100644 --- a/.github/workflows/cmake.yml +++ b/.github/workflows/cmake.yml @@ -2,65 +2,36 @@ name: CMake on: push: - branches: [ master ] + branches: [ noetic ] pull_request: - branches: [ master ] + branches: [ noetic ] env: + # Customize the CMake build type here (Release, Debug, RelWithDebInfo, etc.) BUILD_TYPE: Release jobs: build: + # The CMake configure and build commands are platform agnostic and should work equally well on Windows or Mac. + # You can convert this to a matrix build if you need cross-platform coverage. + # See: https://docs.github.com/en/free-pro-team@latest/actions/learn-github-actions/managing-complex-workflows#using-a-build-matrix runs-on: ubuntu-latest + steps: - uses: actions/checkout@v3 - - name: Configure CMake 1 - run: cmake -E make_directory ${{runner.workspace}}/robotis_controller/build - - - name: Configure CMake 2 - run: cmake -E make_directory ${{runner.workspace}}/robotis_device/build - - - name: Configure CMake 3 - run: cmake -E make_directory ${{runner.workspace}}/robotis_framework/build - - - name: Configure CMake 4 - run: cmake -E make_directory ${{runner.workspace}}/robotis_framework_common/build - - - - - name: Install dependencies - shell: bash - run: sudo apt-get update ; sudo apt-get install Date: Wed, 27 Sep 2023 09:45:50 -0400 Subject: [PATCH 164/238] latest pushes --- .github/workflows/cmake.yml | 67 +++++---------- docker/Dockerfile | 73 ++++++++++++++++ docker/gpu.Dockerfile | 90 ++++++++++++++++++++ requirements/requirements.txt | 39 +++++++++ requirements/ros_repository_requirements.txt | 9 ++ requirements/ros_requirements.txt | 15 ++++ requirements/system_requirements.txt | 29 +++++++ 7 files changed, 274 insertions(+), 48 deletions(-) create mode 100644 docker/Dockerfile create mode 100644 docker/gpu.Dockerfile create mode 100644 requirements/requirements.txt create mode 100644 requirements/ros_repository_requirements.txt create mode 100644 requirements/ros_requirements.txt create mode 100644 requirements/system_requirements.txt diff --git a/.github/workflows/cmake.yml b/.github/workflows/cmake.yml index 31edac6..97f7fe7 100644 --- a/.github/workflows/cmake.yml +++ b/.github/workflows/cmake.yml @@ -2,65 +2,36 @@ name: CMake on: push: - branches: [ master ] + branches: [ noetic ] pull_request: - branches: [ master ] + branches: [ noetic ] env: + # Customize the CMake build type here (Release, Debug, RelWithDebInfo, etc.) BUILD_TYPE: Release jobs: build: + # The CMake configure and build commands are platform agnostic and should work equally well on Windows or Mac. + # You can convert this to a matrix build if you need cross-platform coverage. + # See: https://docs.github.com/en/free-pro-team@latest/actions/learn-github-actions/managing-complex-workflows#using-a-build-matrix runs-on: ubuntu-latest + steps: - uses: actions/checkout@v3 - - name: Configure CMake 1 - run: cmake -E make_directory ${{runner.workspace}}/robotis_controller/build - - - name: Configure CMake 2 - run: cmake -E make_directory ${{runner.workspace}}/robotis_device/build - - - name: Configure CMake 3 - run: cmake -E make_directory ${{runner.workspace}}/robotis_framework/build - - - name: Configure CMake 4 - run: cmake -E make_directory ${{runner.workspace}}/robotis_framework_common/build - - - - - name: Install dependencies - shell: bash - run: sudo apt-get update ; sudo apt-get install > ~/.bashrc +RUN source ~/.bashrc + +RUN cd $CATKIN_WS \ + && rosdep init \ + && rosdep update \ + && rosdep update --rosdistro noetic \ + && rosdep fix-permissions \ + && rosdep install -y --from-paths . --ignore-src --rosdistro noetic + +# Always source catkin_setup.sh when launching bash +RUN echo "source /usr/local/bin/catkin_setup.sh" >> /root/.bashrc +COPY catkin_setup.sh /usr/local/bin/catkin_setup.sh +RUN chmod +x /usr/local/bin/catkin_setup.sh + +ENTRYPOINT ["/usr/local/bin/catkin_setup.sh"] +CMD ["bash"] diff --git a/docker/gpu.Dockerfile b/docker/gpu.Dockerfile new file mode 100644 index 0000000..df15614 --- /dev/null +++ b/docker/gpu.Dockerfile @@ -0,0 +1,90 @@ +ARG ROS_ARCHITECTURE_VERSION=latest + +FROM ubuntu:20.04 as base_build +FROM nvidia/cuda:11.2.1-base-ubuntu20.04 + +ENV DEBIAN_FRONTEND noninteractive +ENV PYTHON_VERSION="3.8" +ENV CUDNN_VERSION=8.1.0.77 +ENV TF_TENSORRT_VERSION=7.2.2 +ENV CUDA=11.2 +ENV LD_LIBRARY_PATH /usr/local/cuda/extras/CUPTI/lib64:$LD_LIBRARY_PATH + +ARG ROS_ARCHITECTURE_VERSION_GIT_BRANCH=master +ARG ROS_ARCHITECTURE_VERSION_GIT_COMMIT=HEAD + +LABEL maintainer=ronaldsonbellande@gmail.com +LABEL ROS_architecture_github_branchtag=${ROS_ARCHITECTURE_VERSION_GIT_BRANCH} +LABEL ROS_architecture_github_commit=${ROS_ARCHITECTURE_VERSION_GIT_COMMIT} + +# Ubuntu setup +RUN apt-get update -y +RUN apt-get upgrade -y + +# RUN workspace and sourcing +WORKDIR ./ +COPY requirements.txt . +COPY system_requirements.txt . +COPY ros_requirements.txt . +COPY ros_repository_requirements.txt . + +# Install dependencies for system +RUN apt-get update && apt-get install -y --no-install-recommends > ~/.bashrc +RUN source ~/.bashrc + +RUN cd $CATKIN_WS \ + && rosdep init \ + && rosdep update \ + && rosdep update --rosdistro noetic \ + && rosdep fix-permissions \ + && rosdep install -y --from-paths . --ignore-src --rosdistro noetic + +# Always source catkin_setup.sh when launching bash +RUN echo "source /usr/local/bin/catkin_setup.sh" >> /root/.bashrc +COPY catkin_setup.sh /usr/local/bin/catkin_setup.sh +RUN chmod +x /usr/local/bin/catkin_setup.sh + +ENTRYPOINT ["/usr/local/bin/catkin_setup.sh"] +CMD ["bash"] + +RUN apt-key adv --fetch-keys https://developer.download.nvidia.com/compute/cuda/repos/ubuntu2004/x86_64/3bf863cc.pub && \ + apt-get update && apt-get install -y --no-install-recommends \ + cuda-nvrtc-${CUDA/./-} \ + libcudnn8=${CUDNN_VERSION}-1+cuda${CUDA} \ + -r cuda_requirements.txt + +# We don't install libnvinfer-dev since we don't need to build against TensorRT +RUN apt-key adv --fetch-keys https://developer.download.nvidia.com/compute/machine-learning/repos/ubuntu2004/x86_64/7fa2af80.pub && \ + echo "deb https://developer.download.nvidia.com/compute/machine-learning/repos/ubuntu2004/x86_64 /" > /etc/apt/sources.list.d/tensorRT.list && \ + apt-get update && \ + apt-get install -y --no-install-recommends libnvinfer7=${TF_TENSORRT_VERSION}-1+cuda11.0 \ + libnvinfer-plugin7=${TF_TENSORRT_VERSION}-1+cuda11.0 \ + && apt-get clean && \ + rm -rf /var/lib/apt/lists/*; + diff --git a/requirements/requirements.txt b/requirements/requirements.txt new file mode 100644 index 0000000..d08c640 --- /dev/null +++ b/requirements/requirements.txt @@ -0,0 +1,39 @@ +# Recomandation for library to install in python + +setuptools +pandas +scipy +sklearn +future +grpcio +h5py +requests +opencv-python +python-math +random2 +pytest-warnings +os.path2 +pydicom +glob2 +pytest-shutil +DateTime +zipfile36 +urllib3 +python-time +trimesh +librosa +gym +matplotlib +image-slicer +nvidia-ml-py3 +imgaug +tqdm +rosdep + +protobuf==3.15.2 +numpy==1.19.2 +numba==0.55.2 +imageio==2.9.0 +pillow==7.2.0 +tensorflow==2.7.0 +keras==2.7.0 diff --git a/requirements/ros_repository_requirements.txt b/requirements/ros_repository_requirements.txt new file mode 100644 index 0000000..09c4578 --- /dev/null +++ b/requirements/ros_repository_requirements.txt @@ -0,0 +1,9 @@ +git clone -b https://github.com/Robotics-Sensors/ROBOTIS-OP3-msgs +git clone -b https://github.com/Robotics-Sensors/ROBOTIS-OP3 +git clone -b https://github.com/Robotics-Sensors/ROBOTIS-OP3-Tools +git clone -b https://github.com/Robotics-Sensors/ROBOTIS-Framework-msgs +git clone -b https://github.com/Robotics-Sensors/ROBOTIS-Framework +git clone -b https://github.com/Robotics-Sensors/ROBOTIS-OP3-Common +git clone -b https://github.com/Robotics-Sensors/ROBOTIS-Utility +git clone -b https://github.com/Robotics-Sensors/ROBOTIS-OP3-Demo +git clone -b https://github.com/Robotics-Sensors/ROBOTIS-Math diff --git a/requirements/ros_requirements.txt b/requirements/ros_requirements.txt new file mode 100644 index 0000000..7b80df2 --- /dev/null +++ b/requirements/ros_requirements.txt @@ -0,0 +1,15 @@ +# Recomandation for dependencies for ros system + +ros-noetic-desktop-full +build-essential +ros-noetic-catkin +python-rosdep +python-rosinstall +python-rosinstall-generator +python-wstool +python-catkin-tools +ros-noetic-pcl-ros +ros-noetic-flexbe-behavior-engine +ros-noetic-moveit +ros-noetic-gazebo-ros-pkgs +ros-noetic-gazebo-ros-control diff --git a/requirements/system_requirements.txt b/requirements/system_requirements.txt new file mode 100644 index 0000000..46eea93 --- /dev/null +++ b/requirements/system_requirements.txt @@ -0,0 +1,29 @@ +# Recomandation for dependencies for linux system + +software-properties-common +python3.8 +neovim +apt-utils +automake +build-essential +ca-certificates +pycurl +git +python3-pip +libcurl3-dev +libfreetype6-dev +libpng-dev +libtool +libzmq3-dev +mlocate +openjdk-8-jdk +openjdk-8-jre-headless +pkg-config +python-dev +software-properties-common +swig +unzip +wget +zip +zlib1g-dev +python3-distutils From 3c7f13c7bd391b7dccbb4402f9ef8bb28dc735cb Mon Sep 17 00:00:00 2001 From: RonaldsonBellande Date: Wed, 27 Sep 2023 16:21:00 -0400 Subject: [PATCH 165/238] latest pushes --- README.md | 33 +++++++++----------- fix_errors.sh | 31 ++++++++++++++++++ requirements/ros_repository_requirements.txt | 10 +++--- 3 files changed, 51 insertions(+), 23 deletions(-) create mode 100755 fix_errors.sh diff --git a/README.md b/README.md index fc06982..60424a7 100755 --- a/README.md +++ b/README.md @@ -1,22 +1,19 @@ -## ROS Packages for ROBOTIS Framework -|Version|Kinetic + Ubuntu Xenial|Melodic + Ubuntu Bionic| -|:---:|:---:|:---:| -|[![GitHub version](https://badge.fury.io/gh/ROBOTIS-GIT%2FROBOTIS-Framework.svg)](https://badge.fury.io/gh/ROBOTIS-GIT%2FROBOTIS-Framework)|[![Build Status](https://travis-ci.org/ROBOTIS-GIT/ROBOTIS-Framework.svg?branch=master)](https://travis-ci.org/ROBOTIS-GIT/ROBOTIS-Framework)|-| +# ROS/ROS2 Humanoid Robot Framework -## ROBOTIS e-Manual for ROBOTIS Framework -- [ROBOTIS e-Manual for ROBOTIS Framework](http://emanual.robotis.com/docs/en/software/robotis_framework_packages) +-------------------------------------------------------------------------------------------------------- +Updated Version [humanoid_robot_module](https://github.com/Robotics-Sensors/humanoid_robot_framework) readme. -## Wiki for robotis_framework Packages -- http://wiki.ros.org/robotis_framework (metapackage) -- http://wiki.ros.org/robotis_controller -- http://wiki.ros.org/robotis_device -- http://wiki.ros.org/robotis_framework_common +-------------------------------------------------------------------------------------------------------- +## Important +The repository has diverged, as the old commits and codes are under the previous License and +the new commits and codes are under New License -## Open Source related to ROBOTIS Framework -- [robotis_framework](https://github.com/ROBOTIS-GIT/ROBOTIS-Framework) -- [robotis_controller_msgs](https://github.com/ROBOTIS-GIT/ROBOTIS-Framework-msgs) -- [dynamixel_sdk](https://github.com/ROBOTIS-GIT/DynamixelSDK) +-------------------------------------------------------------------------------------------------------- +Latest versions and Maintainer is on organization https://github.com/Robotics-Sensors -## Documents and Videos related to ROBOTIS Framework -- [ROBOTIS e-Manual for ROBOTIS Framework](http://emanual.robotis.com/docs/en/software/robotis_framework_packages/) -- [ROBOTIS e-Manual for Dynamixel SDK](http://emanual.robotis.com/docs/en/software/dynamixel/dynamixel_sdk/overview/) \ No newline at end of file + +### Maintainer +* Ronaldson Bellande + +## License +This SDK is distributed under the [Apache License, Version 2.0](https://www.apache.org/licenses/LICENSE-2.0), see [LICENSE](https://github.com/Robotics-Sensors/humanoid_robot_framework/blob/main/LICENSE) and [NOTICE](https://github.com/Robotics-Sensors/humanoid_robot_framework/blob/main/LICENSE) for more information. diff --git a/fix_errors.sh b/fix_errors.sh new file mode 100755 index 0000000..2ebdbee --- /dev/null +++ b/fix_errors.sh @@ -0,0 +1,31 @@ +#!/bin/bash + +# Get the URL from .git/config +git_url=$(git config --get remote.origin.url) + +# Check if a URL is found +if [ -z "$git_url" ]; then + echo "No remote URL found in .git/config." + exit 1 +fi + +# Clone the repository into a temporary folder +git clone "$git_url" tmp_clone + +# Check if the clone was successful +if [ $? -eq 0 ]; then + # Remove the existing .git directory if it exists + if [ -d ".git" ]; then + rm -rf .git + fi + + # Copy the .git directory from the clone to the current repository + cp -r tmp_clone/.git . + + # Remove the clone directory + rm -rf tmp_clone + + echo "Repository cloned and .git directory copied successfully." +else + echo "Failed to clone the repository." +fi diff --git a/requirements/ros_repository_requirements.txt b/requirements/ros_repository_requirements.txt index 09c4578..e3c338f 100644 --- a/requirements/ros_repository_requirements.txt +++ b/requirements/ros_repository_requirements.txt @@ -1,9 +1,9 @@ -git clone -b https://github.com/Robotics-Sensors/ROBOTIS-OP3-msgs -git clone -b https://github.com/Robotics-Sensors/ROBOTIS-OP3 -git clone -b https://github.com/Robotics-Sensors/ROBOTIS-OP3-Tools +git clone -b https://github.com/Robotics-Sensors/ROBOTIS-HUMANOID_ROBOT-msgs +git clone -b https://github.com/Robotics-Sensors/ROBOTIS-HUMANOID_ROBOT +git clone -b https://github.com/Robotics-Sensors/ROBOTIS-HUMANOID_ROBOT-Tools git clone -b https://github.com/Robotics-Sensors/ROBOTIS-Framework-msgs git clone -b https://github.com/Robotics-Sensors/ROBOTIS-Framework -git clone -b https://github.com/Robotics-Sensors/ROBOTIS-OP3-Common +git clone -b https://github.com/Robotics-Sensors/ROBOTIS-HUMANOID_ROBOT-Common git clone -b https://github.com/Robotics-Sensors/ROBOTIS-Utility -git clone -b https://github.com/Robotics-Sensors/ROBOTIS-OP3-Demo +git clone -b https://github.com/Robotics-Sensors/ROBOTIS-HUMANOID_ROBOT-Demo git clone -b https://github.com/Robotics-Sensors/ROBOTIS-Math From f98c64dda1bc30ff2e7a80c6316ce4ba5e1ee9c0 Mon Sep 17 00:00:00 2001 From: RonaldsonBellande Date: Wed, 27 Sep 2023 16:21:00 -0400 Subject: [PATCH 166/238] latest pushes --- README.md | 33 +++++++++----------- requirements/ros_repository_requirements.txt | 10 +++--- 2 files changed, 20 insertions(+), 23 deletions(-) diff --git a/README.md b/README.md index fc06982..60424a7 100755 --- a/README.md +++ b/README.md @@ -1,22 +1,19 @@ -## ROS Packages for ROBOTIS Framework -|Version|Kinetic + Ubuntu Xenial|Melodic + Ubuntu Bionic| -|:---:|:---:|:---:| -|[![GitHub version](https://badge.fury.io/gh/ROBOTIS-GIT%2FROBOTIS-Framework.svg)](https://badge.fury.io/gh/ROBOTIS-GIT%2FROBOTIS-Framework)|[![Build Status](https://travis-ci.org/ROBOTIS-GIT/ROBOTIS-Framework.svg?branch=master)](https://travis-ci.org/ROBOTIS-GIT/ROBOTIS-Framework)|-| +# ROS/ROS2 Humanoid Robot Framework -## ROBOTIS e-Manual for ROBOTIS Framework -- [ROBOTIS e-Manual for ROBOTIS Framework](http://emanual.robotis.com/docs/en/software/robotis_framework_packages) +-------------------------------------------------------------------------------------------------------- +Updated Version [humanoid_robot_module](https://github.com/Robotics-Sensors/humanoid_robot_framework) readme. -## Wiki for robotis_framework Packages -- http://wiki.ros.org/robotis_framework (metapackage) -- http://wiki.ros.org/robotis_controller -- http://wiki.ros.org/robotis_device -- http://wiki.ros.org/robotis_framework_common +-------------------------------------------------------------------------------------------------------- +## Important +The repository has diverged, as the old commits and codes are under the previous License and +the new commits and codes are under New License -## Open Source related to ROBOTIS Framework -- [robotis_framework](https://github.com/ROBOTIS-GIT/ROBOTIS-Framework) -- [robotis_controller_msgs](https://github.com/ROBOTIS-GIT/ROBOTIS-Framework-msgs) -- [dynamixel_sdk](https://github.com/ROBOTIS-GIT/DynamixelSDK) +-------------------------------------------------------------------------------------------------------- +Latest versions and Maintainer is on organization https://github.com/Robotics-Sensors -## Documents and Videos related to ROBOTIS Framework -- [ROBOTIS e-Manual for ROBOTIS Framework](http://emanual.robotis.com/docs/en/software/robotis_framework_packages/) -- [ROBOTIS e-Manual for Dynamixel SDK](http://emanual.robotis.com/docs/en/software/dynamixel/dynamixel_sdk/overview/) \ No newline at end of file + +### Maintainer +* Ronaldson Bellande + +## License +This SDK is distributed under the [Apache License, Version 2.0](https://www.apache.org/licenses/LICENSE-2.0), see [LICENSE](https://github.com/Robotics-Sensors/humanoid_robot_framework/blob/main/LICENSE) and [NOTICE](https://github.com/Robotics-Sensors/humanoid_robot_framework/blob/main/LICENSE) for more information. diff --git a/requirements/ros_repository_requirements.txt b/requirements/ros_repository_requirements.txt index 09c4578..e3c338f 100644 --- a/requirements/ros_repository_requirements.txt +++ b/requirements/ros_repository_requirements.txt @@ -1,9 +1,9 @@ -git clone -b https://github.com/Robotics-Sensors/ROBOTIS-OP3-msgs -git clone -b https://github.com/Robotics-Sensors/ROBOTIS-OP3 -git clone -b https://github.com/Robotics-Sensors/ROBOTIS-OP3-Tools +git clone -b https://github.com/Robotics-Sensors/ROBOTIS-HUMANOID_ROBOT-msgs +git clone -b https://github.com/Robotics-Sensors/ROBOTIS-HUMANOID_ROBOT +git clone -b https://github.com/Robotics-Sensors/ROBOTIS-HUMANOID_ROBOT-Tools git clone -b https://github.com/Robotics-Sensors/ROBOTIS-Framework-msgs git clone -b https://github.com/Robotics-Sensors/ROBOTIS-Framework -git clone -b https://github.com/Robotics-Sensors/ROBOTIS-OP3-Common +git clone -b https://github.com/Robotics-Sensors/ROBOTIS-HUMANOID_ROBOT-Common git clone -b https://github.com/Robotics-Sensors/ROBOTIS-Utility -git clone -b https://github.com/Robotics-Sensors/ROBOTIS-OP3-Demo +git clone -b https://github.com/Robotics-Sensors/ROBOTIS-HUMANOID_ROBOT-Demo git clone -b https://github.com/Robotics-Sensors/ROBOTIS-Math From cfc660d1460821b406152b19ee7daa75f6fff7d8 Mon Sep 17 00:00:00 2001 From: RonaldsonBellande Date: Wed, 27 Sep 2023 21:14:04 -0400 Subject: [PATCH 167/238] latest pushes --- robotis_controller/CHANGELOG.rst | 10 ++++++++++ robotis_device/CHANGELOG.rst | 11 +++++++++++ robotis_device/package.xml | 8 -------- robotis_framework/CHANGELOG.rst | 10 ++++++++++ robotis_framework/package.xml | 8 -------- robotis_framework_common/CHANGELOG.rst | 10 ++++++++++ robotis_framework_common/package.xml | 8 -------- 7 files changed, 41 insertions(+), 24 deletions(-) diff --git a/robotis_controller/CHANGELOG.rst b/robotis_controller/CHANGELOG.rst index ec0f902..4567313 100755 --- a/robotis_controller/CHANGELOG.rst +++ b/robotis_controller/CHANGELOG.rst @@ -2,6 +2,16 @@ Changelog for package robotis_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.3.1 (2023-09-27) +------------------ +* Starting from this point it under a new license +* Fix errors and Issues +* Rename Repository for a completely different purpose +* Make it compatible with ROS/ROS2 +* Upgrade version of all builds and make it more compatible +* Update package.xml and CMakeList.txt for to the latest versions +* Contributors & Maintainer: Ronaldson Bellande + 0.3.0 (2021-05-03) ------------------ * Update package.xml and CMakeList.txt for noetic branch diff --git a/robotis_device/CHANGELOG.rst b/robotis_device/CHANGELOG.rst index daa3c1d..a8a07fd 100755 --- a/robotis_device/CHANGELOG.rst +++ b/robotis_device/CHANGELOG.rst @@ -2,6 +2,17 @@ Changelog for package robotis_device ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.3.1 (2023-09-27) +------------------ +* Starting from this point it under a new license +* Fix errors and Issues +* Rename Repository for a completely different purpose +* Make it compatible with ROS/ROS2 +* Upgrade version of all builds and make it more compatible +* Update package.xml and CMakeList.txt for to the latest versions +* Contributors & Maintainer: Ronaldson Bellande + + 0.3.0 (2021-05-03) ------------------ * Update package.xml and CMakeList.txt for noetic branch diff --git a/robotis_device/package.xml b/robotis_device/package.xml index 7100feb..30fda2e 100755 --- a/robotis_device/package.xml +++ b/robotis_device/package.xml @@ -8,16 +8,8 @@ from the robotis_controller package. Apache 2.0 - Zerom - Kayman - SCH Ronaldson Bellande - http://wiki.ros.org/robotis_device - http://emanual.robotis.com/docs/en/software/robotis_framework_packages/ - https://github.com/ROBOTIS-GIT/ROBOTIS-Framework - https://github.com/ROBOTIS-GIT/ROBOTIS-Framework/issues - catkin roscpp diff --git a/robotis_framework/CHANGELOG.rst b/robotis_framework/CHANGELOG.rst index 794034b..fdf8a77 100755 --- a/robotis_framework/CHANGELOG.rst +++ b/robotis_framework/CHANGELOG.rst @@ -2,6 +2,16 @@ Changelog for package robotis_framework ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.3.1 (2023-09-27) +------------------ +* Starting from this point it under a new license +* Fix errors and Issues +* Rename Repository for a completely different purpose +* Make it compatible with ROS/ROS2 +* Upgrade version of all builds and make it more compatible +* Update package.xml and CMakeList.txt for to the latest versions +* Contributors & Maintainer: Ronaldson Bellande + 0.2.9 (2018-03-22) ------------------ * added serivce for setting module diff --git a/robotis_framework/package.xml b/robotis_framework/package.xml index 91ab8f5..9a78263 100755 --- a/robotis_framework/package.xml +++ b/robotis_framework/package.xml @@ -4,16 +4,8 @@ 0.3.0 ROS packages for the robotis_framework (meta package) Apache 2.0 - Zerom - Kayman - SCH Ronaldson Bellande - http://wiki.ros.org/robotis_framework - http://emanual.robotis.com/docs/en/software/robotis_framework_packages/ - https://github.com/ROBOTIS-GIT/ROBOTIS-Framework - https://github.com/ROBOTIS-GIT/ROBOTIS-Framework/issues - catkin robotis_controller diff --git a/robotis_framework_common/CHANGELOG.rst b/robotis_framework_common/CHANGELOG.rst index 7b3a4de..ed53e19 100755 --- a/robotis_framework_common/CHANGELOG.rst +++ b/robotis_framework_common/CHANGELOG.rst @@ -2,6 +2,16 @@ Changelog for package robotis_framework_common ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.3.1 (2023-09-27) +------------------ +* Starting from this point it under a new license +* Fix errors and Issues +* Rename Repository for a completely different purpose +* Make it compatible with ROS/ROS2 +* Upgrade version of all builds and make it more compatible +* Update package.xml and CMakeList.txt for to the latest versions +* Contributors & Maintainer: Ronaldson Bellande + 0.2.9 (2018-03-22) ------------------ * none diff --git a/robotis_framework_common/package.xml b/robotis_framework_common/package.xml index e2fefcc..1518643 100755 --- a/robotis_framework_common/package.xml +++ b/robotis_framework_common/package.xml @@ -4,16 +4,8 @@ 0.2.9 The package contains commonly used Headers for the ROBOTIS Framework. Apache 2.0 - Zerom - Kayman - SCH Ronaldson Bellande - http://wiki.ros.org/robotis_framework_common - http://emanual.robotis.com/docs/en/software/robotis_framework_packages/ - https://github.com/ROBOTIS-GIT/ROBOTIS-Framework - https://github.com/ROBOTIS-GIT/ROBOTIS-Framework/issues - catkin roscpp From 9c5630715371e8be29a9ef4c6e6bf58c1ccbcfea Mon Sep 17 00:00:00 2001 From: RonaldsonBellande Date: Wed, 27 Sep 2023 21:20:18 -0400 Subject: [PATCH 168/238] latest pushes --- robotis_controller/package.xml | 8 -------- 1 file changed, 8 deletions(-) diff --git a/robotis_controller/package.xml b/robotis_controller/package.xml index 3e123fa..cd32f74 100755 --- a/robotis_controller/package.xml +++ b/robotis_controller/package.xml @@ -4,16 +4,8 @@ 0.3.0 robotis_controller package for ROBOTIS's platform like Manipulator-H, THORMANG and OP series Apache 2.0 - Zerom - Kayman - SCH Ronaldson Bellande - http://wiki.ros.org/robotis_controller - http://emanual.robotis.com/docs/en/software/robotis_framework_packages/ - https://github.com/ROBOTIS-GIT/ROBOTIS-Framework - https://github.com/ROBOTIS-GIT/ROBOTIS-Framework/issues - catkin roscpp From d4a81403088b7a83c4cdb8054d02ea06e8e1f429 Mon Sep 17 00:00:00 2001 From: Ronaldson Bellande <47253433+RonaldsonBellande@users.noreply.github.com> Date: Wed, 27 Sep 2023 23:45:15 -0400 Subject: [PATCH 169/238] Delete catkin_setup.sh --- catkin_setup.sh | 5 ----- 1 file changed, 5 deletions(-) delete mode 100644 catkin_setup.sh diff --git a/catkin_setup.sh b/catkin_setup.sh deleted file mode 100644 index 6ec332f..0000000 --- a/catkin_setup.sh +++ /dev/null @@ -1,5 +0,0 @@ -#!/bin/bash - -catkin build -source "/opt/ros/noetic/setup.bash" -source "$CATKIN_WS/devel/setup.bash" From 1fe77031849c88220aa85b364f0ffb58f4582142 Mon Sep 17 00:00:00 2001 From: Ronaldson Bellande <47253433+RonaldsonBellande@users.noreply.github.com> Date: Wed, 27 Sep 2023 23:45:25 -0400 Subject: [PATCH 170/238] Delete fix_errors.sh --- fix_errors.sh | 31 ------------------------------- 1 file changed, 31 deletions(-) delete mode 100755 fix_errors.sh diff --git a/fix_errors.sh b/fix_errors.sh deleted file mode 100755 index 2ebdbee..0000000 --- a/fix_errors.sh +++ /dev/null @@ -1,31 +0,0 @@ -#!/bin/bash - -# Get the URL from .git/config -git_url=$(git config --get remote.origin.url) - -# Check if a URL is found -if [ -z "$git_url" ]; then - echo "No remote URL found in .git/config." - exit 1 -fi - -# Clone the repository into a temporary folder -git clone "$git_url" tmp_clone - -# Check if the clone was successful -if [ $? -eq 0 ]; then - # Remove the existing .git directory if it exists - if [ -d ".git" ]; then - rm -rf .git - fi - - # Copy the .git directory from the clone to the current repository - cp -r tmp_clone/.git . - - # Remove the clone directory - rm -rf tmp_clone - - echo "Repository cloned and .git directory copied successfully." -else - echo "Failed to clone the repository." -fi From ecca37f58d0624c615818f2823b768a1ddda3083 Mon Sep 17 00:00:00 2001 From: Ronaldson Bellande <47253433+RonaldsonBellande@users.noreply.github.com> Date: Wed, 27 Sep 2023 23:45:35 -0400 Subject: [PATCH 171/238] Delete push.sh --- push.sh | 4 ---- 1 file changed, 4 deletions(-) delete mode 100755 push.sh diff --git a/push.sh b/push.sh deleted file mode 100755 index 3435685..0000000 --- a/push.sh +++ /dev/null @@ -1,4 +0,0 @@ -#!/bin/bash - -# Git push what is already in the repository -git pull --no-edit; git fetch; git add .; git commit -am "latest pushes"; git push From 254f5c0e6d480f9fe6d561d92e0c1297a7006c89 Mon Sep 17 00:00:00 2001 From: RonaldsonBellande Date: Thu, 28 Sep 2023 00:16:47 -0400 Subject: [PATCH 172/238] latest pushes --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 60424a7..c91aa6b 100755 --- a/README.md +++ b/README.md @@ -1,7 +1,7 @@ # ROS/ROS2 Humanoid Robot Framework -------------------------------------------------------------------------------------------------------- -Updated Version [humanoid_robot_module](https://github.com/Robotics-Sensors/humanoid_robot_framework) readme. +Updated Version [humanoid_robot_framework](https://github.com/Robotics-Sensors/humanoid_robot_framework) readme. -------------------------------------------------------------------------------------------------------- ## Important From 8a49fccaf54dca10f25a0962691f359dabf51f88 Mon Sep 17 00:00:00 2001 From: RonaldsonBellande Date: Thu, 28 Sep 2023 00:16:47 -0400 Subject: [PATCH 173/238] latest pushes --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 60424a7..c91aa6b 100755 --- a/README.md +++ b/README.md @@ -1,7 +1,7 @@ # ROS/ROS2 Humanoid Robot Framework -------------------------------------------------------------------------------------------------------- -Updated Version [humanoid_robot_module](https://github.com/Robotics-Sensors/humanoid_robot_framework) readme. +Updated Version [humanoid_robot_framework](https://github.com/Robotics-Sensors/humanoid_robot_framework) readme. -------------------------------------------------------------------------------------------------------- ## Important From cb36d363349648da52e4629ac40691ca176bc63e Mon Sep 17 00:00:00 2001 From: RonaldsonBellande Date: Thu, 28 Sep 2023 09:50:36 -0400 Subject: [PATCH 174/238] latest pushes --- .robotis_framework.rosinstall | 1 - .travis.yml | 2 +- fix_errors.sh | 31 +++++ .../CHANGELOG.rst | 6 +- .../CMakeLists.txt | 24 ++-- .../humanoid_robot_controller.h | 50 ++++---- .../package.xml | 22 ++-- .../humanoid_robot_controller.cpp | 66 +++++------ .../CHANGELOG.rst | 4 +- .../CMakeLists.txt | 18 +-- .../devices/dynamixel/GRIPPER.device | 0 .../devices/dynamixel/GRIPPER_TORQUE.device | 0 .../devices/dynamixel/H42-20-S300-R(A).device | 0 .../devices/dynamixel/H42-20-S300-R.device | 0 .../devices/dynamixel/H42P-020-S300-R.device | 0 .../dynamixel/H54-100-B210-R-NR.device | 0 .../dynamixel/H54-100-S500-R(A).device | 0 .../devices/dynamixel/H54-100-S500-R.device | 0 .../devices/dynamixel/H54-200-B500-R.device | 0 .../dynamixel/H54-200-S500-R(A).device | 0 .../devices/dynamixel/H54-200-S500-R.device | 0 .../devices/dynamixel/H54P-100-S500-R.device | 0 .../devices/dynamixel/H54P-200-B500-R.device | 0 .../devices/dynamixel/H54P-200-S500-R.device | 0 .../devices/dynamixel/L42-10-S300-R.device | 0 .../devices/dynamixel/L54-30-S400-R.device | 0 .../devices/dynamixel/L54-30-S500-R.device | 0 .../devices/dynamixel/L54-50-S290-R.device | 0 .../devices/dynamixel/L54-50-S500-R.device | 0 .../devices/dynamixel/M42-10-S260-R.device | 0 .../devices/dynamixel/M54-40-S250-R.device | 0 .../devices/dynamixel/M54-60-S250-R.device | 0 .../devices/dynamixel/MX-106.device | 0 .../devices/dynamixel/MX-28.device | 0 .../devices/dynamixel/MX-64.device | 0 .../devices/dynamixel/PH42-020-S300-R.device | 0 .../devices/dynamixel/PH54-100-S500-R.device | 0 .../devices/dynamixel/PH54-200-S500-R.device | 0 .../devices/dynamixel/RH-P12-RN(A).device | 0 .../devices/dynamixel/RH-P12-RN.device | 0 .../devices/dynamixel/XM-430.device | 0 .../devices/dynamixel/XM430-W210.device | 0 .../devices/dynamixel/XM430-W350.device | 0 .../devices/dynamixel/XM540-W150.device | 0 .../devices/dynamixel/XM540-W270.device | 0 .../devices/sensor/CM-740.device | 0 .../devices/sensor/OPEN-CR.device | 0 .../control_table_item.h | 4 +- .../include/humanoid_robot_device}/device.h | 2 +- .../humanoid_robot_device}/dynamixel.h | 2 +- .../humanoid_robot_device}/dynamixel_state.h | 2 +- .../include/humanoid_robot_device}/robot.h | 2 +- .../include/humanoid_robot_device}/sensor.h | 2 +- .../humanoid_robot_device}/sensor_state.h | 2 +- .../humanoid_robot_device}/time_stamp.h | 2 +- .../package.xml | 4 +- .../src/humanoid_robot_device}/dynamixel.cpp | 4 +- .../src/humanoid_robot_device}/robot.cpp | 4 +- .../src/humanoid_robot_device}/sensor.cpp | 4 +- .../CHANGELOG.rst | 6 +- .../CMakeLists.txt | 2 +- humanoid_robot_framework/package.xml | 26 +++++ .../CHANGELOG.rst | 2 +- .../CMakeLists.txt | 8 +- .../motion_module.h | 6 +- .../sensor_module.h | 6 +- .../singleton.h | 2 +- .../package.xml | 8 +- push.sh | 4 + repository_recal.sh | 110 ++++++++++++++++++ robotis_framework/package.xml | 26 ----- 71 files changed, 304 insertions(+), 160 deletions(-) delete mode 100644 .robotis_framework.rosinstall create mode 100755 fix_errors.sh rename {robotis_controller => humanoid_robot_controller}/CHANGELOG.rst (96%) rename {robotis_controller => humanoid_robot_controller}/CMakeLists.txt (82%) rename robotis_controller/include/robotis_controller/robotis_controller.h => humanoid_robot_controller/include/humanoid_robot_controller/humanoid_robot_controller.h (78%) rename {robotis_controller => humanoid_robot_controller}/package.xml (61%) rename robotis_controller/src/robotis_controller/robotis_controller.cpp => humanoid_robot_controller/src/humanoid_robot_controller/humanoid_robot_controller.cpp (97%) rename {robotis_device => humanoid_robot_device}/CHANGELOG.rst (97%) rename {robotis_device => humanoid_robot_device}/CMakeLists.txt (84%) rename {robotis_device => humanoid_robot_device}/devices/dynamixel/GRIPPER.device (100%) rename {robotis_device => humanoid_robot_device}/devices/dynamixel/GRIPPER_TORQUE.device (100%) rename {robotis_device => humanoid_robot_device}/devices/dynamixel/H42-20-S300-R(A).device (100%) rename {robotis_device => humanoid_robot_device}/devices/dynamixel/H42-20-S300-R.device (100%) rename {robotis_device => humanoid_robot_device}/devices/dynamixel/H42P-020-S300-R.device (100%) rename {robotis_device => humanoid_robot_device}/devices/dynamixel/H54-100-B210-R-NR.device (100%) rename {robotis_device => humanoid_robot_device}/devices/dynamixel/H54-100-S500-R(A).device (100%) rename {robotis_device => humanoid_robot_device}/devices/dynamixel/H54-100-S500-R.device (100%) rename {robotis_device => humanoid_robot_device}/devices/dynamixel/H54-200-B500-R.device (100%) rename {robotis_device => humanoid_robot_device}/devices/dynamixel/H54-200-S500-R(A).device (100%) rename {robotis_device => humanoid_robot_device}/devices/dynamixel/H54-200-S500-R.device (100%) rename {robotis_device => humanoid_robot_device}/devices/dynamixel/H54P-100-S500-R.device (100%) rename {robotis_device => humanoid_robot_device}/devices/dynamixel/H54P-200-B500-R.device (100%) rename {robotis_device => humanoid_robot_device}/devices/dynamixel/H54P-200-S500-R.device (100%) rename {robotis_device => humanoid_robot_device}/devices/dynamixel/L42-10-S300-R.device (100%) rename {robotis_device => humanoid_robot_device}/devices/dynamixel/L54-30-S400-R.device (100%) rename {robotis_device => humanoid_robot_device}/devices/dynamixel/L54-30-S500-R.device (100%) rename {robotis_device => humanoid_robot_device}/devices/dynamixel/L54-50-S290-R.device (100%) rename {robotis_device => humanoid_robot_device}/devices/dynamixel/L54-50-S500-R.device (100%) rename {robotis_device => humanoid_robot_device}/devices/dynamixel/M42-10-S260-R.device (100%) rename {robotis_device => humanoid_robot_device}/devices/dynamixel/M54-40-S250-R.device (100%) rename {robotis_device => humanoid_robot_device}/devices/dynamixel/M54-60-S250-R.device (100%) rename {robotis_device => humanoid_robot_device}/devices/dynamixel/MX-106.device (100%) rename {robotis_device => humanoid_robot_device}/devices/dynamixel/MX-28.device (100%) rename {robotis_device => humanoid_robot_device}/devices/dynamixel/MX-64.device (100%) rename {robotis_device => humanoid_robot_device}/devices/dynamixel/PH42-020-S300-R.device (100%) rename {robotis_device => humanoid_robot_device}/devices/dynamixel/PH54-100-S500-R.device (100%) rename {robotis_device => humanoid_robot_device}/devices/dynamixel/PH54-200-S500-R.device (100%) rename {robotis_device => humanoid_robot_device}/devices/dynamixel/RH-P12-RN(A).device (100%) rename {robotis_device => humanoid_robot_device}/devices/dynamixel/RH-P12-RN.device (100%) rename {robotis_device => humanoid_robot_device}/devices/dynamixel/XM-430.device (100%) rename {robotis_device => humanoid_robot_device}/devices/dynamixel/XM430-W210.device (100%) rename {robotis_device => humanoid_robot_device}/devices/dynamixel/XM430-W350.device (100%) rename {robotis_device => humanoid_robot_device}/devices/dynamixel/XM540-W150.device (100%) rename {robotis_device => humanoid_robot_device}/devices/dynamixel/XM540-W270.device (100%) rename {robotis_device => humanoid_robot_device}/devices/sensor/CM-740.device (100%) rename {robotis_device => humanoid_robot_device}/devices/sensor/OPEN-CR.device (100%) rename {robotis_device/include/robotis_device => humanoid_robot_device/include/humanoid_robot_device}/control_table_item.h (92%) rename {robotis_device/include/robotis_device => humanoid_robot_device/include/humanoid_robot_device}/device.h (93%) rename {robotis_device/include/robotis_device => humanoid_robot_device/include/humanoid_robot_device}/dynamixel.h (95%) rename {robotis_device/include/robotis_device => humanoid_robot_device/include/humanoid_robot_device}/dynamixel_state.h (94%) rename {robotis_device/include/robotis_device => humanoid_robot_device/include/humanoid_robot_device}/robot.h (95%) rename {robotis_device/include/robotis_device => humanoid_robot_device/include/humanoid_robot_device}/sensor.h (93%) rename {robotis_device/include/robotis_device => humanoid_robot_device/include/humanoid_robot_device}/sensor_state.h (93%) rename {robotis_device/include/robotis_device => humanoid_robot_device/include/humanoid_robot_device}/time_stamp.h (93%) rename {robotis_device => humanoid_robot_device}/package.xml (89%) rename {robotis_device/src/robotis_device => humanoid_robot_device/src/humanoid_robot_device}/dynamixel.cpp (97%) rename {robotis_device/src/robotis_device => humanoid_robot_device/src/humanoid_robot_device}/robot.cpp (97%) rename {robotis_device/src/robotis_device => humanoid_robot_device/src/humanoid_robot_device}/sensor.cpp (90%) rename {robotis_framework => humanoid_robot_framework}/CHANGELOG.rst (93%) rename {robotis_framework => humanoid_robot_framework}/CMakeLists.txt (72%) create mode 100755 humanoid_robot_framework/package.xml rename {robotis_framework_common => humanoid_robot_framework_common}/CHANGELOG.rst (97%) rename {robotis_framework_common => humanoid_robot_framework_common}/CMakeLists.txt (91%) rename {robotis_framework_common/include/robotis_framework_common => humanoid_robot_framework_common/include/humanoid_robot_framework_common}/motion_module.h (90%) rename {robotis_framework_common/include/robotis_framework_common => humanoid_robot_framework_common/include/humanoid_robot_framework_common}/sensor_module.h (88%) rename {robotis_framework_common/include/robotis_framework_common => humanoid_robot_framework_common/include/humanoid_robot_framework_common}/singleton.h (93%) rename {robotis_framework_common => humanoid_robot_framework_common}/package.xml (68%) create mode 100755 push.sh create mode 100755 repository_recal.sh delete mode 100755 robotis_framework/package.xml diff --git a/.robotis_framework.rosinstall b/.robotis_framework.rosinstall deleted file mode 100644 index 9e8f90b..0000000 --- a/.robotis_framework.rosinstall +++ /dev/null @@ -1 +0,0 @@ -- git: {local-name: robotis_controller_msgs, uri: 'https://github.com/Robotics-Sensors/ROBOTIS-Framework-msgs.git', version: noetic} diff --git a/.travis.yml b/.travis.yml index 9696b6b..e1e689a 100644 --- a/.travis.yml +++ b/.travis.yml @@ -18,7 +18,7 @@ notifications: - ronaldsonbellande@gmail.com env: matrix: - - ROS_DISTRO=noetic ROS_REPO=ros-shadow-fixed UPSTREAM_WORKSPACE=file OS_NAME=ubuntu OS_CODE_NAME=focal $ROSINSTALL_FILENAME=".robotis_framework.rosinstall" + - ROS_DISTRO=noetic ROS_REPO=ros-shadow-fixed UPSTREAM_WORKSPACE=file OS_NAME=ubuntu OS_CODE_NAME=focal $ROSINSTALL_FILENAME=".humanoid_robot_framework.rosinstall" branches: only: - master diff --git a/fix_errors.sh b/fix_errors.sh new file mode 100755 index 0000000..2ebdbee --- /dev/null +++ b/fix_errors.sh @@ -0,0 +1,31 @@ +#!/bin/bash + +# Get the URL from .git/config +git_url=$(git config --get remote.origin.url) + +# Check if a URL is found +if [ -z "$git_url" ]; then + echo "No remote URL found in .git/config." + exit 1 +fi + +# Clone the repository into a temporary folder +git clone "$git_url" tmp_clone + +# Check if the clone was successful +if [ $? -eq 0 ]; then + # Remove the existing .git directory if it exists + if [ -d ".git" ]; then + rm -rf .git + fi + + # Copy the .git directory from the clone to the current repository + cp -r tmp_clone/.git . + + # Remove the clone directory + rm -rf tmp_clone + + echo "Repository cloned and .git directory copied successfully." +else + echo "Failed to clone the repository." +fi diff --git a/robotis_controller/CHANGELOG.rst b/humanoid_robot_controller/CHANGELOG.rst similarity index 96% rename from robotis_controller/CHANGELOG.rst rename to humanoid_robot_controller/CHANGELOG.rst index 4567313..2982840 100755 --- a/robotis_controller/CHANGELOG.rst +++ b/humanoid_robot_controller/CHANGELOG.rst @@ -1,5 +1,5 @@ ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package robotis_controller +Changelog for package humanoid_robot_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 0.3.1 (2023-09-27) @@ -63,7 +63,7 @@ Changelog for package robotis_controller 0.2.2 (2017-04-24) ------------------ -* updated robotis_controller.cpp +* updated humanoid_robot_controller.cpp * changed to read control cycle from .robot file * Contributors: Zerom @@ -76,7 +76,7 @@ Changelog for package robotis_controller * - added WriteControlTable msg callback * mode change debugging * - optimized cpu usage by spin loop (by astumpf) -* - robotis_controller process() : processing order changed. +* - humanoid_robot_controller process() : processing order changed. * 1st : packet communication * 2nd : processing modules * - dependencies fixed. (Pull requests `#26 `_) diff --git a/robotis_controller/CMakeLists.txt b/humanoid_robot_controller/CMakeLists.txt similarity index 82% rename from robotis_controller/CMakeLists.txt rename to humanoid_robot_controller/CMakeLists.txt index fa09fd4..f62a080 100755 --- a/robotis_controller/CMakeLists.txt +++ b/humanoid_robot_controller/CMakeLists.txt @@ -2,7 +2,7 @@ # Set minimum required version of cmake, project name and compile options ################################################################################ cmake_minimum_required(VERSION 3.0.2) -project(robotis_controller) +project(humanoid_robot_controller) ## Compile as C++11, supported in ROS Kinetic and newer add_compile_options(-std=c++11) @@ -15,10 +15,10 @@ find_package(catkin REQUIRED COMPONENTS roslib std_msgs sensor_msgs - robotis_controller_msgs + humanoid_robot_controller_msgs dynamixel_sdk - robotis_device - robotis_framework_common + humanoid_robot_device + humanoid_robot_framework_common cmake_modules ) @@ -59,16 +59,16 @@ endif(NOT ${YAML_CPP_VERSION} VERSION_LESS "0.5") ################################################################################ catkin_package( INCLUDE_DIRS include - LIBRARIES robotis_controller + LIBRARIES humanoid_robot_controller CATKIN_DEPENDS roscpp roslib std_msgs sensor_msgs - robotis_controller_msgs + humanoid_robot_controller_msgs dynamixel_sdk - robotis_device - robotis_framework_common + humanoid_robot_device + humanoid_robot_framework_common cmake_modules DEPENDS Boost ) @@ -83,14 +83,14 @@ include_directories( ${YAML_CPP_INCLUDE_DIRS} ) -add_library(robotis_controller src/robotis_controller/robotis_controller.cpp) -add_dependencies(robotis_controller ${catkin_EXPORTED_TARGETS}) -target_link_libraries(robotis_controller ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${YAML_CPP_LIBRARIES}) +add_library(humanoid_robot_controller src/humanoid_robot_controller/humanoid_robot_controller.cpp) +add_dependencies(humanoid_robot_controller ${catkin_EXPORTED_TARGETS}) +target_link_libraries(humanoid_robot_controller ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${YAML_CPP_LIBRARIES}) ################################################################################ # Install ################################################################################ -install(TARGETS robotis_controller +install(TARGETS humanoid_robot_controller ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} diff --git a/robotis_controller/include/robotis_controller/robotis_controller.h b/humanoid_robot_controller/include/humanoid_robot_controller/humanoid_robot_controller.h similarity index 78% rename from robotis_controller/include/robotis_controller/robotis_controller.h rename to humanoid_robot_controller/include/humanoid_robot_controller/humanoid_robot_controller.h index 75a4580..eca9641 100755 --- a/robotis_controller/include/robotis_controller/robotis_controller.h +++ b/humanoid_robot_controller/include/humanoid_robot_controller/humanoid_robot_controller.h @@ -15,7 +15,7 @@ *******************************************************************************/ /* - * robotis_controller.h + * humanoid_robot_controller.h * * Created on: 2016. 1. 15. * Author: zerom @@ -32,21 +32,21 @@ #include #include -#include "robotis_controller_msgs/GetJointModule.h" -#include "robotis_controller_msgs/JointCtrlModule.h" -#include "robotis_controller_msgs/LoadOffset.h" -#include "robotis_controller_msgs/SetJointModule.h" -#include "robotis_controller_msgs/SetModule.h" -#include "robotis_controller_msgs/SyncWriteItem.h" -#include "robotis_controller_msgs/WriteControlTable.h" +#include "humanoid_robot_controller_msgs/GetJointModule.h" +#include "humanoid_robot_controller_msgs/JointCtrlModule.h" +#include "humanoid_robot_controller_msgs/LoadOffset.h" +#include "humanoid_robot_controller_msgs/SetJointModule.h" +#include "humanoid_robot_controller_msgs/SetModule.h" +#include "humanoid_robot_controller_msgs/SyncWriteItem.h" +#include "humanoid_robot_controller_msgs/WriteControlTable.h" #include "dynamixel_sdk/group_bulk_read.h" #include "dynamixel_sdk/group_sync_write.h" -#include "robotis_device/robot.h" -#include "robotis_framework_common/motion_module.h" -#include "robotis_framework_common/sensor_module.h" +#include "humanoid_robot_device/robot.h" +#include "humanoid_robot_framework_common/motion_module.h" +#include "humanoid_robot_framework_common/sensor_module.h" -namespace robotis_framework { +namespace humanoid_robot_framework { enum ControllerMode { MotionModuleMode, DirectControlMode }; @@ -75,7 +75,7 @@ private: void msgQueueThread(); void setCtrlModuleThread(std::string ctrl_module); void setJointCtrlModuleThread( - const robotis_controller_msgs::JointCtrlModule::ConstPtr &msg); + const humanoid_robot_controller_msgs::JointCtrlModule::ConstPtr &msg); bool isTimerStopped(); void initializeSyncWrite(); @@ -143,25 +143,25 @@ public: /* ROS Topic Callback Functions */ void writeControlTableCallback( - const robotis_controller_msgs::WriteControlTable::ConstPtr &msg); + const humanoid_robot_controller_msgs::WriteControlTable::ConstPtr &msg); void syncWriteItemCallback( - const robotis_controller_msgs::SyncWriteItem::ConstPtr &msg); + const humanoid_robot_controller_msgs::SyncWriteItem::ConstPtr &msg); void setControllerModeCallback(const std_msgs::String::ConstPtr &msg); void setJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg); void setJointCtrlModuleCallback( - const robotis_controller_msgs::JointCtrlModule::ConstPtr &msg); + const humanoid_robot_controller_msgs::JointCtrlModule::ConstPtr &msg); void setCtrlModuleCallback(const std_msgs::String::ConstPtr &msg); void enableOffsetCallback(const std_msgs::Bool::ConstPtr &msg); bool getJointCtrlModuleService( - robotis_controller_msgs::GetJointModule::Request &req, - robotis_controller_msgs::GetJointModule::Response &res); + humanoid_robot_controller_msgs::GetJointModule::Request &req, + humanoid_robot_controller_msgs::GetJointModule::Response &res); bool setJointCtrlModuleService( - robotis_controller_msgs::SetJointModule::Request &req, - robotis_controller_msgs::SetJointModule::Response &res); - bool setCtrlModuleService(robotis_controller_msgs::SetModule::Request &req, - robotis_controller_msgs::SetModule::Response &res); - bool loadOffsetService(robotis_controller_msgs::LoadOffset::Request &req, - robotis_controller_msgs::LoadOffset::Response &res); + humanoid_robot_controller_msgs::SetJointModule::Request &req, + humanoid_robot_controller_msgs::SetJointModule::Response &res); + bool setCtrlModuleService(humanoid_robot_controller_msgs::SetModule::Request &req, + humanoid_robot_controller_msgs::SetModule::Response &res); + bool loadOffsetService(humanoid_robot_controller_msgs::LoadOffset::Request &req, + humanoid_robot_controller_msgs::LoadOffset::Response &res); void gazeboJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg); @@ -202,6 +202,6 @@ public: uint8_t *data, uint8_t *error = 0); }; -} // namespace robotis_framework +} // namespace humanoid_robot_framework #endif /* ROBOTIS_CONTROLLER_ROBOTIS_CONTROLLER_H_ */ diff --git a/robotis_controller/package.xml b/humanoid_robot_controller/package.xml similarity index 61% rename from robotis_controller/package.xml rename to humanoid_robot_controller/package.xml index cd32f74..00106e9 100755 --- a/robotis_controller/package.xml +++ b/humanoid_robot_controller/package.xml @@ -1,8 +1,8 @@ - robotis_controller + humanoid_robot_controller 0.3.0 - robotis_controller package for ROBOTIS's platform like Manipulator-H, THORMANG and OP series + humanoid_robot_controller package for ROBOTIS's platform like Manipulator-H, THORMANG and OP series Apache 2.0 Ronaldson Bellande @@ -12,10 +12,10 @@ roslib std_msgs sensor_msgs - robotis_controller_msgs + humanoid_robot_controller_msgs dynamixel_sdk - robotis_device - robotis_framework_common + humanoid_robot_device + humanoid_robot_framework_common cmake_modules yaml-cpp @@ -23,10 +23,10 @@ roslib std_msgs sensor_msgs - robotis_controller_msgs + humanoid_robot_controller_msgs dynamixel_sdk - robotis_device - robotis_framework_common + humanoid_robot_device + humanoid_robot_framework_common cmake_modules yaml-cpp @@ -34,10 +34,10 @@ roslib std_msgs sensor_msgs - robotis_controller_msgs + humanoid_robot_controller_msgs dynamixel_sdk - robotis_device - robotis_framework_common + humanoid_robot_device + humanoid_robot_framework_common cmake_modules yaml-cpp diff --git a/robotis_controller/src/robotis_controller/robotis_controller.cpp b/humanoid_robot_controller/src/humanoid_robot_controller/humanoid_robot_controller.cpp similarity index 97% rename from robotis_controller/src/robotis_controller/robotis_controller.cpp rename to humanoid_robot_controller/src/humanoid_robot_controller/humanoid_robot_controller.cpp index 548bfaa..47b8d2a 100755 --- a/robotis_controller/src/robotis_controller/robotis_controller.cpp +++ b/humanoid_robot_controller/src/humanoid_robot_controller/humanoid_robot_controller.cpp @@ -15,7 +15,7 @@ *******************************************************************************/ /* - * robotis_controller.cpp + * humanoid_robot_controller.cpp * * Created on: 2016. 1. 15. * Author: zerom @@ -24,9 +24,9 @@ #include #include -#include "robotis_controller/robotis_controller.h" +#include "humanoid_robot_controller/humanoid_robot_controller.h" -using namespace robotis_framework; +using namespace humanoid_robot_framework; RobotisController::RobotisController() : is_timer_running_(false), @@ -39,7 +39,7 @@ RobotisController::RobotisController() DEBUG_PRINT(false), robot_(0), gazebo_mode_(false), - gazebo_robot_name_("robotis") + gazebo_robot_name_("humanoid_robot") { direct_sync_write_.clear(); } @@ -198,7 +198,7 @@ void RobotisController::initializeSyncWrite() bool RobotisController::initialize(const std::string robot_file_path, const std::string init_file_path) { - std::string dev_desc_dir_path = ros::package::getPath("robotis_device") + "/devices"; + std::string dev_desc_dir_path = ros::package::getPath("humanoid_robot_device") + "/devices"; // load robot info : port , device robot_ = new Robot(robot_file_path, dev_desc_dir_path); @@ -639,19 +639,19 @@ void RobotisController::msgQueueThread() ros_node.setCallbackQueue(&callback_queue); /* subscriber */ - ros::Subscriber write_control_table_sub = ros_node.subscribe("/robotis/write_control_table", 5, + ros::Subscriber write_control_table_sub = ros_node.subscribe("/humanoid_robot/write_control_table", 5, &RobotisController::writeControlTableCallback, this); - ros::Subscriber sync_write_item_sub = ros_node.subscribe("/robotis/sync_write_item", 10, + ros::Subscriber sync_write_item_sub = ros_node.subscribe("/humanoid_robot/sync_write_item", 10, &RobotisController::syncWriteItemCallback, this); - ros::Subscriber joint_ctrl_modules_sub = ros_node.subscribe("/robotis/set_joint_ctrl_modules", 10, + ros::Subscriber joint_ctrl_modules_sub = ros_node.subscribe("/humanoid_robot/set_joint_ctrl_modules", 10, &RobotisController::setJointCtrlModuleCallback, this); - ros::Subscriber enable_ctrl_module_sub = ros_node.subscribe("/robotis/enable_ctrl_module", 10, + ros::Subscriber enable_ctrl_module_sub = ros_node.subscribe("/humanoid_robot/enable_ctrl_module", 10, &RobotisController::setCtrlModuleCallback, this); - ros::Subscriber control_mode_sub = ros_node.subscribe("/robotis/set_control_mode", 10, + ros::Subscriber control_mode_sub = ros_node.subscribe("/humanoid_robot/set_control_mode", 10, &RobotisController::setControllerModeCallback, this); - ros::Subscriber joint_states_sub = ros_node.subscribe("/robotis/set_joint_states", 10, + ros::Subscriber joint_states_sub = ros_node.subscribe("/humanoid_robot/set_joint_states", 10, &RobotisController::setJointStatesCallback, this); - ros::Subscriber enable_offset_sub = ros_node.subscribe("/robotis/enable_offset", 10, + ros::Subscriber enable_offset_sub = ros_node.subscribe("/humanoid_robot/enable_offset", 10, &RobotisController::enableOffsetCallback, this); ros::Subscriber gazebo_joint_states_sub; @@ -660,10 +660,10 @@ void RobotisController::msgQueueThread() &RobotisController::gazeboJointStatesCallback, this); /* publisher */ - goal_joint_state_pub_ = ros_node.advertise("/robotis/goal_joint_states", 10); - present_joint_state_pub_ = ros_node.advertise("/robotis/present_joint_states", 10); - current_module_pub_ = ros_node.advertise( - "/robotis/present_joint_ctrl_modules", 10); + goal_joint_state_pub_ = ros_node.advertise("/humanoid_robot/goal_joint_states", 10); + present_joint_state_pub_ = ros_node.advertise("/humanoid_robot/present_joint_states", 10); + current_module_pub_ = ros_node.advertise( + "/humanoid_robot/present_joint_ctrl_modules", 10); if (gazebo_mode_ == true) { @@ -679,13 +679,13 @@ void RobotisController::msgQueueThread() } /* service */ - ros::ServiceServer get_joint_module_server = ros_node.advertiseService("/robotis/get_present_joint_ctrl_modules", + ros::ServiceServer get_joint_module_server = ros_node.advertiseService("/humanoid_robot/get_present_joint_ctrl_modules", &RobotisController::getJointCtrlModuleService, this); - ros::ServiceServer set_joint_module_server = ros_node.advertiseService("/robotis/set_present_joint_ctrl_modules", + ros::ServiceServer set_joint_module_server = ros_node.advertiseService("/humanoid_robot/set_present_joint_ctrl_modules", &RobotisController::setJointCtrlModuleService, this); - ros::ServiceServer set_module_server = ros_node.advertiseService("/robotis/set_present_ctrl_modules", + ros::ServiceServer set_module_server = ros_node.advertiseService("/humanoid_robot/set_present_ctrl_modules", &RobotisController::setCtrlModuleService, this); - ros::ServiceServer load_offset_server = ros_node.advertiseService("/robotis/load_offset", + ros::ServiceServer load_offset_server = ros_node.advertiseService("/humanoid_robot/load_offset", &RobotisController::loadOffsetService, this); ros::WallDuration duration(robot_->getControlCycle() / 1000.0); @@ -1568,7 +1568,7 @@ void RobotisController::removeSensorModule(SensorModule *module) sensor_modules_.remove(module); } -void RobotisController::writeControlTableCallback(const robotis_controller_msgs::WriteControlTable::ConstPtr &msg) +void RobotisController::writeControlTableCallback(const humanoid_robot_controller_msgs::WriteControlTable::ConstPtr &msg) { Device *device = NULL; @@ -1626,7 +1626,7 @@ void RobotisController::writeControlTableCallback(const robotis_controller_msgs: } -void RobotisController::syncWriteItemCallback(const robotis_controller_msgs::SyncWriteItem::ConstPtr &msg) +void RobotisController::syncWriteItemCallback(const humanoid_robot_controller_msgs::SyncWriteItem::ConstPtr &msg) { for (int i = 0; i < msg->joint_name.size(); i++) { @@ -1786,7 +1786,7 @@ void RobotisController::setCtrlModule(std::string module_name) set_module_thread_ = boost::thread(boost::bind(&RobotisController::setCtrlModuleThread, this, module_name)); } -void RobotisController::setJointCtrlModuleCallback(const robotis_controller_msgs::JointCtrlModule::ConstPtr &msg) +void RobotisController::setJointCtrlModuleCallback(const humanoid_robot_controller_msgs::JointCtrlModule::ConstPtr &msg) { if (msg->joint_name.size() != msg->module_name.size()) return; @@ -1806,8 +1806,8 @@ void RobotisController::enableOffsetCallback(const std_msgs::Bool::ConstPtr &msg offset_ratio_ = 1.0; } -bool RobotisController::getJointCtrlModuleService(robotis_controller_msgs::GetJointModule::Request &req, - robotis_controller_msgs::GetJointModule::Response &res) +bool RobotisController::getJointCtrlModuleService(humanoid_robot_controller_msgs::GetJointModule::Request &req, + humanoid_robot_controller_msgs::GetJointModule::Response &res) { for (unsigned int idx = 0; idx < req.joint_name.size(); idx++) { @@ -1825,16 +1825,16 @@ bool RobotisController::getJointCtrlModuleService(robotis_controller_msgs::GetJo return true; } -bool RobotisController::setJointCtrlModuleService(robotis_controller_msgs::SetJointModule::Request &req, robotis_controller_msgs::SetJointModule::Response &res) +bool RobotisController::setJointCtrlModuleService(humanoid_robot_controller_msgs::SetJointModule::Request &req, humanoid_robot_controller_msgs::SetJointModule::Response &res) { if(set_module_thread_.joinable()) set_module_thread_.join(); - robotis_controller_msgs::JointCtrlModule modules; + humanoid_robot_controller_msgs::JointCtrlModule modules; modules.joint_name = req.joint_name; modules.module_name = req.module_name; - robotis_controller_msgs::JointCtrlModule::ConstPtr msg_ptr(new robotis_controller_msgs::JointCtrlModule(modules)); + humanoid_robot_controller_msgs::JointCtrlModule::ConstPtr msg_ptr(new humanoid_robot_controller_msgs::JointCtrlModule(modules)); if (modules.joint_name.size() != modules.module_name.size()) return false; @@ -1846,7 +1846,7 @@ bool RobotisController::setJointCtrlModuleService(robotis_controller_msgs::SetJo return true; } -bool RobotisController::setCtrlModuleService(robotis_controller_msgs::SetModule::Request &req, robotis_controller_msgs::SetModule::Response &res) +bool RobotisController::setCtrlModuleService(humanoid_robot_controller_msgs::SetModule::Request &req, humanoid_robot_controller_msgs::SetModule::Response &res) { if(set_module_thread_.joinable()) set_module_thread_.join(); @@ -1861,14 +1861,14 @@ bool RobotisController::setCtrlModuleService(robotis_controller_msgs::SetModule: return true; } -bool RobotisController::loadOffsetService(robotis_controller_msgs::LoadOffset::Request &req, robotis_controller_msgs::LoadOffset::Response &res) +bool RobotisController::loadOffsetService(humanoid_robot_controller_msgs::LoadOffset::Request &req, humanoid_robot_controller_msgs::LoadOffset::Response &res) { loadOffset((std::string)req.file_path); res.result = true; return true; } -void RobotisController::setJointCtrlModuleThread(const robotis_controller_msgs::JointCtrlModule::ConstPtr &msg) +void RobotisController::setJointCtrlModuleThread(const humanoid_robot_controller_msgs::JointCtrlModule::ConstPtr &msg) { // stop module list std::list _stop_modules; @@ -2047,7 +2047,7 @@ void RobotisController::setJointCtrlModuleThread(const robotis_controller_msgs:: queue_mutex_.unlock(); // publish current module - robotis_controller_msgs::JointCtrlModule _current_module_msg; + humanoid_robot_controller_msgs::JointCtrlModule _current_module_msg; for(std::map::iterator _dxl_iter = robot_->dxls_.begin(); _dxl_iter != robot_->dxls_.end(); ++_dxl_iter) { _current_module_msg.joint_name.push_back(_dxl_iter->first); @@ -2263,7 +2263,7 @@ void RobotisController::setCtrlModuleThread(std::string ctrl_module) queue_mutex_.unlock(); // publish current module - robotis_controller_msgs::JointCtrlModule current_module_msg; + humanoid_robot_controller_msgs::JointCtrlModule current_module_msg; for (auto& dxl_iter : robot_->dxls_) { current_module_msg.joint_name.push_back(dxl_iter.first); diff --git a/robotis_device/CHANGELOG.rst b/humanoid_robot_device/CHANGELOG.rst similarity index 97% rename from robotis_device/CHANGELOG.rst rename to humanoid_robot_device/CHANGELOG.rst index a8a07fd..7dcdede 100755 --- a/robotis_device/CHANGELOG.rst +++ b/humanoid_robot_device/CHANGELOG.rst @@ -1,5 +1,5 @@ ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package robotis_device +Changelog for package humanoid_robot_device ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 0.3.1 (2023-09-27) @@ -73,7 +73,7 @@ Changelog for package robotis_device * bug fixed (position pid gain & velocity pid gain sync write). * added velocity_to_value_ratio to DXL Pro-H series. * added velocity p/i/d gain and position i/d gain sync_write code. -* fixed robotis_device build_depend. +* fixed humanoid_robot_device build_depend. * added XM-430-W210 / XM-430-W350 device file. * rename (present_current\_ -> present_torque\_) * modified torque control code diff --git a/robotis_device/CMakeLists.txt b/humanoid_robot_device/CMakeLists.txt similarity index 84% rename from robotis_device/CMakeLists.txt rename to humanoid_robot_device/CMakeLists.txt index 0eb6656..cf0e895 100755 --- a/robotis_device/CMakeLists.txt +++ b/humanoid_robot_device/CMakeLists.txt @@ -2,7 +2,7 @@ # Set minimum required version of cmake, project name and compile options ################################################################################ cmake_minimum_required(VERSION 3.0.2) -project(robotis_device) +project(humanoid_robot_device) ################################################################################ # Find catkin packages and libraries for catkin and system dependencies @@ -29,7 +29,7 @@ find_package(catkin REQUIRED COMPONENTS ################################################################################ catkin_package( INCLUDE_DIRS include - LIBRARIES robotis_device + LIBRARIES humanoid_robot_device CATKIN_DEPENDS roscpp dynamixel_sdk @@ -43,18 +43,18 @@ include_directories( ${catkin_INCLUDE_DIRS} ) -add_library(robotis_device - src/robotis_device/robot.cpp - src/robotis_device/sensor.cpp - src/robotis_device/dynamixel.cpp +add_library(humanoid_robot_device + src/humanoid_robot_device/robot.cpp + src/humanoid_robot_device/sensor.cpp + src/humanoid_robot_device/dynamixel.cpp ) -add_dependencies(robotis_device ${catkin_EXPORTED_TARGETS}) -target_link_libraries(robotis_device ${catkin_LIBRARIES}) +add_dependencies(humanoid_robot_device ${catkin_EXPORTED_TARGETS}) +target_link_libraries(humanoid_robot_device ${catkin_LIBRARIES}) ################################################################################ # Install ################################################################################ -install(TARGETS robotis_device +install(TARGETS humanoid_robot_device ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} diff --git a/robotis_device/devices/dynamixel/GRIPPER.device b/humanoid_robot_device/devices/dynamixel/GRIPPER.device similarity index 100% rename from robotis_device/devices/dynamixel/GRIPPER.device rename to humanoid_robot_device/devices/dynamixel/GRIPPER.device diff --git a/robotis_device/devices/dynamixel/GRIPPER_TORQUE.device b/humanoid_robot_device/devices/dynamixel/GRIPPER_TORQUE.device similarity index 100% rename from robotis_device/devices/dynamixel/GRIPPER_TORQUE.device rename to humanoid_robot_device/devices/dynamixel/GRIPPER_TORQUE.device diff --git a/robotis_device/devices/dynamixel/H42-20-S300-R(A).device b/humanoid_robot_device/devices/dynamixel/H42-20-S300-R(A).device similarity index 100% rename from robotis_device/devices/dynamixel/H42-20-S300-R(A).device rename to humanoid_robot_device/devices/dynamixel/H42-20-S300-R(A).device diff --git a/robotis_device/devices/dynamixel/H42-20-S300-R.device b/humanoid_robot_device/devices/dynamixel/H42-20-S300-R.device similarity index 100% rename from robotis_device/devices/dynamixel/H42-20-S300-R.device rename to humanoid_robot_device/devices/dynamixel/H42-20-S300-R.device diff --git a/robotis_device/devices/dynamixel/H42P-020-S300-R.device b/humanoid_robot_device/devices/dynamixel/H42P-020-S300-R.device similarity index 100% rename from robotis_device/devices/dynamixel/H42P-020-S300-R.device rename to humanoid_robot_device/devices/dynamixel/H42P-020-S300-R.device diff --git a/robotis_device/devices/dynamixel/H54-100-B210-R-NR.device b/humanoid_robot_device/devices/dynamixel/H54-100-B210-R-NR.device similarity index 100% rename from robotis_device/devices/dynamixel/H54-100-B210-R-NR.device rename to humanoid_robot_device/devices/dynamixel/H54-100-B210-R-NR.device diff --git a/robotis_device/devices/dynamixel/H54-100-S500-R(A).device b/humanoid_robot_device/devices/dynamixel/H54-100-S500-R(A).device similarity index 100% rename from robotis_device/devices/dynamixel/H54-100-S500-R(A).device rename to humanoid_robot_device/devices/dynamixel/H54-100-S500-R(A).device diff --git a/robotis_device/devices/dynamixel/H54-100-S500-R.device b/humanoid_robot_device/devices/dynamixel/H54-100-S500-R.device similarity index 100% rename from robotis_device/devices/dynamixel/H54-100-S500-R.device rename to humanoid_robot_device/devices/dynamixel/H54-100-S500-R.device diff --git a/robotis_device/devices/dynamixel/H54-200-B500-R.device b/humanoid_robot_device/devices/dynamixel/H54-200-B500-R.device similarity index 100% rename from robotis_device/devices/dynamixel/H54-200-B500-R.device rename to humanoid_robot_device/devices/dynamixel/H54-200-B500-R.device diff --git a/robotis_device/devices/dynamixel/H54-200-S500-R(A).device b/humanoid_robot_device/devices/dynamixel/H54-200-S500-R(A).device similarity index 100% rename from robotis_device/devices/dynamixel/H54-200-S500-R(A).device rename to humanoid_robot_device/devices/dynamixel/H54-200-S500-R(A).device diff --git a/robotis_device/devices/dynamixel/H54-200-S500-R.device b/humanoid_robot_device/devices/dynamixel/H54-200-S500-R.device similarity index 100% rename from robotis_device/devices/dynamixel/H54-200-S500-R.device rename to humanoid_robot_device/devices/dynamixel/H54-200-S500-R.device diff --git a/robotis_device/devices/dynamixel/H54P-100-S500-R.device b/humanoid_robot_device/devices/dynamixel/H54P-100-S500-R.device similarity index 100% rename from robotis_device/devices/dynamixel/H54P-100-S500-R.device rename to humanoid_robot_device/devices/dynamixel/H54P-100-S500-R.device diff --git a/robotis_device/devices/dynamixel/H54P-200-B500-R.device b/humanoid_robot_device/devices/dynamixel/H54P-200-B500-R.device similarity index 100% rename from robotis_device/devices/dynamixel/H54P-200-B500-R.device rename to humanoid_robot_device/devices/dynamixel/H54P-200-B500-R.device diff --git a/robotis_device/devices/dynamixel/H54P-200-S500-R.device b/humanoid_robot_device/devices/dynamixel/H54P-200-S500-R.device similarity index 100% rename from robotis_device/devices/dynamixel/H54P-200-S500-R.device rename to humanoid_robot_device/devices/dynamixel/H54P-200-S500-R.device diff --git a/robotis_device/devices/dynamixel/L42-10-S300-R.device b/humanoid_robot_device/devices/dynamixel/L42-10-S300-R.device similarity index 100% rename from robotis_device/devices/dynamixel/L42-10-S300-R.device rename to humanoid_robot_device/devices/dynamixel/L42-10-S300-R.device diff --git a/robotis_device/devices/dynamixel/L54-30-S400-R.device b/humanoid_robot_device/devices/dynamixel/L54-30-S400-R.device similarity index 100% rename from robotis_device/devices/dynamixel/L54-30-S400-R.device rename to humanoid_robot_device/devices/dynamixel/L54-30-S400-R.device diff --git a/robotis_device/devices/dynamixel/L54-30-S500-R.device b/humanoid_robot_device/devices/dynamixel/L54-30-S500-R.device similarity index 100% rename from robotis_device/devices/dynamixel/L54-30-S500-R.device rename to humanoid_robot_device/devices/dynamixel/L54-30-S500-R.device diff --git a/robotis_device/devices/dynamixel/L54-50-S290-R.device b/humanoid_robot_device/devices/dynamixel/L54-50-S290-R.device similarity index 100% rename from robotis_device/devices/dynamixel/L54-50-S290-R.device rename to humanoid_robot_device/devices/dynamixel/L54-50-S290-R.device diff --git a/robotis_device/devices/dynamixel/L54-50-S500-R.device b/humanoid_robot_device/devices/dynamixel/L54-50-S500-R.device similarity index 100% rename from robotis_device/devices/dynamixel/L54-50-S500-R.device rename to humanoid_robot_device/devices/dynamixel/L54-50-S500-R.device diff --git a/robotis_device/devices/dynamixel/M42-10-S260-R.device b/humanoid_robot_device/devices/dynamixel/M42-10-S260-R.device similarity index 100% rename from robotis_device/devices/dynamixel/M42-10-S260-R.device rename to humanoid_robot_device/devices/dynamixel/M42-10-S260-R.device diff --git a/robotis_device/devices/dynamixel/M54-40-S250-R.device b/humanoid_robot_device/devices/dynamixel/M54-40-S250-R.device similarity index 100% rename from robotis_device/devices/dynamixel/M54-40-S250-R.device rename to humanoid_robot_device/devices/dynamixel/M54-40-S250-R.device diff --git a/robotis_device/devices/dynamixel/M54-60-S250-R.device b/humanoid_robot_device/devices/dynamixel/M54-60-S250-R.device similarity index 100% rename from robotis_device/devices/dynamixel/M54-60-S250-R.device rename to humanoid_robot_device/devices/dynamixel/M54-60-S250-R.device diff --git a/robotis_device/devices/dynamixel/MX-106.device b/humanoid_robot_device/devices/dynamixel/MX-106.device similarity index 100% rename from robotis_device/devices/dynamixel/MX-106.device rename to humanoid_robot_device/devices/dynamixel/MX-106.device diff --git a/robotis_device/devices/dynamixel/MX-28.device b/humanoid_robot_device/devices/dynamixel/MX-28.device similarity index 100% rename from robotis_device/devices/dynamixel/MX-28.device rename to humanoid_robot_device/devices/dynamixel/MX-28.device diff --git a/robotis_device/devices/dynamixel/MX-64.device b/humanoid_robot_device/devices/dynamixel/MX-64.device similarity index 100% rename from robotis_device/devices/dynamixel/MX-64.device rename to humanoid_robot_device/devices/dynamixel/MX-64.device diff --git a/robotis_device/devices/dynamixel/PH42-020-S300-R.device b/humanoid_robot_device/devices/dynamixel/PH42-020-S300-R.device similarity index 100% rename from robotis_device/devices/dynamixel/PH42-020-S300-R.device rename to humanoid_robot_device/devices/dynamixel/PH42-020-S300-R.device diff --git a/robotis_device/devices/dynamixel/PH54-100-S500-R.device b/humanoid_robot_device/devices/dynamixel/PH54-100-S500-R.device similarity index 100% rename from robotis_device/devices/dynamixel/PH54-100-S500-R.device rename to humanoid_robot_device/devices/dynamixel/PH54-100-S500-R.device diff --git a/robotis_device/devices/dynamixel/PH54-200-S500-R.device b/humanoid_robot_device/devices/dynamixel/PH54-200-S500-R.device similarity index 100% rename from robotis_device/devices/dynamixel/PH54-200-S500-R.device rename to humanoid_robot_device/devices/dynamixel/PH54-200-S500-R.device diff --git a/robotis_device/devices/dynamixel/RH-P12-RN(A).device b/humanoid_robot_device/devices/dynamixel/RH-P12-RN(A).device similarity index 100% rename from robotis_device/devices/dynamixel/RH-P12-RN(A).device rename to humanoid_robot_device/devices/dynamixel/RH-P12-RN(A).device diff --git a/robotis_device/devices/dynamixel/RH-P12-RN.device b/humanoid_robot_device/devices/dynamixel/RH-P12-RN.device similarity index 100% rename from robotis_device/devices/dynamixel/RH-P12-RN.device rename to humanoid_robot_device/devices/dynamixel/RH-P12-RN.device diff --git a/robotis_device/devices/dynamixel/XM-430.device b/humanoid_robot_device/devices/dynamixel/XM-430.device similarity index 100% rename from robotis_device/devices/dynamixel/XM-430.device rename to humanoid_robot_device/devices/dynamixel/XM-430.device diff --git a/robotis_device/devices/dynamixel/XM430-W210.device b/humanoid_robot_device/devices/dynamixel/XM430-W210.device similarity index 100% rename from robotis_device/devices/dynamixel/XM430-W210.device rename to humanoid_robot_device/devices/dynamixel/XM430-W210.device diff --git a/robotis_device/devices/dynamixel/XM430-W350.device b/humanoid_robot_device/devices/dynamixel/XM430-W350.device similarity index 100% rename from robotis_device/devices/dynamixel/XM430-W350.device rename to humanoid_robot_device/devices/dynamixel/XM430-W350.device diff --git a/robotis_device/devices/dynamixel/XM540-W150.device b/humanoid_robot_device/devices/dynamixel/XM540-W150.device similarity index 100% rename from robotis_device/devices/dynamixel/XM540-W150.device rename to humanoid_robot_device/devices/dynamixel/XM540-W150.device diff --git a/robotis_device/devices/dynamixel/XM540-W270.device b/humanoid_robot_device/devices/dynamixel/XM540-W270.device similarity index 100% rename from robotis_device/devices/dynamixel/XM540-W270.device rename to humanoid_robot_device/devices/dynamixel/XM540-W270.device diff --git a/robotis_device/devices/sensor/CM-740.device b/humanoid_robot_device/devices/sensor/CM-740.device similarity index 100% rename from robotis_device/devices/sensor/CM-740.device rename to humanoid_robot_device/devices/sensor/CM-740.device diff --git a/robotis_device/devices/sensor/OPEN-CR.device b/humanoid_robot_device/devices/sensor/OPEN-CR.device similarity index 100% rename from robotis_device/devices/sensor/OPEN-CR.device rename to humanoid_robot_device/devices/sensor/OPEN-CR.device diff --git a/robotis_device/include/robotis_device/control_table_item.h b/humanoid_robot_device/include/humanoid_robot_device/control_table_item.h similarity index 92% rename from robotis_device/include/robotis_device/control_table_item.h rename to humanoid_robot_device/include/humanoid_robot_device/control_table_item.h index 7249a90..971c3d2 100755 --- a/robotis_device/include/robotis_device/control_table_item.h +++ b/humanoid_robot_device/include/humanoid_robot_device/control_table_item.h @@ -26,7 +26,7 @@ #include -namespace robotis_framework { +namespace humanoid_robot_framework { enum AccessType { Read, ReadWrite }; @@ -49,6 +49,6 @@ public: is_signed_(false) {} }; -} // namespace robotis_framework +} // namespace humanoid_robot_framework #endif /* ROBOTIS_DEVICE_CONTROL_TABLE_ITEM_H_ */ diff --git a/robotis_device/include/robotis_device/device.h b/humanoid_robot_device/include/humanoid_robot_device/device.h similarity index 93% rename from robotis_device/include/robotis_device/device.h rename to humanoid_robot_device/include/humanoid_robot_device/device.h index b3b2936..2a74381 100755 --- a/robotis_device/include/robotis_device/device.h +++ b/humanoid_robot_device/include/humanoid_robot_device/device.h @@ -31,7 +31,7 @@ #include "control_table_item.h" -namespace robotis_framework +namespace humanoid_robot_framework { class Device diff --git a/robotis_device/include/robotis_device/dynamixel.h b/humanoid_robot_device/include/humanoid_robot_device/dynamixel.h similarity index 95% rename from robotis_device/include/robotis_device/dynamixel.h rename to humanoid_robot_device/include/humanoid_robot_device/dynamixel.h index 43e8981..36adf26 100755 --- a/robotis_device/include/robotis_device/dynamixel.h +++ b/humanoid_robot_device/include/humanoid_robot_device/dynamixel.h @@ -33,7 +33,7 @@ #include "device.h" #include "dynamixel_state.h" -namespace robotis_framework +namespace humanoid_robot_framework { class Dynamixel : public Device diff --git a/robotis_device/include/robotis_device/dynamixel_state.h b/humanoid_robot_device/include/humanoid_robot_device/dynamixel_state.h similarity index 94% rename from robotis_device/include/robotis_device/dynamixel_state.h rename to humanoid_robot_device/include/humanoid_robot_device/dynamixel_state.h index 05116c6..f1bae5b 100755 --- a/robotis_device/include/robotis_device/dynamixel_state.h +++ b/humanoid_robot_device/include/humanoid_robot_device/dynamixel_state.h @@ -31,7 +31,7 @@ #define INDIRECT_DATA_1 "indirect_data_1" #define INDIRECT_ADDRESS_1 "indirect_address_1" -namespace robotis_framework +namespace humanoid_robot_framework { class DynamixelState diff --git a/robotis_device/include/robotis_device/robot.h b/humanoid_robot_device/include/humanoid_robot_device/robot.h similarity index 95% rename from robotis_device/include/robotis_device/robot.h rename to humanoid_robot_device/include/humanoid_robot_device/robot.h index 58b7a77..843ef8f 100755 --- a/robotis_device/include/robotis_device/robot.h +++ b/humanoid_robot_device/include/humanoid_robot_device/robot.h @@ -43,7 +43,7 @@ #define DEFAULT_CONTROL_CYCLE 8 // milliseconds -namespace robotis_framework +namespace humanoid_robot_framework { class Robot diff --git a/robotis_device/include/robotis_device/sensor.h b/humanoid_robot_device/include/humanoid_robot_device/sensor.h similarity index 93% rename from robotis_device/include/robotis_device/sensor.h rename to humanoid_robot_device/include/humanoid_robot_device/sensor.h index dc98eaa..973dba3 100755 --- a/robotis_device/include/robotis_device/sensor.h +++ b/humanoid_robot_device/include/humanoid_robot_device/sensor.h @@ -32,7 +32,7 @@ #include "sensor_state.h" #include "control_table_item.h" -namespace robotis_framework +namespace humanoid_robot_framework { class Sensor : public Device diff --git a/robotis_device/include/robotis_device/sensor_state.h b/humanoid_robot_device/include/humanoid_robot_device/sensor_state.h similarity index 93% rename from robotis_device/include/robotis_device/sensor_state.h rename to humanoid_robot_device/include/humanoid_robot_device/sensor_state.h index 2cfc98d..e28555c 100755 --- a/robotis_device/include/robotis_device/sensor_state.h +++ b/humanoid_robot_device/include/humanoid_robot_device/sensor_state.h @@ -27,7 +27,7 @@ #include "time_stamp.h" -namespace robotis_framework +namespace humanoid_robot_framework { class SensorState diff --git a/robotis_device/include/robotis_device/time_stamp.h b/humanoid_robot_device/include/humanoid_robot_device/time_stamp.h similarity index 93% rename from robotis_device/include/robotis_device/time_stamp.h rename to humanoid_robot_device/include/humanoid_robot_device/time_stamp.h index 8662ad7..97ed81b 100755 --- a/robotis_device/include/robotis_device/time_stamp.h +++ b/humanoid_robot_device/include/humanoid_robot_device/time_stamp.h @@ -25,7 +25,7 @@ #define ROBOTIS_DEVICE_TIME_STAMP_H_ -namespace robotis_framework +namespace humanoid_robot_framework { class TimeStamp { diff --git a/robotis_device/package.xml b/humanoid_robot_device/package.xml similarity index 89% rename from robotis_device/package.xml rename to humanoid_robot_device/package.xml index 30fda2e..c401d10 100755 --- a/robotis_device/package.xml +++ b/humanoid_robot_device/package.xml @@ -1,11 +1,11 @@ - robotis_device + humanoid_robot_device 0.3.0 The package that manages device information of ROBOTIS robots. This package is used when reading device information with the robot information file - from the robotis_controller package. + from the humanoid_robot_controller package. Apache 2.0 Ronaldson Bellande diff --git a/robotis_device/src/robotis_device/dynamixel.cpp b/humanoid_robot_device/src/humanoid_robot_device/dynamixel.cpp similarity index 97% rename from robotis_device/src/robotis_device/dynamixel.cpp rename to humanoid_robot_device/src/humanoid_robot_device/dynamixel.cpp index beda733..8c346c5 100755 --- a/robotis_device/src/robotis_device/dynamixel.cpp +++ b/humanoid_robot_device/src/humanoid_robot_device/dynamixel.cpp @@ -21,9 +21,9 @@ * Author: zerom */ -#include "robotis_device/dynamixel.h" +#include "humanoid_robot_device/dynamixel.h" -using namespace robotis_framework; +using namespace humanoid_robot_framework; Dynamixel::Dynamixel(int id, std::string model_name, float protocol_version) : ctrl_module_name_("none"), torque_to_current_value_ratio_(1.0), diff --git a/robotis_device/src/robotis_device/robot.cpp b/humanoid_robot_device/src/humanoid_robot_device/robot.cpp similarity index 97% rename from robotis_device/src/robotis_device/robot.cpp rename to humanoid_robot_device/src/humanoid_robot_device/robot.cpp index ea738f1..6362c5b 100755 --- a/robotis_device/src/robotis_device/robot.cpp +++ b/humanoid_robot_device/src/humanoid_robot_device/robot.cpp @@ -25,9 +25,9 @@ #include #include -#include "robotis_device/robot.h" +#include "humanoid_robot_device/robot.h" -using namespace robotis_framework; +using namespace humanoid_robot_framework; static inline std::string <rim(std::string &s) { s.erase(s.begin(), diff --git a/robotis_device/src/robotis_device/sensor.cpp b/humanoid_robot_device/src/humanoid_robot_device/sensor.cpp similarity index 90% rename from robotis_device/src/robotis_device/sensor.cpp rename to humanoid_robot_device/src/humanoid_robot_device/sensor.cpp index 096de2a..166c6dc 100755 --- a/robotis_device/src/robotis_device/sensor.cpp +++ b/humanoid_robot_device/src/humanoid_robot_device/sensor.cpp @@ -21,9 +21,9 @@ * Author: zerom */ -#include "robotis_device/sensor.h" +#include "humanoid_robot_device/sensor.h" -using namespace robotis_framework; +using namespace humanoid_robot_framework; Sensor::Sensor(int id, std::string model_name, float protocol_version) { diff --git a/robotis_framework/CHANGELOG.rst b/humanoid_robot_framework/CHANGELOG.rst similarity index 93% rename from robotis_framework/CHANGELOG.rst rename to humanoid_robot_framework/CHANGELOG.rst index fdf8a77..6e1adb8 100755 --- a/robotis_framework/CHANGELOG.rst +++ b/humanoid_robot_framework/CHANGELOG.rst @@ -1,5 +1,5 @@ ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package robotis_framework +Changelog for package humanoid_robot_framework ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 0.3.1 (2023-09-27) @@ -45,7 +45,7 @@ Changelog for package robotis_framework 0.2.5 (2017-06-09) ------------------ -* updated for yaml-cpp dependencies (robotis_controller) +* updated for yaml-cpp dependencies (humanoid_robot_controller) * Contributors: SCH 0.2.4 (2017-06-07) @@ -61,7 +61,7 @@ Changelog for package robotis_framework 0.2.2 (2017-04-24) ------------------ * added a deivce: OpenCR -* updated robotis_controller.cpp +* updated humanoid_robot_controller.cpp * changed to read control cycle from .robot file * Contributors: Zerom, Kayman diff --git a/robotis_framework/CMakeLists.txt b/humanoid_robot_framework/CMakeLists.txt similarity index 72% rename from robotis_framework/CMakeLists.txt rename to humanoid_robot_framework/CMakeLists.txt index 5777d65..6338953 100644 --- a/robotis_framework/CMakeLists.txt +++ b/humanoid_robot_framework/CMakeLists.txt @@ -1,4 +1,4 @@ cmake_minimum_required(VERSION 3.0.2) -project(robotis_framework) +project(humanoid_robot_framework) find_package(catkin REQUIRED) catkin_metapackage() diff --git a/humanoid_robot_framework/package.xml b/humanoid_robot_framework/package.xml new file mode 100755 index 0000000..1909e5b --- /dev/null +++ b/humanoid_robot_framework/package.xml @@ -0,0 +1,26 @@ + + + humanoid_robot_framework + 0.3.0 + ROS packages for the humanoid_robot_framework (meta package) + Apache 2.0 + Ronaldson Bellande + + catkin + + humanoid_robot_controller + humanoid_robot_device + humanoid_robot_framework_common + + humanoid_robot_controller + humanoid_robot_device + humanoid_robot_framework_common + + humanoid_robot_controller + humanoid_robot_device + humanoid_robot_framework_common + + + + + diff --git a/robotis_framework_common/CHANGELOG.rst b/humanoid_robot_framework_common/CHANGELOG.rst similarity index 97% rename from robotis_framework_common/CHANGELOG.rst rename to humanoid_robot_framework_common/CHANGELOG.rst index ed53e19..cc736f6 100755 --- a/robotis_framework_common/CHANGELOG.rst +++ b/humanoid_robot_framework_common/CHANGELOG.rst @@ -1,5 +1,5 @@ ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package robotis_framework_common +Changelog for package humanoid_robot_framework_common ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 0.3.1 (2023-09-27) diff --git a/robotis_framework_common/CMakeLists.txt b/humanoid_robot_framework_common/CMakeLists.txt similarity index 91% rename from robotis_framework_common/CMakeLists.txt rename to humanoid_robot_framework_common/CMakeLists.txt index e05df3c..0a25d0a 100755 --- a/robotis_framework_common/CMakeLists.txt +++ b/humanoid_robot_framework_common/CMakeLists.txt @@ -2,14 +2,14 @@ # Set minimum required version of cmake, project name and compile options ################################################################################ cmake_minimum_required(VERSION 2.8.3) -project(robotis_framework_common) +project(humanoid_robot_framework_common) ################################################################################ # Find catkin packages and libraries for catkin and system dependencies ################################################################################ find_package(catkin REQUIRED COMPONENTS roscpp - robotis_device + humanoid_robot_device ) ################################################################################ @@ -30,7 +30,7 @@ find_package(catkin REQUIRED COMPONENTS catkin_package( INCLUDE_DIRS include LIBRARIES ${PROJECT_NAME} - CATKIN_DEPENDS roscpp robotis_device + CATKIN_DEPENDS roscpp humanoid_robot_device ) ################################################################################ @@ -51,7 +51,7 @@ set_target_properties(${PROJECT_NAME} PROPERTIES LINKER_LANGUAGE CXX) ################################################################################ # Install ################################################################################ -install(TARGETS robotis_framework_common +install(TARGETS humanoid_robot_framework_common ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} diff --git a/robotis_framework_common/include/robotis_framework_common/motion_module.h b/humanoid_robot_framework_common/include/humanoid_robot_framework_common/motion_module.h similarity index 90% rename from robotis_framework_common/include/robotis_framework_common/motion_module.h rename to humanoid_robot_framework_common/include/humanoid_robot_framework_common/motion_module.h index b18d858..6db3e6e 100755 --- a/robotis_framework_common/include/robotis_framework_common/motion_module.h +++ b/humanoid_robot_framework_common/include/humanoid_robot_framework_common/motion_module.h @@ -29,10 +29,10 @@ #include #include "singleton.h" -#include "robotis_device/robot.h" -#include "robotis_device/dynamixel.h" +#include "humanoid_robot_device/robot.h" +#include "humanoid_robot_device/dynamixel.h" -namespace robotis_framework +namespace humanoid_robot_framework { enum ControlMode diff --git a/robotis_framework_common/include/robotis_framework_common/sensor_module.h b/humanoid_robot_framework_common/include/humanoid_robot_framework_common/sensor_module.h similarity index 88% rename from robotis_framework_common/include/robotis_framework_common/sensor_module.h rename to humanoid_robot_framework_common/include/humanoid_robot_framework_common/sensor_module.h index 356d14b..fe1a8c3 100755 --- a/robotis_framework_common/include/robotis_framework_common/sensor_module.h +++ b/humanoid_robot_framework_common/include/humanoid_robot_framework_common/sensor_module.h @@ -29,10 +29,10 @@ #include #include "singleton.h" -#include "robotis_device/robot.h" -#include "robotis_device/dynamixel.h" +#include "humanoid_robot_device/robot.h" +#include "humanoid_robot_device/dynamixel.h" -namespace robotis_framework +namespace humanoid_robot_framework { class SensorModule diff --git a/robotis_framework_common/include/robotis_framework_common/singleton.h b/humanoid_robot_framework_common/include/humanoid_robot_framework_common/singleton.h similarity index 93% rename from robotis_framework_common/include/robotis_framework_common/singleton.h rename to humanoid_robot_framework_common/include/humanoid_robot_framework_common/singleton.h index 992e2f3..287de76 100755 --- a/robotis_framework_common/include/robotis_framework_common/singleton.h +++ b/humanoid_robot_framework_common/include/humanoid_robot_framework_common/singleton.h @@ -25,7 +25,7 @@ #define ROBOTIS_FRAMEWORK_COMMON_SINGLETON_H_ -namespace robotis_framework +namespace humanoid_robot_framework { template diff --git a/robotis_framework_common/package.xml b/humanoid_robot_framework_common/package.xml similarity index 68% rename from robotis_framework_common/package.xml rename to humanoid_robot_framework_common/package.xml index 1518643..47297e1 100755 --- a/robotis_framework_common/package.xml +++ b/humanoid_robot_framework_common/package.xml @@ -1,6 +1,6 @@ - robotis_framework_common + humanoid_robot_framework_common 0.2.9 The package contains commonly used Headers for the ROBOTIS Framework. Apache 2.0 @@ -9,12 +9,12 @@ catkin roscpp - robotis_device + humanoid_robot_device roscpp - robotis_device + humanoid_robot_device roscpp - robotis_device + humanoid_robot_device diff --git a/push.sh b/push.sh new file mode 100755 index 0000000..3435685 --- /dev/null +++ b/push.sh @@ -0,0 +1,4 @@ +#!/bin/bash + +# Git push what is already in the repository +git pull --no-edit; git fetch; git add .; git commit -am "latest pushes"; git push diff --git a/repository_recal.sh b/repository_recal.sh new file mode 100755 index 0000000..4c7fd4f --- /dev/null +++ b/repository_recal.sh @@ -0,0 +1,110 @@ +#!/bin/bash + +# Git push what is already in the repository +git pull --no-edit; git fetch; git add .; git commit -am "latest pushes"; git push + +# Get the current directory +current_dir=$(pwd) + +# Read the remote repository URL from .git/config +remote_repo_url=$(git -C "$current_dir" config --get remote.origin.url) + +# Create a temporary directory for cloning the repository +temp_dir=$(mktemp -d) + +# Clone the repository into the temporary directory without using local references +git clone --no-local "$current_dir" "$temp_dir" + +# Switch to the temporary directory +cd "$temp_dir" + +# Create a temporary file to store the file list +tmp_file=$(mktemp) +# Create a temporary file to store the processed commits +processed_commits_file=$(mktemp) + +# Function to check if a commit has already been processed +is_commit_processed() { + local commit="$1" + + # Check if the commit is already processed + grep -Fxq "$commit" "$processed_commits_file" +} + +# Function to mark a commit as processed +mark_commit_processed() { + local commit="$1" + + # Mark the commit as processed + echo "$commit" >> "$processed_commits_file" +} + +# Function to check if a file or folder exists in the repository +file_exists_in_repo() { + local file_path="$1" + + # Check if the file or folder exists in the repository + git ls-tree --name-only -r HEAD | grep -Fxq "$file_path" +} + +# Function to process the files and folders in each commit +process_commit_files() { + local commit="$1" + + # Check if the commit has already been processed + if is_commit_processed "$commit"; then + echo "Commit $commit already processed. Skipping..." + return + fi + + # Get the list of files and folders in the commit (including subfolders) + git ls-tree --name-only -r "$commit" >> "$tmp_file" + + # Process each file or folder in the commit + while IFS= read -r line + do + # Check if the file or folder exists in the current push + if file_exists_in_repo "$line"; then + echo "Keeping: $line" + else + echo "Deleting: $line" + git filter-repo --path "$line" --invert-paths + fi + done < "$tmp_file" + + # Mark the commit as processed + mark_commit_processed "$commit" + + # Clear the temporary file + > "$tmp_file" +} + +# Iterate over each commit in the repository +git rev-list --all | while IFS= read -r commit +do + process_commit_files "$commit" +done + +# Push the filtered changes to the original repository +git remote add origin "$remote_repo_url" +git push --force origin main + +# Perform a history rewrite to remove the filtered files +git filter-repo --force + +# Fetch the changes from the remote repository +git -C "$current_dir" fetch origin + +# Merge the remote changes into the local repository +git -C "$current_dir" merge origin/main --no-edit + +# Update the local repository and reduce the size of .git if needed +git -C "$current_dir" gc --prune=now +git -C "$current_dir" reflog expire --expire=now --all +git -C "$current_dir" repack -ad + +# Clean up temporary files and directories +cd "$current_dir" +rm -rf "$temp_dir" +rm "$tmp_file" +rm "$processed_commits_file" diff --git a/robotis_framework/package.xml b/robotis_framework/package.xml deleted file mode 100755 index 9a78263..0000000 --- a/robotis_framework/package.xml +++ /dev/null @@ -1,26 +0,0 @@ - - - robotis_framework - 0.3.0 - ROS packages for the robotis_framework (meta package) - Apache 2.0 - Ronaldson Bellande - - catkin - - robotis_controller - robotis_device - robotis_framework_common - - robotis_controller - robotis_device - robotis_framework_common - - robotis_controller - robotis_device - robotis_framework_common - - - - - From 6007876bec667598e4aa3cc9a715a4da8e611325 Mon Sep 17 00:00:00 2001 From: RonaldsonBellande Date: Thu, 28 Sep 2023 09:50:36 -0400 Subject: [PATCH 175/238] latest pushes --- .travis.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.travis.yml b/.travis.yml index 9696b6b..e1e689a 100644 --- a/.travis.yml +++ b/.travis.yml @@ -18,7 +18,7 @@ notifications: - ronaldsonbellande@gmail.com env: matrix: - - ROS_DISTRO=noetic ROS_REPO=ros-shadow-fixed UPSTREAM_WORKSPACE=file OS_NAME=ubuntu OS_CODE_NAME=focal $ROSINSTALL_FILENAME=".robotis_framework.rosinstall" + - ROS_DISTRO=noetic ROS_REPO=ros-shadow-fixed UPSTREAM_WORKSPACE=file OS_NAME=ubuntu OS_CODE_NAME=focal $ROSINSTALL_FILENAME=".humanoid_robot_framework.rosinstall" branches: only: - master From bcdbec3613569ba32f5f87968d9ec6a8731fccc0 Mon Sep 17 00:00:00 2001 From: RonaldsonBellande Date: Thu, 28 Sep 2023 10:57:28 -0400 Subject: [PATCH 176/238] latest pushes --- .gitignore | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/.gitignore b/.gitignore index 36a5c0a..a78ffcf 100755 --- a/.gitignore +++ b/.gitignore @@ -9,3 +9,13 @@ qtcreator-build *.backup *.user *.autosave + +# Scripts +init_setup.sh +repository_recal.sh +push.sh +publish.sh +ros_publish.sh +prerelease_test.sh +fix_errors.sh +replace_add_index.sh From 680112b0dae6edeea8c03be67e9f611f5f9eafa4 Mon Sep 17 00:00:00 2001 From: RonaldsonBellande Date: Thu, 28 Sep 2023 10:57:28 -0400 Subject: [PATCH 177/238] latest pushes --- .gitignore | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/.gitignore b/.gitignore index 36a5c0a..a78ffcf 100755 --- a/.gitignore +++ b/.gitignore @@ -9,3 +9,13 @@ qtcreator-build *.backup *.user *.autosave + +# Scripts +init_setup.sh +repository_recal.sh +push.sh +publish.sh +ros_publish.sh +prerelease_test.sh +fix_errors.sh +replace_add_index.sh From 1c81a0aaca096ed566625246ac4c73d00aad2ecf Mon Sep 17 00:00:00 2001 From: Ronaldson Bellande <47253433+RonaldsonBellande@users.noreply.github.com> Date: Thu, 28 Sep 2023 11:41:02 -0400 Subject: [PATCH 178/238] Delete fix_errors.sh --- fix_errors.sh | 31 ------------------------------- 1 file changed, 31 deletions(-) delete mode 100755 fix_errors.sh diff --git a/fix_errors.sh b/fix_errors.sh deleted file mode 100755 index 2ebdbee..0000000 --- a/fix_errors.sh +++ /dev/null @@ -1,31 +0,0 @@ -#!/bin/bash - -# Get the URL from .git/config -git_url=$(git config --get remote.origin.url) - -# Check if a URL is found -if [ -z "$git_url" ]; then - echo "No remote URL found in .git/config." - exit 1 -fi - -# Clone the repository into a temporary folder -git clone "$git_url" tmp_clone - -# Check if the clone was successful -if [ $? -eq 0 ]; then - # Remove the existing .git directory if it exists - if [ -d ".git" ]; then - rm -rf .git - fi - - # Copy the .git directory from the clone to the current repository - cp -r tmp_clone/.git . - - # Remove the clone directory - rm -rf tmp_clone - - echo "Repository cloned and .git directory copied successfully." -else - echo "Failed to clone the repository." -fi From 773ed606e270508db052b1161999fe078b57c102 Mon Sep 17 00:00:00 2001 From: Ronaldson Bellande <47253433+RonaldsonBellande@users.noreply.github.com> Date: Thu, 28 Sep 2023 11:41:12 -0400 Subject: [PATCH 179/238] Delete push.sh --- push.sh | 4 ---- 1 file changed, 4 deletions(-) delete mode 100755 push.sh diff --git a/push.sh b/push.sh deleted file mode 100755 index 3435685..0000000 --- a/push.sh +++ /dev/null @@ -1,4 +0,0 @@ -#!/bin/bash - -# Git push what is already in the repository -git pull --no-edit; git fetch; git add .; git commit -am "latest pushes"; git push From 6f48b563e4674c608a461ff70dd939261727fac7 Mon Sep 17 00:00:00 2001 From: Ronaldson Bellande <47253433+RonaldsonBellande@users.noreply.github.com> Date: Thu, 28 Sep 2023 11:41:23 -0400 Subject: [PATCH 180/238] Delete repository_recal.sh --- repository_recal.sh | 110 -------------------------------------------- 1 file changed, 110 deletions(-) delete mode 100755 repository_recal.sh diff --git a/repository_recal.sh b/repository_recal.sh deleted file mode 100755 index 4c7fd4f..0000000 --- a/repository_recal.sh +++ /dev/null @@ -1,110 +0,0 @@ -#!/bin/bash - -# Git push what is already in the repository -git pull --no-edit; git fetch; git add .; git commit -am "latest pushes"; git push - -# Get the current directory -current_dir=$(pwd) - -# Read the remote repository URL from .git/config -remote_repo_url=$(git -C "$current_dir" config --get remote.origin.url) - -# Create a temporary directory for cloning the repository -temp_dir=$(mktemp -d) - -# Clone the repository into the temporary directory without using local references -git clone --no-local "$current_dir" "$temp_dir" - -# Switch to the temporary directory -cd "$temp_dir" - -# Create a temporary file to store the file list -tmp_file=$(mktemp) -# Create a temporary file to store the processed commits -processed_commits_file=$(mktemp) - -# Function to check if a commit has already been processed -is_commit_processed() { - local commit="$1" - - # Check if the commit is already processed - grep -Fxq "$commit" "$processed_commits_file" -} - -# Function to mark a commit as processed -mark_commit_processed() { - local commit="$1" - - # Mark the commit as processed - echo "$commit" >> "$processed_commits_file" -} - -# Function to check if a file or folder exists in the repository -file_exists_in_repo() { - local file_path="$1" - - # Check if the file or folder exists in the repository - git ls-tree --name-only -r HEAD | grep -Fxq "$file_path" -} - -# Function to process the files and folders in each commit -process_commit_files() { - local commit="$1" - - # Check if the commit has already been processed - if is_commit_processed "$commit"; then - echo "Commit $commit already processed. Skipping..." - return - fi - - # Get the list of files and folders in the commit (including subfolders) - git ls-tree --name-only -r "$commit" >> "$tmp_file" - - # Process each file or folder in the commit - while IFS= read -r line - do - # Check if the file or folder exists in the current push - if file_exists_in_repo "$line"; then - echo "Keeping: $line" - else - echo "Deleting: $line" - git filter-repo --path "$line" --invert-paths - fi - done < "$tmp_file" - - # Mark the commit as processed - mark_commit_processed "$commit" - - # Clear the temporary file - > "$tmp_file" -} - -# Iterate over each commit in the repository -git rev-list --all | while IFS= read -r commit -do - process_commit_files "$commit" -done - -# Push the filtered changes to the original repository -git remote add origin "$remote_repo_url" -git push --force origin main - -# Perform a history rewrite to remove the filtered files -git filter-repo --force - -# Fetch the changes from the remote repository -git -C "$current_dir" fetch origin - -# Merge the remote changes into the local repository -git -C "$current_dir" merge origin/main --no-edit - -# Update the local repository and reduce the size of .git if needed -git -C "$current_dir" gc --prune=now -git -C "$current_dir" reflog expire --expire=now --all -git -C "$current_dir" repack -ad - -# Clean up temporary files and directories -cd "$current_dir" -rm -rf "$temp_dir" -rm "$tmp_file" -rm "$processed_commits_file" From 0eb37724316b199e5dd7ecd0e4daaf99825e536d Mon Sep 17 00:00:00 2001 From: RonaldsonBellande Date: Thu, 28 Sep 2023 12:01:13 -0400 Subject: [PATCH 181/238] latest pushes --- README.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/README.md b/README.md index c91aa6b..fd1e2af 100755 --- a/README.md +++ b/README.md @@ -3,6 +3,8 @@ -------------------------------------------------------------------------------------------------------- Updated Version [humanoid_robot_framework](https://github.com/Robotics-Sensors/humanoid_robot_framework) readme. +Old Version/Previous Used for Different Context [ROBOTIS-Framework](https://github.com/ROBOTIS-GIT/ROBOTIS-Framework) readme. + -------------------------------------------------------------------------------------------------------- ## Important The repository has diverged, as the old commits and codes are under the previous License and From 0a8fe2eb15452c5f9045bfcb861ab24d8e6fffa7 Mon Sep 17 00:00:00 2001 From: RonaldsonBellande Date: Thu, 28 Sep 2023 12:01:13 -0400 Subject: [PATCH 182/238] latest pushes --- README.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/README.md b/README.md index c91aa6b..fd1e2af 100755 --- a/README.md +++ b/README.md @@ -3,6 +3,8 @@ -------------------------------------------------------------------------------------------------------- Updated Version [humanoid_robot_framework](https://github.com/Robotics-Sensors/humanoid_robot_framework) readme. +Old Version/Previous Used for Different Context [ROBOTIS-Framework](https://github.com/ROBOTIS-GIT/ROBOTIS-Framework) readme. + -------------------------------------------------------------------------------------------------------- ## Important The repository has diverged, as the old commits and codes are under the previous License and From bff09955f84e472af5167ff56d68e459a3357587 Mon Sep 17 00:00:00 2001 From: RonaldsonBellande Date: Sat, 30 Sep 2023 16:51:48 -0400 Subject: [PATCH 183/238] latest pushes --- README.md | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/README.md b/README.md index fd1e2af..28df683 100755 --- a/README.md +++ b/README.md @@ -14,6 +14,11 @@ the new commits and codes are under New License Latest versions and Maintainer is on organization https://github.com/Robotics-Sensors +# USE CASE +-------------------------------------------------------------------------------------------------------- +* Every repository within our organization is a valuable resource that can be utilized for educational purposes by individuals who actively contribute to the repository or choose to become sponsors of the organization. For those who wish to use our services conversationally, the acquisition of a license and subscription from our company website is mandatory. This ensures that the services are appropriately compensated for their use. Additionally, for the 90% of services designated as private, acquiring a license and subscription can be facilitated through our company website. We encourage interested parties to visit our website to explore and procure the necessary licenses and subscriptions for the diverse range of services and products we offer. Your support and commitment enable us to maintain and enhance the quality of our offerings, contributing to a thriving collaborative environment. +-------------------------------------------------------------------------------------------------------- + ### Maintainer * Ronaldson Bellande From d481e658d6566b9de216b5062dd37d0c60c7a2d4 Mon Sep 17 00:00:00 2001 From: RonaldsonBellande Date: Sat, 30 Sep 2023 16:51:48 -0400 Subject: [PATCH 184/238] latest pushes --- README.md | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/README.md b/README.md index fd1e2af..28df683 100755 --- a/README.md +++ b/README.md @@ -14,6 +14,11 @@ the new commits and codes are under New License Latest versions and Maintainer is on organization https://github.com/Robotics-Sensors +# USE CASE +-------------------------------------------------------------------------------------------------------- +* Every repository within our organization is a valuable resource that can be utilized for educational purposes by individuals who actively contribute to the repository or choose to become sponsors of the organization. For those who wish to use our services conversationally, the acquisition of a license and subscription from our company website is mandatory. This ensures that the services are appropriately compensated for their use. Additionally, for the 90% of services designated as private, acquiring a license and subscription can be facilitated through our company website. We encourage interested parties to visit our website to explore and procure the necessary licenses and subscriptions for the diverse range of services and products we offer. Your support and commitment enable us to maintain and enhance the quality of our offerings, contributing to a thriving collaborative environment. +-------------------------------------------------------------------------------------------------------- + ### Maintainer * Ronaldson Bellande From fa3452a5dec481910ae9af96b7e58da9187ec346 Mon Sep 17 00:00:00 2001 From: RonaldsonBellande Date: Mon, 2 Oct 2023 00:25:22 -0400 Subject: [PATCH 185/238] latest pushes --- humanoid_robot_controller/CMakeLists.txt | 2 +- humanoid_robot_controller/package.xml | 2 +- humanoid_robot_device/CMakeLists.txt | 2 +- humanoid_robot_device/package.xml | 2 +- humanoid_robot_framework/CMakeLists.txt | 2 +- humanoid_robot_framework/package.xml | 2 +- humanoid_robot_framework_common/CMakeLists.txt | 2 +- humanoid_robot_framework_common/package.xml | 2 +- 8 files changed, 8 insertions(+), 8 deletions(-) diff --git a/humanoid_robot_controller/CMakeLists.txt b/humanoid_robot_controller/CMakeLists.txt index f62a080..15cb7bf 100755 --- a/humanoid_robot_controller/CMakeLists.txt +++ b/humanoid_robot_controller/CMakeLists.txt @@ -1,7 +1,7 @@ ################################################################################ # Set minimum required version of cmake, project name and compile options ################################################################################ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.8) project(humanoid_robot_controller) ## Compile as C++11, supported in ROS Kinetic and newer diff --git a/humanoid_robot_controller/package.xml b/humanoid_robot_controller/package.xml index 00106e9..3cfa98f 100755 --- a/humanoid_robot_controller/package.xml +++ b/humanoid_robot_controller/package.xml @@ -1,5 +1,5 @@ - + humanoid_robot_controller 0.3.0 humanoid_robot_controller package for ROBOTIS's platform like Manipulator-H, THORMANG and OP series diff --git a/humanoid_robot_device/CMakeLists.txt b/humanoid_robot_device/CMakeLists.txt index cf0e895..c838c1f 100755 --- a/humanoid_robot_device/CMakeLists.txt +++ b/humanoid_robot_device/CMakeLists.txt @@ -1,7 +1,7 @@ ################################################################################ # Set minimum required version of cmake, project name and compile options ################################################################################ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.8) project(humanoid_robot_device) ################################################################################ diff --git a/humanoid_robot_device/package.xml b/humanoid_robot_device/package.xml index c401d10..f072991 100755 --- a/humanoid_robot_device/package.xml +++ b/humanoid_robot_device/package.xml @@ -1,5 +1,5 @@ - + humanoid_robot_device 0.3.0 diff --git a/humanoid_robot_framework/CMakeLists.txt b/humanoid_robot_framework/CMakeLists.txt index 6338953..482cad1 100644 --- a/humanoid_robot_framework/CMakeLists.txt +++ b/humanoid_robot_framework/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.8) project(humanoid_robot_framework) find_package(catkin REQUIRED) catkin_metapackage() diff --git a/humanoid_robot_framework/package.xml b/humanoid_robot_framework/package.xml index 1909e5b..4461b84 100755 --- a/humanoid_robot_framework/package.xml +++ b/humanoid_robot_framework/package.xml @@ -1,5 +1,5 @@ - + humanoid_robot_framework 0.3.0 ROS packages for the humanoid_robot_framework (meta package) diff --git a/humanoid_robot_framework_common/CMakeLists.txt b/humanoid_robot_framework_common/CMakeLists.txt index 0a25d0a..185de24 100755 --- a/humanoid_robot_framework_common/CMakeLists.txt +++ b/humanoid_robot_framework_common/CMakeLists.txt @@ -1,7 +1,7 @@ ################################################################################ # Set minimum required version of cmake, project name and compile options ################################################################################ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.8) project(humanoid_robot_framework_common) ################################################################################ diff --git a/humanoid_robot_framework_common/package.xml b/humanoid_robot_framework_common/package.xml index 47297e1..fc18204 100755 --- a/humanoid_robot_framework_common/package.xml +++ b/humanoid_robot_framework_common/package.xml @@ -1,5 +1,5 @@ - + humanoid_robot_framework_common 0.2.9 The package contains commonly used Headers for the ROBOTIS Framework. From 00fe52d5253c7b2f2d1bef21c0d3d5c99f9f53f1 Mon Sep 17 00:00:00 2001 From: Ronaldson Bellande <47253433+RonaldsonBellande@users.noreply.github.com> Date: Tue, 3 Oct 2023 09:04:27 -0400 Subject: [PATCH 186/238] Update LICENSE --- LICENSE | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/LICENSE b/LICENSE index c0ee812..ffeeac4 100755 --- a/LICENSE +++ b/LICENSE @@ -186,7 +186,7 @@ same "printed page" as the copyright notice for easier identification within third-party archives. - Copyright {yyyy} {name of copyright owner} + Copyright 2023 Ronaldson Bellande Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. From 1040083aaceaf90ca2489bf65c38df0aad63b771 Mon Sep 17 00:00:00 2001 From: Ronaldson Bellande <47253433+RonaldsonBellande@users.noreply.github.com> Date: Tue, 3 Oct 2023 09:04:27 -0400 Subject: [PATCH 187/238] Update LICENSE --- LICENSE | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/LICENSE b/LICENSE index c0ee812..ffeeac4 100755 --- a/LICENSE +++ b/LICENSE @@ -186,7 +186,7 @@ same "printed page" as the copyright notice for easier identification within third-party archives. - Copyright {yyyy} {name of copyright owner} + Copyright 2023 Ronaldson Bellande Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. From 4c61a1fa2f0ab76ae6c13c50f01c6238b854db1d Mon Sep 17 00:00:00 2001 From: RonaldsonBellande Date: Tue, 3 Oct 2023 12:01:51 -0400 Subject: [PATCH 188/238] latest pushes --- humanoid_robot_controller/package.xml | 4 +++- humanoid_robot_device/CHANGELOG.rst | 1 - humanoid_robot_device/package.xml | 4 +++- humanoid_robot_framework/package.xml | 4 +++- humanoid_robot_framework_common/package.xml | 3 ++- 5 files changed, 11 insertions(+), 5 deletions(-) diff --git a/humanoid_robot_controller/package.xml b/humanoid_robot_controller/package.xml index 3cfa98f..0101650 100755 --- a/humanoid_robot_controller/package.xml +++ b/humanoid_robot_controller/package.xml @@ -6,7 +6,9 @@ Apache 2.0 Ronaldson Bellande - catkin + catkin + ament_cmake + roscpp roslib diff --git a/humanoid_robot_device/CHANGELOG.rst b/humanoid_robot_device/CHANGELOG.rst index 7dcdede..c95d8d6 100755 --- a/humanoid_robot_device/CHANGELOG.rst +++ b/humanoid_robot_device/CHANGELOG.rst @@ -12,7 +12,6 @@ Changelog for package humanoid_robot_device * Update package.xml and CMakeList.txt for to the latest versions * Contributors & Maintainer: Ronaldson Bellande - 0.3.0 (2021-05-03) ------------------ * Update package.xml and CMakeList.txt for noetic branch diff --git a/humanoid_robot_device/package.xml b/humanoid_robot_device/package.xml index f072991..b3e6616 100755 --- a/humanoid_robot_device/package.xml +++ b/humanoid_robot_device/package.xml @@ -10,7 +10,9 @@ Apache 2.0 Ronaldson Bellande - catkin + catkin + ament_cmake + roscpp dynamixel_sdk diff --git a/humanoid_robot_framework/package.xml b/humanoid_robot_framework/package.xml index 4461b84..73920b3 100755 --- a/humanoid_robot_framework/package.xml +++ b/humanoid_robot_framework/package.xml @@ -6,7 +6,9 @@ Apache 2.0 Ronaldson Bellande - catkin + catkin + ament_cmake + humanoid_robot_controller humanoid_robot_device diff --git a/humanoid_robot_framework_common/package.xml b/humanoid_robot_framework_common/package.xml index fc18204..6de7ba9 100755 --- a/humanoid_robot_framework_common/package.xml +++ b/humanoid_robot_framework_common/package.xml @@ -6,7 +6,8 @@ Apache 2.0 Ronaldson Bellande - catkin + catkin + ament_cmake roscpp humanoid_robot_device From dbbadbe0ca3e849e099f609aaef49b8e68eb806d Mon Sep 17 00:00:00 2001 From: RonaldsonBellande Date: Tue, 3 Oct 2023 13:18:46 -0400 Subject: [PATCH 189/238] latest pushes --- humanoid_robot_controller/package.xml | 99 ++++++++++++++------- humanoid_robot_device/package.xml | 28 ++++-- humanoid_robot_framework/package.xml | 36 ++++++-- humanoid_robot_framework_common/package.xml | 28 ++++-- 4 files changed, 140 insertions(+), 51 deletions(-) diff --git a/humanoid_robot_controller/package.xml b/humanoid_robot_controller/package.xml index 0101650..aa0b769 100755 --- a/humanoid_robot_controller/package.xml +++ b/humanoid_robot_controller/package.xml @@ -10,37 +10,76 @@ ament_cmake - roscpp - roslib - std_msgs - sensor_msgs - humanoid_robot_controller_msgs - dynamixel_sdk - humanoid_robot_device - humanoid_robot_framework_common - cmake_modules - yaml-cpp + + roscpp + roslib + std_msgs + sensor_msgs + humanoid_robot_controller_msgs + dynamixel_sdk + humanoid_robot_device + humanoid_robot_framework_common + cmake_modules + yaml-cpp - roscpp - roslib - std_msgs - sensor_msgs - humanoid_robot_controller_msgs - dynamixel_sdk - humanoid_robot_device - humanoid_robot_framework_common - cmake_modules - yaml-cpp + roscpp + roslib + std_msgs + sensor_msgs + humanoid_robot_controller_msgs + dynamixel_sdk + humanoid_robot_device + humanoid_robot_framework_common + cmake_modules + yaml-cpp + + roscpp + roslib + std_msgs + sensor_msgs + humanoid_robot_controller_msgs + dynamixel_sdk + humanoid_robot_device + humanoid_robot_framework_common + cmake_modules + yaml-cpp + + + + + roscpp + roslib + std_msgs + sensor_msgs + humanoid_robot_controller_msgs + dynamixel_sdk + humanoid_robot_device + humanoid_robot_framework_common + cmake_modules + yaml-cpp + + roscpp + roslib + std_msgs + sensor_msgs + humanoid_robot_controller_msgs + dynamixel_sdk + humanoid_robot_device + humanoid_robot_framework_common + cmake_modules + yaml-cpp + + roscpp + roslib + std_msgs + sensor_msgs + humanoid_robot_controller_msgs + dynamixel_sdk + humanoid_robot_device + humanoid_robot_framework_common + cmake_modules + yaml-cpp + - roscpp - roslib - std_msgs - sensor_msgs - humanoid_robot_controller_msgs - dynamixel_sdk - humanoid_robot_device - humanoid_robot_framework_common - cmake_modules - yaml-cpp diff --git a/humanoid_robot_device/package.xml b/humanoid_robot_device/package.xml index b3e6616..8212686 100755 --- a/humanoid_robot_device/package.xml +++ b/humanoid_robot_device/package.xml @@ -14,12 +14,28 @@ ament_cmake - roscpp - dynamixel_sdk + + roscpp + dynamixel_sdk + + roscpp + dynamixel_sdk + + roscpp + dynamixel_sdk + + + + + roscpp + dynamixel_sdk + + roscpp + dynamixel_sdk + + roscpp + dynamixel_sdk + - roscpp - dynamixel_sdk - roscpp - dynamixel_sdk diff --git a/humanoid_robot_framework/package.xml b/humanoid_robot_framework/package.xml index 73920b3..8d245e3 100755 --- a/humanoid_robot_framework/package.xml +++ b/humanoid_robot_framework/package.xml @@ -10,17 +10,35 @@ ament_cmake - humanoid_robot_controller - humanoid_robot_device - humanoid_robot_framework_common + + humanoid_robot_controller + humanoid_robot_device + humanoid_robot_framework_common - humanoid_robot_controller - humanoid_robot_device - humanoid_robot_framework_common + humanoid_robot_controller + humanoid_robot_device + humanoid_robot_framework_common + + humanoid_robot_controller + humanoid_robot_device + humanoid_robot_framework_common + + + + + humanoid_robot_controller + humanoid_robot_device + humanoid_robot_framework_common + + humanoid_robot_controller + humanoid_robot_device + humanoid_robot_framework_common + + humanoid_robot_controller + humanoid_robot_device + humanoid_robot_framework_common + - humanoid_robot_controller - humanoid_robot_device - humanoid_robot_framework_common diff --git a/humanoid_robot_framework_common/package.xml b/humanoid_robot_framework_common/package.xml index 6de7ba9..f7aa9b2 100755 --- a/humanoid_robot_framework_common/package.xml +++ b/humanoid_robot_framework_common/package.xml @@ -9,13 +9,29 @@ catkin ament_cmake - roscpp - humanoid_robot_device - roscpp - humanoid_robot_device + + roscpp + humanoid_robot_device + + roscpp + humanoid_robot_device + + roscpp + humanoid_robot_device + + + + + roscpp + humanoid_robot_device + + roscpp + humanoid_robot_device + + roscpp + humanoid_robot_device + - roscpp - humanoid_robot_device From c7565da012d1060ccda99467d47f449c113811b5 Mon Sep 17 00:00:00 2001 From: RonaldsonBellande Date: Tue, 3 Oct 2023 21:27:53 -0400 Subject: [PATCH 190/238] latest pushes --- humanoid_robot_controller/CMakeLists.txt | 98 +++++------- .../humanoid_robot_controller.h | 146 +++++++++--------- humanoid_robot_device/CMakeLists.txt | 58 +++---- .../control_table_item.h | 42 ++--- .../humanoid_robot_device/time_stamp.h | 40 ++--- .../src/humanoid_robot_device/robot.cpp | 108 ++++++------- .../src/humanoid_robot_device/sensor.cpp | 48 +++--- humanoid_robot_framework/CMakeLists.txt | 14 +- .../CMakeLists.txt | 56 +++---- 9 files changed, 280 insertions(+), 330 deletions(-) diff --git a/humanoid_robot_controller/CMakeLists.txt b/humanoid_robot_controller/CMakeLists.txt index 15cb7bf..3cdb850 100755 --- a/humanoid_robot_controller/CMakeLists.txt +++ b/humanoid_robot_controller/CMakeLists.txt @@ -1,32 +1,31 @@ -################################################################################ -# Set minimum required version of cmake, project name and compile options -################################################################################ cmake_minimum_required(VERSION 3.8) project(humanoid_robot_controller) ## Compile as C++11, supported in ROS Kinetic and newer add_compile_options(-std=c++11) -################################################################################ -# Find catkin packages and libraries for catkin and system dependencies -################################################################################ -find_package(catkin REQUIRED COMPONENTS - roscpp - roslib - std_msgs - sensor_msgs - humanoid_robot_controller_msgs - dynamixel_sdk - humanoid_robot_device - humanoid_robot_framework_common - cmake_modules -) -find_package(Boost REQUIRED) +if($ENV{ROS_VERSION} EQUAL 1) + find_package(catkin REQUIRED COMPONENTS + roscpp + roslib + std_msgs + sensor_msgs + humanoid_robot_controller_msgs + dynamixel_sdk + humanoid_robot_device + humanoid_robot_framework_common + cmake_modules + ) + + find_package(Boost REQUIRED) + + find_package(PkgConfig REQUIRED) +else() + find_package(ament_cmake REQUIRED) +endif() + -# Resolve system dependency on yaml-cpp, which apparently does not -# provide a CMake find_package() module. -find_package(PkgConfig REQUIRED) pkg_check_modules(YAML_CPP REQUIRED yaml-cpp) find_path(YAML_CPP_INCLUDE_DIR NAMES yaml_cpp.h @@ -42,40 +41,28 @@ if(NOT ${YAML_CPP_VERSION} VERSION_LESS "0.5") add_definitions(-DHAVE_NEW_YAMLCPP) endif(NOT ${YAML_CPP_VERSION} VERSION_LESS "0.5") -################################################################################ -# Setup for python modules and scripts -################################################################################ -################################################################################ -# Declare ROS messages, services and actions -################################################################################ +if($ENV{ROS_VERSION} EQUAL 1) + catkin_package( + INCLUDE_DIRS include + LIBRARIES humanoid_robot_controller + CATKIN_DEPENDS + roscpp + roslib + std_msgs + sensor_msgs + humanoid_robot_controller_msgs + dynamixel_sdk + humanoid_robot_device + humanoid_robot_framework_common + cmake_modules + DEPENDS Boost + ) +else() + ament_package() +endif() -################################################################################ -# Declare ROS dynamic reconfigure parameters -################################################################################ -################################################################################ -# Declare catkin specific configuration to be passed to dependent projects -################################################################################ -catkin_package( - INCLUDE_DIRS include - LIBRARIES humanoid_robot_controller - CATKIN_DEPENDS - roscpp - roslib - std_msgs - sensor_msgs - humanoid_robot_controller_msgs - dynamixel_sdk - humanoid_robot_device - humanoid_robot_framework_common - cmake_modules - DEPENDS Boost -) - -################################################################################ -# Build -################################################################################ include_directories( include ${catkin_INCLUDE_DIRS} @@ -87,9 +74,6 @@ add_library(humanoid_robot_controller src/humanoid_robot_controller/humanoid_rob add_dependencies(humanoid_robot_controller ${catkin_EXPORTED_TARGETS}) target_link_libraries(humanoid_robot_controller ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${YAML_CPP_LIBRARIES}) -################################################################################ -# Install -################################################################################ install(TARGETS humanoid_robot_controller ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} @@ -99,7 +83,3 @@ install(TARGETS humanoid_robot_controller install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} ) - -################################################################################ -# Test -################################################################################ diff --git a/humanoid_robot_controller/include/humanoid_robot_controller/humanoid_robot_controller.h b/humanoid_robot_controller/include/humanoid_robot_controller/humanoid_robot_controller.h index eca9641..c9d09e3 100755 --- a/humanoid_robot_controller/include/humanoid_robot_controller/humanoid_robot_controller.h +++ b/humanoid_robot_controller/include/humanoid_robot_controller/humanoid_robot_controller.h @@ -1,17 +1,17 @@ -/******************************************************************************* - * Copyright 2018 ROBOTIS CO., LTD. - * - * 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. +/******************************************************************************* + * Copyright 2018 ROBOTIS CO., LTD. + * + * 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. *******************************************************************************/ /* @@ -22,29 +22,29 @@ */ #ifndef ROBOTIS_CONTROLLER_ROBOTIS_CONTROLLER_H_ -#define ROBOTIS_CONTROLLER_ROBOTIS_CONTROLLER_H_ - -#include -#include -#include -#include -#include -#include -#include +#define ROBOTIS_CONTROLLER_ROBOTIS_CONTROLLER_H_ -#include "humanoid_robot_controller_msgs/GetJointModule.h" -#include "humanoid_robot_controller_msgs/JointCtrlModule.h" -#include "humanoid_robot_controller_msgs/LoadOffset.h" -#include "humanoid_robot_controller_msgs/SetJointModule.h" -#include "humanoid_robot_controller_msgs/SetModule.h" -#include "humanoid_robot_controller_msgs/SyncWriteItem.h" -#include "humanoid_robot_controller_msgs/WriteControlTable.h" +#include +#include +#include +#include +#include +#include +#include -#include "dynamixel_sdk/group_bulk_read.h" -#include "dynamixel_sdk/group_sync_write.h" -#include "humanoid_robot_device/robot.h" -#include "humanoid_robot_framework_common/motion_module.h" -#include "humanoid_robot_framework_common/sensor_module.h" +#include "humanoid_robot_controller_msgs/GetJointModule.h" +#include "humanoid_robot_controller_msgs/JointCtrlModule.h" +#include "humanoid_robot_controller_msgs/LoadOffset.h" +#include "humanoid_robot_controller_msgs/SetJointModule.h" +#include "humanoid_robot_controller_msgs/SetModule.h" +#include "humanoid_robot_controller_msgs/SyncWriteItem.h" +#include "humanoid_robot_controller_msgs/WriteControlTable.h" + +#include "dynamixel_sdk/group_bulk_read.h" +#include "dynamixel_sdk/group_sync_write.h" +#include "humanoid_robot_device/robot.h" +#include "humanoid_robot_framework_common/motion_module.h" +#include "humanoid_robot_framework_common/sensor_module.h" namespace humanoid_robot_framework { @@ -74,7 +74,7 @@ private: void gazeboTimerThread(); void msgQueueThread(); void setCtrlModuleThread(std::string ctrl_module); - void setJointCtrlModuleThread( + void setJointCtrlModuleThread( const humanoid_robot_controller_msgs::JointCtrlModule::ConstPtr &msg); bool isTimerStopped(); @@ -92,23 +92,23 @@ public: std::map port_to_bulk_read_; /* sync write */ - std::map + std::map port_to_sync_write_position_; - std::map + std::map port_to_sync_write_velocity_; - std::map + std::map port_to_sync_write_current_; - std::map + std::map port_to_sync_write_position_p_gain_; - std::map + std::map port_to_sync_write_position_i_gain_; - std::map + std::map port_to_sync_write_position_d_gain_; - std::map + std::map port_to_sync_write_velocity_p_gain_; - std::map + std::map port_to_sync_write_velocity_i_gain_; - std::map + std::map port_to_sync_write_velocity_d_gain_; /* publisher */ @@ -124,7 +124,7 @@ public: RobotisController(); - bool initialize(const std::string robot_file_path, + bool initialize(const std::string robot_file_path, const std::string init_file_path); void initializeDevice(const std::string init_file_path); void process(); @@ -142,66 +142,68 @@ public: void loadOffset(const std::string path); /* ROS Topic Callback Functions */ - void writeControlTableCallback( + void writeControlTableCallback( const humanoid_robot_controller_msgs::WriteControlTable::ConstPtr &msg); - void syncWriteItemCallback( + void syncWriteItemCallback( const humanoid_robot_controller_msgs::SyncWriteItem::ConstPtr &msg); void setControllerModeCallback(const std_msgs::String::ConstPtr &msg); void setJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg); - void setJointCtrlModuleCallback( + void setJointCtrlModuleCallback( const humanoid_robot_controller_msgs::JointCtrlModule::ConstPtr &msg); void setCtrlModuleCallback(const std_msgs::String::ConstPtr &msg); void enableOffsetCallback(const std_msgs::Bool::ConstPtr &msg); - bool getJointCtrlModuleService( - humanoid_robot_controller_msgs::GetJointModule::Request &req, + bool getJointCtrlModuleService( + humanoid_robot_controller_msgs::GetJointModule::Request &req, humanoid_robot_controller_msgs::GetJointModule::Response &res); - bool setJointCtrlModuleService( - humanoid_robot_controller_msgs::SetJointModule::Request &req, + bool setJointCtrlModuleService( + humanoid_robot_controller_msgs::SetJointModule::Request &req, humanoid_robot_controller_msgs::SetJointModule::Response &res); - bool setCtrlModuleService(humanoid_robot_controller_msgs::SetModule::Request &req, - humanoid_robot_controller_msgs::SetModule::Response &res); - bool loadOffsetService(humanoid_robot_controller_msgs::LoadOffset::Request &req, - humanoid_robot_controller_msgs::LoadOffset::Response &res); + bool setCtrlModuleService( + humanoid_robot_controller_msgs::SetModule::Request &req, + humanoid_robot_controller_msgs::SetModule::Response &res); + bool + loadOffsetService(humanoid_robot_controller_msgs::LoadOffset::Request &req, + humanoid_robot_controller_msgs::LoadOffset::Response &res); void gazeboJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg); int ping(const std::string joint_name, uint8_t *error = 0); - int ping(const std::string joint_name, uint16_t *model_number, + int ping(const std::string joint_name, uint16_t *model_number, uint8_t *error = 0); int action(const std::string joint_name); int reboot(const std::string joint_name, uint8_t *error = 0); - int factoryReset(const std::string joint_name, uint8_t option = 0, + int factoryReset(const std::string joint_name, uint8_t option = 0, uint8_t *error = 0); - int read(const std::string joint_name, uint16_t address, uint16_t length, + int read(const std::string joint_name, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0); - int readCtrlItem(const std::string joint_name, const std::string item_name, + int readCtrlItem(const std::string joint_name, const std::string item_name, uint32_t *data, uint8_t *error = 0); - int read1Byte(const std::string joint_name, uint16_t address, uint8_t *data, + int read1Byte(const std::string joint_name, uint16_t address, uint8_t *data, uint8_t *error = 0); - int read2Byte(const std::string joint_name, uint16_t address, uint16_t *data, + int read2Byte(const std::string joint_name, uint16_t address, uint16_t *data, uint8_t *error = 0); - int read4Byte(const std::string joint_name, uint16_t address, uint32_t *data, + int read4Byte(const std::string joint_name, uint16_t address, uint32_t *data, uint8_t *error = 0); - int write(const std::string joint_name, uint16_t address, uint16_t length, + int write(const std::string joint_name, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0); - int writeCtrlItem(const std::string joint_name, const std::string item_name, + int writeCtrlItem(const std::string joint_name, const std::string item_name, uint32_t data, uint8_t *error = 0); - int write1Byte(const std::string joint_name, uint16_t address, uint8_t data, + int write1Byte(const std::string joint_name, uint16_t address, uint8_t data, uint8_t *error = 0); - int write2Byte(const std::string joint_name, uint16_t address, uint16_t data, + int write2Byte(const std::string joint_name, uint16_t address, uint16_t data, uint8_t *error = 0); - int write4Byte(const std::string joint_name, uint16_t address, uint32_t data, + int write4Byte(const std::string joint_name, uint16_t address, uint32_t data, uint8_t *error = 0); - int regWrite(const std::string joint_name, uint16_t address, uint16_t length, + int regWrite(const std::string joint_name, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0); }; -} // namespace humanoid_robot_framework - +} // namespace humanoid_robot_framework + #endif /* ROBOTIS_CONTROLLER_ROBOTIS_CONTROLLER_H_ */ diff --git a/humanoid_robot_device/CMakeLists.txt b/humanoid_robot_device/CMakeLists.txt index c838c1f..2e140bb 100755 --- a/humanoid_robot_device/CMakeLists.txt +++ b/humanoid_robot_device/CMakeLists.txt @@ -1,43 +1,30 @@ -################################################################################ -# Set minimum required version of cmake, project name and compile options -################################################################################ cmake_minimum_required(VERSION 3.8) project(humanoid_robot_device) -################################################################################ -# Find catkin packages and libraries for catkin and system dependencies -################################################################################ -find_package(catkin REQUIRED COMPONENTS - roscpp - dynamixel_sdk -) -################################################################################ -# Setup for python modules and scripts -################################################################################ +if($ENV{ROS_VERSION} EQUAL 1) + find_package(catkin REQUIRED COMPONENTS + roscpp + dynamixel_sdk + ) +else() + find_package(ament_cmake REQUIRED) +endif() -################################################################################ -# Declare ROS messages, services and actions -################################################################################ -################################################################################ -# Declare ROS dynamic reconfigure parameters -################################################################################ +if($ENV{ROS_VERSION} EQUAL 1) + catkin_package( + INCLUDE_DIRS include + LIBRARIES humanoid_robot_device + CATKIN_DEPENDS + roscpp + dynamixel_sdk + ) +else() + ament_package() +endif() -################################################################################ -# Declare catkin specific configuration to be passed to dependent projects -################################################################################ -catkin_package( - INCLUDE_DIRS include - LIBRARIES humanoid_robot_device - CATKIN_DEPENDS - roscpp - dynamixel_sdk -) -################################################################################ -# Build -################################################################################ include_directories( include ${catkin_INCLUDE_DIRS} @@ -51,9 +38,6 @@ add_library(humanoid_robot_device add_dependencies(humanoid_robot_device ${catkin_EXPORTED_TARGETS}) target_link_libraries(humanoid_robot_device ${catkin_LIBRARIES}) -################################################################################ -# Install -################################################################################ install(TARGETS humanoid_robot_device ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} @@ -67,7 +51,3 @@ install(DIRECTORY include/${PROJECT_NAME}/ install(DIRECTORY devices DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) - -################################################################################ -# Test -################################################################################ diff --git a/humanoid_robot_device/include/humanoid_robot_device/control_table_item.h b/humanoid_robot_device/include/humanoid_robot_device/control_table_item.h index 971c3d2..a5338df 100755 --- a/humanoid_robot_device/include/humanoid_robot_device/control_table_item.h +++ b/humanoid_robot_device/include/humanoid_robot_device/control_table_item.h @@ -1,17 +1,17 @@ -/******************************************************************************* - * Copyright 2018 ROBOTIS CO., LTD. - * - * 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. +/******************************************************************************* + * Copyright 2018 ROBOTIS CO., LTD. + * + * 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. *******************************************************************************/ /* @@ -22,8 +22,8 @@ */ #ifndef ROBOTIS_DEVICE_CONTROL_TABLE_ITEM_H_ -#define ROBOTIS_DEVICE_CONTROL_TABLE_ITEM_H_ - +#define ROBOTIS_DEVICE_CONTROL_TABLE_ITEM_H_ + #include namespace humanoid_robot_framework { @@ -43,12 +43,12 @@ public: int32_t data_max_value_; bool is_signed_; - ControlTableItem() - : item_name_(""), address_(0), access_type_(Read), memory_type_(RAM), - data_length_(0), data_min_value_(0), data_max_value_(0), + ControlTableItem() + : item_name_(""), address_(0), access_type_(Read), memory_type_(RAM), + data_length_(0), data_min_value_(0), data_max_value_(0), is_signed_(false) {} }; -} // namespace humanoid_robot_framework - +} // namespace humanoid_robot_framework + #endif /* ROBOTIS_DEVICE_CONTROL_TABLE_ITEM_H_ */ diff --git a/humanoid_robot_device/include/humanoid_robot_device/time_stamp.h b/humanoid_robot_device/include/humanoid_robot_device/time_stamp.h index 97ed81b..b2bc3f7 100755 --- a/humanoid_robot_device/include/humanoid_robot_device/time_stamp.h +++ b/humanoid_robot_device/include/humanoid_robot_device/time_stamp.h @@ -1,18 +1,18 @@ /******************************************************************************* -* Copyright 2018 ROBOTIS CO., LTD. -* -* 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. -*******************************************************************************/ + * Copyright 2018 ROBOTIS CO., LTD. + * + * 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. + *******************************************************************************/ /* * time_stamp.h @@ -24,22 +24,16 @@ #ifndef ROBOTIS_DEVICE_TIME_STAMP_H_ #define ROBOTIS_DEVICE_TIME_STAMP_H_ - -namespace humanoid_robot_framework -{ +namespace humanoid_robot_framework { class TimeStamp { public: long sec_; long nsec_; - TimeStamp(long sec, long nsec) - : sec_(sec), - nsec_(nsec) - { } + TimeStamp(long sec, long nsec) : sec_(sec), nsec_(nsec) {} }; -} - +} // namespace humanoid_robot_framework #endif /* ROBOTIS_DEVICE_TIME_STAMP_H_ */ diff --git a/humanoid_robot_device/src/humanoid_robot_device/robot.cpp b/humanoid_robot_device/src/humanoid_robot_device/robot.cpp index 6362c5b..4ae7219 100755 --- a/humanoid_robot_device/src/humanoid_robot_device/robot.cpp +++ b/humanoid_robot_device/src/humanoid_robot_device/robot.cpp @@ -1,17 +1,17 @@ -/******************************************************************************* - * Copyright 2018 ROBOTIS CO., LTD. - * - * 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. +/******************************************************************************* + * Copyright 2018 ROBOTIS CO., LTD. + * + * 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. *******************************************************************************/ /* @@ -21,30 +21,30 @@ * Author: zerom */ -#include -#include -#include +#include +#include +#include #include "humanoid_robot_device/robot.h" using namespace humanoid_robot_framework; static inline std::string <rim(std::string &s) { - s.erase(s.begin(), - std::find_if(s.begin(), s.end(), + s.erase(s.begin(), + std::find_if(s.begin(), s.end(), std::not1(std::ptr_fun(std::isspace)))); return s; } static inline std::string &rtrim(std::string &s) { - s.erase(std::find_if(s.rbegin(), s.rend(), - std::not1(std::ptr_fun(std::isspace))) - .base(), + s.erase(std::find_if(s.rbegin(), s.rend(), + std::not1(std::ptr_fun(std::isspace))) + .base(), s.end()); return s; } static inline std::string &trim(std::string &s) { return ltrim(rtrim(s)); } -static inline std::vector split(const std::string &text, +static inline std::vector split(const std::string &text, char sep) { std::vector tokens; std::size_t start = 0, end = 0; @@ -60,7 +60,7 @@ static inline std::vector split(const std::string &text, return tokens; } -Robot::Robot(std::string robot_file_path, std::string dev_desc_dir_path) +Robot::Robot(std::string robot_file_path, std::string dev_desc_dir_path) : control_cycle_msec_(DEFAULT_CONTROL_CYCLE) { if (dev_desc_dir_path.compare(dev_desc_dir_path.size() - 1, 1, "/") != 0) dev_desc_dir_path += "/"; @@ -81,10 +81,10 @@ Robot::Robot(std::string robot_file_path, std::string dev_desc_dir_path) input_str = trim(input_str); // find session - if (!input_str.compare(0, 1, "[") && + if (!input_str.compare(0, 1, "[") && !input_str.compare(input_str.size() - 1, 1, "]")) { input_str = input_str.substr(1, input_str.size() - 2); - std::transform(input_str.begin(), input_str.end(), input_str.begin(), + std::transform(input_str.begin(), input_str.end(), input_str.begin(), ::tolower); session = trim(input_str); continue; @@ -102,10 +102,10 @@ Robot::Robot(std::string robot_file_path, std::string dev_desc_dir_path) if (tokens.size() != 3) continue; - std::cout << tokens[0] << " added. (baudrate: " << tokens[1] << ")" + std::cout << tokens[0] << " added. (baudrate: " << tokens[1] << ")" << std::endl; - ports_[tokens[0]] = + ports_[tokens[0]] = dynamixel::PortHandler::getPortHandler(tokens[0].c_str()); ports_[tokens[0]]->setBaudRate(std::atoi(tokens[1].c_str())); port_default_device_[tokens[0]] = tokens[2]; @@ -115,7 +115,7 @@ Robot::Robot(std::string robot_file_path, std::string dev_desc_dir_path) continue; if (tokens[0] == DYNAMIXEL) { - std::string file_path = + std::string file_path = dev_desc_dir_path + tokens[0] + "/" + tokens[3] + ".device"; int id = std::atoi(tokens[2].c_str()); std::string port = tokens[1]; @@ -127,20 +127,20 @@ Robot::Robot(std::string robot_file_path, std::string dev_desc_dir_path) Dynamixel *dxl = dxls_[dev_name]; std::vector sub_tokens = split(tokens[6], ','); if (sub_tokens.size() > 0 && sub_tokens[0] != "") { - std::map::iterator indirect_it = + std::map::iterator indirect_it = dxl->ctrl_table_.find(INDIRECT_ADDRESS_1); - if (indirect_it != + if (indirect_it != dxl->ctrl_table_.end()) // INDIRECT_ADDRESS_1 exist { - uint16_t indirect_data_addr = + uint16_t indirect_data_addr = dxl->ctrl_table_[INDIRECT_DATA_1]->address_; for (int _i = 0; _i < sub_tokens.size(); _i++) { - std::map::iterator + std::map::iterator bulkread_it = dxl->ctrl_table_.find(sub_tokens[_i]); if (bulkread_it == dxl->ctrl_table_.end()) { - fprintf( - stderr, - "\n ##### BULK READ ITEM [ %s ] NOT FOUND!! #####\n\n", + fprintf( + stderr, + "\n ##### BULK READ ITEM [ %s ] NOT FOUND!! #####\n\n", sub_tokens[_i].c_str()); continue; } @@ -164,13 +164,13 @@ Robot::Robot(std::string robot_file_path, std::string dev_desc_dir_path) { for (int i = 0; i < sub_tokens.size(); i++) { if (dxl->ctrl_table_[sub_tokens[i]] != NULL) - dxl->bulk_read_items_.push_back( + dxl->bulk_read_items_.push_back( dxl->ctrl_table_[sub_tokens[i]]); } } } } else if (tokens[0] == SENSOR) { - std::string file_path = + std::string file_path = dev_desc_dir_path + tokens[0] + "/" + tokens[3] + ".device"; int id = std::atoi(tokens[2].c_str()); std::string port = tokens[1]; @@ -182,12 +182,12 @@ Robot::Robot(std::string robot_file_path, std::string dev_desc_dir_path) Sensor *sensor = sensors_[dev_name]; std::vector sub_tokens = split(tokens[6], ','); if (sub_tokens.size() > 0 && sub_tokens[0] != "") { - std::map::iterator indirect_it = + std::map::iterator indirect_it = sensor->ctrl_table_.find(INDIRECT_ADDRESS_1); - if (indirect_it != + if (indirect_it != sensor->ctrl_table_.end()) // INDIRECT_ADDRESS_1 exist { - uint16_t indirect_data_addr = + uint16_t indirect_data_addr = sensor->ctrl_table_[INDIRECT_DATA_1]->address_; for (int i = 0; i < sub_tokens.size(); i++) { sensor->bulk_read_items_.push_back(new ControlTableItem()); @@ -208,7 +208,7 @@ Robot::Robot(std::string robot_file_path, std::string dev_desc_dir_path) } else // INDIRECT_ADDRESS_1 exist { for (int i = 0; i < sub_tokens.size(); i++) - sensor->bulk_read_items_.push_back( + sensor->bulk_read_items_.push_back( sensor->ctrl_table_[sub_tokens[i]]); } } @@ -221,7 +221,7 @@ Robot::Robot(std::string robot_file_path, std::string dev_desc_dir_path) } } -Sensor *Robot::getSensor(std::string path, int id, std::string port, +Sensor *Robot::getSensor(std::string path, int id, std::string port, float protocol_version) { Sensor *sensor; @@ -245,10 +245,10 @@ Sensor *Robot::getSensor(std::string path, int id, std::string port, continue; // find _session - if (!input_str.compare(0, 1, "[") && + if (!input_str.compare(0, 1, "[") && !input_str.compare(input_str.size() - 1, 1, "]")) { input_str = input_str.substr(1, input_str.size() - 2); - std::transform(input_str.begin(), input_str.end(), input_str.begin(), + std::transform(input_str.begin(), input_str.end(), input_str.begin(), ::tolower); session = trim(input_str); continue; @@ -293,9 +293,9 @@ Sensor *Robot::getSensor(std::string path, int id, std::string port, } sensor->port_name_ = port; - fprintf(stderr, "(%s) [ID:%3d] %14s added. \n", port.c_str(), sensor->id_, + fprintf(stderr, "(%s) [ID:%3d] %14s added. \n", port.c_str(), sensor->id_, sensor->model_name_.c_str()); - // std::cout << "[ID:" << (int)(_sensor->id) << "] " << _sensor->model_name + // std::cout << "[ID:" << (int)(_sensor->id) << "] " << _sensor->model_name // << " added. (" << port << ")" << std::endl; file.close(); } else @@ -304,7 +304,7 @@ Sensor *Robot::getSensor(std::string path, int id, std::string port, return sensor; } -Dynamixel *Robot::getDynamixel(std::string path, int id, std::string port, +Dynamixel *Robot::getDynamixel(std::string path, int id, std::string port, float protocol_version) { Dynamixel *dxl; @@ -342,10 +342,10 @@ Dynamixel *Robot::getDynamixel(std::string path, int id, std::string port, continue; // find session - if (!input_str.compare(0, 1, "[") && + if (!input_str.compare(0, 1, "[") && !input_str.compare(input_str.size() - 1, 1, "]")) { input_str = input_str.substr(1, input_str.size() - 2); - std::transform(input_str.begin(), input_str.end(), input_str.begin(), + std::transform(input_str.begin(), input_str.end(), input_str.begin(), ::tolower); session = trim(input_str); continue; @@ -440,10 +440,10 @@ Dynamixel *Robot::getDynamixel(std::string path, int id, std::string port, if (dxl->ctrl_table_[torque_enable_item_name] != NULL) dxl->torque_enable_item_ = dxl->ctrl_table_[torque_enable_item_name]; if (dxl->ctrl_table_[present_position_item_name] != NULL) - dxl->present_position_item_ = + dxl->present_position_item_ = dxl->ctrl_table_[present_position_item_name]; if (dxl->ctrl_table_[present_velocity_item_name] != NULL) - dxl->present_velocity_item_ = + dxl->present_velocity_item_ = dxl->ctrl_table_[present_velocity_item_name]; if (dxl->ctrl_table_[present_current_item_name] != NULL) dxl->present_current_item_ = dxl->ctrl_table_[present_current_item_name]; @@ -466,9 +466,9 @@ Dynamixel *Robot::getDynamixel(std::string path, int id, std::string port, if (dxl->ctrl_table_[velocity_p_gain_item_name] != NULL) dxl->velocity_p_gain_item_ = dxl->ctrl_table_[velocity_p_gain_item_name]; - fprintf(stderr, "(%s) [ID:%3d] %14s added. \n", port.c_str(), dxl->id_, + fprintf(stderr, "(%s) [ID:%3d] %14s added. \n", port.c_str(), dxl->id_, dxl->model_name_.c_str()); - // std::cout << "[ID:" << (int)(_dxl->id) << "] " << _dxl->model_name << " + // std::cout << "[ID:" << (int)(_dxl->id) << "] " << _dxl->model_name << " // added. (" << port << ")" << std::endl; file.close(); } else diff --git a/humanoid_robot_device/src/humanoid_robot_device/sensor.cpp b/humanoid_robot_device/src/humanoid_robot_device/sensor.cpp index 166c6dc..0be04df 100755 --- a/humanoid_robot_device/src/humanoid_robot_device/sensor.cpp +++ b/humanoid_robot_device/src/humanoid_robot_device/sensor.cpp @@ -1,17 +1,17 @@ -/******************************************************************************* - * Copyright 2018 ROBOTIS CO., LTD. - * - * 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. +/******************************************************************************* + * Copyright 2018 ROBOTIS CO., LTD. + * + * 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. *******************************************************************************/ /* @@ -25,15 +25,15 @@ using namespace humanoid_robot_framework; -Sensor::Sensor(int id, std::string model_name, float protocol_version) { - - this->id_ = id; - this->model_name_ = model_name; - this->port_name_ = ""; - this->protocol_version_ = protocol_version; - ctrl_table_.clear(); - - sensor_state_ = new SensorState(); - +Sensor::Sensor(int id, std::string model_name, float protocol_version) { + + this->id_ = id; + this->model_name_ = model_name; + this->port_name_ = ""; + this->protocol_version_ = protocol_version; + ctrl_table_.clear(); + + sensor_state_ = new SensorState(); + bulk_read_items_.clear(); } diff --git a/humanoid_robot_framework/CMakeLists.txt b/humanoid_robot_framework/CMakeLists.txt index 482cad1..15783dc 100644 --- a/humanoid_robot_framework/CMakeLists.txt +++ b/humanoid_robot_framework/CMakeLists.txt @@ -1,4 +1,16 @@ cmake_minimum_required(VERSION 3.8) project(humanoid_robot_framework) -find_package(catkin REQUIRED) + +if($ENV{ROS_VERSION} EQUAL 1) + find_package(catkin REQUIRED) +else() + find_package(ament_cmake REQUIRED) +endif() + +if($ENV{ROS_VERSION} EQUAL 1) + catkin_package() +else() + ament_package() +endif() + catkin_metapackage() diff --git a/humanoid_robot_framework_common/CMakeLists.txt b/humanoid_robot_framework_common/CMakeLists.txt index 185de24..f5d0175 100755 --- a/humanoid_robot_framework_common/CMakeLists.txt +++ b/humanoid_robot_framework_common/CMakeLists.txt @@ -1,41 +1,30 @@ -################################################################################ -# Set minimum required version of cmake, project name and compile options -################################################################################ cmake_minimum_required(VERSION 3.8) project(humanoid_robot_framework_common) -################################################################################ -# Find catkin packages and libraries for catkin and system dependencies -################################################################################ -find_package(catkin REQUIRED COMPONENTS - roscpp - humanoid_robot_device -) -################################################################################ -# Setup for python modules and scripts -################################################################################ +if($ENV{ROS_VERSION} EQUAL 1) + find_package(catkin REQUIRED COMPONENTS + roscpp + humanoid_robot_device + ) +else() + find_package(ament_cmake REQUIRED) +endif() -################################################################################ -# Declare ROS messages, services and actions -################################################################################ -################################################################################ -# Declare ROS dynamic reconfigure parameters -################################################################################ +if($ENV{ROS_VERSION} EQUAL 1) + catkin_package( + INCLUDE_DIRS include + LIBRARIES ${PROJECT_NAME} + CATKIN_DEPENDS + roscpp + humanoid_robot_device + ) +else() + ament_package() +endif() -################################################################################ -# Declare catkin specific configuration to be passed to dependent projects -################################################################################ -catkin_package( - INCLUDE_DIRS include - LIBRARIES ${PROJECT_NAME} - CATKIN_DEPENDS roscpp humanoid_robot_device -) -################################################################################ -# Build -################################################################################ include_directories( include ${catkin_INCLUDE_DIRS} @@ -48,9 +37,6 @@ add_library(${PROJECT_NAME} ) set_target_properties(${PROJECT_NAME} PROPERTIES LINKER_LANGUAGE CXX) -################################################################################ -# Install -################################################################################ install(TARGETS humanoid_robot_framework_common ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} @@ -60,7 +46,3 @@ install(TARGETS humanoid_robot_framework_common install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} ) - -################################################################################ -# Test -################################################################################ From 4995ba061ecc5c1670436c299435d994a8bfa226 Mon Sep 17 00:00:00 2001 From: RonaldsonBellande Date: Wed, 4 Oct 2023 00:51:58 -0400 Subject: [PATCH 191/238] latest pushes --- humanoid_robot_controller/CHANGELOG.rst | 7 +++++++ humanoid_robot_device/CHANGELOG.rst | 7 +++++++ humanoid_robot_framework/CHANGELOG.rst | 7 +++++++ humanoid_robot_framework_common/CHANGELOG.rst | 7 +++++++ 4 files changed, 28 insertions(+) diff --git a/humanoid_robot_controller/CHANGELOG.rst b/humanoid_robot_controller/CHANGELOG.rst index 2982840..f886d0a 100755 --- a/humanoid_robot_controller/CHANGELOG.rst +++ b/humanoid_robot_controller/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package humanoid_robot_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.3.2 (2023-10-03) +------------------ +* Make it compatible for ROS1/ROS2 +* Fix bugs +* Update package.xml and CMakeList.txt for to the latest versions +* Contributors & Maintainer: Ronaldson Bellande + 0.3.1 (2023-09-27) ------------------ * Starting from this point it under a new license diff --git a/humanoid_robot_device/CHANGELOG.rst b/humanoid_robot_device/CHANGELOG.rst index c95d8d6..35e45da 100755 --- a/humanoid_robot_device/CHANGELOG.rst +++ b/humanoid_robot_device/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package humanoid_robot_device ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.3.2 (2023-10-03) +------------------ +* Make it compatible for ROS1/ROS2 +* Fix bugs +* Update package.xml and CMakeList.txt for to the latest versions +* Contributors & Maintainer: Ronaldson Bellande + 0.3.1 (2023-09-27) ------------------ * Starting from this point it under a new license diff --git a/humanoid_robot_framework/CHANGELOG.rst b/humanoid_robot_framework/CHANGELOG.rst index 6e1adb8..4f91270 100755 --- a/humanoid_robot_framework/CHANGELOG.rst +++ b/humanoid_robot_framework/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package humanoid_robot_framework ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.3.2 (2023-10-03) +------------------ +* Make it compatible for ROS1/ROS2 +* Fix bugs +* Update package.xml and CMakeList.txt for to the latest versions +* Contributors & Maintainer: Ronaldson Bellande + 0.3.1 (2023-09-27) ------------------ * Starting from this point it under a new license diff --git a/humanoid_robot_framework_common/CHANGELOG.rst b/humanoid_robot_framework_common/CHANGELOG.rst index cc736f6..2952d37 100755 --- a/humanoid_robot_framework_common/CHANGELOG.rst +++ b/humanoid_robot_framework_common/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package humanoid_robot_framework_common ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.3.2 (2023-10-03) +------------------ +* Make it compatible for ROS1/ROS2 +* Fix bugs +* Update package.xml and CMakeList.txt for to the latest versions +* Contributors & Maintainer: Ronaldson Bellande + 0.3.1 (2023-09-27) ------------------ * Starting from this point it under a new license From cfbdd82d0f6312cb19b5ce984aa1780ad1279d5a Mon Sep 17 00:00:00 2001 From: RonaldsonBellande Date: Thu, 5 Oct 2023 22:36:59 -0400 Subject: [PATCH 192/238] latest pushes --- humanoid_robot_controller/CMakeLists.txt | 2 - humanoid_robot_controller/package.xml | 138 +++++++++--------- humanoid_robot_device/CMakeLists.txt | 2 - humanoid_robot_device/package.xml | 43 +++--- humanoid_robot_framework/CMakeLists.txt | 2 - humanoid_robot_framework/package.xml | 54 ++++--- .../CMakeLists.txt | 2 - humanoid_robot_framework_common/package.xml | 44 +++--- 8 files changed, 132 insertions(+), 155 deletions(-) diff --git a/humanoid_robot_controller/CMakeLists.txt b/humanoid_robot_controller/CMakeLists.txt index 3cdb850..70bcf63 100755 --- a/humanoid_robot_controller/CMakeLists.txt +++ b/humanoid_robot_controller/CMakeLists.txt @@ -58,8 +58,6 @@ if($ENV{ROS_VERSION} EQUAL 1) cmake_modules DEPENDS Boost ) -else() - ament_package() endif() diff --git a/humanoid_robot_controller/package.xml b/humanoid_robot_controller/package.xml index aa0b769..1a94e13 100755 --- a/humanoid_robot_controller/package.xml +++ b/humanoid_robot_controller/package.xml @@ -1,85 +1,81 @@ humanoid_robot_controller - 0.3.0 + 0.3.2 humanoid_robot_controller package for ROBOTIS's platform like Manipulator-H, THORMANG and OP series Apache 2.0 Ronaldson Bellande + catkin + + roscpp + roslib + std_msgs + sensor_msgs + humanoid_robot_controller_msgs + dynamixel_sdk + humanoid_robot_device + humanoid_robot_framework_common + cmake_modules + yaml-cpp + + roscpp + roslib + std_msgs + sensor_msgs + humanoid_robot_controller_msgs + dynamixel_sdk + humanoid_robot_device + humanoid_robot_framework_common + cmake_modules + yaml-cpp + + roscpp + roslib + std_msgs + sensor_msgs + humanoid_robot_controller_msgs + dynamixel_sdk + humanoid_robot_device + humanoid_robot_framework_common + cmake_modules + yaml-cpp + + ament_cmake + roscpp + roslib + std_msgs + sensor_msgs + humanoid_robot_controller_msgs + dynamixel_sdk + humanoid_robot_device + humanoid_robot_framework_common + cmake_modules + yaml-cpp - - roscpp - roslib - std_msgs - sensor_msgs - humanoid_robot_controller_msgs - dynamixel_sdk - humanoid_robot_device - humanoid_robot_framework_common - cmake_modules - yaml-cpp - - roscpp - roslib - std_msgs - sensor_msgs - humanoid_robot_controller_msgs - dynamixel_sdk - humanoid_robot_device - humanoid_robot_framework_common - cmake_modules - yaml-cpp - - roscpp - roslib - std_msgs - sensor_msgs - humanoid_robot_controller_msgs - dynamixel_sdk - humanoid_robot_device - humanoid_robot_framework_common - cmake_modules - yaml-cpp - - - - - roscpp - roslib - std_msgs - sensor_msgs - humanoid_robot_controller_msgs - dynamixel_sdk - humanoid_robot_device - humanoid_robot_framework_common - cmake_modules - yaml-cpp - - roscpp - roslib - std_msgs - sensor_msgs - humanoid_robot_controller_msgs - dynamixel_sdk - humanoid_robot_device - humanoid_robot_framework_common - cmake_modules - yaml-cpp - - roscpp - roslib - std_msgs - sensor_msgs - humanoid_robot_controller_msgs - dynamixel_sdk - humanoid_robot_device - humanoid_robot_framework_common - cmake_modules - yaml-cpp - + roscpp + roslib + std_msgs + sensor_msgs + humanoid_robot_controller_msgs + dynamixel_sdk + humanoid_robot_device + humanoid_robot_framework_common + cmake_modules + yaml-cpp + roscpp + roslib + std_msgs + sensor_msgs + humanoid_robot_controller_msgs + dynamixel_sdk + humanoid_robot_device + humanoid_robot_framework_common + cmake_modules + yaml-cpp diff --git a/humanoid_robot_device/CMakeLists.txt b/humanoid_robot_device/CMakeLists.txt index 2e140bb..6b135e4 100755 --- a/humanoid_robot_device/CMakeLists.txt +++ b/humanoid_robot_device/CMakeLists.txt @@ -20,8 +20,6 @@ if($ENV{ROS_VERSION} EQUAL 1) roscpp dynamixel_sdk ) -else() - ament_package() endif() diff --git a/humanoid_robot_device/package.xml b/humanoid_robot_device/package.xml index 8212686..a762ecb 100755 --- a/humanoid_robot_device/package.xml +++ b/humanoid_robot_device/package.xml @@ -1,41 +1,38 @@ humanoid_robot_device - 0.3.0 + 0.3.2 The package that manages device information of ROBOTIS robots. This package is used when reading device information with the robot information file from the humanoid_robot_controller package. Apache 2.0 + Ronaldson Bellande + catkin + + roscpp + dynamixel_sdk + + roscpp + dynamixel_sdk + + roscpp + dynamixel_sdk + + ament_cmake + roscpp + dynamixel_sdk - - roscpp - dynamixel_sdk - - roscpp - dynamixel_sdk - - roscpp - dynamixel_sdk - - - - - roscpp - dynamixel_sdk - - roscpp - dynamixel_sdk - - roscpp - dynamixel_sdk - + roscpp + dynamixel_sdk + roscpp + dynamixel_sdk diff --git a/humanoid_robot_framework/CMakeLists.txt b/humanoid_robot_framework/CMakeLists.txt index 15783dc..3b60b66 100644 --- a/humanoid_robot_framework/CMakeLists.txt +++ b/humanoid_robot_framework/CMakeLists.txt @@ -9,8 +9,6 @@ endif() if($ENV{ROS_VERSION} EQUAL 1) catkin_package() -else() - ament_package() endif() catkin_metapackage() diff --git a/humanoid_robot_framework/package.xml b/humanoid_robot_framework/package.xml index 8d245e3..365606a 100755 --- a/humanoid_robot_framework/package.xml +++ b/humanoid_robot_framework/package.xml @@ -1,44 +1,40 @@ humanoid_robot_framework - 0.3.0 + 0.3.2 ROS packages for the humanoid_robot_framework (meta package) Apache 2.0 Ronaldson Bellande + catkin + + humanoid_robot_controller + humanoid_robot_device + humanoid_robot_framework_common + + humanoid_robot_controller + humanoid_robot_device + humanoid_robot_framework_common + + humanoid_robot_controller + humanoid_robot_device + humanoid_robot_framework_common + + ament_cmake + humanoid_robot_controller + humanoid_robot_device + humanoid_robot_framework_common - - humanoid_robot_controller - humanoid_robot_device - humanoid_robot_framework_common - - humanoid_robot_controller - humanoid_robot_device - humanoid_robot_framework_common - - humanoid_robot_controller - humanoid_robot_device - humanoid_robot_framework_common - - - - - humanoid_robot_controller - humanoid_robot_device - humanoid_robot_framework_common - - humanoid_robot_controller - humanoid_robot_device - humanoid_robot_framework_common - - humanoid_robot_controller - humanoid_robot_device - humanoid_robot_framework_common - + humanoid_robot_controller + humanoid_robot_device + humanoid_robot_framework_common + humanoid_robot_controller + humanoid_robot_device + humanoid_robot_framework_common diff --git a/humanoid_robot_framework_common/CMakeLists.txt b/humanoid_robot_framework_common/CMakeLists.txt index f5d0175..4c306a2 100755 --- a/humanoid_robot_framework_common/CMakeLists.txt +++ b/humanoid_robot_framework_common/CMakeLists.txt @@ -20,8 +20,6 @@ if($ENV{ROS_VERSION} EQUAL 1) roscpp humanoid_robot_device ) -else() - ament_package() endif() diff --git a/humanoid_robot_framework_common/package.xml b/humanoid_robot_framework_common/package.xml index f7aa9b2..49f6de3 100755 --- a/humanoid_robot_framework_common/package.xml +++ b/humanoid_robot_framework_common/package.xml @@ -1,37 +1,33 @@ humanoid_robot_framework_common - 0.2.9 - The package contains commonly used Headers for the ROBOTIS Framework. + 0.3.2 + The package contains commonly used headers for the humanoid_robot_framework Apache 2.0 Ronaldson Bellande + catkin + + roscpp + humanoid_robot_device + + roscpp + humanoid_robot_device + + roscpp + humanoid_robot_device + + ament_cmake + roscpp + humanoid_robot_device - - roscpp - humanoid_robot_device - - roscpp - humanoid_robot_device - - roscpp - humanoid_robot_device - - - - - roscpp - humanoid_robot_device - - roscpp - humanoid_robot_device - - roscpp - humanoid_robot_device - + roscpp + humanoid_robot_device + roscpp + humanoid_robot_device From af4df6a648807f64d53c08885dfc13c67a597e8b Mon Sep 17 00:00:00 2001 From: Ronaldson Bellande <47253433+RonaldsonBellande@users.noreply.github.com> Date: Wed, 11 Oct 2023 11:43:46 -0400 Subject: [PATCH 193/238] Create jekyll-gh-pages.yml --- .github/workflows/jekyll-gh-pages.yml | 51 +++++++++++++++++++++++++++ 1 file changed, 51 insertions(+) create mode 100644 .github/workflows/jekyll-gh-pages.yml diff --git a/.github/workflows/jekyll-gh-pages.yml b/.github/workflows/jekyll-gh-pages.yml new file mode 100644 index 0000000..559bddf --- /dev/null +++ b/.github/workflows/jekyll-gh-pages.yml @@ -0,0 +1,51 @@ +# Sample workflow for building and deploying a Jekyll site to GitHub Pages +name: Deploy Jekyll with GitHub Pages dependencies preinstalled + +on: + # Runs on pushes targeting the default branch + push: + branches: ["main"] + + # Allows you to run this workflow manually from the Actions tab + workflow_dispatch: + +# Sets permissions of the GITHUB_TOKEN to allow deployment to GitHub Pages +permissions: + contents: read + pages: write + id-token: write + +# Allow only one concurrent deployment, skipping runs queued between the run in-progress and latest queued. +# However, do NOT cancel in-progress runs as we want to allow these production deployments to complete. +concurrency: + group: "pages" + cancel-in-progress: false + +jobs: + # Build job + build: + runs-on: ubuntu-latest + steps: + - name: Checkout + uses: actions/checkout@v3 + - name: Setup Pages + uses: actions/configure-pages@v3 + - name: Build with Jekyll + uses: actions/jekyll-build-pages@v1 + with: + source: ./ + destination: ./_site + - name: Upload artifact + uses: actions/upload-pages-artifact@v2 + + # Deployment job + deploy: + environment: + name: github-pages + url: ${{ steps.deployment.outputs.page_url }} + runs-on: ubuntu-latest + needs: build + steps: + - name: Deploy to GitHub Pages + id: deployment + uses: actions/deploy-pages@v2 From 742de0ea3280e63e71b52c05efcb0cc809eb9c37 Mon Sep 17 00:00:00 2001 From: Ronaldson Bellande <47253433+RonaldsonBellande@users.noreply.github.com> Date: Wed, 11 Oct 2023 11:43:46 -0400 Subject: [PATCH 194/238] Create jekyll-gh-pages.yml --- .github/workflows/jekyll-gh-pages.yml | 51 +++++++++++++++++++++++++++ 1 file changed, 51 insertions(+) create mode 100644 .github/workflows/jekyll-gh-pages.yml diff --git a/.github/workflows/jekyll-gh-pages.yml b/.github/workflows/jekyll-gh-pages.yml new file mode 100644 index 0000000..559bddf --- /dev/null +++ b/.github/workflows/jekyll-gh-pages.yml @@ -0,0 +1,51 @@ +# Sample workflow for building and deploying a Jekyll site to GitHub Pages +name: Deploy Jekyll with GitHub Pages dependencies preinstalled + +on: + # Runs on pushes targeting the default branch + push: + branches: ["main"] + + # Allows you to run this workflow manually from the Actions tab + workflow_dispatch: + +# Sets permissions of the GITHUB_TOKEN to allow deployment to GitHub Pages +permissions: + contents: read + pages: write + id-token: write + +# Allow only one concurrent deployment, skipping runs queued between the run in-progress and latest queued. +# However, do NOT cancel in-progress runs as we want to allow these production deployments to complete. +concurrency: + group: "pages" + cancel-in-progress: false + +jobs: + # Build job + build: + runs-on: ubuntu-latest + steps: + - name: Checkout + uses: actions/checkout@v3 + - name: Setup Pages + uses: actions/configure-pages@v3 + - name: Build with Jekyll + uses: actions/jekyll-build-pages@v1 + with: + source: ./ + destination: ./_site + - name: Upload artifact + uses: actions/upload-pages-artifact@v2 + + # Deployment job + deploy: + environment: + name: github-pages + url: ${{ steps.deployment.outputs.page_url }} + runs-on: ubuntu-latest + needs: build + steps: + - name: Deploy to GitHub Pages + id: deployment + uses: actions/deploy-pages@v2 From 52f9b6783bd781c4762d71f025a4aa560ffa9d61 Mon Sep 17 00:00:00 2001 From: Ronaldson Bellande <47253433+RonaldsonBellande@users.noreply.github.com> Date: Thu, 12 Oct 2023 17:47:20 -0400 Subject: [PATCH 195/238] Update README.md --- README.md | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/README.md b/README.md index 28df683..a33ed5e 100755 --- a/README.md +++ b/README.md @@ -1,5 +1,9 @@ # ROS/ROS2 Humanoid Robot Framework +-------------------------------------------------------------------------------------------------------- +# Website +https://robotics-sensors.github.io/humanoid_robot_framework + -------------------------------------------------------------------------------------------------------- Updated Version [humanoid_robot_framework](https://github.com/Robotics-Sensors/humanoid_robot_framework) readme. From b0d8d4c926f84e8a1e8627aa531f139e1bd06182 Mon Sep 17 00:00:00 2001 From: Ronaldson Bellande <47253433+RonaldsonBellande@users.noreply.github.com> Date: Thu, 12 Oct 2023 17:47:20 -0400 Subject: [PATCH 196/238] Update README.md --- README.md | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/README.md b/README.md index 28df683..a33ed5e 100755 --- a/README.md +++ b/README.md @@ -1,5 +1,9 @@ # ROS/ROS2 Humanoid Robot Framework +-------------------------------------------------------------------------------------------------------- +# Website +https://robotics-sensors.github.io/humanoid_robot_framework + -------------------------------------------------------------------------------------------------------- Updated Version [humanoid_robot_framework](https://github.com/Robotics-Sensors/humanoid_robot_framework) readme. From 7d98629015244ad437614090e0a12fe3a1bf96c6 Mon Sep 17 00:00:00 2001 From: Ronaldson Bellande <47253433+RonaldsonBellande@users.noreply.github.com> Date: Tue, 24 Oct 2023 08:55:08 -0400 Subject: [PATCH 197/238] Update README.md --- README.md | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index a33ed5e..0feb9f3 100755 --- a/README.md +++ b/README.md @@ -1,7 +1,10 @@ # ROS/ROS2 Humanoid Robot Framework +[![Website](https://img.shields.io/badge/Visit%20our-Website-0099cc?style=for-the-badge)](https://robotics-sensors.github.io) +[![Discord](https://img.shields.io/badge/Join%20our-Discord-7289DA?logo=discord&style=for-the-badge)](https://discord.gg/Yc72nd4w) +[![Sponsor](https://img.shields.io/badge/Sponsor-Robotics%20Sensors%20Research-red?style=for-the-badge&logo=github)](https://github.com/sponsors/Robotics-Sensors) -------------------------------------------------------------------------------------------------------- -# Website +# Repository Website https://robotics-sensors.github.io/humanoid_robot_framework -------------------------------------------------------------------------------------------------------- From f026762b0f126bd30b9444a7d44c1ef18990479a Mon Sep 17 00:00:00 2001 From: Ronaldson Bellande <47253433+RonaldsonBellande@users.noreply.github.com> Date: Tue, 24 Oct 2023 08:55:08 -0400 Subject: [PATCH 198/238] Update README.md --- README.md | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index a33ed5e..0feb9f3 100755 --- a/README.md +++ b/README.md @@ -1,7 +1,10 @@ # ROS/ROS2 Humanoid Robot Framework +[![Website](https://img.shields.io/badge/Visit%20our-Website-0099cc?style=for-the-badge)](https://robotics-sensors.github.io) +[![Discord](https://img.shields.io/badge/Join%20our-Discord-7289DA?logo=discord&style=for-the-badge)](https://discord.gg/Yc72nd4w) +[![Sponsor](https://img.shields.io/badge/Sponsor-Robotics%20Sensors%20Research-red?style=for-the-badge&logo=github)](https://github.com/sponsors/Robotics-Sensors) -------------------------------------------------------------------------------------------------------- -# Website +# Repository Website https://robotics-sensors.github.io/humanoid_robot_framework -------------------------------------------------------------------------------------------------------- From 235ad37b352a0cedb3a4f2f954aa040349d6fcec Mon Sep 17 00:00:00 2001 From: RonaldsonBellande Date: Wed, 1 Nov 2023 14:27:32 -0400 Subject: [PATCH 199/238] latest pushes --- humanoid_robot_framework/CMakeLists.txt | 2 -- 1 file changed, 2 deletions(-) diff --git a/humanoid_robot_framework/CMakeLists.txt b/humanoid_robot_framework/CMakeLists.txt index 3b60b66..aa57783 100644 --- a/humanoid_robot_framework/CMakeLists.txt +++ b/humanoid_robot_framework/CMakeLists.txt @@ -10,5 +10,3 @@ endif() if($ENV{ROS_VERSION} EQUAL 1) catkin_package() endif() - -catkin_metapackage() From dcd2729f63e93cbaa44b384097a448a45a66bbcc Mon Sep 17 00:00:00 2001 From: Ronaldson Bellande <47253433+RonaldsonBellande@users.noreply.github.com> Date: Thu, 2 Nov 2023 08:34:14 -0400 Subject: [PATCH 200/238] Create dependabot.yml --- .github/dependabot.yml | 11 +++++++++++ 1 file changed, 11 insertions(+) create mode 100644 .github/dependabot.yml diff --git a/.github/dependabot.yml b/.github/dependabot.yml new file mode 100644 index 0000000..ac6621f --- /dev/null +++ b/.github/dependabot.yml @@ -0,0 +1,11 @@ +# To get started with Dependabot version updates, you'll need to specify which +# package ecosystems to update and where the package manifests are located. +# Please see the documentation for all configuration options: +# https://docs.github.com/github/administering-a-repository/configuration-options-for-dependency-updates + +version: 2 +updates: + - package-ecosystem: "" # See documentation for possible values + directory: "/" # Location of package manifests + schedule: + interval: "weekly" From 8a8b9497300dbc6c904449edf326af741facda61 Mon Sep 17 00:00:00 2001 From: Ronaldson Bellande <47253433+RonaldsonBellande@users.noreply.github.com> Date: Thu, 2 Nov 2023 08:34:14 -0400 Subject: [PATCH 201/238] Create dependabot.yml --- .github/dependabot.yml | 11 +++++++++++ 1 file changed, 11 insertions(+) create mode 100644 .github/dependabot.yml diff --git a/.github/dependabot.yml b/.github/dependabot.yml new file mode 100644 index 0000000..ac6621f --- /dev/null +++ b/.github/dependabot.yml @@ -0,0 +1,11 @@ +# To get started with Dependabot version updates, you'll need to specify which +# package ecosystems to update and where the package manifests are located. +# Please see the documentation for all configuration options: +# https://docs.github.com/github/administering-a-repository/configuration-options-for-dependency-updates + +version: 2 +updates: + - package-ecosystem: "" # See documentation for possible values + directory: "/" # Location of package manifests + schedule: + interval: "weekly" From 8206ea19cb06172c3836f5d89439f4c8f4eb9dc7 Mon Sep 17 00:00:00 2001 From: Ronaldson Bellande <47253433+RonaldsonBellande@users.noreply.github.com> Date: Thu, 2 Nov 2023 08:34:22 -0400 Subject: [PATCH 202/238] Create codeql.yml --- .github/workflows/codeql.yml | 82 ++++++++++++++++++++++++++++++++++++ 1 file changed, 82 insertions(+) create mode 100644 .github/workflows/codeql.yml diff --git a/.github/workflows/codeql.yml b/.github/workflows/codeql.yml new file mode 100644 index 0000000..8b343fe --- /dev/null +++ b/.github/workflows/codeql.yml @@ -0,0 +1,82 @@ +# For most projects, this workflow file will not need changing; you simply need +# to commit it to your repository. +# +# You may wish to alter this file to override the set of languages analyzed, +# or to provide custom queries or build logic. +# +# ******** NOTE ******** +# We have attempted to detect the languages in your repository. Please check +# the `language` matrix defined below to confirm you have the correct set of +# supported CodeQL languages. +# +name: "CodeQL" + +on: + push: + branches: [ "main" ] + pull_request: + # The branches below must be a subset of the branches above + branches: [ "main" ] + schedule: + - cron: '37 12 * * 4' + +jobs: + analyze: + name: Analyze + # Runner size impacts CodeQL analysis time. To learn more, please see: + # - https://gh.io/recommended-hardware-resources-for-running-codeql + # - https://gh.io/supported-runners-and-hardware-resources + # - https://gh.io/using-larger-runners + # Consider using larger runners for possible analysis time improvements. + runs-on: ${{ (matrix.language == 'swift' && 'macos-latest') || 'ubuntu-latest' }} + timeout-minutes: ${{ (matrix.language == 'swift' && 120) || 360 }} + permissions: + actions: read + contents: read + security-events: write + + strategy: + fail-fast: false + matrix: + language: [ 'c-cpp' ] + # CodeQL supports [ 'c-cpp', 'csharp', 'go', 'java-kotlin', 'javascript-typescript', 'python', 'ruby', 'swift' ] + # Use only 'java-kotlin' to analyze code written in Java, Kotlin or both + # Use only 'javascript-typescript' to analyze code written in JavaScript, TypeScript or both + # Learn more about CodeQL language support at https://aka.ms/codeql-docs/language-support + + steps: + - name: Checkout repository + uses: actions/checkout@v3 + + # Initializes the CodeQL tools for scanning. + - name: Initialize CodeQL + uses: github/codeql-action/init@v2 + with: + languages: ${{ matrix.language }} + # If you wish to specify custom queries, you can do so here or in a config file. + # By default, queries listed here will override any specified in a config file. + # Prefix the list here with "+" to use these queries and those in the config file. + + # For more details on CodeQL's query packs, refer to: https://docs.github.com/en/code-security/code-scanning/automatically-scanning-your-code-for-vulnerabilities-and-errors/configuring-code-scanning#using-queries-in-ql-packs + # queries: security-extended,security-and-quality + + + # Autobuild attempts to build any compiled languages (C/C++, C#, Go, Java, or Swift). + # If this step fails, then you should remove it and run the build manually (see below) + - name: Autobuild + uses: github/codeql-action/autobuild@v2 + + # ℹ️ Command-line programs to run using the OS shell. + # 📚 See https://docs.github.com/en/actions/using-workflows/workflow-syntax-for-github-actions#jobsjob_idstepsrun + + # If the Autobuild fails above, remove it and uncomment the following three lines. + # modify them (or add more) to build your code if your project, please refer to the EXAMPLE below for guidance. + + # - run: | + # echo "Run, Build Application using script" + # ./location_of_script_within_repo/buildscript.sh + + - name: Perform CodeQL Analysis + uses: github/codeql-action/analyze@v2 + with: + category: "/language:${{matrix.language}}" From 438764c723c9483a9f63aeb9fe5b0bc0cf30896e Mon Sep 17 00:00:00 2001 From: Ronaldson Bellande <47253433+RonaldsonBellande@users.noreply.github.com> Date: Thu, 2 Nov 2023 08:34:22 -0400 Subject: [PATCH 203/238] Create codeql.yml --- .github/workflows/codeql.yml | 82 ++++++++++++++++++++++++++++++++++++ 1 file changed, 82 insertions(+) create mode 100644 .github/workflows/codeql.yml diff --git a/.github/workflows/codeql.yml b/.github/workflows/codeql.yml new file mode 100644 index 0000000..8b343fe --- /dev/null +++ b/.github/workflows/codeql.yml @@ -0,0 +1,82 @@ +# For most projects, this workflow file will not need changing; you simply need +# to commit it to your repository. +# +# You may wish to alter this file to override the set of languages analyzed, +# or to provide custom queries or build logic. +# +# ******** NOTE ******** +# We have attempted to detect the languages in your repository. Please check +# the `language` matrix defined below to confirm you have the correct set of +# supported CodeQL languages. +# +name: "CodeQL" + +on: + push: + branches: [ "main" ] + pull_request: + # The branches below must be a subset of the branches above + branches: [ "main" ] + schedule: + - cron: '37 12 * * 4' + +jobs: + analyze: + name: Analyze + # Runner size impacts CodeQL analysis time. To learn more, please see: + # - https://gh.io/recommended-hardware-resources-for-running-codeql + # - https://gh.io/supported-runners-and-hardware-resources + # - https://gh.io/using-larger-runners + # Consider using larger runners for possible analysis time improvements. + runs-on: ${{ (matrix.language == 'swift' && 'macos-latest') || 'ubuntu-latest' }} + timeout-minutes: ${{ (matrix.language == 'swift' && 120) || 360 }} + permissions: + actions: read + contents: read + security-events: write + + strategy: + fail-fast: false + matrix: + language: [ 'c-cpp' ] + # CodeQL supports [ 'c-cpp', 'csharp', 'go', 'java-kotlin', 'javascript-typescript', 'python', 'ruby', 'swift' ] + # Use only 'java-kotlin' to analyze code written in Java, Kotlin or both + # Use only 'javascript-typescript' to analyze code written in JavaScript, TypeScript or both + # Learn more about CodeQL language support at https://aka.ms/codeql-docs/language-support + + steps: + - name: Checkout repository + uses: actions/checkout@v3 + + # Initializes the CodeQL tools for scanning. + - name: Initialize CodeQL + uses: github/codeql-action/init@v2 + with: + languages: ${{ matrix.language }} + # If you wish to specify custom queries, you can do so here or in a config file. + # By default, queries listed here will override any specified in a config file. + # Prefix the list here with "+" to use these queries and those in the config file. + + # For more details on CodeQL's query packs, refer to: https://docs.github.com/en/code-security/code-scanning/automatically-scanning-your-code-for-vulnerabilities-and-errors/configuring-code-scanning#using-queries-in-ql-packs + # queries: security-extended,security-and-quality + + + # Autobuild attempts to build any compiled languages (C/C++, C#, Go, Java, or Swift). + # If this step fails, then you should remove it and run the build manually (see below) + - name: Autobuild + uses: github/codeql-action/autobuild@v2 + + # ℹ️ Command-line programs to run using the OS shell. + # 📚 See https://docs.github.com/en/actions/using-workflows/workflow-syntax-for-github-actions#jobsjob_idstepsrun + + # If the Autobuild fails above, remove it and uncomment the following three lines. + # modify them (or add more) to build your code if your project, please refer to the EXAMPLE below for guidance. + + # - run: | + # echo "Run, Build Application using script" + # ./location_of_script_within_repo/buildscript.sh + + - name: Perform CodeQL Analysis + uses: github/codeql-action/analyze@v2 + with: + category: "/language:${{matrix.language}}" From 8e1c39b247a3ec64aafc57119dedc19c0878249b Mon Sep 17 00:00:00 2001 From: Ronaldson Bellande <47253433+RonaldsonBellande@users.noreply.github.com> Date: Fri, 3 Nov 2023 13:27:21 -0400 Subject: [PATCH 204/238] Update README.md --- README.md | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index 0feb9f3..be2980b 100755 --- a/README.md +++ b/README.md @@ -9,9 +9,11 @@ https://robotics-sensors.github.io/humanoid_robot_framework -------------------------------------------------------------------------------------------------------- Updated Version [humanoid_robot_framework](https://github.com/Robotics-Sensors/humanoid_robot_framework) readme. - Old Version/Previous Used for Different Context [ROBOTIS-Framework](https://github.com/ROBOTIS-GIT/ROBOTIS-Framework) readme. +# Contact +If you are interested in accessing the complete version of this repository, we kindly request that you reach out to the organization's director or the company behind this project. We invite you to explore the USE CASE to understand the specific terms and conditions for usage. To utilize this repository, it is imperative that you adhere to the guidelines outlined in the USE CASE. For those interested in showing their support, becoming a sponsor of the organization is an option, and detailed information can be found within the USE CASE and License documents. Furthermore, we encourage you to join our official Discord community, where you can engage with like-minded individuals, contribute to the project, and stay up-to-date with the latest developments. It's worth noting that while a fraction of research is publicly accessible, the majority remains private. To gain insights into the wealth of knowledge held by the Company or the organization's director, we recommend direct contact for more information. + -------------------------------------------------------------------------------------------------------- ## Important The repository has diverged, as the old commits and codes are under the previous License and From d749a81e2340087f93441e8c87ddd1f8bac42434 Mon Sep 17 00:00:00 2001 From: Ronaldson Bellande <47253433+RonaldsonBellande@users.noreply.github.com> Date: Fri, 3 Nov 2023 13:27:21 -0400 Subject: [PATCH 205/238] Update README.md --- README.md | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index 0feb9f3..be2980b 100755 --- a/README.md +++ b/README.md @@ -9,9 +9,11 @@ https://robotics-sensors.github.io/humanoid_robot_framework -------------------------------------------------------------------------------------------------------- Updated Version [humanoid_robot_framework](https://github.com/Robotics-Sensors/humanoid_robot_framework) readme. - Old Version/Previous Used for Different Context [ROBOTIS-Framework](https://github.com/ROBOTIS-GIT/ROBOTIS-Framework) readme. +# Contact +If you are interested in accessing the complete version of this repository, we kindly request that you reach out to the organization's director or the company behind this project. We invite you to explore the USE CASE to understand the specific terms and conditions for usage. To utilize this repository, it is imperative that you adhere to the guidelines outlined in the USE CASE. For those interested in showing their support, becoming a sponsor of the organization is an option, and detailed information can be found within the USE CASE and License documents. Furthermore, we encourage you to join our official Discord community, where you can engage with like-minded individuals, contribute to the project, and stay up-to-date with the latest developments. It's worth noting that while a fraction of research is publicly accessible, the majority remains private. To gain insights into the wealth of knowledge held by the Company or the organization's director, we recommend direct contact for more information. + -------------------------------------------------------------------------------------------------------- ## Important The repository has diverged, as the old commits and codes are under the previous License and From 3aa752898232920329014becff9c16d9a70f46cc Mon Sep 17 00:00:00 2001 From: Ronaldson Bellande <47253433+RonaldsonBellande@users.noreply.github.com> Date: Sat, 4 Nov 2023 10:46:11 -0400 Subject: [PATCH 206/238] Update README.md --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index be2980b..b891c4f 100755 --- a/README.md +++ b/README.md @@ -12,7 +12,7 @@ Updated Version [humanoid_robot_framework](https://github.com/Robotics-Sensors/h Old Version/Previous Used for Different Context [ROBOTIS-Framework](https://github.com/ROBOTIS-GIT/ROBOTIS-Framework) readme. # Contact -If you are interested in accessing the complete version of this repository, we kindly request that you reach out to the organization's director or the company behind this project. We invite you to explore the USE CASE to understand the specific terms and conditions for usage. To utilize this repository, it is imperative that you adhere to the guidelines outlined in the USE CASE. For those interested in showing their support, becoming a sponsor of the organization is an option, and detailed information can be found within the USE CASE and License documents. Furthermore, we encourage you to join our official Discord community, where you can engage with like-minded individuals, contribute to the project, and stay up-to-date with the latest developments. It's worth noting that while a fraction of research is publicly accessible, the majority remains private. To gain insights into the wealth of knowledge held by the Company or the organization's director, we recommend direct contact for more information. +Depeding on the repository, If you are interested in accessing the complete version or other repository related to this repository, we kindly request that you reach out to the organization's director or the company behind this project. We invite you to explore the USE CASE to understand the specific terms and conditions for usage. To utilize this repository, it is imperative that you adhere to the guidelines outlined in the USE CASE. For those interested in showing their support, becoming a sponsor of the organization is an option, and detailed information can be found within the USE CASE and License documents. Furthermore, we encourage you to join our official Discord community, where you can engage with like-minded individuals, contribute to the project, and stay up-to-date with the latest developments. It's worth noting that while a fraction of research is publicly accessible, the majority remains private. To gain insights into the wealth of knowledge held by the Company or the organization's director, we recommend direct contact for more information. -------------------------------------------------------------------------------------------------------- ## Important From 79c80e965dbbb3dd411aae60982dd4675ac201c0 Mon Sep 17 00:00:00 2001 From: Ronaldson Bellande <47253433+RonaldsonBellande@users.noreply.github.com> Date: Sat, 4 Nov 2023 10:46:11 -0400 Subject: [PATCH 207/238] Update README.md --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index be2980b..b891c4f 100755 --- a/README.md +++ b/README.md @@ -12,7 +12,7 @@ Updated Version [humanoid_robot_framework](https://github.com/Robotics-Sensors/h Old Version/Previous Used for Different Context [ROBOTIS-Framework](https://github.com/ROBOTIS-GIT/ROBOTIS-Framework) readme. # Contact -If you are interested in accessing the complete version of this repository, we kindly request that you reach out to the organization's director or the company behind this project. We invite you to explore the USE CASE to understand the specific terms and conditions for usage. To utilize this repository, it is imperative that you adhere to the guidelines outlined in the USE CASE. For those interested in showing their support, becoming a sponsor of the organization is an option, and detailed information can be found within the USE CASE and License documents. Furthermore, we encourage you to join our official Discord community, where you can engage with like-minded individuals, contribute to the project, and stay up-to-date with the latest developments. It's worth noting that while a fraction of research is publicly accessible, the majority remains private. To gain insights into the wealth of knowledge held by the Company or the organization's director, we recommend direct contact for more information. +Depeding on the repository, If you are interested in accessing the complete version or other repository related to this repository, we kindly request that you reach out to the organization's director or the company behind this project. We invite you to explore the USE CASE to understand the specific terms and conditions for usage. To utilize this repository, it is imperative that you adhere to the guidelines outlined in the USE CASE. For those interested in showing their support, becoming a sponsor of the organization is an option, and detailed information can be found within the USE CASE and License documents. Furthermore, we encourage you to join our official Discord community, where you can engage with like-minded individuals, contribute to the project, and stay up-to-date with the latest developments. It's worth noting that while a fraction of research is publicly accessible, the majority remains private. To gain insights into the wealth of knowledge held by the Company or the organization's director, we recommend direct contact for more information. -------------------------------------------------------------------------------------------------------- ## Important From 7edddeb9067045b31a76a7403e00edded2e23983 Mon Sep 17 00:00:00 2001 From: Ronaldson Bellande <47253433+RonaldsonBellande@users.noreply.github.com> Date: Sun, 5 Nov 2023 01:52:15 -0500 Subject: [PATCH 208/238] Update README.md --- README.md | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/README.md b/README.md index b891c4f..9968397 100755 --- a/README.md +++ b/README.md @@ -3,6 +3,19 @@ [![Website](https://img.shields.io/badge/Visit%20our-Website-0099cc?style=for-the-badge)](https://robotics-sensors.github.io) [![Discord](https://img.shields.io/badge/Join%20our-Discord-7289DA?logo=discord&style=for-the-badge)](https://discord.gg/Yc72nd4w) [![Sponsor](https://img.shields.io/badge/Sponsor-Robotics%20Sensors%20Research-red?style=for-the-badge&logo=github)](https://github.com/sponsors/Robotics-Sensors) + +# Stats +[![GitHub stars](https://img.shields.io/github/stars/Robotics-Sensors/humanoid_robot_framework.svg?style=social)](https://github.com/Robotics-Sensors/humanoid_robot_framework/stargazers) +[![GitHub forks](https://img.shields.io/github/forks/Robotics-Sensors/humanoid_robot_framework.svg?style=social)](https://github.com/Robotics-Sensors/humanoid_robot_framework/network) +[![GitHub watchers](https://img.shields.io/github/watchers/Robotics-Sensors/humanoid_robot_framework.svg?style=social)](https://github.com/Robotics-Sensors/humanoid_robot_framework/watchers) + +[![GitHub issues](https://img.shields.io/github/issues/Robotics-Sensors/humanoid_robot_framework.svg)](https://github.com/Robotics-Sensors/humanoid_robot_framework/issues) +[![GitHub pull requests](https://img.shields.io/github/issues-pr/Robotics-Sensors/humanoid_robot_framework.svg)](https://github.com/Robotics-Sensors/humanoid_robot_framework/pulls) +[![GitHub license](https://img.shields.io/github/license/Robotics-Sensors/humanoid_robot_framework.svg)](https://github.com/Robotics-Sensors/humanoid_robot_framework/blob/main/LICENSE) + +[![GitHub last commit](https://img.shields.io/github/last-commit/Robotics-Sensors/humanoid_robot_framework.svg)](https://github.com/Robotics-Sensors/humanoid_robot_framework/commits) +[![Visitor & Github Clones](https://img.shields.io/badge/dynamic/json?color=2e8b57&label=Visitor%20%26%20GitHub%20Clones&query=$.count&url=https://api.github.com/repos/Robotics-Sensors/humanoid_robot_framework/traffic)](https://github.com/Robotics-Sensors/humanoid_robot_framework) + -------------------------------------------------------------------------------------------------------- # Repository Website https://robotics-sensors.github.io/humanoid_robot_framework @@ -11,6 +24,9 @@ https://robotics-sensors.github.io/humanoid_robot_framework Updated Version [humanoid_robot_framework](https://github.com/Robotics-Sensors/humanoid_robot_framework) readme. Old Version/Previous Used for Different Context [ROBOTIS-Framework](https://github.com/ROBOTIS-GIT/ROBOTIS-Framework) readme. +# Release +[![Latest Release](https://img.shields.io/github/v/release/Robotics-Sensors/humanoid_robot_tools?style=for-the-badge&color=yellow)](https://github.com/Robotics-Sensors/humanoid_robot_framework/releases/) + # Contact Depeding on the repository, If you are interested in accessing the complete version or other repository related to this repository, we kindly request that you reach out to the organization's director or the company behind this project. We invite you to explore the USE CASE to understand the specific terms and conditions for usage. To utilize this repository, it is imperative that you adhere to the guidelines outlined in the USE CASE. For those interested in showing their support, becoming a sponsor of the organization is an option, and detailed information can be found within the USE CASE and License documents. Furthermore, we encourage you to join our official Discord community, where you can engage with like-minded individuals, contribute to the project, and stay up-to-date with the latest developments. It's worth noting that while a fraction of research is publicly accessible, the majority remains private. To gain insights into the wealth of knowledge held by the Company or the organization's director, we recommend direct contact for more information. From 9bfd17ca910af3dafd3635a42305845f48e09668 Mon Sep 17 00:00:00 2001 From: Ronaldson Bellande <47253433+RonaldsonBellande@users.noreply.github.com> Date: Sun, 5 Nov 2023 01:52:15 -0500 Subject: [PATCH 209/238] Update README.md --- README.md | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/README.md b/README.md index b891c4f..9968397 100755 --- a/README.md +++ b/README.md @@ -3,6 +3,19 @@ [![Website](https://img.shields.io/badge/Visit%20our-Website-0099cc?style=for-the-badge)](https://robotics-sensors.github.io) [![Discord](https://img.shields.io/badge/Join%20our-Discord-7289DA?logo=discord&style=for-the-badge)](https://discord.gg/Yc72nd4w) [![Sponsor](https://img.shields.io/badge/Sponsor-Robotics%20Sensors%20Research-red?style=for-the-badge&logo=github)](https://github.com/sponsors/Robotics-Sensors) + +# Stats +[![GitHub stars](https://img.shields.io/github/stars/Robotics-Sensors/humanoid_robot_framework.svg?style=social)](https://github.com/Robotics-Sensors/humanoid_robot_framework/stargazers) +[![GitHub forks](https://img.shields.io/github/forks/Robotics-Sensors/humanoid_robot_framework.svg?style=social)](https://github.com/Robotics-Sensors/humanoid_robot_framework/network) +[![GitHub watchers](https://img.shields.io/github/watchers/Robotics-Sensors/humanoid_robot_framework.svg?style=social)](https://github.com/Robotics-Sensors/humanoid_robot_framework/watchers) + +[![GitHub issues](https://img.shields.io/github/issues/Robotics-Sensors/humanoid_robot_framework.svg)](https://github.com/Robotics-Sensors/humanoid_robot_framework/issues) +[![GitHub pull requests](https://img.shields.io/github/issues-pr/Robotics-Sensors/humanoid_robot_framework.svg)](https://github.com/Robotics-Sensors/humanoid_robot_framework/pulls) +[![GitHub license](https://img.shields.io/github/license/Robotics-Sensors/humanoid_robot_framework.svg)](https://github.com/Robotics-Sensors/humanoid_robot_framework/blob/main/LICENSE) + +[![GitHub last commit](https://img.shields.io/github/last-commit/Robotics-Sensors/humanoid_robot_framework.svg)](https://github.com/Robotics-Sensors/humanoid_robot_framework/commits) +[![Visitor & Github Clones](https://img.shields.io/badge/dynamic/json?color=2e8b57&label=Visitor%20%26%20GitHub%20Clones&query=$.count&url=https://api.github.com/repos/Robotics-Sensors/humanoid_robot_framework/traffic)](https://github.com/Robotics-Sensors/humanoid_robot_framework) + -------------------------------------------------------------------------------------------------------- # Repository Website https://robotics-sensors.github.io/humanoid_robot_framework @@ -11,6 +24,9 @@ https://robotics-sensors.github.io/humanoid_robot_framework Updated Version [humanoid_robot_framework](https://github.com/Robotics-Sensors/humanoid_robot_framework) readme. Old Version/Previous Used for Different Context [ROBOTIS-Framework](https://github.com/ROBOTIS-GIT/ROBOTIS-Framework) readme. +# Release +[![Latest Release](https://img.shields.io/github/v/release/Robotics-Sensors/humanoid_robot_tools?style=for-the-badge&color=yellow)](https://github.com/Robotics-Sensors/humanoid_robot_framework/releases/) + # Contact Depeding on the repository, If you are interested in accessing the complete version or other repository related to this repository, we kindly request that you reach out to the organization's director or the company behind this project. We invite you to explore the USE CASE to understand the specific terms and conditions for usage. To utilize this repository, it is imperative that you adhere to the guidelines outlined in the USE CASE. For those interested in showing their support, becoming a sponsor of the organization is an option, and detailed information can be found within the USE CASE and License documents. Furthermore, we encourage you to join our official Discord community, where you can engage with like-minded individuals, contribute to the project, and stay up-to-date with the latest developments. It's worth noting that while a fraction of research is publicly accessible, the majority remains private. To gain insights into the wealth of knowledge held by the Company or the organization's director, we recommend direct contact for more information. From 98cb5049c95d927cbdfdc99e087ec3111393d317 Mon Sep 17 00:00:00 2001 From: RonaldsonBellande Date: Sat, 11 Nov 2023 10:25:49 -0500 Subject: [PATCH 210/238] latest pushes --- humanoid_robot_framework/package.xml | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/humanoid_robot_framework/package.xml b/humanoid_robot_framework/package.xml index 365606a..1ff3e96 100755 --- a/humanoid_robot_framework/package.xml +++ b/humanoid_robot_framework/package.xml @@ -36,7 +36,5 @@ humanoid_robot_device humanoid_robot_framework_common - - - + From f64f3441845db54b33b2e120863a26df45a09c75 Mon Sep 17 00:00:00 2001 From: RonaldsonBellande Date: Mon, 13 Nov 2023 17:55:45 -0500 Subject: [PATCH 211/238] latest pushes --- .travis.yml | 2 +- README.md | 24 +- .../humanoid_robot_controller.cpp | 2600 ---------------- humanoid_robot_framework/package.xml | 40 - .../CHANGELOG.rst | 6 +- .../CMakeLists.txt | 24 +- ...t_intelligence_control_system_controller.h | 50 +- .../package.xml | 40 +- ...intelligence_control_system_controller.cpp | 2625 +++++++++++++++++ .../CHANGELOG.rst | 4 +- .../CMakeLists.txt | 18 +- .../devices/dynamixel/GRIPPER.device | 0 .../devices/dynamixel/GRIPPER_TORQUE.device | 0 .../devices/dynamixel/H42-20-S300-R(A).device | 0 .../devices/dynamixel/H42-20-S300-R.device | 0 .../devices/dynamixel/H42P-020-S300-R.device | 0 .../dynamixel/H54-100-B210-R-NR.device | 0 .../dynamixel/H54-100-S500-R(A).device | 0 .../devices/dynamixel/H54-100-S500-R.device | 0 .../devices/dynamixel/H54-200-B500-R.device | 0 .../dynamixel/H54-200-S500-R(A).device | 0 .../devices/dynamixel/H54-200-S500-R.device | 0 .../devices/dynamixel/H54P-100-S500-R.device | 0 .../devices/dynamixel/H54P-200-B500-R.device | 0 .../devices/dynamixel/H54P-200-S500-R.device | 0 .../devices/dynamixel/L42-10-S300-R.device | 0 .../devices/dynamixel/L54-30-S400-R.device | 0 .../devices/dynamixel/L54-30-S500-R.device | 0 .../devices/dynamixel/L54-50-S290-R.device | 0 .../devices/dynamixel/L54-50-S500-R.device | 0 .../devices/dynamixel/M42-10-S260-R.device | 0 .../devices/dynamixel/M54-40-S250-R.device | 0 .../devices/dynamixel/M54-60-S250-R.device | 0 .../devices/dynamixel/MX-106.device | 0 .../devices/dynamixel/MX-28.device | 0 .../devices/dynamixel/MX-64.device | 0 .../devices/dynamixel/PH42-020-S300-R.device | 0 .../devices/dynamixel/PH54-100-S500-R.device | 0 .../devices/dynamixel/PH54-200-S500-R.device | 0 .../devices/dynamixel/RH-P12-RN(A).device | 0 .../devices/dynamixel/RH-P12-RN.device | 0 .../devices/dynamixel/XM-430.device | 0 .../devices/dynamixel/XM430-W210.device | 0 .../devices/dynamixel/XM430-W350.device | 0 .../devices/dynamixel/XM540-W150.device | 0 .../devices/dynamixel/XM540-W270.device | 0 .../devices/sensor/CM-740.device | 0 .../devices/sensor/OPEN-CR.device | 0 .../control_table_item.h | 4 +- .../device.h | 2 +- .../dynamixel.h | 2 +- .../dynamixel_state.h | 2 +- .../robot.h | 2 +- .../sensor.h | 2 +- .../sensor_state.h | 2 +- .../time_stamp.h | 4 +- .../package.xml | 4 +- .../dynamixel.cpp | 4 +- .../robot.cpp | 4 +- .../sensor.cpp | 4 +- .../CHANGELOG.rst | 6 +- .../CMakeLists.txt | 2 +- .../package.xml | 40 + .../CHANGELOG.rst | 2 +- .../CMakeLists.txt | 8 +- .../motion_module.h | 6 +- .../sensor_module.h | 6 +- .../singleton.h | 2 +- .../package.xml | 16 +- 69 files changed, 2791 insertions(+), 2766 deletions(-) delete mode 100755 humanoid_robot_controller/src/humanoid_robot_controller/humanoid_robot_controller.cpp delete mode 100755 humanoid_robot_framework/package.xml rename {humanoid_robot_controller => humanoid_robot_intelligence_control_system_controller}/CHANGELOG.rst (94%) rename {humanoid_robot_controller => humanoid_robot_intelligence_control_system_controller}/CMakeLists.txt (56%) rename humanoid_robot_controller/include/humanoid_robot_controller/humanoid_robot_controller.h => humanoid_robot_intelligence_control_system_controller/include/humanoid_robot_intelligence_control_system_controller/humanoid_robot_intelligence_control_system_controller.h (72%) rename {humanoid_robot_controller => humanoid_robot_intelligence_control_system_controller}/package.xml (65%) create mode 100755 humanoid_robot_intelligence_control_system_controller/src/humanoid_robot_intelligence_control_system_controller/humanoid_robot_intelligence_control_system_controller.cpp rename {humanoid_robot_device => humanoid_robot_intelligence_control_system_device}/CHANGELOG.rst (95%) rename {humanoid_robot_device => humanoid_robot_intelligence_control_system_device}/CMakeLists.txt (51%) rename {humanoid_robot_device => humanoid_robot_intelligence_control_system_device}/devices/dynamixel/GRIPPER.device (100%) rename {humanoid_robot_device => humanoid_robot_intelligence_control_system_device}/devices/dynamixel/GRIPPER_TORQUE.device (100%) rename {humanoid_robot_device => humanoid_robot_intelligence_control_system_device}/devices/dynamixel/H42-20-S300-R(A).device (100%) rename {humanoid_robot_device => humanoid_robot_intelligence_control_system_device}/devices/dynamixel/H42-20-S300-R.device (100%) rename {humanoid_robot_device => humanoid_robot_intelligence_control_system_device}/devices/dynamixel/H42P-020-S300-R.device (100%) rename {humanoid_robot_device => humanoid_robot_intelligence_control_system_device}/devices/dynamixel/H54-100-B210-R-NR.device (100%) rename {humanoid_robot_device => humanoid_robot_intelligence_control_system_device}/devices/dynamixel/H54-100-S500-R(A).device (100%) rename {humanoid_robot_device => humanoid_robot_intelligence_control_system_device}/devices/dynamixel/H54-100-S500-R.device (100%) rename {humanoid_robot_device => humanoid_robot_intelligence_control_system_device}/devices/dynamixel/H54-200-B500-R.device (100%) rename {humanoid_robot_device => humanoid_robot_intelligence_control_system_device}/devices/dynamixel/H54-200-S500-R(A).device (100%) rename {humanoid_robot_device => humanoid_robot_intelligence_control_system_device}/devices/dynamixel/H54-200-S500-R.device (100%) rename {humanoid_robot_device => humanoid_robot_intelligence_control_system_device}/devices/dynamixel/H54P-100-S500-R.device (100%) rename {humanoid_robot_device => humanoid_robot_intelligence_control_system_device}/devices/dynamixel/H54P-200-B500-R.device (100%) rename {humanoid_robot_device => humanoid_robot_intelligence_control_system_device}/devices/dynamixel/H54P-200-S500-R.device (100%) rename {humanoid_robot_device => humanoid_robot_intelligence_control_system_device}/devices/dynamixel/L42-10-S300-R.device (100%) rename {humanoid_robot_device => humanoid_robot_intelligence_control_system_device}/devices/dynamixel/L54-30-S400-R.device (100%) rename {humanoid_robot_device => humanoid_robot_intelligence_control_system_device}/devices/dynamixel/L54-30-S500-R.device (100%) rename {humanoid_robot_device => humanoid_robot_intelligence_control_system_device}/devices/dynamixel/L54-50-S290-R.device (100%) rename {humanoid_robot_device => humanoid_robot_intelligence_control_system_device}/devices/dynamixel/L54-50-S500-R.device (100%) rename {humanoid_robot_device => humanoid_robot_intelligence_control_system_device}/devices/dynamixel/M42-10-S260-R.device (100%) rename {humanoid_robot_device => humanoid_robot_intelligence_control_system_device}/devices/dynamixel/M54-40-S250-R.device (100%) rename {humanoid_robot_device => humanoid_robot_intelligence_control_system_device}/devices/dynamixel/M54-60-S250-R.device (100%) rename {humanoid_robot_device => humanoid_robot_intelligence_control_system_device}/devices/dynamixel/MX-106.device (100%) rename {humanoid_robot_device => humanoid_robot_intelligence_control_system_device}/devices/dynamixel/MX-28.device (100%) rename {humanoid_robot_device => humanoid_robot_intelligence_control_system_device}/devices/dynamixel/MX-64.device (100%) rename {humanoid_robot_device => humanoid_robot_intelligence_control_system_device}/devices/dynamixel/PH42-020-S300-R.device (100%) rename {humanoid_robot_device => humanoid_robot_intelligence_control_system_device}/devices/dynamixel/PH54-100-S500-R.device (100%) rename {humanoid_robot_device => humanoid_robot_intelligence_control_system_device}/devices/dynamixel/PH54-200-S500-R.device (100%) rename {humanoid_robot_device => humanoid_robot_intelligence_control_system_device}/devices/dynamixel/RH-P12-RN(A).device (100%) rename {humanoid_robot_device => humanoid_robot_intelligence_control_system_device}/devices/dynamixel/RH-P12-RN.device (100%) rename {humanoid_robot_device => humanoid_robot_intelligence_control_system_device}/devices/dynamixel/XM-430.device (100%) rename {humanoid_robot_device => humanoid_robot_intelligence_control_system_device}/devices/dynamixel/XM430-W210.device (100%) rename {humanoid_robot_device => humanoid_robot_intelligence_control_system_device}/devices/dynamixel/XM430-W350.device (100%) rename {humanoid_robot_device => humanoid_robot_intelligence_control_system_device}/devices/dynamixel/XM540-W150.device (100%) rename {humanoid_robot_device => humanoid_robot_intelligence_control_system_device}/devices/dynamixel/XM540-W270.device (100%) rename {humanoid_robot_device => humanoid_robot_intelligence_control_system_device}/devices/sensor/CM-740.device (100%) rename {humanoid_robot_device => humanoid_robot_intelligence_control_system_device}/devices/sensor/OPEN-CR.device (100%) rename {humanoid_robot_device/include/humanoid_robot_device => humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device}/control_table_item.h (88%) rename {humanoid_robot_device/include/humanoid_robot_device => humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device}/device.h (91%) rename {humanoid_robot_device/include/humanoid_robot_device => humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device}/dynamixel.h (94%) rename {humanoid_robot_device/include/humanoid_robot_device => humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device}/dynamixel_state.h (93%) rename {humanoid_robot_device/include/humanoid_robot_device => humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device}/robot.h (93%) rename {humanoid_robot_device/include/humanoid_robot_device => humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device}/sensor.h (91%) rename {humanoid_robot_device/include/humanoid_robot_device => humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device}/sensor_state.h (91%) rename {humanoid_robot_device/include/humanoid_robot_device => humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device}/time_stamp.h (86%) rename {humanoid_robot_device => humanoid_robot_intelligence_control_system_device}/package.xml (91%) rename {humanoid_robot_device/src/humanoid_robot_device => humanoid_robot_intelligence_control_system_device/src/humanoid_robot_intelligence_control_system_device}/dynamixel.cpp (96%) rename {humanoid_robot_device/src/humanoid_robot_device => humanoid_robot_intelligence_control_system_device/src/humanoid_robot_intelligence_control_system_device}/robot.cpp (96%) rename {humanoid_robot_device/src/humanoid_robot_device => humanoid_robot_intelligence_control_system_device/src/humanoid_robot_intelligence_control_system_device}/sensor.cpp (86%) rename {humanoid_robot_framework => humanoid_robot_intelligence_control_system_framework}/CHANGELOG.rst (91%) rename {humanoid_robot_framework => humanoid_robot_intelligence_control_system_framework}/CMakeLists.txt (77%) create mode 100755 humanoid_robot_intelligence_control_system_framework/package.xml rename {humanoid_robot_framework_common => humanoid_robot_intelligence_control_system_framework_common}/CHANGELOG.rst (96%) rename {humanoid_robot_framework_common => humanoid_robot_intelligence_control_system_framework_common}/CMakeLists.txt (74%) rename {humanoid_robot_framework_common/include/humanoid_robot_framework_common => humanoid_robot_intelligence_control_system_framework_common/include/humanoid_robot_intelligence_control_system_framework_common}/motion_module.h (87%) rename {humanoid_robot_framework_common/include/humanoid_robot_framework_common => humanoid_robot_intelligence_control_system_framework_common/include/humanoid_robot_intelligence_control_system_framework_common}/sensor_module.h (84%) rename {humanoid_robot_framework_common/include/humanoid_robot_framework_common => humanoid_robot_intelligence_control_system_framework_common/include/humanoid_robot_intelligence_control_system_framework_common}/singleton.h (92%) rename {humanoid_robot_framework_common => humanoid_robot_intelligence_control_system_framework_common}/package.xml (58%) diff --git a/.travis.yml b/.travis.yml index e1e689a..43bc280 100644 --- a/.travis.yml +++ b/.travis.yml @@ -18,7 +18,7 @@ notifications: - ronaldsonbellande@gmail.com env: matrix: - - ROS_DISTRO=noetic ROS_REPO=ros-shadow-fixed UPSTREAM_WORKSPACE=file OS_NAME=ubuntu OS_CODE_NAME=focal $ROSINSTALL_FILENAME=".humanoid_robot_framework.rosinstall" + - ROS_DISTRO=noetic ROS_REPO=ros-shadow-fixed UPSTREAM_WORKSPACE=file OS_NAME=ubuntu OS_CODE_NAME=focal $ROSINSTALL_FILENAME=".humanoid_robot_intelligence_control_system_framework.rosinstall" branches: only: - master diff --git a/README.md b/README.md index 9968397..8b0d5d4 100755 --- a/README.md +++ b/README.md @@ -5,27 +5,27 @@ [![Sponsor](https://img.shields.io/badge/Sponsor-Robotics%20Sensors%20Research-red?style=for-the-badge&logo=github)](https://github.com/sponsors/Robotics-Sensors) # Stats -[![GitHub stars](https://img.shields.io/github/stars/Robotics-Sensors/humanoid_robot_framework.svg?style=social)](https://github.com/Robotics-Sensors/humanoid_robot_framework/stargazers) -[![GitHub forks](https://img.shields.io/github/forks/Robotics-Sensors/humanoid_robot_framework.svg?style=social)](https://github.com/Robotics-Sensors/humanoid_robot_framework/network) -[![GitHub watchers](https://img.shields.io/github/watchers/Robotics-Sensors/humanoid_robot_framework.svg?style=social)](https://github.com/Robotics-Sensors/humanoid_robot_framework/watchers) +[![GitHub stars](https://img.shields.io/github/stars/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework.svg?style=social)](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework/stargazers) +[![GitHub forks](https://img.shields.io/github/forks/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework.svg?style=social)](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework/network) +[![GitHub watchers](https://img.shields.io/github/watchers/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework.svg?style=social)](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework/watchers) -[![GitHub issues](https://img.shields.io/github/issues/Robotics-Sensors/humanoid_robot_framework.svg)](https://github.com/Robotics-Sensors/humanoid_robot_framework/issues) -[![GitHub pull requests](https://img.shields.io/github/issues-pr/Robotics-Sensors/humanoid_robot_framework.svg)](https://github.com/Robotics-Sensors/humanoid_robot_framework/pulls) -[![GitHub license](https://img.shields.io/github/license/Robotics-Sensors/humanoid_robot_framework.svg)](https://github.com/Robotics-Sensors/humanoid_robot_framework/blob/main/LICENSE) +[![GitHub issues](https://img.shields.io/github/issues/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework.svg)](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework/issues) +[![GitHub pull requests](https://img.shields.io/github/issues-pr/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework.svg)](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework/pulls) +[![GitHub license](https://img.shields.io/github/license/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework.svg)](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework/blob/main/LICENSE) -[![GitHub last commit](https://img.shields.io/github/last-commit/Robotics-Sensors/humanoid_robot_framework.svg)](https://github.com/Robotics-Sensors/humanoid_robot_framework/commits) -[![Visitor & Github Clones](https://img.shields.io/badge/dynamic/json?color=2e8b57&label=Visitor%20%26%20GitHub%20Clones&query=$.count&url=https://api.github.com/repos/Robotics-Sensors/humanoid_robot_framework/traffic)](https://github.com/Robotics-Sensors/humanoid_robot_framework) +[![GitHub last commit](https://img.shields.io/github/last-commit/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework.svg)](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework/commits) +[![Visitor & Github Clones](https://img.shields.io/badge/dynamic/json?color=2e8b57&label=Visitor%20%26%20GitHub%20Clones&query=$.count&url=https://api.github.com/repos/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework/traffic)](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework) -------------------------------------------------------------------------------------------------------- # Repository Website -https://robotics-sensors.github.io/humanoid_robot_framework +https://robotics-sensors.github.io/humanoid_robot_intelligence_control_system_framework -------------------------------------------------------------------------------------------------------- -Updated Version [humanoid_robot_framework](https://github.com/Robotics-Sensors/humanoid_robot_framework) readme. +Updated Version [humanoid_robot_intelligence_control_system_framework](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework) readme. Old Version/Previous Used for Different Context [ROBOTIS-Framework](https://github.com/ROBOTIS-GIT/ROBOTIS-Framework) readme. # Release -[![Latest Release](https://img.shields.io/github/v/release/Robotics-Sensors/humanoid_robot_tools?style=for-the-badge&color=yellow)](https://github.com/Robotics-Sensors/humanoid_robot_framework/releases/) +[![Latest Release](https://img.shields.io/github/v/release/Robotics-Sensors/humanoid_robot_intelligence_control_system_tools?style=for-the-badge&color=yellow)](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework/releases/) # Contact Depeding on the repository, If you are interested in accessing the complete version or other repository related to this repository, we kindly request that you reach out to the organization's director or the company behind this project. We invite you to explore the USE CASE to understand the specific terms and conditions for usage. To utilize this repository, it is imperative that you adhere to the guidelines outlined in the USE CASE. For those interested in showing their support, becoming a sponsor of the organization is an option, and detailed information can be found within the USE CASE and License documents. Furthermore, we encourage you to join our official Discord community, where you can engage with like-minded individuals, contribute to the project, and stay up-to-date with the latest developments. It's worth noting that while a fraction of research is publicly accessible, the majority remains private. To gain insights into the wealth of knowledge held by the Company or the organization's director, we recommend direct contact for more information. @@ -48,4 +48,4 @@ Latest versions and Maintainer is on organization https://github.com/Robotics-Se * Ronaldson Bellande ## License -This SDK is distributed under the [Apache License, Version 2.0](https://www.apache.org/licenses/LICENSE-2.0), see [LICENSE](https://github.com/Robotics-Sensors/humanoid_robot_framework/blob/main/LICENSE) and [NOTICE](https://github.com/Robotics-Sensors/humanoid_robot_framework/blob/main/LICENSE) for more information. +This SDK is distributed under the [Apache License, Version 2.0](https://www.apache.org/licenses/LICENSE-2.0), see [LICENSE](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework/blob/main/LICENSE) and [NOTICE](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework/blob/main/LICENSE) for more information. diff --git a/humanoid_robot_controller/src/humanoid_robot_controller/humanoid_robot_controller.cpp b/humanoid_robot_controller/src/humanoid_robot_controller/humanoid_robot_controller.cpp deleted file mode 100755 index 47b8d2a..0000000 --- a/humanoid_robot_controller/src/humanoid_robot_controller/humanoid_robot_controller.cpp +++ /dev/null @@ -1,2600 +0,0 @@ -/******************************************************************************* -* Copyright 2018 ROBOTIS CO., LTD. -* -* 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. -*******************************************************************************/ - -/* - * humanoid_robot_controller.cpp - * - * Created on: 2016. 1. 15. - * Author: zerom - */ - -#include -#include - -#include "humanoid_robot_controller/humanoid_robot_controller.h" - -using namespace humanoid_robot_framework; - -RobotisController::RobotisController() - : is_timer_running_(false), - is_offset_enabled_(true), - offset_ratio_(1.0), - stop_timer_(false), - init_pose_loaded_(false), - timer_thread_(0), - controller_mode_(MotionModuleMode), - DEBUG_PRINT(false), - robot_(0), - gazebo_mode_(false), - gazebo_robot_name_("humanoid_robot") -{ - direct_sync_write_.clear(); -} - -void RobotisController::initializeSyncWrite() -{ - if (gazebo_mode_ == true) - return; - - //ROS_INFO("FIRST BULKREAD"); - for (auto& it : port_to_bulk_read_) - it.second->txRxPacket(); - for(auto& it : port_to_bulk_read_) - { - int error_count = 0; - int result = COMM_SUCCESS; - do - { - if (++error_count > 10) - { - ROS_ERROR("[RobotisController] first bulk read fail!!"); - exit(-1); - } - usleep(10 * 1000); - result = it.second->txRxPacket(); - } while (result != COMM_SUCCESS); - } - init_pose_loaded_ = true; - //ROS_INFO("FIRST BULKREAD END"); - - // clear syncwrite param setting - for (auto& it : port_to_sync_write_position_) - { - if (it.second != NULL) - it.second->clearParam(); - } - for (auto& it : port_to_sync_write_position_p_gain_) - { - if (it.second != NULL) - it.second->clearParam(); - } - for (auto& it : port_to_sync_write_position_i_gain_) - { - if (it.second != NULL) - it.second->clearParam(); - } - for (auto& it : port_to_sync_write_position_d_gain_) - { - if (it.second != NULL) - it.second->clearParam(); - } - for (auto& it : port_to_sync_write_velocity_) - { - if (it.second != NULL) - it.second->clearParam(); - } - for (auto& it : port_to_sync_write_velocity_p_gain_) - { - if (it.second != NULL) - it.second->clearParam(); - } - for (auto& it : port_to_sync_write_velocity_i_gain_) - { - if (it.second != NULL) - it.second->clearParam(); - } - for (auto& it : port_to_sync_write_velocity_d_gain_) - { - if (it.second != NULL) - it.second->clearParam(); - } - for (auto& it : port_to_sync_write_current_) - { - if (it.second != NULL) - it.second->clearParam(); - } - - // set init syncwrite param(from data of bulkread) - for (auto& it : robot_->dxls_) - { - std::string joint_name = it.first; - Dynamixel *dxl = it.second; - - for (int i = 0; i < dxl->bulk_read_items_.size(); i++) - { - uint32_t read_data = 0; - uint8_t sync_write_data[4]; - - if (port_to_bulk_read_[dxl->port_name_]->isAvailable(dxl->id_, - dxl->bulk_read_items_[i]->address_, - dxl->bulk_read_items_[i]->data_length_) == true) - { - read_data = port_to_bulk_read_[dxl->port_name_]->getData(dxl->id_, - dxl->bulk_read_items_[i]->address_, - dxl->bulk_read_items_[i]->data_length_); - - sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(read_data)); - sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(read_data)); - sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(read_data)); - sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(read_data)); - - if ((dxl->present_position_item_ != 0) && - (dxl->bulk_read_items_[i]->item_name_ == dxl->present_position_item_->item_name_)) - { - dxl->dxl_state_->present_position_ = dxl->convertValue2Radian(read_data) - dxl->dxl_state_->position_offset_ * offset_ratio_; - dxl->dxl_state_->goal_position_ = dxl->dxl_state_->present_position_; - - port_to_sync_write_position_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); - } - else if ((dxl->position_p_gain_item_ != 0) && - (dxl->bulk_read_items_[i]->item_name_ == dxl->position_p_gain_item_->item_name_)) - { - dxl->dxl_state_->position_p_gain_ = read_data; - } - else if ((dxl->position_i_gain_item_ != 0) && - (dxl->bulk_read_items_[i]->item_name_ == dxl->position_i_gain_item_->item_name_)) - { - dxl->dxl_state_->position_i_gain_ = read_data; - } - else if ((dxl->position_d_gain_item_ != 0) && - (dxl->bulk_read_items_[i]->item_name_ == dxl->position_d_gain_item_->item_name_)) - { - dxl->dxl_state_->position_d_gain_ = read_data; - } - else if ((dxl->present_velocity_item_ != 0) && - (dxl->bulk_read_items_[i]->item_name_ == dxl->present_velocity_item_->item_name_)) - { - dxl->dxl_state_->present_velocity_ = dxl->convertValue2Velocity(read_data); - dxl->dxl_state_->goal_velocity_ = dxl->dxl_state_->present_velocity_; - } - else if ((dxl->velocity_p_gain_item_ != 0) && - (dxl->bulk_read_items_[i]->item_name_ == dxl->velocity_p_gain_item_->item_name_)) - { - dxl->dxl_state_->velocity_p_gain_ = read_data; - } - else if ((dxl->velocity_i_gain_item_ != 0) && - (dxl->bulk_read_items_[i]->item_name_ == dxl->velocity_i_gain_item_->item_name_)) - { - dxl->dxl_state_->velocity_i_gain_ = read_data; - } - else if ((dxl->velocity_d_gain_item_ != 0) && - (dxl->bulk_read_items_[i]->item_name_ == dxl->velocity_d_gain_item_->item_name_)) - { - dxl->dxl_state_->velocity_d_gain_ = read_data; - } - else if ((dxl->present_current_item_ != 0) && - (dxl->bulk_read_items_[i]->item_name_ == dxl->present_current_item_->item_name_)) - { - dxl->dxl_state_->present_torque_ = dxl->convertValue2Torque(read_data); - dxl->dxl_state_->goal_torque_ = dxl->dxl_state_->present_torque_; - } - } - } - } -} - -bool RobotisController::initialize(const std::string robot_file_path, const std::string init_file_path) -{ - std::string dev_desc_dir_path = ros::package::getPath("humanoid_robot_device") + "/devices"; - - // load robot info : port , device - robot_ = new Robot(robot_file_path, dev_desc_dir_path); - - if (gazebo_mode_ == true) - { - queue_thread_ = boost::thread(boost::bind(&RobotisController::msgQueueThread, this)); - return true; - } - - for (auto& it : robot_->ports_) - { - std::string port_name = it.first; - dynamixel::PortHandler *port = it.second; - dynamixel::PacketHandler *default_pkt_handler = dynamixel::PacketHandler::getPacketHandler(2.0); - - if (port->setBaudRate(port->getBaudRate()) == false) - { - ROS_ERROR("PORT [%s] SETUP ERROR! (baudrate: %d)", port_name.c_str(), port->getBaudRate()); - exit(-1); - } - - // get the default device info of the port - std::string default_device_name = robot_->port_default_device_[port_name]; - auto dxl_it = robot_->dxls_.find(default_device_name); - auto sensor_it = robot_->sensors_.find(default_device_name); - if (dxl_it != robot_->dxls_.end()) - { - Dynamixel *default_device = dxl_it->second; - default_pkt_handler = dynamixel::PacketHandler::getPacketHandler(default_device->protocol_version_); - - if (default_device->goal_position_item_ != 0) - { - port_to_sync_write_position_[port_name] - = new dynamixel::GroupSyncWrite(port, - default_pkt_handler, - default_device->goal_position_item_->address_, - default_device->goal_position_item_->data_length_); - } - - if (default_device->position_p_gain_item_ != 0) - { - port_to_sync_write_position_p_gain_[port_name] - = new dynamixel::GroupSyncWrite(port, - default_pkt_handler, - default_device->position_p_gain_item_->address_, - default_device->position_p_gain_item_->data_length_); - } - - if (default_device->position_i_gain_item_ != 0) - { - port_to_sync_write_position_i_gain_[port_name] - = new dynamixel::GroupSyncWrite(port, - default_pkt_handler, - default_device->position_i_gain_item_->address_, - default_device->position_i_gain_item_->data_length_); - } - - if (default_device->position_d_gain_item_ != 0) - { - port_to_sync_write_position_d_gain_[port_name] - = new dynamixel::GroupSyncWrite(port, - default_pkt_handler, - default_device->position_d_gain_item_->address_, - default_device->position_d_gain_item_->data_length_); - } - - if (default_device->goal_velocity_item_ != 0) - { - port_to_sync_write_velocity_[port_name] - = new dynamixel::GroupSyncWrite(port, - default_pkt_handler, - default_device->goal_velocity_item_->address_, - default_device->goal_velocity_item_->data_length_); - } - - if (default_device->velocity_p_gain_item_ != 0) - { - port_to_sync_write_velocity_p_gain_[port_name] - = new dynamixel::GroupSyncWrite(port, - default_pkt_handler, - default_device->velocity_p_gain_item_->address_, - default_device->velocity_p_gain_item_->data_length_); - } - - if (default_device->velocity_i_gain_item_ != 0) - { - port_to_sync_write_velocity_i_gain_[port_name] - = new dynamixel::GroupSyncWrite(port, - default_pkt_handler, - default_device->velocity_i_gain_item_->address_, - default_device->velocity_i_gain_item_->data_length_); - } - - if (default_device->velocity_d_gain_item_ != 0) - { - port_to_sync_write_velocity_d_gain_[port_name] - = new dynamixel::GroupSyncWrite(port, - default_pkt_handler, - default_device->velocity_d_gain_item_->address_, - default_device->velocity_d_gain_item_->data_length_); - } - - if (default_device->goal_current_item_ != 0) - { - port_to_sync_write_current_[port_name] - = new dynamixel::GroupSyncWrite(port, - default_pkt_handler, - default_device->goal_current_item_->address_, - default_device->goal_current_item_->data_length_); - } - } - else if (sensor_it != robot_->sensors_.end()) - { - Sensor *_default_device = sensor_it->second; - default_pkt_handler = dynamixel::PacketHandler::getPacketHandler(_default_device->protocol_version_); - } - - port_to_bulk_read_[port_name] = new dynamixel::GroupBulkRead(port, default_pkt_handler); - } - - // (for loop) check all dxls are connected. - for (auto& it : robot_->dxls_) - { - std::string joint_name = it.first; - Dynamixel *dxl = it.second; - - if (ping(joint_name) != 0) - { - usleep(10 * 1000); - if (ping(joint_name) != 0) - ROS_ERROR("JOINT[%s] does NOT respond!!", joint_name.c_str()); - } - } - - initializeDevice(init_file_path); - - queue_thread_ = boost::thread(boost::bind(&RobotisController::msgQueueThread, this)); - return true; -} - -void RobotisController::initializeDevice(const std::string init_file_path) -{ - // device initialize - if (DEBUG_PRINT) - ROS_WARN("INIT FILE LOAD"); - - YAML::Node doc; - try - { - doc = YAML::LoadFile(init_file_path.c_str()); - - for (YAML::const_iterator it_doc = doc.begin(); it_doc != doc.end(); it_doc++) - { - std::string joint_name = it_doc->first.as(); - - YAML::Node joint_node = doc[joint_name]; - if (joint_node.size() == 0) - continue; - - Dynamixel *dxl = NULL; - auto dxl_it = robot_->dxls_.find(joint_name); - if (dxl_it != robot_->dxls_.end()) - dxl = dxl_it->second; - - if (dxl == NULL) - { - ROS_WARN("Joint [%s] was not found.", joint_name.c_str()); - continue; - } - if (DEBUG_PRINT) - ROS_INFO("JOINT_NAME: %s", joint_name.c_str()); - - uint8_t torque_enabled = 0; - read1Byte(joint_name, dxl->torque_enable_item_->address_, &torque_enabled); - - for (YAML::const_iterator it_joint = joint_node.begin(); it_joint != joint_node.end(); it_joint++) - { - std::string item_name = it_joint->first.as(); - - if (DEBUG_PRINT) - ROS_INFO(" ITEM_NAME: %s", item_name.c_str()); - - uint32_t value = it_joint->second.as(); - - ControlTableItem *item = dxl->ctrl_table_[item_name]; - if (item == NULL) - { - ROS_WARN("Control Item [%s] was not found.", item_name.c_str()); - continue; - } - - if (item->memory_type_ == EEPROM) - { - uint8_t data8 = 0; - uint16_t data16 = 0; - uint32_t data32 = 0; - - switch (item->data_length_) - { - case 1: - read1Byte(joint_name, item->address_, &data8); - if (data8 == value) - continue; - break; - case 2: - read2Byte(joint_name, item->address_, &data16); - if (data16 == value) - continue; - break; - case 4: - read4Byte(joint_name, item->address_, &data32); - if (data32 == value) - continue; - break; - default: - break; - } - - if (torque_enabled == 1) - { - ROS_ERROR("################\nThe initial value of the EEPROM area has been changed. \nTurn off Torque Enable and try again."); - exit(-1); - } - } - - switch (item->data_length_) - { - case 1: - write1Byte(joint_name, item->address_, (uint8_t) value); - break; - case 2: - write2Byte(joint_name, item->address_, (uint16_t) value); - break; - case 4: - write4Byte(joint_name, item->address_, value); - break; - default: - break; - } - - if (item->memory_type_ == EEPROM) - { - // Write to EEPROM -> delay is required (max delay: 55 msec per byte) - usleep(item->data_length_ * 55 * 1000); - } - } - } - } catch (const std::exception& e) - { - ROS_INFO("Dynamixel Init file not found."); - } - - // [ BulkRead ] StartAddress : Present Position , Length : 10 ( Position/Velocity/Current ) - for (auto& it : robot_->ports_) - { - if (port_to_bulk_read_[it.first] != 0) - port_to_bulk_read_[it.first]->clearParam(); - } - for (auto& it : robot_->dxls_) - { - std::string joint_name = it.first; - Dynamixel *dxl = it.second; - - if (dxl == NULL) - continue; - - int bulkread_start_addr = 0; - int bulkread_data_length = 0; - -// // bulk read default : present position -// if(dxl->present_position_item != 0) -// { -// bulkread_start_addr = dxl->present_position_item->address; -// bulkread_data_length = dxl->present_position_item->data_length; -// } - - uint8_t torque_enabled = 0; - read1Byte(joint_name, dxl->torque_enable_item_->address_, &torque_enabled); - - // calculate bulk read start address & data length - auto indirect_addr_it = dxl->ctrl_table_.find(INDIRECT_ADDRESS_1); - if (indirect_addr_it != dxl->ctrl_table_.end()) // INDIRECT_ADDRESS_1 exist - { - if (dxl->bulk_read_items_.size() != 0) - { - uint16_t data16 = 0; - - bulkread_start_addr = dxl->bulk_read_items_[0]->address_; - bulkread_data_length = 0; - - // set indirect address - int indirect_addr = indirect_addr_it->second->address_; - for (int i = 0; i < dxl->bulk_read_items_.size(); i++) - { - int addr_leng = dxl->bulk_read_items_[i]->data_length_; - - bulkread_data_length += addr_leng; - for (int l = 0; l < addr_leng; l++) - { - // ROS_WARN("[%12s] INDIR_ADDR: %d, ITEM_ADDR: %d", joint_name.c_str(), indirect_addr, dxl->ctrl_table[dxl->bulk_read_items[i]->item_name]->address + _l); - - read2Byte(joint_name, indirect_addr, &data16); - if (data16 != dxl->ctrl_table_[dxl->bulk_read_items_[i]->item_name_]->address_ + l) - { - if (torque_enabled == 1) - { - ROS_ERROR("################\nThe indirect address of the EEPROM area has been changed. \nTurn off Torque Enable and try again."); - exit(-1); - } - write2Byte(joint_name, indirect_addr, dxl->ctrl_table_[dxl->bulk_read_items_[i]->item_name_]->address_ + l); - } - indirect_addr += 2; - } - } - } - } - else // INDIRECT_ADDRESS_1 NOT exist - { - if (dxl->bulk_read_items_.size() != 0) - { - bulkread_start_addr = dxl->bulk_read_items_[0]->address_; - bulkread_data_length = 0; - - ControlTableItem *last_item = dxl->bulk_read_items_[0]; - - for (int i = 0; i < dxl->bulk_read_items_.size(); i++) - { - int addr = dxl->bulk_read_items_[i]->address_; - if (addr < bulkread_start_addr) - bulkread_start_addr = addr; - else if (last_item->address_ < addr) - last_item = dxl->bulk_read_items_[i]; - } - - bulkread_data_length = last_item->address_ - bulkread_start_addr + last_item->data_length_; - } - } - -// ROS_WARN("[%12s] start_addr: %d, data_length: %d", joint_name.c_str(), bulkread_start_addr, bulkread_data_length); - if (bulkread_start_addr != 0) - port_to_bulk_read_[dxl->port_name_]->addParam(dxl->id_, bulkread_start_addr, bulkread_data_length); - - // Torque ON - if (writeCtrlItem(joint_name, dxl->torque_enable_item_->item_name_, 1) != COMM_SUCCESS) - writeCtrlItem(joint_name, dxl->torque_enable_item_->item_name_, 1); - } - - for (auto& it : robot_->sensors_) - { - std::string sensor_name = it.first; - Sensor *sensor = it.second; - - if (sensor == NULL) - continue; - - int bulkread_start_addr = 0; - int bulkread_data_length = 0; - - // calculate bulk read start address & data length - auto indirect_addr_it = sensor->ctrl_table_.find(INDIRECT_ADDRESS_1); - if (indirect_addr_it != sensor->ctrl_table_.end()) // INDIRECT_ADDRESS_1 exist - { - if (sensor->bulk_read_items_.size() != 0) - { - uint16_t data16 = 0; - - bulkread_start_addr = sensor->bulk_read_items_[0]->address_; - bulkread_data_length = 0; - - // set indirect address - int indirect_addr = indirect_addr_it->second->address_; - for (int i = 0; i < sensor->bulk_read_items_.size(); i++) - { - int addr_leng = sensor->bulk_read_items_[i]->data_length_; - - bulkread_data_length += addr_leng; - for (int l = 0; l < addr_leng; l++) - { -// ROS_WARN("[%12s] INDIR_ADDR: %d, ITEM_ADDR: %d", sensor_name.c_str(), indirect_addr, sensor->ctrl_table[sensor->bulk_read_items[i]->item_name]->address + _l); - read2Byte(sensor_name, indirect_addr, &data16); - if (data16 != sensor->ctrl_table_[sensor->bulk_read_items_[i]->item_name_]->address_ + l) - { - write2Byte(sensor_name, - indirect_addr, - sensor->ctrl_table_[sensor->bulk_read_items_[i]->item_name_]->address_ + l); - } - indirect_addr += 2; - } - } - } - } - else // INDIRECT_ADDRESS_1 NOT exist - { - if (sensor->bulk_read_items_.size() != 0) - { - bulkread_start_addr = sensor->bulk_read_items_[0]->address_; - bulkread_data_length = 0; - - ControlTableItem *last_item = sensor->bulk_read_items_[0]; - - for (int i = 0; i < sensor->bulk_read_items_.size(); i++) - { - int addr = sensor->bulk_read_items_[i]->address_; - if (addr < bulkread_start_addr) - bulkread_start_addr = addr; - else if (last_item->address_ < addr) - last_item = sensor->bulk_read_items_[i]; - } - - bulkread_data_length = last_item->address_ - bulkread_start_addr + last_item->data_length_; - } - } - - //ROS_WARN("[%12s] start_addr: %d, data_length: %d", sensor_name.c_str(), bulkread_start_addr, bulkread_data_length); - if (bulkread_start_addr != 0) - port_to_bulk_read_[sensor->port_name_]->addParam(sensor->id_, bulkread_start_addr, bulkread_data_length); - } -} - -void RobotisController::gazeboTimerThread() -{ - ros::Rate gazebo_rate(1000 / robot_->getControlCycle()); - - while (!stop_timer_) - { - if (init_pose_loaded_ == true) - process(); - gazebo_rate.sleep(); - } -} - -void RobotisController::msgQueueThread() -{ - ros::NodeHandle ros_node; - ros::CallbackQueue callback_queue; - - ros_node.setCallbackQueue(&callback_queue); - - /* subscriber */ - ros::Subscriber write_control_table_sub = ros_node.subscribe("/humanoid_robot/write_control_table", 5, - &RobotisController::writeControlTableCallback, this); - ros::Subscriber sync_write_item_sub = ros_node.subscribe("/humanoid_robot/sync_write_item", 10, - &RobotisController::syncWriteItemCallback, this); - ros::Subscriber joint_ctrl_modules_sub = ros_node.subscribe("/humanoid_robot/set_joint_ctrl_modules", 10, - &RobotisController::setJointCtrlModuleCallback, this); - ros::Subscriber enable_ctrl_module_sub = ros_node.subscribe("/humanoid_robot/enable_ctrl_module", 10, - &RobotisController::setCtrlModuleCallback, this); - ros::Subscriber control_mode_sub = ros_node.subscribe("/humanoid_robot/set_control_mode", 10, - &RobotisController::setControllerModeCallback, this); - ros::Subscriber joint_states_sub = ros_node.subscribe("/humanoid_robot/set_joint_states", 10, - &RobotisController::setJointStatesCallback, this); - ros::Subscriber enable_offset_sub = ros_node.subscribe("/humanoid_robot/enable_offset", 10, - &RobotisController::enableOffsetCallback, this); - - ros::Subscriber gazebo_joint_states_sub; - if (gazebo_mode_ == true) - gazebo_joint_states_sub = ros_node.subscribe("/" + gazebo_robot_name_ + "/joint_states", 10, - &RobotisController::gazeboJointStatesCallback, this); - - /* publisher */ - goal_joint_state_pub_ = ros_node.advertise("/humanoid_robot/goal_joint_states", 10); - present_joint_state_pub_ = ros_node.advertise("/humanoid_robot/present_joint_states", 10); - current_module_pub_ = ros_node.advertise( - "/humanoid_robot/present_joint_ctrl_modules", 10); - - if (gazebo_mode_ == true) - { - for (auto& it : robot_->dxls_) - { - gazebo_joint_position_pub_[it.first] = ros_node.advertise( - "/" + gazebo_robot_name_ + "/" + it.first + "_position/command", 1); - gazebo_joint_velocity_pub_[it.first] = ros_node.advertise( - "/" + gazebo_robot_name_ + "/" + it.first + "_velocity/command", 1); - gazebo_joint_effort_pub_[it.first] = ros_node.advertise( - "/" + gazebo_robot_name_ + "/" + it.first + "_effort/command", 1); - } - } - - /* service */ - ros::ServiceServer get_joint_module_server = ros_node.advertiseService("/humanoid_robot/get_present_joint_ctrl_modules", - &RobotisController::getJointCtrlModuleService, this); - ros::ServiceServer set_joint_module_server = ros_node.advertiseService("/humanoid_robot/set_present_joint_ctrl_modules", - &RobotisController::setJointCtrlModuleService, this); - ros::ServiceServer set_module_server = ros_node.advertiseService("/humanoid_robot/set_present_ctrl_modules", - &RobotisController::setCtrlModuleService, this); - ros::ServiceServer load_offset_server = ros_node.advertiseService("/humanoid_robot/load_offset", - &RobotisController::loadOffsetService, this); - - ros::WallDuration duration(robot_->getControlCycle() / 1000.0); - while(ros_node.ok()) - callback_queue.callAvailable(duration); -} - -void *RobotisController::timerThread(void *param) -{ - RobotisController *controller = (RobotisController *) param; - static struct timespec next_time; - static struct timespec curr_time; - - ROS_DEBUG("controller::thread_proc started"); - - clock_gettime(CLOCK_MONOTONIC, &next_time); - - while (!controller->stop_timer_) - { - next_time.tv_sec += (next_time.tv_nsec + controller->robot_->getControlCycle() * 1000000) / 1000000000; - next_time.tv_nsec = (next_time.tv_nsec + controller->robot_->getControlCycle() * 1000000) % 1000000000; - - controller->process(); - - clock_gettime(CLOCK_MONOTONIC, &curr_time); - long delta_nsec = (next_time.tv_sec - curr_time.tv_sec) * 1000000000 + (next_time.tv_nsec - curr_time.tv_nsec); - if (delta_nsec < -100000) - { - if (controller->DEBUG_PRINT == true) - { - fprintf(stderr, "[RobotisController::ThreadProc] NEXT TIME < CURR TIME.. (%f)[%ld.%09ld / %ld.%09ld]", - delta_nsec / 1000000.0, (long )next_time.tv_sec, (long )next_time.tv_nsec, - (long )curr_time.tv_sec, (long )curr_time.tv_nsec); - } - - // next_time = curr_time + 3 msec - next_time.tv_sec = curr_time.tv_sec + (curr_time.tv_nsec + 3000000) / 1000000000; - next_time.tv_nsec = (curr_time.tv_nsec + 3000000) % 1000000000; - } - - clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &next_time, NULL); - } - return 0; -} - -void RobotisController::startTimer() -{ - if (this->is_timer_running_ == true) - return; - - if (this->gazebo_mode_ == true) - { - // create and start the thread - gazebo_thread_ = boost::thread(boost::bind(&RobotisController::gazeboTimerThread, this)); - } - else - { - initializeSyncWrite(); - - for (auto& it : port_to_bulk_read_) - { - it.second->txPacket(); - } - - usleep(8 * 1000); - - int error; - struct sched_param param; - pthread_attr_t attr; - - pthread_attr_init(&attr); - - error = pthread_attr_setschedpolicy(&attr, SCHED_RR); - if (error != 0) - ROS_ERROR("pthread_attr_setschedpolicy error = %d\n", error); - error = pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED); - if (error != 0) - ROS_ERROR("pthread_attr_setinheritsched error = %d\n", error); - - memset(¶m, 0, sizeof(param)); - param.sched_priority = 31; // RT - error = pthread_attr_setschedparam(&attr, ¶m); - if (error != 0) - ROS_ERROR("pthread_attr_setschedparam error = %d\n", error); - - // create and start the thread - if ((error = pthread_create(&this->timer_thread_, &attr, this->timerThread, this)) != 0) - { - ROS_ERROR("Creating timer thread failed!!"); - exit(-1); - } - } - - this->is_timer_running_ = true; -} - -void RobotisController::stopTimer() -{ - int error = 0; - - // set the flag to stop the thread - if (this->is_timer_running_) - { - this->stop_timer_ = true; - - if (this->gazebo_mode_ == false) - { - // wait until the thread is stopped. - if ((error = pthread_join(this->timer_thread_, NULL)) != 0) - exit(-1); - - for (auto& it : port_to_bulk_read_) - { - if (it.second != NULL) - it.second->rxPacket(); - } - - for (auto& it : port_to_sync_write_position_) - { - if (it.second != NULL) - it.second->clearParam(); - } - for (auto& it : port_to_sync_write_position_p_gain_) - { - if (it.second != NULL) - it.second->clearParam(); - } - for (auto& it : port_to_sync_write_position_i_gain_) - { - if (it.second != NULL) - it.second->clearParam(); - } - for (auto& it : port_to_sync_write_position_d_gain_) - { - if (it.second != NULL) - it.second->clearParam(); - } - for (auto& it : port_to_sync_write_velocity_) - { - if (it.second != NULL) - it.second->clearParam(); - } - for (auto& it : port_to_sync_write_velocity_p_gain_) - { - if (it.second != NULL) - it.second->clearParam(); - } - for (auto& it : port_to_sync_write_velocity_i_gain_) - { - if (it.second != NULL) - it.second->clearParam(); - } - for (auto& it : port_to_sync_write_velocity_d_gain_) - { - if (it.second != NULL) - it.second->clearParam(); - } - for (auto& it : port_to_sync_write_current_) - { - if (it.second != NULL) - it.second->clearParam(); - } - } - else - { - // wait until the thread is stopped. - gazebo_thread_.join(); - } - - this->stop_timer_ = false; - this->is_timer_running_ = false; - } -} - -bool RobotisController::isTimerRunning() -{ - return this->is_timer_running_; -} - -void RobotisController::loadOffset(const std::string path) -{ - YAML::Node doc; - try - { - doc = YAML::LoadFile(path.c_str()); - } catch (const std::exception& e) - { - ROS_WARN("Fail to load offset yaml."); - return; - } - - YAML::Node offset_node = doc["offset"]; - if (offset_node.size() == 0) - return; - - ROS_INFO("Load offsets..."); - for (YAML::const_iterator it = offset_node.begin(); it != offset_node.end(); it++) - { - std::string joint_name = it->first.as(); - double offset = it->second.as(); - - auto dxl_it = robot_->dxls_.find(joint_name); - if (dxl_it != robot_->dxls_.end()) - dxl_it->second->dxl_state_->position_offset_ = offset; - } -} - -void RobotisController::process() -{ - // avoid duplicated function call - static bool is_process_running = false; - if (is_process_running == true) - return; - is_process_running = true; - - // ROS_INFO("Controller::Process()"); - // offset ratio - if(is_offset_enabled_) - { - if(offset_ratio_ < 1.0) - offset_ratio_ += 0.01; - else - offset_ratio_ = 1.0; - } - else - { - if(offset_ratio_ > 0.0) - offset_ratio_ -= 0.01; - else - offset_ratio_ = 0.0; - } - - - ros::Time start_time; - ros::Duration time_duration; - - if (DEBUG_PRINT) - start_time = ros::Time::now(); - - sensor_msgs::JointState goal_state; - sensor_msgs::JointState present_state; - - present_state.header.stamp = ros::Time::now(); - goal_state.header.stamp = present_state.header.stamp; - - if (controller_mode_ == MotionModuleMode) - { - if (gazebo_mode_ == false) - { - // BulkRead Rx - for (auto& it : port_to_bulk_read_) - { - robot_->ports_[it.first]->setPacketTimeout(0.0); - if(DEBUG_PRINT) - { - int result = it.second->rxPacket(); - if(result != COMM_SUCCESS) - ROS_ERROR_STREAM("Bulk Read Fail : " << it.first); - } - else - it.second->rxPacket(); - } - - // -> save to robot->dxls_[]->dxl_state_ - if (robot_->dxls_.size() > 0) - { - for (auto& dxl_it : robot_->dxls_) - { - Dynamixel *dxl = dxl_it.second; - std::string port_name = dxl_it.second->port_name_; - std::string joint_name = dxl_it.first; - - if (dxl->bulk_read_items_.size() > 0) - { - bool updated = false; - uint32_t data = 0; - for (int i = 0; i < dxl->bulk_read_items_.size(); i++) - { - ControlTableItem *item = dxl->bulk_read_items_[i]; - if (port_to_bulk_read_[port_name]->isAvailable(dxl->id_, item->address_, item->data_length_) == true) - { - updated = true; - data = port_to_bulk_read_[port_name]->getData(dxl->id_, item->address_, item->data_length_); - - // change dxl_state - if (dxl->present_position_item_ != 0 && item->item_name_ == dxl->present_position_item_->item_name_) - { - dxl->dxl_state_->present_position_ = dxl->convertValue2Radian(data) - dxl->dxl_state_->position_offset_ * offset_ratio_; - } - else if (dxl->present_velocity_item_ != 0 && item->item_name_ == dxl->present_velocity_item_->item_name_) - dxl->dxl_state_->present_velocity_ = dxl->convertValue2Velocity(data); - else if (dxl->present_current_item_ != 0 && item->item_name_ == dxl->present_current_item_->item_name_) - dxl->dxl_state_->present_torque_ = dxl->convertValue2Torque(data); - else if (dxl->goal_position_item_ != 0 && item->item_name_ == dxl->goal_position_item_->item_name_) - { - dxl->dxl_state_->goal_position_ = dxl->convertValue2Radian(data) - dxl->dxl_state_->position_offset_ * offset_ratio_; - } - else if (dxl->goal_velocity_item_ != 0 && item->item_name_ == dxl->goal_velocity_item_->item_name_) - dxl->dxl_state_->goal_velocity_ = dxl->convertValue2Velocity(data); - else if (dxl->goal_current_item_ != 0 && item->item_name_ == dxl->goal_current_item_->item_name_) - dxl->dxl_state_->goal_torque_ = dxl->convertValue2Torque(data); - - dxl->dxl_state_->bulk_read_table_[item->item_name_] = data; - } - } - - // -> update time stamp to Robot->dxls[]->dynamixel_state.update_time_stamp - if (updated == true) - dxl->dxl_state_->update_time_stamp_ = TimeStamp(present_state.header.stamp.sec, present_state.header.stamp.nsec); - } - } - } - - // -> save to robot->sensors_[]->sensor_state_ - if (robot_->sensors_.size() > 0) - { - for (auto& sensor_it : robot_->sensors_) - { - Sensor *sensor = sensor_it.second; - std::string port_name = sensor_it.second->port_name_; - std::string sensor_name = sensor_it.first; - - if (sensor->bulk_read_items_.size() > 0) - { - bool updated = false; - uint32_t data = 0; - for (int i = 0; i < sensor->bulk_read_items_.size(); i++) - { - ControlTableItem *item = sensor->bulk_read_items_[i]; - if (port_to_bulk_read_[port_name]->isAvailable(sensor->id_, item->address_, item->data_length_) == true) - { - updated = true; - data = port_to_bulk_read_[port_name]->getData(sensor->id_, item->address_, item->data_length_); - - // change sensor_state - sensor->sensor_state_->bulk_read_table_[item->item_name_] = data; - } - } - - // -> update time stamp to Robot->dxls[]->dynamixel_state.update_time_stamp - if (updated == true) - sensor->sensor_state_->update_time_stamp_ = TimeStamp(present_state.header.stamp.sec, present_state.header.stamp.nsec); - } - } - } - - if (DEBUG_PRINT) - { - time_duration = ros::Time::now() - start_time; - fprintf(stderr, "(%2.6f) BulkRead Rx & update state \n", time_duration.nsec * 0.000001); - } - - // SyncWrite - queue_mutex_.lock(); - - if (direct_sync_write_.size() > 0) - { - for (int i = 0; i < direct_sync_write_.size(); i++) - { - direct_sync_write_[i]->txPacket(); - direct_sync_write_[i]->clearParam(); - } - direct_sync_write_.clear(); - } - - if (port_to_sync_write_position_p_gain_.size() > 0) - { - for (auto& it : port_to_sync_write_position_p_gain_) - { - it.second->txPacket(); - it.second->clearParam(); - } - } - if (port_to_sync_write_position_i_gain_.size() > 0) - { - for (auto& it : port_to_sync_write_position_i_gain_) - { - it.second->txPacket(); - it.second->clearParam(); - } - } - if (port_to_sync_write_position_d_gain_.size() > 0) - { - for (auto& it : port_to_sync_write_position_d_gain_) - { - it.second->txPacket(); - it.second->clearParam(); - } - } - if (port_to_sync_write_velocity_p_gain_.size() > 0) - { - for (auto& it : port_to_sync_write_velocity_p_gain_) - { - it.second->txPacket(); - it.second->clearParam(); - } - } - if (port_to_sync_write_velocity_i_gain_.size() > 0) - { - for (auto& it : port_to_sync_write_velocity_i_gain_) - { - it.second->txPacket(); - it.second->clearParam(); - } - } - if (port_to_sync_write_velocity_d_gain_.size() > 0) - { - for (auto& it : port_to_sync_write_velocity_d_gain_) - { - it.second->txPacket(); - it.second->clearParam(); - } - } - for (auto& it : port_to_sync_write_position_) - { - if (it.second != NULL) - it.second->txPacket(); - } - for (auto& it : port_to_sync_write_velocity_) - { - if (it.second != NULL) - it.second->txPacket(); - } - for (auto& it : port_to_sync_write_current_) - { - if (it.second != NULL) - it.second->txPacket(); - } - - queue_mutex_.unlock(); - - // BulkRead Tx - for (auto& it : port_to_bulk_read_) - it.second->txPacket(); - - if (DEBUG_PRINT) - { - time_duration = ros::Time::now() - start_time; - fprintf(stderr, "(%2.6f) SyncWrite & BulkRead Tx \n", time_duration.nsec * 0.000001); - } - } - else if (gazebo_mode_ == true) - { - std_msgs::Float64 joint_msg; - - for (auto& dxl_it : robot_->dxls_) - { - std::string joint_name = dxl_it.first; - Dynamixel *dxl = dxl_it.second; - DynamixelState *dxl_state = dxl_it.second->dxl_state_; - - if (dxl->ctrl_module_name_ == "none") - { - joint_msg.data = dxl_state->goal_position_; - gazebo_joint_position_pub_[joint_name].publish(joint_msg); - } - } - - for (auto module_it = motion_modules_.begin(); module_it != motion_modules_.end(); module_it++) - { - if ((*module_it)->getModuleEnable() == false) - continue; - - for (auto& dxl_it : robot_->dxls_) - { - std::string joint_name = dxl_it.first; - Dynamixel *dxl = dxl_it.second; - DynamixelState *dxl_state = dxl_it.second->dxl_state_; - - if (dxl->ctrl_module_name_ == (*module_it)->getModuleName()) - { - if ((*module_it)->getControlMode() == PositionControl) - { - joint_msg.data = dxl_state->goal_position_; - gazebo_joint_position_pub_[joint_name].publish(joint_msg); - } - else if ((*module_it)->getControlMode() == VelocityControl) - { - joint_msg.data = dxl_state->goal_velocity_; - gazebo_joint_velocity_pub_[joint_name].publish(joint_msg); - } - else if ((*module_it)->getControlMode() == TorqueControl) - { - joint_msg.data = dxl_state->goal_torque_; - gazebo_joint_effort_pub_[joint_name].publish(joint_msg); - } - } - } - } - } - } - else if (controller_mode_ == DirectControlMode) - { - if(gazebo_mode_ == false) - { - // BulkRead Rx - for (auto& it : port_to_bulk_read_) - { - robot_->ports_[it.first]->setPacketTimeout(0.0); - it.second->rxPacket(); - } - - // -> save to robot->dxls_[]->dxl_state_ - if (robot_->dxls_.size() > 0) - { - for (auto& dxl_it : robot_->dxls_) - { - Dynamixel *dxl = dxl_it.second; - std::string port_name = dxl_it.second->port_name_; - std::string joint_name = dxl_it.first; - - if (dxl->bulk_read_items_.size() > 0) - { - uint32_t data = 0; - for (int i = 0; i < dxl->bulk_read_items_.size(); i++) - { - ControlTableItem *item = dxl->bulk_read_items_[i]; - if (port_to_bulk_read_[port_name]->isAvailable(dxl->id_, item->address_, item->data_length_) == true) - { - data = port_to_bulk_read_[port_name]->getData(dxl->id_, item->address_, item->data_length_); - - // change dxl_state - if (dxl->present_position_item_ != 0 && item->item_name_ == dxl->present_position_item_->item_name_) - { - dxl->dxl_state_->present_position_ = dxl->convertValue2Radian(data) - dxl->dxl_state_->position_offset_ * offset_ratio_; - } - else if (dxl->present_velocity_item_ != 0 && item->item_name_ == dxl->present_velocity_item_->item_name_) - dxl->dxl_state_->present_velocity_ = dxl->convertValue2Velocity(data); - else if (dxl->present_current_item_ != 0 && item->item_name_ == dxl->present_current_item_->item_name_) - dxl->dxl_state_->present_torque_ = dxl->convertValue2Torque(data); - else if (dxl->goal_position_item_ != 0 && item->item_name_ == dxl->goal_position_item_->item_name_) - { - dxl->dxl_state_->goal_position_ = dxl->convertValue2Radian(data) - dxl->dxl_state_->position_offset_ * offset_ratio_; - } - else if (dxl->goal_velocity_item_ != 0 && item->item_name_ == dxl->goal_velocity_item_->item_name_) - dxl->dxl_state_->goal_velocity_ = dxl->convertValue2Velocity(data); - else if (dxl->goal_current_item_ != 0 && item->item_name_ == dxl->goal_current_item_->item_name_) - dxl->dxl_state_->goal_torque_ = dxl->convertValue2Torque(data); - - dxl->dxl_state_->bulk_read_table_[item->item_name_] = data; - } - } - - // -> update time stamp to Robot->dxls[]->dynamixel_state.update_time_stamp - dxl->dxl_state_->update_time_stamp_ = TimeStamp(present_state.header.stamp.sec, present_state.header.stamp.nsec); - } - } - } - - queue_mutex_.lock(); - -// for (auto& it : port_to_sync_write_position_) -// { -// it.second->txPacket(); -// it.second->clearParam(); -// } - - if (direct_sync_write_.size() > 0) - { - for (int i = 0; i < direct_sync_write_.size(); i++) - { - direct_sync_write_[i]->txPacket(); - direct_sync_write_[i]->clearParam(); - } - direct_sync_write_.clear(); - } - - queue_mutex_.unlock(); - - // BulkRead Tx - for (auto& it : port_to_bulk_read_) - it.second->txPacket(); - } - } - - // Call SensorModule Process() - // -> for loop : call SensorModule list -> Process() - if (sensor_modules_.size() > 0) - { - for (auto module_it = sensor_modules_.begin(); module_it != sensor_modules_.end(); module_it++) - { - (*module_it)->process(robot_->dxls_, robot_->sensors_); - - for (auto& it : (*module_it)->result_) - sensor_result_[it.first] = it.second; - } - } - - if (DEBUG_PRINT) - { - time_duration = ros::Time::now() - start_time; - fprintf(stderr, "(%2.6f) SensorModule Process() & save result \n", time_duration.nsec * 0.000001); - } - - if (controller_mode_ == MotionModuleMode) - { - // Call MotionModule Process() - // -> for loop : call MotionModule list -> Process() - if (motion_modules_.size() > 0) - { - queue_mutex_.lock(); - - for (auto module_it = motion_modules_.begin(); module_it != motion_modules_.end(); module_it++) - { - if ((*module_it)->getModuleEnable() == false) - continue; - - (*module_it)->process(robot_->dxls_, sensor_result_); - - // for loop : joint list - for (auto& dxl_it : robot_->dxls_) - { - std::string joint_name = dxl_it.first; - Dynamixel *dxl = dxl_it.second; - DynamixelState *dxl_state = dxl_it.second->dxl_state_; - - if (dxl->ctrl_module_name_ == (*module_it)->getModuleName()) - { - //do_sync_write = true; - DynamixelState *result_state = (*module_it)->result_[joint_name]; - - if (result_state == NULL) - { - ROS_ERROR("[%s] %s ", (*module_it)->getModuleName().c_str(), joint_name.c_str()); - continue; - } - - // TODO: check update time stamp ? - - if ((*module_it)->getControlMode() == PositionControl) - { - dxl_state->goal_position_ = result_state->goal_position_; - - if (gazebo_mode_ == false) - { - // add offset - uint32_t pos_data; - pos_data= dxl->convertRadian2Value(dxl_state->goal_position_ + dxl_state->position_offset_ * offset_ratio_); - - uint8_t sync_write_data[4] = { 0 }; - sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(pos_data)); - sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(pos_data)); - sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(pos_data)); - sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(pos_data)); - - if (port_to_sync_write_position_[dxl->port_name_] != NULL) - port_to_sync_write_position_[dxl->port_name_]->changeParam(dxl->id_, sync_write_data); - - // if position p gain value is changed -> sync write - if (result_state->position_p_gain_ != NONE_GAIN && dxl_state->position_p_gain_ != result_state->position_p_gain_) - { - dxl_state->position_p_gain_ = result_state->position_p_gain_; - uint8_t sync_write_data[4] = { 0 }; - sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(dxl_state->position_p_gain_)); - sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(dxl_state->position_p_gain_)); - sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(dxl_state->position_p_gain_)); - sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(dxl_state->position_p_gain_)); - - if (port_to_sync_write_position_p_gain_[dxl->port_name_] != NULL) - port_to_sync_write_position_p_gain_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); - } - - // if position i gain value is changed -> sync write - if (result_state->position_i_gain_ != NONE_GAIN && dxl_state->position_i_gain_ != result_state->position_i_gain_) - { - dxl_state->position_i_gain_ = result_state->position_i_gain_; - uint8_t sync_write_data[4] = { 0 }; - sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(dxl_state->position_i_gain_)); - sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(dxl_state->position_i_gain_)); - sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(dxl_state->position_i_gain_)); - sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(dxl_state->position_i_gain_)); - - if (port_to_sync_write_position_i_gain_[dxl->port_name_] != NULL) - port_to_sync_write_position_i_gain_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); - } - - // if position d gain value is changed -> sync write - if (result_state->position_d_gain_ != NONE_GAIN && dxl_state->position_d_gain_ != result_state->position_d_gain_) - { - dxl_state->position_d_gain_ = result_state->position_d_gain_; - uint8_t sync_write_data[4] = { 0 }; - sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(dxl_state->position_d_gain_)); - sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(dxl_state->position_d_gain_)); - sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(dxl_state->position_d_gain_)); - sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(dxl_state->position_d_gain_)); - - if (port_to_sync_write_position_d_gain_[dxl->port_name_] != NULL) - port_to_sync_write_position_d_gain_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); - } - - // if velocity p gain gain value is changed -> sync write - if (result_state->velocity_p_gain_ != NONE_GAIN && dxl_state->velocity_p_gain_ != result_state->velocity_p_gain_) - { - dxl_state->velocity_p_gain_ = result_state->velocity_p_gain_; - uint8_t sync_write_data[4] = { 0 }; - sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(dxl_state->velocity_p_gain_)); - sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(dxl_state->velocity_p_gain_)); - sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(dxl_state->velocity_p_gain_)); - sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(dxl_state->velocity_p_gain_)); - - if (port_to_sync_write_velocity_p_gain_[dxl->port_name_] != NULL) - port_to_sync_write_velocity_p_gain_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); - } - - // if velocity i gain value is changed -> sync write - if (result_state->velocity_i_gain_ != NONE_GAIN && dxl_state->velocity_i_gain_ != result_state->velocity_i_gain_) - { - dxl_state->velocity_i_gain_ = result_state->velocity_i_gain_; - uint8_t sync_write_data[4] = { 0 }; - sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(dxl_state->velocity_i_gain_)); - sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(dxl_state->velocity_i_gain_)); - sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(dxl_state->velocity_i_gain_)); - sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(dxl_state->velocity_i_gain_)); - - if (port_to_sync_write_velocity_i_gain_[dxl->port_name_] != NULL) - port_to_sync_write_velocity_i_gain_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); - } - - } - } - else if ((*module_it)->getControlMode() == VelocityControl) - { - dxl_state->goal_velocity_ = result_state->goal_velocity_; - - if (gazebo_mode_ == false) - { - uint32_t vel_data = dxl->convertVelocity2Value(dxl_state->goal_velocity_); - uint8_t sync_write_data[4] = { 0 }; - sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(vel_data)); - sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(vel_data)); - sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(vel_data)); - sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(vel_data)); - - if (port_to_sync_write_velocity_[dxl->port_name_] != NULL) - port_to_sync_write_velocity_[dxl->port_name_]->changeParam(dxl->id_, sync_write_data); - - // if velocity p gain gain value is changed -> sync write - if (result_state->velocity_p_gain_ != NONE_GAIN && dxl_state->velocity_p_gain_ != result_state->velocity_p_gain_) - { - dxl_state->velocity_p_gain_ = result_state->velocity_p_gain_; - uint8_t sync_write_data[4] = { 0 }; - sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(dxl_state->velocity_p_gain_)); - sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(dxl_state->velocity_p_gain_)); - sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(dxl_state->velocity_p_gain_)); - sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(dxl_state->velocity_p_gain_)); - - if (port_to_sync_write_velocity_p_gain_[dxl->port_name_] != NULL) - port_to_sync_write_velocity_p_gain_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); - } - - // if velocity i gain value is changed -> sync write - if (result_state->velocity_i_gain_ != NONE_GAIN && dxl_state->velocity_i_gain_ != result_state->velocity_i_gain_) - { - dxl_state->velocity_i_gain_ = result_state->velocity_i_gain_; - uint8_t sync_write_data[4] = { 0 }; - sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(dxl_state->velocity_i_gain_)); - sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(dxl_state->velocity_i_gain_)); - sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(dxl_state->velocity_i_gain_)); - sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(dxl_state->velocity_i_gain_)); - - if (port_to_sync_write_velocity_i_gain_[dxl->port_name_] != NULL) - port_to_sync_write_velocity_i_gain_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); - } - - // if velocity d gain value is changed -> sync write - if (result_state->velocity_d_gain_ != NONE_GAIN && dxl_state->velocity_d_gain_ != result_state->velocity_d_gain_) - { - dxl_state->velocity_d_gain_ = result_state->velocity_d_gain_; - uint8_t sync_write_data[4] = { 0 }; - sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(dxl_state->velocity_d_gain_)); - sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(dxl_state->velocity_d_gain_)); - sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(dxl_state->velocity_d_gain_)); - sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(dxl_state->velocity_d_gain_)); - - if (port_to_sync_write_velocity_d_gain_[dxl->port_name_] != NULL) - port_to_sync_write_velocity_d_gain_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); - } - } - } - else if ((*module_it)->getControlMode() == TorqueControl) - { - dxl_state->goal_torque_ = result_state->goal_torque_; - - if (gazebo_mode_ == false) - { - uint32_t curr_data = dxl->convertTorque2Value(dxl_state->goal_torque_); - uint8_t sync_write_data[2] = { 0 }; - sync_write_data[0] = DXL_LOBYTE(curr_data); - sync_write_data[1] = DXL_HIBYTE(curr_data); - - if (port_to_sync_write_current_[dxl->port_name_] != NULL) - port_to_sync_write_current_[dxl->port_name_]->changeParam(dxl->id_, sync_write_data); - } - } - } - } - } - - queue_mutex_.unlock(); - } - - if (DEBUG_PRINT) - { - time_duration = ros::Time::now() - start_time; - fprintf(stderr, "(%2.6f) MotionModule Process() & save result \n", time_duration.nsec * 0.000001); - } - } - - // publish present & goal position - for (auto& dxl_it : robot_->dxls_) - { - std::string joint_name = dxl_it.first; - Dynamixel *dxl = dxl_it.second; - - present_state.name.push_back(joint_name); - present_state.position.push_back(dxl->dxl_state_->present_position_); - present_state.velocity.push_back(dxl->dxl_state_->present_velocity_); - present_state.effort.push_back(dxl->dxl_state_->present_torque_); - - goal_state.name.push_back(joint_name); - goal_state.position.push_back(dxl->dxl_state_->goal_position_); - goal_state.velocity.push_back(dxl->dxl_state_->goal_velocity_); - goal_state.effort.push_back(dxl->dxl_state_->goal_torque_); - } - - // -> publish present joint_states & goal joint states topic - present_joint_state_pub_.publish(present_state); - goal_joint_state_pub_.publish(goal_state); - - if (DEBUG_PRINT) - { - time_duration = ros::Time::now() - start_time; - fprintf(stderr, "(%2.6f) Process() DONE \n", time_duration.nsec * 0.000001); - } - - is_process_running = false; -} - -void RobotisController::addMotionModule(MotionModule *module) -{ - // check whether the module name already exists - for (auto m_it = motion_modules_.begin(); m_it != motion_modules_.end(); m_it++) - { - if ((*m_it)->getModuleName() == module->getModuleName()) - { - ROS_ERROR("Motion Module Name [%s] already exist !!", module->getModuleName().c_str()); - return; - } - } - - module->initialize(robot_->getControlCycle(), robot_); - motion_modules_.push_back(module); - motion_modules_.unique(); -} - -void RobotisController::removeMotionModule(MotionModule *module) -{ - motion_modules_.remove(module); -} - -void RobotisController::addSensorModule(SensorModule *module) -{ - // check whether the module name already exists - for (auto m_it = sensor_modules_.begin(); m_it != sensor_modules_.end(); m_it++) - { - if ((*m_it)->getModuleName() == module->getModuleName()) - { - ROS_ERROR("Sensor Module Name [%s] already exist !!", module->getModuleName().c_str()); - return; - } - } - - module->initialize(robot_->getControlCycle(), robot_); - sensor_modules_.push_back(module); - sensor_modules_.unique(); -} - -void RobotisController::removeSensorModule(SensorModule *module) -{ - sensor_modules_.remove(module); -} - -void RobotisController::writeControlTableCallback(const humanoid_robot_controller_msgs::WriteControlTable::ConstPtr &msg) -{ - Device *device = NULL; - - if (DEBUG_PRINT) - fprintf(stderr, "[WriteControlTable] led control msg received\n"); - - auto dev_it1 = robot_->dxls_.find(msg->joint_name); - if(dev_it1 != robot_->dxls_.end()) - { - device = dev_it1->second; - } - else - { - auto dev_it2 = robot_->sensors_.find(msg->joint_name); - if(dev_it2 != robot_->sensors_.end()) - { - device = dev_it2->second; - } - else - { - ROS_WARN("[WriteControlTable] Unknown device : %s", msg->joint_name.c_str()); - return; - } - } - - ControlTableItem *item = NULL; - auto item_it = device->ctrl_table_.find(msg->start_item_name); - if(item_it != device->ctrl_table_.end()) - { - item = item_it->second; - } - else - { - ROS_WARN("[WriteControlTable] Unknown item : %s", msg->start_item_name.c_str()); - return; - } - - dynamixel::PortHandler *port = robot_->ports_[device->port_name_]; - dynamixel::PacketHandler *packet_handler = dynamixel::PacketHandler::getPacketHandler(device->protocol_version_); - - if (item->access_type_ == Read) - return; - - queue_mutex_.lock(); - - direct_sync_write_.push_back(new dynamixel::GroupSyncWrite(port, packet_handler, item->address_, msg->data_length)); - direct_sync_write_[direct_sync_write_.size() - 1]->addParam(device->id_, (uint8_t *)(msg->data.data())); - -// fprintf(stderr, "[WriteControlTable] %s -> %s : ", msg->joint_name.c_str(), msg->start_item_name.c_str()); -// for (auto &dt : msg->data) -// fprintf(stderr, "%02X ", dt); -// fprintf(stderr, "\n"); - - queue_mutex_.unlock(); - -} - -void RobotisController::syncWriteItemCallback(const humanoid_robot_controller_msgs::SyncWriteItem::ConstPtr &msg) -{ - for (int i = 0; i < msg->joint_name.size(); i++) - { - Device *device; - - auto d_it1 = robot_->dxls_.find(msg->joint_name[i]); - if (d_it1 != robot_->dxls_.end()) - { - device = d_it1->second; - } - else - { - auto d_it2 = robot_->sensors_.find(msg->joint_name[i]); - if (d_it2 != robot_->sensors_.end()) - { - device = d_it2->second; - } - else - { - ROS_WARN("[SyncWriteItem] Unknown device : %s", msg->joint_name[i].c_str()); - continue; - } - } - -// ControlTableItem *item = device->ctrl_table_[msg->item_name]; - ControlTableItem *item = NULL; - auto item_it = device->ctrl_table_.find(msg->item_name); - if(item_it != device->ctrl_table_.end()) - { - item = item_it->second; - } - else - { - ROS_WARN("SyncWriteItem] Unknown item : %s", msg->item_name.c_str()); - continue; - } - - dynamixel::PortHandler *port = robot_->ports_[device->port_name_]; - dynamixel::PacketHandler *packet_handler = dynamixel::PacketHandler::getPacketHandler(device->protocol_version_); - - if (item->access_type_ == Read) - continue; - - queue_mutex_.lock(); - - int idx = 0; - if (direct_sync_write_.size() == 0) - { - direct_sync_write_.push_back(new dynamixel::GroupSyncWrite(port, packet_handler, item->address_, item->data_length_)); - idx = 0; - } - else - { - for (idx = 0; idx < direct_sync_write_.size(); idx++) - { - if (direct_sync_write_[idx]->getPortHandler() == port && direct_sync_write_[idx]->getPacketHandler() == packet_handler) - break; - } - - if (idx == direct_sync_write_.size()) - direct_sync_write_.push_back(new dynamixel::GroupSyncWrite(port, packet_handler, item->address_, item->data_length_)); - } - - uint8_t *data = new uint8_t[item->data_length_]; - if (item->data_length_ == 1) - data[0] = (uint8_t) msg->value[i]; - else if (item->data_length_ == 2) - { - data[0] = DXL_LOBYTE((uint16_t )msg->value[i]); - data[1] = DXL_HIBYTE((uint16_t )msg->value[i]); - } - else if (item->data_length_ == 4) - { - data[0] = DXL_LOBYTE(DXL_LOWORD((uint32_t)msg->value[i])); - data[1] = DXL_HIBYTE(DXL_LOWORD((uint32_t)msg->value[i])); - data[2] = DXL_LOBYTE(DXL_HIWORD((uint32_t)msg->value[i])); - data[3] = DXL_HIBYTE(DXL_HIWORD((uint32_t)msg->value[i])); - } - direct_sync_write_[idx]->addParam(device->id_, data); - delete[] data; - - queue_mutex_.unlock(); - } -} - -void RobotisController::setControllerModeCallback(const std_msgs::String::ConstPtr &msg) -{ - if (msg->data == "DirectControlMode") - { - for (auto& it : port_to_bulk_read_) - { - robot_->ports_[it.first]->setPacketTimeout(0.0); - it.second->rxPacket(); - } - controller_mode_ = DirectControlMode; - } - else if (msg->data == "MotionModuleMode") - { - for (auto& it : port_to_bulk_read_) - { - it.second->txPacket(); - } - controller_mode_ = MotionModuleMode; - } -} - -void RobotisController::setJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg) -{ - queue_mutex_.lock(); - - for (int i = 0; i < msg->name.size(); i++) - { - Dynamixel *dxl = robot_->dxls_[msg->name[i]]; - if (dxl == NULL) - continue; - - if ((controller_mode_ == DirectControlMode) || - (controller_mode_ == MotionModuleMode && dxl->ctrl_module_name_ == "none")) - { - dxl->dxl_state_->goal_position_ = (double) msg->position[i]; - - if (gazebo_mode_ == false) - { - // add offset - uint32_t pos_data; - pos_data = dxl->convertRadian2Value(dxl->dxl_state_->goal_position_ + dxl->dxl_state_->position_offset_ * offset_ratio_); - - uint8_t sync_write_data[4] = { 0 }; - sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(pos_data)); - sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(pos_data)); - sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(pos_data)); - sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(pos_data)); - - if (port_to_sync_write_position_[dxl->port_name_] != NULL) - port_to_sync_write_position_[dxl->port_name_]->changeParam(dxl->id_, sync_write_data); - } - } - } - - queue_mutex_.unlock(); -} - -void RobotisController::setCtrlModuleCallback(const std_msgs::String::ConstPtr &msg) -{ - if(set_module_thread_.joinable()) - set_module_thread_.join(); - - std::string _module_name_to_set = msg->data; - - set_module_thread_ = boost::thread(boost::bind(&RobotisController::setCtrlModuleThread, this, _module_name_to_set)); -} - -void RobotisController::setCtrlModule(std::string module_name) -{ - if(set_module_thread_.joinable()) - set_module_thread_.join(); - - set_module_thread_ = boost::thread(boost::bind(&RobotisController::setCtrlModuleThread, this, module_name)); -} -void RobotisController::setJointCtrlModuleCallback(const humanoid_robot_controller_msgs::JointCtrlModule::ConstPtr &msg) -{ - if (msg->joint_name.size() != msg->module_name.size()) - return; - - if(set_module_thread_.joinable()) - set_module_thread_.join(); - - set_module_thread_ = boost::thread(boost::bind(&RobotisController::setJointCtrlModuleThread, this, msg)); -} - -void RobotisController::enableOffsetCallback(const std_msgs::Bool::ConstPtr &msg) -{ - is_offset_enabled_ = (bool)msg->data; - if(is_offset_enabled_) - offset_ratio_ = 0.0; - else - offset_ratio_ = 1.0; -} - -bool RobotisController::getJointCtrlModuleService(humanoid_robot_controller_msgs::GetJointModule::Request &req, - humanoid_robot_controller_msgs::GetJointModule::Response &res) -{ - for (unsigned int idx = 0; idx < req.joint_name.size(); idx++) - { - auto d_it = robot_->dxls_.find((std::string) (req.joint_name[idx])); - if (d_it != robot_->dxls_.end()) - { - res.joint_name.push_back(req.joint_name[idx]); - res.module_name.push_back(d_it->second->ctrl_module_name_); - } - } - - if (res.joint_name.size() == 0) - return false; - - return true; -} - -bool RobotisController::setJointCtrlModuleService(humanoid_robot_controller_msgs::SetJointModule::Request &req, humanoid_robot_controller_msgs::SetJointModule::Response &res) -{ - if(set_module_thread_.joinable()) - set_module_thread_.join(); - - humanoid_robot_controller_msgs::JointCtrlModule modules; - modules.joint_name = req.joint_name; - modules.module_name = req.module_name; - - humanoid_robot_controller_msgs::JointCtrlModule::ConstPtr msg_ptr(new humanoid_robot_controller_msgs::JointCtrlModule(modules)); - - if (modules.joint_name.size() != modules.module_name.size()) - return false; - - set_module_thread_ = boost::thread(boost::bind(&RobotisController::setJointCtrlModuleThread, this, msg_ptr)); - - set_module_thread_.join(); - - return true; -} - -bool RobotisController::setCtrlModuleService(humanoid_robot_controller_msgs::SetModule::Request &req, humanoid_robot_controller_msgs::SetModule::Response &res) -{ - if(set_module_thread_.joinable()) - set_module_thread_.join(); - - std::string _module_name_to_set = req.module_name; - - set_module_thread_ = boost::thread(boost::bind(&RobotisController::setCtrlModuleThread, this, _module_name_to_set)); - - set_module_thread_.join(); - - res.result = true; - return true; -} - -bool RobotisController::loadOffsetService(humanoid_robot_controller_msgs::LoadOffset::Request &req, humanoid_robot_controller_msgs::LoadOffset::Response &res) -{ - loadOffset((std::string)req.file_path); - res.result = true; - return true; -} - -void RobotisController::setJointCtrlModuleThread(const humanoid_robot_controller_msgs::JointCtrlModule::ConstPtr &msg) -{ - // stop module list - std::list _stop_modules; - std::list _enable_modules; - - for(unsigned int idx = 0; idx < msg->joint_name.size(); idx++) - { - Dynamixel *_dxl = NULL; - std::map::iterator _dxl_it = robot_->dxls_.find((std::string)(msg->joint_name[idx])); - if(_dxl_it != robot_->dxls_.end()) - _dxl = _dxl_it->second; - else - continue; - - // enqueue - if(_dxl->ctrl_module_name_ != msg->module_name[idx]) - { - for(std::list::iterator _stop_m_it = motion_modules_.begin(); _stop_m_it != motion_modules_.end(); _stop_m_it++) - { - if((*_stop_m_it)->getModuleName() == _dxl->ctrl_module_name_ && (*_stop_m_it)->getModuleEnable() == true) - _stop_modules.push_back(*_stop_m_it); - } - } - } - - // stop the module - _stop_modules.unique(); - for(std::list::iterator _stop_m_it = _stop_modules.begin(); _stop_m_it != _stop_modules.end(); _stop_m_it++) - { - (*_stop_m_it)->stop(); - } - - // wait to stop - for(std::list::iterator _stop_m_it = _stop_modules.begin(); _stop_m_it != _stop_modules.end(); _stop_m_it++) - { - while((*_stop_m_it)->isRunning()) - usleep(robot_->getControlCycle() * 1000); - } - - // disable module(s) - for(std::list::iterator _stop_m_it = _stop_modules.begin(); _stop_m_it != _stop_modules.end(); _stop_m_it++) - { - (*_stop_m_it)->setModuleEnable(false); - } - - // set ctrl module - queue_mutex_.lock(); - - for(unsigned int idx = 0; idx < msg->joint_name.size(); idx++) - { - std::string ctrl_module = msg->module_name[idx]; - std::string joint_name = msg->joint_name[idx]; - - Dynamixel *_dxl = NULL; - std::map::iterator _dxl_it = robot_->dxls_.find(joint_name); - if(_dxl_it != robot_->dxls_.end()) - _dxl = _dxl_it->second; - else - continue; - - // none - if(ctrl_module == "" || ctrl_module == "none") - { - _dxl->ctrl_module_name_ = "none"; - - if(gazebo_mode_ == true) - continue; - - uint32_t _pos_data; - _pos_data = _dxl->convertRadian2Value(_dxl->dxl_state_->goal_position_ + _dxl->dxl_state_->position_offset_ * offset_ratio_); - - uint8_t _sync_write_data[4]; - _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_pos_data)); - _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_pos_data)); - _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_pos_data)); - _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_pos_data)); - - if(port_to_sync_write_position_[_dxl->port_name_] != NULL) - port_to_sync_write_position_[_dxl->port_name_]->addParam(_dxl->id_, _sync_write_data); - - if(port_to_sync_write_current_[_dxl->port_name_] != NULL) - port_to_sync_write_current_[_dxl->port_name_]->removeParam(_dxl->id_); - if(port_to_sync_write_velocity_[_dxl->port_name_] != NULL) - port_to_sync_write_velocity_[_dxl->port_name_]->removeParam(_dxl->id_); - } - else - { - // check whether the module exist - for(std::list::iterator _m_it = motion_modules_.begin(); _m_it != motion_modules_.end(); _m_it++) - { - // if it exist - if((*_m_it)->getModuleName() == ctrl_module) - { - std::map::iterator _result_it = (*_m_it)->result_.find(joint_name); - if(_result_it == (*_m_it)->result_.end()) - break; - - _dxl->ctrl_module_name_ = ctrl_module; - - // enqueue enable module list - _enable_modules.push_back(*_m_it); - ControlMode _mode = (*_m_it)->getControlMode(); - - if(gazebo_mode_ == true) - break; - - if(_mode == PositionControl) - { - uint32_t _pos_data; - _pos_data = _dxl->convertRadian2Value(_dxl->dxl_state_->goal_position_ + _dxl->dxl_state_->position_offset_ * offset_ratio_); - - uint8_t _sync_write_data[4]; - _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_pos_data)); - _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_pos_data)); - _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_pos_data)); - _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_pos_data)); - - if(port_to_sync_write_position_[_dxl->port_name_] != NULL) - port_to_sync_write_position_[_dxl->port_name_]->addParam(_dxl->id_, _sync_write_data); - - if(port_to_sync_write_current_[_dxl->port_name_] != NULL) - port_to_sync_write_current_[_dxl->port_name_]->removeParam(_dxl->id_); - if(port_to_sync_write_velocity_[_dxl->port_name_] != NULL) - port_to_sync_write_velocity_[_dxl->port_name_]->removeParam(_dxl->id_); - } - else if(_mode == VelocityControl) - { - uint32_t _vel_data = _dxl->convertVelocity2Value(_dxl->dxl_state_->goal_velocity_); - uint8_t _sync_write_data[4]; - _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_vel_data)); - _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_vel_data)); - _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_vel_data)); - _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_vel_data)); - - if(port_to_sync_write_velocity_[_dxl->port_name_] != NULL) - port_to_sync_write_velocity_[_dxl->port_name_]->addParam(_dxl->id_, _sync_write_data); - - if(port_to_sync_write_current_[_dxl->port_name_] != NULL) - port_to_sync_write_current_[_dxl->port_name_]->removeParam(_dxl->id_); - if(port_to_sync_write_position_[_dxl->port_name_] != NULL) - port_to_sync_write_position_[_dxl->port_name_]->removeParam(_dxl->id_); - } - else if(_mode == TorqueControl) - { - uint32_t _curr_data = _dxl->convertTorque2Value(_dxl->dxl_state_->goal_torque_); - uint8_t _sync_write_data[4]; - _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_curr_data)); - _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_curr_data)); - _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_curr_data)); - _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_curr_data)); - - if(port_to_sync_write_current_[_dxl->port_name_] != NULL) - port_to_sync_write_current_[_dxl->port_name_]->addParam(_dxl->id_, _sync_write_data); - - if(port_to_sync_write_velocity_[_dxl->port_name_] != NULL) - port_to_sync_write_velocity_[_dxl->port_name_]->removeParam(_dxl->id_); - if(port_to_sync_write_position_[_dxl->port_name_] != NULL) - port_to_sync_write_position_[_dxl->port_name_]->removeParam(_dxl->id_); - } - break; - } - } - } - } - - // enable module(s) - _enable_modules.unique(); - for(std::list::iterator _m_it = _enable_modules.begin(); _m_it != _enable_modules.end(); _m_it++) - { - (*_m_it)->setModuleEnable(true); - } - - // TODO: set indirect address - // -> check module's control_mode - - queue_mutex_.unlock(); - - // publish current module - humanoid_robot_controller_msgs::JointCtrlModule _current_module_msg; - for(std::map::iterator _dxl_iter = robot_->dxls_.begin(); _dxl_iter != robot_->dxls_.end(); ++_dxl_iter) - { - _current_module_msg.joint_name.push_back(_dxl_iter->first); - _current_module_msg.module_name.push_back(_dxl_iter->second->ctrl_module_name_); - } - - if(_current_module_msg.joint_name.size() == _current_module_msg.module_name.size()) - current_module_pub_.publish(_current_module_msg); -} - -void RobotisController::setCtrlModuleThread(std::string ctrl_module) -{ - // stop module - std::list stop_modules; - - if (ctrl_module == "" || ctrl_module == "none") - { - // enqueue all modules in order to stop - for (auto m_it = motion_modules_.begin(); m_it != motion_modules_.end(); m_it++) - { - if ((*m_it)->getModuleEnable() == true) - stop_modules.push_back(*m_it); - } - } - else - { - for (auto m_it = motion_modules_.begin(); m_it != motion_modules_.end(); m_it++) - { - // if it exist - if ((*m_it)->getModuleName() == ctrl_module) - { - // enqueue the module which lost control of joint in order to stop - for (auto& result_it : (*m_it)->result_) - { - auto d_it = robot_->dxls_.find(result_it.first); - - if (d_it != robot_->dxls_.end()) - { - // enqueue - if (d_it->second->ctrl_module_name_ != ctrl_module) - { - for (auto stop_m_it = motion_modules_.begin(); stop_m_it != motion_modules_.end(); stop_m_it++) - { - if (((*stop_m_it)->getModuleName() == d_it->second->ctrl_module_name_) && - ((*stop_m_it)->getModuleEnable() == true)) - { - stop_modules.push_back(*stop_m_it); - } - } - } - } - } - - break; - } - } - } - - // stop the module - stop_modules.unique(); - for (auto stop_m_it = stop_modules.begin(); stop_m_it != stop_modules.end(); stop_m_it++) - { - (*stop_m_it)->stop(); - } - - // wait to stop - for (auto stop_m_it = stop_modules.begin(); stop_m_it != stop_modules.end(); stop_m_it++) - { - while ((*stop_m_it)->isRunning()) - usleep(robot_->getControlCycle() * 1000); - } - - // disable module(s) - for(std::list::iterator _stop_m_it = stop_modules.begin(); _stop_m_it != stop_modules.end(); _stop_m_it++) - { - (*_stop_m_it)->setModuleEnable(false); - } - - - // set ctrl module - queue_mutex_.lock(); - - if (DEBUG_PRINT) - ROS_INFO_STREAM("set module : " << ctrl_module); - - // none - if ((ctrl_module == "") || (ctrl_module == "none")) - { - // set dxl's control module to "none" - for (auto& d_it : robot_->dxls_) - { - Dynamixel *dxl = d_it.second; - dxl->ctrl_module_name_ = "none"; - - if (gazebo_mode_ == true) - continue; - - uint32_t pos_data; - pos_data = dxl->convertRadian2Value(dxl->dxl_state_->goal_position_ + dxl->dxl_state_->position_offset_ * offset_ratio_); - - uint8_t sync_write_data[4] = { 0 }; - sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(pos_data)); - sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(pos_data)); - sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(pos_data)); - sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(pos_data)); - - if (port_to_sync_write_position_[dxl->port_name_] != NULL) - port_to_sync_write_position_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); - - if (port_to_sync_write_current_[dxl->port_name_] != NULL) - port_to_sync_write_current_[dxl->port_name_]->removeParam(dxl->id_); - if (port_to_sync_write_velocity_[dxl->port_name_] != NULL) - port_to_sync_write_velocity_[dxl->port_name_]->removeParam(dxl->id_); - } - } - else - { - // check whether the module exist - for (auto m_it = motion_modules_.begin(); m_it != motion_modules_.end(); m_it++) - { - // if it exist - if ((*m_it)->getModuleName() == ctrl_module) - { - ControlMode mode = (*m_it)->getControlMode(); - for (auto& result_it : (*m_it)->result_) - { - auto d_it = robot_->dxls_.find(result_it.first); - if (d_it != robot_->dxls_.end()) - { - Dynamixel *dxl = d_it->second; - dxl->ctrl_module_name_ = ctrl_module; - - if (gazebo_mode_ == true) - continue; - - if (mode == PositionControl) - { - uint32_t pos_data; - pos_data = dxl->convertRadian2Value(dxl->dxl_state_->goal_position_ + dxl->dxl_state_->position_offset_ * offset_ratio_); - - uint8_t sync_write_data[4] = { 0 }; - sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(pos_data)); - sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(pos_data)); - sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(pos_data)); - sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(pos_data)); - - if (port_to_sync_write_position_[dxl->port_name_] != NULL) - port_to_sync_write_position_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); - - if (port_to_sync_write_current_[dxl->port_name_] != NULL) - port_to_sync_write_current_[dxl->port_name_]->removeParam(dxl->id_); - if (port_to_sync_write_velocity_[dxl->port_name_] != NULL) - port_to_sync_write_velocity_[dxl->port_name_]->removeParam(dxl->id_); - } - else if (mode == VelocityControl) - { - uint32_t vel_data = dxl->convertVelocity2Value(dxl->dxl_state_->goal_velocity_); - uint8_t sync_write_data[4] = { 0 }; - sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(vel_data)); - sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(vel_data)); - sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(vel_data)); - sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(vel_data)); - - if (port_to_sync_write_velocity_[dxl->port_name_] != NULL) - port_to_sync_write_velocity_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); - - if (port_to_sync_write_current_[dxl->port_name_] != NULL) - port_to_sync_write_current_[dxl->port_name_]->removeParam(dxl->id_); - if (port_to_sync_write_position_[dxl->port_name_] != NULL) - port_to_sync_write_position_[dxl->port_name_]->removeParam(dxl->id_); - } - else if (mode == TorqueControl) - { - uint32_t curr_data = dxl->convertTorque2Value(dxl->dxl_state_->goal_torque_); - uint8_t sync_write_data[4] = { 0 }; - sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(curr_data)); - sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(curr_data)); - sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(curr_data)); - sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(curr_data)); - - if (port_to_sync_write_current_[dxl->port_name_] != NULL) - port_to_sync_write_current_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); - - if (port_to_sync_write_velocity_[dxl->port_name_] != NULL) - port_to_sync_write_velocity_[dxl->port_name_]->removeParam(dxl->id_); - if (port_to_sync_write_position_[dxl->port_name_] != NULL) - port_to_sync_write_position_[dxl->port_name_]->removeParam(dxl->id_); - } - } - } - - break; - } - } - } - - for (auto m_it = motion_modules_.begin(); m_it != motion_modules_.end(); m_it++) - { - // set all used modules -> enable - for (auto& d_it : robot_->dxls_) - { - if (d_it.second->ctrl_module_name_ == (*m_it)->getModuleName()) - { - (*m_it)->setModuleEnable(true); - break; - } - } - } - - // TODO: set indirect address - // -> check module's control_mode - - queue_mutex_.unlock(); - - // publish current module - humanoid_robot_controller_msgs::JointCtrlModule current_module_msg; - for (auto& dxl_iter : robot_->dxls_) - { - current_module_msg.joint_name.push_back(dxl_iter.first); - current_module_msg.module_name.push_back(dxl_iter.second->ctrl_module_name_); - } - - if (current_module_msg.joint_name.size() == current_module_msg.module_name.size()) - current_module_pub_.publish(current_module_msg); -} - -void RobotisController::gazeboJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg) -{ - queue_mutex_.lock(); - - for (unsigned int i = 0; i < msg->name.size(); i++) - { - auto d_it = robot_->dxls_.find((std::string) msg->name[i]); - if (d_it != robot_->dxls_.end()) - { - d_it->second->dxl_state_->present_position_ = msg->position[i]; - d_it->second->dxl_state_->present_velocity_ = msg->velocity[i]; - d_it->second->dxl_state_->present_torque_ = msg->effort[i]; - } - } - - if (init_pose_loaded_ == false) - { - for (auto& it : robot_->dxls_) - it.second->dxl_state_->goal_position_ = it.second->dxl_state_->present_position_; - init_pose_loaded_ = true; - } - - queue_mutex_.unlock(); -} - -bool RobotisController::isTimerStopped() -{ - if (this->is_timer_running_) - { - if (DEBUG_PRINT == true) - ROS_WARN("Process Timer is running.. STOP the timer first."); - return false; - } - return true; -} - -int RobotisController::ping(const std::string joint_name, uint8_t *error) -{ - return ping(joint_name, 0, error); -} -int RobotisController::ping(const std::string joint_name, uint16_t* model_number, uint8_t *error) -{ - if (isTimerStopped() == false) - return COMM_PORT_BUSY; - - Dynamixel *dxl = robot_->dxls_[joint_name]; - if (dxl == NULL) - return COMM_NOT_AVAILABLE; - - dynamixel::PacketHandler *pkt_handler = dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); - dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; - - return pkt_handler->ping(port_handler, dxl->id_, model_number, error); -} - -int RobotisController::action(const std::string joint_name) -{ - if (isTimerStopped() == false) - return COMM_PORT_BUSY; - - Dynamixel *dxl = robot_->dxls_[joint_name]; - if (dxl == NULL) - return COMM_NOT_AVAILABLE; - - dynamixel::PacketHandler *pkt_handler = dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); - dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; - - return pkt_handler->action(port_handler, dxl->id_); -} -int RobotisController::reboot(const std::string joint_name, uint8_t *error) -{ - if (isTimerStopped() == false) - return COMM_PORT_BUSY; - - Dynamixel *dxl = robot_->dxls_[joint_name]; - if (dxl == NULL) - return COMM_NOT_AVAILABLE; - - dynamixel::PacketHandler *pkt_handler = dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); - dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; - - return pkt_handler->reboot(port_handler, dxl->id_, error); -} -int RobotisController::factoryReset(const std::string joint_name, uint8_t option, uint8_t *error) -{ - if (isTimerStopped() == false) - return COMM_PORT_BUSY; - - Dynamixel *dxl = robot_->dxls_[joint_name]; - if (dxl == NULL) - return COMM_NOT_AVAILABLE; - - dynamixel::PacketHandler *pkt_handler = dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); - dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; - - return pkt_handler->factoryReset(port_handler, dxl->id_, option, error); -} - -int RobotisController::read(const std::string joint_name, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error) -{ - if (isTimerStopped() == false) - return COMM_PORT_BUSY; - - Dynamixel *dxl = robot_->dxls_[joint_name]; - if (dxl == NULL) - return COMM_NOT_AVAILABLE; - - dynamixel::PacketHandler *pkt_handler = dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); - dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; - - return pkt_handler->readTxRx(port_handler, dxl->id_, address, length, data, error); -} - -int RobotisController::readCtrlItem(const std::string joint_name, const std::string item_name, uint32_t *data, uint8_t *error) -{ - if (isTimerStopped() == false) - return COMM_PORT_BUSY; - - Dynamixel *dxl = robot_->dxls_[joint_name]; - if (dxl == NULL) - return COMM_NOT_AVAILABLE; - - ControlTableItem *item = dxl->ctrl_table_[item_name]; - if (item == NULL) - return COMM_NOT_AVAILABLE; - - dynamixel::PacketHandler *pkt_handler = dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); - dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; - - int result = COMM_NOT_AVAILABLE; - switch (item->data_length_) - { - case 1: - { - uint8_t read_data = 0; - result = pkt_handler->read1ByteTxRx(port_handler, dxl->id_, item->address_, &read_data, error); - if (result == COMM_SUCCESS) - *data = read_data; - break; - } - case 2: - { - uint16_t read_data = 0; - result = pkt_handler->read2ByteTxRx(port_handler, dxl->id_, item->address_, &read_data, error); - if (result == COMM_SUCCESS) - *data = read_data; - break; - } - case 4: - { - uint32_t read_data = 0; - result = pkt_handler->read4ByteTxRx(port_handler, dxl->id_, item->address_, &read_data, error); - if (result == COMM_SUCCESS) - *data = read_data; - break; - } - default: - break; - } - return result; -} - -int RobotisController::read1Byte(const std::string joint_name, uint16_t address, uint8_t *data, uint8_t *error) -{ - if (isTimerStopped() == false) - return COMM_PORT_BUSY; - - Dynamixel *dxl = robot_->dxls_[joint_name]; - if (dxl == NULL) - return COMM_NOT_AVAILABLE; - - dynamixel::PacketHandler *pkt_handler = dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); - dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; - - return pkt_handler->read1ByteTxRx(port_handler, dxl->id_, address, data, error); -} - -int RobotisController::read2Byte(const std::string joint_name, uint16_t address, uint16_t *data, uint8_t *error) -{ - if (isTimerStopped() == false) - return COMM_PORT_BUSY; - - Dynamixel *dxl = robot_->dxls_[joint_name]; - if (dxl == NULL) - return COMM_NOT_AVAILABLE; - - dynamixel::PacketHandler *pkt_handler = dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); - dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; - - return pkt_handler->read2ByteTxRx(port_handler, dxl->id_, address, data, error); -} - -int RobotisController::read4Byte(const std::string joint_name, uint16_t address, uint32_t *data, uint8_t *error) -{ - if (isTimerStopped() == false) - return COMM_PORT_BUSY; - - Dynamixel *dxl = robot_->dxls_[joint_name]; - if (dxl == NULL) - return COMM_NOT_AVAILABLE; - - dynamixel::PacketHandler *pkt_handler = dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); - dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; - - return pkt_handler->read4ByteTxRx(port_handler, dxl->id_, address, data, error); -} - -int RobotisController::write(const std::string joint_name, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error) -{ - if (isTimerStopped() == false) - return COMM_PORT_BUSY; - - Dynamixel *dxl = robot_->dxls_[joint_name]; - if (dxl == NULL) - return COMM_NOT_AVAILABLE; - - dynamixel::PacketHandler *pkt_handler = dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); - dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; - - return pkt_handler->writeTxRx(port_handler, dxl->id_, address, length, data, error); -} - -int RobotisController::writeCtrlItem(const std::string joint_name, const std::string item_name, uint32_t data, uint8_t *error) -{ - if (isTimerStopped() == false) - return COMM_PORT_BUSY; - - Dynamixel *dxl = robot_->dxls_[joint_name]; - if (dxl == NULL) - return COMM_NOT_AVAILABLE; - - ControlTableItem *item = dxl->ctrl_table_[item_name]; - if (item == NULL) - return COMM_NOT_AVAILABLE; - - dynamixel::PacketHandler *pkt_handler = dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); - dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; - - int result = COMM_NOT_AVAILABLE; - uint8_t *write_data = new uint8_t[item->data_length_]; - if (item->data_length_ == 1) - { - write_data[0] = (uint8_t) data; - result = pkt_handler->write1ByteTxRx(port_handler, dxl->id_, item->address_, data, error); - } - else if (item->data_length_ == 2) - { - write_data[0] = DXL_LOBYTE((uint16_t )data); - write_data[1] = DXL_HIBYTE((uint16_t )data); - result = pkt_handler->write2ByteTxRx(port_handler, dxl->id_, item->address_, data, error); - } - else if (item->data_length_ == 4) - { - write_data[0] = DXL_LOBYTE(DXL_LOWORD((uint32_t)data)); - write_data[1] = DXL_HIBYTE(DXL_LOWORD((uint32_t)data)); - write_data[2] = DXL_LOBYTE(DXL_HIWORD((uint32_t)data)); - write_data[3] = DXL_HIBYTE(DXL_HIWORD((uint32_t)data)); - result = pkt_handler->write4ByteTxRx(port_handler, dxl->id_, item->address_, data, error); - } - delete[] write_data; - return result; -} - -int RobotisController::write1Byte(const std::string joint_name, uint16_t address, uint8_t data, uint8_t *error) -{ - if (isTimerStopped() == false) - return COMM_PORT_BUSY; - - Dynamixel *dxl = robot_->dxls_[joint_name]; - if (dxl == NULL) - return COMM_NOT_AVAILABLE; - - dynamixel::PacketHandler *pkt_handler = dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); - dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; - - return pkt_handler->write1ByteTxRx(port_handler, dxl->id_, address, data, error); -} - -int RobotisController::write2Byte(const std::string joint_name, uint16_t address, uint16_t data, uint8_t *error) -{ - if (isTimerStopped() == false) - return COMM_PORT_BUSY; - - Dynamixel *dxl = robot_->dxls_[joint_name]; - if (dxl == NULL) - return COMM_NOT_AVAILABLE; - - dynamixel::PacketHandler *pkt_handler = dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); - dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; - - return pkt_handler->write2ByteTxRx(port_handler, dxl->id_, address, data, error); -} - -int RobotisController::write4Byte(const std::string joint_name, uint16_t address, uint32_t data, uint8_t *error) -{ - if (isTimerStopped() == false) - return COMM_PORT_BUSY; - - Dynamixel *dxl = robot_->dxls_[joint_name]; - if (dxl == NULL) - return COMM_NOT_AVAILABLE; - - dynamixel::PacketHandler *pkt_handler = dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); - dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; - - return pkt_handler->write4ByteTxRx(port_handler, dxl->id_, address, data, error); -} - -int RobotisController::regWrite(const std::string joint_name, uint16_t address, uint16_t length, uint8_t *data, - uint8_t *error) -{ - if (isTimerStopped() == false) - return COMM_PORT_BUSY; - - Dynamixel *dxl = robot_->dxls_[joint_name]; - if (dxl == NULL) - return COMM_NOT_AVAILABLE; - - dynamixel::PacketHandler *pkt_handler = dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); - dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; - - return pkt_handler->regWriteTxRx(port_handler, dxl->id_, address, length, data, error); -} - diff --git a/humanoid_robot_framework/package.xml b/humanoid_robot_framework/package.xml deleted file mode 100755 index 1ff3e96..0000000 --- a/humanoid_robot_framework/package.xml +++ /dev/null @@ -1,40 +0,0 @@ - - - humanoid_robot_framework - 0.3.2 - ROS packages for the humanoid_robot_framework (meta package) - Apache 2.0 - Ronaldson Bellande - - - catkin - - humanoid_robot_controller - humanoid_robot_device - humanoid_robot_framework_common - - humanoid_robot_controller - humanoid_robot_device - humanoid_robot_framework_common - - humanoid_robot_controller - humanoid_robot_device - humanoid_robot_framework_common - - - ament_cmake - - humanoid_robot_controller - humanoid_robot_device - humanoid_robot_framework_common - - humanoid_robot_controller - humanoid_robot_device - humanoid_robot_framework_common - - humanoid_robot_controller - humanoid_robot_device - humanoid_robot_framework_common - - - diff --git a/humanoid_robot_controller/CHANGELOG.rst b/humanoid_robot_intelligence_control_system_controller/CHANGELOG.rst similarity index 94% rename from humanoid_robot_controller/CHANGELOG.rst rename to humanoid_robot_intelligence_control_system_controller/CHANGELOG.rst index f886d0a..0b85c9e 100755 --- a/humanoid_robot_controller/CHANGELOG.rst +++ b/humanoid_robot_intelligence_control_system_controller/CHANGELOG.rst @@ -1,5 +1,5 @@ ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package humanoid_robot_controller +Changelog for package humanoid_robot_intelligence_control_system_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 0.3.2 (2023-10-03) @@ -70,7 +70,7 @@ Changelog for package humanoid_robot_controller 0.2.2 (2017-04-24) ------------------ -* updated humanoid_robot_controller.cpp +* updated humanoid_robot_intelligence_control_system_controller.cpp * changed to read control cycle from .robot file * Contributors: Zerom @@ -83,7 +83,7 @@ Changelog for package humanoid_robot_controller * - added WriteControlTable msg callback * mode change debugging * - optimized cpu usage by spin loop (by astumpf) -* - humanoid_robot_controller process() : processing order changed. +* - humanoid_robot_intelligence_control_system_controller process() : processing order changed. * 1st : packet communication * 2nd : processing modules * - dependencies fixed. (Pull requests `#26 `_) diff --git a/humanoid_robot_controller/CMakeLists.txt b/humanoid_robot_intelligence_control_system_controller/CMakeLists.txt similarity index 56% rename from humanoid_robot_controller/CMakeLists.txt rename to humanoid_robot_intelligence_control_system_controller/CMakeLists.txt index 70bcf63..c69c671 100755 --- a/humanoid_robot_controller/CMakeLists.txt +++ b/humanoid_robot_intelligence_control_system_controller/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.8) -project(humanoid_robot_controller) +project(humanoid_robot_intelligence_control_system_controller) ## Compile as C++11, supported in ROS Kinetic and newer add_compile_options(-std=c++11) @@ -11,10 +11,10 @@ if($ENV{ROS_VERSION} EQUAL 1) roslib std_msgs sensor_msgs - humanoid_robot_controller_msgs + humanoid_robot_intelligence_control_system_controller_msgs dynamixel_sdk - humanoid_robot_device - humanoid_robot_framework_common + humanoid_robot_intelligence_control_system_device + humanoid_robot_intelligence_control_system_framework_common cmake_modules ) @@ -45,16 +45,16 @@ endif(NOT ${YAML_CPP_VERSION} VERSION_LESS "0.5") if($ENV{ROS_VERSION} EQUAL 1) catkin_package( INCLUDE_DIRS include - LIBRARIES humanoid_robot_controller + LIBRARIES humanoid_robot_intelligence_control_system_controller CATKIN_DEPENDS roscpp roslib std_msgs sensor_msgs - humanoid_robot_controller_msgs + humanoid_robot_intelligence_control_system_controller_msgs dynamixel_sdk - humanoid_robot_device - humanoid_robot_framework_common + humanoid_robot_intelligence_control_system_device + humanoid_robot_intelligence_control_system_framework_common cmake_modules DEPENDS Boost ) @@ -68,11 +68,11 @@ include_directories( ${YAML_CPP_INCLUDE_DIRS} ) -add_library(humanoid_robot_controller src/humanoid_robot_controller/humanoid_robot_controller.cpp) -add_dependencies(humanoid_robot_controller ${catkin_EXPORTED_TARGETS}) -target_link_libraries(humanoid_robot_controller ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${YAML_CPP_LIBRARIES}) +add_library(humanoid_robot_intelligence_control_system_controller src/humanoid_robot_intelligence_control_system_controller/humanoid_robot_intelligence_control_system_controller.cpp) +add_dependencies(humanoid_robot_intelligence_control_system_controller ${catkin_EXPORTED_TARGETS}) +target_link_libraries(humanoid_robot_intelligence_control_system_controller ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${YAML_CPP_LIBRARIES}) -install(TARGETS humanoid_robot_controller +install(TARGETS humanoid_robot_intelligence_control_system_controller ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} diff --git a/humanoid_robot_controller/include/humanoid_robot_controller/humanoid_robot_controller.h b/humanoid_robot_intelligence_control_system_controller/include/humanoid_robot_intelligence_control_system_controller/humanoid_robot_intelligence_control_system_controller.h similarity index 72% rename from humanoid_robot_controller/include/humanoid_robot_controller/humanoid_robot_controller.h rename to humanoid_robot_intelligence_control_system_controller/include/humanoid_robot_intelligence_control_system_controller/humanoid_robot_intelligence_control_system_controller.h index c9d09e3..019a9bb 100755 --- a/humanoid_robot_controller/include/humanoid_robot_controller/humanoid_robot_controller.h +++ b/humanoid_robot_intelligence_control_system_controller/include/humanoid_robot_intelligence_control_system_controller/humanoid_robot_intelligence_control_system_controller.h @@ -15,7 +15,7 @@ *******************************************************************************/ /* - * humanoid_robot_controller.h + * humanoid_robot_intelligence_control_system_controller.h * * Created on: 2016. 1. 15. * Author: zerom @@ -32,21 +32,21 @@ #include #include -#include "humanoid_robot_controller_msgs/GetJointModule.h" -#include "humanoid_robot_controller_msgs/JointCtrlModule.h" -#include "humanoid_robot_controller_msgs/LoadOffset.h" -#include "humanoid_robot_controller_msgs/SetJointModule.h" -#include "humanoid_robot_controller_msgs/SetModule.h" -#include "humanoid_robot_controller_msgs/SyncWriteItem.h" -#include "humanoid_robot_controller_msgs/WriteControlTable.h" +#include "humanoid_robot_intelligence_control_system_controller_msgs/GetJointModule.h" +#include "humanoid_robot_intelligence_control_system_controller_msgs/JointCtrlModule.h" +#include "humanoid_robot_intelligence_control_system_controller_msgs/LoadOffset.h" +#include "humanoid_robot_intelligence_control_system_controller_msgs/SetJointModule.h" +#include "humanoid_robot_intelligence_control_system_controller_msgs/SetModule.h" +#include "humanoid_robot_intelligence_control_system_controller_msgs/SyncWriteItem.h" +#include "humanoid_robot_intelligence_control_system_controller_msgs/WriteControlTable.h" #include "dynamixel_sdk/group_bulk_read.h" #include "dynamixel_sdk/group_sync_write.h" -#include "humanoid_robot_device/robot.h" -#include "humanoid_robot_framework_common/motion_module.h" -#include "humanoid_robot_framework_common/sensor_module.h" +#include "humanoid_robot_intelligence_control_system_device/robot.h" +#include "humanoid_robot_intelligence_control_system_framework_common/motion_module.h" +#include "humanoid_robot_intelligence_control_system_framework_common/sensor_module.h" -namespace humanoid_robot_framework { +namespace humanoid_robot_intelligence_control_system_framework { enum ControllerMode { MotionModuleMode, DirectControlMode }; @@ -75,7 +75,7 @@ private: void msgQueueThread(); void setCtrlModuleThread(std::string ctrl_module); void setJointCtrlModuleThread( - const humanoid_robot_controller_msgs::JointCtrlModule::ConstPtr &msg); + const humanoid_robot_intelligence_control_system_controller_msgs::JointCtrlModule::ConstPtr &msg); bool isTimerStopped(); void initializeSyncWrite(); @@ -143,27 +143,27 @@ public: /* ROS Topic Callback Functions */ void writeControlTableCallback( - const humanoid_robot_controller_msgs::WriteControlTable::ConstPtr &msg); + const humanoid_robot_intelligence_control_system_controller_msgs::WriteControlTable::ConstPtr &msg); void syncWriteItemCallback( - const humanoid_robot_controller_msgs::SyncWriteItem::ConstPtr &msg); + const humanoid_robot_intelligence_control_system_controller_msgs::SyncWriteItem::ConstPtr &msg); void setControllerModeCallback(const std_msgs::String::ConstPtr &msg); void setJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg); void setJointCtrlModuleCallback( - const humanoid_robot_controller_msgs::JointCtrlModule::ConstPtr &msg); + const humanoid_robot_intelligence_control_system_controller_msgs::JointCtrlModule::ConstPtr &msg); void setCtrlModuleCallback(const std_msgs::String::ConstPtr &msg); void enableOffsetCallback(const std_msgs::Bool::ConstPtr &msg); bool getJointCtrlModuleService( - humanoid_robot_controller_msgs::GetJointModule::Request &req, - humanoid_robot_controller_msgs::GetJointModule::Response &res); + humanoid_robot_intelligence_control_system_controller_msgs::GetJointModule::Request &req, + humanoid_robot_intelligence_control_system_controller_msgs::GetJointModule::Response &res); bool setJointCtrlModuleService( - humanoid_robot_controller_msgs::SetJointModule::Request &req, - humanoid_robot_controller_msgs::SetJointModule::Response &res); + humanoid_robot_intelligence_control_system_controller_msgs::SetJointModule::Request &req, + humanoid_robot_intelligence_control_system_controller_msgs::SetJointModule::Response &res); bool setCtrlModuleService( - humanoid_robot_controller_msgs::SetModule::Request &req, - humanoid_robot_controller_msgs::SetModule::Response &res); + humanoid_robot_intelligence_control_system_controller_msgs::SetModule::Request &req, + humanoid_robot_intelligence_control_system_controller_msgs::SetModule::Response &res); bool - loadOffsetService(humanoid_robot_controller_msgs::LoadOffset::Request &req, - humanoid_robot_controller_msgs::LoadOffset::Response &res); + loadOffsetService(humanoid_robot_intelligence_control_system_controller_msgs::LoadOffset::Request &req, + humanoid_robot_intelligence_control_system_controller_msgs::LoadOffset::Response &res); void gazeboJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg); @@ -204,6 +204,6 @@ public: uint8_t *data, uint8_t *error = 0); }; -} // namespace humanoid_robot_framework +} // namespace humanoid_robot_intelligence_control_system_framework #endif /* ROBOTIS_CONTROLLER_ROBOTIS_CONTROLLER_H_ */ diff --git a/humanoid_robot_controller/package.xml b/humanoid_robot_intelligence_control_system_controller/package.xml similarity index 65% rename from humanoid_robot_controller/package.xml rename to humanoid_robot_intelligence_control_system_controller/package.xml index 1a94e13..302552d 100755 --- a/humanoid_robot_controller/package.xml +++ b/humanoid_robot_intelligence_control_system_controller/package.xml @@ -1,8 +1,8 @@ - humanoid_robot_controller + humanoid_robot_intelligence_control_system_controller 0.3.2 - humanoid_robot_controller package for ROBOTIS's platform like Manipulator-H, THORMANG and OP series + humanoid_robot_intelligence_control_system_controller package for ROBOTIS's platform like Manipulator-H, THORMANG and OP series Apache 2.0 Ronaldson Bellande @@ -13,10 +13,10 @@ roslib std_msgs sensor_msgs - humanoid_robot_controller_msgs + humanoid_robot_intelligence_control_system_controller_msgs dynamixel_sdk - humanoid_robot_device - humanoid_robot_framework_common + humanoid_robot_intelligence_control_system_device + humanoid_robot_intelligence_control_system_framework_common cmake_modules yaml-cpp @@ -24,10 +24,10 @@ roslib std_msgs sensor_msgs - humanoid_robot_controller_msgs + humanoid_robot_intelligence_control_system_controller_msgs dynamixel_sdk - humanoid_robot_device - humanoid_robot_framework_common + humanoid_robot_intelligence_control_system_device + humanoid_robot_intelligence_control_system_framework_common cmake_modules yaml-cpp @@ -35,10 +35,10 @@ roslib std_msgs sensor_msgs - humanoid_robot_controller_msgs + humanoid_robot_intelligence_control_system_controller_msgs dynamixel_sdk - humanoid_robot_device - humanoid_robot_framework_common + humanoid_robot_intelligence_control_system_device + humanoid_robot_intelligence_control_system_framework_common cmake_modules yaml-cpp @@ -49,10 +49,10 @@ roslib std_msgs sensor_msgs - humanoid_robot_controller_msgs + humanoid_robot_intelligence_control_system_controller_msgs dynamixel_sdk - humanoid_robot_device - humanoid_robot_framework_common + humanoid_robot_intelligence_control_system_device + humanoid_robot_intelligence_control_system_framework_common cmake_modules yaml-cpp @@ -60,10 +60,10 @@ roslib std_msgs sensor_msgs - humanoid_robot_controller_msgs + humanoid_robot_intelligence_control_system_controller_msgs dynamixel_sdk - humanoid_robot_device - humanoid_robot_framework_common + humanoid_robot_intelligence_control_system_device + humanoid_robot_intelligence_control_system_framework_common cmake_modules yaml-cpp @@ -71,10 +71,10 @@ roslib std_msgs sensor_msgs - humanoid_robot_controller_msgs + humanoid_robot_intelligence_control_system_controller_msgs dynamixel_sdk - humanoid_robot_device - humanoid_robot_framework_common + humanoid_robot_intelligence_control_system_device + humanoid_robot_intelligence_control_system_framework_common cmake_modules yaml-cpp diff --git a/humanoid_robot_intelligence_control_system_controller/src/humanoid_robot_intelligence_control_system_controller/humanoid_robot_intelligence_control_system_controller.cpp b/humanoid_robot_intelligence_control_system_controller/src/humanoid_robot_intelligence_control_system_controller/humanoid_robot_intelligence_control_system_controller.cpp new file mode 100755 index 0000000..1204a29 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_controller/src/humanoid_robot_intelligence_control_system_controller/humanoid_robot_intelligence_control_system_controller.cpp @@ -0,0 +1,2625 @@ +/******************************************************************************* + * Copyright 2018 ROBOTIS CO., LTD. + * + * 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. + *******************************************************************************/ + +/* + * humanoid_robot_intelligence_control_system_controller.cpp + * + * Created on: 2016. 1. 15. + * Author: zerom + */ + +#include +#include + +#include "humanoid_robot_intelligence_control_system_controller/humanoid_robot_intelligence_control_system_controller.h" + +using namespace humanoid_robot_intelligence_control_system_framework; + +RobotisController::RobotisController() + : is_timer_running_(false), is_offset_enabled_(true), offset_ratio_(1.0), + stop_timer_(false), init_pose_loaded_(false), timer_thread_(0), + controller_mode_(MotionModuleMode), DEBUG_PRINT(false), robot_(0), + gazebo_mode_(false), + gazebo_robot_name_("humanoid_robot_intelligence_control_system") { + direct_sync_write_.clear(); +} + +void RobotisController::initializeSyncWrite() { + if (gazebo_mode_ == true) + return; + + // ROS_INFO("FIRST BULKREAD"); + for (auto &it : port_to_bulk_read_) + it.second->txRxPacket(); + for (auto &it : port_to_bulk_read_) { + int error_count = 0; + int result = COMM_SUCCESS; + do { + if (++error_count > 10) { + ROS_ERROR("[RobotisController] first bulk read fail!!"); + exit(-1); + } + usleep(10 * 1000); + result = it.second->txRxPacket(); + } while (result != COMM_SUCCESS); + } + init_pose_loaded_ = true; + // ROS_INFO("FIRST BULKREAD END"); + + // clear syncwrite param setting + for (auto &it : port_to_sync_write_position_) { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto &it : port_to_sync_write_position_p_gain_) { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto &it : port_to_sync_write_position_i_gain_) { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto &it : port_to_sync_write_position_d_gain_) { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto &it : port_to_sync_write_velocity_) { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto &it : port_to_sync_write_velocity_p_gain_) { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto &it : port_to_sync_write_velocity_i_gain_) { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto &it : port_to_sync_write_velocity_d_gain_) { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto &it : port_to_sync_write_current_) { + if (it.second != NULL) + it.second->clearParam(); + } + + // set init syncwrite param(from data of bulkread) + for (auto &it : robot_->dxls_) { + std::string joint_name = it.first; + Dynamixel *dxl = it.second; + + for (int i = 0; i < dxl->bulk_read_items_.size(); i++) { + uint32_t read_data = 0; + uint8_t sync_write_data[4]; + + if (port_to_bulk_read_[dxl->port_name_]->isAvailable( + dxl->id_, dxl->bulk_read_items_[i]->address_, + dxl->bulk_read_items_[i]->data_length_) == true) { + read_data = port_to_bulk_read_[dxl->port_name_]->getData( + dxl->id_, dxl->bulk_read_items_[i]->address_, + dxl->bulk_read_items_[i]->data_length_); + + sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(read_data)); + sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(read_data)); + sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(read_data)); + sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(read_data)); + + if ((dxl->present_position_item_ != 0) && + (dxl->bulk_read_items_[i]->item_name_ == + dxl->present_position_item_->item_name_)) { + dxl->dxl_state_->present_position_ = + dxl->convertValue2Radian(read_data) - + dxl->dxl_state_->position_offset_ * offset_ratio_; + dxl->dxl_state_->goal_position_ = dxl->dxl_state_->present_position_; + + port_to_sync_write_position_[dxl->port_name_]->addParam( + dxl->id_, sync_write_data); + } else if ((dxl->position_p_gain_item_ != 0) && + (dxl->bulk_read_items_[i]->item_name_ == + dxl->position_p_gain_item_->item_name_)) { + dxl->dxl_state_->position_p_gain_ = read_data; + } else if ((dxl->position_i_gain_item_ != 0) && + (dxl->bulk_read_items_[i]->item_name_ == + dxl->position_i_gain_item_->item_name_)) { + dxl->dxl_state_->position_i_gain_ = read_data; + } else if ((dxl->position_d_gain_item_ != 0) && + (dxl->bulk_read_items_[i]->item_name_ == + dxl->position_d_gain_item_->item_name_)) { + dxl->dxl_state_->position_d_gain_ = read_data; + } else if ((dxl->present_velocity_item_ != 0) && + (dxl->bulk_read_items_[i]->item_name_ == + dxl->present_velocity_item_->item_name_)) { + dxl->dxl_state_->present_velocity_ = + dxl->convertValue2Velocity(read_data); + dxl->dxl_state_->goal_velocity_ = dxl->dxl_state_->present_velocity_; + } else if ((dxl->velocity_p_gain_item_ != 0) && + (dxl->bulk_read_items_[i]->item_name_ == + dxl->velocity_p_gain_item_->item_name_)) { + dxl->dxl_state_->velocity_p_gain_ = read_data; + } else if ((dxl->velocity_i_gain_item_ != 0) && + (dxl->bulk_read_items_[i]->item_name_ == + dxl->velocity_i_gain_item_->item_name_)) { + dxl->dxl_state_->velocity_i_gain_ = read_data; + } else if ((dxl->velocity_d_gain_item_ != 0) && + (dxl->bulk_read_items_[i]->item_name_ == + dxl->velocity_d_gain_item_->item_name_)) { + dxl->dxl_state_->velocity_d_gain_ = read_data; + } else if ((dxl->present_current_item_ != 0) && + (dxl->bulk_read_items_[i]->item_name_ == + dxl->present_current_item_->item_name_)) { + dxl->dxl_state_->present_torque_ = + dxl->convertValue2Torque(read_data); + dxl->dxl_state_->goal_torque_ = dxl->dxl_state_->present_torque_; + } + } + } + } +} + +bool RobotisController::initialize(const std::string robot_file_path, + const std::string init_file_path) { + std::string dev_desc_dir_path = + ros::package::getPath( + "humanoid_robot_intelligence_control_system_device") + + "/devices"; + + // load robot info : port , device + robot_ = new Robot(robot_file_path, dev_desc_dir_path); + + if (gazebo_mode_ == true) { + queue_thread_ = + boost::thread(boost::bind(&RobotisController::msgQueueThread, this)); + return true; + } + + for (auto &it : robot_->ports_) { + std::string port_name = it.first; + dynamixel::PortHandler *port = it.second; + dynamixel::PacketHandler *default_pkt_handler = + dynamixel::PacketHandler::getPacketHandler(2.0); + + if (port->setBaudRate(port->getBaudRate()) == false) { + ROS_ERROR("PORT [%s] SETUP ERROR! (baudrate: %d)", port_name.c_str(), + port->getBaudRate()); + exit(-1); + } + + // get the default device info of the port + std::string default_device_name = robot_->port_default_device_[port_name]; + auto dxl_it = robot_->dxls_.find(default_device_name); + auto sensor_it = robot_->sensors_.find(default_device_name); + if (dxl_it != robot_->dxls_.end()) { + Dynamixel *default_device = dxl_it->second; + default_pkt_handler = dynamixel::PacketHandler::getPacketHandler( + default_device->protocol_version_); + + if (default_device->goal_position_item_ != 0) { + port_to_sync_write_position_[port_name] = new dynamixel::GroupSyncWrite( + port, default_pkt_handler, + default_device->goal_position_item_->address_, + default_device->goal_position_item_->data_length_); + } + + if (default_device->position_p_gain_item_ != 0) { + port_to_sync_write_position_p_gain_[port_name] = + new dynamixel::GroupSyncWrite( + port, default_pkt_handler, + default_device->position_p_gain_item_->address_, + default_device->position_p_gain_item_->data_length_); + } + + if (default_device->position_i_gain_item_ != 0) { + port_to_sync_write_position_i_gain_[port_name] = + new dynamixel::GroupSyncWrite( + port, default_pkt_handler, + default_device->position_i_gain_item_->address_, + default_device->position_i_gain_item_->data_length_); + } + + if (default_device->position_d_gain_item_ != 0) { + port_to_sync_write_position_d_gain_[port_name] = + new dynamixel::GroupSyncWrite( + port, default_pkt_handler, + default_device->position_d_gain_item_->address_, + default_device->position_d_gain_item_->data_length_); + } + + if (default_device->goal_velocity_item_ != 0) { + port_to_sync_write_velocity_[port_name] = new dynamixel::GroupSyncWrite( + port, default_pkt_handler, + default_device->goal_velocity_item_->address_, + default_device->goal_velocity_item_->data_length_); + } + + if (default_device->velocity_p_gain_item_ != 0) { + port_to_sync_write_velocity_p_gain_[port_name] = + new dynamixel::GroupSyncWrite( + port, default_pkt_handler, + default_device->velocity_p_gain_item_->address_, + default_device->velocity_p_gain_item_->data_length_); + } + + if (default_device->velocity_i_gain_item_ != 0) { + port_to_sync_write_velocity_i_gain_[port_name] = + new dynamixel::GroupSyncWrite( + port, default_pkt_handler, + default_device->velocity_i_gain_item_->address_, + default_device->velocity_i_gain_item_->data_length_); + } + + if (default_device->velocity_d_gain_item_ != 0) { + port_to_sync_write_velocity_d_gain_[port_name] = + new dynamixel::GroupSyncWrite( + port, default_pkt_handler, + default_device->velocity_d_gain_item_->address_, + default_device->velocity_d_gain_item_->data_length_); + } + + if (default_device->goal_current_item_ != 0) { + port_to_sync_write_current_[port_name] = new dynamixel::GroupSyncWrite( + port, default_pkt_handler, + default_device->goal_current_item_->address_, + default_device->goal_current_item_->data_length_); + } + } else if (sensor_it != robot_->sensors_.end()) { + Sensor *_default_device = sensor_it->second; + default_pkt_handler = dynamixel::PacketHandler::getPacketHandler( + _default_device->protocol_version_); + } + + port_to_bulk_read_[port_name] = + new dynamixel::GroupBulkRead(port, default_pkt_handler); + } + + // (for loop) check all dxls are connected. + for (auto &it : robot_->dxls_) { + std::string joint_name = it.first; + Dynamixel *dxl = it.second; + + if (ping(joint_name) != 0) { + usleep(10 * 1000); + if (ping(joint_name) != 0) + ROS_ERROR("JOINT[%s] does NOT respond!!", joint_name.c_str()); + } + } + + initializeDevice(init_file_path); + + queue_thread_ = + boost::thread(boost::bind(&RobotisController::msgQueueThread, this)); + return true; +} + +void RobotisController::initializeDevice(const std::string init_file_path) { + // device initialize + if (DEBUG_PRINT) + ROS_WARN("INIT FILE LOAD"); + + YAML::Node doc; + try { + doc = YAML::LoadFile(init_file_path.c_str()); + + for (YAML::const_iterator it_doc = doc.begin(); it_doc != doc.end(); + it_doc++) { + std::string joint_name = it_doc->first.as(); + + YAML::Node joint_node = doc[joint_name]; + if (joint_node.size() == 0) + continue; + + Dynamixel *dxl = NULL; + auto dxl_it = robot_->dxls_.find(joint_name); + if (dxl_it != robot_->dxls_.end()) + dxl = dxl_it->second; + + if (dxl == NULL) { + ROS_WARN("Joint [%s] was not found.", joint_name.c_str()); + continue; + } + if (DEBUG_PRINT) + ROS_INFO("JOINT_NAME: %s", joint_name.c_str()); + + uint8_t torque_enabled = 0; + read1Byte(joint_name, dxl->torque_enable_item_->address_, + &torque_enabled); + + for (YAML::const_iterator it_joint = joint_node.begin(); + it_joint != joint_node.end(); it_joint++) { + std::string item_name = it_joint->first.as(); + + if (DEBUG_PRINT) + ROS_INFO(" ITEM_NAME: %s", item_name.c_str()); + + uint32_t value = it_joint->second.as(); + + ControlTableItem *item = dxl->ctrl_table_[item_name]; + if (item == NULL) { + ROS_WARN("Control Item [%s] was not found.", item_name.c_str()); + continue; + } + + if (item->memory_type_ == EEPROM) { + uint8_t data8 = 0; + uint16_t data16 = 0; + uint32_t data32 = 0; + + switch (item->data_length_) { + case 1: + read1Byte(joint_name, item->address_, &data8); + if (data8 == value) + continue; + break; + case 2: + read2Byte(joint_name, item->address_, &data16); + if (data16 == value) + continue; + break; + case 4: + read4Byte(joint_name, item->address_, &data32); + if (data32 == value) + continue; + break; + default: + break; + } + + if (torque_enabled == 1) { + ROS_ERROR( + "################\nThe initial value of the EEPROM area has " + "been changed. \nTurn off Torque Enable and try again."); + exit(-1); + } + } + + switch (item->data_length_) { + case 1: + write1Byte(joint_name, item->address_, (uint8_t)value); + break; + case 2: + write2Byte(joint_name, item->address_, (uint16_t)value); + break; + case 4: + write4Byte(joint_name, item->address_, value); + break; + default: + break; + } + + if (item->memory_type_ == EEPROM) { + // Write to EEPROM -> delay is required (max delay: 55 msec per byte) + usleep(item->data_length_ * 55 * 1000); + } + } + } + } catch (const std::exception &e) { + ROS_INFO("Dynamixel Init file not found."); + } + + // [ BulkRead ] StartAddress : Present Position , Length : 10 ( + // Position/Velocity/Current ) + for (auto &it : robot_->ports_) { + if (port_to_bulk_read_[it.first] != 0) + port_to_bulk_read_[it.first]->clearParam(); + } + for (auto &it : robot_->dxls_) { + std::string joint_name = it.first; + Dynamixel *dxl = it.second; + + if (dxl == NULL) + continue; + + int bulkread_start_addr = 0; + int bulkread_data_length = 0; + + // // bulk read default : present position + // if(dxl->present_position_item != 0) + // { + // bulkread_start_addr = dxl->present_position_item->address; + // bulkread_data_length = dxl->present_position_item->data_length; + // } + + uint8_t torque_enabled = 0; + read1Byte(joint_name, dxl->torque_enable_item_->address_, &torque_enabled); + + // calculate bulk read start address & data length + auto indirect_addr_it = dxl->ctrl_table_.find(INDIRECT_ADDRESS_1); + if (indirect_addr_it != dxl->ctrl_table_.end()) // INDIRECT_ADDRESS_1 exist + { + if (dxl->bulk_read_items_.size() != 0) { + uint16_t data16 = 0; + + bulkread_start_addr = dxl->bulk_read_items_[0]->address_; + bulkread_data_length = 0; + + // set indirect address + int indirect_addr = indirect_addr_it->second->address_; + for (int i = 0; i < dxl->bulk_read_items_.size(); i++) { + int addr_leng = dxl->bulk_read_items_[i]->data_length_; + + bulkread_data_length += addr_leng; + for (int l = 0; l < addr_leng; l++) { + // ROS_WARN("[%12s] INDIR_ADDR: %d, ITEM_ADDR: %d", + // joint_name.c_str(), indirect_addr, + // dxl->ctrl_table[dxl->bulk_read_items[i]->item_name]->address + + // _l); + + read2Byte(joint_name, indirect_addr, &data16); + if (data16 != dxl->ctrl_table_[dxl->bulk_read_items_[i]->item_name_] + ->address_ + + l) { + if (torque_enabled == 1) { + ROS_ERROR("################\nThe indirect address of the " + "EEPROM area has been changed. \nTurn off Torque " + "Enable and try again."); + exit(-1); + } + write2Byte(joint_name, indirect_addr, + dxl->ctrl_table_[dxl->bulk_read_items_[i]->item_name_] + ->address_ + + l); + } + indirect_addr += 2; + } + } + } + } else // INDIRECT_ADDRESS_1 NOT exist + { + if (dxl->bulk_read_items_.size() != 0) { + bulkread_start_addr = dxl->bulk_read_items_[0]->address_; + bulkread_data_length = 0; + + ControlTableItem *last_item = dxl->bulk_read_items_[0]; + + for (int i = 0; i < dxl->bulk_read_items_.size(); i++) { + int addr = dxl->bulk_read_items_[i]->address_; + if (addr < bulkread_start_addr) + bulkread_start_addr = addr; + else if (last_item->address_ < addr) + last_item = dxl->bulk_read_items_[i]; + } + + bulkread_data_length = + last_item->address_ - bulkread_start_addr + last_item->data_length_; + } + } + + // ROS_WARN("[%12s] start_addr: %d, data_length: %d", joint_name.c_str(), + // bulkread_start_addr, bulkread_data_length); + if (bulkread_start_addr != 0) + port_to_bulk_read_[dxl->port_name_]->addParam( + dxl->id_, bulkread_start_addr, bulkread_data_length); + + // Torque ON + if (writeCtrlItem(joint_name, dxl->torque_enable_item_->item_name_, 1) != + COMM_SUCCESS) + writeCtrlItem(joint_name, dxl->torque_enable_item_->item_name_, 1); + } + + for (auto &it : robot_->sensors_) { + std::string sensor_name = it.first; + Sensor *sensor = it.second; + + if (sensor == NULL) + continue; + + int bulkread_start_addr = 0; + int bulkread_data_length = 0; + + // calculate bulk read start address & data length + auto indirect_addr_it = sensor->ctrl_table_.find(INDIRECT_ADDRESS_1); + if (indirect_addr_it != + sensor->ctrl_table_.end()) // INDIRECT_ADDRESS_1 exist + { + if (sensor->bulk_read_items_.size() != 0) { + uint16_t data16 = 0; + + bulkread_start_addr = sensor->bulk_read_items_[0]->address_; + bulkread_data_length = 0; + + // set indirect address + int indirect_addr = indirect_addr_it->second->address_; + for (int i = 0; i < sensor->bulk_read_items_.size(); i++) { + int addr_leng = sensor->bulk_read_items_[i]->data_length_; + + bulkread_data_length += addr_leng; + for (int l = 0; l < addr_leng; l++) { + // ROS_WARN("[%12s] INDIR_ADDR: %d, ITEM_ADDR: %d", + // sensor_name.c_str(), indirect_addr, + // sensor->ctrl_table[sensor->bulk_read_items[i]->item_name]->address + // + _l); + read2Byte(sensor_name, indirect_addr, &data16); + if (data16 != + sensor->ctrl_table_[sensor->bulk_read_items_[i]->item_name_] + ->address_ + + l) { + write2Byte( + sensor_name, indirect_addr, + sensor->ctrl_table_[sensor->bulk_read_items_[i]->item_name_] + ->address_ + + l); + } + indirect_addr += 2; + } + } + } + } else // INDIRECT_ADDRESS_1 NOT exist + { + if (sensor->bulk_read_items_.size() != 0) { + bulkread_start_addr = sensor->bulk_read_items_[0]->address_; + bulkread_data_length = 0; + + ControlTableItem *last_item = sensor->bulk_read_items_[0]; + + for (int i = 0; i < sensor->bulk_read_items_.size(); i++) { + int addr = sensor->bulk_read_items_[i]->address_; + if (addr < bulkread_start_addr) + bulkread_start_addr = addr; + else if (last_item->address_ < addr) + last_item = sensor->bulk_read_items_[i]; + } + + bulkread_data_length = + last_item->address_ - bulkread_start_addr + last_item->data_length_; + } + } + + // ROS_WARN("[%12s] start_addr: %d, data_length: %d", sensor_name.c_str(), + // bulkread_start_addr, bulkread_data_length); + if (bulkread_start_addr != 0) + port_to_bulk_read_[sensor->port_name_]->addParam( + sensor->id_, bulkread_start_addr, bulkread_data_length); + } +} + +void RobotisController::gazeboTimerThread() { + ros::Rate gazebo_rate(1000 / robot_->getControlCycle()); + + while (!stop_timer_) { + if (init_pose_loaded_ == true) + process(); + gazebo_rate.sleep(); + } +} + +void RobotisController::msgQueueThread() { + ros::NodeHandle ros_node; + ros::CallbackQueue callback_queue; + + ros_node.setCallbackQueue(&callback_queue); + + /* subscriber */ + ros::Subscriber write_control_table_sub = ros_node.subscribe( + "/humanoid_robot_intelligence_control_system/write_control_table", 5, + &RobotisController::writeControlTableCallback, this); + ros::Subscriber sync_write_item_sub = ros_node.subscribe( + "/humanoid_robot_intelligence_control_system/sync_write_item", 10, + &RobotisController::syncWriteItemCallback, this); + ros::Subscriber joint_ctrl_modules_sub = ros_node.subscribe( + "/humanoid_robot_intelligence_control_system/set_joint_ctrl_modules", 10, + &RobotisController::setJointCtrlModuleCallback, this); + ros::Subscriber enable_ctrl_module_sub = ros_node.subscribe( + "/humanoid_robot_intelligence_control_system/enable_ctrl_module", 10, + &RobotisController::setCtrlModuleCallback, this); + ros::Subscriber control_mode_sub = ros_node.subscribe( + "/humanoid_robot_intelligence_control_system/set_control_mode", 10, + &RobotisController::setControllerModeCallback, this); + ros::Subscriber joint_states_sub = ros_node.subscribe( + "/humanoid_robot_intelligence_control_system/set_joint_states", 10, + &RobotisController::setJointStatesCallback, this); + ros::Subscriber enable_offset_sub = ros_node.subscribe( + "/humanoid_robot_intelligence_control_system/enable_offset", 10, + &RobotisController::enableOffsetCallback, this); + + ros::Subscriber gazebo_joint_states_sub; + if (gazebo_mode_ == true) + gazebo_joint_states_sub = + ros_node.subscribe("/" + gazebo_robot_name_ + "/joint_states", 10, + &RobotisController::gazeboJointStatesCallback, this); + + /* publisher */ + goal_joint_state_pub_ = ros_node.advertise( + "/humanoid_robot_intelligence_control_system/goal_joint_states", 10); + present_joint_state_pub_ = ros_node.advertise( + "/humanoid_robot_intelligence_control_system/present_joint_states", 10); + current_module_pub_ = ros_node.advertise< + humanoid_robot_intelligence_control_system_controller_msgs:: + JointCtrlModule>( + "/humanoid_robot_intelligence_control_system/present_joint_ctrl_modules", + 10); + + if (gazebo_mode_ == true) { + for (auto &it : robot_->dxls_) { + gazebo_joint_position_pub_[it.first] = + ros_node.advertise( + "/" + gazebo_robot_name_ + "/" + it.first + "_position/command", + 1); + gazebo_joint_velocity_pub_[it.first] = + ros_node.advertise( + "/" + gazebo_robot_name_ + "/" + it.first + "_velocity/command", + 1); + gazebo_joint_effort_pub_[it.first] = + ros_node.advertise( + "/" + gazebo_robot_name_ + "/" + it.first + "_effort/command", 1); + } + } + + /* service */ + ros::ServiceServer get_joint_module_server = ros_node.advertiseService( + "/humanoid_robot_intelligence_control_system/" + "get_present_joint_ctrl_modules", + &RobotisController::getJointCtrlModuleService, this); + ros::ServiceServer set_joint_module_server = ros_node.advertiseService( + "/humanoid_robot_intelligence_control_system/" + "set_present_joint_ctrl_modules", + &RobotisController::setJointCtrlModuleService, this); + ros::ServiceServer set_module_server = ros_node.advertiseService( + "/humanoid_robot_intelligence_control_system/set_present_ctrl_modules", + &RobotisController::setCtrlModuleService, this); + ros::ServiceServer load_offset_server = ros_node.advertiseService( + "/humanoid_robot_intelligence_control_system/load_offset", + &RobotisController::loadOffsetService, this); + + ros::WallDuration duration(robot_->getControlCycle() / 1000.0); + while (ros_node.ok()) + callback_queue.callAvailable(duration); +} + +void *RobotisController::timerThread(void *param) { + RobotisController *controller = (RobotisController *)param; + static struct timespec next_time; + static struct timespec curr_time; + + ROS_DEBUG("controller::thread_proc started"); + + clock_gettime(CLOCK_MONOTONIC, &next_time); + + while (!controller->stop_timer_) { + next_time.tv_sec += + (next_time.tv_nsec + controller->robot_->getControlCycle() * 1000000) / + 1000000000; + next_time.tv_nsec = + (next_time.tv_nsec + controller->robot_->getControlCycle() * 1000000) % + 1000000000; + + controller->process(); + + clock_gettime(CLOCK_MONOTONIC, &curr_time); + long delta_nsec = (next_time.tv_sec - curr_time.tv_sec) * 1000000000 + + (next_time.tv_nsec - curr_time.tv_nsec); + if (delta_nsec < -100000) { + if (controller->DEBUG_PRINT == true) { + fprintf(stderr, + "[RobotisController::ThreadProc] NEXT TIME < CURR TIME.. " + "(%f)[%ld.%09ld / %ld.%09ld]", + delta_nsec / 1000000.0, (long)next_time.tv_sec, + (long)next_time.tv_nsec, (long)curr_time.tv_sec, + (long)curr_time.tv_nsec); + } + + // next_time = curr_time + 3 msec + next_time.tv_sec = + curr_time.tv_sec + (curr_time.tv_nsec + 3000000) / 1000000000; + next_time.tv_nsec = (curr_time.tv_nsec + 3000000) % 1000000000; + } + + clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &next_time, NULL); + } + return 0; +} + +void RobotisController::startTimer() { + if (this->is_timer_running_ == true) + return; + + if (this->gazebo_mode_ == true) { + // create and start the thread + gazebo_thread_ = + boost::thread(boost::bind(&RobotisController::gazeboTimerThread, this)); + } else { + initializeSyncWrite(); + + for (auto &it : port_to_bulk_read_) { + it.second->txPacket(); + } + + usleep(8 * 1000); + + int error; + struct sched_param param; + pthread_attr_t attr; + + pthread_attr_init(&attr); + + error = pthread_attr_setschedpolicy(&attr, SCHED_RR); + if (error != 0) + ROS_ERROR("pthread_attr_setschedpolicy error = %d\n", error); + error = pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED); + if (error != 0) + ROS_ERROR("pthread_attr_setinheritsched error = %d\n", error); + + memset(¶m, 0, sizeof(param)); + param.sched_priority = 31; // RT + error = pthread_attr_setschedparam(&attr, ¶m); + if (error != 0) + ROS_ERROR("pthread_attr_setschedparam error = %d\n", error); + + // create and start the thread + if ((error = pthread_create(&this->timer_thread_, &attr, this->timerThread, + this)) != 0) { + ROS_ERROR("Creating timer thread failed!!"); + exit(-1); + } + } + + this->is_timer_running_ = true; +} + +void RobotisController::stopTimer() { + int error = 0; + + // set the flag to stop the thread + if (this->is_timer_running_) { + this->stop_timer_ = true; + + if (this->gazebo_mode_ == false) { + // wait until the thread is stopped. + if ((error = pthread_join(this->timer_thread_, NULL)) != 0) + exit(-1); + + for (auto &it : port_to_bulk_read_) { + if (it.second != NULL) + it.second->rxPacket(); + } + + for (auto &it : port_to_sync_write_position_) { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto &it : port_to_sync_write_position_p_gain_) { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto &it : port_to_sync_write_position_i_gain_) { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto &it : port_to_sync_write_position_d_gain_) { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto &it : port_to_sync_write_velocity_) { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto &it : port_to_sync_write_velocity_p_gain_) { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto &it : port_to_sync_write_velocity_i_gain_) { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto &it : port_to_sync_write_velocity_d_gain_) { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto &it : port_to_sync_write_current_) { + if (it.second != NULL) + it.second->clearParam(); + } + } else { + // wait until the thread is stopped. + gazebo_thread_.join(); + } + + this->stop_timer_ = false; + this->is_timer_running_ = false; + } +} + +bool RobotisController::isTimerRunning() { return this->is_timer_running_; } + +void RobotisController::loadOffset(const std::string path) { + YAML::Node doc; + try { + doc = YAML::LoadFile(path.c_str()); + } catch (const std::exception &e) { + ROS_WARN("Fail to load offset yaml."); + return; + } + + YAML::Node offset_node = doc["offset"]; + if (offset_node.size() == 0) + return; + + ROS_INFO("Load offsets..."); + for (YAML::const_iterator it = offset_node.begin(); it != offset_node.end(); + it++) { + std::string joint_name = it->first.as(); + double offset = it->second.as(); + + auto dxl_it = robot_->dxls_.find(joint_name); + if (dxl_it != robot_->dxls_.end()) + dxl_it->second->dxl_state_->position_offset_ = offset; + } +} + +void RobotisController::process() { + // avoid duplicated function call + static bool is_process_running = false; + if (is_process_running == true) + return; + is_process_running = true; + + // ROS_INFO("Controller::Process()"); + // offset ratio + if (is_offset_enabled_) { + if (offset_ratio_ < 1.0) + offset_ratio_ += 0.01; + else + offset_ratio_ = 1.0; + } else { + if (offset_ratio_ > 0.0) + offset_ratio_ -= 0.01; + else + offset_ratio_ = 0.0; + } + + ros::Time start_time; + ros::Duration time_duration; + + if (DEBUG_PRINT) + start_time = ros::Time::now(); + + sensor_msgs::JointState goal_state; + sensor_msgs::JointState present_state; + + present_state.header.stamp = ros::Time::now(); + goal_state.header.stamp = present_state.header.stamp; + + if (controller_mode_ == MotionModuleMode) { + if (gazebo_mode_ == false) { + // BulkRead Rx + for (auto &it : port_to_bulk_read_) { + robot_->ports_[it.first]->setPacketTimeout(0.0); + if (DEBUG_PRINT) { + int result = it.second->rxPacket(); + if (result != COMM_SUCCESS) + ROS_ERROR_STREAM("Bulk Read Fail : " << it.first); + } else + it.second->rxPacket(); + } + + // -> save to robot->dxls_[]->dxl_state_ + if (robot_->dxls_.size() > 0) { + for (auto &dxl_it : robot_->dxls_) { + Dynamixel *dxl = dxl_it.second; + std::string port_name = dxl_it.second->port_name_; + std::string joint_name = dxl_it.first; + + if (dxl->bulk_read_items_.size() > 0) { + bool updated = false; + uint32_t data = 0; + for (int i = 0; i < dxl->bulk_read_items_.size(); i++) { + ControlTableItem *item = dxl->bulk_read_items_[i]; + if (port_to_bulk_read_[port_name]->isAvailable( + dxl->id_, item->address_, item->data_length_) == true) { + updated = true; + data = port_to_bulk_read_[port_name]->getData( + dxl->id_, item->address_, item->data_length_); + + // change dxl_state + if (dxl->present_position_item_ != 0 && + item->item_name_ == + dxl->present_position_item_->item_name_) { + dxl->dxl_state_->present_position_ = + dxl->convertValue2Radian(data) - + dxl->dxl_state_->position_offset_ * offset_ratio_; + } else if (dxl->present_velocity_item_ != 0 && + item->item_name_ == + dxl->present_velocity_item_->item_name_) + dxl->dxl_state_->present_velocity_ = + dxl->convertValue2Velocity(data); + else if (dxl->present_current_item_ != 0 && + item->item_name_ == + dxl->present_current_item_->item_name_) + dxl->dxl_state_->present_torque_ = + dxl->convertValue2Torque(data); + else if (dxl->goal_position_item_ != 0 && + item->item_name_ == + dxl->goal_position_item_->item_name_) { + dxl->dxl_state_->goal_position_ = + dxl->convertValue2Radian(data) - + dxl->dxl_state_->position_offset_ * offset_ratio_; + } else if (dxl->goal_velocity_item_ != 0 && + item->item_name_ == + dxl->goal_velocity_item_->item_name_) + dxl->dxl_state_->goal_velocity_ = + dxl->convertValue2Velocity(data); + else if (dxl->goal_current_item_ != 0 && + item->item_name_ == + dxl->goal_current_item_->item_name_) + dxl->dxl_state_->goal_torque_ = + dxl->convertValue2Torque(data); + + dxl->dxl_state_->bulk_read_table_[item->item_name_] = data; + } + } + + // -> update time stamp to + // Robot->dxls[]->dynamixel_state.update_time_stamp + if (updated == true) + dxl->dxl_state_->update_time_stamp_ = + TimeStamp(present_state.header.stamp.sec, + present_state.header.stamp.nsec); + } + } + } + + // -> save to robot->sensors_[]->sensor_state_ + if (robot_->sensors_.size() > 0) { + for (auto &sensor_it : robot_->sensors_) { + Sensor *sensor = sensor_it.second; + std::string port_name = sensor_it.second->port_name_; + std::string sensor_name = sensor_it.first; + + if (sensor->bulk_read_items_.size() > 0) { + bool updated = false; + uint32_t data = 0; + for (int i = 0; i < sensor->bulk_read_items_.size(); i++) { + ControlTableItem *item = sensor->bulk_read_items_[i]; + if (port_to_bulk_read_[port_name]->isAvailable( + sensor->id_, item->address_, item->data_length_) == + true) { + updated = true; + data = port_to_bulk_read_[port_name]->getData( + sensor->id_, item->address_, item->data_length_); + + // change sensor_state + sensor->sensor_state_->bulk_read_table_[item->item_name_] = + data; + } + } + + // -> update time stamp to + // Robot->dxls[]->dynamixel_state.update_time_stamp + if (updated == true) + sensor->sensor_state_->update_time_stamp_ = + TimeStamp(present_state.header.stamp.sec, + present_state.header.stamp.nsec); + } + } + } + + if (DEBUG_PRINT) { + time_duration = ros::Time::now() - start_time; + fprintf(stderr, "(%2.6f) BulkRead Rx & update state \n", + time_duration.nsec * 0.000001); + } + + // SyncWrite + queue_mutex_.lock(); + + if (direct_sync_write_.size() > 0) { + for (int i = 0; i < direct_sync_write_.size(); i++) { + direct_sync_write_[i]->txPacket(); + direct_sync_write_[i]->clearParam(); + } + direct_sync_write_.clear(); + } + + if (port_to_sync_write_position_p_gain_.size() > 0) { + for (auto &it : port_to_sync_write_position_p_gain_) { + it.second->txPacket(); + it.second->clearParam(); + } + } + if (port_to_sync_write_position_i_gain_.size() > 0) { + for (auto &it : port_to_sync_write_position_i_gain_) { + it.second->txPacket(); + it.second->clearParam(); + } + } + if (port_to_sync_write_position_d_gain_.size() > 0) { + for (auto &it : port_to_sync_write_position_d_gain_) { + it.second->txPacket(); + it.second->clearParam(); + } + } + if (port_to_sync_write_velocity_p_gain_.size() > 0) { + for (auto &it : port_to_sync_write_velocity_p_gain_) { + it.second->txPacket(); + it.second->clearParam(); + } + } + if (port_to_sync_write_velocity_i_gain_.size() > 0) { + for (auto &it : port_to_sync_write_velocity_i_gain_) { + it.second->txPacket(); + it.second->clearParam(); + } + } + if (port_to_sync_write_velocity_d_gain_.size() > 0) { + for (auto &it : port_to_sync_write_velocity_d_gain_) { + it.second->txPacket(); + it.second->clearParam(); + } + } + for (auto &it : port_to_sync_write_position_) { + if (it.second != NULL) + it.second->txPacket(); + } + for (auto &it : port_to_sync_write_velocity_) { + if (it.second != NULL) + it.second->txPacket(); + } + for (auto &it : port_to_sync_write_current_) { + if (it.second != NULL) + it.second->txPacket(); + } + + queue_mutex_.unlock(); + + // BulkRead Tx + for (auto &it : port_to_bulk_read_) + it.second->txPacket(); + + if (DEBUG_PRINT) { + time_duration = ros::Time::now() - start_time; + fprintf(stderr, "(%2.6f) SyncWrite & BulkRead Tx \n", + time_duration.nsec * 0.000001); + } + } else if (gazebo_mode_ == true) { + std_msgs::Float64 joint_msg; + + for (auto &dxl_it : robot_->dxls_) { + std::string joint_name = dxl_it.first; + Dynamixel *dxl = dxl_it.second; + DynamixelState *dxl_state = dxl_it.second->dxl_state_; + + if (dxl->ctrl_module_name_ == "none") { + joint_msg.data = dxl_state->goal_position_; + gazebo_joint_position_pub_[joint_name].publish(joint_msg); + } + } + + for (auto module_it = motion_modules_.begin(); + module_it != motion_modules_.end(); module_it++) { + if ((*module_it)->getModuleEnable() == false) + continue; + + for (auto &dxl_it : robot_->dxls_) { + std::string joint_name = dxl_it.first; + Dynamixel *dxl = dxl_it.second; + DynamixelState *dxl_state = dxl_it.second->dxl_state_; + + if (dxl->ctrl_module_name_ == (*module_it)->getModuleName()) { + if ((*module_it)->getControlMode() == PositionControl) { + joint_msg.data = dxl_state->goal_position_; + gazebo_joint_position_pub_[joint_name].publish(joint_msg); + } else if ((*module_it)->getControlMode() == VelocityControl) { + joint_msg.data = dxl_state->goal_velocity_; + gazebo_joint_velocity_pub_[joint_name].publish(joint_msg); + } else if ((*module_it)->getControlMode() == TorqueControl) { + joint_msg.data = dxl_state->goal_torque_; + gazebo_joint_effort_pub_[joint_name].publish(joint_msg); + } + } + } + } + } + } else if (controller_mode_ == DirectControlMode) { + if (gazebo_mode_ == false) { + // BulkRead Rx + for (auto &it : port_to_bulk_read_) { + robot_->ports_[it.first]->setPacketTimeout(0.0); + it.second->rxPacket(); + } + + // -> save to robot->dxls_[]->dxl_state_ + if (robot_->dxls_.size() > 0) { + for (auto &dxl_it : robot_->dxls_) { + Dynamixel *dxl = dxl_it.second; + std::string port_name = dxl_it.second->port_name_; + std::string joint_name = dxl_it.first; + + if (dxl->bulk_read_items_.size() > 0) { + uint32_t data = 0; + for (int i = 0; i < dxl->bulk_read_items_.size(); i++) { + ControlTableItem *item = dxl->bulk_read_items_[i]; + if (port_to_bulk_read_[port_name]->isAvailable( + dxl->id_, item->address_, item->data_length_) == true) { + data = port_to_bulk_read_[port_name]->getData( + dxl->id_, item->address_, item->data_length_); + + // change dxl_state + if (dxl->present_position_item_ != 0 && + item->item_name_ == + dxl->present_position_item_->item_name_) { + dxl->dxl_state_->present_position_ = + dxl->convertValue2Radian(data) - + dxl->dxl_state_->position_offset_ * offset_ratio_; + } else if (dxl->present_velocity_item_ != 0 && + item->item_name_ == + dxl->present_velocity_item_->item_name_) + dxl->dxl_state_->present_velocity_ = + dxl->convertValue2Velocity(data); + else if (dxl->present_current_item_ != 0 && + item->item_name_ == + dxl->present_current_item_->item_name_) + dxl->dxl_state_->present_torque_ = + dxl->convertValue2Torque(data); + else if (dxl->goal_position_item_ != 0 && + item->item_name_ == + dxl->goal_position_item_->item_name_) { + dxl->dxl_state_->goal_position_ = + dxl->convertValue2Radian(data) - + dxl->dxl_state_->position_offset_ * offset_ratio_; + } else if (dxl->goal_velocity_item_ != 0 && + item->item_name_ == + dxl->goal_velocity_item_->item_name_) + dxl->dxl_state_->goal_velocity_ = + dxl->convertValue2Velocity(data); + else if (dxl->goal_current_item_ != 0 && + item->item_name_ == + dxl->goal_current_item_->item_name_) + dxl->dxl_state_->goal_torque_ = + dxl->convertValue2Torque(data); + + dxl->dxl_state_->bulk_read_table_[item->item_name_] = data; + } + } + + // -> update time stamp to + // Robot->dxls[]->dynamixel_state.update_time_stamp + dxl->dxl_state_->update_time_stamp_ = + TimeStamp(present_state.header.stamp.sec, + present_state.header.stamp.nsec); + } + } + } + + queue_mutex_.lock(); + + // for (auto& it : port_to_sync_write_position_) + // { + // it.second->txPacket(); + // it.second->clearParam(); + // } + + if (direct_sync_write_.size() > 0) { + for (int i = 0; i < direct_sync_write_.size(); i++) { + direct_sync_write_[i]->txPacket(); + direct_sync_write_[i]->clearParam(); + } + direct_sync_write_.clear(); + } + + queue_mutex_.unlock(); + + // BulkRead Tx + for (auto &it : port_to_bulk_read_) + it.second->txPacket(); + } + } + + // Call SensorModule Process() + // -> for loop : call SensorModule list -> Process() + if (sensor_modules_.size() > 0) { + for (auto module_it = sensor_modules_.begin(); + module_it != sensor_modules_.end(); module_it++) { + (*module_it)->process(robot_->dxls_, robot_->sensors_); + + for (auto &it : (*module_it)->result_) + sensor_result_[it.first] = it.second; + } + } + + if (DEBUG_PRINT) { + time_duration = ros::Time::now() - start_time; + fprintf(stderr, "(%2.6f) SensorModule Process() & save result \n", + time_duration.nsec * 0.000001); + } + + if (controller_mode_ == MotionModuleMode) { + // Call MotionModule Process() + // -> for loop : call MotionModule list -> Process() + if (motion_modules_.size() > 0) { + queue_mutex_.lock(); + + for (auto module_it = motion_modules_.begin(); + module_it != motion_modules_.end(); module_it++) { + if ((*module_it)->getModuleEnable() == false) + continue; + + (*module_it)->process(robot_->dxls_, sensor_result_); + + // for loop : joint list + for (auto &dxl_it : robot_->dxls_) { + std::string joint_name = dxl_it.first; + Dynamixel *dxl = dxl_it.second; + DynamixelState *dxl_state = dxl_it.second->dxl_state_; + + if (dxl->ctrl_module_name_ == (*module_it)->getModuleName()) { + // do_sync_write = true; + DynamixelState *result_state = (*module_it)->result_[joint_name]; + + if (result_state == NULL) { + ROS_ERROR("[%s] %s ", (*module_it)->getModuleName().c_str(), + joint_name.c_str()); + continue; + } + + // TODO: check update time stamp ? + + if ((*module_it)->getControlMode() == PositionControl) { + dxl_state->goal_position_ = result_state->goal_position_; + + if (gazebo_mode_ == false) { + // add offset + uint32_t pos_data; + pos_data = dxl->convertRadian2Value( + dxl_state->goal_position_ + + dxl_state->position_offset_ * offset_ratio_); + + uint8_t sync_write_data[4] = {0}; + sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(pos_data)); + sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(pos_data)); + sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(pos_data)); + sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(pos_data)); + + if (port_to_sync_write_position_[dxl->port_name_] != NULL) + port_to_sync_write_position_[dxl->port_name_]->changeParam( + dxl->id_, sync_write_data); + + // if position p gain value is changed -> sync write + if (result_state->position_p_gain_ != NONE_GAIN && + dxl_state->position_p_gain_ != + result_state->position_p_gain_) { + dxl_state->position_p_gain_ = result_state->position_p_gain_; + uint8_t sync_write_data[4] = {0}; + sync_write_data[0] = + DXL_LOBYTE(DXL_LOWORD(dxl_state->position_p_gain_)); + sync_write_data[1] = + DXL_HIBYTE(DXL_LOWORD(dxl_state->position_p_gain_)); + sync_write_data[2] = + DXL_LOBYTE(DXL_HIWORD(dxl_state->position_p_gain_)); + sync_write_data[3] = + DXL_HIBYTE(DXL_HIWORD(dxl_state->position_p_gain_)); + + if (port_to_sync_write_position_p_gain_[dxl->port_name_] != + NULL) + port_to_sync_write_position_p_gain_[dxl->port_name_] + ->addParam(dxl->id_, sync_write_data); + } + + // if position i gain value is changed -> sync write + if (result_state->position_i_gain_ != NONE_GAIN && + dxl_state->position_i_gain_ != + result_state->position_i_gain_) { + dxl_state->position_i_gain_ = result_state->position_i_gain_; + uint8_t sync_write_data[4] = {0}; + sync_write_data[0] = + DXL_LOBYTE(DXL_LOWORD(dxl_state->position_i_gain_)); + sync_write_data[1] = + DXL_HIBYTE(DXL_LOWORD(dxl_state->position_i_gain_)); + sync_write_data[2] = + DXL_LOBYTE(DXL_HIWORD(dxl_state->position_i_gain_)); + sync_write_data[3] = + DXL_HIBYTE(DXL_HIWORD(dxl_state->position_i_gain_)); + + if (port_to_sync_write_position_i_gain_[dxl->port_name_] != + NULL) + port_to_sync_write_position_i_gain_[dxl->port_name_] + ->addParam(dxl->id_, sync_write_data); + } + + // if position d gain value is changed -> sync write + if (result_state->position_d_gain_ != NONE_GAIN && + dxl_state->position_d_gain_ != + result_state->position_d_gain_) { + dxl_state->position_d_gain_ = result_state->position_d_gain_; + uint8_t sync_write_data[4] = {0}; + sync_write_data[0] = + DXL_LOBYTE(DXL_LOWORD(dxl_state->position_d_gain_)); + sync_write_data[1] = + DXL_HIBYTE(DXL_LOWORD(dxl_state->position_d_gain_)); + sync_write_data[2] = + DXL_LOBYTE(DXL_HIWORD(dxl_state->position_d_gain_)); + sync_write_data[3] = + DXL_HIBYTE(DXL_HIWORD(dxl_state->position_d_gain_)); + + if (port_to_sync_write_position_d_gain_[dxl->port_name_] != + NULL) + port_to_sync_write_position_d_gain_[dxl->port_name_] + ->addParam(dxl->id_, sync_write_data); + } + + // if velocity p gain gain value is changed -> sync write + if (result_state->velocity_p_gain_ != NONE_GAIN && + dxl_state->velocity_p_gain_ != + result_state->velocity_p_gain_) { + dxl_state->velocity_p_gain_ = result_state->velocity_p_gain_; + uint8_t sync_write_data[4] = {0}; + sync_write_data[0] = + DXL_LOBYTE(DXL_LOWORD(dxl_state->velocity_p_gain_)); + sync_write_data[1] = + DXL_HIBYTE(DXL_LOWORD(dxl_state->velocity_p_gain_)); + sync_write_data[2] = + DXL_LOBYTE(DXL_HIWORD(dxl_state->velocity_p_gain_)); + sync_write_data[3] = + DXL_HIBYTE(DXL_HIWORD(dxl_state->velocity_p_gain_)); + + if (port_to_sync_write_velocity_p_gain_[dxl->port_name_] != + NULL) + port_to_sync_write_velocity_p_gain_[dxl->port_name_] + ->addParam(dxl->id_, sync_write_data); + } + + // if velocity i gain value is changed -> sync write + if (result_state->velocity_i_gain_ != NONE_GAIN && + dxl_state->velocity_i_gain_ != + result_state->velocity_i_gain_) { + dxl_state->velocity_i_gain_ = result_state->velocity_i_gain_; + uint8_t sync_write_data[4] = {0}; + sync_write_data[0] = + DXL_LOBYTE(DXL_LOWORD(dxl_state->velocity_i_gain_)); + sync_write_data[1] = + DXL_HIBYTE(DXL_LOWORD(dxl_state->velocity_i_gain_)); + sync_write_data[2] = + DXL_LOBYTE(DXL_HIWORD(dxl_state->velocity_i_gain_)); + sync_write_data[3] = + DXL_HIBYTE(DXL_HIWORD(dxl_state->velocity_i_gain_)); + + if (port_to_sync_write_velocity_i_gain_[dxl->port_name_] != + NULL) + port_to_sync_write_velocity_i_gain_[dxl->port_name_] + ->addParam(dxl->id_, sync_write_data); + } + } + } else if ((*module_it)->getControlMode() == VelocityControl) { + dxl_state->goal_velocity_ = result_state->goal_velocity_; + + if (gazebo_mode_ == false) { + uint32_t vel_data = + dxl->convertVelocity2Value(dxl_state->goal_velocity_); + uint8_t sync_write_data[4] = {0}; + sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(vel_data)); + sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(vel_data)); + sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(vel_data)); + sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(vel_data)); + + if (port_to_sync_write_velocity_[dxl->port_name_] != NULL) + port_to_sync_write_velocity_[dxl->port_name_]->changeParam( + dxl->id_, sync_write_data); + + // if velocity p gain gain value is changed -> sync write + if (result_state->velocity_p_gain_ != NONE_GAIN && + dxl_state->velocity_p_gain_ != + result_state->velocity_p_gain_) { + dxl_state->velocity_p_gain_ = result_state->velocity_p_gain_; + uint8_t sync_write_data[4] = {0}; + sync_write_data[0] = + DXL_LOBYTE(DXL_LOWORD(dxl_state->velocity_p_gain_)); + sync_write_data[1] = + DXL_HIBYTE(DXL_LOWORD(dxl_state->velocity_p_gain_)); + sync_write_data[2] = + DXL_LOBYTE(DXL_HIWORD(dxl_state->velocity_p_gain_)); + sync_write_data[3] = + DXL_HIBYTE(DXL_HIWORD(dxl_state->velocity_p_gain_)); + + if (port_to_sync_write_velocity_p_gain_[dxl->port_name_] != + NULL) + port_to_sync_write_velocity_p_gain_[dxl->port_name_] + ->addParam(dxl->id_, sync_write_data); + } + + // if velocity i gain value is changed -> sync write + if (result_state->velocity_i_gain_ != NONE_GAIN && + dxl_state->velocity_i_gain_ != + result_state->velocity_i_gain_) { + dxl_state->velocity_i_gain_ = result_state->velocity_i_gain_; + uint8_t sync_write_data[4] = {0}; + sync_write_data[0] = + DXL_LOBYTE(DXL_LOWORD(dxl_state->velocity_i_gain_)); + sync_write_data[1] = + DXL_HIBYTE(DXL_LOWORD(dxl_state->velocity_i_gain_)); + sync_write_data[2] = + DXL_LOBYTE(DXL_HIWORD(dxl_state->velocity_i_gain_)); + sync_write_data[3] = + DXL_HIBYTE(DXL_HIWORD(dxl_state->velocity_i_gain_)); + + if (port_to_sync_write_velocity_i_gain_[dxl->port_name_] != + NULL) + port_to_sync_write_velocity_i_gain_[dxl->port_name_] + ->addParam(dxl->id_, sync_write_data); + } + + // if velocity d gain value is changed -> sync write + if (result_state->velocity_d_gain_ != NONE_GAIN && + dxl_state->velocity_d_gain_ != + result_state->velocity_d_gain_) { + dxl_state->velocity_d_gain_ = result_state->velocity_d_gain_; + uint8_t sync_write_data[4] = {0}; + sync_write_data[0] = + DXL_LOBYTE(DXL_LOWORD(dxl_state->velocity_d_gain_)); + sync_write_data[1] = + DXL_HIBYTE(DXL_LOWORD(dxl_state->velocity_d_gain_)); + sync_write_data[2] = + DXL_LOBYTE(DXL_HIWORD(dxl_state->velocity_d_gain_)); + sync_write_data[3] = + DXL_HIBYTE(DXL_HIWORD(dxl_state->velocity_d_gain_)); + + if (port_to_sync_write_velocity_d_gain_[dxl->port_name_] != + NULL) + port_to_sync_write_velocity_d_gain_[dxl->port_name_] + ->addParam(dxl->id_, sync_write_data); + } + } + } else if ((*module_it)->getControlMode() == TorqueControl) { + dxl_state->goal_torque_ = result_state->goal_torque_; + + if (gazebo_mode_ == false) { + uint32_t curr_data = + dxl->convertTorque2Value(dxl_state->goal_torque_); + uint8_t sync_write_data[2] = {0}; + sync_write_data[0] = DXL_LOBYTE(curr_data); + sync_write_data[1] = DXL_HIBYTE(curr_data); + + if (port_to_sync_write_current_[dxl->port_name_] != NULL) + port_to_sync_write_current_[dxl->port_name_]->changeParam( + dxl->id_, sync_write_data); + } + } + } + } + } + + queue_mutex_.unlock(); + } + + if (DEBUG_PRINT) { + time_duration = ros::Time::now() - start_time; + fprintf(stderr, "(%2.6f) MotionModule Process() & save result \n", + time_duration.nsec * 0.000001); + } + } + + // publish present & goal position + for (auto &dxl_it : robot_->dxls_) { + std::string joint_name = dxl_it.first; + Dynamixel *dxl = dxl_it.second; + + present_state.name.push_back(joint_name); + present_state.position.push_back(dxl->dxl_state_->present_position_); + present_state.velocity.push_back(dxl->dxl_state_->present_velocity_); + present_state.effort.push_back(dxl->dxl_state_->present_torque_); + + goal_state.name.push_back(joint_name); + goal_state.position.push_back(dxl->dxl_state_->goal_position_); + goal_state.velocity.push_back(dxl->dxl_state_->goal_velocity_); + goal_state.effort.push_back(dxl->dxl_state_->goal_torque_); + } + + // -> publish present joint_states & goal joint states topic + present_joint_state_pub_.publish(present_state); + goal_joint_state_pub_.publish(goal_state); + + if (DEBUG_PRINT) { + time_duration = ros::Time::now() - start_time; + fprintf(stderr, "(%2.6f) Process() DONE \n", time_duration.nsec * 0.000001); + } + + is_process_running = false; +} + +void RobotisController::addMotionModule(MotionModule *module) { + // check whether the module name already exists + for (auto m_it = motion_modules_.begin(); m_it != motion_modules_.end(); + m_it++) { + if ((*m_it)->getModuleName() == module->getModuleName()) { + ROS_ERROR("Motion Module Name [%s] already exist !!", + module->getModuleName().c_str()); + return; + } + } + + module->initialize(robot_->getControlCycle(), robot_); + motion_modules_.push_back(module); + motion_modules_.unique(); +} + +void RobotisController::removeMotionModule(MotionModule *module) { + motion_modules_.remove(module); +} + +void RobotisController::addSensorModule(SensorModule *module) { + // check whether the module name already exists + for (auto m_it = sensor_modules_.begin(); m_it != sensor_modules_.end(); + m_it++) { + if ((*m_it)->getModuleName() == module->getModuleName()) { + ROS_ERROR("Sensor Module Name [%s] already exist !!", + module->getModuleName().c_str()); + return; + } + } + + module->initialize(robot_->getControlCycle(), robot_); + sensor_modules_.push_back(module); + sensor_modules_.unique(); +} + +void RobotisController::removeSensorModule(SensorModule *module) { + sensor_modules_.remove(module); +} + +void RobotisController::writeControlTableCallback( + const humanoid_robot_intelligence_control_system_controller_msgs:: + WriteControlTable::ConstPtr &msg) { + Device *device = NULL; + + if (DEBUG_PRINT) + fprintf(stderr, "[WriteControlTable] led control msg received\n"); + + auto dev_it1 = robot_->dxls_.find(msg->joint_name); + if (dev_it1 != robot_->dxls_.end()) { + device = dev_it1->second; + } else { + auto dev_it2 = robot_->sensors_.find(msg->joint_name); + if (dev_it2 != robot_->sensors_.end()) { + device = dev_it2->second; + } else { + ROS_WARN("[WriteControlTable] Unknown device : %s", + msg->joint_name.c_str()); + return; + } + } + + ControlTableItem *item = NULL; + auto item_it = device->ctrl_table_.find(msg->start_item_name); + if (item_it != device->ctrl_table_.end()) { + item = item_it->second; + } else { + ROS_WARN("[WriteControlTable] Unknown item : %s", + msg->start_item_name.c_str()); + return; + } + + dynamixel::PortHandler *port = robot_->ports_[device->port_name_]; + dynamixel::PacketHandler *packet_handler = + dynamixel::PacketHandler::getPacketHandler(device->protocol_version_); + + if (item->access_type_ == Read) + return; + + queue_mutex_.lock(); + + direct_sync_write_.push_back(new dynamixel::GroupSyncWrite( + port, packet_handler, item->address_, msg->data_length)); + direct_sync_write_[direct_sync_write_.size() - 1]->addParam( + device->id_, (uint8_t *)(msg->data.data())); + + // fprintf(stderr, "[WriteControlTable] %s -> %s : ", + // msg->joint_name.c_str(), msg->start_item_name.c_str()); for (auto &dt : + // msg->data) + // fprintf(stderr, "%02X ", dt); + // fprintf(stderr, "\n"); + + queue_mutex_.unlock(); +} + +void RobotisController::syncWriteItemCallback( + const humanoid_robot_intelligence_control_system_controller_msgs:: + SyncWriteItem::ConstPtr &msg) { + for (int i = 0; i < msg->joint_name.size(); i++) { + Device *device; + + auto d_it1 = robot_->dxls_.find(msg->joint_name[i]); + if (d_it1 != robot_->dxls_.end()) { + device = d_it1->second; + } else { + auto d_it2 = robot_->sensors_.find(msg->joint_name[i]); + if (d_it2 != robot_->sensors_.end()) { + device = d_it2->second; + } else { + ROS_WARN("[SyncWriteItem] Unknown device : %s", + msg->joint_name[i].c_str()); + continue; + } + } + + // ControlTableItem *item = device->ctrl_table_[msg->item_name]; + ControlTableItem *item = NULL; + auto item_it = device->ctrl_table_.find(msg->item_name); + if (item_it != device->ctrl_table_.end()) { + item = item_it->second; + } else { + ROS_WARN("SyncWriteItem] Unknown item : %s", msg->item_name.c_str()); + continue; + } + + dynamixel::PortHandler *port = robot_->ports_[device->port_name_]; + dynamixel::PacketHandler *packet_handler = + dynamixel::PacketHandler::getPacketHandler(device->protocol_version_); + + if (item->access_type_ == Read) + continue; + + queue_mutex_.lock(); + + int idx = 0; + if (direct_sync_write_.size() == 0) { + direct_sync_write_.push_back(new dynamixel::GroupSyncWrite( + port, packet_handler, item->address_, item->data_length_)); + idx = 0; + } else { + for (idx = 0; idx < direct_sync_write_.size(); idx++) { + if (direct_sync_write_[idx]->getPortHandler() == port && + direct_sync_write_[idx]->getPacketHandler() == packet_handler) + break; + } + + if (idx == direct_sync_write_.size()) + direct_sync_write_.push_back(new dynamixel::GroupSyncWrite( + port, packet_handler, item->address_, item->data_length_)); + } + + uint8_t *data = new uint8_t[item->data_length_]; + if (item->data_length_ == 1) + data[0] = (uint8_t)msg->value[i]; + else if (item->data_length_ == 2) { + data[0] = DXL_LOBYTE((uint16_t)msg->value[i]); + data[1] = DXL_HIBYTE((uint16_t)msg->value[i]); + } else if (item->data_length_ == 4) { + data[0] = DXL_LOBYTE(DXL_LOWORD((uint32_t)msg->value[i])); + data[1] = DXL_HIBYTE(DXL_LOWORD((uint32_t)msg->value[i])); + data[2] = DXL_LOBYTE(DXL_HIWORD((uint32_t)msg->value[i])); + data[3] = DXL_HIBYTE(DXL_HIWORD((uint32_t)msg->value[i])); + } + direct_sync_write_[idx]->addParam(device->id_, data); + delete[] data; + + queue_mutex_.unlock(); + } +} + +void RobotisController::setControllerModeCallback( + const std_msgs::String::ConstPtr &msg) { + if (msg->data == "DirectControlMode") { + for (auto &it : port_to_bulk_read_) { + robot_->ports_[it.first]->setPacketTimeout(0.0); + it.second->rxPacket(); + } + controller_mode_ = DirectControlMode; + } else if (msg->data == "MotionModuleMode") { + for (auto &it : port_to_bulk_read_) { + it.second->txPacket(); + } + controller_mode_ = MotionModuleMode; + } +} + +void RobotisController::setJointStatesCallback( + const sensor_msgs::JointState::ConstPtr &msg) { + queue_mutex_.lock(); + + for (int i = 0; i < msg->name.size(); i++) { + Dynamixel *dxl = robot_->dxls_[msg->name[i]]; + if (dxl == NULL) + continue; + + if ((controller_mode_ == DirectControlMode) || + (controller_mode_ == MotionModuleMode && + dxl->ctrl_module_name_ == "none")) { + dxl->dxl_state_->goal_position_ = (double)msg->position[i]; + + if (gazebo_mode_ == false) { + // add offset + uint32_t pos_data; + pos_data = dxl->convertRadian2Value(dxl->dxl_state_->goal_position_ + + dxl->dxl_state_->position_offset_ * + offset_ratio_); + + uint8_t sync_write_data[4] = {0}; + sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(pos_data)); + sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(pos_data)); + sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(pos_data)); + sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(pos_data)); + + if (port_to_sync_write_position_[dxl->port_name_] != NULL) + port_to_sync_write_position_[dxl->port_name_]->changeParam( + dxl->id_, sync_write_data); + } + } + } + + queue_mutex_.unlock(); +} + +void RobotisController::setCtrlModuleCallback( + const std_msgs::String::ConstPtr &msg) { + if (set_module_thread_.joinable()) + set_module_thread_.join(); + + std::string _module_name_to_set = msg->data; + + set_module_thread_ = boost::thread(boost::bind( + &RobotisController::setCtrlModuleThread, this, _module_name_to_set)); +} + +void RobotisController::setCtrlModule(std::string module_name) { + if (set_module_thread_.joinable()) + set_module_thread_.join(); + + set_module_thread_ = boost::thread( + boost::bind(&RobotisController::setCtrlModuleThread, this, module_name)); +} +void RobotisController::setJointCtrlModuleCallback( + const humanoid_robot_intelligence_control_system_controller_msgs:: + JointCtrlModule::ConstPtr &msg) { + if (msg->joint_name.size() != msg->module_name.size()) + return; + + if (set_module_thread_.joinable()) + set_module_thread_.join(); + + set_module_thread_ = boost::thread( + boost::bind(&RobotisController::setJointCtrlModuleThread, this, msg)); +} + +void RobotisController::enableOffsetCallback( + const std_msgs::Bool::ConstPtr &msg) { + is_offset_enabled_ = (bool)msg->data; + if (is_offset_enabled_) + offset_ratio_ = 0.0; + else + offset_ratio_ = 1.0; +} + +bool RobotisController::getJointCtrlModuleService( + humanoid_robot_intelligence_control_system_controller_msgs::GetJointModule:: + Request &req, + humanoid_robot_intelligence_control_system_controller_msgs::GetJointModule:: + Response &res) { + for (unsigned int idx = 0; idx < req.joint_name.size(); idx++) { + auto d_it = robot_->dxls_.find((std::string)(req.joint_name[idx])); + if (d_it != robot_->dxls_.end()) { + res.joint_name.push_back(req.joint_name[idx]); + res.module_name.push_back(d_it->second->ctrl_module_name_); + } + } + + if (res.joint_name.size() == 0) + return false; + + return true; +} + +bool RobotisController::setJointCtrlModuleService( + humanoid_robot_intelligence_control_system_controller_msgs::SetJointModule:: + Request &req, + humanoid_robot_intelligence_control_system_controller_msgs::SetJointModule:: + Response &res) { + if (set_module_thread_.joinable()) + set_module_thread_.join(); + + humanoid_robot_intelligence_control_system_controller_msgs::JointCtrlModule + modules; + modules.joint_name = req.joint_name; + modules.module_name = req.module_name; + + humanoid_robot_intelligence_control_system_controller_msgs::JointCtrlModule:: + ConstPtr msg_ptr( + new humanoid_robot_intelligence_control_system_controller_msgs:: + JointCtrlModule(modules)); + + if (modules.joint_name.size() != modules.module_name.size()) + return false; + + set_module_thread_ = boost::thread( + boost::bind(&RobotisController::setJointCtrlModuleThread, this, msg_ptr)); + + set_module_thread_.join(); + + return true; +} + +bool RobotisController::setCtrlModuleService( + humanoid_robot_intelligence_control_system_controller_msgs::SetModule:: + Request &req, + humanoid_robot_intelligence_control_system_controller_msgs::SetModule:: + Response &res) { + if (set_module_thread_.joinable()) + set_module_thread_.join(); + + std::string _module_name_to_set = req.module_name; + + set_module_thread_ = boost::thread(boost::bind( + &RobotisController::setCtrlModuleThread, this, _module_name_to_set)); + + set_module_thread_.join(); + + res.result = true; + return true; +} + +bool RobotisController::loadOffsetService( + humanoid_robot_intelligence_control_system_controller_msgs::LoadOffset:: + Request &req, + humanoid_robot_intelligence_control_system_controller_msgs::LoadOffset:: + Response &res) { + loadOffset((std::string)req.file_path); + res.result = true; + return true; +} + +void RobotisController::setJointCtrlModuleThread( + const humanoid_robot_intelligence_control_system_controller_msgs:: + JointCtrlModule::ConstPtr &msg) { + // stop module list + std::list _stop_modules; + std::list _enable_modules; + + for (unsigned int idx = 0; idx < msg->joint_name.size(); idx++) { + Dynamixel *_dxl = NULL; + std::map::iterator _dxl_it = + robot_->dxls_.find((std::string)(msg->joint_name[idx])); + if (_dxl_it != robot_->dxls_.end()) + _dxl = _dxl_it->second; + else + continue; + + // enqueue + if (_dxl->ctrl_module_name_ != msg->module_name[idx]) { + for (std::list::iterator _stop_m_it = + motion_modules_.begin(); + _stop_m_it != motion_modules_.end(); _stop_m_it++) { + if ((*_stop_m_it)->getModuleName() == _dxl->ctrl_module_name_ && + (*_stop_m_it)->getModuleEnable() == true) + _stop_modules.push_back(*_stop_m_it); + } + } + } + + // stop the module + _stop_modules.unique(); + for (std::list::iterator _stop_m_it = _stop_modules.begin(); + _stop_m_it != _stop_modules.end(); _stop_m_it++) { + (*_stop_m_it)->stop(); + } + + // wait to stop + for (std::list::iterator _stop_m_it = _stop_modules.begin(); + _stop_m_it != _stop_modules.end(); _stop_m_it++) { + while ((*_stop_m_it)->isRunning()) + usleep(robot_->getControlCycle() * 1000); + } + + // disable module(s) + for (std::list::iterator _stop_m_it = _stop_modules.begin(); + _stop_m_it != _stop_modules.end(); _stop_m_it++) { + (*_stop_m_it)->setModuleEnable(false); + } + + // set ctrl module + queue_mutex_.lock(); + + for (unsigned int idx = 0; idx < msg->joint_name.size(); idx++) { + std::string ctrl_module = msg->module_name[idx]; + std::string joint_name = msg->joint_name[idx]; + + Dynamixel *_dxl = NULL; + std::map::iterator _dxl_it = + robot_->dxls_.find(joint_name); + if (_dxl_it != robot_->dxls_.end()) + _dxl = _dxl_it->second; + else + continue; + + // none + if (ctrl_module == "" || ctrl_module == "none") { + _dxl->ctrl_module_name_ = "none"; + + if (gazebo_mode_ == true) + continue; + + uint32_t _pos_data; + _pos_data = _dxl->convertRadian2Value(_dxl->dxl_state_->goal_position_ + + _dxl->dxl_state_->position_offset_ * + offset_ratio_); + + uint8_t _sync_write_data[4]; + _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_pos_data)); + _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_pos_data)); + _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_pos_data)); + _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_pos_data)); + + if (port_to_sync_write_position_[_dxl->port_name_] != NULL) + port_to_sync_write_position_[_dxl->port_name_]->addParam( + _dxl->id_, _sync_write_data); + + if (port_to_sync_write_current_[_dxl->port_name_] != NULL) + port_to_sync_write_current_[_dxl->port_name_]->removeParam(_dxl->id_); + if (port_to_sync_write_velocity_[_dxl->port_name_] != NULL) + port_to_sync_write_velocity_[_dxl->port_name_]->removeParam(_dxl->id_); + } else { + // check whether the module exist + for (std::list::iterator _m_it = motion_modules_.begin(); + _m_it != motion_modules_.end(); _m_it++) { + // if it exist + if ((*_m_it)->getModuleName() == ctrl_module) { + std::map::iterator _result_it = + (*_m_it)->result_.find(joint_name); + if (_result_it == (*_m_it)->result_.end()) + break; + + _dxl->ctrl_module_name_ = ctrl_module; + + // enqueue enable module list + _enable_modules.push_back(*_m_it); + ControlMode _mode = (*_m_it)->getControlMode(); + + if (gazebo_mode_ == true) + break; + + if (_mode == PositionControl) { + uint32_t _pos_data; + _pos_data = _dxl->convertRadian2Value( + _dxl->dxl_state_->goal_position_ + + _dxl->dxl_state_->position_offset_ * offset_ratio_); + + uint8_t _sync_write_data[4]; + _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_pos_data)); + _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_pos_data)); + _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_pos_data)); + _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_pos_data)); + + if (port_to_sync_write_position_[_dxl->port_name_] != NULL) + port_to_sync_write_position_[_dxl->port_name_]->addParam( + _dxl->id_, _sync_write_data); + + if (port_to_sync_write_current_[_dxl->port_name_] != NULL) + port_to_sync_write_current_[_dxl->port_name_]->removeParam( + _dxl->id_); + if (port_to_sync_write_velocity_[_dxl->port_name_] != NULL) + port_to_sync_write_velocity_[_dxl->port_name_]->removeParam( + _dxl->id_); + } else if (_mode == VelocityControl) { + uint32_t _vel_data = + _dxl->convertVelocity2Value(_dxl->dxl_state_->goal_velocity_); + uint8_t _sync_write_data[4]; + _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_vel_data)); + _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_vel_data)); + _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_vel_data)); + _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_vel_data)); + + if (port_to_sync_write_velocity_[_dxl->port_name_] != NULL) + port_to_sync_write_velocity_[_dxl->port_name_]->addParam( + _dxl->id_, _sync_write_data); + + if (port_to_sync_write_current_[_dxl->port_name_] != NULL) + port_to_sync_write_current_[_dxl->port_name_]->removeParam( + _dxl->id_); + if (port_to_sync_write_position_[_dxl->port_name_] != NULL) + port_to_sync_write_position_[_dxl->port_name_]->removeParam( + _dxl->id_); + } else if (_mode == TorqueControl) { + uint32_t _curr_data = + _dxl->convertTorque2Value(_dxl->dxl_state_->goal_torque_); + uint8_t _sync_write_data[4]; + _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_curr_data)); + _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_curr_data)); + _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_curr_data)); + _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_curr_data)); + + if (port_to_sync_write_current_[_dxl->port_name_] != NULL) + port_to_sync_write_current_[_dxl->port_name_]->addParam( + _dxl->id_, _sync_write_data); + + if (port_to_sync_write_velocity_[_dxl->port_name_] != NULL) + port_to_sync_write_velocity_[_dxl->port_name_]->removeParam( + _dxl->id_); + if (port_to_sync_write_position_[_dxl->port_name_] != NULL) + port_to_sync_write_position_[_dxl->port_name_]->removeParam( + _dxl->id_); + } + break; + } + } + } + } + + // enable module(s) + _enable_modules.unique(); + for (std::list::iterator _m_it = _enable_modules.begin(); + _m_it != _enable_modules.end(); _m_it++) { + (*_m_it)->setModuleEnable(true); + } + + // TODO: set indirect address + // -> check module's control_mode + + queue_mutex_.unlock(); + + // publish current module + humanoid_robot_intelligence_control_system_controller_msgs::JointCtrlModule + _current_module_msg; + for (std::map::iterator _dxl_iter = + robot_->dxls_.begin(); + _dxl_iter != robot_->dxls_.end(); ++_dxl_iter) { + _current_module_msg.joint_name.push_back(_dxl_iter->first); + _current_module_msg.module_name.push_back( + _dxl_iter->second->ctrl_module_name_); + } + + if (_current_module_msg.joint_name.size() == + _current_module_msg.module_name.size()) + current_module_pub_.publish(_current_module_msg); +} + +void RobotisController::setCtrlModuleThread(std::string ctrl_module) { + // stop module + std::list stop_modules; + + if (ctrl_module == "" || ctrl_module == "none") { + // enqueue all modules in order to stop + for (auto m_it = motion_modules_.begin(); m_it != motion_modules_.end(); + m_it++) { + if ((*m_it)->getModuleEnable() == true) + stop_modules.push_back(*m_it); + } + } else { + for (auto m_it = motion_modules_.begin(); m_it != motion_modules_.end(); + m_it++) { + // if it exist + if ((*m_it)->getModuleName() == ctrl_module) { + // enqueue the module which lost control of joint in order to stop + for (auto &result_it : (*m_it)->result_) { + auto d_it = robot_->dxls_.find(result_it.first); + + if (d_it != robot_->dxls_.end()) { + // enqueue + if (d_it->second->ctrl_module_name_ != ctrl_module) { + for (auto stop_m_it = motion_modules_.begin(); + stop_m_it != motion_modules_.end(); stop_m_it++) { + if (((*stop_m_it)->getModuleName() == + d_it->second->ctrl_module_name_) && + ((*stop_m_it)->getModuleEnable() == true)) { + stop_modules.push_back(*stop_m_it); + } + } + } + } + } + + break; + } + } + } + + // stop the module + stop_modules.unique(); + for (auto stop_m_it = stop_modules.begin(); stop_m_it != stop_modules.end(); + stop_m_it++) { + (*stop_m_it)->stop(); + } + + // wait to stop + for (auto stop_m_it = stop_modules.begin(); stop_m_it != stop_modules.end(); + stop_m_it++) { + while ((*stop_m_it)->isRunning()) + usleep(robot_->getControlCycle() * 1000); + } + + // disable module(s) + for (std::list::iterator _stop_m_it = stop_modules.begin(); + _stop_m_it != stop_modules.end(); _stop_m_it++) { + (*_stop_m_it)->setModuleEnable(false); + } + + // set ctrl module + queue_mutex_.lock(); + + if (DEBUG_PRINT) + ROS_INFO_STREAM("set module : " << ctrl_module); + + // none + if ((ctrl_module == "") || (ctrl_module == "none")) { + // set dxl's control module to "none" + for (auto &d_it : robot_->dxls_) { + Dynamixel *dxl = d_it.second; + dxl->ctrl_module_name_ = "none"; + + if (gazebo_mode_ == true) + continue; + + uint32_t pos_data; + pos_data = dxl->convertRadian2Value(dxl->dxl_state_->goal_position_ + + dxl->dxl_state_->position_offset_ * + offset_ratio_); + + uint8_t sync_write_data[4] = {0}; + sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(pos_data)); + sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(pos_data)); + sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(pos_data)); + sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(pos_data)); + + if (port_to_sync_write_position_[dxl->port_name_] != NULL) + port_to_sync_write_position_[dxl->port_name_]->addParam( + dxl->id_, sync_write_data); + + if (port_to_sync_write_current_[dxl->port_name_] != NULL) + port_to_sync_write_current_[dxl->port_name_]->removeParam(dxl->id_); + if (port_to_sync_write_velocity_[dxl->port_name_] != NULL) + port_to_sync_write_velocity_[dxl->port_name_]->removeParam(dxl->id_); + } + } else { + // check whether the module exist + for (auto m_it = motion_modules_.begin(); m_it != motion_modules_.end(); + m_it++) { + // if it exist + if ((*m_it)->getModuleName() == ctrl_module) { + ControlMode mode = (*m_it)->getControlMode(); + for (auto &result_it : (*m_it)->result_) { + auto d_it = robot_->dxls_.find(result_it.first); + if (d_it != robot_->dxls_.end()) { + Dynamixel *dxl = d_it->second; + dxl->ctrl_module_name_ = ctrl_module; + + if (gazebo_mode_ == true) + continue; + + if (mode == PositionControl) { + uint32_t pos_data; + pos_data = dxl->convertRadian2Value( + dxl->dxl_state_->goal_position_ + + dxl->dxl_state_->position_offset_ * offset_ratio_); + + uint8_t sync_write_data[4] = {0}; + sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(pos_data)); + sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(pos_data)); + sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(pos_data)); + sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(pos_data)); + + if (port_to_sync_write_position_[dxl->port_name_] != NULL) + port_to_sync_write_position_[dxl->port_name_]->addParam( + dxl->id_, sync_write_data); + + if (port_to_sync_write_current_[dxl->port_name_] != NULL) + port_to_sync_write_current_[dxl->port_name_]->removeParam( + dxl->id_); + if (port_to_sync_write_velocity_[dxl->port_name_] != NULL) + port_to_sync_write_velocity_[dxl->port_name_]->removeParam( + dxl->id_); + } else if (mode == VelocityControl) { + uint32_t vel_data = + dxl->convertVelocity2Value(dxl->dxl_state_->goal_velocity_); + uint8_t sync_write_data[4] = {0}; + sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(vel_data)); + sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(vel_data)); + sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(vel_data)); + sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(vel_data)); + + if (port_to_sync_write_velocity_[dxl->port_name_] != NULL) + port_to_sync_write_velocity_[dxl->port_name_]->addParam( + dxl->id_, sync_write_data); + + if (port_to_sync_write_current_[dxl->port_name_] != NULL) + port_to_sync_write_current_[dxl->port_name_]->removeParam( + dxl->id_); + if (port_to_sync_write_position_[dxl->port_name_] != NULL) + port_to_sync_write_position_[dxl->port_name_]->removeParam( + dxl->id_); + } else if (mode == TorqueControl) { + uint32_t curr_data = + dxl->convertTorque2Value(dxl->dxl_state_->goal_torque_); + uint8_t sync_write_data[4] = {0}; + sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(curr_data)); + sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(curr_data)); + sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(curr_data)); + sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(curr_data)); + + if (port_to_sync_write_current_[dxl->port_name_] != NULL) + port_to_sync_write_current_[dxl->port_name_]->addParam( + dxl->id_, sync_write_data); + + if (port_to_sync_write_velocity_[dxl->port_name_] != NULL) + port_to_sync_write_velocity_[dxl->port_name_]->removeParam( + dxl->id_); + if (port_to_sync_write_position_[dxl->port_name_] != NULL) + port_to_sync_write_position_[dxl->port_name_]->removeParam( + dxl->id_); + } + } + } + + break; + } + } + } + + for (auto m_it = motion_modules_.begin(); m_it != motion_modules_.end(); + m_it++) { + // set all used modules -> enable + for (auto &d_it : robot_->dxls_) { + if (d_it.second->ctrl_module_name_ == (*m_it)->getModuleName()) { + (*m_it)->setModuleEnable(true); + break; + } + } + } + + // TODO: set indirect address + // -> check module's control_mode + + queue_mutex_.unlock(); + + // publish current module + humanoid_robot_intelligence_control_system_controller_msgs::JointCtrlModule + current_module_msg; + for (auto &dxl_iter : robot_->dxls_) { + current_module_msg.joint_name.push_back(dxl_iter.first); + current_module_msg.module_name.push_back( + dxl_iter.second->ctrl_module_name_); + } + + if (current_module_msg.joint_name.size() == + current_module_msg.module_name.size()) + current_module_pub_.publish(current_module_msg); +} + +void RobotisController::gazeboJointStatesCallback( + const sensor_msgs::JointState::ConstPtr &msg) { + queue_mutex_.lock(); + + for (unsigned int i = 0; i < msg->name.size(); i++) { + auto d_it = robot_->dxls_.find((std::string)msg->name[i]); + if (d_it != robot_->dxls_.end()) { + d_it->second->dxl_state_->present_position_ = msg->position[i]; + d_it->second->dxl_state_->present_velocity_ = msg->velocity[i]; + d_it->second->dxl_state_->present_torque_ = msg->effort[i]; + } + } + + if (init_pose_loaded_ == false) { + for (auto &it : robot_->dxls_) + it.second->dxl_state_->goal_position_ = + it.second->dxl_state_->present_position_; + init_pose_loaded_ = true; + } + + queue_mutex_.unlock(); +} + +bool RobotisController::isTimerStopped() { + if (this->is_timer_running_) { + if (DEBUG_PRINT == true) + ROS_WARN("Process Timer is running.. STOP the timer first."); + return false; + } + return true; +} + +int RobotisController::ping(const std::string joint_name, uint8_t *error) { + return ping(joint_name, 0, error); +} +int RobotisController::ping(const std::string joint_name, + uint16_t *model_number, uint8_t *error) { + if (isTimerStopped() == false) + return COMM_PORT_BUSY; + + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; + + dynamixel::PacketHandler *pkt_handler = + dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; + + return pkt_handler->ping(port_handler, dxl->id_, model_number, error); +} + +int RobotisController::action(const std::string joint_name) { + if (isTimerStopped() == false) + return COMM_PORT_BUSY; + + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; + + dynamixel::PacketHandler *pkt_handler = + dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; + + return pkt_handler->action(port_handler, dxl->id_); +} +int RobotisController::reboot(const std::string joint_name, uint8_t *error) { + if (isTimerStopped() == false) + return COMM_PORT_BUSY; + + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; + + dynamixel::PacketHandler *pkt_handler = + dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; + + return pkt_handler->reboot(port_handler, dxl->id_, error); +} +int RobotisController::factoryReset(const std::string joint_name, + uint8_t option, uint8_t *error) { + if (isTimerStopped() == false) + return COMM_PORT_BUSY; + + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; + + dynamixel::PacketHandler *pkt_handler = + dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; + + return pkt_handler->factoryReset(port_handler, dxl->id_, option, error); +} + +int RobotisController::read(const std::string joint_name, uint16_t address, + uint16_t length, uint8_t *data, uint8_t *error) { + if (isTimerStopped() == false) + return COMM_PORT_BUSY; + + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; + + dynamixel::PacketHandler *pkt_handler = + dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; + + return pkt_handler->readTxRx(port_handler, dxl->id_, address, length, data, + error); +} + +int RobotisController::readCtrlItem(const std::string joint_name, + const std::string item_name, uint32_t *data, + uint8_t *error) { + if (isTimerStopped() == false) + return COMM_PORT_BUSY; + + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; + + ControlTableItem *item = dxl->ctrl_table_[item_name]; + if (item == NULL) + return COMM_NOT_AVAILABLE; + + dynamixel::PacketHandler *pkt_handler = + dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; + + int result = COMM_NOT_AVAILABLE; + switch (item->data_length_) { + case 1: { + uint8_t read_data = 0; + result = pkt_handler->read1ByteTxRx(port_handler, dxl->id_, item->address_, + &read_data, error); + if (result == COMM_SUCCESS) + *data = read_data; + break; + } + case 2: { + uint16_t read_data = 0; + result = pkt_handler->read2ByteTxRx(port_handler, dxl->id_, item->address_, + &read_data, error); + if (result == COMM_SUCCESS) + *data = read_data; + break; + } + case 4: { + uint32_t read_data = 0; + result = pkt_handler->read4ByteTxRx(port_handler, dxl->id_, item->address_, + &read_data, error); + if (result == COMM_SUCCESS) + *data = read_data; + break; + } + default: + break; + } + return result; +} + +int RobotisController::read1Byte(const std::string joint_name, uint16_t address, + uint8_t *data, uint8_t *error) { + if (isTimerStopped() == false) + return COMM_PORT_BUSY; + + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; + + dynamixel::PacketHandler *pkt_handler = + dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; + + return pkt_handler->read1ByteTxRx(port_handler, dxl->id_, address, data, + error); +} + +int RobotisController::read2Byte(const std::string joint_name, uint16_t address, + uint16_t *data, uint8_t *error) { + if (isTimerStopped() == false) + return COMM_PORT_BUSY; + + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; + + dynamixel::PacketHandler *pkt_handler = + dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; + + return pkt_handler->read2ByteTxRx(port_handler, dxl->id_, address, data, + error); +} + +int RobotisController::read4Byte(const std::string joint_name, uint16_t address, + uint32_t *data, uint8_t *error) { + if (isTimerStopped() == false) + return COMM_PORT_BUSY; + + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; + + dynamixel::PacketHandler *pkt_handler = + dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; + + return pkt_handler->read4ByteTxRx(port_handler, dxl->id_, address, data, + error); +} + +int RobotisController::write(const std::string joint_name, uint16_t address, + uint16_t length, uint8_t *data, uint8_t *error) { + if (isTimerStopped() == false) + return COMM_PORT_BUSY; + + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; + + dynamixel::PacketHandler *pkt_handler = + dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; + + return pkt_handler->writeTxRx(port_handler, dxl->id_, address, length, data, + error); +} + +int RobotisController::writeCtrlItem(const std::string joint_name, + const std::string item_name, uint32_t data, + uint8_t *error) { + if (isTimerStopped() == false) + return COMM_PORT_BUSY; + + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; + + ControlTableItem *item = dxl->ctrl_table_[item_name]; + if (item == NULL) + return COMM_NOT_AVAILABLE; + + dynamixel::PacketHandler *pkt_handler = + dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; + + int result = COMM_NOT_AVAILABLE; + uint8_t *write_data = new uint8_t[item->data_length_]; + if (item->data_length_ == 1) { + write_data[0] = (uint8_t)data; + result = pkt_handler->write1ByteTxRx(port_handler, dxl->id_, item->address_, + data, error); + } else if (item->data_length_ == 2) { + write_data[0] = DXL_LOBYTE((uint16_t)data); + write_data[1] = DXL_HIBYTE((uint16_t)data); + result = pkt_handler->write2ByteTxRx(port_handler, dxl->id_, item->address_, + data, error); + } else if (item->data_length_ == 4) { + write_data[0] = DXL_LOBYTE(DXL_LOWORD((uint32_t)data)); + write_data[1] = DXL_HIBYTE(DXL_LOWORD((uint32_t)data)); + write_data[2] = DXL_LOBYTE(DXL_HIWORD((uint32_t)data)); + write_data[3] = DXL_HIBYTE(DXL_HIWORD((uint32_t)data)); + result = pkt_handler->write4ByteTxRx(port_handler, dxl->id_, item->address_, + data, error); + } + delete[] write_data; + return result; +} + +int RobotisController::write1Byte(const std::string joint_name, + uint16_t address, uint8_t data, + uint8_t *error) { + if (isTimerStopped() == false) + return COMM_PORT_BUSY; + + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; + + dynamixel::PacketHandler *pkt_handler = + dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; + + return pkt_handler->write1ByteTxRx(port_handler, dxl->id_, address, data, + error); +} + +int RobotisController::write2Byte(const std::string joint_name, + uint16_t address, uint16_t data, + uint8_t *error) { + if (isTimerStopped() == false) + return COMM_PORT_BUSY; + + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; + + dynamixel::PacketHandler *pkt_handler = + dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; + + return pkt_handler->write2ByteTxRx(port_handler, dxl->id_, address, data, + error); +} + +int RobotisController::write4Byte(const std::string joint_name, + uint16_t address, uint32_t data, + uint8_t *error) { + if (isTimerStopped() == false) + return COMM_PORT_BUSY; + + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; + + dynamixel::PacketHandler *pkt_handler = + dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; + + return pkt_handler->write4ByteTxRx(port_handler, dxl->id_, address, data, + error); +} + +int RobotisController::regWrite(const std::string joint_name, uint16_t address, + uint16_t length, uint8_t *data, + uint8_t *error) { + if (isTimerStopped() == false) + return COMM_PORT_BUSY; + + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; + + dynamixel::PacketHandler *pkt_handler = + dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; + + return pkt_handler->regWriteTxRx(port_handler, dxl->id_, address, length, + data, error); +} diff --git a/humanoid_robot_device/CHANGELOG.rst b/humanoid_robot_intelligence_control_system_device/CHANGELOG.rst similarity index 95% rename from humanoid_robot_device/CHANGELOG.rst rename to humanoid_robot_intelligence_control_system_device/CHANGELOG.rst index 35e45da..10a041c 100755 --- a/humanoid_robot_device/CHANGELOG.rst +++ b/humanoid_robot_intelligence_control_system_device/CHANGELOG.rst @@ -1,5 +1,5 @@ ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package humanoid_robot_device +Changelog for package humanoid_robot_intelligence_control_system_device ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 0.3.2 (2023-10-03) @@ -79,7 +79,7 @@ Changelog for package humanoid_robot_device * bug fixed (position pid gain & velocity pid gain sync write). * added velocity_to_value_ratio to DXL Pro-H series. * added velocity p/i/d gain and position i/d gain sync_write code. -* fixed humanoid_robot_device build_depend. +* fixed humanoid_robot_intelligence_control_system_device build_depend. * added XM-430-W210 / XM-430-W350 device file. * rename (present_current\_ -> present_torque\_) * modified torque control code diff --git a/humanoid_robot_device/CMakeLists.txt b/humanoid_robot_intelligence_control_system_device/CMakeLists.txt similarity index 51% rename from humanoid_robot_device/CMakeLists.txt rename to humanoid_robot_intelligence_control_system_device/CMakeLists.txt index 6b135e4..a9c5f39 100755 --- a/humanoid_robot_device/CMakeLists.txt +++ b/humanoid_robot_intelligence_control_system_device/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.8) -project(humanoid_robot_device) +project(humanoid_robot_intelligence_control_system_device) if($ENV{ROS_VERSION} EQUAL 1) @@ -15,7 +15,7 @@ endif() if($ENV{ROS_VERSION} EQUAL 1) catkin_package( INCLUDE_DIRS include - LIBRARIES humanoid_robot_device + LIBRARIES humanoid_robot_intelligence_control_system_device CATKIN_DEPENDS roscpp dynamixel_sdk @@ -28,15 +28,15 @@ include_directories( ${catkin_INCLUDE_DIRS} ) -add_library(humanoid_robot_device - src/humanoid_robot_device/robot.cpp - src/humanoid_robot_device/sensor.cpp - src/humanoid_robot_device/dynamixel.cpp +add_library(humanoid_robot_intelligence_control_system_device + src/humanoid_robot_intelligence_control_system_device/robot.cpp + src/humanoid_robot_intelligence_control_system_device/sensor.cpp + src/humanoid_robot_intelligence_control_system_device/dynamixel.cpp ) -add_dependencies(humanoid_robot_device ${catkin_EXPORTED_TARGETS}) -target_link_libraries(humanoid_robot_device ${catkin_LIBRARIES}) +add_dependencies(humanoid_robot_intelligence_control_system_device ${catkin_EXPORTED_TARGETS}) +target_link_libraries(humanoid_robot_intelligence_control_system_device ${catkin_LIBRARIES}) -install(TARGETS humanoid_robot_device +install(TARGETS humanoid_robot_intelligence_control_system_device ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} diff --git a/humanoid_robot_device/devices/dynamixel/GRIPPER.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/GRIPPER.device similarity index 100% rename from humanoid_robot_device/devices/dynamixel/GRIPPER.device rename to humanoid_robot_intelligence_control_system_device/devices/dynamixel/GRIPPER.device diff --git a/humanoid_robot_device/devices/dynamixel/GRIPPER_TORQUE.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/GRIPPER_TORQUE.device similarity index 100% rename from humanoid_robot_device/devices/dynamixel/GRIPPER_TORQUE.device rename to humanoid_robot_intelligence_control_system_device/devices/dynamixel/GRIPPER_TORQUE.device diff --git a/humanoid_robot_device/devices/dynamixel/H42-20-S300-R(A).device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/H42-20-S300-R(A).device similarity index 100% rename from humanoid_robot_device/devices/dynamixel/H42-20-S300-R(A).device rename to humanoid_robot_intelligence_control_system_device/devices/dynamixel/H42-20-S300-R(A).device diff --git a/humanoid_robot_device/devices/dynamixel/H42-20-S300-R.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/H42-20-S300-R.device similarity index 100% rename from humanoid_robot_device/devices/dynamixel/H42-20-S300-R.device rename to humanoid_robot_intelligence_control_system_device/devices/dynamixel/H42-20-S300-R.device diff --git a/humanoid_robot_device/devices/dynamixel/H42P-020-S300-R.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/H42P-020-S300-R.device similarity index 100% rename from humanoid_robot_device/devices/dynamixel/H42P-020-S300-R.device rename to humanoid_robot_intelligence_control_system_device/devices/dynamixel/H42P-020-S300-R.device diff --git a/humanoid_robot_device/devices/dynamixel/H54-100-B210-R-NR.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/H54-100-B210-R-NR.device similarity index 100% rename from humanoid_robot_device/devices/dynamixel/H54-100-B210-R-NR.device rename to humanoid_robot_intelligence_control_system_device/devices/dynamixel/H54-100-B210-R-NR.device diff --git a/humanoid_robot_device/devices/dynamixel/H54-100-S500-R(A).device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/H54-100-S500-R(A).device similarity index 100% rename from humanoid_robot_device/devices/dynamixel/H54-100-S500-R(A).device rename to humanoid_robot_intelligence_control_system_device/devices/dynamixel/H54-100-S500-R(A).device diff --git a/humanoid_robot_device/devices/dynamixel/H54-100-S500-R.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/H54-100-S500-R.device similarity index 100% rename from humanoid_robot_device/devices/dynamixel/H54-100-S500-R.device rename to humanoid_robot_intelligence_control_system_device/devices/dynamixel/H54-100-S500-R.device diff --git a/humanoid_robot_device/devices/dynamixel/H54-200-B500-R.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/H54-200-B500-R.device similarity index 100% rename from humanoid_robot_device/devices/dynamixel/H54-200-B500-R.device rename to humanoid_robot_intelligence_control_system_device/devices/dynamixel/H54-200-B500-R.device diff --git a/humanoid_robot_device/devices/dynamixel/H54-200-S500-R(A).device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/H54-200-S500-R(A).device similarity index 100% rename from humanoid_robot_device/devices/dynamixel/H54-200-S500-R(A).device rename to humanoid_robot_intelligence_control_system_device/devices/dynamixel/H54-200-S500-R(A).device diff --git a/humanoid_robot_device/devices/dynamixel/H54-200-S500-R.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/H54-200-S500-R.device similarity index 100% rename from humanoid_robot_device/devices/dynamixel/H54-200-S500-R.device rename to humanoid_robot_intelligence_control_system_device/devices/dynamixel/H54-200-S500-R.device diff --git a/humanoid_robot_device/devices/dynamixel/H54P-100-S500-R.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/H54P-100-S500-R.device similarity index 100% rename from humanoid_robot_device/devices/dynamixel/H54P-100-S500-R.device rename to humanoid_robot_intelligence_control_system_device/devices/dynamixel/H54P-100-S500-R.device diff --git a/humanoid_robot_device/devices/dynamixel/H54P-200-B500-R.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/H54P-200-B500-R.device similarity index 100% rename from humanoid_robot_device/devices/dynamixel/H54P-200-B500-R.device rename to humanoid_robot_intelligence_control_system_device/devices/dynamixel/H54P-200-B500-R.device diff --git a/humanoid_robot_device/devices/dynamixel/H54P-200-S500-R.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/H54P-200-S500-R.device similarity index 100% rename from humanoid_robot_device/devices/dynamixel/H54P-200-S500-R.device rename to humanoid_robot_intelligence_control_system_device/devices/dynamixel/H54P-200-S500-R.device diff --git a/humanoid_robot_device/devices/dynamixel/L42-10-S300-R.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/L42-10-S300-R.device similarity index 100% rename from humanoid_robot_device/devices/dynamixel/L42-10-S300-R.device rename to humanoid_robot_intelligence_control_system_device/devices/dynamixel/L42-10-S300-R.device diff --git a/humanoid_robot_device/devices/dynamixel/L54-30-S400-R.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/L54-30-S400-R.device similarity index 100% rename from humanoid_robot_device/devices/dynamixel/L54-30-S400-R.device rename to humanoid_robot_intelligence_control_system_device/devices/dynamixel/L54-30-S400-R.device diff --git a/humanoid_robot_device/devices/dynamixel/L54-30-S500-R.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/L54-30-S500-R.device similarity index 100% rename from humanoid_robot_device/devices/dynamixel/L54-30-S500-R.device rename to humanoid_robot_intelligence_control_system_device/devices/dynamixel/L54-30-S500-R.device diff --git a/humanoid_robot_device/devices/dynamixel/L54-50-S290-R.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/L54-50-S290-R.device similarity index 100% rename from humanoid_robot_device/devices/dynamixel/L54-50-S290-R.device rename to humanoid_robot_intelligence_control_system_device/devices/dynamixel/L54-50-S290-R.device diff --git a/humanoid_robot_device/devices/dynamixel/L54-50-S500-R.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/L54-50-S500-R.device similarity index 100% rename from humanoid_robot_device/devices/dynamixel/L54-50-S500-R.device rename to humanoid_robot_intelligence_control_system_device/devices/dynamixel/L54-50-S500-R.device diff --git a/humanoid_robot_device/devices/dynamixel/M42-10-S260-R.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/M42-10-S260-R.device similarity index 100% rename from humanoid_robot_device/devices/dynamixel/M42-10-S260-R.device rename to humanoid_robot_intelligence_control_system_device/devices/dynamixel/M42-10-S260-R.device diff --git a/humanoid_robot_device/devices/dynamixel/M54-40-S250-R.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/M54-40-S250-R.device similarity index 100% rename from humanoid_robot_device/devices/dynamixel/M54-40-S250-R.device rename to humanoid_robot_intelligence_control_system_device/devices/dynamixel/M54-40-S250-R.device diff --git a/humanoid_robot_device/devices/dynamixel/M54-60-S250-R.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/M54-60-S250-R.device similarity index 100% rename from humanoid_robot_device/devices/dynamixel/M54-60-S250-R.device rename to humanoid_robot_intelligence_control_system_device/devices/dynamixel/M54-60-S250-R.device diff --git a/humanoid_robot_device/devices/dynamixel/MX-106.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/MX-106.device similarity index 100% rename from humanoid_robot_device/devices/dynamixel/MX-106.device rename to humanoid_robot_intelligence_control_system_device/devices/dynamixel/MX-106.device diff --git a/humanoid_robot_device/devices/dynamixel/MX-28.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/MX-28.device similarity index 100% rename from humanoid_robot_device/devices/dynamixel/MX-28.device rename to humanoid_robot_intelligence_control_system_device/devices/dynamixel/MX-28.device diff --git a/humanoid_robot_device/devices/dynamixel/MX-64.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/MX-64.device similarity index 100% rename from humanoid_robot_device/devices/dynamixel/MX-64.device rename to humanoid_robot_intelligence_control_system_device/devices/dynamixel/MX-64.device diff --git a/humanoid_robot_device/devices/dynamixel/PH42-020-S300-R.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/PH42-020-S300-R.device similarity index 100% rename from humanoid_robot_device/devices/dynamixel/PH42-020-S300-R.device rename to humanoid_robot_intelligence_control_system_device/devices/dynamixel/PH42-020-S300-R.device diff --git a/humanoid_robot_device/devices/dynamixel/PH54-100-S500-R.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/PH54-100-S500-R.device similarity index 100% rename from humanoid_robot_device/devices/dynamixel/PH54-100-S500-R.device rename to humanoid_robot_intelligence_control_system_device/devices/dynamixel/PH54-100-S500-R.device diff --git a/humanoid_robot_device/devices/dynamixel/PH54-200-S500-R.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/PH54-200-S500-R.device similarity index 100% rename from humanoid_robot_device/devices/dynamixel/PH54-200-S500-R.device rename to humanoid_robot_intelligence_control_system_device/devices/dynamixel/PH54-200-S500-R.device diff --git a/humanoid_robot_device/devices/dynamixel/RH-P12-RN(A).device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/RH-P12-RN(A).device similarity index 100% rename from humanoid_robot_device/devices/dynamixel/RH-P12-RN(A).device rename to humanoid_robot_intelligence_control_system_device/devices/dynamixel/RH-P12-RN(A).device diff --git a/humanoid_robot_device/devices/dynamixel/RH-P12-RN.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/RH-P12-RN.device similarity index 100% rename from humanoid_robot_device/devices/dynamixel/RH-P12-RN.device rename to humanoid_robot_intelligence_control_system_device/devices/dynamixel/RH-P12-RN.device diff --git a/humanoid_robot_device/devices/dynamixel/XM-430.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/XM-430.device similarity index 100% rename from humanoid_robot_device/devices/dynamixel/XM-430.device rename to humanoid_robot_intelligence_control_system_device/devices/dynamixel/XM-430.device diff --git a/humanoid_robot_device/devices/dynamixel/XM430-W210.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/XM430-W210.device similarity index 100% rename from humanoid_robot_device/devices/dynamixel/XM430-W210.device rename to humanoid_robot_intelligence_control_system_device/devices/dynamixel/XM430-W210.device diff --git a/humanoid_robot_device/devices/dynamixel/XM430-W350.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/XM430-W350.device similarity index 100% rename from humanoid_robot_device/devices/dynamixel/XM430-W350.device rename to humanoid_robot_intelligence_control_system_device/devices/dynamixel/XM430-W350.device diff --git a/humanoid_robot_device/devices/dynamixel/XM540-W150.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/XM540-W150.device similarity index 100% rename from humanoid_robot_device/devices/dynamixel/XM540-W150.device rename to humanoid_robot_intelligence_control_system_device/devices/dynamixel/XM540-W150.device diff --git a/humanoid_robot_device/devices/dynamixel/XM540-W270.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/XM540-W270.device similarity index 100% rename from humanoid_robot_device/devices/dynamixel/XM540-W270.device rename to humanoid_robot_intelligence_control_system_device/devices/dynamixel/XM540-W270.device diff --git a/humanoid_robot_device/devices/sensor/CM-740.device b/humanoid_robot_intelligence_control_system_device/devices/sensor/CM-740.device similarity index 100% rename from humanoid_robot_device/devices/sensor/CM-740.device rename to humanoid_robot_intelligence_control_system_device/devices/sensor/CM-740.device diff --git a/humanoid_robot_device/devices/sensor/OPEN-CR.device b/humanoid_robot_intelligence_control_system_device/devices/sensor/OPEN-CR.device similarity index 100% rename from humanoid_robot_device/devices/sensor/OPEN-CR.device rename to humanoid_robot_intelligence_control_system_device/devices/sensor/OPEN-CR.device diff --git a/humanoid_robot_device/include/humanoid_robot_device/control_table_item.h b/humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device/control_table_item.h similarity index 88% rename from humanoid_robot_device/include/humanoid_robot_device/control_table_item.h rename to humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device/control_table_item.h index a5338df..9b0a878 100755 --- a/humanoid_robot_device/include/humanoid_robot_device/control_table_item.h +++ b/humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device/control_table_item.h @@ -26,7 +26,7 @@ #include -namespace humanoid_robot_framework { +namespace humanoid_robot_intelligence_control_system_framework { enum AccessType { Read, ReadWrite }; @@ -49,6 +49,6 @@ public: is_signed_(false) {} }; -} // namespace humanoid_robot_framework +} // namespace humanoid_robot_intelligence_control_system_framework #endif /* ROBOTIS_DEVICE_CONTROL_TABLE_ITEM_H_ */ diff --git a/humanoid_robot_device/include/humanoid_robot_device/device.h b/humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device/device.h similarity index 91% rename from humanoid_robot_device/include/humanoid_robot_device/device.h rename to humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device/device.h index 2a74381..c14deb0 100755 --- a/humanoid_robot_device/include/humanoid_robot_device/device.h +++ b/humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device/device.h @@ -31,7 +31,7 @@ #include "control_table_item.h" -namespace humanoid_robot_framework +namespace humanoid_robot_intelligence_control_system_framework { class Device diff --git a/humanoid_robot_device/include/humanoid_robot_device/dynamixel.h b/humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device/dynamixel.h similarity index 94% rename from humanoid_robot_device/include/humanoid_robot_device/dynamixel.h rename to humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device/dynamixel.h index 36adf26..329b626 100755 --- a/humanoid_robot_device/include/humanoid_robot_device/dynamixel.h +++ b/humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device/dynamixel.h @@ -33,7 +33,7 @@ #include "device.h" #include "dynamixel_state.h" -namespace humanoid_robot_framework +namespace humanoid_robot_intelligence_control_system_framework { class Dynamixel : public Device diff --git a/humanoid_robot_device/include/humanoid_robot_device/dynamixel_state.h b/humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device/dynamixel_state.h similarity index 93% rename from humanoid_robot_device/include/humanoid_robot_device/dynamixel_state.h rename to humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device/dynamixel_state.h index f1bae5b..b915824 100755 --- a/humanoid_robot_device/include/humanoid_robot_device/dynamixel_state.h +++ b/humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device/dynamixel_state.h @@ -31,7 +31,7 @@ #define INDIRECT_DATA_1 "indirect_data_1" #define INDIRECT_ADDRESS_1 "indirect_address_1" -namespace humanoid_robot_framework +namespace humanoid_robot_intelligence_control_system_framework { class DynamixelState diff --git a/humanoid_robot_device/include/humanoid_robot_device/robot.h b/humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device/robot.h similarity index 93% rename from humanoid_robot_device/include/humanoid_robot_device/robot.h rename to humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device/robot.h index 843ef8f..15350bd 100755 --- a/humanoid_robot_device/include/humanoid_robot_device/robot.h +++ b/humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device/robot.h @@ -43,7 +43,7 @@ #define DEFAULT_CONTROL_CYCLE 8 // milliseconds -namespace humanoid_robot_framework +namespace humanoid_robot_intelligence_control_system_framework { class Robot diff --git a/humanoid_robot_device/include/humanoid_robot_device/sensor.h b/humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device/sensor.h similarity index 91% rename from humanoid_robot_device/include/humanoid_robot_device/sensor.h rename to humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device/sensor.h index 973dba3..32abe6c 100755 --- a/humanoid_robot_device/include/humanoid_robot_device/sensor.h +++ b/humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device/sensor.h @@ -32,7 +32,7 @@ #include "sensor_state.h" #include "control_table_item.h" -namespace humanoid_robot_framework +namespace humanoid_robot_intelligence_control_system_framework { class Sensor : public Device diff --git a/humanoid_robot_device/include/humanoid_robot_device/sensor_state.h b/humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device/sensor_state.h similarity index 91% rename from humanoid_robot_device/include/humanoid_robot_device/sensor_state.h rename to humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device/sensor_state.h index e28555c..08f61ba 100755 --- a/humanoid_robot_device/include/humanoid_robot_device/sensor_state.h +++ b/humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device/sensor_state.h @@ -27,7 +27,7 @@ #include "time_stamp.h" -namespace humanoid_robot_framework +namespace humanoid_robot_intelligence_control_system_framework { class SensorState diff --git a/humanoid_robot_device/include/humanoid_robot_device/time_stamp.h b/humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device/time_stamp.h similarity index 86% rename from humanoid_robot_device/include/humanoid_robot_device/time_stamp.h rename to humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device/time_stamp.h index b2bc3f7..87f59f4 100755 --- a/humanoid_robot_device/include/humanoid_robot_device/time_stamp.h +++ b/humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device/time_stamp.h @@ -24,7 +24,7 @@ #ifndef ROBOTIS_DEVICE_TIME_STAMP_H_ #define ROBOTIS_DEVICE_TIME_STAMP_H_ -namespace humanoid_robot_framework { +namespace humanoid_robot_intelligence_control_system_framework { class TimeStamp { public: @@ -34,6 +34,6 @@ public: TimeStamp(long sec, long nsec) : sec_(sec), nsec_(nsec) {} }; -} // namespace humanoid_robot_framework +} // namespace humanoid_robot_intelligence_control_system_framework #endif /* ROBOTIS_DEVICE_TIME_STAMP_H_ */ diff --git a/humanoid_robot_device/package.xml b/humanoid_robot_intelligence_control_system_device/package.xml similarity index 91% rename from humanoid_robot_device/package.xml rename to humanoid_robot_intelligence_control_system_device/package.xml index a762ecb..b21c17d 100755 --- a/humanoid_robot_device/package.xml +++ b/humanoid_robot_intelligence_control_system_device/package.xml @@ -1,11 +1,11 @@ - humanoid_robot_device + humanoid_robot_intelligence_control_system_device 0.3.2 The package that manages device information of ROBOTIS robots. This package is used when reading device information with the robot information file - from the humanoid_robot_controller package. + from the humanoid_robot_intelligence_control_system_controller package. Apache 2.0 diff --git a/humanoid_robot_device/src/humanoid_robot_device/dynamixel.cpp b/humanoid_robot_intelligence_control_system_device/src/humanoid_robot_intelligence_control_system_device/dynamixel.cpp similarity index 96% rename from humanoid_robot_device/src/humanoid_robot_device/dynamixel.cpp rename to humanoid_robot_intelligence_control_system_device/src/humanoid_robot_intelligence_control_system_device/dynamixel.cpp index 8c346c5..95a01ca 100755 --- a/humanoid_robot_device/src/humanoid_robot_device/dynamixel.cpp +++ b/humanoid_robot_intelligence_control_system_device/src/humanoid_robot_intelligence_control_system_device/dynamixel.cpp @@ -21,9 +21,9 @@ * Author: zerom */ -#include "humanoid_robot_device/dynamixel.h" +#include "humanoid_robot_intelligence_control_system_device/dynamixel.h" -using namespace humanoid_robot_framework; +using namespace humanoid_robot_intelligence_control_system_framework; Dynamixel::Dynamixel(int id, std::string model_name, float protocol_version) : ctrl_module_name_("none"), torque_to_current_value_ratio_(1.0), diff --git a/humanoid_robot_device/src/humanoid_robot_device/robot.cpp b/humanoid_robot_intelligence_control_system_device/src/humanoid_robot_intelligence_control_system_device/robot.cpp similarity index 96% rename from humanoid_robot_device/src/humanoid_robot_device/robot.cpp rename to humanoid_robot_intelligence_control_system_device/src/humanoid_robot_intelligence_control_system_device/robot.cpp index 4ae7219..c930e86 100755 --- a/humanoid_robot_device/src/humanoid_robot_device/robot.cpp +++ b/humanoid_robot_intelligence_control_system_device/src/humanoid_robot_intelligence_control_system_device/robot.cpp @@ -25,9 +25,9 @@ #include #include -#include "humanoid_robot_device/robot.h" +#include "humanoid_robot_intelligence_control_system_device/robot.h" -using namespace humanoid_robot_framework; +using namespace humanoid_robot_intelligence_control_system_framework; static inline std::string <rim(std::string &s) { s.erase(s.begin(), diff --git a/humanoid_robot_device/src/humanoid_robot_device/sensor.cpp b/humanoid_robot_intelligence_control_system_device/src/humanoid_robot_intelligence_control_system_device/sensor.cpp similarity index 86% rename from humanoid_robot_device/src/humanoid_robot_device/sensor.cpp rename to humanoid_robot_intelligence_control_system_device/src/humanoid_robot_intelligence_control_system_device/sensor.cpp index 0be04df..6d5e619 100755 --- a/humanoid_robot_device/src/humanoid_robot_device/sensor.cpp +++ b/humanoid_robot_intelligence_control_system_device/src/humanoid_robot_intelligence_control_system_device/sensor.cpp @@ -21,9 +21,9 @@ * Author: zerom */ -#include "humanoid_robot_device/sensor.h" +#include "humanoid_robot_intelligence_control_system_device/sensor.h" -using namespace humanoid_robot_framework; +using namespace humanoid_robot_intelligence_control_system_framework; Sensor::Sensor(int id, std::string model_name, float protocol_version) { diff --git a/humanoid_robot_framework/CHANGELOG.rst b/humanoid_robot_intelligence_control_system_framework/CHANGELOG.rst similarity index 91% rename from humanoid_robot_framework/CHANGELOG.rst rename to humanoid_robot_intelligence_control_system_framework/CHANGELOG.rst index 4f91270..5376392 100755 --- a/humanoid_robot_framework/CHANGELOG.rst +++ b/humanoid_robot_intelligence_control_system_framework/CHANGELOG.rst @@ -1,5 +1,5 @@ ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package humanoid_robot_framework +Changelog for package humanoid_robot_intelligence_control_system_framework ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 0.3.2 (2023-10-03) @@ -52,7 +52,7 @@ Changelog for package humanoid_robot_framework 0.2.5 (2017-06-09) ------------------ -* updated for yaml-cpp dependencies (humanoid_robot_controller) +* updated for yaml-cpp dependencies (humanoid_robot_intelligence_control_system_controller) * Contributors: SCH 0.2.4 (2017-06-07) @@ -68,7 +68,7 @@ Changelog for package humanoid_robot_framework 0.2.2 (2017-04-24) ------------------ * added a deivce: OpenCR -* updated humanoid_robot_controller.cpp +* updated humanoid_robot_intelligence_control_system_controller.cpp * changed to read control cycle from .robot file * Contributors: Zerom, Kayman diff --git a/humanoid_robot_framework/CMakeLists.txt b/humanoid_robot_intelligence_control_system_framework/CMakeLists.txt similarity index 77% rename from humanoid_robot_framework/CMakeLists.txt rename to humanoid_robot_intelligence_control_system_framework/CMakeLists.txt index aa57783..dda922c 100644 --- a/humanoid_robot_framework/CMakeLists.txt +++ b/humanoid_robot_intelligence_control_system_framework/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.8) -project(humanoid_robot_framework) +project(humanoid_robot_intelligence_control_system_framework) if($ENV{ROS_VERSION} EQUAL 1) find_package(catkin REQUIRED) diff --git a/humanoid_robot_intelligence_control_system_framework/package.xml b/humanoid_robot_intelligence_control_system_framework/package.xml new file mode 100755 index 0000000..39be137 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_framework/package.xml @@ -0,0 +1,40 @@ + + + humanoid_robot_intelligence_control_system_framework + 0.3.2 + ROS packages for the humanoid_robot_intelligence_control_system_framework (meta package) + Apache 2.0 + Ronaldson Bellande + + + catkin + + humanoid_robot_intelligence_control_system_controller + humanoid_robot_intelligence_control_system_device + humanoid_robot_intelligence_control_system_framework_common + + humanoid_robot_intelligence_control_system_controller + humanoid_robot_intelligence_control_system_device + humanoid_robot_intelligence_control_system_framework_common + + humanoid_robot_intelligence_control_system_controller + humanoid_robot_intelligence_control_system_device + humanoid_robot_intelligence_control_system_framework_common + + + ament_cmake + + humanoid_robot_intelligence_control_system_controller + humanoid_robot_intelligence_control_system_device + humanoid_robot_intelligence_control_system_framework_common + + humanoid_robot_intelligence_control_system_controller + humanoid_robot_intelligence_control_system_device + humanoid_robot_intelligence_control_system_framework_common + + humanoid_robot_intelligence_control_system_controller + humanoid_robot_intelligence_control_system_device + humanoid_robot_intelligence_control_system_framework_common + + + diff --git a/humanoid_robot_framework_common/CHANGELOG.rst b/humanoid_robot_intelligence_control_system_framework_common/CHANGELOG.rst similarity index 96% rename from humanoid_robot_framework_common/CHANGELOG.rst rename to humanoid_robot_intelligence_control_system_framework_common/CHANGELOG.rst index 2952d37..0af7c89 100755 --- a/humanoid_robot_framework_common/CHANGELOG.rst +++ b/humanoid_robot_intelligence_control_system_framework_common/CHANGELOG.rst @@ -1,5 +1,5 @@ ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package humanoid_robot_framework_common +Changelog for package humanoid_robot_intelligence_control_system_framework_common ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 0.3.2 (2023-10-03) diff --git a/humanoid_robot_framework_common/CMakeLists.txt b/humanoid_robot_intelligence_control_system_framework_common/CMakeLists.txt similarity index 74% rename from humanoid_robot_framework_common/CMakeLists.txt rename to humanoid_robot_intelligence_control_system_framework_common/CMakeLists.txt index 4c306a2..d90d27c 100755 --- a/humanoid_robot_framework_common/CMakeLists.txt +++ b/humanoid_robot_intelligence_control_system_framework_common/CMakeLists.txt @@ -1,11 +1,11 @@ cmake_minimum_required(VERSION 3.8) -project(humanoid_robot_framework_common) +project(humanoid_robot_intelligence_control_system_framework_common) if($ENV{ROS_VERSION} EQUAL 1) find_package(catkin REQUIRED COMPONENTS roscpp - humanoid_robot_device + humanoid_robot_intelligence_control_system_device ) else() find_package(ament_cmake REQUIRED) @@ -18,7 +18,7 @@ if($ENV{ROS_VERSION} EQUAL 1) LIBRARIES ${PROJECT_NAME} CATKIN_DEPENDS roscpp - humanoid_robot_device + humanoid_robot_intelligence_control_system_device ) endif() @@ -35,7 +35,7 @@ add_library(${PROJECT_NAME} ) set_target_properties(${PROJECT_NAME} PROPERTIES LINKER_LANGUAGE CXX) -install(TARGETS humanoid_robot_framework_common +install(TARGETS humanoid_robot_intelligence_control_system_framework_common ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} diff --git a/humanoid_robot_framework_common/include/humanoid_robot_framework_common/motion_module.h b/humanoid_robot_intelligence_control_system_framework_common/include/humanoid_robot_intelligence_control_system_framework_common/motion_module.h similarity index 87% rename from humanoid_robot_framework_common/include/humanoid_robot_framework_common/motion_module.h rename to humanoid_robot_intelligence_control_system_framework_common/include/humanoid_robot_intelligence_control_system_framework_common/motion_module.h index 6db3e6e..9df7326 100755 --- a/humanoid_robot_framework_common/include/humanoid_robot_framework_common/motion_module.h +++ b/humanoid_robot_intelligence_control_system_framework_common/include/humanoid_robot_intelligence_control_system_framework_common/motion_module.h @@ -29,10 +29,10 @@ #include #include "singleton.h" -#include "humanoid_robot_device/robot.h" -#include "humanoid_robot_device/dynamixel.h" +#include "humanoid_robot_intelligence_control_system_device/robot.h" +#include "humanoid_robot_intelligence_control_system_device/dynamixel.h" -namespace humanoid_robot_framework +namespace humanoid_robot_intelligence_control_system_framework { enum ControlMode diff --git a/humanoid_robot_framework_common/include/humanoid_robot_framework_common/sensor_module.h b/humanoid_robot_intelligence_control_system_framework_common/include/humanoid_robot_intelligence_control_system_framework_common/sensor_module.h similarity index 84% rename from humanoid_robot_framework_common/include/humanoid_robot_framework_common/sensor_module.h rename to humanoid_robot_intelligence_control_system_framework_common/include/humanoid_robot_intelligence_control_system_framework_common/sensor_module.h index fe1a8c3..23e6292 100755 --- a/humanoid_robot_framework_common/include/humanoid_robot_framework_common/sensor_module.h +++ b/humanoid_robot_intelligence_control_system_framework_common/include/humanoid_robot_intelligence_control_system_framework_common/sensor_module.h @@ -29,10 +29,10 @@ #include #include "singleton.h" -#include "humanoid_robot_device/robot.h" -#include "humanoid_robot_device/dynamixel.h" +#include "humanoid_robot_intelligence_control_system_device/robot.h" +#include "humanoid_robot_intelligence_control_system_device/dynamixel.h" -namespace humanoid_robot_framework +namespace humanoid_robot_intelligence_control_system_framework { class SensorModule diff --git a/humanoid_robot_framework_common/include/humanoid_robot_framework_common/singleton.h b/humanoid_robot_intelligence_control_system_framework_common/include/humanoid_robot_intelligence_control_system_framework_common/singleton.h similarity index 92% rename from humanoid_robot_framework_common/include/humanoid_robot_framework_common/singleton.h rename to humanoid_robot_intelligence_control_system_framework_common/include/humanoid_robot_intelligence_control_system_framework_common/singleton.h index 287de76..5f1c4ab 100755 --- a/humanoid_robot_framework_common/include/humanoid_robot_framework_common/singleton.h +++ b/humanoid_robot_intelligence_control_system_framework_common/include/humanoid_robot_intelligence_control_system_framework_common/singleton.h @@ -25,7 +25,7 @@ #define ROBOTIS_FRAMEWORK_COMMON_SINGLETON_H_ -namespace humanoid_robot_framework +namespace humanoid_robot_intelligence_control_system_framework { template diff --git a/humanoid_robot_framework_common/package.xml b/humanoid_robot_intelligence_control_system_framework_common/package.xml similarity index 58% rename from humanoid_robot_framework_common/package.xml rename to humanoid_robot_intelligence_control_system_framework_common/package.xml index 49f6de3..c12c3b2 100755 --- a/humanoid_robot_framework_common/package.xml +++ b/humanoid_robot_intelligence_control_system_framework_common/package.xml @@ -1,8 +1,8 @@ - humanoid_robot_framework_common + humanoid_robot_intelligence_control_system_framework_common 0.3.2 - The package contains commonly used headers for the humanoid_robot_framework + The package contains commonly used headers for the humanoid_robot_intelligence_control_system_framework Apache 2.0 Ronaldson Bellande @@ -10,24 +10,24 @@ catkin roscpp - humanoid_robot_device + humanoid_robot_intelligence_control_system_device roscpp - humanoid_robot_device + humanoid_robot_intelligence_control_system_device roscpp - humanoid_robot_device + humanoid_robot_intelligence_control_system_device ament_cmake roscpp - humanoid_robot_device + humanoid_robot_intelligence_control_system_device roscpp - humanoid_robot_device + humanoid_robot_intelligence_control_system_device roscpp - humanoid_robot_device + humanoid_robot_intelligence_control_system_device From cad9453b6a03ac56aacbb4eb3f9592bf1615a465 Mon Sep 17 00:00:00 2001 From: RonaldsonBellande Date: Mon, 13 Nov 2023 17:55:45 -0500 Subject: [PATCH 212/238] latest pushes --- .travis.yml | 2 +- README.md | 24 +- .../CHANGELOG.rst | 136 + .../CMakeLists.txt | 83 + ...t_intelligence_control_system_controller.h | 209 ++ .../package.xml | 81 + ...intelligence_control_system_controller.cpp | 2625 +++++++++++++++++ .../CHANGELOG.rst | 110 + .../CMakeLists.txt | 51 + .../devices/dynamixel/GRIPPER.device | 73 + .../devices/dynamixel/GRIPPER_TORQUE.device | 74 + .../devices/dynamixel/H42-20-S300-R(A).device | 92 + .../devices/dynamixel/H42-20-S300-R.device | 76 + .../devices/dynamixel/H42P-020-S300-R.device | 92 + .../dynamixel/H54-100-B210-R-NR.device | 76 + .../dynamixel/H54-100-S500-R(A).device | 92 + .../devices/dynamixel/H54-100-S500-R.device | 76 + .../devices/dynamixel/H54-200-B500-R.device | 76 + .../dynamixel/H54-200-S500-R(A).device | 92 + .../devices/dynamixel/H54-200-S500-R.device | 76 + .../devices/dynamixel/H54P-100-S500-R.device | 92 + .../devices/dynamixel/H54P-200-B500-R.device | 92 + .../devices/dynamixel/H54P-200-S500-R.device | 92 + .../devices/dynamixel/L42-10-S300-R.device | 73 + .../devices/dynamixel/L54-30-S400-R.device | 74 + .../devices/dynamixel/L54-30-S500-R.device | 74 + .../devices/dynamixel/L54-50-S290-R.device | 74 + .../devices/dynamixel/L54-50-S500-R.device | 74 + .../devices/dynamixel/M42-10-S260-R.device | 74 + .../devices/dynamixel/M54-40-S250-R.device | 74 + .../devices/dynamixel/M54-60-S250-R.device | 74 + .../devices/dynamixel/MX-106.device | 66 + .../devices/dynamixel/MX-28.device | 62 + .../devices/dynamixel/MX-64.device | 66 + .../devices/dynamixel/PH42-020-S300-R.device | 92 + .../devices/dynamixel/PH54-100-S500-R.device | 92 + .../devices/dynamixel/PH54-200-S500-R.device | 92 + .../devices/dynamixel/RH-P12-RN(A).device | 90 + .../devices/dynamixel/RH-P12-RN.device | 73 + .../devices/dynamixel/XM-430.device | 82 + .../devices/dynamixel/XM430-W210.device | 83 + .../devices/dynamixel/XM430-W350.device | 83 + .../devices/dynamixel/XM540-W150.device | 89 + .../devices/dynamixel/XM540-W270.device | 89 + .../devices/sensor/CM-740.device | 25 + .../devices/sensor/OPEN-CR.device | 30 + .../control_table_item.h | 54 + .../device.h | 54 + .../dynamixel.h | 83 + .../dynamixel_state.h | 82 + .../robot.h | 72 + .../sensor.h | 49 + .../sensor_state.h | 50 + .../time_stamp.h | 39 + .../package.xml | 38 + .../dynamixel.cpp | 119 + .../robot.cpp | 480 +++ .../sensor.cpp | 39 + .../CHANGELOG.rst | 82 + .../CMakeLists.txt | 46 + .../motion_module.h | 87 + .../sensor_module.h | 57 + .../singleton.h | 65 + .../package.xml | 33 + 64 files changed, 7713 insertions(+), 13 deletions(-) create mode 100755 humanoid_robot_intelligence_control_system_controller/CHANGELOG.rst create mode 100755 humanoid_robot_intelligence_control_system_controller/CMakeLists.txt create mode 100755 humanoid_robot_intelligence_control_system_controller/include/humanoid_robot_intelligence_control_system_controller/humanoid_robot_intelligence_control_system_controller.h create mode 100755 humanoid_robot_intelligence_control_system_controller/package.xml create mode 100755 humanoid_robot_intelligence_control_system_controller/src/humanoid_robot_intelligence_control_system_controller/humanoid_robot_intelligence_control_system_controller.cpp create mode 100755 humanoid_robot_intelligence_control_system_device/CHANGELOG.rst create mode 100755 humanoid_robot_intelligence_control_system_device/CMakeLists.txt create mode 100755 humanoid_robot_intelligence_control_system_device/devices/dynamixel/GRIPPER.device create mode 100755 humanoid_robot_intelligence_control_system_device/devices/dynamixel/GRIPPER_TORQUE.device create mode 100644 humanoid_robot_intelligence_control_system_device/devices/dynamixel/H42-20-S300-R(A).device create mode 100755 humanoid_robot_intelligence_control_system_device/devices/dynamixel/H42-20-S300-R.device create mode 100644 humanoid_robot_intelligence_control_system_device/devices/dynamixel/H42P-020-S300-R.device create mode 100755 humanoid_robot_intelligence_control_system_device/devices/dynamixel/H54-100-B210-R-NR.device create mode 100644 humanoid_robot_intelligence_control_system_device/devices/dynamixel/H54-100-S500-R(A).device create mode 100755 humanoid_robot_intelligence_control_system_device/devices/dynamixel/H54-100-S500-R.device create mode 100755 humanoid_robot_intelligence_control_system_device/devices/dynamixel/H54-200-B500-R.device create mode 100644 humanoid_robot_intelligence_control_system_device/devices/dynamixel/H54-200-S500-R(A).device create mode 100755 humanoid_robot_intelligence_control_system_device/devices/dynamixel/H54-200-S500-R.device create mode 100644 humanoid_robot_intelligence_control_system_device/devices/dynamixel/H54P-100-S500-R.device create mode 100644 humanoid_robot_intelligence_control_system_device/devices/dynamixel/H54P-200-B500-R.device create mode 100644 humanoid_robot_intelligence_control_system_device/devices/dynamixel/H54P-200-S500-R.device create mode 100755 humanoid_robot_intelligence_control_system_device/devices/dynamixel/L42-10-S300-R.device create mode 100755 humanoid_robot_intelligence_control_system_device/devices/dynamixel/L54-30-S400-R.device create mode 100755 humanoid_robot_intelligence_control_system_device/devices/dynamixel/L54-30-S500-R.device create mode 100755 humanoid_robot_intelligence_control_system_device/devices/dynamixel/L54-50-S290-R.device create mode 100755 humanoid_robot_intelligence_control_system_device/devices/dynamixel/L54-50-S500-R.device create mode 100755 humanoid_robot_intelligence_control_system_device/devices/dynamixel/M42-10-S260-R.device create mode 100755 humanoid_robot_intelligence_control_system_device/devices/dynamixel/M54-40-S250-R.device create mode 100755 humanoid_robot_intelligence_control_system_device/devices/dynamixel/M54-60-S250-R.device create mode 100755 humanoid_robot_intelligence_control_system_device/devices/dynamixel/MX-106.device create mode 100755 humanoid_robot_intelligence_control_system_device/devices/dynamixel/MX-28.device create mode 100755 humanoid_robot_intelligence_control_system_device/devices/dynamixel/MX-64.device create mode 100644 humanoid_robot_intelligence_control_system_device/devices/dynamixel/PH42-020-S300-R.device create mode 100644 humanoid_robot_intelligence_control_system_device/devices/dynamixel/PH54-100-S500-R.device create mode 100644 humanoid_robot_intelligence_control_system_device/devices/dynamixel/PH54-200-S500-R.device create mode 100644 humanoid_robot_intelligence_control_system_device/devices/dynamixel/RH-P12-RN(A).device create mode 100755 humanoid_robot_intelligence_control_system_device/devices/dynamixel/RH-P12-RN.device create mode 100755 humanoid_robot_intelligence_control_system_device/devices/dynamixel/XM-430.device create mode 100644 humanoid_robot_intelligence_control_system_device/devices/dynamixel/XM430-W210.device create mode 100644 humanoid_robot_intelligence_control_system_device/devices/dynamixel/XM430-W350.device create mode 100755 humanoid_robot_intelligence_control_system_device/devices/dynamixel/XM540-W150.device create mode 100755 humanoid_robot_intelligence_control_system_device/devices/dynamixel/XM540-W270.device create mode 100755 humanoid_robot_intelligence_control_system_device/devices/sensor/CM-740.device create mode 100755 humanoid_robot_intelligence_control_system_device/devices/sensor/OPEN-CR.device create mode 100755 humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device/control_table_item.h create mode 100755 humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device/device.h create mode 100755 humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device/dynamixel.h create mode 100755 humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device/dynamixel_state.h create mode 100755 humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device/robot.h create mode 100755 humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device/sensor.h create mode 100755 humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device/sensor_state.h create mode 100755 humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device/time_stamp.h create mode 100755 humanoid_robot_intelligence_control_system_device/package.xml create mode 100755 humanoid_robot_intelligence_control_system_device/src/humanoid_robot_intelligence_control_system_device/dynamixel.cpp create mode 100755 humanoid_robot_intelligence_control_system_device/src/humanoid_robot_intelligence_control_system_device/robot.cpp create mode 100755 humanoid_robot_intelligence_control_system_device/src/humanoid_robot_intelligence_control_system_device/sensor.cpp create mode 100755 humanoid_robot_intelligence_control_system_framework_common/CHANGELOG.rst create mode 100755 humanoid_robot_intelligence_control_system_framework_common/CMakeLists.txt create mode 100755 humanoid_robot_intelligence_control_system_framework_common/include/humanoid_robot_intelligence_control_system_framework_common/motion_module.h create mode 100755 humanoid_robot_intelligence_control_system_framework_common/include/humanoid_robot_intelligence_control_system_framework_common/sensor_module.h create mode 100755 humanoid_robot_intelligence_control_system_framework_common/include/humanoid_robot_intelligence_control_system_framework_common/singleton.h create mode 100755 humanoid_robot_intelligence_control_system_framework_common/package.xml diff --git a/.travis.yml b/.travis.yml index e1e689a..43bc280 100644 --- a/.travis.yml +++ b/.travis.yml @@ -18,7 +18,7 @@ notifications: - ronaldsonbellande@gmail.com env: matrix: - - ROS_DISTRO=noetic ROS_REPO=ros-shadow-fixed UPSTREAM_WORKSPACE=file OS_NAME=ubuntu OS_CODE_NAME=focal $ROSINSTALL_FILENAME=".humanoid_robot_framework.rosinstall" + - ROS_DISTRO=noetic ROS_REPO=ros-shadow-fixed UPSTREAM_WORKSPACE=file OS_NAME=ubuntu OS_CODE_NAME=focal $ROSINSTALL_FILENAME=".humanoid_robot_intelligence_control_system_framework.rosinstall" branches: only: - master diff --git a/README.md b/README.md index 9968397..8b0d5d4 100755 --- a/README.md +++ b/README.md @@ -5,27 +5,27 @@ [![Sponsor](https://img.shields.io/badge/Sponsor-Robotics%20Sensors%20Research-red?style=for-the-badge&logo=github)](https://github.com/sponsors/Robotics-Sensors) # Stats -[![GitHub stars](https://img.shields.io/github/stars/Robotics-Sensors/humanoid_robot_framework.svg?style=social)](https://github.com/Robotics-Sensors/humanoid_robot_framework/stargazers) -[![GitHub forks](https://img.shields.io/github/forks/Robotics-Sensors/humanoid_robot_framework.svg?style=social)](https://github.com/Robotics-Sensors/humanoid_robot_framework/network) -[![GitHub watchers](https://img.shields.io/github/watchers/Robotics-Sensors/humanoid_robot_framework.svg?style=social)](https://github.com/Robotics-Sensors/humanoid_robot_framework/watchers) +[![GitHub stars](https://img.shields.io/github/stars/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework.svg?style=social)](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework/stargazers) +[![GitHub forks](https://img.shields.io/github/forks/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework.svg?style=social)](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework/network) +[![GitHub watchers](https://img.shields.io/github/watchers/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework.svg?style=social)](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework/watchers) -[![GitHub issues](https://img.shields.io/github/issues/Robotics-Sensors/humanoid_robot_framework.svg)](https://github.com/Robotics-Sensors/humanoid_robot_framework/issues) -[![GitHub pull requests](https://img.shields.io/github/issues-pr/Robotics-Sensors/humanoid_robot_framework.svg)](https://github.com/Robotics-Sensors/humanoid_robot_framework/pulls) -[![GitHub license](https://img.shields.io/github/license/Robotics-Sensors/humanoid_robot_framework.svg)](https://github.com/Robotics-Sensors/humanoid_robot_framework/blob/main/LICENSE) +[![GitHub issues](https://img.shields.io/github/issues/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework.svg)](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework/issues) +[![GitHub pull requests](https://img.shields.io/github/issues-pr/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework.svg)](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework/pulls) +[![GitHub license](https://img.shields.io/github/license/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework.svg)](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework/blob/main/LICENSE) -[![GitHub last commit](https://img.shields.io/github/last-commit/Robotics-Sensors/humanoid_robot_framework.svg)](https://github.com/Robotics-Sensors/humanoid_robot_framework/commits) -[![Visitor & Github Clones](https://img.shields.io/badge/dynamic/json?color=2e8b57&label=Visitor%20%26%20GitHub%20Clones&query=$.count&url=https://api.github.com/repos/Robotics-Sensors/humanoid_robot_framework/traffic)](https://github.com/Robotics-Sensors/humanoid_robot_framework) +[![GitHub last commit](https://img.shields.io/github/last-commit/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework.svg)](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework/commits) +[![Visitor & Github Clones](https://img.shields.io/badge/dynamic/json?color=2e8b57&label=Visitor%20%26%20GitHub%20Clones&query=$.count&url=https://api.github.com/repos/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework/traffic)](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework) -------------------------------------------------------------------------------------------------------- # Repository Website -https://robotics-sensors.github.io/humanoid_robot_framework +https://robotics-sensors.github.io/humanoid_robot_intelligence_control_system_framework -------------------------------------------------------------------------------------------------------- -Updated Version [humanoid_robot_framework](https://github.com/Robotics-Sensors/humanoid_robot_framework) readme. +Updated Version [humanoid_robot_intelligence_control_system_framework](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework) readme. Old Version/Previous Used for Different Context [ROBOTIS-Framework](https://github.com/ROBOTIS-GIT/ROBOTIS-Framework) readme. # Release -[![Latest Release](https://img.shields.io/github/v/release/Robotics-Sensors/humanoid_robot_tools?style=for-the-badge&color=yellow)](https://github.com/Robotics-Sensors/humanoid_robot_framework/releases/) +[![Latest Release](https://img.shields.io/github/v/release/Robotics-Sensors/humanoid_robot_intelligence_control_system_tools?style=for-the-badge&color=yellow)](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework/releases/) # Contact Depeding on the repository, If you are interested in accessing the complete version or other repository related to this repository, we kindly request that you reach out to the organization's director or the company behind this project. We invite you to explore the USE CASE to understand the specific terms and conditions for usage. To utilize this repository, it is imperative that you adhere to the guidelines outlined in the USE CASE. For those interested in showing their support, becoming a sponsor of the organization is an option, and detailed information can be found within the USE CASE and License documents. Furthermore, we encourage you to join our official Discord community, where you can engage with like-minded individuals, contribute to the project, and stay up-to-date with the latest developments. It's worth noting that while a fraction of research is publicly accessible, the majority remains private. To gain insights into the wealth of knowledge held by the Company or the organization's director, we recommend direct contact for more information. @@ -48,4 +48,4 @@ Latest versions and Maintainer is on organization https://github.com/Robotics-Se * Ronaldson Bellande ## License -This SDK is distributed under the [Apache License, Version 2.0](https://www.apache.org/licenses/LICENSE-2.0), see [LICENSE](https://github.com/Robotics-Sensors/humanoid_robot_framework/blob/main/LICENSE) and [NOTICE](https://github.com/Robotics-Sensors/humanoid_robot_framework/blob/main/LICENSE) for more information. +This SDK is distributed under the [Apache License, Version 2.0](https://www.apache.org/licenses/LICENSE-2.0), see [LICENSE](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework/blob/main/LICENSE) and [NOTICE](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework/blob/main/LICENSE) for more information. diff --git a/humanoid_robot_intelligence_control_system_controller/CHANGELOG.rst b/humanoid_robot_intelligence_control_system_controller/CHANGELOG.rst new file mode 100755 index 0000000..0b85c9e --- /dev/null +++ b/humanoid_robot_intelligence_control_system_controller/CHANGELOG.rst @@ -0,0 +1,136 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package humanoid_robot_intelligence_control_system_controller +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.3.2 (2023-10-03) +------------------ +* Make it compatible for ROS1/ROS2 +* Fix bugs +* Update package.xml and CMakeList.txt for to the latest versions +* Contributors & Maintainer: Ronaldson Bellande + +0.3.1 (2023-09-27) +------------------ +* Starting from this point it under a new license +* Fix errors and Issues +* Rename Repository for a completely different purpose +* Make it compatible with ROS/ROS2 +* Upgrade version of all builds and make it more compatible +* Update package.xml and CMakeList.txt for to the latest versions +* Contributors & Maintainer: Ronaldson Bellande + +0.3.0 (2021-05-03) +------------------ +* Update package.xml and CMakeList.txt for noetic branch +* Contributors: Ronaldson Bellande + +0.2.9 (2018-03-22) +------------------ +* added serivce for setting module +* deleted comment for debug +* modified to prevent duplicate indirect address write +* added boost system dependencies +* fixed a bug that occure when handling bulk read item that does not exist. +* Contributors: Kayman, Zerom, Pyo + +0.2.8 (2018-03-20) +------------------ +* modified CMakeLists.txt for system dependencies (yaml-cpp) +* Contributors: Zerom, Pyo + +0.2.7 (2018-03-15) +------------------ +* changed the License and package format to version 2 +* changed all values read by bulk read are saved to dxl_state\_->bulk_read_table\_. +* Modified to prevent duplicate indirect address write +* Contributors: SCH, Zerom, Pyo + +0.2.6 (2017-08-09) +------------------ +* multi thread bug fixed. +* unnecessary debug print removed. +* OpenCR control table item name changed. (torque_enable -> dynamixel_power) +* fixed to not update update_time_stamp\_ if bulk read fails. +* Contributors: Zerom + +0.2.5 (2017-06-09) +------------------ +* updated for yaml-cpp dependencies +* Contributors: SCH + +0.2.4 (2017-06-07) +------------------ +* added cmake_modules in package.xml +* Contributors: SCH + +0.2.3 (2017-05-23) +------------------ +* updated the cmake file for ros install +* Contributors: SCH + +0.2.2 (2017-04-24) +------------------ +* updated humanoid_robot_intelligence_control_system_controller.cpp +* changed to read control cycle from .robot file +* Contributors: Zerom + +0.2.1 (2016-11-23) +------------------ +* Merge the changes and update +* - Direct Control Mode bug fixed. +* update +* - added writeControlTableCallback +* - added WriteControlTable msg callback +* mode change debugging +* - optimized cpu usage by spin loop (by astumpf) +* - humanoid_robot_intelligence_control_system_controller process() : processing order changed. + * 1st : packet communication + * 2nd : processing modules +* - dependencies fixed. (Pull requests `#26 `_) +* - make setJointCtrlModuleCallback() to the thread function & improved. +* - modified dependency problem. +* - reduce CPU consumption +* Contributors: Jay Song, Pyo, Zerom, SCH + +0.2.0 (2016-08-31) +------------------ +* bug fixed (position pid gain & velocity pid gain sync write). +* added velocity_to_value_ratio to DXL Pro-H series. +* changed some debug messages. +* added velocity p/i/d gain and position i/d gain sync_write code. +* SyncWriteItem bug fixed. +* add function / modified the code simple (using auto / range based for loop) +* added XM-430-W210 / XM-430-W350 device file. +* rename ControlMode(CurrentControl -> TorqueControl) +* rename (port_to_sync_write_torque\_ -> port_to_sync_write_current\_) +* rename (present_current\_ -> present_torque\_) +* modified torque control code +* fixed typos / changed ROS_INFO -> fprintf (for processing speed) +* startTimer() : after bulkread txpacket(), need some sleep() +* changed the order of processing in the Process() function. +* added missing mutex for gazebo +* fixed crash when running in gazebo simulation +* sync write bug fix. +* added position_p_gain sync write +* MotionModule/SensorModule member variable access changed (public -> protected). +* Contributors: Jay Song, Zerom, Pyo, SCH + +0.1.1 (2016-08-18) +------------------ +* updated the package information + +0.1.0 (2016-08-12) +------------------ +* first public release for Kinetic +* modified the package information for release +* develop branch -> master branch +* function name changed : DeviceInit() -> InitDevice() +* Fixed high CPU consumption due to busy waits +* add SensorState + add Singleton template +* XM-430 / CM-740 device file added. + Sensor device added. +* added code to support the gazebo simulator +* added first bulk read failure protection code +* renewal +* Contributors: Alexander Stumpf, Jay Song, Zerom, Pyo diff --git a/humanoid_robot_intelligence_control_system_controller/CMakeLists.txt b/humanoid_robot_intelligence_control_system_controller/CMakeLists.txt new file mode 100755 index 0000000..c69c671 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_controller/CMakeLists.txt @@ -0,0 +1,83 @@ +cmake_minimum_required(VERSION 3.8) +project(humanoid_robot_intelligence_control_system_controller) + +## Compile as C++11, supported in ROS Kinetic and newer +add_compile_options(-std=c++11) + + +if($ENV{ROS_VERSION} EQUAL 1) + find_package(catkin REQUIRED COMPONENTS + roscpp + roslib + std_msgs + sensor_msgs + humanoid_robot_intelligence_control_system_controller_msgs + dynamixel_sdk + humanoid_robot_intelligence_control_system_device + humanoid_robot_intelligence_control_system_framework_common + cmake_modules + ) + + find_package(Boost REQUIRED) + + find_package(PkgConfig REQUIRED) +else() + find_package(ament_cmake REQUIRED) +endif() + + +pkg_check_modules(YAML_CPP REQUIRED yaml-cpp) +find_path(YAML_CPP_INCLUDE_DIR + NAMES yaml_cpp.h + PATHS ${YAML_CPP_INCLUDE_DIRS} +) +find_library(YAML_CPP_LIBRARY + NAMES YAML_CPP + PATHS ${YAML_CPP_LIBRARY_DIRS} +) +link_directories(${YAML_CPP_LIBRARY_DIRS}) + +if(NOT ${YAML_CPP_VERSION} VERSION_LESS "0.5") +add_definitions(-DHAVE_NEW_YAMLCPP) +endif(NOT ${YAML_CPP_VERSION} VERSION_LESS "0.5") + + +if($ENV{ROS_VERSION} EQUAL 1) + catkin_package( + INCLUDE_DIRS include + LIBRARIES humanoid_robot_intelligence_control_system_controller + CATKIN_DEPENDS + roscpp + roslib + std_msgs + sensor_msgs + humanoid_robot_intelligence_control_system_controller_msgs + dynamixel_sdk + humanoid_robot_intelligence_control_system_device + humanoid_robot_intelligence_control_system_framework_common + cmake_modules + DEPENDS Boost + ) +endif() + + +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} + ${YAML_CPP_INCLUDE_DIRS} +) + +add_library(humanoid_robot_intelligence_control_system_controller src/humanoid_robot_intelligence_control_system_controller/humanoid_robot_intelligence_control_system_controller.cpp) +add_dependencies(humanoid_robot_intelligence_control_system_controller ${catkin_EXPORTED_TARGETS}) +target_link_libraries(humanoid_robot_intelligence_control_system_controller ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${YAML_CPP_LIBRARIES}) + +install(TARGETS humanoid_robot_intelligence_control_system_controller + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) diff --git a/humanoid_robot_intelligence_control_system_controller/include/humanoid_robot_intelligence_control_system_controller/humanoid_robot_intelligence_control_system_controller.h b/humanoid_robot_intelligence_control_system_controller/include/humanoid_robot_intelligence_control_system_controller/humanoid_robot_intelligence_control_system_controller.h new file mode 100755 index 0000000..019a9bb --- /dev/null +++ b/humanoid_robot_intelligence_control_system_controller/include/humanoid_robot_intelligence_control_system_controller/humanoid_robot_intelligence_control_system_controller.h @@ -0,0 +1,209 @@ +/******************************************************************************* + * Copyright 2018 ROBOTIS CO., LTD. + * + * 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. + *******************************************************************************/ + +/* + * humanoid_robot_intelligence_control_system_controller.h + * + * Created on: 2016. 1. 15. + * Author: zerom + */ + +#ifndef ROBOTIS_CONTROLLER_ROBOTIS_CONTROLLER_H_ +#define ROBOTIS_CONTROLLER_ROBOTIS_CONTROLLER_H_ + +#include +#include +#include +#include +#include +#include +#include + +#include "humanoid_robot_intelligence_control_system_controller_msgs/GetJointModule.h" +#include "humanoid_robot_intelligence_control_system_controller_msgs/JointCtrlModule.h" +#include "humanoid_robot_intelligence_control_system_controller_msgs/LoadOffset.h" +#include "humanoid_robot_intelligence_control_system_controller_msgs/SetJointModule.h" +#include "humanoid_robot_intelligence_control_system_controller_msgs/SetModule.h" +#include "humanoid_robot_intelligence_control_system_controller_msgs/SyncWriteItem.h" +#include "humanoid_robot_intelligence_control_system_controller_msgs/WriteControlTable.h" + +#include "dynamixel_sdk/group_bulk_read.h" +#include "dynamixel_sdk/group_sync_write.h" +#include "humanoid_robot_intelligence_control_system_device/robot.h" +#include "humanoid_robot_intelligence_control_system_framework_common/motion_module.h" +#include "humanoid_robot_intelligence_control_system_framework_common/sensor_module.h" + +namespace humanoid_robot_intelligence_control_system_framework { + +enum ControllerMode { MotionModuleMode, DirectControlMode }; + +class RobotisController : public Singleton { +private: + boost::thread queue_thread_; + boost::thread gazebo_thread_; + boost::thread set_module_thread_; + boost::mutex queue_mutex_; + + bool init_pose_loaded_; + bool is_timer_running_; + bool is_offset_enabled_; + double offset_ratio_; + bool stop_timer_; + pthread_t timer_thread_; + ControllerMode controller_mode_; + + std::list motion_modules_; + std::list sensor_modules_; + std::vector direct_sync_write_; + + std::map sensor_result_; + + void gazeboTimerThread(); + void msgQueueThread(); + void setCtrlModuleThread(std::string ctrl_module); + void setJointCtrlModuleThread( + const humanoid_robot_intelligence_control_system_controller_msgs::JointCtrlModule::ConstPtr &msg); + + bool isTimerStopped(); + void initializeSyncWrite(); + +public: + const int NONE_GAIN = 65535; + bool DEBUG_PRINT; + Robot *robot_; + + bool gazebo_mode_; + std::string gazebo_robot_name_; + + /* bulk read */ + std::map port_to_bulk_read_; + + /* sync write */ + std::map + port_to_sync_write_position_; + std::map + port_to_sync_write_velocity_; + std::map + port_to_sync_write_current_; + std::map + port_to_sync_write_position_p_gain_; + std::map + port_to_sync_write_position_i_gain_; + std::map + port_to_sync_write_position_d_gain_; + std::map + port_to_sync_write_velocity_p_gain_; + std::map + port_to_sync_write_velocity_i_gain_; + std::map + port_to_sync_write_velocity_d_gain_; + + /* publisher */ + ros::Publisher goal_joint_state_pub_; + ros::Publisher present_joint_state_pub_; + ros::Publisher current_module_pub_; + + std::map gazebo_joint_position_pub_; + std::map gazebo_joint_velocity_pub_; + std::map gazebo_joint_effort_pub_; + + static void *timerThread(void *param); + + RobotisController(); + + bool initialize(const std::string robot_file_path, + const std::string init_file_path); + void initializeDevice(const std::string init_file_path); + void process(); + + void addMotionModule(MotionModule *module); + void removeMotionModule(MotionModule *module); + void addSensorModule(SensorModule *module); + void removeSensorModule(SensorModule *module); + + void startTimer(); + void stopTimer(); + bool isTimerRunning(); + + void setCtrlModule(std::string module_name); + void loadOffset(const std::string path); + + /* ROS Topic Callback Functions */ + void writeControlTableCallback( + const humanoid_robot_intelligence_control_system_controller_msgs::WriteControlTable::ConstPtr &msg); + void syncWriteItemCallback( + const humanoid_robot_intelligence_control_system_controller_msgs::SyncWriteItem::ConstPtr &msg); + void setControllerModeCallback(const std_msgs::String::ConstPtr &msg); + void setJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg); + void setJointCtrlModuleCallback( + const humanoid_robot_intelligence_control_system_controller_msgs::JointCtrlModule::ConstPtr &msg); + void setCtrlModuleCallback(const std_msgs::String::ConstPtr &msg); + void enableOffsetCallback(const std_msgs::Bool::ConstPtr &msg); + bool getJointCtrlModuleService( + humanoid_robot_intelligence_control_system_controller_msgs::GetJointModule::Request &req, + humanoid_robot_intelligence_control_system_controller_msgs::GetJointModule::Response &res); + bool setJointCtrlModuleService( + humanoid_robot_intelligence_control_system_controller_msgs::SetJointModule::Request &req, + humanoid_robot_intelligence_control_system_controller_msgs::SetJointModule::Response &res); + bool setCtrlModuleService( + humanoid_robot_intelligence_control_system_controller_msgs::SetModule::Request &req, + humanoid_robot_intelligence_control_system_controller_msgs::SetModule::Response &res); + bool + loadOffsetService(humanoid_robot_intelligence_control_system_controller_msgs::LoadOffset::Request &req, + humanoid_robot_intelligence_control_system_controller_msgs::LoadOffset::Response &res); + + void gazeboJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg); + + int ping(const std::string joint_name, uint8_t *error = 0); + int ping(const std::string joint_name, uint16_t *model_number, + uint8_t *error = 0); + + int action(const std::string joint_name); + int reboot(const std::string joint_name, uint8_t *error = 0); + int factoryReset(const std::string joint_name, uint8_t option = 0, + uint8_t *error = 0); + + int read(const std::string joint_name, uint16_t address, uint16_t length, + uint8_t *data, uint8_t *error = 0); + int readCtrlItem(const std::string joint_name, const std::string item_name, + uint32_t *data, uint8_t *error = 0); + + int read1Byte(const std::string joint_name, uint16_t address, uint8_t *data, + uint8_t *error = 0); + int read2Byte(const std::string joint_name, uint16_t address, uint16_t *data, + uint8_t *error = 0); + int read4Byte(const std::string joint_name, uint16_t address, uint32_t *data, + uint8_t *error = 0); + + int write(const std::string joint_name, uint16_t address, uint16_t length, + uint8_t *data, uint8_t *error = 0); + int writeCtrlItem(const std::string joint_name, const std::string item_name, + uint32_t data, uint8_t *error = 0); + + int write1Byte(const std::string joint_name, uint16_t address, uint8_t data, + uint8_t *error = 0); + int write2Byte(const std::string joint_name, uint16_t address, uint16_t data, + uint8_t *error = 0); + int write4Byte(const std::string joint_name, uint16_t address, uint32_t data, + uint8_t *error = 0); + + int regWrite(const std::string joint_name, uint16_t address, uint16_t length, + uint8_t *data, uint8_t *error = 0); +}; + +} // namespace humanoid_robot_intelligence_control_system_framework + +#endif /* ROBOTIS_CONTROLLER_ROBOTIS_CONTROLLER_H_ */ diff --git a/humanoid_robot_intelligence_control_system_controller/package.xml b/humanoid_robot_intelligence_control_system_controller/package.xml new file mode 100755 index 0000000..302552d --- /dev/null +++ b/humanoid_robot_intelligence_control_system_controller/package.xml @@ -0,0 +1,81 @@ + + + humanoid_robot_intelligence_control_system_controller + 0.3.2 + humanoid_robot_intelligence_control_system_controller package for ROBOTIS's platform like Manipulator-H, THORMANG and OP series + Apache 2.0 + Ronaldson Bellande + + + catkin + + roscpp + roslib + std_msgs + sensor_msgs + humanoid_robot_intelligence_control_system_controller_msgs + dynamixel_sdk + humanoid_robot_intelligence_control_system_device + humanoid_robot_intelligence_control_system_framework_common + cmake_modules + yaml-cpp + + roscpp + roslib + std_msgs + sensor_msgs + humanoid_robot_intelligence_control_system_controller_msgs + dynamixel_sdk + humanoid_robot_intelligence_control_system_device + humanoid_robot_intelligence_control_system_framework_common + cmake_modules + yaml-cpp + + roscpp + roslib + std_msgs + sensor_msgs + humanoid_robot_intelligence_control_system_controller_msgs + dynamixel_sdk + humanoid_robot_intelligence_control_system_device + humanoid_robot_intelligence_control_system_framework_common + cmake_modules + yaml-cpp + + + ament_cmake + + roscpp + roslib + std_msgs + sensor_msgs + humanoid_robot_intelligence_control_system_controller_msgs + dynamixel_sdk + humanoid_robot_intelligence_control_system_device + humanoid_robot_intelligence_control_system_framework_common + cmake_modules + yaml-cpp + + roscpp + roslib + std_msgs + sensor_msgs + humanoid_robot_intelligence_control_system_controller_msgs + dynamixel_sdk + humanoid_robot_intelligence_control_system_device + humanoid_robot_intelligence_control_system_framework_common + cmake_modules + yaml-cpp + + roscpp + roslib + std_msgs + sensor_msgs + humanoid_robot_intelligence_control_system_controller_msgs + dynamixel_sdk + humanoid_robot_intelligence_control_system_device + humanoid_robot_intelligence_control_system_framework_common + cmake_modules + yaml-cpp + + diff --git a/humanoid_robot_intelligence_control_system_controller/src/humanoid_robot_intelligence_control_system_controller/humanoid_robot_intelligence_control_system_controller.cpp b/humanoid_robot_intelligence_control_system_controller/src/humanoid_robot_intelligence_control_system_controller/humanoid_robot_intelligence_control_system_controller.cpp new file mode 100755 index 0000000..1204a29 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_controller/src/humanoid_robot_intelligence_control_system_controller/humanoid_robot_intelligence_control_system_controller.cpp @@ -0,0 +1,2625 @@ +/******************************************************************************* + * Copyright 2018 ROBOTIS CO., LTD. + * + * 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. + *******************************************************************************/ + +/* + * humanoid_robot_intelligence_control_system_controller.cpp + * + * Created on: 2016. 1. 15. + * Author: zerom + */ + +#include +#include + +#include "humanoid_robot_intelligence_control_system_controller/humanoid_robot_intelligence_control_system_controller.h" + +using namespace humanoid_robot_intelligence_control_system_framework; + +RobotisController::RobotisController() + : is_timer_running_(false), is_offset_enabled_(true), offset_ratio_(1.0), + stop_timer_(false), init_pose_loaded_(false), timer_thread_(0), + controller_mode_(MotionModuleMode), DEBUG_PRINT(false), robot_(0), + gazebo_mode_(false), + gazebo_robot_name_("humanoid_robot_intelligence_control_system") { + direct_sync_write_.clear(); +} + +void RobotisController::initializeSyncWrite() { + if (gazebo_mode_ == true) + return; + + // ROS_INFO("FIRST BULKREAD"); + for (auto &it : port_to_bulk_read_) + it.second->txRxPacket(); + for (auto &it : port_to_bulk_read_) { + int error_count = 0; + int result = COMM_SUCCESS; + do { + if (++error_count > 10) { + ROS_ERROR("[RobotisController] first bulk read fail!!"); + exit(-1); + } + usleep(10 * 1000); + result = it.second->txRxPacket(); + } while (result != COMM_SUCCESS); + } + init_pose_loaded_ = true; + // ROS_INFO("FIRST BULKREAD END"); + + // clear syncwrite param setting + for (auto &it : port_to_sync_write_position_) { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto &it : port_to_sync_write_position_p_gain_) { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto &it : port_to_sync_write_position_i_gain_) { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto &it : port_to_sync_write_position_d_gain_) { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto &it : port_to_sync_write_velocity_) { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto &it : port_to_sync_write_velocity_p_gain_) { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto &it : port_to_sync_write_velocity_i_gain_) { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto &it : port_to_sync_write_velocity_d_gain_) { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto &it : port_to_sync_write_current_) { + if (it.second != NULL) + it.second->clearParam(); + } + + // set init syncwrite param(from data of bulkread) + for (auto &it : robot_->dxls_) { + std::string joint_name = it.first; + Dynamixel *dxl = it.second; + + for (int i = 0; i < dxl->bulk_read_items_.size(); i++) { + uint32_t read_data = 0; + uint8_t sync_write_data[4]; + + if (port_to_bulk_read_[dxl->port_name_]->isAvailable( + dxl->id_, dxl->bulk_read_items_[i]->address_, + dxl->bulk_read_items_[i]->data_length_) == true) { + read_data = port_to_bulk_read_[dxl->port_name_]->getData( + dxl->id_, dxl->bulk_read_items_[i]->address_, + dxl->bulk_read_items_[i]->data_length_); + + sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(read_data)); + sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(read_data)); + sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(read_data)); + sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(read_data)); + + if ((dxl->present_position_item_ != 0) && + (dxl->bulk_read_items_[i]->item_name_ == + dxl->present_position_item_->item_name_)) { + dxl->dxl_state_->present_position_ = + dxl->convertValue2Radian(read_data) - + dxl->dxl_state_->position_offset_ * offset_ratio_; + dxl->dxl_state_->goal_position_ = dxl->dxl_state_->present_position_; + + port_to_sync_write_position_[dxl->port_name_]->addParam( + dxl->id_, sync_write_data); + } else if ((dxl->position_p_gain_item_ != 0) && + (dxl->bulk_read_items_[i]->item_name_ == + dxl->position_p_gain_item_->item_name_)) { + dxl->dxl_state_->position_p_gain_ = read_data; + } else if ((dxl->position_i_gain_item_ != 0) && + (dxl->bulk_read_items_[i]->item_name_ == + dxl->position_i_gain_item_->item_name_)) { + dxl->dxl_state_->position_i_gain_ = read_data; + } else if ((dxl->position_d_gain_item_ != 0) && + (dxl->bulk_read_items_[i]->item_name_ == + dxl->position_d_gain_item_->item_name_)) { + dxl->dxl_state_->position_d_gain_ = read_data; + } else if ((dxl->present_velocity_item_ != 0) && + (dxl->bulk_read_items_[i]->item_name_ == + dxl->present_velocity_item_->item_name_)) { + dxl->dxl_state_->present_velocity_ = + dxl->convertValue2Velocity(read_data); + dxl->dxl_state_->goal_velocity_ = dxl->dxl_state_->present_velocity_; + } else if ((dxl->velocity_p_gain_item_ != 0) && + (dxl->bulk_read_items_[i]->item_name_ == + dxl->velocity_p_gain_item_->item_name_)) { + dxl->dxl_state_->velocity_p_gain_ = read_data; + } else if ((dxl->velocity_i_gain_item_ != 0) && + (dxl->bulk_read_items_[i]->item_name_ == + dxl->velocity_i_gain_item_->item_name_)) { + dxl->dxl_state_->velocity_i_gain_ = read_data; + } else if ((dxl->velocity_d_gain_item_ != 0) && + (dxl->bulk_read_items_[i]->item_name_ == + dxl->velocity_d_gain_item_->item_name_)) { + dxl->dxl_state_->velocity_d_gain_ = read_data; + } else if ((dxl->present_current_item_ != 0) && + (dxl->bulk_read_items_[i]->item_name_ == + dxl->present_current_item_->item_name_)) { + dxl->dxl_state_->present_torque_ = + dxl->convertValue2Torque(read_data); + dxl->dxl_state_->goal_torque_ = dxl->dxl_state_->present_torque_; + } + } + } + } +} + +bool RobotisController::initialize(const std::string robot_file_path, + const std::string init_file_path) { + std::string dev_desc_dir_path = + ros::package::getPath( + "humanoid_robot_intelligence_control_system_device") + + "/devices"; + + // load robot info : port , device + robot_ = new Robot(robot_file_path, dev_desc_dir_path); + + if (gazebo_mode_ == true) { + queue_thread_ = + boost::thread(boost::bind(&RobotisController::msgQueueThread, this)); + return true; + } + + for (auto &it : robot_->ports_) { + std::string port_name = it.first; + dynamixel::PortHandler *port = it.second; + dynamixel::PacketHandler *default_pkt_handler = + dynamixel::PacketHandler::getPacketHandler(2.0); + + if (port->setBaudRate(port->getBaudRate()) == false) { + ROS_ERROR("PORT [%s] SETUP ERROR! (baudrate: %d)", port_name.c_str(), + port->getBaudRate()); + exit(-1); + } + + // get the default device info of the port + std::string default_device_name = robot_->port_default_device_[port_name]; + auto dxl_it = robot_->dxls_.find(default_device_name); + auto sensor_it = robot_->sensors_.find(default_device_name); + if (dxl_it != robot_->dxls_.end()) { + Dynamixel *default_device = dxl_it->second; + default_pkt_handler = dynamixel::PacketHandler::getPacketHandler( + default_device->protocol_version_); + + if (default_device->goal_position_item_ != 0) { + port_to_sync_write_position_[port_name] = new dynamixel::GroupSyncWrite( + port, default_pkt_handler, + default_device->goal_position_item_->address_, + default_device->goal_position_item_->data_length_); + } + + if (default_device->position_p_gain_item_ != 0) { + port_to_sync_write_position_p_gain_[port_name] = + new dynamixel::GroupSyncWrite( + port, default_pkt_handler, + default_device->position_p_gain_item_->address_, + default_device->position_p_gain_item_->data_length_); + } + + if (default_device->position_i_gain_item_ != 0) { + port_to_sync_write_position_i_gain_[port_name] = + new dynamixel::GroupSyncWrite( + port, default_pkt_handler, + default_device->position_i_gain_item_->address_, + default_device->position_i_gain_item_->data_length_); + } + + if (default_device->position_d_gain_item_ != 0) { + port_to_sync_write_position_d_gain_[port_name] = + new dynamixel::GroupSyncWrite( + port, default_pkt_handler, + default_device->position_d_gain_item_->address_, + default_device->position_d_gain_item_->data_length_); + } + + if (default_device->goal_velocity_item_ != 0) { + port_to_sync_write_velocity_[port_name] = new dynamixel::GroupSyncWrite( + port, default_pkt_handler, + default_device->goal_velocity_item_->address_, + default_device->goal_velocity_item_->data_length_); + } + + if (default_device->velocity_p_gain_item_ != 0) { + port_to_sync_write_velocity_p_gain_[port_name] = + new dynamixel::GroupSyncWrite( + port, default_pkt_handler, + default_device->velocity_p_gain_item_->address_, + default_device->velocity_p_gain_item_->data_length_); + } + + if (default_device->velocity_i_gain_item_ != 0) { + port_to_sync_write_velocity_i_gain_[port_name] = + new dynamixel::GroupSyncWrite( + port, default_pkt_handler, + default_device->velocity_i_gain_item_->address_, + default_device->velocity_i_gain_item_->data_length_); + } + + if (default_device->velocity_d_gain_item_ != 0) { + port_to_sync_write_velocity_d_gain_[port_name] = + new dynamixel::GroupSyncWrite( + port, default_pkt_handler, + default_device->velocity_d_gain_item_->address_, + default_device->velocity_d_gain_item_->data_length_); + } + + if (default_device->goal_current_item_ != 0) { + port_to_sync_write_current_[port_name] = new dynamixel::GroupSyncWrite( + port, default_pkt_handler, + default_device->goal_current_item_->address_, + default_device->goal_current_item_->data_length_); + } + } else if (sensor_it != robot_->sensors_.end()) { + Sensor *_default_device = sensor_it->second; + default_pkt_handler = dynamixel::PacketHandler::getPacketHandler( + _default_device->protocol_version_); + } + + port_to_bulk_read_[port_name] = + new dynamixel::GroupBulkRead(port, default_pkt_handler); + } + + // (for loop) check all dxls are connected. + for (auto &it : robot_->dxls_) { + std::string joint_name = it.first; + Dynamixel *dxl = it.second; + + if (ping(joint_name) != 0) { + usleep(10 * 1000); + if (ping(joint_name) != 0) + ROS_ERROR("JOINT[%s] does NOT respond!!", joint_name.c_str()); + } + } + + initializeDevice(init_file_path); + + queue_thread_ = + boost::thread(boost::bind(&RobotisController::msgQueueThread, this)); + return true; +} + +void RobotisController::initializeDevice(const std::string init_file_path) { + // device initialize + if (DEBUG_PRINT) + ROS_WARN("INIT FILE LOAD"); + + YAML::Node doc; + try { + doc = YAML::LoadFile(init_file_path.c_str()); + + for (YAML::const_iterator it_doc = doc.begin(); it_doc != doc.end(); + it_doc++) { + std::string joint_name = it_doc->first.as(); + + YAML::Node joint_node = doc[joint_name]; + if (joint_node.size() == 0) + continue; + + Dynamixel *dxl = NULL; + auto dxl_it = robot_->dxls_.find(joint_name); + if (dxl_it != robot_->dxls_.end()) + dxl = dxl_it->second; + + if (dxl == NULL) { + ROS_WARN("Joint [%s] was not found.", joint_name.c_str()); + continue; + } + if (DEBUG_PRINT) + ROS_INFO("JOINT_NAME: %s", joint_name.c_str()); + + uint8_t torque_enabled = 0; + read1Byte(joint_name, dxl->torque_enable_item_->address_, + &torque_enabled); + + for (YAML::const_iterator it_joint = joint_node.begin(); + it_joint != joint_node.end(); it_joint++) { + std::string item_name = it_joint->first.as(); + + if (DEBUG_PRINT) + ROS_INFO(" ITEM_NAME: %s", item_name.c_str()); + + uint32_t value = it_joint->second.as(); + + ControlTableItem *item = dxl->ctrl_table_[item_name]; + if (item == NULL) { + ROS_WARN("Control Item [%s] was not found.", item_name.c_str()); + continue; + } + + if (item->memory_type_ == EEPROM) { + uint8_t data8 = 0; + uint16_t data16 = 0; + uint32_t data32 = 0; + + switch (item->data_length_) { + case 1: + read1Byte(joint_name, item->address_, &data8); + if (data8 == value) + continue; + break; + case 2: + read2Byte(joint_name, item->address_, &data16); + if (data16 == value) + continue; + break; + case 4: + read4Byte(joint_name, item->address_, &data32); + if (data32 == value) + continue; + break; + default: + break; + } + + if (torque_enabled == 1) { + ROS_ERROR( + "################\nThe initial value of the EEPROM area has " + "been changed. \nTurn off Torque Enable and try again."); + exit(-1); + } + } + + switch (item->data_length_) { + case 1: + write1Byte(joint_name, item->address_, (uint8_t)value); + break; + case 2: + write2Byte(joint_name, item->address_, (uint16_t)value); + break; + case 4: + write4Byte(joint_name, item->address_, value); + break; + default: + break; + } + + if (item->memory_type_ == EEPROM) { + // Write to EEPROM -> delay is required (max delay: 55 msec per byte) + usleep(item->data_length_ * 55 * 1000); + } + } + } + } catch (const std::exception &e) { + ROS_INFO("Dynamixel Init file not found."); + } + + // [ BulkRead ] StartAddress : Present Position , Length : 10 ( + // Position/Velocity/Current ) + for (auto &it : robot_->ports_) { + if (port_to_bulk_read_[it.first] != 0) + port_to_bulk_read_[it.first]->clearParam(); + } + for (auto &it : robot_->dxls_) { + std::string joint_name = it.first; + Dynamixel *dxl = it.second; + + if (dxl == NULL) + continue; + + int bulkread_start_addr = 0; + int bulkread_data_length = 0; + + // // bulk read default : present position + // if(dxl->present_position_item != 0) + // { + // bulkread_start_addr = dxl->present_position_item->address; + // bulkread_data_length = dxl->present_position_item->data_length; + // } + + uint8_t torque_enabled = 0; + read1Byte(joint_name, dxl->torque_enable_item_->address_, &torque_enabled); + + // calculate bulk read start address & data length + auto indirect_addr_it = dxl->ctrl_table_.find(INDIRECT_ADDRESS_1); + if (indirect_addr_it != dxl->ctrl_table_.end()) // INDIRECT_ADDRESS_1 exist + { + if (dxl->bulk_read_items_.size() != 0) { + uint16_t data16 = 0; + + bulkread_start_addr = dxl->bulk_read_items_[0]->address_; + bulkread_data_length = 0; + + // set indirect address + int indirect_addr = indirect_addr_it->second->address_; + for (int i = 0; i < dxl->bulk_read_items_.size(); i++) { + int addr_leng = dxl->bulk_read_items_[i]->data_length_; + + bulkread_data_length += addr_leng; + for (int l = 0; l < addr_leng; l++) { + // ROS_WARN("[%12s] INDIR_ADDR: %d, ITEM_ADDR: %d", + // joint_name.c_str(), indirect_addr, + // dxl->ctrl_table[dxl->bulk_read_items[i]->item_name]->address + + // _l); + + read2Byte(joint_name, indirect_addr, &data16); + if (data16 != dxl->ctrl_table_[dxl->bulk_read_items_[i]->item_name_] + ->address_ + + l) { + if (torque_enabled == 1) { + ROS_ERROR("################\nThe indirect address of the " + "EEPROM area has been changed. \nTurn off Torque " + "Enable and try again."); + exit(-1); + } + write2Byte(joint_name, indirect_addr, + dxl->ctrl_table_[dxl->bulk_read_items_[i]->item_name_] + ->address_ + + l); + } + indirect_addr += 2; + } + } + } + } else // INDIRECT_ADDRESS_1 NOT exist + { + if (dxl->bulk_read_items_.size() != 0) { + bulkread_start_addr = dxl->bulk_read_items_[0]->address_; + bulkread_data_length = 0; + + ControlTableItem *last_item = dxl->bulk_read_items_[0]; + + for (int i = 0; i < dxl->bulk_read_items_.size(); i++) { + int addr = dxl->bulk_read_items_[i]->address_; + if (addr < bulkread_start_addr) + bulkread_start_addr = addr; + else if (last_item->address_ < addr) + last_item = dxl->bulk_read_items_[i]; + } + + bulkread_data_length = + last_item->address_ - bulkread_start_addr + last_item->data_length_; + } + } + + // ROS_WARN("[%12s] start_addr: %d, data_length: %d", joint_name.c_str(), + // bulkread_start_addr, bulkread_data_length); + if (bulkread_start_addr != 0) + port_to_bulk_read_[dxl->port_name_]->addParam( + dxl->id_, bulkread_start_addr, bulkread_data_length); + + // Torque ON + if (writeCtrlItem(joint_name, dxl->torque_enable_item_->item_name_, 1) != + COMM_SUCCESS) + writeCtrlItem(joint_name, dxl->torque_enable_item_->item_name_, 1); + } + + for (auto &it : robot_->sensors_) { + std::string sensor_name = it.first; + Sensor *sensor = it.second; + + if (sensor == NULL) + continue; + + int bulkread_start_addr = 0; + int bulkread_data_length = 0; + + // calculate bulk read start address & data length + auto indirect_addr_it = sensor->ctrl_table_.find(INDIRECT_ADDRESS_1); + if (indirect_addr_it != + sensor->ctrl_table_.end()) // INDIRECT_ADDRESS_1 exist + { + if (sensor->bulk_read_items_.size() != 0) { + uint16_t data16 = 0; + + bulkread_start_addr = sensor->bulk_read_items_[0]->address_; + bulkread_data_length = 0; + + // set indirect address + int indirect_addr = indirect_addr_it->second->address_; + for (int i = 0; i < sensor->bulk_read_items_.size(); i++) { + int addr_leng = sensor->bulk_read_items_[i]->data_length_; + + bulkread_data_length += addr_leng; + for (int l = 0; l < addr_leng; l++) { + // ROS_WARN("[%12s] INDIR_ADDR: %d, ITEM_ADDR: %d", + // sensor_name.c_str(), indirect_addr, + // sensor->ctrl_table[sensor->bulk_read_items[i]->item_name]->address + // + _l); + read2Byte(sensor_name, indirect_addr, &data16); + if (data16 != + sensor->ctrl_table_[sensor->bulk_read_items_[i]->item_name_] + ->address_ + + l) { + write2Byte( + sensor_name, indirect_addr, + sensor->ctrl_table_[sensor->bulk_read_items_[i]->item_name_] + ->address_ + + l); + } + indirect_addr += 2; + } + } + } + } else // INDIRECT_ADDRESS_1 NOT exist + { + if (sensor->bulk_read_items_.size() != 0) { + bulkread_start_addr = sensor->bulk_read_items_[0]->address_; + bulkread_data_length = 0; + + ControlTableItem *last_item = sensor->bulk_read_items_[0]; + + for (int i = 0; i < sensor->bulk_read_items_.size(); i++) { + int addr = sensor->bulk_read_items_[i]->address_; + if (addr < bulkread_start_addr) + bulkread_start_addr = addr; + else if (last_item->address_ < addr) + last_item = sensor->bulk_read_items_[i]; + } + + bulkread_data_length = + last_item->address_ - bulkread_start_addr + last_item->data_length_; + } + } + + // ROS_WARN("[%12s] start_addr: %d, data_length: %d", sensor_name.c_str(), + // bulkread_start_addr, bulkread_data_length); + if (bulkread_start_addr != 0) + port_to_bulk_read_[sensor->port_name_]->addParam( + sensor->id_, bulkread_start_addr, bulkread_data_length); + } +} + +void RobotisController::gazeboTimerThread() { + ros::Rate gazebo_rate(1000 / robot_->getControlCycle()); + + while (!stop_timer_) { + if (init_pose_loaded_ == true) + process(); + gazebo_rate.sleep(); + } +} + +void RobotisController::msgQueueThread() { + ros::NodeHandle ros_node; + ros::CallbackQueue callback_queue; + + ros_node.setCallbackQueue(&callback_queue); + + /* subscriber */ + ros::Subscriber write_control_table_sub = ros_node.subscribe( + "/humanoid_robot_intelligence_control_system/write_control_table", 5, + &RobotisController::writeControlTableCallback, this); + ros::Subscriber sync_write_item_sub = ros_node.subscribe( + "/humanoid_robot_intelligence_control_system/sync_write_item", 10, + &RobotisController::syncWriteItemCallback, this); + ros::Subscriber joint_ctrl_modules_sub = ros_node.subscribe( + "/humanoid_robot_intelligence_control_system/set_joint_ctrl_modules", 10, + &RobotisController::setJointCtrlModuleCallback, this); + ros::Subscriber enable_ctrl_module_sub = ros_node.subscribe( + "/humanoid_robot_intelligence_control_system/enable_ctrl_module", 10, + &RobotisController::setCtrlModuleCallback, this); + ros::Subscriber control_mode_sub = ros_node.subscribe( + "/humanoid_robot_intelligence_control_system/set_control_mode", 10, + &RobotisController::setControllerModeCallback, this); + ros::Subscriber joint_states_sub = ros_node.subscribe( + "/humanoid_robot_intelligence_control_system/set_joint_states", 10, + &RobotisController::setJointStatesCallback, this); + ros::Subscriber enable_offset_sub = ros_node.subscribe( + "/humanoid_robot_intelligence_control_system/enable_offset", 10, + &RobotisController::enableOffsetCallback, this); + + ros::Subscriber gazebo_joint_states_sub; + if (gazebo_mode_ == true) + gazebo_joint_states_sub = + ros_node.subscribe("/" + gazebo_robot_name_ + "/joint_states", 10, + &RobotisController::gazeboJointStatesCallback, this); + + /* publisher */ + goal_joint_state_pub_ = ros_node.advertise( + "/humanoid_robot_intelligence_control_system/goal_joint_states", 10); + present_joint_state_pub_ = ros_node.advertise( + "/humanoid_robot_intelligence_control_system/present_joint_states", 10); + current_module_pub_ = ros_node.advertise< + humanoid_robot_intelligence_control_system_controller_msgs:: + JointCtrlModule>( + "/humanoid_robot_intelligence_control_system/present_joint_ctrl_modules", + 10); + + if (gazebo_mode_ == true) { + for (auto &it : robot_->dxls_) { + gazebo_joint_position_pub_[it.first] = + ros_node.advertise( + "/" + gazebo_robot_name_ + "/" + it.first + "_position/command", + 1); + gazebo_joint_velocity_pub_[it.first] = + ros_node.advertise( + "/" + gazebo_robot_name_ + "/" + it.first + "_velocity/command", + 1); + gazebo_joint_effort_pub_[it.first] = + ros_node.advertise( + "/" + gazebo_robot_name_ + "/" + it.first + "_effort/command", 1); + } + } + + /* service */ + ros::ServiceServer get_joint_module_server = ros_node.advertiseService( + "/humanoid_robot_intelligence_control_system/" + "get_present_joint_ctrl_modules", + &RobotisController::getJointCtrlModuleService, this); + ros::ServiceServer set_joint_module_server = ros_node.advertiseService( + "/humanoid_robot_intelligence_control_system/" + "set_present_joint_ctrl_modules", + &RobotisController::setJointCtrlModuleService, this); + ros::ServiceServer set_module_server = ros_node.advertiseService( + "/humanoid_robot_intelligence_control_system/set_present_ctrl_modules", + &RobotisController::setCtrlModuleService, this); + ros::ServiceServer load_offset_server = ros_node.advertiseService( + "/humanoid_robot_intelligence_control_system/load_offset", + &RobotisController::loadOffsetService, this); + + ros::WallDuration duration(robot_->getControlCycle() / 1000.0); + while (ros_node.ok()) + callback_queue.callAvailable(duration); +} + +void *RobotisController::timerThread(void *param) { + RobotisController *controller = (RobotisController *)param; + static struct timespec next_time; + static struct timespec curr_time; + + ROS_DEBUG("controller::thread_proc started"); + + clock_gettime(CLOCK_MONOTONIC, &next_time); + + while (!controller->stop_timer_) { + next_time.tv_sec += + (next_time.tv_nsec + controller->robot_->getControlCycle() * 1000000) / + 1000000000; + next_time.tv_nsec = + (next_time.tv_nsec + controller->robot_->getControlCycle() * 1000000) % + 1000000000; + + controller->process(); + + clock_gettime(CLOCK_MONOTONIC, &curr_time); + long delta_nsec = (next_time.tv_sec - curr_time.tv_sec) * 1000000000 + + (next_time.tv_nsec - curr_time.tv_nsec); + if (delta_nsec < -100000) { + if (controller->DEBUG_PRINT == true) { + fprintf(stderr, + "[RobotisController::ThreadProc] NEXT TIME < CURR TIME.. " + "(%f)[%ld.%09ld / %ld.%09ld]", + delta_nsec / 1000000.0, (long)next_time.tv_sec, + (long)next_time.tv_nsec, (long)curr_time.tv_sec, + (long)curr_time.tv_nsec); + } + + // next_time = curr_time + 3 msec + next_time.tv_sec = + curr_time.tv_sec + (curr_time.tv_nsec + 3000000) / 1000000000; + next_time.tv_nsec = (curr_time.tv_nsec + 3000000) % 1000000000; + } + + clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &next_time, NULL); + } + return 0; +} + +void RobotisController::startTimer() { + if (this->is_timer_running_ == true) + return; + + if (this->gazebo_mode_ == true) { + // create and start the thread + gazebo_thread_ = + boost::thread(boost::bind(&RobotisController::gazeboTimerThread, this)); + } else { + initializeSyncWrite(); + + for (auto &it : port_to_bulk_read_) { + it.second->txPacket(); + } + + usleep(8 * 1000); + + int error; + struct sched_param param; + pthread_attr_t attr; + + pthread_attr_init(&attr); + + error = pthread_attr_setschedpolicy(&attr, SCHED_RR); + if (error != 0) + ROS_ERROR("pthread_attr_setschedpolicy error = %d\n", error); + error = pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED); + if (error != 0) + ROS_ERROR("pthread_attr_setinheritsched error = %d\n", error); + + memset(¶m, 0, sizeof(param)); + param.sched_priority = 31; // RT + error = pthread_attr_setschedparam(&attr, ¶m); + if (error != 0) + ROS_ERROR("pthread_attr_setschedparam error = %d\n", error); + + // create and start the thread + if ((error = pthread_create(&this->timer_thread_, &attr, this->timerThread, + this)) != 0) { + ROS_ERROR("Creating timer thread failed!!"); + exit(-1); + } + } + + this->is_timer_running_ = true; +} + +void RobotisController::stopTimer() { + int error = 0; + + // set the flag to stop the thread + if (this->is_timer_running_) { + this->stop_timer_ = true; + + if (this->gazebo_mode_ == false) { + // wait until the thread is stopped. + if ((error = pthread_join(this->timer_thread_, NULL)) != 0) + exit(-1); + + for (auto &it : port_to_bulk_read_) { + if (it.second != NULL) + it.second->rxPacket(); + } + + for (auto &it : port_to_sync_write_position_) { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto &it : port_to_sync_write_position_p_gain_) { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto &it : port_to_sync_write_position_i_gain_) { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto &it : port_to_sync_write_position_d_gain_) { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto &it : port_to_sync_write_velocity_) { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto &it : port_to_sync_write_velocity_p_gain_) { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto &it : port_to_sync_write_velocity_i_gain_) { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto &it : port_to_sync_write_velocity_d_gain_) { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto &it : port_to_sync_write_current_) { + if (it.second != NULL) + it.second->clearParam(); + } + } else { + // wait until the thread is stopped. + gazebo_thread_.join(); + } + + this->stop_timer_ = false; + this->is_timer_running_ = false; + } +} + +bool RobotisController::isTimerRunning() { return this->is_timer_running_; } + +void RobotisController::loadOffset(const std::string path) { + YAML::Node doc; + try { + doc = YAML::LoadFile(path.c_str()); + } catch (const std::exception &e) { + ROS_WARN("Fail to load offset yaml."); + return; + } + + YAML::Node offset_node = doc["offset"]; + if (offset_node.size() == 0) + return; + + ROS_INFO("Load offsets..."); + for (YAML::const_iterator it = offset_node.begin(); it != offset_node.end(); + it++) { + std::string joint_name = it->first.as(); + double offset = it->second.as(); + + auto dxl_it = robot_->dxls_.find(joint_name); + if (dxl_it != robot_->dxls_.end()) + dxl_it->second->dxl_state_->position_offset_ = offset; + } +} + +void RobotisController::process() { + // avoid duplicated function call + static bool is_process_running = false; + if (is_process_running == true) + return; + is_process_running = true; + + // ROS_INFO("Controller::Process()"); + // offset ratio + if (is_offset_enabled_) { + if (offset_ratio_ < 1.0) + offset_ratio_ += 0.01; + else + offset_ratio_ = 1.0; + } else { + if (offset_ratio_ > 0.0) + offset_ratio_ -= 0.01; + else + offset_ratio_ = 0.0; + } + + ros::Time start_time; + ros::Duration time_duration; + + if (DEBUG_PRINT) + start_time = ros::Time::now(); + + sensor_msgs::JointState goal_state; + sensor_msgs::JointState present_state; + + present_state.header.stamp = ros::Time::now(); + goal_state.header.stamp = present_state.header.stamp; + + if (controller_mode_ == MotionModuleMode) { + if (gazebo_mode_ == false) { + // BulkRead Rx + for (auto &it : port_to_bulk_read_) { + robot_->ports_[it.first]->setPacketTimeout(0.0); + if (DEBUG_PRINT) { + int result = it.second->rxPacket(); + if (result != COMM_SUCCESS) + ROS_ERROR_STREAM("Bulk Read Fail : " << it.first); + } else + it.second->rxPacket(); + } + + // -> save to robot->dxls_[]->dxl_state_ + if (robot_->dxls_.size() > 0) { + for (auto &dxl_it : robot_->dxls_) { + Dynamixel *dxl = dxl_it.second; + std::string port_name = dxl_it.second->port_name_; + std::string joint_name = dxl_it.first; + + if (dxl->bulk_read_items_.size() > 0) { + bool updated = false; + uint32_t data = 0; + for (int i = 0; i < dxl->bulk_read_items_.size(); i++) { + ControlTableItem *item = dxl->bulk_read_items_[i]; + if (port_to_bulk_read_[port_name]->isAvailable( + dxl->id_, item->address_, item->data_length_) == true) { + updated = true; + data = port_to_bulk_read_[port_name]->getData( + dxl->id_, item->address_, item->data_length_); + + // change dxl_state + if (dxl->present_position_item_ != 0 && + item->item_name_ == + dxl->present_position_item_->item_name_) { + dxl->dxl_state_->present_position_ = + dxl->convertValue2Radian(data) - + dxl->dxl_state_->position_offset_ * offset_ratio_; + } else if (dxl->present_velocity_item_ != 0 && + item->item_name_ == + dxl->present_velocity_item_->item_name_) + dxl->dxl_state_->present_velocity_ = + dxl->convertValue2Velocity(data); + else if (dxl->present_current_item_ != 0 && + item->item_name_ == + dxl->present_current_item_->item_name_) + dxl->dxl_state_->present_torque_ = + dxl->convertValue2Torque(data); + else if (dxl->goal_position_item_ != 0 && + item->item_name_ == + dxl->goal_position_item_->item_name_) { + dxl->dxl_state_->goal_position_ = + dxl->convertValue2Radian(data) - + dxl->dxl_state_->position_offset_ * offset_ratio_; + } else if (dxl->goal_velocity_item_ != 0 && + item->item_name_ == + dxl->goal_velocity_item_->item_name_) + dxl->dxl_state_->goal_velocity_ = + dxl->convertValue2Velocity(data); + else if (dxl->goal_current_item_ != 0 && + item->item_name_ == + dxl->goal_current_item_->item_name_) + dxl->dxl_state_->goal_torque_ = + dxl->convertValue2Torque(data); + + dxl->dxl_state_->bulk_read_table_[item->item_name_] = data; + } + } + + // -> update time stamp to + // Robot->dxls[]->dynamixel_state.update_time_stamp + if (updated == true) + dxl->dxl_state_->update_time_stamp_ = + TimeStamp(present_state.header.stamp.sec, + present_state.header.stamp.nsec); + } + } + } + + // -> save to robot->sensors_[]->sensor_state_ + if (robot_->sensors_.size() > 0) { + for (auto &sensor_it : robot_->sensors_) { + Sensor *sensor = sensor_it.second; + std::string port_name = sensor_it.second->port_name_; + std::string sensor_name = sensor_it.first; + + if (sensor->bulk_read_items_.size() > 0) { + bool updated = false; + uint32_t data = 0; + for (int i = 0; i < sensor->bulk_read_items_.size(); i++) { + ControlTableItem *item = sensor->bulk_read_items_[i]; + if (port_to_bulk_read_[port_name]->isAvailable( + sensor->id_, item->address_, item->data_length_) == + true) { + updated = true; + data = port_to_bulk_read_[port_name]->getData( + sensor->id_, item->address_, item->data_length_); + + // change sensor_state + sensor->sensor_state_->bulk_read_table_[item->item_name_] = + data; + } + } + + // -> update time stamp to + // Robot->dxls[]->dynamixel_state.update_time_stamp + if (updated == true) + sensor->sensor_state_->update_time_stamp_ = + TimeStamp(present_state.header.stamp.sec, + present_state.header.stamp.nsec); + } + } + } + + if (DEBUG_PRINT) { + time_duration = ros::Time::now() - start_time; + fprintf(stderr, "(%2.6f) BulkRead Rx & update state \n", + time_duration.nsec * 0.000001); + } + + // SyncWrite + queue_mutex_.lock(); + + if (direct_sync_write_.size() > 0) { + for (int i = 0; i < direct_sync_write_.size(); i++) { + direct_sync_write_[i]->txPacket(); + direct_sync_write_[i]->clearParam(); + } + direct_sync_write_.clear(); + } + + if (port_to_sync_write_position_p_gain_.size() > 0) { + for (auto &it : port_to_sync_write_position_p_gain_) { + it.second->txPacket(); + it.second->clearParam(); + } + } + if (port_to_sync_write_position_i_gain_.size() > 0) { + for (auto &it : port_to_sync_write_position_i_gain_) { + it.second->txPacket(); + it.second->clearParam(); + } + } + if (port_to_sync_write_position_d_gain_.size() > 0) { + for (auto &it : port_to_sync_write_position_d_gain_) { + it.second->txPacket(); + it.second->clearParam(); + } + } + if (port_to_sync_write_velocity_p_gain_.size() > 0) { + for (auto &it : port_to_sync_write_velocity_p_gain_) { + it.second->txPacket(); + it.second->clearParam(); + } + } + if (port_to_sync_write_velocity_i_gain_.size() > 0) { + for (auto &it : port_to_sync_write_velocity_i_gain_) { + it.second->txPacket(); + it.second->clearParam(); + } + } + if (port_to_sync_write_velocity_d_gain_.size() > 0) { + for (auto &it : port_to_sync_write_velocity_d_gain_) { + it.second->txPacket(); + it.second->clearParam(); + } + } + for (auto &it : port_to_sync_write_position_) { + if (it.second != NULL) + it.second->txPacket(); + } + for (auto &it : port_to_sync_write_velocity_) { + if (it.second != NULL) + it.second->txPacket(); + } + for (auto &it : port_to_sync_write_current_) { + if (it.second != NULL) + it.second->txPacket(); + } + + queue_mutex_.unlock(); + + // BulkRead Tx + for (auto &it : port_to_bulk_read_) + it.second->txPacket(); + + if (DEBUG_PRINT) { + time_duration = ros::Time::now() - start_time; + fprintf(stderr, "(%2.6f) SyncWrite & BulkRead Tx \n", + time_duration.nsec * 0.000001); + } + } else if (gazebo_mode_ == true) { + std_msgs::Float64 joint_msg; + + for (auto &dxl_it : robot_->dxls_) { + std::string joint_name = dxl_it.first; + Dynamixel *dxl = dxl_it.second; + DynamixelState *dxl_state = dxl_it.second->dxl_state_; + + if (dxl->ctrl_module_name_ == "none") { + joint_msg.data = dxl_state->goal_position_; + gazebo_joint_position_pub_[joint_name].publish(joint_msg); + } + } + + for (auto module_it = motion_modules_.begin(); + module_it != motion_modules_.end(); module_it++) { + if ((*module_it)->getModuleEnable() == false) + continue; + + for (auto &dxl_it : robot_->dxls_) { + std::string joint_name = dxl_it.first; + Dynamixel *dxl = dxl_it.second; + DynamixelState *dxl_state = dxl_it.second->dxl_state_; + + if (dxl->ctrl_module_name_ == (*module_it)->getModuleName()) { + if ((*module_it)->getControlMode() == PositionControl) { + joint_msg.data = dxl_state->goal_position_; + gazebo_joint_position_pub_[joint_name].publish(joint_msg); + } else if ((*module_it)->getControlMode() == VelocityControl) { + joint_msg.data = dxl_state->goal_velocity_; + gazebo_joint_velocity_pub_[joint_name].publish(joint_msg); + } else if ((*module_it)->getControlMode() == TorqueControl) { + joint_msg.data = dxl_state->goal_torque_; + gazebo_joint_effort_pub_[joint_name].publish(joint_msg); + } + } + } + } + } + } else if (controller_mode_ == DirectControlMode) { + if (gazebo_mode_ == false) { + // BulkRead Rx + for (auto &it : port_to_bulk_read_) { + robot_->ports_[it.first]->setPacketTimeout(0.0); + it.second->rxPacket(); + } + + // -> save to robot->dxls_[]->dxl_state_ + if (robot_->dxls_.size() > 0) { + for (auto &dxl_it : robot_->dxls_) { + Dynamixel *dxl = dxl_it.second; + std::string port_name = dxl_it.second->port_name_; + std::string joint_name = dxl_it.first; + + if (dxl->bulk_read_items_.size() > 0) { + uint32_t data = 0; + for (int i = 0; i < dxl->bulk_read_items_.size(); i++) { + ControlTableItem *item = dxl->bulk_read_items_[i]; + if (port_to_bulk_read_[port_name]->isAvailable( + dxl->id_, item->address_, item->data_length_) == true) { + data = port_to_bulk_read_[port_name]->getData( + dxl->id_, item->address_, item->data_length_); + + // change dxl_state + if (dxl->present_position_item_ != 0 && + item->item_name_ == + dxl->present_position_item_->item_name_) { + dxl->dxl_state_->present_position_ = + dxl->convertValue2Radian(data) - + dxl->dxl_state_->position_offset_ * offset_ratio_; + } else if (dxl->present_velocity_item_ != 0 && + item->item_name_ == + dxl->present_velocity_item_->item_name_) + dxl->dxl_state_->present_velocity_ = + dxl->convertValue2Velocity(data); + else if (dxl->present_current_item_ != 0 && + item->item_name_ == + dxl->present_current_item_->item_name_) + dxl->dxl_state_->present_torque_ = + dxl->convertValue2Torque(data); + else if (dxl->goal_position_item_ != 0 && + item->item_name_ == + dxl->goal_position_item_->item_name_) { + dxl->dxl_state_->goal_position_ = + dxl->convertValue2Radian(data) - + dxl->dxl_state_->position_offset_ * offset_ratio_; + } else if (dxl->goal_velocity_item_ != 0 && + item->item_name_ == + dxl->goal_velocity_item_->item_name_) + dxl->dxl_state_->goal_velocity_ = + dxl->convertValue2Velocity(data); + else if (dxl->goal_current_item_ != 0 && + item->item_name_ == + dxl->goal_current_item_->item_name_) + dxl->dxl_state_->goal_torque_ = + dxl->convertValue2Torque(data); + + dxl->dxl_state_->bulk_read_table_[item->item_name_] = data; + } + } + + // -> update time stamp to + // Robot->dxls[]->dynamixel_state.update_time_stamp + dxl->dxl_state_->update_time_stamp_ = + TimeStamp(present_state.header.stamp.sec, + present_state.header.stamp.nsec); + } + } + } + + queue_mutex_.lock(); + + // for (auto& it : port_to_sync_write_position_) + // { + // it.second->txPacket(); + // it.second->clearParam(); + // } + + if (direct_sync_write_.size() > 0) { + for (int i = 0; i < direct_sync_write_.size(); i++) { + direct_sync_write_[i]->txPacket(); + direct_sync_write_[i]->clearParam(); + } + direct_sync_write_.clear(); + } + + queue_mutex_.unlock(); + + // BulkRead Tx + for (auto &it : port_to_bulk_read_) + it.second->txPacket(); + } + } + + // Call SensorModule Process() + // -> for loop : call SensorModule list -> Process() + if (sensor_modules_.size() > 0) { + for (auto module_it = sensor_modules_.begin(); + module_it != sensor_modules_.end(); module_it++) { + (*module_it)->process(robot_->dxls_, robot_->sensors_); + + for (auto &it : (*module_it)->result_) + sensor_result_[it.first] = it.second; + } + } + + if (DEBUG_PRINT) { + time_duration = ros::Time::now() - start_time; + fprintf(stderr, "(%2.6f) SensorModule Process() & save result \n", + time_duration.nsec * 0.000001); + } + + if (controller_mode_ == MotionModuleMode) { + // Call MotionModule Process() + // -> for loop : call MotionModule list -> Process() + if (motion_modules_.size() > 0) { + queue_mutex_.lock(); + + for (auto module_it = motion_modules_.begin(); + module_it != motion_modules_.end(); module_it++) { + if ((*module_it)->getModuleEnable() == false) + continue; + + (*module_it)->process(robot_->dxls_, sensor_result_); + + // for loop : joint list + for (auto &dxl_it : robot_->dxls_) { + std::string joint_name = dxl_it.first; + Dynamixel *dxl = dxl_it.second; + DynamixelState *dxl_state = dxl_it.second->dxl_state_; + + if (dxl->ctrl_module_name_ == (*module_it)->getModuleName()) { + // do_sync_write = true; + DynamixelState *result_state = (*module_it)->result_[joint_name]; + + if (result_state == NULL) { + ROS_ERROR("[%s] %s ", (*module_it)->getModuleName().c_str(), + joint_name.c_str()); + continue; + } + + // TODO: check update time stamp ? + + if ((*module_it)->getControlMode() == PositionControl) { + dxl_state->goal_position_ = result_state->goal_position_; + + if (gazebo_mode_ == false) { + // add offset + uint32_t pos_data; + pos_data = dxl->convertRadian2Value( + dxl_state->goal_position_ + + dxl_state->position_offset_ * offset_ratio_); + + uint8_t sync_write_data[4] = {0}; + sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(pos_data)); + sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(pos_data)); + sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(pos_data)); + sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(pos_data)); + + if (port_to_sync_write_position_[dxl->port_name_] != NULL) + port_to_sync_write_position_[dxl->port_name_]->changeParam( + dxl->id_, sync_write_data); + + // if position p gain value is changed -> sync write + if (result_state->position_p_gain_ != NONE_GAIN && + dxl_state->position_p_gain_ != + result_state->position_p_gain_) { + dxl_state->position_p_gain_ = result_state->position_p_gain_; + uint8_t sync_write_data[4] = {0}; + sync_write_data[0] = + DXL_LOBYTE(DXL_LOWORD(dxl_state->position_p_gain_)); + sync_write_data[1] = + DXL_HIBYTE(DXL_LOWORD(dxl_state->position_p_gain_)); + sync_write_data[2] = + DXL_LOBYTE(DXL_HIWORD(dxl_state->position_p_gain_)); + sync_write_data[3] = + DXL_HIBYTE(DXL_HIWORD(dxl_state->position_p_gain_)); + + if (port_to_sync_write_position_p_gain_[dxl->port_name_] != + NULL) + port_to_sync_write_position_p_gain_[dxl->port_name_] + ->addParam(dxl->id_, sync_write_data); + } + + // if position i gain value is changed -> sync write + if (result_state->position_i_gain_ != NONE_GAIN && + dxl_state->position_i_gain_ != + result_state->position_i_gain_) { + dxl_state->position_i_gain_ = result_state->position_i_gain_; + uint8_t sync_write_data[4] = {0}; + sync_write_data[0] = + DXL_LOBYTE(DXL_LOWORD(dxl_state->position_i_gain_)); + sync_write_data[1] = + DXL_HIBYTE(DXL_LOWORD(dxl_state->position_i_gain_)); + sync_write_data[2] = + DXL_LOBYTE(DXL_HIWORD(dxl_state->position_i_gain_)); + sync_write_data[3] = + DXL_HIBYTE(DXL_HIWORD(dxl_state->position_i_gain_)); + + if (port_to_sync_write_position_i_gain_[dxl->port_name_] != + NULL) + port_to_sync_write_position_i_gain_[dxl->port_name_] + ->addParam(dxl->id_, sync_write_data); + } + + // if position d gain value is changed -> sync write + if (result_state->position_d_gain_ != NONE_GAIN && + dxl_state->position_d_gain_ != + result_state->position_d_gain_) { + dxl_state->position_d_gain_ = result_state->position_d_gain_; + uint8_t sync_write_data[4] = {0}; + sync_write_data[0] = + DXL_LOBYTE(DXL_LOWORD(dxl_state->position_d_gain_)); + sync_write_data[1] = + DXL_HIBYTE(DXL_LOWORD(dxl_state->position_d_gain_)); + sync_write_data[2] = + DXL_LOBYTE(DXL_HIWORD(dxl_state->position_d_gain_)); + sync_write_data[3] = + DXL_HIBYTE(DXL_HIWORD(dxl_state->position_d_gain_)); + + if (port_to_sync_write_position_d_gain_[dxl->port_name_] != + NULL) + port_to_sync_write_position_d_gain_[dxl->port_name_] + ->addParam(dxl->id_, sync_write_data); + } + + // if velocity p gain gain value is changed -> sync write + if (result_state->velocity_p_gain_ != NONE_GAIN && + dxl_state->velocity_p_gain_ != + result_state->velocity_p_gain_) { + dxl_state->velocity_p_gain_ = result_state->velocity_p_gain_; + uint8_t sync_write_data[4] = {0}; + sync_write_data[0] = + DXL_LOBYTE(DXL_LOWORD(dxl_state->velocity_p_gain_)); + sync_write_data[1] = + DXL_HIBYTE(DXL_LOWORD(dxl_state->velocity_p_gain_)); + sync_write_data[2] = + DXL_LOBYTE(DXL_HIWORD(dxl_state->velocity_p_gain_)); + sync_write_data[3] = + DXL_HIBYTE(DXL_HIWORD(dxl_state->velocity_p_gain_)); + + if (port_to_sync_write_velocity_p_gain_[dxl->port_name_] != + NULL) + port_to_sync_write_velocity_p_gain_[dxl->port_name_] + ->addParam(dxl->id_, sync_write_data); + } + + // if velocity i gain value is changed -> sync write + if (result_state->velocity_i_gain_ != NONE_GAIN && + dxl_state->velocity_i_gain_ != + result_state->velocity_i_gain_) { + dxl_state->velocity_i_gain_ = result_state->velocity_i_gain_; + uint8_t sync_write_data[4] = {0}; + sync_write_data[0] = + DXL_LOBYTE(DXL_LOWORD(dxl_state->velocity_i_gain_)); + sync_write_data[1] = + DXL_HIBYTE(DXL_LOWORD(dxl_state->velocity_i_gain_)); + sync_write_data[2] = + DXL_LOBYTE(DXL_HIWORD(dxl_state->velocity_i_gain_)); + sync_write_data[3] = + DXL_HIBYTE(DXL_HIWORD(dxl_state->velocity_i_gain_)); + + if (port_to_sync_write_velocity_i_gain_[dxl->port_name_] != + NULL) + port_to_sync_write_velocity_i_gain_[dxl->port_name_] + ->addParam(dxl->id_, sync_write_data); + } + } + } else if ((*module_it)->getControlMode() == VelocityControl) { + dxl_state->goal_velocity_ = result_state->goal_velocity_; + + if (gazebo_mode_ == false) { + uint32_t vel_data = + dxl->convertVelocity2Value(dxl_state->goal_velocity_); + uint8_t sync_write_data[4] = {0}; + sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(vel_data)); + sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(vel_data)); + sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(vel_data)); + sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(vel_data)); + + if (port_to_sync_write_velocity_[dxl->port_name_] != NULL) + port_to_sync_write_velocity_[dxl->port_name_]->changeParam( + dxl->id_, sync_write_data); + + // if velocity p gain gain value is changed -> sync write + if (result_state->velocity_p_gain_ != NONE_GAIN && + dxl_state->velocity_p_gain_ != + result_state->velocity_p_gain_) { + dxl_state->velocity_p_gain_ = result_state->velocity_p_gain_; + uint8_t sync_write_data[4] = {0}; + sync_write_data[0] = + DXL_LOBYTE(DXL_LOWORD(dxl_state->velocity_p_gain_)); + sync_write_data[1] = + DXL_HIBYTE(DXL_LOWORD(dxl_state->velocity_p_gain_)); + sync_write_data[2] = + DXL_LOBYTE(DXL_HIWORD(dxl_state->velocity_p_gain_)); + sync_write_data[3] = + DXL_HIBYTE(DXL_HIWORD(dxl_state->velocity_p_gain_)); + + if (port_to_sync_write_velocity_p_gain_[dxl->port_name_] != + NULL) + port_to_sync_write_velocity_p_gain_[dxl->port_name_] + ->addParam(dxl->id_, sync_write_data); + } + + // if velocity i gain value is changed -> sync write + if (result_state->velocity_i_gain_ != NONE_GAIN && + dxl_state->velocity_i_gain_ != + result_state->velocity_i_gain_) { + dxl_state->velocity_i_gain_ = result_state->velocity_i_gain_; + uint8_t sync_write_data[4] = {0}; + sync_write_data[0] = + DXL_LOBYTE(DXL_LOWORD(dxl_state->velocity_i_gain_)); + sync_write_data[1] = + DXL_HIBYTE(DXL_LOWORD(dxl_state->velocity_i_gain_)); + sync_write_data[2] = + DXL_LOBYTE(DXL_HIWORD(dxl_state->velocity_i_gain_)); + sync_write_data[3] = + DXL_HIBYTE(DXL_HIWORD(dxl_state->velocity_i_gain_)); + + if (port_to_sync_write_velocity_i_gain_[dxl->port_name_] != + NULL) + port_to_sync_write_velocity_i_gain_[dxl->port_name_] + ->addParam(dxl->id_, sync_write_data); + } + + // if velocity d gain value is changed -> sync write + if (result_state->velocity_d_gain_ != NONE_GAIN && + dxl_state->velocity_d_gain_ != + result_state->velocity_d_gain_) { + dxl_state->velocity_d_gain_ = result_state->velocity_d_gain_; + uint8_t sync_write_data[4] = {0}; + sync_write_data[0] = + DXL_LOBYTE(DXL_LOWORD(dxl_state->velocity_d_gain_)); + sync_write_data[1] = + DXL_HIBYTE(DXL_LOWORD(dxl_state->velocity_d_gain_)); + sync_write_data[2] = + DXL_LOBYTE(DXL_HIWORD(dxl_state->velocity_d_gain_)); + sync_write_data[3] = + DXL_HIBYTE(DXL_HIWORD(dxl_state->velocity_d_gain_)); + + if (port_to_sync_write_velocity_d_gain_[dxl->port_name_] != + NULL) + port_to_sync_write_velocity_d_gain_[dxl->port_name_] + ->addParam(dxl->id_, sync_write_data); + } + } + } else if ((*module_it)->getControlMode() == TorqueControl) { + dxl_state->goal_torque_ = result_state->goal_torque_; + + if (gazebo_mode_ == false) { + uint32_t curr_data = + dxl->convertTorque2Value(dxl_state->goal_torque_); + uint8_t sync_write_data[2] = {0}; + sync_write_data[0] = DXL_LOBYTE(curr_data); + sync_write_data[1] = DXL_HIBYTE(curr_data); + + if (port_to_sync_write_current_[dxl->port_name_] != NULL) + port_to_sync_write_current_[dxl->port_name_]->changeParam( + dxl->id_, sync_write_data); + } + } + } + } + } + + queue_mutex_.unlock(); + } + + if (DEBUG_PRINT) { + time_duration = ros::Time::now() - start_time; + fprintf(stderr, "(%2.6f) MotionModule Process() & save result \n", + time_duration.nsec * 0.000001); + } + } + + // publish present & goal position + for (auto &dxl_it : robot_->dxls_) { + std::string joint_name = dxl_it.first; + Dynamixel *dxl = dxl_it.second; + + present_state.name.push_back(joint_name); + present_state.position.push_back(dxl->dxl_state_->present_position_); + present_state.velocity.push_back(dxl->dxl_state_->present_velocity_); + present_state.effort.push_back(dxl->dxl_state_->present_torque_); + + goal_state.name.push_back(joint_name); + goal_state.position.push_back(dxl->dxl_state_->goal_position_); + goal_state.velocity.push_back(dxl->dxl_state_->goal_velocity_); + goal_state.effort.push_back(dxl->dxl_state_->goal_torque_); + } + + // -> publish present joint_states & goal joint states topic + present_joint_state_pub_.publish(present_state); + goal_joint_state_pub_.publish(goal_state); + + if (DEBUG_PRINT) { + time_duration = ros::Time::now() - start_time; + fprintf(stderr, "(%2.6f) Process() DONE \n", time_duration.nsec * 0.000001); + } + + is_process_running = false; +} + +void RobotisController::addMotionModule(MotionModule *module) { + // check whether the module name already exists + for (auto m_it = motion_modules_.begin(); m_it != motion_modules_.end(); + m_it++) { + if ((*m_it)->getModuleName() == module->getModuleName()) { + ROS_ERROR("Motion Module Name [%s] already exist !!", + module->getModuleName().c_str()); + return; + } + } + + module->initialize(robot_->getControlCycle(), robot_); + motion_modules_.push_back(module); + motion_modules_.unique(); +} + +void RobotisController::removeMotionModule(MotionModule *module) { + motion_modules_.remove(module); +} + +void RobotisController::addSensorModule(SensorModule *module) { + // check whether the module name already exists + for (auto m_it = sensor_modules_.begin(); m_it != sensor_modules_.end(); + m_it++) { + if ((*m_it)->getModuleName() == module->getModuleName()) { + ROS_ERROR("Sensor Module Name [%s] already exist !!", + module->getModuleName().c_str()); + return; + } + } + + module->initialize(robot_->getControlCycle(), robot_); + sensor_modules_.push_back(module); + sensor_modules_.unique(); +} + +void RobotisController::removeSensorModule(SensorModule *module) { + sensor_modules_.remove(module); +} + +void RobotisController::writeControlTableCallback( + const humanoid_robot_intelligence_control_system_controller_msgs:: + WriteControlTable::ConstPtr &msg) { + Device *device = NULL; + + if (DEBUG_PRINT) + fprintf(stderr, "[WriteControlTable] led control msg received\n"); + + auto dev_it1 = robot_->dxls_.find(msg->joint_name); + if (dev_it1 != robot_->dxls_.end()) { + device = dev_it1->second; + } else { + auto dev_it2 = robot_->sensors_.find(msg->joint_name); + if (dev_it2 != robot_->sensors_.end()) { + device = dev_it2->second; + } else { + ROS_WARN("[WriteControlTable] Unknown device : %s", + msg->joint_name.c_str()); + return; + } + } + + ControlTableItem *item = NULL; + auto item_it = device->ctrl_table_.find(msg->start_item_name); + if (item_it != device->ctrl_table_.end()) { + item = item_it->second; + } else { + ROS_WARN("[WriteControlTable] Unknown item : %s", + msg->start_item_name.c_str()); + return; + } + + dynamixel::PortHandler *port = robot_->ports_[device->port_name_]; + dynamixel::PacketHandler *packet_handler = + dynamixel::PacketHandler::getPacketHandler(device->protocol_version_); + + if (item->access_type_ == Read) + return; + + queue_mutex_.lock(); + + direct_sync_write_.push_back(new dynamixel::GroupSyncWrite( + port, packet_handler, item->address_, msg->data_length)); + direct_sync_write_[direct_sync_write_.size() - 1]->addParam( + device->id_, (uint8_t *)(msg->data.data())); + + // fprintf(stderr, "[WriteControlTable] %s -> %s : ", + // msg->joint_name.c_str(), msg->start_item_name.c_str()); for (auto &dt : + // msg->data) + // fprintf(stderr, "%02X ", dt); + // fprintf(stderr, "\n"); + + queue_mutex_.unlock(); +} + +void RobotisController::syncWriteItemCallback( + const humanoid_robot_intelligence_control_system_controller_msgs:: + SyncWriteItem::ConstPtr &msg) { + for (int i = 0; i < msg->joint_name.size(); i++) { + Device *device; + + auto d_it1 = robot_->dxls_.find(msg->joint_name[i]); + if (d_it1 != robot_->dxls_.end()) { + device = d_it1->second; + } else { + auto d_it2 = robot_->sensors_.find(msg->joint_name[i]); + if (d_it2 != robot_->sensors_.end()) { + device = d_it2->second; + } else { + ROS_WARN("[SyncWriteItem] Unknown device : %s", + msg->joint_name[i].c_str()); + continue; + } + } + + // ControlTableItem *item = device->ctrl_table_[msg->item_name]; + ControlTableItem *item = NULL; + auto item_it = device->ctrl_table_.find(msg->item_name); + if (item_it != device->ctrl_table_.end()) { + item = item_it->second; + } else { + ROS_WARN("SyncWriteItem] Unknown item : %s", msg->item_name.c_str()); + continue; + } + + dynamixel::PortHandler *port = robot_->ports_[device->port_name_]; + dynamixel::PacketHandler *packet_handler = + dynamixel::PacketHandler::getPacketHandler(device->protocol_version_); + + if (item->access_type_ == Read) + continue; + + queue_mutex_.lock(); + + int idx = 0; + if (direct_sync_write_.size() == 0) { + direct_sync_write_.push_back(new dynamixel::GroupSyncWrite( + port, packet_handler, item->address_, item->data_length_)); + idx = 0; + } else { + for (idx = 0; idx < direct_sync_write_.size(); idx++) { + if (direct_sync_write_[idx]->getPortHandler() == port && + direct_sync_write_[idx]->getPacketHandler() == packet_handler) + break; + } + + if (idx == direct_sync_write_.size()) + direct_sync_write_.push_back(new dynamixel::GroupSyncWrite( + port, packet_handler, item->address_, item->data_length_)); + } + + uint8_t *data = new uint8_t[item->data_length_]; + if (item->data_length_ == 1) + data[0] = (uint8_t)msg->value[i]; + else if (item->data_length_ == 2) { + data[0] = DXL_LOBYTE((uint16_t)msg->value[i]); + data[1] = DXL_HIBYTE((uint16_t)msg->value[i]); + } else if (item->data_length_ == 4) { + data[0] = DXL_LOBYTE(DXL_LOWORD((uint32_t)msg->value[i])); + data[1] = DXL_HIBYTE(DXL_LOWORD((uint32_t)msg->value[i])); + data[2] = DXL_LOBYTE(DXL_HIWORD((uint32_t)msg->value[i])); + data[3] = DXL_HIBYTE(DXL_HIWORD((uint32_t)msg->value[i])); + } + direct_sync_write_[idx]->addParam(device->id_, data); + delete[] data; + + queue_mutex_.unlock(); + } +} + +void RobotisController::setControllerModeCallback( + const std_msgs::String::ConstPtr &msg) { + if (msg->data == "DirectControlMode") { + for (auto &it : port_to_bulk_read_) { + robot_->ports_[it.first]->setPacketTimeout(0.0); + it.second->rxPacket(); + } + controller_mode_ = DirectControlMode; + } else if (msg->data == "MotionModuleMode") { + for (auto &it : port_to_bulk_read_) { + it.second->txPacket(); + } + controller_mode_ = MotionModuleMode; + } +} + +void RobotisController::setJointStatesCallback( + const sensor_msgs::JointState::ConstPtr &msg) { + queue_mutex_.lock(); + + for (int i = 0; i < msg->name.size(); i++) { + Dynamixel *dxl = robot_->dxls_[msg->name[i]]; + if (dxl == NULL) + continue; + + if ((controller_mode_ == DirectControlMode) || + (controller_mode_ == MotionModuleMode && + dxl->ctrl_module_name_ == "none")) { + dxl->dxl_state_->goal_position_ = (double)msg->position[i]; + + if (gazebo_mode_ == false) { + // add offset + uint32_t pos_data; + pos_data = dxl->convertRadian2Value(dxl->dxl_state_->goal_position_ + + dxl->dxl_state_->position_offset_ * + offset_ratio_); + + uint8_t sync_write_data[4] = {0}; + sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(pos_data)); + sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(pos_data)); + sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(pos_data)); + sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(pos_data)); + + if (port_to_sync_write_position_[dxl->port_name_] != NULL) + port_to_sync_write_position_[dxl->port_name_]->changeParam( + dxl->id_, sync_write_data); + } + } + } + + queue_mutex_.unlock(); +} + +void RobotisController::setCtrlModuleCallback( + const std_msgs::String::ConstPtr &msg) { + if (set_module_thread_.joinable()) + set_module_thread_.join(); + + std::string _module_name_to_set = msg->data; + + set_module_thread_ = boost::thread(boost::bind( + &RobotisController::setCtrlModuleThread, this, _module_name_to_set)); +} + +void RobotisController::setCtrlModule(std::string module_name) { + if (set_module_thread_.joinable()) + set_module_thread_.join(); + + set_module_thread_ = boost::thread( + boost::bind(&RobotisController::setCtrlModuleThread, this, module_name)); +} +void RobotisController::setJointCtrlModuleCallback( + const humanoid_robot_intelligence_control_system_controller_msgs:: + JointCtrlModule::ConstPtr &msg) { + if (msg->joint_name.size() != msg->module_name.size()) + return; + + if (set_module_thread_.joinable()) + set_module_thread_.join(); + + set_module_thread_ = boost::thread( + boost::bind(&RobotisController::setJointCtrlModuleThread, this, msg)); +} + +void RobotisController::enableOffsetCallback( + const std_msgs::Bool::ConstPtr &msg) { + is_offset_enabled_ = (bool)msg->data; + if (is_offset_enabled_) + offset_ratio_ = 0.0; + else + offset_ratio_ = 1.0; +} + +bool RobotisController::getJointCtrlModuleService( + humanoid_robot_intelligence_control_system_controller_msgs::GetJointModule:: + Request &req, + humanoid_robot_intelligence_control_system_controller_msgs::GetJointModule:: + Response &res) { + for (unsigned int idx = 0; idx < req.joint_name.size(); idx++) { + auto d_it = robot_->dxls_.find((std::string)(req.joint_name[idx])); + if (d_it != robot_->dxls_.end()) { + res.joint_name.push_back(req.joint_name[idx]); + res.module_name.push_back(d_it->second->ctrl_module_name_); + } + } + + if (res.joint_name.size() == 0) + return false; + + return true; +} + +bool RobotisController::setJointCtrlModuleService( + humanoid_robot_intelligence_control_system_controller_msgs::SetJointModule:: + Request &req, + humanoid_robot_intelligence_control_system_controller_msgs::SetJointModule:: + Response &res) { + if (set_module_thread_.joinable()) + set_module_thread_.join(); + + humanoid_robot_intelligence_control_system_controller_msgs::JointCtrlModule + modules; + modules.joint_name = req.joint_name; + modules.module_name = req.module_name; + + humanoid_robot_intelligence_control_system_controller_msgs::JointCtrlModule:: + ConstPtr msg_ptr( + new humanoid_robot_intelligence_control_system_controller_msgs:: + JointCtrlModule(modules)); + + if (modules.joint_name.size() != modules.module_name.size()) + return false; + + set_module_thread_ = boost::thread( + boost::bind(&RobotisController::setJointCtrlModuleThread, this, msg_ptr)); + + set_module_thread_.join(); + + return true; +} + +bool RobotisController::setCtrlModuleService( + humanoid_robot_intelligence_control_system_controller_msgs::SetModule:: + Request &req, + humanoid_robot_intelligence_control_system_controller_msgs::SetModule:: + Response &res) { + if (set_module_thread_.joinable()) + set_module_thread_.join(); + + std::string _module_name_to_set = req.module_name; + + set_module_thread_ = boost::thread(boost::bind( + &RobotisController::setCtrlModuleThread, this, _module_name_to_set)); + + set_module_thread_.join(); + + res.result = true; + return true; +} + +bool RobotisController::loadOffsetService( + humanoid_robot_intelligence_control_system_controller_msgs::LoadOffset:: + Request &req, + humanoid_robot_intelligence_control_system_controller_msgs::LoadOffset:: + Response &res) { + loadOffset((std::string)req.file_path); + res.result = true; + return true; +} + +void RobotisController::setJointCtrlModuleThread( + const humanoid_robot_intelligence_control_system_controller_msgs:: + JointCtrlModule::ConstPtr &msg) { + // stop module list + std::list _stop_modules; + std::list _enable_modules; + + for (unsigned int idx = 0; idx < msg->joint_name.size(); idx++) { + Dynamixel *_dxl = NULL; + std::map::iterator _dxl_it = + robot_->dxls_.find((std::string)(msg->joint_name[idx])); + if (_dxl_it != robot_->dxls_.end()) + _dxl = _dxl_it->second; + else + continue; + + // enqueue + if (_dxl->ctrl_module_name_ != msg->module_name[idx]) { + for (std::list::iterator _stop_m_it = + motion_modules_.begin(); + _stop_m_it != motion_modules_.end(); _stop_m_it++) { + if ((*_stop_m_it)->getModuleName() == _dxl->ctrl_module_name_ && + (*_stop_m_it)->getModuleEnable() == true) + _stop_modules.push_back(*_stop_m_it); + } + } + } + + // stop the module + _stop_modules.unique(); + for (std::list::iterator _stop_m_it = _stop_modules.begin(); + _stop_m_it != _stop_modules.end(); _stop_m_it++) { + (*_stop_m_it)->stop(); + } + + // wait to stop + for (std::list::iterator _stop_m_it = _stop_modules.begin(); + _stop_m_it != _stop_modules.end(); _stop_m_it++) { + while ((*_stop_m_it)->isRunning()) + usleep(robot_->getControlCycle() * 1000); + } + + // disable module(s) + for (std::list::iterator _stop_m_it = _stop_modules.begin(); + _stop_m_it != _stop_modules.end(); _stop_m_it++) { + (*_stop_m_it)->setModuleEnable(false); + } + + // set ctrl module + queue_mutex_.lock(); + + for (unsigned int idx = 0; idx < msg->joint_name.size(); idx++) { + std::string ctrl_module = msg->module_name[idx]; + std::string joint_name = msg->joint_name[idx]; + + Dynamixel *_dxl = NULL; + std::map::iterator _dxl_it = + robot_->dxls_.find(joint_name); + if (_dxl_it != robot_->dxls_.end()) + _dxl = _dxl_it->second; + else + continue; + + // none + if (ctrl_module == "" || ctrl_module == "none") { + _dxl->ctrl_module_name_ = "none"; + + if (gazebo_mode_ == true) + continue; + + uint32_t _pos_data; + _pos_data = _dxl->convertRadian2Value(_dxl->dxl_state_->goal_position_ + + _dxl->dxl_state_->position_offset_ * + offset_ratio_); + + uint8_t _sync_write_data[4]; + _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_pos_data)); + _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_pos_data)); + _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_pos_data)); + _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_pos_data)); + + if (port_to_sync_write_position_[_dxl->port_name_] != NULL) + port_to_sync_write_position_[_dxl->port_name_]->addParam( + _dxl->id_, _sync_write_data); + + if (port_to_sync_write_current_[_dxl->port_name_] != NULL) + port_to_sync_write_current_[_dxl->port_name_]->removeParam(_dxl->id_); + if (port_to_sync_write_velocity_[_dxl->port_name_] != NULL) + port_to_sync_write_velocity_[_dxl->port_name_]->removeParam(_dxl->id_); + } else { + // check whether the module exist + for (std::list::iterator _m_it = motion_modules_.begin(); + _m_it != motion_modules_.end(); _m_it++) { + // if it exist + if ((*_m_it)->getModuleName() == ctrl_module) { + std::map::iterator _result_it = + (*_m_it)->result_.find(joint_name); + if (_result_it == (*_m_it)->result_.end()) + break; + + _dxl->ctrl_module_name_ = ctrl_module; + + // enqueue enable module list + _enable_modules.push_back(*_m_it); + ControlMode _mode = (*_m_it)->getControlMode(); + + if (gazebo_mode_ == true) + break; + + if (_mode == PositionControl) { + uint32_t _pos_data; + _pos_data = _dxl->convertRadian2Value( + _dxl->dxl_state_->goal_position_ + + _dxl->dxl_state_->position_offset_ * offset_ratio_); + + uint8_t _sync_write_data[4]; + _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_pos_data)); + _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_pos_data)); + _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_pos_data)); + _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_pos_data)); + + if (port_to_sync_write_position_[_dxl->port_name_] != NULL) + port_to_sync_write_position_[_dxl->port_name_]->addParam( + _dxl->id_, _sync_write_data); + + if (port_to_sync_write_current_[_dxl->port_name_] != NULL) + port_to_sync_write_current_[_dxl->port_name_]->removeParam( + _dxl->id_); + if (port_to_sync_write_velocity_[_dxl->port_name_] != NULL) + port_to_sync_write_velocity_[_dxl->port_name_]->removeParam( + _dxl->id_); + } else if (_mode == VelocityControl) { + uint32_t _vel_data = + _dxl->convertVelocity2Value(_dxl->dxl_state_->goal_velocity_); + uint8_t _sync_write_data[4]; + _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_vel_data)); + _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_vel_data)); + _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_vel_data)); + _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_vel_data)); + + if (port_to_sync_write_velocity_[_dxl->port_name_] != NULL) + port_to_sync_write_velocity_[_dxl->port_name_]->addParam( + _dxl->id_, _sync_write_data); + + if (port_to_sync_write_current_[_dxl->port_name_] != NULL) + port_to_sync_write_current_[_dxl->port_name_]->removeParam( + _dxl->id_); + if (port_to_sync_write_position_[_dxl->port_name_] != NULL) + port_to_sync_write_position_[_dxl->port_name_]->removeParam( + _dxl->id_); + } else if (_mode == TorqueControl) { + uint32_t _curr_data = + _dxl->convertTorque2Value(_dxl->dxl_state_->goal_torque_); + uint8_t _sync_write_data[4]; + _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_curr_data)); + _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_curr_data)); + _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_curr_data)); + _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_curr_data)); + + if (port_to_sync_write_current_[_dxl->port_name_] != NULL) + port_to_sync_write_current_[_dxl->port_name_]->addParam( + _dxl->id_, _sync_write_data); + + if (port_to_sync_write_velocity_[_dxl->port_name_] != NULL) + port_to_sync_write_velocity_[_dxl->port_name_]->removeParam( + _dxl->id_); + if (port_to_sync_write_position_[_dxl->port_name_] != NULL) + port_to_sync_write_position_[_dxl->port_name_]->removeParam( + _dxl->id_); + } + break; + } + } + } + } + + // enable module(s) + _enable_modules.unique(); + for (std::list::iterator _m_it = _enable_modules.begin(); + _m_it != _enable_modules.end(); _m_it++) { + (*_m_it)->setModuleEnable(true); + } + + // TODO: set indirect address + // -> check module's control_mode + + queue_mutex_.unlock(); + + // publish current module + humanoid_robot_intelligence_control_system_controller_msgs::JointCtrlModule + _current_module_msg; + for (std::map::iterator _dxl_iter = + robot_->dxls_.begin(); + _dxl_iter != robot_->dxls_.end(); ++_dxl_iter) { + _current_module_msg.joint_name.push_back(_dxl_iter->first); + _current_module_msg.module_name.push_back( + _dxl_iter->second->ctrl_module_name_); + } + + if (_current_module_msg.joint_name.size() == + _current_module_msg.module_name.size()) + current_module_pub_.publish(_current_module_msg); +} + +void RobotisController::setCtrlModuleThread(std::string ctrl_module) { + // stop module + std::list stop_modules; + + if (ctrl_module == "" || ctrl_module == "none") { + // enqueue all modules in order to stop + for (auto m_it = motion_modules_.begin(); m_it != motion_modules_.end(); + m_it++) { + if ((*m_it)->getModuleEnable() == true) + stop_modules.push_back(*m_it); + } + } else { + for (auto m_it = motion_modules_.begin(); m_it != motion_modules_.end(); + m_it++) { + // if it exist + if ((*m_it)->getModuleName() == ctrl_module) { + // enqueue the module which lost control of joint in order to stop + for (auto &result_it : (*m_it)->result_) { + auto d_it = robot_->dxls_.find(result_it.first); + + if (d_it != robot_->dxls_.end()) { + // enqueue + if (d_it->second->ctrl_module_name_ != ctrl_module) { + for (auto stop_m_it = motion_modules_.begin(); + stop_m_it != motion_modules_.end(); stop_m_it++) { + if (((*stop_m_it)->getModuleName() == + d_it->second->ctrl_module_name_) && + ((*stop_m_it)->getModuleEnable() == true)) { + stop_modules.push_back(*stop_m_it); + } + } + } + } + } + + break; + } + } + } + + // stop the module + stop_modules.unique(); + for (auto stop_m_it = stop_modules.begin(); stop_m_it != stop_modules.end(); + stop_m_it++) { + (*stop_m_it)->stop(); + } + + // wait to stop + for (auto stop_m_it = stop_modules.begin(); stop_m_it != stop_modules.end(); + stop_m_it++) { + while ((*stop_m_it)->isRunning()) + usleep(robot_->getControlCycle() * 1000); + } + + // disable module(s) + for (std::list::iterator _stop_m_it = stop_modules.begin(); + _stop_m_it != stop_modules.end(); _stop_m_it++) { + (*_stop_m_it)->setModuleEnable(false); + } + + // set ctrl module + queue_mutex_.lock(); + + if (DEBUG_PRINT) + ROS_INFO_STREAM("set module : " << ctrl_module); + + // none + if ((ctrl_module == "") || (ctrl_module == "none")) { + // set dxl's control module to "none" + for (auto &d_it : robot_->dxls_) { + Dynamixel *dxl = d_it.second; + dxl->ctrl_module_name_ = "none"; + + if (gazebo_mode_ == true) + continue; + + uint32_t pos_data; + pos_data = dxl->convertRadian2Value(dxl->dxl_state_->goal_position_ + + dxl->dxl_state_->position_offset_ * + offset_ratio_); + + uint8_t sync_write_data[4] = {0}; + sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(pos_data)); + sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(pos_data)); + sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(pos_data)); + sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(pos_data)); + + if (port_to_sync_write_position_[dxl->port_name_] != NULL) + port_to_sync_write_position_[dxl->port_name_]->addParam( + dxl->id_, sync_write_data); + + if (port_to_sync_write_current_[dxl->port_name_] != NULL) + port_to_sync_write_current_[dxl->port_name_]->removeParam(dxl->id_); + if (port_to_sync_write_velocity_[dxl->port_name_] != NULL) + port_to_sync_write_velocity_[dxl->port_name_]->removeParam(dxl->id_); + } + } else { + // check whether the module exist + for (auto m_it = motion_modules_.begin(); m_it != motion_modules_.end(); + m_it++) { + // if it exist + if ((*m_it)->getModuleName() == ctrl_module) { + ControlMode mode = (*m_it)->getControlMode(); + for (auto &result_it : (*m_it)->result_) { + auto d_it = robot_->dxls_.find(result_it.first); + if (d_it != robot_->dxls_.end()) { + Dynamixel *dxl = d_it->second; + dxl->ctrl_module_name_ = ctrl_module; + + if (gazebo_mode_ == true) + continue; + + if (mode == PositionControl) { + uint32_t pos_data; + pos_data = dxl->convertRadian2Value( + dxl->dxl_state_->goal_position_ + + dxl->dxl_state_->position_offset_ * offset_ratio_); + + uint8_t sync_write_data[4] = {0}; + sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(pos_data)); + sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(pos_data)); + sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(pos_data)); + sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(pos_data)); + + if (port_to_sync_write_position_[dxl->port_name_] != NULL) + port_to_sync_write_position_[dxl->port_name_]->addParam( + dxl->id_, sync_write_data); + + if (port_to_sync_write_current_[dxl->port_name_] != NULL) + port_to_sync_write_current_[dxl->port_name_]->removeParam( + dxl->id_); + if (port_to_sync_write_velocity_[dxl->port_name_] != NULL) + port_to_sync_write_velocity_[dxl->port_name_]->removeParam( + dxl->id_); + } else if (mode == VelocityControl) { + uint32_t vel_data = + dxl->convertVelocity2Value(dxl->dxl_state_->goal_velocity_); + uint8_t sync_write_data[4] = {0}; + sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(vel_data)); + sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(vel_data)); + sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(vel_data)); + sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(vel_data)); + + if (port_to_sync_write_velocity_[dxl->port_name_] != NULL) + port_to_sync_write_velocity_[dxl->port_name_]->addParam( + dxl->id_, sync_write_data); + + if (port_to_sync_write_current_[dxl->port_name_] != NULL) + port_to_sync_write_current_[dxl->port_name_]->removeParam( + dxl->id_); + if (port_to_sync_write_position_[dxl->port_name_] != NULL) + port_to_sync_write_position_[dxl->port_name_]->removeParam( + dxl->id_); + } else if (mode == TorqueControl) { + uint32_t curr_data = + dxl->convertTorque2Value(dxl->dxl_state_->goal_torque_); + uint8_t sync_write_data[4] = {0}; + sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(curr_data)); + sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(curr_data)); + sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(curr_data)); + sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(curr_data)); + + if (port_to_sync_write_current_[dxl->port_name_] != NULL) + port_to_sync_write_current_[dxl->port_name_]->addParam( + dxl->id_, sync_write_data); + + if (port_to_sync_write_velocity_[dxl->port_name_] != NULL) + port_to_sync_write_velocity_[dxl->port_name_]->removeParam( + dxl->id_); + if (port_to_sync_write_position_[dxl->port_name_] != NULL) + port_to_sync_write_position_[dxl->port_name_]->removeParam( + dxl->id_); + } + } + } + + break; + } + } + } + + for (auto m_it = motion_modules_.begin(); m_it != motion_modules_.end(); + m_it++) { + // set all used modules -> enable + for (auto &d_it : robot_->dxls_) { + if (d_it.second->ctrl_module_name_ == (*m_it)->getModuleName()) { + (*m_it)->setModuleEnable(true); + break; + } + } + } + + // TODO: set indirect address + // -> check module's control_mode + + queue_mutex_.unlock(); + + // publish current module + humanoid_robot_intelligence_control_system_controller_msgs::JointCtrlModule + current_module_msg; + for (auto &dxl_iter : robot_->dxls_) { + current_module_msg.joint_name.push_back(dxl_iter.first); + current_module_msg.module_name.push_back( + dxl_iter.second->ctrl_module_name_); + } + + if (current_module_msg.joint_name.size() == + current_module_msg.module_name.size()) + current_module_pub_.publish(current_module_msg); +} + +void RobotisController::gazeboJointStatesCallback( + const sensor_msgs::JointState::ConstPtr &msg) { + queue_mutex_.lock(); + + for (unsigned int i = 0; i < msg->name.size(); i++) { + auto d_it = robot_->dxls_.find((std::string)msg->name[i]); + if (d_it != robot_->dxls_.end()) { + d_it->second->dxl_state_->present_position_ = msg->position[i]; + d_it->second->dxl_state_->present_velocity_ = msg->velocity[i]; + d_it->second->dxl_state_->present_torque_ = msg->effort[i]; + } + } + + if (init_pose_loaded_ == false) { + for (auto &it : robot_->dxls_) + it.second->dxl_state_->goal_position_ = + it.second->dxl_state_->present_position_; + init_pose_loaded_ = true; + } + + queue_mutex_.unlock(); +} + +bool RobotisController::isTimerStopped() { + if (this->is_timer_running_) { + if (DEBUG_PRINT == true) + ROS_WARN("Process Timer is running.. STOP the timer first."); + return false; + } + return true; +} + +int RobotisController::ping(const std::string joint_name, uint8_t *error) { + return ping(joint_name, 0, error); +} +int RobotisController::ping(const std::string joint_name, + uint16_t *model_number, uint8_t *error) { + if (isTimerStopped() == false) + return COMM_PORT_BUSY; + + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; + + dynamixel::PacketHandler *pkt_handler = + dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; + + return pkt_handler->ping(port_handler, dxl->id_, model_number, error); +} + +int RobotisController::action(const std::string joint_name) { + if (isTimerStopped() == false) + return COMM_PORT_BUSY; + + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; + + dynamixel::PacketHandler *pkt_handler = + dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; + + return pkt_handler->action(port_handler, dxl->id_); +} +int RobotisController::reboot(const std::string joint_name, uint8_t *error) { + if (isTimerStopped() == false) + return COMM_PORT_BUSY; + + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; + + dynamixel::PacketHandler *pkt_handler = + dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; + + return pkt_handler->reboot(port_handler, dxl->id_, error); +} +int RobotisController::factoryReset(const std::string joint_name, + uint8_t option, uint8_t *error) { + if (isTimerStopped() == false) + return COMM_PORT_BUSY; + + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; + + dynamixel::PacketHandler *pkt_handler = + dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; + + return pkt_handler->factoryReset(port_handler, dxl->id_, option, error); +} + +int RobotisController::read(const std::string joint_name, uint16_t address, + uint16_t length, uint8_t *data, uint8_t *error) { + if (isTimerStopped() == false) + return COMM_PORT_BUSY; + + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; + + dynamixel::PacketHandler *pkt_handler = + dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; + + return pkt_handler->readTxRx(port_handler, dxl->id_, address, length, data, + error); +} + +int RobotisController::readCtrlItem(const std::string joint_name, + const std::string item_name, uint32_t *data, + uint8_t *error) { + if (isTimerStopped() == false) + return COMM_PORT_BUSY; + + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; + + ControlTableItem *item = dxl->ctrl_table_[item_name]; + if (item == NULL) + return COMM_NOT_AVAILABLE; + + dynamixel::PacketHandler *pkt_handler = + dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; + + int result = COMM_NOT_AVAILABLE; + switch (item->data_length_) { + case 1: { + uint8_t read_data = 0; + result = pkt_handler->read1ByteTxRx(port_handler, dxl->id_, item->address_, + &read_data, error); + if (result == COMM_SUCCESS) + *data = read_data; + break; + } + case 2: { + uint16_t read_data = 0; + result = pkt_handler->read2ByteTxRx(port_handler, dxl->id_, item->address_, + &read_data, error); + if (result == COMM_SUCCESS) + *data = read_data; + break; + } + case 4: { + uint32_t read_data = 0; + result = pkt_handler->read4ByteTxRx(port_handler, dxl->id_, item->address_, + &read_data, error); + if (result == COMM_SUCCESS) + *data = read_data; + break; + } + default: + break; + } + return result; +} + +int RobotisController::read1Byte(const std::string joint_name, uint16_t address, + uint8_t *data, uint8_t *error) { + if (isTimerStopped() == false) + return COMM_PORT_BUSY; + + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; + + dynamixel::PacketHandler *pkt_handler = + dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; + + return pkt_handler->read1ByteTxRx(port_handler, dxl->id_, address, data, + error); +} + +int RobotisController::read2Byte(const std::string joint_name, uint16_t address, + uint16_t *data, uint8_t *error) { + if (isTimerStopped() == false) + return COMM_PORT_BUSY; + + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; + + dynamixel::PacketHandler *pkt_handler = + dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; + + return pkt_handler->read2ByteTxRx(port_handler, dxl->id_, address, data, + error); +} + +int RobotisController::read4Byte(const std::string joint_name, uint16_t address, + uint32_t *data, uint8_t *error) { + if (isTimerStopped() == false) + return COMM_PORT_BUSY; + + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; + + dynamixel::PacketHandler *pkt_handler = + dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; + + return pkt_handler->read4ByteTxRx(port_handler, dxl->id_, address, data, + error); +} + +int RobotisController::write(const std::string joint_name, uint16_t address, + uint16_t length, uint8_t *data, uint8_t *error) { + if (isTimerStopped() == false) + return COMM_PORT_BUSY; + + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; + + dynamixel::PacketHandler *pkt_handler = + dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; + + return pkt_handler->writeTxRx(port_handler, dxl->id_, address, length, data, + error); +} + +int RobotisController::writeCtrlItem(const std::string joint_name, + const std::string item_name, uint32_t data, + uint8_t *error) { + if (isTimerStopped() == false) + return COMM_PORT_BUSY; + + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; + + ControlTableItem *item = dxl->ctrl_table_[item_name]; + if (item == NULL) + return COMM_NOT_AVAILABLE; + + dynamixel::PacketHandler *pkt_handler = + dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; + + int result = COMM_NOT_AVAILABLE; + uint8_t *write_data = new uint8_t[item->data_length_]; + if (item->data_length_ == 1) { + write_data[0] = (uint8_t)data; + result = pkt_handler->write1ByteTxRx(port_handler, dxl->id_, item->address_, + data, error); + } else if (item->data_length_ == 2) { + write_data[0] = DXL_LOBYTE((uint16_t)data); + write_data[1] = DXL_HIBYTE((uint16_t)data); + result = pkt_handler->write2ByteTxRx(port_handler, dxl->id_, item->address_, + data, error); + } else if (item->data_length_ == 4) { + write_data[0] = DXL_LOBYTE(DXL_LOWORD((uint32_t)data)); + write_data[1] = DXL_HIBYTE(DXL_LOWORD((uint32_t)data)); + write_data[2] = DXL_LOBYTE(DXL_HIWORD((uint32_t)data)); + write_data[3] = DXL_HIBYTE(DXL_HIWORD((uint32_t)data)); + result = pkt_handler->write4ByteTxRx(port_handler, dxl->id_, item->address_, + data, error); + } + delete[] write_data; + return result; +} + +int RobotisController::write1Byte(const std::string joint_name, + uint16_t address, uint8_t data, + uint8_t *error) { + if (isTimerStopped() == false) + return COMM_PORT_BUSY; + + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; + + dynamixel::PacketHandler *pkt_handler = + dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; + + return pkt_handler->write1ByteTxRx(port_handler, dxl->id_, address, data, + error); +} + +int RobotisController::write2Byte(const std::string joint_name, + uint16_t address, uint16_t data, + uint8_t *error) { + if (isTimerStopped() == false) + return COMM_PORT_BUSY; + + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; + + dynamixel::PacketHandler *pkt_handler = + dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; + + return pkt_handler->write2ByteTxRx(port_handler, dxl->id_, address, data, + error); +} + +int RobotisController::write4Byte(const std::string joint_name, + uint16_t address, uint32_t data, + uint8_t *error) { + if (isTimerStopped() == false) + return COMM_PORT_BUSY; + + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; + + dynamixel::PacketHandler *pkt_handler = + dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; + + return pkt_handler->write4ByteTxRx(port_handler, dxl->id_, address, data, + error); +} + +int RobotisController::regWrite(const std::string joint_name, uint16_t address, + uint16_t length, uint8_t *data, + uint8_t *error) { + if (isTimerStopped() == false) + return COMM_PORT_BUSY; + + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; + + dynamixel::PacketHandler *pkt_handler = + dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; + + return pkt_handler->regWriteTxRx(port_handler, dxl->id_, address, length, + data, error); +} diff --git a/humanoid_robot_intelligence_control_system_device/CHANGELOG.rst b/humanoid_robot_intelligence_control_system_device/CHANGELOG.rst new file mode 100755 index 0000000..10a041c --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/CHANGELOG.rst @@ -0,0 +1,110 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package humanoid_robot_intelligence_control_system_device +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.3.2 (2023-10-03) +------------------ +* Make it compatible for ROS1/ROS2 +* Fix bugs +* Update package.xml and CMakeList.txt for to the latest versions +* Contributors & Maintainer: Ronaldson Bellande + +0.3.1 (2023-09-27) +------------------ +* Starting from this point it under a new license +* Fix errors and Issues +* Rename Repository for a completely different purpose +* Make it compatible with ROS/ROS2 +* Upgrade version of all builds and make it more compatible +* Update package.xml and CMakeList.txt for to the latest versions +* Contributors & Maintainer: Ronaldson Bellande + +0.3.0 (2021-05-03) +------------------ +* Update package.xml and CMakeList.txt for noetic branch +* Contributors: Ronaldson Bellande + +0.2.9 (2018-03-22) +------------------ +* modified to prevent duplicate indirect address write +* fixed a bug that occure when handling bulk read item that does not exist +* Contributors: Zerom + +0.2.8 (2018-03-20) +------------------ +* added RH-P12-RN.device file +* Contributors: Zerom, Pyo + +0.2.7 (2018-03-15) +------------------ +* fixed a bug that occur when handling bulk read item that does not exist +* changed the License and package format to version 2 +* Contributors: SCH, Pyo + +0.2.6 (2017-08-09) +------------------ +* OpenCR control table item name changed. (torque_enable -> dynamixel_power) +* fixed to not update update_time_stamp\_ if bulk read fails. +* Contributors: Zerom + +0.2.5 (2017-06-09) +------------------ +* none + +0.2.4 (2017-06-07) +------------------ +* none + +0.2.3 (2017-05-23) +------------------ +* updated the cmake file for ros install +* Contributors: SCH + +0.2.2 (2017-04-24) +------------------ +* added a deivce: OpenCR +* changed to read control cycle from .robot file +* Contributors: Zerom, Kayman + +0.2.1 (2016-11-23) +------------------ +* Merge the changes and update +* mode change debugging +* - convertRadian2Value / convertValue2Radian : commented out the code that limits the maximum/minimum value. +* - modified dependency problem. +* Contributors: Jay Song, Pyo, Zerom, SCH + +0.2.0 (2016-08-31) +------------------ +* bug fixed (position pid gain & velocity pid gain sync write). +* added velocity_to_value_ratio to DXL Pro-H series. +* added velocity p/i/d gain and position i/d gain sync_write code. +* fixed humanoid_robot_intelligence_control_system_device build_depend. +* added XM-430-W210 / XM-430-W350 device file. +* rename (present_current\_ -> present_torque\_) +* modified torque control code +* added device file for MX-64 / MX-106 +* adjusted position min/max value. (MX-28, XM-430) +* Contributors: Zerom, Pyo + +0.1.1 (2016-08-18) +------------------ +* updated the package information +* Contributors: Zerom + +0.1.0 (2016-08-12) +------------------ +* first public release for Kinetic +* modified the package information for release +* develop branch -> master branch +* Setting the license to BSD. +* add SensorState + add Singleton template +* XM-430 / CM-740 device file added. + Sensor device added. +* modified. +* variable name changed. + ConvertRadian2Value / ConvertValue2Radian function bug fixed. +* added code to support the gazebo simulator +* renewal +* Contributors: Zerom diff --git a/humanoid_robot_intelligence_control_system_device/CMakeLists.txt b/humanoid_robot_intelligence_control_system_device/CMakeLists.txt new file mode 100755 index 0000000..a9c5f39 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/CMakeLists.txt @@ -0,0 +1,51 @@ +cmake_minimum_required(VERSION 3.8) +project(humanoid_robot_intelligence_control_system_device) + + +if($ENV{ROS_VERSION} EQUAL 1) + find_package(catkin REQUIRED COMPONENTS + roscpp + dynamixel_sdk + ) +else() + find_package(ament_cmake REQUIRED) +endif() + + +if($ENV{ROS_VERSION} EQUAL 1) + catkin_package( + INCLUDE_DIRS include + LIBRARIES humanoid_robot_intelligence_control_system_device + CATKIN_DEPENDS + roscpp + dynamixel_sdk + ) +endif() + + +include_directories( + include + ${catkin_INCLUDE_DIRS} +) + +add_library(humanoid_robot_intelligence_control_system_device + src/humanoid_robot_intelligence_control_system_device/robot.cpp + src/humanoid_robot_intelligence_control_system_device/sensor.cpp + src/humanoid_robot_intelligence_control_system_device/dynamixel.cpp +) +add_dependencies(humanoid_robot_intelligence_control_system_device ${catkin_EXPORTED_TARGETS}) +target_link_libraries(humanoid_robot_intelligence_control_system_device ${catkin_LIBRARIES}) + +install(TARGETS humanoid_robot_intelligence_control_system_device + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) + +install(DIRECTORY devices + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) diff --git a/humanoid_robot_intelligence_control_system_device/devices/dynamixel/GRIPPER.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/GRIPPER.device new file mode 100755 index 0000000..7e0846d --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/GRIPPER.device @@ -0,0 +1,73 @@ +[device info] +model_name = GRIPPER +device_type = dynamixel + +[type info] +value_of_0_radian_position = 0 +value_of_min_radian_position = 0 +value_of_max_radian_position = 750 +min_radian = 0 +max_radian = 1.150767 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_torque +position_d_gain_item_name = +position_i_gain_item_name = +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 30 | torque_limit | 2 | RW | EEPROM | 0 | 32767 | N + 32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 562 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 586 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N + 588 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N + 594 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N + 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y + 606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N + 610 | is_moving | 1 | R | RAM | 0 | 1 | N + 611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 621 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 623 | present_voltage | 2 | R | RAM | 0 | 500 | N + 625 | present_temperature | 1 | R | RAM | 0 | 200 | N + 626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 891 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N diff --git a/humanoid_robot_intelligence_control_system_device/devices/dynamixel/GRIPPER_TORQUE.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/GRIPPER_TORQUE.device new file mode 100755 index 0000000..208b7cb --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/GRIPPER_TORQUE.device @@ -0,0 +1,74 @@ +[device info] +model_name = GRIPPER +device_type = dynamixel + +[type info] +value_of_0_radian_position = 0 +value_of_min_radian_position = 0 +value_of_max_radian_position = 750 +min_radian = 0 +max_radian = 1.150767 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_torque +position_d_gain_item_name = +position_i_gain_item_name = +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 30 | torque_limit | 2 | RW | EEPROM | 0 | 32767 | N + 32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 562 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 586 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N + 588 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N + 590 | position_d_gain | 2 | RW | RAM | 0 | 2047 | N + 594 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N + 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y + 606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N + 610 | is_moving | 1 | R | RAM | 0 | 1 | N + 611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 621 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 623 | present_voltage | 2 | R | RAM | 0 | 500 | N + 625 | present_temperature | 1 | R | RAM | 0 | 200 | N + 626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 891 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N diff --git a/humanoid_robot_intelligence_control_system_device/devices/dynamixel/H42-20-S300-R(A).device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/H42-20-S300-R(A).device new file mode 100644 index 0000000..a6f523c --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/H42-20-S300-R(A).device @@ -0,0 +1,92 @@ +[device info] +model_name = H42-20-S300-R(A) +device_type = dynamixel + +[type info] +torque_to_current_value_ratio = 109.3747778 +velocity_to_value_ratio = 954.93 +value_of_0_radian_position = 0 +value_of_min_radian_position = -303750 +value_of_max_radian_position = 303750 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_current +position_d_gain_item_name = position_d_gain +position_i_gain_item_name = position_i_gain +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 10 | drive_mode | 1 | RW | EEPROM | 0 | 3 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 12 | secondary_id | 1 | RW | EEPROM | 0 | 255 | N + 13 | protocol_version | 1 | RW | EEPROM | 1 | 2 | N + 20 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 24 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 31 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 32 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 34 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 36 | pwm_limit | 2 | RW | EEPROM | 0 | 885 | N + 38 | current_limit | 2 | RW | EEPROM | 0 | 32767 | N + 40 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 44 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 48 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 52 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 56 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 57 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 58 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 59 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 63 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 168 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 512 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 513 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 514 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 515 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 516 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 517 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 518 | hardware_error_status | 1 | R | RAM | 0 | 31 | N + 524 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N + 526 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N + 528 | position_d_gain | 2 | RW | RAM | 0 | 2047 | N + 530 | position_i_gain | 2 | RW | RAM | 0 | 2047 | N + 532 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N + 536 | feedforward_2nd_gain | 2 | RW | RAM | 0 | 32767 | N + 538 | feedforward_1st_gain | 2 | RW | RAM | 0 | 32767 | N + 546 | bus_watchdog | 1 | RW | RAM | -1 | 127 | Y + 548 | goal_pwm | 2 | RW | RAM | -2100 | 2100 | Y + 550 | goal_current | 2 | RW | RAM | -32768 | 32767 | Y + 552 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 556 | profile_acceleration | 4 | RW | RAM | 0 | 32767 | N + 560 | profile_velocity | 4 | RW | RAM | 0 | 1023 | N + 564 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 568 | realtime_tick | 2 | R | RAM | 0 | 32767 | N + 570 | moving | 1 | R | RAM | 0 | 1 | N + 571 | moving_status | 1 | R | RAM | 0 | 255 | N + 572 | present_pwm | 2 | R | RAM | -2100 | 2100 | Y + 574 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 576 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 580 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 584 | velocity_trajectory | 4 | R | RAM | 0 | 1023 | N + 588 | position_trajectory | 4 | R | RAM | 0 | 4095 | N + 592 | present_voltage | 2 | R | RAM | 0 | 500 | N + 594 | present_temperature | 1 | R | RAM | 0 | 200 | N + 600 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 602 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 604 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 606 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N diff --git a/humanoid_robot_intelligence_control_system_device/devices/dynamixel/H42-20-S300-R.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/H42-20-S300-R.device new file mode 100755 index 0000000..ab7aecb --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/H42-20-S300-R.device @@ -0,0 +1,76 @@ +[device info] +model_name = H42-20-S300-R +device_type = dynamixel + +[type info] +torque_to_current_value_ratio = 27.15146 +velocity_to_value_ratio = 2900.59884 +value_of_0_radian_position = 0 +value_of_min_radian_position = -151900 +value_of_max_radian_position = 151900 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_torque +position_d_gain_item_name = +position_i_gain_item_name = +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 13 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 30 | torque_limit | 2 | RW | EEPROM | 0 | 32767 | N + 32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 562 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 586 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N + 588 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N + 594 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N + 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y + 606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N + 610 | is_moving | 1 | R | RAM | 0 | 1 | N + 611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 621 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 623 | present_voltage | 2 | R | RAM | 0 | 500 | N + 625 | present_temperature | 1 | R | RAM | 0 | 200 | N + 626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 891 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N diff --git a/humanoid_robot_intelligence_control_system_device/devices/dynamixel/H42P-020-S300-R.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/H42P-020-S300-R.device new file mode 100644 index 0000000..92d405a --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/H42P-020-S300-R.device @@ -0,0 +1,92 @@ +[device info] +model_name = H42P-020-S300-R +device_type = dynamixel + +[type info] +torque_to_current_value_ratio = 109.3747778 +velocity_to_value_ratio = 954.93 +value_of_0_radian_position = 0 +value_of_min_radian_position = -303750 +value_of_max_radian_position = 303750 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_current +position_d_gain_item_name = position_d_gain +position_i_gain_item_name = position_i_gain +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 10 | drive_mode | 1 | RW | EEPROM | 0 | 3 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 12 | secondary_id | 1 | RW | EEPROM | 0 | 255 | N + 13 | protocol_version | 1 | RW | EEPROM | 1 | 2 | N + 20 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 24 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 31 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 32 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 34 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 36 | pwm_limit | 2 | RW | EEPROM | 0 | 885 | N + 38 | current_limit | 2 | RW | EEPROM | 0 | 32767 | N + 40 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 44 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 48 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 52 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 56 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 57 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 58 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 59 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 63 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 168 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 512 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 513 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 514 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 515 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 516 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 517 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 518 | hardware_error_status | 1 | R | RAM | 0 | 31 | N + 524 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N + 526 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N + 528 | position_d_gain | 2 | RW | RAM | 0 | 2047 | N + 530 | position_i_gain | 2 | RW | RAM | 0 | 2047 | N + 532 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N + 536 | feedforward_2nd_gain | 2 | RW | RAM | 0 | 32767 | N + 538 | feedforward_1st_gain | 2 | RW | RAM | 0 | 32767 | N + 546 | bus_watchdog | 1 | RW | RAM | -1 | 127 | Y + 548 | goal_pwm | 2 | RW | RAM | -2100 | 2100 | Y + 550 | goal_current | 2 | RW | RAM | -32768 | 32767 | Y + 552 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 556 | profile_acceleration | 4 | RW | RAM | 0 | 32767 | N + 560 | profile_velocity | 4 | RW | RAM | 0 | 1023 | N + 564 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 568 | realtime_tick | 2 | R | RAM | 0 | 32767 | N + 570 | moving | 1 | R | RAM | 0 | 1 | N + 571 | moving_status | 1 | R | RAM | 0 | 255 | N + 572 | present_pwm | 2 | R | RAM | -2100 | 2100 | Y + 574 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 576 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 580 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 584 | velocity_trajectory | 4 | R | RAM | 0 | 1023 | N + 588 | position_trajectory | 4 | R | RAM | 0 | 4095 | N + 592 | present_voltage | 2 | R | RAM | 0 | 500 | N + 594 | present_temperature | 1 | R | RAM | 0 | 200 | N + 600 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 602 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 604 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 606 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N diff --git a/humanoid_robot_intelligence_control_system_device/devices/dynamixel/H54-100-B210-R-NR.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/H54-100-B210-R-NR.device new file mode 100755 index 0000000..42342ae --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/H54-100-B210-R-NR.device @@ -0,0 +1,76 @@ +[device info] +model_name = H54-100-B210-R-NR +device_type = dynamixel + +[type info] +torque_to_current_value_ratio = 9.09201 +velocity_to_value_ratio = 2046.2777 +value_of_0_radian_position = 0 +value_of_min_radian_position = -250950 +value_of_max_radian_position = 250950 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_torque +position_d_gain_item_name = +position_i_gain_item_name = +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 13 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 30 | torque_limit | 2 | RW | EEPROM | 0 | 32767 | N + 32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 562 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 586 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N + 588 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N + 594 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N + 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y + 606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N + 610 | is_moving | 1 | R | RAM | 0 | 1 | N + 611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 621 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 623 | present_voltage | 2 | R | RAM | 0 | 500 | N + 625 | present_temperature | 1 | R | RAM | 0 | 200 | N + 626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 891 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N diff --git a/humanoid_robot_intelligence_control_system_device/devices/dynamixel/H54-100-S500-R(A).device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/H54-100-S500-R(A).device new file mode 100644 index 0000000..fe706cc --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/H54-100-S500-R(A).device @@ -0,0 +1,92 @@ +[device info] +model_name = H54-100-S500-R(A) +device_type = dynamixel + +[type info] +torque_to_current_value_ratio = 155.658486 +velocity_to_value_ratio = 954.93 +value_of_0_radian_position = 0 +value_of_min_radian_position = -501923 +value_of_max_radian_position = 501923 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_current +position_d_gain_item_name = position_d_gain +position_i_gain_item_name = position_i_gain +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 10 | drive_mode | 1 | RW | EEPROM | 0 | 3 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 12 | secondary_id | 1 | RW | EEPROM | 0 | 255 | N + 13 | protocol_version | 1 | RW | EEPROM | 1 | 2 | N + 20 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 24 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 31 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 32 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 34 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 36 | pwm_limit | 2 | RW | EEPROM | 0 | 885 | N + 38 | current_limit | 2 | RW | EEPROM | 0 | 32767 | N + 40 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 44 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 48 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 52 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 56 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 57 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 58 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 59 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 63 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 168 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 512 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 513 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 514 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 515 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 516 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 517 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 518 | hardware_error_status | 1 | R | RAM | 0 | 31 | N + 524 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N + 526 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N + 528 | position_d_gain | 2 | RW | RAM | 0 | 2047 | N + 530 | position_i_gain | 2 | RW | RAM | 0 | 2047 | N + 532 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N + 536 | feedforward_2nd_gain | 2 | RW | RAM | 0 | 32767 | N + 538 | feedforward_1st_gain | 2 | RW | RAM | 0 | 32767 | N + 546 | bus_watchdog | 1 | RW | RAM | -1 | 127 | Y + 548 | goal_pwm | 2 | RW | RAM | -2100 | 2100 | Y + 550 | goal_current | 2 | RW | RAM | -32768 | 32767 | Y + 552 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 556 | profile_acceleration | 4 | RW | RAM | 0 | 32767 | N + 560 | profile_velocity | 4 | RW | RAM | 0 | 1023 | N + 564 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 568 | realtime_tick | 2 | R | RAM | 0 | 32767 | N + 570 | moving | 1 | R | RAM | 0 | 1 | N + 571 | moving_status | 1 | R | RAM | 0 | 255 | N + 572 | present_pwm | 2 | R | RAM | -2100 | 2100 | Y + 574 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 576 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 580 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 584 | velocity_trajectory | 4 | R | RAM | 0 | 1023 | N + 588 | position_trajectory | 4 | R | RAM | 0 | 4095 | N + 592 | present_voltage | 2 | R | RAM | 0 | 500 | N + 594 | present_temperature | 1 | R | RAM | 0 | 200 | N + 600 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 602 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 604 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 606 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N diff --git a/humanoid_robot_intelligence_control_system_device/devices/dynamixel/H54-100-S500-R.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/H54-100-S500-R.device new file mode 100755 index 0000000..b042f27 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/H54-100-S500-R.device @@ -0,0 +1,76 @@ +[device info] +model_name = H54-100-S500-R +device_type = dynamixel + +[type info] +torque_to_current_value_ratio = 9.66026 +velocity_to_value_ratio = 4793.01226 +value_of_0_radian_position = 0 +value_of_min_radian_position = -250950 +value_of_max_radian_position = 250950 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_torque +position_d_gain_item_name = +position_i_gain_item_name = +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 13 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 30 | torque_limit | 2 | RW | EEPROM | 0 | 32767 | N + 32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 562 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 586 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N + 588 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N + 594 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N + 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y + 606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N + 610 | is_moving | 1 | R | RAM | 0 | 1 | N + 611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 621 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 623 | present_voltage | 2 | R | RAM | 0 | 500 | N + 625 | present_temperature | 1 | R | RAM | 0 | 200 | N + 626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 891 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N diff --git a/humanoid_robot_intelligence_control_system_device/devices/dynamixel/H54-200-B500-R.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/H54-200-B500-R.device new file mode 100755 index 0000000..ca52162 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/H54-200-B500-R.device @@ -0,0 +1,76 @@ +[device info] +model_name = H54-200-B500-R +device_type = dynamixel + +[type info] +torque_to_current_value_ratio = 9.09201 +velocity_to_value_ratio = 4793.01226 +value_of_0_radian_position = 0 +value_of_min_radian_position = -250950 +value_of_max_radian_position = 250950 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_torque +position_d_gain_item_name = +position_i_gain_item_name = +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 13 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 30 | torque_limit | 2 | RW | EEPROM | 0 | 32767 | N + 32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 562 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 586 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N + 588 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N + 594 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N + 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y + 606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N + 610 | is_moving | 1 | R | RAM | 0 | 1 | N + 611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 621 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 623 | present_voltage | 2 | R | RAM | 0 | 500 | N + 625 | present_temperature | 1 | R | RAM | 0 | 200 | N + 626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 891 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N diff --git a/humanoid_robot_intelligence_control_system_device/devices/dynamixel/H54-200-S500-R(A).device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/H54-200-S500-R(A).device new file mode 100644 index 0000000..3cfb758 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/H54-200-S500-R(A).device @@ -0,0 +1,92 @@ +[device info] +model_name = H54-200-S500-R(A) +device_type = dynamixel + +[type info] +torque_to_current_value_ratio = 146.502114 +velocity_to_value_ratio = 954.93 +value_of_0_radian_position = 0 +value_of_min_radian_position = -501923 +value_of_max_radian_position = 501923 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_current +position_d_gain_item_name = position_d_gain +position_i_gain_item_name = position_i_gain +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 10 | drive_mode | 1 | RW | EEPROM | 0 | 3 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 12 | secondary_id | 1 | RW | EEPROM | 0 | 255 | N + 13 | protocol_version | 1 | RW | EEPROM | 1 | 2 | N + 20 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 24 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 31 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 32 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 34 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 36 | pwm_limit | 2 | RW | EEPROM | 0 | 885 | N + 38 | current_limit | 2 | RW | EEPROM | 0 | 32767 | N + 40 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 44 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 48 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 52 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 56 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 57 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 58 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 59 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 63 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 168 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 512 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 513 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 514 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 515 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 516 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 517 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 518 | hardware_error_status | 1 | R | RAM | 0 | 31 | N + 524 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N + 526 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N + 528 | position_d_gain | 2 | RW | RAM | 0 | 2047 | N + 530 | position_i_gain | 2 | RW | RAM | 0 | 2047 | N + 532 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N + 536 | feedforward_2nd_gain | 2 | RW | RAM | 0 | 32767 | N + 538 | feedforward_1st_gain | 2 | RW | RAM | 0 | 32767 | N + 546 | bus_watchdog | 1 | RW | RAM | -1 | 127 | Y + 548 | goal_pwm | 2 | RW | RAM | -2100 | 2100 | Y + 550 | goal_current | 2 | RW | RAM | -32768 | 32767 | Y + 552 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 556 | profile_acceleration | 4 | RW | RAM | 0 | 32767 | N + 560 | profile_velocity | 4 | RW | RAM | 0 | 1023 | N + 564 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 568 | realtime_tick | 2 | R | RAM | 0 | 32767 | N + 570 | moving | 1 | R | RAM | 0 | 1 | N + 571 | moving_status | 1 | R | RAM | 0 | 255 | N + 572 | present_pwm | 2 | R | RAM | -2100 | 2100 | Y + 574 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 576 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 580 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 584 | velocity_trajectory | 4 | R | RAM | 0 | 1023 | N + 588 | position_trajectory | 4 | R | RAM | 0 | 4095 | N + 592 | present_voltage | 2 | R | RAM | 0 | 500 | N + 594 | present_temperature | 1 | R | RAM | 0 | 200 | N + 600 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 602 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 604 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 606 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N diff --git a/humanoid_robot_intelligence_control_system_device/devices/dynamixel/H54-200-S500-R.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/H54-200-S500-R.device new file mode 100755 index 0000000..4f81222 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/H54-200-S500-R.device @@ -0,0 +1,76 @@ +[device info] +model_name = H54-200-S500-R +device_type = dynamixel + +[type info] +torque_to_current_value_ratio = 9.09201 +velocity_to_value_ratio = 4793.01226 +value_of_0_radian_position = 0 +value_of_min_radian_position = -250950 +value_of_max_radian_position = 250950 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_torque +position_d_gain_item_name = +position_i_gain_item_name = +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 13 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 30 | torque_limit | 2 | RW | EEPROM | 0 | 32767 | N + 32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 562 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 586 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N + 588 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N + 594 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N + 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y + 606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N + 610 | is_moving | 1 | R | RAM | 0 | 1 | N + 611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 621 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 623 | present_voltage | 2 | R | RAM | 0 | 500 | N + 625 | present_temperature | 1 | R | RAM | 0 | 200 | N + 626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 891 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N diff --git a/humanoid_robot_intelligence_control_system_device/devices/dynamixel/H54P-100-S500-R.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/H54P-100-S500-R.device new file mode 100644 index 0000000..e9661e0 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/H54P-100-S500-R.device @@ -0,0 +1,92 @@ +[device info] +model_name = H54P-100-S500-R +device_type = dynamixel + +[type info] +torque_to_current_value_ratio = 155.658486 +velocity_to_value_ratio = 954.93 +value_of_0_radian_position = 0 +value_of_min_radian_position = -501923 +value_of_max_radian_position = 501923 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_current +position_d_gain_item_name = position_d_gain +position_i_gain_item_name = position_i_gain +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 10 | drive_mode | 1 | RW | EEPROM | 0 | 3 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 12 | secondary_id | 1 | RW | EEPROM | 0 | 255 | N + 13 | protocol_version | 1 | RW | EEPROM | 1 | 2 | N + 20 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 24 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 31 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 32 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 34 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 36 | pwm_limit | 2 | RW | EEPROM | 0 | 885 | N + 38 | current_limit | 2 | RW | EEPROM | 0 | 32767 | N + 40 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 44 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 48 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 52 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 56 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 57 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 58 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 59 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 63 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 168 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 512 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 513 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 514 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 515 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 516 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 517 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 518 | hardware_error_status | 1 | R | RAM | 0 | 31 | N + 524 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N + 526 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N + 528 | position_d_gain | 2 | RW | RAM | 0 | 2047 | N + 530 | position_i_gain | 2 | RW | RAM | 0 | 2047 | N + 532 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N + 536 | feedforward_2nd_gain | 2 | RW | RAM | 0 | 32767 | N + 538 | feedforward_1st_gain | 2 | RW | RAM | 0 | 32767 | N + 546 | bus_watchdog | 1 | RW | RAM | -1 | 127 | Y + 548 | goal_pwm | 2 | RW | RAM | -2100 | 2100 | Y + 550 | goal_current | 2 | RW | RAM | -32768 | 32767 | Y + 552 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 556 | profile_acceleration | 4 | RW | RAM | 0 | 32767 | N + 560 | profile_velocity | 4 | RW | RAM | 0 | 1023 | N + 564 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 568 | realtime_tick | 2 | R | RAM | 0 | 32767 | N + 570 | moving | 1 | R | RAM | 0 | 1 | N + 571 | moving_status | 1 | R | RAM | 0 | 255 | N + 572 | present_pwm | 2 | R | RAM | -2100 | 2100 | Y + 574 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 576 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 580 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 584 | velocity_trajectory | 4 | R | RAM | 0 | 1023 | N + 588 | position_trajectory | 4 | R | RAM | 0 | 4095 | N + 592 | present_voltage | 2 | R | RAM | 0 | 500 | N + 594 | present_temperature | 1 | R | RAM | 0 | 200 | N + 600 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 602 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 604 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 606 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N diff --git a/humanoid_robot_intelligence_control_system_device/devices/dynamixel/H54P-200-B500-R.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/H54P-200-B500-R.device new file mode 100644 index 0000000..97f44c5 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/H54P-200-B500-R.device @@ -0,0 +1,92 @@ +[device info] +model_name = H54P-200-B500-R +device_type = dynamixel + +[type info] +torque_to_current_value_ratio = 146.502114 +velocity_to_value_ratio = 954.93 +value_of_0_radian_position = 0 +value_of_min_radian_position = -501923 +value_of_max_radian_position = 501923 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_current +position_d_gain_item_name = position_d_gain +position_i_gain_item_name = position_i_gain +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 10 | drive_mode | 1 | RW | EEPROM | 0 | 3 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 12 | secondary_id | 1 | RW | EEPROM | 0 | 255 | N + 13 | protocol_version | 1 | RW | EEPROM | 1 | 2 | N + 20 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 24 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 31 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 32 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 34 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 36 | pwm_limit | 2 | RW | EEPROM | 0 | 885 | N + 38 | current_limit | 2 | RW | EEPROM | 0 | 32767 | N + 40 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 44 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 48 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 52 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 56 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 57 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 58 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 59 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 63 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 168 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 512 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 513 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 514 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 515 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 516 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 517 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 518 | hardware_error_status | 1 | R | RAM | 0 | 31 | N + 524 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N + 526 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N + 528 | position_d_gain | 2 | RW | RAM | 0 | 2047 | N + 530 | position_i_gain | 2 | RW | RAM | 0 | 2047 | N + 532 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N + 536 | feedforward_2nd_gain | 2 | RW | RAM | 0 | 32767 | N + 538 | feedforward_1st_gain | 2 | RW | RAM | 0 | 32767 | N + 546 | bus_watchdog | 1 | RW | RAM | -1 | 127 | Y + 548 | goal_pwm | 2 | RW | RAM | -2100 | 2100 | Y + 550 | goal_current | 2 | RW | RAM | -32768 | 32767 | Y + 552 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 556 | profile_acceleration | 4 | RW | RAM | 0 | 32767 | N + 560 | profile_velocity | 4 | RW | RAM | 0 | 1023 | N + 564 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 568 | realtime_tick | 2 | R | RAM | 0 | 32767 | N + 570 | moving | 1 | R | RAM | 0 | 1 | N + 571 | moving_status | 1 | R | RAM | 0 | 255 | N + 572 | present_pwm | 2 | R | RAM | -2100 | 2100 | Y + 574 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 576 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 580 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 584 | velocity_trajectory | 4 | R | RAM | 0 | 1023 | N + 588 | position_trajectory | 4 | R | RAM | 0 | 4095 | N + 592 | present_voltage | 2 | R | RAM | 0 | 500 | N + 594 | present_temperature | 1 | R | RAM | 0 | 200 | N + 600 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 602 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 604 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 606 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N diff --git a/humanoid_robot_intelligence_control_system_device/devices/dynamixel/H54P-200-S500-R.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/H54P-200-S500-R.device new file mode 100644 index 0000000..c58460e --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/H54P-200-S500-R.device @@ -0,0 +1,92 @@ +[device info] +model_name = H54P-200-S500-R +device_type = dynamixel + +[type info] +torque_to_current_value_ratio = 146.502114 +velocity_to_value_ratio = 954.93 +value_of_0_radian_position = 0 +value_of_min_radian_position = -501923 +value_of_max_radian_position = 501923 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_current +position_d_gain_item_name = position_d_gain +position_i_gain_item_name = position_i_gain +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 10 | drive_mode | 1 | RW | EEPROM | 0 | 3 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 12 | secondary_id | 1 | RW | EEPROM | 0 | 255 | N + 13 | protocol_version | 1 | RW | EEPROM | 1 | 2 | N + 20 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 24 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 31 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 32 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 34 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 36 | pwm_limit | 2 | RW | EEPROM | 0 | 885 | N + 38 | current_limit | 2 | RW | EEPROM | 0 | 32767 | N + 40 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 44 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 48 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 52 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 56 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 57 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 58 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 59 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 63 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 168 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 512 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 513 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 514 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 515 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 516 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 517 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 518 | hardware_error_status | 1 | R | RAM | 0 | 31 | N + 524 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N + 526 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N + 528 | position_d_gain | 2 | RW | RAM | 0 | 2047 | N + 530 | position_i_gain | 2 | RW | RAM | 0 | 2047 | N + 532 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N + 536 | feedforward_2nd_gain | 2 | RW | RAM | 0 | 32767 | N + 538 | feedforward_1st_gain | 2 | RW | RAM | 0 | 32767 | N + 546 | bus_watchdog | 1 | RW | RAM | -1 | 127 | Y + 548 | goal_pwm | 2 | RW | RAM | -2100 | 2100 | Y + 550 | goal_current | 2 | RW | RAM | -32768 | 32767 | Y + 552 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 556 | profile_acceleration | 4 | RW | RAM | 0 | 32767 | N + 560 | profile_velocity | 4 | RW | RAM | 0 | 1023 | N + 564 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 568 | realtime_tick | 2 | R | RAM | 0 | 32767 | N + 570 | moving | 1 | R | RAM | 0 | 1 | N + 571 | moving_status | 1 | R | RAM | 0 | 255 | N + 572 | present_pwm | 2 | R | RAM | -2100 | 2100 | Y + 574 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 576 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 580 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 584 | velocity_trajectory | 4 | R | RAM | 0 | 1023 | N + 588 | position_trajectory | 4 | R | RAM | 0 | 4095 | N + 592 | present_voltage | 2 | R | RAM | 0 | 500 | N + 594 | present_temperature | 1 | R | RAM | 0 | 200 | N + 600 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 602 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 604 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 606 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N diff --git a/humanoid_robot_intelligence_control_system_device/devices/dynamixel/L42-10-S300-R.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/L42-10-S300-R.device new file mode 100755 index 0000000..8d861f1 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/L42-10-S300-R.device @@ -0,0 +1,73 @@ +[device info] +model_name = L42-10-S300-R +device_type = dynamixel + +[type info] +value_of_0_radian_position = 0 +value_of_min_radian_position = -2047 +value_of_max_radian_position = 2048 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_torque +position_d_gain_item_name = +position_i_gain_item_name = +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 30 | torque_limit | 2 | RW | EEPROM | 0 | 32767 | N + 32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 562 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 586 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N + 588 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N + 594 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N + 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y + 606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N + 610 | is_moving | 1 | R | RAM | 0 | 1 | N + 611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 621 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 623 | present_voltage | 2 | R | RAM | 0 | 500 | N + 625 | present_temperature | 1 | R | RAM | 0 | 200 | N + 626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 891 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N diff --git a/humanoid_robot_intelligence_control_system_device/devices/dynamixel/L54-30-S400-R.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/L54-30-S400-R.device new file mode 100755 index 0000000..71fa09d --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/L54-30-S400-R.device @@ -0,0 +1,74 @@ +[device info] +model_name = L54-30-S400-R +device_type = dynamixel + +[type info] +value_of_0_radian_position = 0 +value_of_min_radian_position = -144198 +value_of_max_radian_position = 144198 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_torque +position_d_gain_item_name = +position_i_gain_item_name = +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 13 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 30 | torque_limit | 2 | RW | EEPROM | 0 | 32767 | N + 32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 562 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 586 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N + 588 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N + 594 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N + 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y + 606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N + 610 | is_moving | 1 | R | RAM | 0 | 1 | N + 611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 621 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 623 | present_voltage | 2 | R | RAM | 0 | 500 | N + 625 | present_temperature | 1 | R | RAM | 0 | 200 | N + 626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 891 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N diff --git a/humanoid_robot_intelligence_control_system_device/devices/dynamixel/L54-30-S500-R.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/L54-30-S500-R.device new file mode 100755 index 0000000..5a8add7 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/L54-30-S500-R.device @@ -0,0 +1,74 @@ +[device info] +model_name = L54-30-S500-R +device_type = dynamixel + +[type info] +value_of_0_radian_position = 0 +value_of_min_radian_position = -180684 +value_of_max_radian_position = 180684 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_torque +position_d_gain_item_name = +position_i_gain_item_name = +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 13 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 30 | torque_limit | 2 | RW | EEPROM | 0 | 32767 | N + 32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 562 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 586 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N + 588 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N + 594 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N + 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y + 606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N + 610 | is_moving | 1 | R | RAM | 0 | 1 | N + 611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 621 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 623 | present_voltage | 2 | R | RAM | 0 | 500 | N + 625 | present_temperature | 1 | R | RAM | 0 | 200 | N + 626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 891 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N diff --git a/humanoid_robot_intelligence_control_system_device/devices/dynamixel/L54-50-S290-R.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/L54-50-S290-R.device new file mode 100755 index 0000000..7af9c22 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/L54-50-S290-R.device @@ -0,0 +1,74 @@ +[device info] +model_name = L54-50-S290-R +device_type = dynamixel + +[type info] +value_of_0_radian_position = 0 +value_of_min_radian_position = -103860 +value_of_max_radian_position = 103860 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_torque +position_d_gain_item_name = +position_i_gain_item_name = +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 13 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 30 | torque_limit | 2 | RW | EEPROM | 0 | 32767 | N + 32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 562 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 586 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N + 588 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N + 594 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N + 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y + 606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N + 610 | is_moving | 1 | R | RAM | 0 | 1 | N + 611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 621 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 623 | present_voltage | 2 | R | RAM | 0 | 500 | N + 625 | present_temperature | 1 | R | RAM | 0 | 200 | N + 626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 891 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N diff --git a/humanoid_robot_intelligence_control_system_device/devices/dynamixel/L54-50-S500-R.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/L54-50-S500-R.device new file mode 100755 index 0000000..11d63b3 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/L54-50-S500-R.device @@ -0,0 +1,74 @@ +[device info] +model_name = L54-50-S500-R +device_type = dynamixel + +[type info] +value_of_0_radian_position = 0 +value_of_min_radian_position = -180684 +value_of_max_radian_position = 180684 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_torque +position_d_gain_item_name = +position_i_gain_item_name = +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 13 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 30 | torque_limit | 2 | RW | EEPROM | 0 | 32767 | N + 32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 562 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 586 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N + 588 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N + 594 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N + 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y + 606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N + 610 | is_moving | 1 | R | RAM | 0 | 1 | N + 611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 621 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 623 | present_voltage | 2 | R | RAM | 0 | 500 | N + 625 | present_temperature | 1 | R | RAM | 0 | 200 | N + 626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 891 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N diff --git a/humanoid_robot_intelligence_control_system_device/devices/dynamixel/M42-10-S260-R.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/M42-10-S260-R.device new file mode 100755 index 0000000..ba205bb --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/M42-10-S260-R.device @@ -0,0 +1,74 @@ +[device info] +model_name = M42-10-S260-R +device_type = dynamixel + +[type info] +value_of_0_radian_position = 0 +value_of_min_radian_position = -131584 +value_of_max_radian_position = 131584 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_torque +position_d_gain_item_name = +position_i_gain_item_name = +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 13 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 30 | torque_limit | 2 | RW | EEPROM | 0 | 32767 | N + 32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 562 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 586 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N + 588 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N + 594 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N + 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y + 606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N + 610 | is_moving | 1 | R | RAM | 0 | 1 | N + 611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 621 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 623 | present_voltage | 2 | R | RAM | 0 | 500 | N + 625 | present_temperature | 1 | R | RAM | 0 | 200 | N + 626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 891 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N diff --git a/humanoid_robot_intelligence_control_system_device/devices/dynamixel/M54-40-S250-R.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/M54-40-S250-R.device new file mode 100755 index 0000000..62f9990 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/M54-40-S250-R.device @@ -0,0 +1,74 @@ +[device info] +model_name = M54-40-S250-R +device_type = dynamixel + +[type info] +value_of_0_radian_position = 0 +value_of_min_radian_position = -125700 +value_of_max_radian_position = 125700 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_torque +position_d_gain_item_name = +position_i_gain_item_name = +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 13 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 30 | torque_limit | 2 | RW | EEPROM | 0 | 32767 | N + 32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 562 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 586 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N + 588 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N + 594 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N + 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y + 606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N + 610 | is_moving | 1 | R | RAM | 0 | 1 | N + 611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 621 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 623 | present_voltage | 2 | R | RAM | 0 | 500 | N + 625 | present_temperature | 1 | R | RAM | 0 | 200 | N + 626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 891 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N diff --git a/humanoid_robot_intelligence_control_system_device/devices/dynamixel/M54-60-S250-R.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/M54-60-S250-R.device new file mode 100755 index 0000000..dd2efde --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/M54-60-S250-R.device @@ -0,0 +1,74 @@ +[device info] +model_name = M54-60-S250-R +device_type = dynamixel + +[type info] +value_of_0_radian_position = 0 +value_of_min_radian_position = -125700 +value_of_max_radian_position = 125700 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_torque +position_d_gain_item_name = +position_i_gain_item_name = +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 13 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 30 | torque_limit | 2 | RW | EEPROM | 0 | 32767 | N + 32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 562 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 586 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N + 588 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N + 594 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N + 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y + 606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N + 610 | is_moving | 1 | R | RAM | 0 | 1 | N + 611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 621 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 623 | present_voltage | 2 | R | RAM | 0 | 500 | N + 625 | present_temperature | 1 | R | RAM | 0 | 200 | N + 626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 891 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N diff --git a/humanoid_robot_intelligence_control_system_device/devices/dynamixel/MX-106.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/MX-106.device new file mode 100755 index 0000000..a122831 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/MX-106.device @@ -0,0 +1,66 @@ +[device info] +model_name = MX-106 +device_type = dynamixel + +[type info] +value_of_0_radian_position = 2048 +value_of_min_radian_position = 0 +value_of_max_radian_position = 4095 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = +position_d_gain_item_name = position_d_gain +position_i_gain_item_name = position_i_gain +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = +velocity_p_gain_item_name = + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 2 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 3 | ID | 1 | RW | EEPROM | 0 | 252 | N + 4 | baudrate | 1 | RW | EEPROM | 0 | 252 | N + 5 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 6 | CW_angle_limit | 2 | RW | EEPROM | 0 | 4095 | N + 8 | CCW_angle_limit | 2 | RW | EEPROM | 0 | 4095 | N + 10 | drive_mode | 1 | RW | EEPROM | 0 | 3 | N + 11 | max_temperature_limit | 1 | RW | EEPROM | 0 | 99 | N + 12 | min_voltage_limit | 1 | RW | EEPROM | 0 | 250 | N + 13 | max_voltage_limit | 1 | RW | EEPROM | 0 | 250 | N + 14 | max_torque | 2 | RW | EEPROM | 0 | 1023 | N + 16 | status_return_level | 1 | RW | EEPROM | 0 | 2 | N + 17 | alarm_LED | 1 | RW | EEPROM | 0 | 127 | N + 18 | alarm_shutdown | 1 | RW | EEPROM | 0 | 127 | N + 20 | multi_turn_offset | 2 | RW | EEPROM | -26624 | 26624 | Y + 22 | resolution_dividor | 1 | RW | EEPROM | 1 | 255 | N + 24 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 25 | LED | 1 | RW | RAM | 0 | 1 | N + 26 | position_d_gain | 1 | RW | RAM | 0 | 254 | N + 27 | position_i_gain | 1 | RW | RAM | 0 | 254 | N + 28 | position_p_gain | 1 | RW | RAM | 0 | 254 | N + 30 | goal_position | 2 | RW | RAM | -28672 | 28672 | Y + 32 | goal_velocity | 2 | RW | RAM | 0 | 1023 | N + 34 | goal_torque | 2 | RW | RAM | 0 | 1023 | N + 36 | present_position | 2 | R | RAM | -32768 | 32767 | Y + 38 | present_velocity | 2 | R | RAM | 0 | 2048 | N + 40 | present_load | 2 | R | RAM | 0 | 2048 | N + 42 | present_voltage | 1 | R | RAM | 50 | 250 | N + 43 | present_temperature | 1 | R | RAM | 0 | 99 | N + 44 | registered_instruction | 1 | R | RAM | 0 | 1 | N + 46 | is_moving | 1 | R | RAM | 0 | 1 | N + 47 | EEPROM_lock | 1 | RW | RAM | 0 | 1 | N + 48 | punch | 2 | RW | RAM | 0 | 1023 | N + 68 | current_consumption | 2 | RW | RAM | 0 | 4095 | N + 70 | torque_control_mode | 1 | RW | RAM | 0 | 1 | N + 71 | torque_control_goal | 2 | RW | RAM | 0 | 2047 | N + 73 | goal_acceleration | 1 | RW | RAM | 0 | 254 | N + diff --git a/humanoid_robot_intelligence_control_system_device/devices/dynamixel/MX-28.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/MX-28.device new file mode 100755 index 0000000..ae1f877 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/MX-28.device @@ -0,0 +1,62 @@ +[device info] +model_name = MX-28 +device_type = dynamixel + +[type info] +value_of_0_radian_position = 2048 +value_of_min_radian_position = 0 +value_of_max_radian_position = 4095 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = +position_d_gain_item_name = position_d_gain +position_i_gain_item_name = position_i_gain +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = +velocity_p_gain_item_name = + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 2 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 3 | ID | 1 | RW | EEPROM | 0 | 252 | N + 4 | baudrate | 1 | RW | EEPROM | 0 | 252 | N + 5 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 6 | CW_angle_limit | 2 | RW | EEPROM | 0 | 4095 | N + 8 | CCW_angle_limit | 2 | RW | EEPROM | 0 | 4095 | N + 10 | drive_mode | 1 | RW | EEPROM | 0 | 3 | N + 11 | max_temperature_limit | 1 | RW | EEPROM | 0 | 99 | N + 12 | min_voltage_limit | 1 | RW | EEPROM | 0 | 250 | N + 13 | max_voltage_limit | 1 | RW | EEPROM | 0 | 250 | N + 14 | max_torque | 2 | RW | EEPROM | 0 | 1023 | N + 16 | status_return_level | 1 | RW | EEPROM | 0 | 2 | N + 17 | alarm_LED | 1 | RW | EEPROM | 0 | 127 | N + 18 | alarm_shutdown | 1 | RW | EEPROM | 0 | 127 | N + 20 | multi_turn_offset | 2 | RW | EEPROM | -26624 | 26624 | Y + 22 | resolution_dividor | 1 | RW | EEPROM | 1 | 255 | N + 24 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 25 | LED | 1 | RW | RAM | 0 | 1 | N + 26 | position_d_gain | 1 | RW | RAM | 0 | 254 | N + 27 | position_i_gain | 1 | RW | RAM | 0 | 254 | N + 28 | position_p_gain | 1 | RW | RAM | 0 | 254 | N + 30 | goal_position | 2 | RW | RAM | -28672 | 28672 | Y + 32 | goal_velocity | 2 | RW | RAM | 0 | 1023 | N + 34 | goal_torque | 2 | RW | RAM | 0 | 1023 | N + 36 | present_position | 2 | R | RAM | -32768 | 32767 | Y + 38 | present_velocity | 2 | R | RAM | 0 | 2048 | N + 40 | present_load | 2 | R | RAM | 0 | 2048 | N + 42 | present_voltage | 1 | R | RAM | 50 | 250 | N + 43 | present_temperature | 1 | R | RAM | 0 | 99 | N + 44 | registered_instruction | 1 | R | RAM | 0 | 1 | N + 46 | is_moving | 1 | R | RAM | 0 | 1 | N + 47 | EEPROM_lock | 1 | RW | RAM | 0 | 1 | N + 48 | punch | 2 | RW | RAM | 0 | 1023 | N + 73 | goal_acceleration | 1 | RW | RAM | 0 | 254 | N diff --git a/humanoid_robot_intelligence_control_system_device/devices/dynamixel/MX-64.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/MX-64.device new file mode 100755 index 0000000..c6a50de --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/MX-64.device @@ -0,0 +1,66 @@ +[device info] +model_name = MX-64 +device_type = dynamixel + +[type info] +value_of_0_radian_position = 2048 +value_of_min_radian_position = 0 +value_of_max_radian_position = 4095 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = +position_d_gain_item_name = position_d_gain +position_i_gain_item_name = position_i_gain +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = +velocity_p_gain_item_name = + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 2 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 3 | ID | 1 | RW | EEPROM | 0 | 252 | N + 4 | baudrate | 1 | RW | EEPROM | 0 | 252 | N + 5 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 6 | CW_angle_limit | 2 | RW | EEPROM | 0 | 4095 | N + 8 | CCW_angle_limit | 2 | RW | EEPROM | 0 | 4095 | N + 10 | drive_mode | 1 | RW | EEPROM | 0 | 3 | N + 11 | max_temperature_limit | 1 | RW | EEPROM | 0 | 99 | N + 12 | min_voltage_limit | 1 | RW | EEPROM | 0 | 250 | N + 13 | max_voltage_limit | 1 | RW | EEPROM | 0 | 250 | N + 14 | max_torque | 2 | RW | EEPROM | 0 | 1023 | N + 16 | status_return_level | 1 | RW | EEPROM | 0 | 2 | N + 17 | alarm_LED | 1 | RW | EEPROM | 0 | 127 | N + 18 | alarm_shutdown | 1 | RW | EEPROM | 0 | 127 | N + 20 | multi_turn_offset | 2 | RW | EEPROM | -26624 | 26624 | Y + 22 | resolution_dividor | 1 | RW | EEPROM | 1 | 255 | N + 24 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 25 | LED | 1 | RW | RAM | 0 | 1 | N + 26 | position_d_gain | 1 | RW | RAM | 0 | 254 | N + 27 | position_i_gain | 1 | RW | RAM | 0 | 254 | N + 28 | position_p_gain | 1 | RW | RAM | 0 | 254 | N + 30 | goal_position | 2 | RW | RAM | -28672 | 28672 | Y + 32 | goal_velocity | 2 | RW | RAM | 0 | 1023 | N + 34 | goal_torque | 2 | RW | RAM | 0 | 1023 | N + 36 | present_position | 2 | R | RAM | -32768 | 32767 | Y + 38 | present_velocity | 2 | R | RAM | 0 | 2048 | N + 40 | present_load | 2 | R | RAM | 0 | 2048 | N + 42 | present_voltage | 1 | R | RAM | 50 | 250 | N + 43 | present_temperature | 1 | R | RAM | 0 | 99 | N + 44 | registered_instruction | 1 | R | RAM | 0 | 1 | N + 46 | is_moving | 1 | R | RAM | 0 | 1 | N + 47 | EEPROM_lock | 1 | RW | RAM | 0 | 1 | N + 48 | punch | 2 | RW | RAM | 0 | 1023 | N + 68 | current_consumption | 2 | RW | RAM | 0 | 4095 | N + 70 | torque_control_mode | 1 | RW | RAM | 0 | 1 | N + 71 | torque_control_goal | 2 | RW | RAM | 0 | 2047 | N + 73 | goal_acceleration | 1 | RW | RAM | 0 | 254 | N + diff --git a/humanoid_robot_intelligence_control_system_device/devices/dynamixel/PH42-020-S300-R.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/PH42-020-S300-R.device new file mode 100644 index 0000000..92e9df6 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/PH42-020-S300-R.device @@ -0,0 +1,92 @@ +[device info] +model_name = PH42-020-S300-R +device_type = dynamixel + +[type info] +torque_to_current_value_ratio = 109.3747778 +velocity_to_value_ratio = 954.93 +value_of_0_radian_position = 0 +value_of_min_radian_position = -303750 +value_of_max_radian_position = 303750 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_current +position_d_gain_item_name = position_d_gain +position_i_gain_item_name = position_i_gain +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 10 | drive_mode | 1 | RW | EEPROM | 0 | 3 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 12 | secondary_id | 1 | RW | EEPROM | 0 | 255 | N + 13 | protocol_version | 1 | RW | EEPROM | 1 | 2 | N + 20 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 24 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 31 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 32 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 34 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 36 | pwm_limit | 2 | RW | EEPROM | 0 | 885 | N + 38 | current_limit | 2 | RW | EEPROM | 0 | 32767 | N + 40 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 44 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 48 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 52 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 56 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 57 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 58 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 59 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 63 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 168 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 512 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 513 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 514 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 515 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 516 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 517 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 518 | hardware_error_status | 1 | R | RAM | 0 | 31 | N + 524 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N + 526 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N + 528 | position_d_gain | 2 | RW | RAM | 0 | 2047 | N + 530 | position_i_gain | 2 | RW | RAM | 0 | 2047 | N + 532 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N + 536 | feedforward_2nd_gain | 2 | RW | RAM | 0 | 32767 | N + 538 | feedforward_1st_gain | 2 | RW | RAM | 0 | 32767 | N + 546 | bus_watchdog | 1 | RW | RAM | -1 | 127 | Y + 548 | goal_pwm | 2 | RW | RAM | -2100 | 2100 | Y + 550 | goal_current | 2 | RW | RAM | -32768 | 32767 | Y + 552 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 556 | profile_acceleration | 4 | RW | RAM | 0 | 32767 | N + 560 | profile_velocity | 4 | RW | RAM | 0 | 1023 | N + 564 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 568 | realtime_tick | 2 | R | RAM | 0 | 32767 | N + 570 | moving | 1 | R | RAM | 0 | 1 | N + 571 | moving_status | 1 | R | RAM | 0 | 255 | N + 572 | present_pwm | 2 | R | RAM | -2100 | 2100 | Y + 574 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 576 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 580 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 584 | velocity_trajectory | 4 | R | RAM | 0 | 1023 | N + 588 | position_trajectory | 4 | R | RAM | 0 | 4095 | N + 592 | present_voltage | 2 | R | RAM | 0 | 500 | N + 594 | present_temperature | 1 | R | RAM | 0 | 200 | N + 600 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 602 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 604 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 606 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N diff --git a/humanoid_robot_intelligence_control_system_device/devices/dynamixel/PH54-100-S500-R.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/PH54-100-S500-R.device new file mode 100644 index 0000000..6dd1ece --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/PH54-100-S500-R.device @@ -0,0 +1,92 @@ +[device info] +model_name = PH54-100-S500-R +device_type = dynamixel + +[type info] +torque_to_current_value_ratio = 155.658486 +velocity_to_value_ratio = 954.93 +value_of_0_radian_position = 0 +value_of_min_radian_position = -501923 +value_of_max_radian_position = 501923 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_current +position_d_gain_item_name = position_d_gain +position_i_gain_item_name = position_i_gain +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 10 | drive_mode | 1 | RW | EEPROM | 0 | 3 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 12 | secondary_id | 1 | RW | EEPROM | 0 | 255 | N + 13 | protocol_version | 1 | RW | EEPROM | 1 | 2 | N + 20 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 24 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 31 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 32 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 34 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 36 | pwm_limit | 2 | RW | EEPROM | 0 | 885 | N + 38 | current_limit | 2 | RW | EEPROM | 0 | 32767 | N + 40 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 44 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 48 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 52 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 56 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 57 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 58 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 59 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 63 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 168 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 512 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 513 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 514 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 515 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 516 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 517 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 518 | hardware_error_status | 1 | R | RAM | 0 | 31 | N + 524 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N + 526 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N + 528 | position_d_gain | 2 | RW | RAM | 0 | 2047 | N + 530 | position_i_gain | 2 | RW | RAM | 0 | 2047 | N + 532 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N + 536 | feedforward_2nd_gain | 2 | RW | RAM | 0 | 32767 | N + 538 | feedforward_1st_gain | 2 | RW | RAM | 0 | 32767 | N + 546 | bus_watchdog | 1 | RW | RAM | -1 | 127 | Y + 548 | goal_pwm | 2 | RW | RAM | -2100 | 2100 | Y + 550 | goal_current | 2 | RW | RAM | -32768 | 32767 | Y + 552 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 556 | profile_acceleration | 4 | RW | RAM | 0 | 32767 | N + 560 | profile_velocity | 4 | RW | RAM | 0 | 1023 | N + 564 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 568 | realtime_tick | 2 | R | RAM | 0 | 32767 | N + 570 | moving | 1 | R | RAM | 0 | 1 | N + 571 | moving_status | 1 | R | RAM | 0 | 255 | N + 572 | present_pwm | 2 | R | RAM | -2100 | 2100 | Y + 574 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 576 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 580 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 584 | velocity_trajectory | 4 | R | RAM | 0 | 1023 | N + 588 | position_trajectory | 4 | R | RAM | 0 | 4095 | N + 592 | present_voltage | 2 | R | RAM | 0 | 500 | N + 594 | present_temperature | 1 | R | RAM | 0 | 200 | N + 600 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 602 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 604 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 606 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N diff --git a/humanoid_robot_intelligence_control_system_device/devices/dynamixel/PH54-200-S500-R.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/PH54-200-S500-R.device new file mode 100644 index 0000000..b758315 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/PH54-200-S500-R.device @@ -0,0 +1,92 @@ +[device info] +model_name = PH54-200-S500-R +device_type = dynamixel + +[type info] +torque_to_current_value_ratio = 146.502114 +velocity_to_value_ratio = 954.93 +value_of_0_radian_position = 0 +value_of_min_radian_position = -501923 +value_of_max_radian_position = 501923 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_current +position_d_gain_item_name = position_d_gain +position_i_gain_item_name = position_i_gain +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 10 | drive_mode | 1 | RW | EEPROM | 0 | 3 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 12 | secondary_id | 1 | RW | EEPROM | 0 | 255 | N + 13 | protocol_version | 1 | RW | EEPROM | 1 | 2 | N + 20 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 24 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 31 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 32 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 34 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 36 | pwm_limit | 2 | RW | EEPROM | 0 | 885 | N + 38 | current_limit | 2 | RW | EEPROM | 0 | 32767 | N + 40 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 44 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 48 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 52 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 56 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 57 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 58 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 59 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 63 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 168 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 512 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 513 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 514 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 515 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 516 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 517 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 518 | hardware_error_status | 1 | R | RAM | 0 | 31 | N + 524 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N + 526 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N + 528 | position_d_gain | 2 | RW | RAM | 0 | 2047 | N + 530 | position_i_gain | 2 | RW | RAM | 0 | 2047 | N + 532 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N + 536 | feedforward_2nd_gain | 2 | RW | RAM | 0 | 32767 | N + 538 | feedforward_1st_gain | 2 | RW | RAM | 0 | 32767 | N + 546 | bus_watchdog | 1 | RW | RAM | -1 | 127 | Y + 548 | goal_pwm | 2 | RW | RAM | -2100 | 2100 | Y + 550 | goal_current | 2 | RW | RAM | -32768 | 32767 | Y + 552 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 556 | profile_acceleration | 4 | RW | RAM | 0 | 32767 | N + 560 | profile_velocity | 4 | RW | RAM | 0 | 1023 | N + 564 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 568 | realtime_tick | 2 | R | RAM | 0 | 32767 | N + 570 | moving | 1 | R | RAM | 0 | 1 | N + 571 | moving_status | 1 | R | RAM | 0 | 255 | N + 572 | present_pwm | 2 | R | RAM | -2100 | 2100 | Y + 574 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 576 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 580 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 584 | velocity_trajectory | 4 | R | RAM | 0 | 1023 | N + 588 | position_trajectory | 4 | R | RAM | 0 | 4095 | N + 592 | present_voltage | 2 | R | RAM | 0 | 500 | N + 594 | present_temperature | 1 | R | RAM | 0 | 200 | N + 600 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 602 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 604 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 606 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N diff --git a/humanoid_robot_intelligence_control_system_device/devices/dynamixel/RH-P12-RN(A).device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/RH-P12-RN(A).device new file mode 100644 index 0000000..31d253b --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/RH-P12-RN(A).device @@ -0,0 +1,90 @@ +[device info] +model_name = RH-P12-RN(A) +device_type = dynamixel + +[type info] +value_of_0_radian_position = 0 +value_of_min_radian_position = 0 +value_of_max_radian_position = 740 +min_radian = 0 +max_radian = 1.150767 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_current +position_d_gain_item_name = position_d_gain +position_i_gain_item_name = position_i_gain +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 9 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 10 | drive_mode | 1 | RW | EEPROM | 0 | 1 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 12 | secondary_ID | 1 | RW | EEPROM | 0 | 255 | N + 20 | homing_offset | 4 | RW | EEPROM | -1150 | 1150 | Y + 24 | moving_threshold | 4 | RW | EEPROM | 0 | 2970 | N + 31 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 32 | max_voltage_limit | 2 | RW | EEPROM | 0 | 350 | N + 34 | min_voltage_limit | 2 | RW | EEPROM | 0 | 350 | N + 36 | pwm_limit | 2 | RW | EEPROM | 0 | 2009 | N + 38 | current_limit | 2 | RW | EEPROM | 0 | 1984 | N + 40 | acceleration_limit | 4 | RW | EEPROM | 0 | 1378788 | N + 44 | velocity_limit | 4 | RW | EEPROM | 0 | 2970 | N + 48 | max_position_limit | 4 | RW | EEPROM | 0 | 1150 | N + 52 | min_position_limit | 4 | RW | EEPROM | 0 | 1150 | N + 56 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 57 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 58 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 59 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 63 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 168 | indirect_address_1 | 2 | RW | EEPROM | 512 | 1023 | N + 512 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 513 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 514 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 515 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 516 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 517 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 518 | hardware_error_status | 2 | R | RAM | 0 | 63 | N + 524 | velocity_i_gain | 2 | RW | RAM | 0 | 32767 | N + 526 | velocity_p_gain | 2 | RW | RAM | 0 | 32767 | N + 528 | position_d_gain | 2 | RW | RAM | 0 | 32767 | N + 530 | position_i_gain | 2 | RW | RAM | 0 | 32767 | N + 532 | position_p_gain | 2 | RW | RAM | 0 | 32767 | N + 536 | feedforward_2nd_gain | 2 | RW | RAM | 0 | 32767 | N + 538 | feedforward_1st_gain | 2 | RW | RAM | 0 | 32767 | N + 546 | bus_watchdog | 1 | RW | RAM | -1 | 127 | Y + 548 | goal_pwm | 2 | RW | RAM | -2009 | 2009 | Y + 550 | goal_current | 2 | RW | RAM | -1984 | 1984 | Y + 552 | goal_velocity | 4 | RW | RAM | -2970 | 2970 | Y + 556 | profile_acceleration | 4 | RW | RAM | 0 | 1378788 | N + 560 | profile_velocity | 4 | RW | RAM | 0 | 2970 | N + 564 | goal_position | 4 | RW | RAM | 0 | 1150 | N + 568 | realtime_tick | 2 | R | RAM | 0 | 32767 | N + 570 | is_moving | 1 | R | RAM | 0 | 1 | N + 571 | moving_status | 1 | R | RAM | 0 | 255 | N + 572 | present_pwm | 2 | R | RAM | -2009 | 2009 | Y + 574 | present_current | 2 | R | RAM | -1984 | 1984 | Y + 576 | present_velocity | 4 | R | RAM | -2970 | 2970 | Y + 580 | present_position | 4 | R | RAM | -1150 | 1150 | Y + 584 | velocity_trajectory | 4 | R | RAM | -2147483648 | 2147483648 | Y + 588 | position_trajectory | 4 | R | RAM | -2147483648 | 2147483648 | Y + 592 | present_voltage | 2 | R | RAM | 0 | 350 | N + 594 | present_temperature | 1 | R | RAM | 0 | 100 | N + 595 | grip_detection | 1 | R | RAM | 0 | 1 | N + 600 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 602 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 604 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 606 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N diff --git a/humanoid_robot_intelligence_control_system_device/devices/dynamixel/RH-P12-RN.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/RH-P12-RN.device new file mode 100755 index 0000000..14ed73b --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/RH-P12-RN.device @@ -0,0 +1,73 @@ +[device info] +model_name = RH-P12-RN +device_type = dynamixel + +[type info] +value_of_0_radian_position = 0 +value_of_min_radian_position = 0 +value_of_max_radian_position = 740 +min_radian = 0 +max_radian = 1.150767 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_current +position_d_gain_item_name = position_d_gain +position_i_gain_item_name = position_i_gain +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = +velocity_p_gain_item_name = + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 5 | N + 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 30 | current_limit | 2 | RW | EEPROM | 0 | 32767 | N + 32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 562 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 590 | position_d_gain | 2 | RW | RAM | 0 | 2047 | N + 592 | position_i_gain | 2 | RW | RAM | 0 | 2047 | N + 594 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N + 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 604 | goal_current | 2 | RW | RAM | -32768 | 32767 | Y + 606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N + 610 | is_moving | 1 | R | RAM | 0 | 1 | N + 611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 621 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 623 | present_voltage | 2 | R | RAM | 0 | 500 | N + 625 | present_temperature | 1 | R | RAM | 0 | 200 | N + 626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 891 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N diff --git a/humanoid_robot_intelligence_control_system_device/devices/dynamixel/XM-430.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/XM-430.device new file mode 100755 index 0000000..7dfe91d --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/XM-430.device @@ -0,0 +1,82 @@ +[device info] +model_name = XM-430 +device_type = dynamixel + +[type info] +torque_to_current_value_ratio = 149.795386991 + +value_of_0_radian_position = 2048 +value_of_min_radian_position = 0 +value_of_max_radian_position = 4095 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_current +position_d_gain_item_name = position_d_gain +position_i_gain_item_name = position_i_gain +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 7 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 10 | drive_mode | 1 | RW | EEPROM | 0 | 1 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 16 | N + 12 | secondary_id | 1 | RW | EEPROM | 0 | 255 | N + 20 | homing_offset | 4 | RW | EEPROM | -1044479 | 1044479 | Y + 24 | moving_threshold | 4 | RW | EEPROM | 0 | 1023 | N + 31 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 32 | max_voltage_limit | 2 | RW | EEPROM | 95 | 160 | N + 34 | min_voltage_limit | 2 | RW | EEPROM | 95 | 160 | N + 36 | pwm_limit | 2 | RW | EEPROM | 0 | 885 | N + 38 | current_limit | 2 | RW | EEPROM | 0 | 1193 | N + 44 | velocity_limit | 4 | RW | EEPROM | 0 | 1023 | N + 48 | max_position_limit | 4 | RW | EEPROM | 0 | 4095 | N + 52 | min_position_limit | 4 | RW | EEPROM | 0 | 4095 | N + 63 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 64 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 65 | LED | 1 | RW | RAM | 0 | 1 | N + 68 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 69 | registered_instruction | 1 | R | RAM | 0 | 1 | N + 70 | hardware_error_status | 1 | R | RAM | 0 | 255 | N + 76 | velocity_i_gain | 2 | RW | RAM | 0 | 32767 | N + 78 | velocity_p_gain | 2 | RW | RAM | 0 | 32767 | N + 80 | position_d_gain | 2 | RW | RAM | 0 | 32767 | N + 82 | position_i_gain | 2 | RW | RAM | 0 | 32767 | N + 84 | position_p_gain | 2 | RW | RAM | 0 | 32767 | N + 88 | feedforward_2nd_gain | 2 | RW | RAM | 0 | 32767 | N + 90 | feedforward_1st_gain | 2 | RW | RAM | 0 | 32767 | N + 98 | bus_watchdog | 1 | RW | RAM | -1 | 127 | Y + 100 | goal_pwm | 2 | RW | RAM | 0 | 885 | N + 102 | goal_current | 2 | RW | RAM | 0 | 1193 | N + 104 | goal_velocity | 4 | RW | RAM | 0 | 1023 | N + 108 | profile_acceleration | 4 | RW | RAM | 0 | 32767 | N + 112 | profile_velocity | 4 | RW | RAM | 0 | 1023 | N + 116 | goal_position | 4 | RW | RAM | -1048575 | 1048575 | Y + 120 | realtime_tick | 2 | R | RAM | 0 | 32767 | N + 122 | moving | 1 | R | RAM | 0 | 1 | N + 123 | moving_status | 1 | R | RAM | 0 | 255 | N + 124 | present_pwm | 2 | R | RAM | 0 | 885 | N + 126 | present_current | 2 | R | RAM | 0 | 1193 | N + 128 | present_velocity | 4 | R | RAM | 0 | 1023 | N + 132 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 136 | velocity_trajectory | 4 | R | RAM | 0 | 1023 | N + 140 | position_trajectory | 4 | R | RAM | 0 | 4095 | N + 144 | present_input_voltage | 2 | R | RAM | 95 | 160 | N + 146 | present_temperature | 1 | R | RAM | 0 | 100 | N + 168 | indirect_address_1 | 2 | RW | RAM | 0 | 65535 | N + 224 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 578 | indirect_address_29 | 2 | RW | RAM | 0 | 65535 | N + 634 | indirect_data_29 | 1 | RW | RAM | 0 | 255 | N diff --git a/humanoid_robot_intelligence_control_system_device/devices/dynamixel/XM430-W210.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/XM430-W210.device new file mode 100644 index 0000000..6113e04 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/XM430-W210.device @@ -0,0 +1,83 @@ +[device info] +model_name = XM430-W210 +device_type = dynamixel + +[type info] +torque_to_current_value_ratio = 235.53647082 +velocity_to_value_ratio = 41.707853 + +value_of_0_radian_position = 2048 +value_of_min_radian_position = 0 +value_of_max_radian_position = 4095 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_current +position_d_gain_item_name = position_d_gain +position_i_gain_item_name = position_i_gain +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 7 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 10 | drive_mode | 1 | RW | EEPROM | 0 | 1 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 16 | N + 12 | secondary_id | 1 | RW | EEPROM | 0 | 255 | N + 20 | homing_offset | 4 | RW | EEPROM | -1044479 | 1044479 | Y + 24 | moving_threshold | 4 | RW | EEPROM | 0 | 1023 | N + 31 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 32 | max_voltage_limit | 2 | RW | EEPROM | 95 | 160 | N + 34 | min_voltage_limit | 2 | RW | EEPROM | 95 | 160 | N + 36 | pwm_limit | 2 | RW | EEPROM | 0 | 885 | N + 38 | current_limit | 2 | RW | EEPROM | 0 | 1193 | N + 44 | velocity_limit | 4 | RW | EEPROM | 0 | 1023 | N + 48 | max_position_limit | 4 | RW | EEPROM | 0 | 4095 | N + 52 | min_position_limit | 4 | RW | EEPROM | 0 | 4095 | N + 63 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 64 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 65 | LED | 1 | RW | RAM | 0 | 1 | N + 68 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 69 | registered_instruction | 1 | R | RAM | 0 | 1 | N + 70 | hardware_error_status | 1 | R | RAM | 0 | 255 | N + 76 | velocity_i_gain | 2 | RW | RAM | 0 | 32767 | N + 78 | velocity_p_gain | 2 | RW | RAM | 0 | 32767 | N + 80 | position_d_gain | 2 | RW | RAM | 0 | 32767 | N + 82 | position_i_gain | 2 | RW | RAM | 0 | 32767 | N + 84 | position_p_gain | 2 | RW | RAM | 0 | 32767 | N + 88 | feedforward_2nd_gain | 2 | RW | RAM | 0 | 32767 | N + 90 | feedforward_1st_gain | 2 | RW | RAM | 0 | 32767 | N + 98 | bus_watchdog | 1 | RW | RAM | -1 | 127 | Y + 100 | goal_pwm | 2 | RW | RAM | 0 | 885 | N + 102 | goal_current | 2 | RW | RAM | 0 | 1193 | N + 104 | goal_velocity | 4 | RW | RAM | 0 | 1023 | N + 108 | profile_acceleration | 4 | RW | RAM | 0 | 32767 | N + 112 | profile_velocity | 4 | RW | RAM | 0 | 1023 | N + 116 | goal_position | 4 | RW | RAM | -1048575 | 1048575 | Y + 120 | realtime_tick | 2 | R | RAM | 0 | 32767 | N + 122 | moving | 1 | R | RAM | 0 | 1 | N + 123 | moving_status | 1 | R | RAM | 0 | 255 | N + 124 | present_pwm | 2 | R | RAM | 0 | 885 | N + 126 | present_current | 2 | R | RAM | 0 | 1193 | N + 128 | present_velocity | 4 | R | RAM | 0 | 1023 | N + 132 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 136 | velocity_trajectory | 4 | R | RAM | 0 | 1023 | N + 140 | position_trajectory | 4 | R | RAM | 0 | 4095 | N + 144 | present_input_voltage | 2 | R | RAM | 95 | 160 | N + 146 | present_temperature | 1 | R | RAM | 0 | 100 | N + 168 | indirect_address_1 | 2 | RW | RAM | 0 | 65535 | N + 224 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 578 | indirect_address_29 | 2 | RW | RAM | 0 | 65535 | N + 634 | indirect_data_29 | 1 | RW | RAM | 0 | 255 | N diff --git a/humanoid_robot_intelligence_control_system_device/devices/dynamixel/XM430-W350.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/XM430-W350.device new file mode 100644 index 0000000..e1be362 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/XM430-W350.device @@ -0,0 +1,83 @@ +[device info] +model_name = XM430-W350 +device_type = dynamixel + +[type info] +torque_to_current_value_ratio = 149.795386991 +velocity_to_value_ratio = 41.707853 + +value_of_0_radian_position = 2048 +value_of_min_radian_position = 0 +value_of_max_radian_position = 4095 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_current +position_d_gain_item_name = position_d_gain +position_i_gain_item_name = position_i_gain +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 7 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 10 | drive_mode | 1 | RW | EEPROM | 0 | 1 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 16 | N + 12 | secondary_id | 1 | RW | EEPROM | 0 | 255 | N + 20 | homing_offset | 4 | RW | EEPROM | -1044479 | 1044479 | Y + 24 | moving_threshold | 4 | RW | EEPROM | 0 | 1023 | N + 31 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 32 | max_voltage_limit | 2 | RW | EEPROM | 95 | 160 | N + 34 | min_voltage_limit | 2 | RW | EEPROM | 95 | 160 | N + 36 | pwm_limit | 2 | RW | EEPROM | 0 | 885 | N + 38 | current_limit | 2 | RW | EEPROM | 0 | 1193 | N + 44 | velocity_limit | 4 | RW | EEPROM | 0 | 1023 | N + 48 | max_position_limit | 4 | RW | EEPROM | 0 | 4095 | N + 52 | min_position_limit | 4 | RW | EEPROM | 0 | 4095 | N + 63 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 64 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 65 | LED | 1 | RW | RAM | 0 | 1 | N + 68 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 69 | registered_instruction | 1 | R | RAM | 0 | 1 | N + 70 | hardware_error_status | 1 | R | RAM | 0 | 255 | N + 76 | velocity_i_gain | 2 | RW | RAM | 0 | 32767 | N + 78 | velocity_p_gain | 2 | RW | RAM | 0 | 32767 | N + 80 | position_d_gain | 2 | RW | RAM | 0 | 32767 | N + 82 | position_i_gain | 2 | RW | RAM | 0 | 32767 | N + 84 | position_p_gain | 2 | RW | RAM | 0 | 32767 | N + 88 | feedforward_2nd_gain | 2 | RW | RAM | 0 | 32767 | N + 90 | feedforward_1st_gain | 2 | RW | RAM | 0 | 32767 | N + 98 | bus_watchdog | 1 | RW | RAM | -1 | 127 | Y + 100 | goal_pwm | 2 | RW | RAM | 0 | 885 | N + 102 | goal_current | 2 | RW | RAM | 0 | 1193 | N + 104 | goal_velocity | 4 | RW | RAM | 0 | 1023 | N + 108 | profile_acceleration | 4 | RW | RAM | 0 | 32767 | N + 112 | profile_velocity | 4 | RW | RAM | 0 | 1023 | N + 116 | goal_position | 4 | RW | RAM | -1048575 | 1048575 | Y + 120 | realtime_tick | 2 | R | RAM | 0 | 32767 | N + 122 | moving | 1 | R | RAM | 0 | 1 | N + 123 | moving_status | 1 | R | RAM | 0 | 255 | N + 124 | present_pwm | 2 | R | RAM | 0 | 885 | N + 126 | present_current | 2 | R | RAM | 0 | 1193 | N + 128 | present_velocity | 4 | R | RAM | 0 | 1023 | N + 132 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 136 | velocity_trajectory | 4 | R | RAM | 0 | 1023 | N + 140 | position_trajectory | 4 | R | RAM | 0 | 4095 | N + 144 | present_input_voltage | 2 | R | RAM | 95 | 160 | N + 146 | present_temperature | 1 | R | RAM | 0 | 100 | N + 168 | indirect_address_1 | 2 | RW | RAM | 0 | 65535 | N + 224 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 578 | indirect_address_29 | 2 | RW | RAM | 0 | 65535 | N + 634 | indirect_data_29 | 1 | RW | RAM | 0 | 255 | N diff --git a/humanoid_robot_intelligence_control_system_device/devices/dynamixel/XM540-W150.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/XM540-W150.device new file mode 100755 index 0000000..64fcf3b --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/XM540-W150.device @@ -0,0 +1,89 @@ +[device info] +model_name = XM540-W150 +device_type = dynamixel + +[type info] +torque_to_current_value_ratio = 289.13672036 +velocity_to_value_ratio = 41.707853 + +value_of_0_radian_position = 2048 +value_of_min_radian_position = 0 +value_of_max_radian_position = 4095 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_current +position_d_gain_item_name = position_d_gain +position_i_gain_item_name = position_i_gain +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 7 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 10 | drive_mode | 1 | RW | EEPROM | 0 | 1 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 16 | N + 12 | secondary_id | 1 | RW | EEPROM | 0 | 255 | N + 20 | homing_offset | 4 | RW | EEPROM | -1044479 | 1044479 | Y + 24 | moving_threshold | 4 | RW | EEPROM | 0 | 1023 | N + 31 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 32 | max_voltage_limit | 2 | RW | EEPROM | 95 | 160 | N + 34 | min_voltage_limit | 2 | RW | EEPROM | 95 | 160 | N + 36 | pwm_limit | 2 | RW | EEPROM | 0 | 885 | N + 38 | current_limit | 2 | RW | EEPROM | 0 | 1193 | N + 44 | velocity_limit | 4 | RW | EEPROM | 0 | 1023 | N + 48 | max_position_limit | 4 | RW | EEPROM | 0 | 4095 | N + 52 | min_position_limit | 4 | RW | EEPROM | 0 | 4095 | N + 56 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 57 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 58 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 63 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 64 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 65 | LED | 1 | RW | RAM | 0 | 1 | N + 68 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 69 | registered_instruction | 1 | R | RAM | 0 | 1 | N + 70 | hardware_error_status | 1 | R | RAM | 0 | 255 | N + 76 | velocity_i_gain | 2 | RW | RAM | 0 | 32767 | N + 78 | velocity_p_gain | 2 | RW | RAM | 0 | 32767 | N + 80 | position_d_gain | 2 | RW | RAM | 0 | 32767 | N + 82 | position_i_gain | 2 | RW | RAM | 0 | 32767 | N + 84 | position_p_gain | 2 | RW | RAM | 0 | 32767 | N + 88 | feedforward_2nd_gain | 2 | RW | RAM | 0 | 32767 | N + 90 | feedforward_1st_gain | 2 | RW | RAM | 0 | 32767 | N + 98 | bus_watchdog | 1 | RW | RAM | -1 | 127 | Y + 100 | goal_pwm | 2 | RW | RAM | 0 | 885 | N + 102 | goal_current | 2 | RW | RAM | 0 | 1193 | N + 104 | goal_velocity | 4 | RW | RAM | 0 | 1023 | N + 108 | profile_acceleration | 4 | RW | RAM | 0 | 32767 | N + 112 | profile_velocity | 4 | RW | RAM | 0 | 1023 | N + 116 | goal_position | 4 | RW | RAM | -1048575 | 1048575 | Y + 120 | realtime_tick | 2 | R | RAM | 0 | 32767 | N + 122 | moving | 1 | R | RAM | 0 | 1 | N + 123 | moving_status | 1 | R | RAM | 0 | 255 | N + 124 | present_pwm | 2 | R | RAM | 0 | 885 | N + 126 | present_current | 2 | R | RAM | 0 | 1193 | N + 128 | present_velocity | 4 | R | RAM | 0 | 1023 | N + 132 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 136 | velocity_trajectory | 4 | R | RAM | 0 | 1023 | N + 140 | position_trajectory | 4 | R | RAM | 0 | 4095 | N + 144 | present_input_voltage | 2 | R | RAM | 95 | 160 | N + 146 | present_temperature | 1 | R | RAM | 0 | 100 | N + 152 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 154 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 156 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 168 | indirect_address_1 | 2 | RW | RAM | 0 | 65535 | N + 224 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 578 | indirect_address_29 | 2 | RW | RAM | 0 | 65535 | N + 634 | indirect_data_29 | 1 | RW | RAM | 0 | 255 | N diff --git a/humanoid_robot_intelligence_control_system_device/devices/dynamixel/XM540-W270.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/XM540-W270.device new file mode 100755 index 0000000..c787cba --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/XM540-W270.device @@ -0,0 +1,89 @@ +[device info] +model_name = XM540-W270 +device_type = dynamixel + +[type info] +torque_to_current_value_ratio = 156.133829 +velocity_to_value_ratio = 41.707853 + +value_of_0_radian_position = 2048 +value_of_min_radian_position = 0 +value_of_max_radian_position = 4095 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_current +position_d_gain_item_name = position_d_gain +position_i_gain_item_name = position_i_gain +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 7 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 10 | drive_mode | 1 | RW | EEPROM | 0 | 1 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 16 | N + 12 | secondary_id | 1 | RW | EEPROM | 0 | 255 | N + 20 | homing_offset | 4 | RW | EEPROM | -1044479 | 1044479 | Y + 24 | moving_threshold | 4 | RW | EEPROM | 0 | 1023 | N + 31 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 32 | max_voltage_limit | 2 | RW | EEPROM | 95 | 160 | N + 34 | min_voltage_limit | 2 | RW | EEPROM | 95 | 160 | N + 36 | pwm_limit | 2 | RW | EEPROM | 0 | 885 | N + 38 | current_limit | 2 | RW | EEPROM | 0 | 1193 | N + 44 | velocity_limit | 4 | RW | EEPROM | 0 | 1023 | N + 48 | max_position_limit | 4 | RW | EEPROM | 0 | 4095 | N + 52 | min_position_limit | 4 | RW | EEPROM | 0 | 4095 | N + 56 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 57 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 58 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 63 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 64 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 65 | LED | 1 | RW | RAM | 0 | 1 | N + 68 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 69 | registered_instruction | 1 | R | RAM | 0 | 1 | N + 70 | hardware_error_status | 1 | R | RAM | 0 | 255 | N + 76 | velocity_i_gain | 2 | RW | RAM | 0 | 32767 | N + 78 | velocity_p_gain | 2 | RW | RAM | 0 | 32767 | N + 80 | position_d_gain | 2 | RW | RAM | 0 | 32767 | N + 82 | position_i_gain | 2 | RW | RAM | 0 | 32767 | N + 84 | position_p_gain | 2 | RW | RAM | 0 | 32767 | N + 88 | feedforward_2nd_gain | 2 | RW | RAM | 0 | 32767 | N + 90 | feedforward_1st_gain | 2 | RW | RAM | 0 | 32767 | N + 98 | bus_watchdog | 1 | RW | RAM | -1 | 127 | Y + 100 | goal_pwm | 2 | RW | RAM | 0 | 885 | N + 102 | goal_current | 2 | RW | RAM | 0 | 1193 | N + 104 | goal_velocity | 4 | RW | RAM | 0 | 1023 | N + 108 | profile_acceleration | 4 | RW | RAM | 0 | 32767 | N + 112 | profile_velocity | 4 | RW | RAM | 0 | 1023 | N + 116 | goal_position | 4 | RW | RAM | -1048575 | 1048575 | Y + 120 | realtime_tick | 2 | R | RAM | 0 | 32767 | N + 122 | moving | 1 | R | RAM | 0 | 1 | N + 123 | moving_status | 1 | R | RAM | 0 | 255 | N + 124 | present_pwm | 2 | R | RAM | 0 | 885 | N + 126 | present_current | 2 | R | RAM | 0 | 1193 | N + 128 | present_velocity | 4 | R | RAM | 0 | 1023 | N + 132 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 136 | velocity_trajectory | 4 | R | RAM | 0 | 1023 | N + 140 | position_trajectory | 4 | R | RAM | 0 | 4095 | N + 144 | present_input_voltage | 2 | R | RAM | 95 | 160 | N + 146 | present_temperature | 1 | R | RAM | 0 | 100 | N + 152 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 154 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 156 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 168 | indirect_address_1 | 2 | RW | RAM | 0 | 65535 | N + 224 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 578 | indirect_address_29 | 2 | RW | RAM | 0 | 65535 | N + 634 | indirect_data_29 | 1 | RW | RAM | 0 | 255 | N diff --git a/humanoid_robot_intelligence_control_system_device/devices/sensor/CM-740.device b/humanoid_robot_intelligence_control_system_device/devices/sensor/CM-740.device new file mode 100755 index 0000000..5cdf45f --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/devices/sensor/CM-740.device @@ -0,0 +1,25 @@ +[device info] +model_name = CM-740 +device_type = sensor + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 2 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 3 | ID | 1 | RW | EEPROM | 0 | 252 | N + 4 | baudrate | 1 | RW | EEPROM | 0 | 252 | N + 5 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 16 | status_return_level | 1 | RW | EEPROM | 0 | 2 | N + 24 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 25 | LED | 1 | RW | RAM | 0 | 7 | N + 26 | LED_5 | 2 | RW | RAM | 0 | 32767 | N + 28 | LED_6 | 2 | RW | RAM | 0 | 32767 | N + 30 | button | 1 | RW | RAM | 0 | 3 | N + 38 | gyro_z | 2 | R | RAM | 0 | 1023 | N + 40 | gyro_y | 2 | R | RAM | 0 | 1023 | N + 42 | gyro_x | 2 | R | RAM | 0 | 1023 | N + 44 | acc_x | 2 | R | RAM | 0 | 1023 | N + 46 | acc_y | 2 | R | RAM | 0 | 1023 | N + 48 | acc_z | 2 | R | RAM | 0 | 1023 | N + 50 | present_voltage | 1 | R | RAM | 50 | 250 | N + \ No newline at end of file diff --git a/humanoid_robot_intelligence_control_system_device/devices/sensor/OPEN-CR.device b/humanoid_robot_intelligence_control_system_device/devices/sensor/OPEN-CR.device new file mode 100755 index 0000000..71cee8e --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/devices/sensor/OPEN-CR.device @@ -0,0 +1,30 @@ +[device info] +model_name = OPEN-CR +device_type = sensor + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 2 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 3 | ID | 1 | RW | EEPROM | 0 | 252 | N + 4 | baudrate | 1 | RW | EEPROM | 0 | 252 | N + 5 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 16 | status_return_level | 1 | RW | EEPROM | 0 | 2 | N + 24 | dynamixel_power | 1 | RW | RAM | 0 | 1 | N + 25 | LED | 1 | RW | RAM | 0 | 7 | N + 26 | LED_RGB | 2 | RW | RAM | 0 | 32767 | N + 28 | buzzer | 2 | RW | RAM | 0 | 65535 | N + 30 | button | 1 | R | RAM | 0 | 15 | N + 31 | present_voltage | 1 | R | RAM | 50 | 250 | N + 32 | gyro_x | 2 | R | RAM | -32800 | 32800 | Y + 34 | gyro_y | 2 | R | RAM | -32800 | 32800 | Y + 36 | gyro_z | 2 | R | RAM | -32800 | 32800 | Y + 38 | acc_x | 2 | R | RAM | -32768 | 32768 | Y + 40 | acc_y | 2 | R | RAM | -32768 | 32768 | Y + 42 | acc_z | 2 | R | RAM | -32768 | 32768 | Y + 44 | roll | 2 | R | RAM | 0 | 4096 | N + 46 | pitch | 2 | R | RAM | 0 | 4096 | N + 48 | yaw | 2 | R | RAM | 0 | 4096 | N + 50 | imu_control | 1 | RW | RAM | 0 | 255 | N + + \ No newline at end of file diff --git a/humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device/control_table_item.h b/humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device/control_table_item.h new file mode 100755 index 0000000..9b0a878 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device/control_table_item.h @@ -0,0 +1,54 @@ +/******************************************************************************* + * Copyright 2018 ROBOTIS CO., LTD. + * + * 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. + *******************************************************************************/ + +/* + * control_table_item.h + * + * Created on: 2015. 12. 16. + * Author: zerom + */ + +#ifndef ROBOTIS_DEVICE_CONTROL_TABLE_ITEM_H_ +#define ROBOTIS_DEVICE_CONTROL_TABLE_ITEM_H_ + +#include + +namespace humanoid_robot_intelligence_control_system_framework { + +enum AccessType { Read, ReadWrite }; + +enum MemoryType { EEPROM, RAM }; + +class ControlTableItem { +public: + std::string item_name_; + uint16_t address_; + AccessType access_type_; + MemoryType memory_type_; + uint8_t data_length_; + int32_t data_min_value_; + int32_t data_max_value_; + bool is_signed_; + + ControlTableItem() + : item_name_(""), address_(0), access_type_(Read), memory_type_(RAM), + data_length_(0), data_min_value_(0), data_max_value_(0), + is_signed_(false) {} +}; + +} // namespace humanoid_robot_intelligence_control_system_framework + +#endif /* ROBOTIS_DEVICE_CONTROL_TABLE_ITEM_H_ */ diff --git a/humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device/device.h b/humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device/device.h new file mode 100755 index 0000000..c14deb0 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device/device.h @@ -0,0 +1,54 @@ +/******************************************************************************* +* Copyright 2018 ROBOTIS CO., LTD. +* +* 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. +*******************************************************************************/ + +/* + * device.h + * + * Created on: 2016. 5. 12. + * Author: zerom + */ + +#ifndef ROBOTIS_DEVICE_DEVICE_H_ +#define ROBOTIS_DEVICE_DEVICE_H_ + + +#include +#include +#include + +#include "control_table_item.h" + +namespace humanoid_robot_intelligence_control_system_framework +{ + +class Device +{ +public: + uint8_t id_; + float protocol_version_; + std::string model_name_; + std::string port_name_; + + std::map ctrl_table_; + std::vector bulk_read_items_; + + virtual ~Device() { } +}; + +} + + +#endif /* ROBOTIS_DEVICE_DEVICE_H_ */ diff --git a/humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device/dynamixel.h b/humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device/dynamixel.h new file mode 100755 index 0000000..329b626 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device/dynamixel.h @@ -0,0 +1,83 @@ +/******************************************************************************* +* Copyright 2018 ROBOTIS CO., LTD. +* +* 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. +*******************************************************************************/ + +/* + * dynamixel.h + * + * Created on: 2015. 12. 8. + * Author: zerom + */ + +#ifndef ROBOTIS_DEVICE_DYNAMIXEL_H_ +#define ROBOTIS_DEVICE_DYNAMIXEL_H_ + + +#include +#include +#include + +#include "control_table_item.h" +#include "device.h" +#include "dynamixel_state.h" + +namespace humanoid_robot_intelligence_control_system_framework +{ + +class Dynamixel : public Device +{ +public: + std::string ctrl_module_name_; + DynamixelState *dxl_state_; + + double velocity_to_value_ratio_; + double torque_to_current_value_ratio_; + + int32_t value_of_0_radian_position_; + int32_t value_of_min_radian_position_; + int32_t value_of_max_radian_position_; + double min_radian_; + double max_radian_; + + ControlTableItem *torque_enable_item_; + ControlTableItem *present_position_item_; + ControlTableItem *present_velocity_item_; + ControlTableItem *present_current_item_; + ControlTableItem *goal_position_item_; + ControlTableItem *goal_velocity_item_; + ControlTableItem *goal_current_item_; + ControlTableItem *position_p_gain_item_; + ControlTableItem *position_i_gain_item_; + ControlTableItem *position_d_gain_item_; + ControlTableItem *velocity_p_gain_item_; + ControlTableItem *velocity_i_gain_item_; + ControlTableItem *velocity_d_gain_item_; + + Dynamixel(int id, std::string model_name, float protocol_version); + + double convertValue2Radian(int32_t value); + int32_t convertRadian2Value(double radian); + + double convertValue2Velocity(int32_t value); + int32_t convertVelocity2Value(double velocity); + + double convertValue2Torque(int16_t value); + int16_t convertTorque2Value(double torque); +}; + +} + + +#endif /* ROBOTIS_DEVICE_DYNAMIXEL_H_ */ diff --git a/humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device/dynamixel_state.h b/humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device/dynamixel_state.h new file mode 100755 index 0000000..b915824 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device/dynamixel_state.h @@ -0,0 +1,82 @@ +/******************************************************************************* +* Copyright 2018 ROBOTIS CO., LTD. +* +* 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. +*******************************************************************************/ + +/* + * dynamixel_state.h + * + * Created on: 2015. 12. 8. + * Author: zerom + */ + +#ifndef ROBOTIS_DEVICE_DYNAMIXEL_STATE_H_ +#define ROBOTIS_DEVICE_DYNAMIXEL_STATE_H_ + +#include + +#include "time_stamp.h" + +#define INDIRECT_DATA_1 "indirect_data_1" +#define INDIRECT_ADDRESS_1 "indirect_address_1" + +namespace humanoid_robot_intelligence_control_system_framework +{ + +class DynamixelState +{ +public: + TimeStamp update_time_stamp_; + + double present_position_; + double present_velocity_; + double present_torque_; + double goal_position_; + double goal_velocity_; + double goal_torque_; + int position_p_gain_; + int position_i_gain_; + int position_d_gain_; + int velocity_p_gain_; + int velocity_i_gain_; + int velocity_d_gain_; + + std::map bulk_read_table_; + + double position_offset_; + + DynamixelState() + : update_time_stamp_(0, 0), + present_position_(0.0), + present_velocity_(0.0), + present_torque_(0.0), + goal_position_(0.0), + goal_velocity_(0.0), + goal_torque_(0.0), + position_p_gain_(65535), + position_i_gain_(65535), + position_d_gain_(65535), + velocity_p_gain_(65535), + velocity_i_gain_(65535), + velocity_d_gain_(65535), + position_offset_(0) + { + bulk_read_table_.clear(); + } +}; + +} + + +#endif /* ROBOTIS_DEVICE_DYNAMIXEL_STATE_H_ */ diff --git a/humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device/robot.h b/humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device/robot.h new file mode 100755 index 0000000..15350bd --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device/robot.h @@ -0,0 +1,72 @@ +/******************************************************************************* +* Copyright 2018 ROBOTIS CO., LTD. +* +* 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. +*******************************************************************************/ + +/* + * robot.h + * + * Created on: 2015. 12. 11. + * Author: zerom + */ + +#ifndef ROBOTIS_DEVICE_ROBOT_H_ +#define ROBOTIS_DEVICE_ROBOT_H_ + + +#include + +#include "sensor.h" +#include "dynamixel.h" +#include "dynamixel_sdk/port_handler.h" + +#define DYNAMIXEL "dynamixel" +#define SENSOR "sensor" + +#define SESSION_CONTROL_INFO "control info" +#define SESSION_PORT_INFO "port info" +#define SESSION_DEVICE_INFO "device info" + +#define SESSION_TYPE_INFO "type info" +#define SESSION_CONTROL_TABLE "control table" + +#define DEFAULT_CONTROL_CYCLE 8 // milliseconds + +namespace humanoid_robot_intelligence_control_system_framework +{ + +class Robot +{ +private: + int control_cycle_msec_; + +public: + std::map ports_; // string: port name + std::map port_default_device_; // port name, default device name + + std::map dxls_; // string: joint name + std::map sensors_; // string: sensor name + + Robot(std::string robot_file_path, std::string dev_desc_dir_path); + + Sensor *getSensor(std::string path, int id, std::string port, float protocol_version); + Dynamixel *getDynamixel(std::string path, int id, std::string port, float protocol_version); + + int getControlCycle(); +}; + +} + + +#endif /* ROBOTIS_DEVICE_ROBOT_H_ */ diff --git a/humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device/sensor.h b/humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device/sensor.h new file mode 100755 index 0000000..32abe6c --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device/sensor.h @@ -0,0 +1,49 @@ +/******************************************************************************* +* Copyright 2018 ROBOTIS CO., LTD. +* +* 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. +*******************************************************************************/ + +/* + * sensor.h + * + * Created on: 2015. 12. 16. + * Author: zerom + */ + +#ifndef ROBOTIS_DEVICE_SENSOR_H_ +#define ROBOTIS_DEVICE_SENSOR_H_ + +#include +#include +#include + +#include "device.h" +#include "sensor_state.h" +#include "control_table_item.h" + +namespace humanoid_robot_intelligence_control_system_framework +{ + +class Sensor : public Device +{ +public: + SensorState *sensor_state_; + + Sensor(int id, std::string model_name, float protocol_version); +}; + +} + + +#endif /* ROBOTIS_DEVICE_SENSOR_H_ */ diff --git a/humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device/sensor_state.h b/humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device/sensor_state.h new file mode 100755 index 0000000..08f61ba --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device/sensor_state.h @@ -0,0 +1,50 @@ +/******************************************************************************* +* Copyright 2018 ROBOTIS CO., LTD. +* +* 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. +*******************************************************************************/ + +/* + * sensor_state.h + * + * Created on: 2016. 5. 16. + * Author: zerom + */ + +#ifndef ROBOTIS_DEVICE_SENSOR_STATE_H_ +#define ROBOTIS_DEVICE_SENSOR_STATE_H_ + + +#include "time_stamp.h" + +namespace humanoid_robot_intelligence_control_system_framework +{ + +class SensorState +{ +public: + TimeStamp update_time_stamp_; + + std::map bulk_read_table_; + + SensorState() + : update_time_stamp_(0, 0) + { + bulk_read_table_.clear(); + } +}; + +} + + +#endif /* ROBOTIS_DEVICE_SENSOR_STATE_H_ */ diff --git a/humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device/time_stamp.h b/humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device/time_stamp.h new file mode 100755 index 0000000..87f59f4 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device/time_stamp.h @@ -0,0 +1,39 @@ +/******************************************************************************* + * Copyright 2018 ROBOTIS CO., LTD. + * + * 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. + *******************************************************************************/ + +/* + * time_stamp.h + * + * Created on: 2016. 5. 16. + * Author: zerom + */ + +#ifndef ROBOTIS_DEVICE_TIME_STAMP_H_ +#define ROBOTIS_DEVICE_TIME_STAMP_H_ + +namespace humanoid_robot_intelligence_control_system_framework { + +class TimeStamp { +public: + long sec_; + long nsec_; + + TimeStamp(long sec, long nsec) : sec_(sec), nsec_(nsec) {} +}; + +} // namespace humanoid_robot_intelligence_control_system_framework + +#endif /* ROBOTIS_DEVICE_TIME_STAMP_H_ */ diff --git a/humanoid_robot_intelligence_control_system_device/package.xml b/humanoid_robot_intelligence_control_system_device/package.xml new file mode 100755 index 0000000..b21c17d --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/package.xml @@ -0,0 +1,38 @@ + + + humanoid_robot_intelligence_control_system_device + 0.3.2 + + The package that manages device information of ROBOTIS robots. + This package is used when reading device information with the robot information file + from the humanoid_robot_intelligence_control_system_controller package. + + Apache 2.0 + + Ronaldson Bellande + + + catkin + + roscpp + dynamixel_sdk + + roscpp + dynamixel_sdk + + roscpp + dynamixel_sdk + + + ament_cmake + + roscpp + dynamixel_sdk + + roscpp + dynamixel_sdk + + roscpp + dynamixel_sdk + + diff --git a/humanoid_robot_intelligence_control_system_device/src/humanoid_robot_intelligence_control_system_device/dynamixel.cpp b/humanoid_robot_intelligence_control_system_device/src/humanoid_robot_intelligence_control_system_device/dynamixel.cpp new file mode 100755 index 0000000..95a01ca --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/src/humanoid_robot_intelligence_control_system_device/dynamixel.cpp @@ -0,0 +1,119 @@ +/******************************************************************************* + * Copyright 2018 ROBOTIS CO., LTD. + * + * 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. + *******************************************************************************/ + +/* + * dynamixel.cpp + * + * Created on: 2015. 12. 8. + * Author: zerom + */ + +#include "humanoid_robot_intelligence_control_system_device/dynamixel.h" + +using namespace humanoid_robot_intelligence_control_system_framework; + +Dynamixel::Dynamixel(int id, std::string model_name, float protocol_version) + : ctrl_module_name_("none"), torque_to_current_value_ratio_(1.0), + velocity_to_value_ratio_(1.0), value_of_0_radian_position_(0), + value_of_min_radian_position_(0), value_of_max_radian_position_(0), + min_radian_(-3.14159265), max_radian_(3.14159265), torque_enable_item_(0), + present_position_item_(0), present_velocity_item_(0), + present_current_item_(0), goal_position_item_(0), goal_velocity_item_(0), + goal_current_item_(0), position_p_gain_item_(0), position_i_gain_item_(0), + position_d_gain_item_(0), velocity_p_gain_item_(0), + velocity_i_gain_item_(0), velocity_d_gain_item_(0) { + this->id_ = id; + this->model_name_ = model_name; + this->port_name_ = ""; + this->protocol_version_ = protocol_version; + + ctrl_table_.clear(); + dxl_state_ = new DynamixelState(); + + bulk_read_items_.clear(); +} + +double Dynamixel::convertValue2Radian(int32_t value) { + double radian = 0.0; + if (value > value_of_0_radian_position_) { + if (max_radian_ <= 0) + return max_radian_; + + radian = + (double)(value - value_of_0_radian_position_) * max_radian_ / + (double)(value_of_max_radian_position_ - value_of_0_radian_position_); + } else if (value < value_of_0_radian_position_) { + if (min_radian_ >= 0) + return min_radian_; + + radian = + (double)(value - value_of_0_radian_position_) * min_radian_ / + (double)(value_of_min_radian_position_ - value_of_0_radian_position_); + } + + // if (radian > max_radian_) + // return max_radian_; + // else if (radian < min_radian_) + // return min_radian_; + + return radian; +} + +int32_t Dynamixel::convertRadian2Value(double radian) { + int32_t value = 0; + if (radian > 0) { + if (value_of_max_radian_position_ <= value_of_0_radian_position_) + return value_of_max_radian_position_; + + value = (radian * + (value_of_max_radian_position_ - value_of_0_radian_position_) / + max_radian_) + + value_of_0_radian_position_; + } else if (radian < 0) { + if (value_of_min_radian_position_ >= value_of_0_radian_position_) + return value_of_min_radian_position_; + + value = (radian * + (value_of_min_radian_position_ - value_of_0_radian_position_) / + min_radian_) + + value_of_0_radian_position_; + } else + value = value_of_0_radian_position_; + + // if (value > value_of_max_radian_position_) + // return value_of_max_radian_position_; + // else if (value < value_of_min_radian_position_) + // return value_of_min_radian_position_; + + return value; +} + +double Dynamixel::convertValue2Velocity(int32_t value) { + return (double)value / velocity_to_value_ratio_; +} + +int32_t Dynamixel::convertVelocity2Value(double velocity) { + return (int32_t)(velocity * velocity_to_value_ratio_); + ; +} + +double Dynamixel::convertValue2Torque(int16_t value) { + return (double)value / torque_to_current_value_ratio_; +} + +int16_t Dynamixel::convertTorque2Value(double torque) { + return (int16_t)(torque * torque_to_current_value_ratio_); +} diff --git a/humanoid_robot_intelligence_control_system_device/src/humanoid_robot_intelligence_control_system_device/robot.cpp b/humanoid_robot_intelligence_control_system_device/src/humanoid_robot_intelligence_control_system_device/robot.cpp new file mode 100755 index 0000000..c930e86 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/src/humanoid_robot_intelligence_control_system_device/robot.cpp @@ -0,0 +1,480 @@ +/******************************************************************************* + * Copyright 2018 ROBOTIS CO., LTD. + * + * 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. + *******************************************************************************/ + +/* + * robot.cpp + * + * Created on: 2015. 12. 11. + * Author: zerom + */ + +#include +#include +#include + +#include "humanoid_robot_intelligence_control_system_device/robot.h" + +using namespace humanoid_robot_intelligence_control_system_framework; + +static inline std::string <rim(std::string &s) { + s.erase(s.begin(), + std::find_if(s.begin(), s.end(), + std::not1(std::ptr_fun(std::isspace)))); + return s; +} +static inline std::string &rtrim(std::string &s) { + s.erase(std::find_if(s.rbegin(), s.rend(), + std::not1(std::ptr_fun(std::isspace))) + .base(), + s.end()); + return s; +} +static inline std::string &trim(std::string &s) { return ltrim(rtrim(s)); } + +static inline std::vector split(const std::string &text, + char sep) { + std::vector tokens; + std::size_t start = 0, end = 0; + + while ((end = text.find(sep, start)) != (std::string::npos)) { + tokens.push_back(text.substr(start, end - start)); + trim(tokens.back()); + start = end + 1; + } + tokens.push_back(text.substr(start)); + trim(tokens.back()); + + return tokens; +} + +Robot::Robot(std::string robot_file_path, std::string dev_desc_dir_path) + : control_cycle_msec_(DEFAULT_CONTROL_CYCLE) { + if (dev_desc_dir_path.compare(dev_desc_dir_path.size() - 1, 1, "/") != 0) + dev_desc_dir_path += "/"; + + std::ifstream file(robot_file_path.c_str()); + if (file.is_open()) { + std::string session = ""; + std::string input_str; + while (!file.eof()) { + std::getline(file, input_str); + + // remove comment ( # ) + std::size_t pos = input_str.find("#"); + if (pos != std::string::npos) + input_str = input_str.substr(0, pos); + + // trim + input_str = trim(input_str); + + // find session + if (!input_str.compare(0, 1, "[") && + !input_str.compare(input_str.size() - 1, 1, "]")) { + input_str = input_str.substr(1, input_str.size() - 2); + std::transform(input_str.begin(), input_str.end(), input_str.begin(), + ::tolower); + session = trim(input_str); + continue; + } + + if (session == SESSION_CONTROL_INFO) { + std::vector tokens = split(input_str, '='); + if (tokens.size() != 2) + continue; + + if (tokens[0] == "control_cycle") + control_cycle_msec_ = std::atoi(tokens[1].c_str()); + } else if (session == SESSION_PORT_INFO) { + std::vector tokens = split(input_str, '|'); + if (tokens.size() != 3) + continue; + + std::cout << tokens[0] << " added. (baudrate: " << tokens[1] << ")" + << std::endl; + + ports_[tokens[0]] = + dynamixel::PortHandler::getPortHandler(tokens[0].c_str()); + ports_[tokens[0]]->setBaudRate(std::atoi(tokens[1].c_str())); + port_default_device_[tokens[0]] = tokens[2]; + } else if (session == SESSION_DEVICE_INFO) { + std::vector tokens = split(input_str, '|'); + if (tokens.size() != 7) + continue; + + if (tokens[0] == DYNAMIXEL) { + std::string file_path = + dev_desc_dir_path + tokens[0] + "/" + tokens[3] + ".device"; + int id = std::atoi(tokens[2].c_str()); + std::string port = tokens[1]; + float protocol = std::atof(tokens[4].c_str()); + std::string dev_name = tokens[5]; + + dxls_[dev_name] = getDynamixel(file_path, id, port, protocol); + + Dynamixel *dxl = dxls_[dev_name]; + std::vector sub_tokens = split(tokens[6], ','); + if (sub_tokens.size() > 0 && sub_tokens[0] != "") { + std::map::iterator indirect_it = + dxl->ctrl_table_.find(INDIRECT_ADDRESS_1); + if (indirect_it != + dxl->ctrl_table_.end()) // INDIRECT_ADDRESS_1 exist + { + uint16_t indirect_data_addr = + dxl->ctrl_table_[INDIRECT_DATA_1]->address_; + for (int _i = 0; _i < sub_tokens.size(); _i++) { + std::map::iterator + bulkread_it = dxl->ctrl_table_.find(sub_tokens[_i]); + if (bulkread_it == dxl->ctrl_table_.end()) { + fprintf( + stderr, + "\n ##### BULK READ ITEM [ %s ] NOT FOUND!! #####\n\n", + sub_tokens[_i].c_str()); + continue; + } + + dxl->bulk_read_items_.push_back(new ControlTableItem()); + ControlTableItem *dest_item = dxl->bulk_read_items_[_i]; + ControlTableItem *src_item = dxl->ctrl_table_[sub_tokens[_i]]; + + dest_item->item_name_ = src_item->item_name_; + dest_item->address_ = indirect_data_addr; + dest_item->access_type_ = src_item->access_type_; + dest_item->memory_type_ = src_item->memory_type_; + dest_item->data_length_ = src_item->data_length_; + dest_item->data_min_value_ = src_item->data_min_value_; + dest_item->data_max_value_ = src_item->data_max_value_; + dest_item->is_signed_ = src_item->is_signed_; + + indirect_data_addr += dest_item->data_length_; + } + } else // INDIRECT_ADDRESS_1 not exist + { + for (int i = 0; i < sub_tokens.size(); i++) { + if (dxl->ctrl_table_[sub_tokens[i]] != NULL) + dxl->bulk_read_items_.push_back( + dxl->ctrl_table_[sub_tokens[i]]); + } + } + } + } else if (tokens[0] == SENSOR) { + std::string file_path = + dev_desc_dir_path + tokens[0] + "/" + tokens[3] + ".device"; + int id = std::atoi(tokens[2].c_str()); + std::string port = tokens[1]; + float protocol = std::atof(tokens[4].c_str()); + std::string dev_name = tokens[5]; + + sensors_[dev_name] = getSensor(file_path, id, port, protocol); + + Sensor *sensor = sensors_[dev_name]; + std::vector sub_tokens = split(tokens[6], ','); + if (sub_tokens.size() > 0 && sub_tokens[0] != "") { + std::map::iterator indirect_it = + sensor->ctrl_table_.find(INDIRECT_ADDRESS_1); + if (indirect_it != + sensor->ctrl_table_.end()) // INDIRECT_ADDRESS_1 exist + { + uint16_t indirect_data_addr = + sensor->ctrl_table_[INDIRECT_DATA_1]->address_; + for (int i = 0; i < sub_tokens.size(); i++) { + sensor->bulk_read_items_.push_back(new ControlTableItem()); + ControlTableItem *dest_item = sensor->bulk_read_items_[i]; + ControlTableItem *src_item = sensor->ctrl_table_[sub_tokens[i]]; + + dest_item->item_name_ = src_item->item_name_; + dest_item->address_ = indirect_data_addr; + dest_item->access_type_ = src_item->access_type_; + dest_item->memory_type_ = src_item->memory_type_; + dest_item->data_length_ = src_item->data_length_; + dest_item->data_min_value_ = src_item->data_min_value_; + dest_item->data_max_value_ = src_item->data_max_value_; + dest_item->is_signed_ = src_item->is_signed_; + + indirect_data_addr += dest_item->data_length_; + } + } else // INDIRECT_ADDRESS_1 exist + { + for (int i = 0; i < sub_tokens.size(); i++) + sensor->bulk_read_items_.push_back( + sensor->ctrl_table_[sub_tokens[i]]); + } + } + } + } + } + file.close(); + } else { + std::cout << "Unable to open file : " + robot_file_path << std::endl; + } +} + +Sensor *Robot::getSensor(std::string path, int id, std::string port, + float protocol_version) { + Sensor *sensor; + + // load file model_name.device + std::ifstream file(path.c_str()); + if (file.is_open()) { + std::string session = ""; + std::string input_str; + + while (!file.eof()) { + std::getline(file, input_str); + + // remove comment ( # ) + std::size_t pos = input_str.find("#"); + if (pos != std::string::npos) + input_str = input_str.substr(0, pos); + + // trim + input_str = trim(input_str); + if (input_str == "") + continue; + + // find _session + if (!input_str.compare(0, 1, "[") && + !input_str.compare(input_str.size() - 1, 1, "]")) { + input_str = input_str.substr(1, input_str.size() - 2); + std::transform(input_str.begin(), input_str.end(), input_str.begin(), + ::tolower); + session = trim(input_str); + continue; + } + + if (session == SESSION_DEVICE_INFO) { + std::vector tokens = split(input_str, '='); + if (tokens.size() != 2) + continue; + + if (tokens[0] == "model_name") + sensor = new Sensor(id, tokens[1], protocol_version); + } else if (session == SESSION_CONTROL_TABLE) { + std::vector tokens = split(input_str, '|'); + if (tokens.size() != 8) + continue; + + ControlTableItem *item = new ControlTableItem(); + item->item_name_ = tokens[1]; + item->address_ = std::atoi(tokens[0].c_str()); + item->data_length_ = std::atoi(tokens[2].c_str()); + + if (tokens[3] == "R") + item->access_type_ = Read; + else if (tokens[3] == "RW") + item->access_type_ = ReadWrite; + + if (tokens[4] == "EEPROM") + item->memory_type_ = EEPROM; + else if (tokens[4] == "RAM") + item->memory_type_ = RAM; + + item->data_min_value_ = std::atol(tokens[5].c_str()); + item->data_max_value_ = std::atol(tokens[6].c_str()); + + if (tokens[7] == "Y") + item->is_signed_ = true; + else if (tokens[7] == "N") + item->is_signed_ = false; + sensor->ctrl_table_[tokens[1]] = item; + } + } + sensor->port_name_ = port; + + fprintf(stderr, "(%s) [ID:%3d] %14s added. \n", port.c_str(), sensor->id_, + sensor->model_name_.c_str()); + // std::cout << "[ID:" << (int)(_sensor->id) << "] " << _sensor->model_name + // << " added. (" << port << ")" << std::endl; + file.close(); + } else + std::cout << "Unable to open file : " + path << std::endl; + + return sensor; +} + +Dynamixel *Robot::getDynamixel(std::string path, int id, std::string port, + float protocol_version) { + Dynamixel *dxl; + + // load file model_name.device + std::ifstream file(path.c_str()); + if (file.is_open()) { + std::string session = ""; + std::string input_str; + + std::string torque_enable_item_name = ""; + std::string present_position_item_name = ""; + std::string present_velocity_item_name = ""; + std::string present_current_item_name = ""; + std::string goal_position_item_name = ""; + std::string goal_velocity_item_name = ""; + std::string goal_current_item_name = ""; + std::string position_d_gain_item_name = ""; + std::string position_i_gain_item_name = ""; + std::string position_p_gain_item_name = ""; + std::string velocity_d_gain_item_name = ""; + std::string velocity_i_gain_item_name = ""; + std::string velocity_p_gain_item_name = ""; + + while (!file.eof()) { + std::getline(file, input_str); + + // remove comment ( # ) + std::size_t pos = input_str.find("#"); + if (pos != std::string::npos) + input_str = input_str.substr(0, pos); + + // trim + input_str = trim(input_str); + if (input_str == "") + continue; + + // find session + if (!input_str.compare(0, 1, "[") && + !input_str.compare(input_str.size() - 1, 1, "]")) { + input_str = input_str.substr(1, input_str.size() - 2); + std::transform(input_str.begin(), input_str.end(), input_str.begin(), + ::tolower); + session = trim(input_str); + continue; + } + + if (session == SESSION_DEVICE_INFO) { + std::vector tokens = split(input_str, '='); + if (tokens.size() != 2) + continue; + + if (tokens[0] == "model_name") + dxl = new Dynamixel(id, tokens[1], protocol_version); + } else if (session == SESSION_TYPE_INFO) { + std::vector tokens = split(input_str, '='); + if (tokens.size() != 2) + continue; + + if (tokens[0] == "torque_to_current_value_ratio") + dxl->torque_to_current_value_ratio_ = std::atof(tokens[1].c_str()); + else if (tokens[0] == "velocity_to_value_ratio") + dxl->velocity_to_value_ratio_ = std::atof(tokens[1].c_str()); + + else if (tokens[0] == "value_of_0_radian_position") + dxl->value_of_0_radian_position_ = std::atoi(tokens[1].c_str()); + else if (tokens[0] == "value_of_min_radian_position") + dxl->value_of_min_radian_position_ = std::atoi(tokens[1].c_str()); + else if (tokens[0] == "value_of_max_radian_position") + dxl->value_of_max_radian_position_ = std::atoi(tokens[1].c_str()); + else if (tokens[0] == "min_radian") + dxl->min_radian_ = std::atof(tokens[1].c_str()); + else if (tokens[0] == "max_radian") + dxl->max_radian_ = std::atof(tokens[1].c_str()); + + else if (tokens[0] == "torque_enable_item_name") + torque_enable_item_name = tokens[1]; + else if (tokens[0] == "present_position_item_name") + present_position_item_name = tokens[1]; + else if (tokens[0] == "present_velocity_item_name") + present_velocity_item_name = tokens[1]; + else if (tokens[0] == "present_current_item_name") + present_current_item_name = tokens[1]; + else if (tokens[0] == "goal_position_item_name") + goal_position_item_name = tokens[1]; + else if (tokens[0] == "goal_velocity_item_name") + goal_velocity_item_name = tokens[1]; + else if (tokens[0] == "goal_current_item_name") + goal_current_item_name = tokens[1]; + else if (tokens[0] == "position_d_gain_item_name") + position_d_gain_item_name = tokens[1]; + else if (tokens[0] == "position_i_gain_item_name") + position_i_gain_item_name = tokens[1]; + else if (tokens[0] == "position_p_gain_item_name") + position_p_gain_item_name = tokens[1]; + else if (tokens[0] == "velocity_d_gain_item_name") + velocity_d_gain_item_name = tokens[1]; + else if (tokens[0] == "velocity_i_gain_item_name") + velocity_i_gain_item_name = tokens[1]; + else if (tokens[0] == "velocity_p_gain_item_name") + velocity_p_gain_item_name = tokens[1]; + } else if (session == SESSION_CONTROL_TABLE) { + std::vector tokens = split(input_str, '|'); + if (tokens.size() != 8) + continue; + + ControlTableItem *item = new ControlTableItem(); + item->item_name_ = tokens[1]; + item->address_ = std::atoi(tokens[0].c_str()); + item->data_length_ = std::atoi(tokens[2].c_str()); + + if (tokens[3] == "R") + item->access_type_ = Read; + else if (tokens[3] == "RW") + item->access_type_ = ReadWrite; + + if (tokens[4] == "EEPROM") + item->memory_type_ = EEPROM; + else if (tokens[4] == "RAM") + item->memory_type_ = RAM; + + item->data_min_value_ = std::atol(tokens[5].c_str()); + item->data_max_value_ = std::atol(tokens[6].c_str()); + + if (tokens[7] == "Y") + item->is_signed_ = true; + else if (tokens[7] == "N") + item->is_signed_ = false; + dxl->ctrl_table_[tokens[1]] = item; + } + } + dxl->port_name_ = port; + + if (dxl->ctrl_table_[torque_enable_item_name] != NULL) + dxl->torque_enable_item_ = dxl->ctrl_table_[torque_enable_item_name]; + if (dxl->ctrl_table_[present_position_item_name] != NULL) + dxl->present_position_item_ = + dxl->ctrl_table_[present_position_item_name]; + if (dxl->ctrl_table_[present_velocity_item_name] != NULL) + dxl->present_velocity_item_ = + dxl->ctrl_table_[present_velocity_item_name]; + if (dxl->ctrl_table_[present_current_item_name] != NULL) + dxl->present_current_item_ = dxl->ctrl_table_[present_current_item_name]; + if (dxl->ctrl_table_[goal_position_item_name] != NULL) + dxl->goal_position_item_ = dxl->ctrl_table_[goal_position_item_name]; + if (dxl->ctrl_table_[goal_velocity_item_name] != NULL) + dxl->goal_velocity_item_ = dxl->ctrl_table_[goal_velocity_item_name]; + if (dxl->ctrl_table_[goal_current_item_name] != NULL) + dxl->goal_current_item_ = dxl->ctrl_table_[goal_current_item_name]; + if (dxl->ctrl_table_[position_d_gain_item_name] != NULL) + dxl->position_d_gain_item_ = dxl->ctrl_table_[position_d_gain_item_name]; + if (dxl->ctrl_table_[position_i_gain_item_name] != NULL) + dxl->position_i_gain_item_ = dxl->ctrl_table_[position_i_gain_item_name]; + if (dxl->ctrl_table_[position_p_gain_item_name] != NULL) + dxl->position_p_gain_item_ = dxl->ctrl_table_[position_p_gain_item_name]; + if (dxl->ctrl_table_[velocity_d_gain_item_name] != NULL) + dxl->velocity_d_gain_item_ = dxl->ctrl_table_[velocity_d_gain_item_name]; + if (dxl->ctrl_table_[velocity_i_gain_item_name] != NULL) + dxl->velocity_i_gain_item_ = dxl->ctrl_table_[velocity_i_gain_item_name]; + if (dxl->ctrl_table_[velocity_p_gain_item_name] != NULL) + dxl->velocity_p_gain_item_ = dxl->ctrl_table_[velocity_p_gain_item_name]; + + fprintf(stderr, "(%s) [ID:%3d] %14s added. \n", port.c_str(), dxl->id_, + dxl->model_name_.c_str()); + // std::cout << "[ID:" << (int)(_dxl->id) << "] " << _dxl->model_name << " + // added. (" << port << ")" << std::endl; + file.close(); + } else + std::cout << "Unable to open file : " + path << std::endl; + + return dxl; +} + +int Robot::getControlCycle() { return control_cycle_msec_; } diff --git a/humanoid_robot_intelligence_control_system_device/src/humanoid_robot_intelligence_control_system_device/sensor.cpp b/humanoid_robot_intelligence_control_system_device/src/humanoid_robot_intelligence_control_system_device/sensor.cpp new file mode 100755 index 0000000..6d5e619 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/src/humanoid_robot_intelligence_control_system_device/sensor.cpp @@ -0,0 +1,39 @@ +/******************************************************************************* + * Copyright 2018 ROBOTIS CO., LTD. + * + * 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. + *******************************************************************************/ + +/* + * sensor.cpp + * + * Created on: 2016. 5. 11. + * Author: zerom + */ + +#include "humanoid_robot_intelligence_control_system_device/sensor.h" + +using namespace humanoid_robot_intelligence_control_system_framework; + +Sensor::Sensor(int id, std::string model_name, float protocol_version) { + + this->id_ = id; + this->model_name_ = model_name; + this->port_name_ = ""; + this->protocol_version_ = protocol_version; + ctrl_table_.clear(); + + sensor_state_ = new SensorState(); + + bulk_read_items_.clear(); +} diff --git a/humanoid_robot_intelligence_control_system_framework_common/CHANGELOG.rst b/humanoid_robot_intelligence_control_system_framework_common/CHANGELOG.rst new file mode 100755 index 0000000..0af7c89 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_framework_common/CHANGELOG.rst @@ -0,0 +1,82 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package humanoid_robot_intelligence_control_system_framework_common +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.3.2 (2023-10-03) +------------------ +* Make it compatible for ROS1/ROS2 +* Fix bugs +* Update package.xml and CMakeList.txt for to the latest versions +* Contributors & Maintainer: Ronaldson Bellande + +0.3.1 (2023-09-27) +------------------ +* Starting from this point it under a new license +* Fix errors and Issues +* Rename Repository for a completely different purpose +* Make it compatible with ROS/ROS2 +* Upgrade version of all builds and make it more compatible +* Update package.xml and CMakeList.txt for to the latest versions +* Contributors & Maintainer: Ronaldson Bellande + +0.2.9 (2018-03-22) +------------------ +* none + +0.2.8 (2018-03-20) +------------------ +* tested for system dependencies +* Contributors: Pyo + +0.2.7 (2018-03-15) +------------------ +* change the License and package format to version 2 +* Contributors: Pyo + +0.2.6 (2017-08-09) +------------------ +* none + +0.2.5 (2017-06-09) +------------------ +* none + +0.2.4 (2017-06-07) +------------------ +* none + +0.2.3 (2017-05-23) +------------------ +* updated the cmake file for ros install +* Contributors: SCH + +0.2.2 (2017-04-24) +------------------ +* updated for other packages +* Contributors: Zerom, Kayman + +0.2.1 (2016-11-23) +------------------ +* Merge the changes and update +* - dependencies fixed. (Pull requests `#26 `_) +* - modified dependency problem. +* Contributors: Jay Song, Pyo, Zerom + +0.2.0 (2016-08-31) +------------------ +* updated CHANGLOG.rst for minor release +* rename ControlMode(CurrentControl -> TorqueControl) +* rename (port_to_sync_write_torque\_ -> port_to_sync_write_current\_) +* Contributors: Zerom, Pyo + +0.1.1 (2016-08-18) +------------------ +* updated the package information + +0.1.0 (2016-08-12) +------------------ +* modified the package information for release +* Setting the license to BSD. +* add SensorState + add Singleton template +* Contributors: Jay Song, Zerom, Pyo diff --git a/humanoid_robot_intelligence_control_system_framework_common/CMakeLists.txt b/humanoid_robot_intelligence_control_system_framework_common/CMakeLists.txt new file mode 100755 index 0000000..d90d27c --- /dev/null +++ b/humanoid_robot_intelligence_control_system_framework_common/CMakeLists.txt @@ -0,0 +1,46 @@ +cmake_minimum_required(VERSION 3.8) +project(humanoid_robot_intelligence_control_system_framework_common) + + +if($ENV{ROS_VERSION} EQUAL 1) + find_package(catkin REQUIRED COMPONENTS + roscpp + humanoid_robot_intelligence_control_system_device + ) +else() + find_package(ament_cmake REQUIRED) +endif() + + +if($ENV{ROS_VERSION} EQUAL 1) + catkin_package( + INCLUDE_DIRS include + LIBRARIES ${PROJECT_NAME} + CATKIN_DEPENDS + roscpp + humanoid_robot_intelligence_control_system_device + ) +endif() + + +include_directories( + include + ${catkin_INCLUDE_DIRS} +) + +add_library(${PROJECT_NAME} + include/${PROJECT_NAME}/motion_module.h + include/${PROJECT_NAME}/sensor_module.h + include/${PROJECT_NAME}/singleton.h +) +set_target_properties(${PROJECT_NAME} PROPERTIES LINKER_LANGUAGE CXX) + +install(TARGETS humanoid_robot_intelligence_control_system_framework_common + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) diff --git a/humanoid_robot_intelligence_control_system_framework_common/include/humanoid_robot_intelligence_control_system_framework_common/motion_module.h b/humanoid_robot_intelligence_control_system_framework_common/include/humanoid_robot_intelligence_control_system_framework_common/motion_module.h new file mode 100755 index 0000000..9df7326 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_framework_common/include/humanoid_robot_intelligence_control_system_framework_common/motion_module.h @@ -0,0 +1,87 @@ +/******************************************************************************* +* Copyright 2018 ROBOTIS CO., LTD. +* +* 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. +*******************************************************************************/ + +/* + * motion_module.h + * + * Created on: 2016. 1. 15. + * Author: zerom + */ + +#ifndef ROBOTIS_FRAMEWORK_COMMON_MOTION_MODULE_H_ +#define ROBOTIS_FRAMEWORK_COMMON_MOTION_MODULE_H_ + + +#include +#include + +#include "singleton.h" +#include "humanoid_robot_intelligence_control_system_device/robot.h" +#include "humanoid_robot_intelligence_control_system_device/dynamixel.h" + +namespace humanoid_robot_intelligence_control_system_framework +{ + +enum ControlMode +{ + PositionControl, + VelocityControl, + TorqueControl +}; + +class MotionModule +{ +protected: + bool enable_; + std::string module_name_; + ControlMode control_mode_; + +public: + std::map result_; + + virtual ~MotionModule() { } + + std::string getModuleName() { return module_name_; } + ControlMode getControlMode() { return control_mode_; } + + void setModuleEnable(bool enable) + { + if(this->enable_ == enable) + return; + + this->enable_ = enable; + if(enable) + onModuleEnable(); + else + onModuleDisable(); + } + bool getModuleEnable() { return enable_; } + + virtual void onModuleEnable() { } + virtual void onModuleDisable() { } + + virtual void initialize(const int control_cycle_msec, Robot *robot) = 0; + virtual void process(std::map dxls, std::map sensors) = 0; + + virtual void stop() = 0; + virtual bool isRunning() = 0; +}; + + +} + + +#endif /* ROBOTIS_FRAMEWORK_COMMON_MOTION_MODULE_H_ */ diff --git a/humanoid_robot_intelligence_control_system_framework_common/include/humanoid_robot_intelligence_control_system_framework_common/sensor_module.h b/humanoid_robot_intelligence_control_system_framework_common/include/humanoid_robot_intelligence_control_system_framework_common/sensor_module.h new file mode 100755 index 0000000..23e6292 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_framework_common/include/humanoid_robot_intelligence_control_system_framework_common/sensor_module.h @@ -0,0 +1,57 @@ +/******************************************************************************* +* Copyright 2018 ROBOTIS CO., LTD. +* +* 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. +*******************************************************************************/ + +/* + * sensor_module.h + * + * Created on: 2016. 3. 30. + * Author: zerom + */ + +#ifndef ROBOTIS_FRAMEWORK_COMMON_SENSOR_MODULE_H_ +#define ROBOTIS_FRAMEWORK_COMMON_SENSOR_MODULE_H_ + + +#include +#include + +#include "singleton.h" +#include "humanoid_robot_intelligence_control_system_device/robot.h" +#include "humanoid_robot_intelligence_control_system_device/dynamixel.h" + +namespace humanoid_robot_intelligence_control_system_framework +{ + +class SensorModule +{ +protected: + std::string module_name_; + +public: + std::map result_; + + virtual ~SensorModule() { } + + std::string getModuleName() { return module_name_; } + + virtual void initialize(const int control_cycle_msec, Robot *robot) = 0; + virtual void process(std::map dxls, std::map sensors) = 0; +}; + +} + + +#endif /* ROBOTIS_FRAMEWORK_COMMON_SENSOR_MODULE_H_ */ diff --git a/humanoid_robot_intelligence_control_system_framework_common/include/humanoid_robot_intelligence_control_system_framework_common/singleton.h b/humanoid_robot_intelligence_control_system_framework_common/include/humanoid_robot_intelligence_control_system_framework_common/singleton.h new file mode 100755 index 0000000..5f1c4ab --- /dev/null +++ b/humanoid_robot_intelligence_control_system_framework_common/include/humanoid_robot_intelligence_control_system_framework_common/singleton.h @@ -0,0 +1,65 @@ +/******************************************************************************* +* Copyright 2018 ROBOTIS CO., LTD. +* +* 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. +*******************************************************************************/ + +/* + * singleton.h + * + * Created on: 2016. 5. 17. + * Author: zerom + */ + +#ifndef ROBOTIS_FRAMEWORK_COMMON_SINGLETON_H_ +#define ROBOTIS_FRAMEWORK_COMMON_SINGLETON_H_ + + +namespace humanoid_robot_intelligence_control_system_framework +{ + +template +class Singleton +{ +private: + static T *unique_instance_; + +protected: + Singleton() { } + Singleton(Singleton const&) { } + Singleton& operator=(Singleton const&) { return *this; } + +public: + static T* getInstance() + { + if(unique_instance_ == NULL) + unique_instance_ = new T; + return unique_instance_; + } + + static void destroyInstance() + { + if(unique_instance_) + { + delete unique_instance_; + unique_instance_ = NULL; + } + } +}; + +template T* Singleton::unique_instance_ = NULL; + +} + + +#endif /* ROBOTIS_FRAMEWORK_COMMON_SINGLETON_H_ */ diff --git a/humanoid_robot_intelligence_control_system_framework_common/package.xml b/humanoid_robot_intelligence_control_system_framework_common/package.xml new file mode 100755 index 0000000..c12c3b2 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_framework_common/package.xml @@ -0,0 +1,33 @@ + + + humanoid_robot_intelligence_control_system_framework_common + 0.3.2 + The package contains commonly used headers for the humanoid_robot_intelligence_control_system_framework + Apache 2.0 + Ronaldson Bellande + + + catkin + + roscpp + humanoid_robot_intelligence_control_system_device + + roscpp + humanoid_robot_intelligence_control_system_device + + roscpp + humanoid_robot_intelligence_control_system_device + + + ament_cmake + + roscpp + humanoid_robot_intelligence_control_system_device + + roscpp + humanoid_robot_intelligence_control_system_device + + roscpp + humanoid_robot_intelligence_control_system_device + + From 31a4027b4298801bde48a11e7ca547a3950e67ff Mon Sep 17 00:00:00 2001 From: RonaldsonBellande Date: Mon, 13 Nov 2023 17:55:45 -0500 Subject: [PATCH 213/238] latest pushes --- .travis.yml | 2 +- README.md | 24 +- .../CHANGELOG.rst | 136 + .../CMakeLists.txt | 83 + ...t_intelligence_control_system_controller.h | 209 ++ .../package.xml | 81 + ...intelligence_control_system_controller.cpp | 2625 +++++++++++++++++ .../CHANGELOG.rst | 110 + .../CMakeLists.txt | 51 + .../devices/dynamixel/GRIPPER.device | 73 + .../devices/dynamixel/GRIPPER_TORQUE.device | 74 + .../devices/dynamixel/H42-20-S300-R(A).device | 92 + .../devices/dynamixel/H42-20-S300-R.device | 76 + .../devices/dynamixel/H42P-020-S300-R.device | 92 + .../dynamixel/H54-100-B210-R-NR.device | 76 + .../dynamixel/H54-100-S500-R(A).device | 92 + .../devices/dynamixel/H54-100-S500-R.device | 76 + .../devices/dynamixel/H54-200-B500-R.device | 76 + .../dynamixel/H54-200-S500-R(A).device | 92 + .../devices/dynamixel/H54-200-S500-R.device | 76 + .../devices/dynamixel/H54P-100-S500-R.device | 92 + .../devices/dynamixel/H54P-200-B500-R.device | 92 + .../devices/dynamixel/H54P-200-S500-R.device | 92 + .../devices/dynamixel/L42-10-S300-R.device | 73 + .../devices/dynamixel/L54-30-S400-R.device | 74 + .../devices/dynamixel/L54-30-S500-R.device | 74 + .../devices/dynamixel/L54-50-S290-R.device | 74 + .../devices/dynamixel/L54-50-S500-R.device | 74 + .../devices/dynamixel/M42-10-S260-R.device | 74 + .../devices/dynamixel/M54-40-S250-R.device | 74 + .../devices/dynamixel/M54-60-S250-R.device | 74 + .../devices/dynamixel/MX-106.device | 66 + .../devices/dynamixel/MX-28.device | 62 + .../devices/dynamixel/MX-64.device | 66 + .../devices/dynamixel/PH42-020-S300-R.device | 92 + .../devices/dynamixel/PH54-100-S500-R.device | 92 + .../devices/dynamixel/PH54-200-S500-R.device | 92 + .../devices/dynamixel/RH-P12-RN(A).device | 90 + .../devices/dynamixel/RH-P12-RN.device | 73 + .../devices/dynamixel/XM-430.device | 82 + .../devices/dynamixel/XM430-W210.device | 83 + .../devices/dynamixel/XM430-W350.device | 83 + .../devices/dynamixel/XM540-W150.device | 89 + .../devices/dynamixel/XM540-W270.device | 89 + .../devices/sensor/CM-740.device | 25 + .../devices/sensor/OPEN-CR.device | 30 + .../control_table_item.h | 54 + .../device.h | 54 + .../dynamixel.h | 83 + .../dynamixel_state.h | 82 + .../robot.h | 72 + .../sensor.h | 49 + .../sensor_state.h | 50 + .../time_stamp.h | 39 + .../package.xml | 38 + .../dynamixel.cpp | 119 + .../robot.cpp | 480 +++ .../sensor.cpp | 39 + .../CHANGELOG.rst | 89 + .../CMakeLists.txt | 12 + .../package.xml | 40 + .../CHANGELOG.rst | 82 + .../CMakeLists.txt | 46 + .../motion_module.h | 87 + .../sensor_module.h | 57 + .../singleton.h | 65 + .../package.xml | 33 + 67 files changed, 7854 insertions(+), 13 deletions(-) create mode 100755 humanoid_robot_intelligence_control_system_controller/CHANGELOG.rst create mode 100755 humanoid_robot_intelligence_control_system_controller/CMakeLists.txt create mode 100755 humanoid_robot_intelligence_control_system_controller/include/humanoid_robot_intelligence_control_system_controller/humanoid_robot_intelligence_control_system_controller.h create mode 100755 humanoid_robot_intelligence_control_system_controller/package.xml create mode 100755 humanoid_robot_intelligence_control_system_controller/src/humanoid_robot_intelligence_control_system_controller/humanoid_robot_intelligence_control_system_controller.cpp create mode 100755 humanoid_robot_intelligence_control_system_device/CHANGELOG.rst create mode 100755 humanoid_robot_intelligence_control_system_device/CMakeLists.txt create mode 100755 humanoid_robot_intelligence_control_system_device/devices/dynamixel/GRIPPER.device create mode 100755 humanoid_robot_intelligence_control_system_device/devices/dynamixel/GRIPPER_TORQUE.device create mode 100644 humanoid_robot_intelligence_control_system_device/devices/dynamixel/H42-20-S300-R(A).device create mode 100755 humanoid_robot_intelligence_control_system_device/devices/dynamixel/H42-20-S300-R.device create mode 100644 humanoid_robot_intelligence_control_system_device/devices/dynamixel/H42P-020-S300-R.device create mode 100755 humanoid_robot_intelligence_control_system_device/devices/dynamixel/H54-100-B210-R-NR.device create mode 100644 humanoid_robot_intelligence_control_system_device/devices/dynamixel/H54-100-S500-R(A).device create mode 100755 humanoid_robot_intelligence_control_system_device/devices/dynamixel/H54-100-S500-R.device create mode 100755 humanoid_robot_intelligence_control_system_device/devices/dynamixel/H54-200-B500-R.device create mode 100644 humanoid_robot_intelligence_control_system_device/devices/dynamixel/H54-200-S500-R(A).device create mode 100755 humanoid_robot_intelligence_control_system_device/devices/dynamixel/H54-200-S500-R.device create mode 100644 humanoid_robot_intelligence_control_system_device/devices/dynamixel/H54P-100-S500-R.device create mode 100644 humanoid_robot_intelligence_control_system_device/devices/dynamixel/H54P-200-B500-R.device create mode 100644 humanoid_robot_intelligence_control_system_device/devices/dynamixel/H54P-200-S500-R.device create mode 100755 humanoid_robot_intelligence_control_system_device/devices/dynamixel/L42-10-S300-R.device create mode 100755 humanoid_robot_intelligence_control_system_device/devices/dynamixel/L54-30-S400-R.device create mode 100755 humanoid_robot_intelligence_control_system_device/devices/dynamixel/L54-30-S500-R.device create mode 100755 humanoid_robot_intelligence_control_system_device/devices/dynamixel/L54-50-S290-R.device create mode 100755 humanoid_robot_intelligence_control_system_device/devices/dynamixel/L54-50-S500-R.device create mode 100755 humanoid_robot_intelligence_control_system_device/devices/dynamixel/M42-10-S260-R.device create mode 100755 humanoid_robot_intelligence_control_system_device/devices/dynamixel/M54-40-S250-R.device create mode 100755 humanoid_robot_intelligence_control_system_device/devices/dynamixel/M54-60-S250-R.device create mode 100755 humanoid_robot_intelligence_control_system_device/devices/dynamixel/MX-106.device create mode 100755 humanoid_robot_intelligence_control_system_device/devices/dynamixel/MX-28.device create mode 100755 humanoid_robot_intelligence_control_system_device/devices/dynamixel/MX-64.device create mode 100644 humanoid_robot_intelligence_control_system_device/devices/dynamixel/PH42-020-S300-R.device create mode 100644 humanoid_robot_intelligence_control_system_device/devices/dynamixel/PH54-100-S500-R.device create mode 100644 humanoid_robot_intelligence_control_system_device/devices/dynamixel/PH54-200-S500-R.device create mode 100644 humanoid_robot_intelligence_control_system_device/devices/dynamixel/RH-P12-RN(A).device create mode 100755 humanoid_robot_intelligence_control_system_device/devices/dynamixel/RH-P12-RN.device create mode 100755 humanoid_robot_intelligence_control_system_device/devices/dynamixel/XM-430.device create mode 100644 humanoid_robot_intelligence_control_system_device/devices/dynamixel/XM430-W210.device create mode 100644 humanoid_robot_intelligence_control_system_device/devices/dynamixel/XM430-W350.device create mode 100755 humanoid_robot_intelligence_control_system_device/devices/dynamixel/XM540-W150.device create mode 100755 humanoid_robot_intelligence_control_system_device/devices/dynamixel/XM540-W270.device create mode 100755 humanoid_robot_intelligence_control_system_device/devices/sensor/CM-740.device create mode 100755 humanoid_robot_intelligence_control_system_device/devices/sensor/OPEN-CR.device create mode 100755 humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device/control_table_item.h create mode 100755 humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device/device.h create mode 100755 humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device/dynamixel.h create mode 100755 humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device/dynamixel_state.h create mode 100755 humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device/robot.h create mode 100755 humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device/sensor.h create mode 100755 humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device/sensor_state.h create mode 100755 humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device/time_stamp.h create mode 100755 humanoid_robot_intelligence_control_system_device/package.xml create mode 100755 humanoid_robot_intelligence_control_system_device/src/humanoid_robot_intelligence_control_system_device/dynamixel.cpp create mode 100755 humanoid_robot_intelligence_control_system_device/src/humanoid_robot_intelligence_control_system_device/robot.cpp create mode 100755 humanoid_robot_intelligence_control_system_device/src/humanoid_robot_intelligence_control_system_device/sensor.cpp create mode 100755 humanoid_robot_intelligence_control_system_framework/CHANGELOG.rst create mode 100644 humanoid_robot_intelligence_control_system_framework/CMakeLists.txt create mode 100755 humanoid_robot_intelligence_control_system_framework/package.xml create mode 100755 humanoid_robot_intelligence_control_system_framework_common/CHANGELOG.rst create mode 100755 humanoid_robot_intelligence_control_system_framework_common/CMakeLists.txt create mode 100755 humanoid_robot_intelligence_control_system_framework_common/include/humanoid_robot_intelligence_control_system_framework_common/motion_module.h create mode 100755 humanoid_robot_intelligence_control_system_framework_common/include/humanoid_robot_intelligence_control_system_framework_common/sensor_module.h create mode 100755 humanoid_robot_intelligence_control_system_framework_common/include/humanoid_robot_intelligence_control_system_framework_common/singleton.h create mode 100755 humanoid_robot_intelligence_control_system_framework_common/package.xml diff --git a/.travis.yml b/.travis.yml index e1e689a..43bc280 100644 --- a/.travis.yml +++ b/.travis.yml @@ -18,7 +18,7 @@ notifications: - ronaldsonbellande@gmail.com env: matrix: - - ROS_DISTRO=noetic ROS_REPO=ros-shadow-fixed UPSTREAM_WORKSPACE=file OS_NAME=ubuntu OS_CODE_NAME=focal $ROSINSTALL_FILENAME=".humanoid_robot_framework.rosinstall" + - ROS_DISTRO=noetic ROS_REPO=ros-shadow-fixed UPSTREAM_WORKSPACE=file OS_NAME=ubuntu OS_CODE_NAME=focal $ROSINSTALL_FILENAME=".humanoid_robot_intelligence_control_system_framework.rosinstall" branches: only: - master diff --git a/README.md b/README.md index 9968397..8b0d5d4 100755 --- a/README.md +++ b/README.md @@ -5,27 +5,27 @@ [![Sponsor](https://img.shields.io/badge/Sponsor-Robotics%20Sensors%20Research-red?style=for-the-badge&logo=github)](https://github.com/sponsors/Robotics-Sensors) # Stats -[![GitHub stars](https://img.shields.io/github/stars/Robotics-Sensors/humanoid_robot_framework.svg?style=social)](https://github.com/Robotics-Sensors/humanoid_robot_framework/stargazers) -[![GitHub forks](https://img.shields.io/github/forks/Robotics-Sensors/humanoid_robot_framework.svg?style=social)](https://github.com/Robotics-Sensors/humanoid_robot_framework/network) -[![GitHub watchers](https://img.shields.io/github/watchers/Robotics-Sensors/humanoid_robot_framework.svg?style=social)](https://github.com/Robotics-Sensors/humanoid_robot_framework/watchers) +[![GitHub stars](https://img.shields.io/github/stars/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework.svg?style=social)](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework/stargazers) +[![GitHub forks](https://img.shields.io/github/forks/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework.svg?style=social)](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework/network) +[![GitHub watchers](https://img.shields.io/github/watchers/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework.svg?style=social)](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework/watchers) -[![GitHub issues](https://img.shields.io/github/issues/Robotics-Sensors/humanoid_robot_framework.svg)](https://github.com/Robotics-Sensors/humanoid_robot_framework/issues) -[![GitHub pull requests](https://img.shields.io/github/issues-pr/Robotics-Sensors/humanoid_robot_framework.svg)](https://github.com/Robotics-Sensors/humanoid_robot_framework/pulls) -[![GitHub license](https://img.shields.io/github/license/Robotics-Sensors/humanoid_robot_framework.svg)](https://github.com/Robotics-Sensors/humanoid_robot_framework/blob/main/LICENSE) +[![GitHub issues](https://img.shields.io/github/issues/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework.svg)](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework/issues) +[![GitHub pull requests](https://img.shields.io/github/issues-pr/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework.svg)](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework/pulls) +[![GitHub license](https://img.shields.io/github/license/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework.svg)](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework/blob/main/LICENSE) -[![GitHub last commit](https://img.shields.io/github/last-commit/Robotics-Sensors/humanoid_robot_framework.svg)](https://github.com/Robotics-Sensors/humanoid_robot_framework/commits) -[![Visitor & Github Clones](https://img.shields.io/badge/dynamic/json?color=2e8b57&label=Visitor%20%26%20GitHub%20Clones&query=$.count&url=https://api.github.com/repos/Robotics-Sensors/humanoid_robot_framework/traffic)](https://github.com/Robotics-Sensors/humanoid_robot_framework) +[![GitHub last commit](https://img.shields.io/github/last-commit/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework.svg)](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework/commits) +[![Visitor & Github Clones](https://img.shields.io/badge/dynamic/json?color=2e8b57&label=Visitor%20%26%20GitHub%20Clones&query=$.count&url=https://api.github.com/repos/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework/traffic)](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework) -------------------------------------------------------------------------------------------------------- # Repository Website -https://robotics-sensors.github.io/humanoid_robot_framework +https://robotics-sensors.github.io/humanoid_robot_intelligence_control_system_framework -------------------------------------------------------------------------------------------------------- -Updated Version [humanoid_robot_framework](https://github.com/Robotics-Sensors/humanoid_robot_framework) readme. +Updated Version [humanoid_robot_intelligence_control_system_framework](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework) readme. Old Version/Previous Used for Different Context [ROBOTIS-Framework](https://github.com/ROBOTIS-GIT/ROBOTIS-Framework) readme. # Release -[![Latest Release](https://img.shields.io/github/v/release/Robotics-Sensors/humanoid_robot_tools?style=for-the-badge&color=yellow)](https://github.com/Robotics-Sensors/humanoid_robot_framework/releases/) +[![Latest Release](https://img.shields.io/github/v/release/Robotics-Sensors/humanoid_robot_intelligence_control_system_tools?style=for-the-badge&color=yellow)](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework/releases/) # Contact Depeding on the repository, If you are interested in accessing the complete version or other repository related to this repository, we kindly request that you reach out to the organization's director or the company behind this project. We invite you to explore the USE CASE to understand the specific terms and conditions for usage. To utilize this repository, it is imperative that you adhere to the guidelines outlined in the USE CASE. For those interested in showing their support, becoming a sponsor of the organization is an option, and detailed information can be found within the USE CASE and License documents. Furthermore, we encourage you to join our official Discord community, where you can engage with like-minded individuals, contribute to the project, and stay up-to-date with the latest developments. It's worth noting that while a fraction of research is publicly accessible, the majority remains private. To gain insights into the wealth of knowledge held by the Company or the organization's director, we recommend direct contact for more information. @@ -48,4 +48,4 @@ Latest versions and Maintainer is on organization https://github.com/Robotics-Se * Ronaldson Bellande ## License -This SDK is distributed under the [Apache License, Version 2.0](https://www.apache.org/licenses/LICENSE-2.0), see [LICENSE](https://github.com/Robotics-Sensors/humanoid_robot_framework/blob/main/LICENSE) and [NOTICE](https://github.com/Robotics-Sensors/humanoid_robot_framework/blob/main/LICENSE) for more information. +This SDK is distributed under the [Apache License, Version 2.0](https://www.apache.org/licenses/LICENSE-2.0), see [LICENSE](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework/blob/main/LICENSE) and [NOTICE](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework/blob/main/LICENSE) for more information. diff --git a/humanoid_robot_intelligence_control_system_controller/CHANGELOG.rst b/humanoid_robot_intelligence_control_system_controller/CHANGELOG.rst new file mode 100755 index 0000000..0b85c9e --- /dev/null +++ b/humanoid_robot_intelligence_control_system_controller/CHANGELOG.rst @@ -0,0 +1,136 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package humanoid_robot_intelligence_control_system_controller +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.3.2 (2023-10-03) +------------------ +* Make it compatible for ROS1/ROS2 +* Fix bugs +* Update package.xml and CMakeList.txt for to the latest versions +* Contributors & Maintainer: Ronaldson Bellande + +0.3.1 (2023-09-27) +------------------ +* Starting from this point it under a new license +* Fix errors and Issues +* Rename Repository for a completely different purpose +* Make it compatible with ROS/ROS2 +* Upgrade version of all builds and make it more compatible +* Update package.xml and CMakeList.txt for to the latest versions +* Contributors & Maintainer: Ronaldson Bellande + +0.3.0 (2021-05-03) +------------------ +* Update package.xml and CMakeList.txt for noetic branch +* Contributors: Ronaldson Bellande + +0.2.9 (2018-03-22) +------------------ +* added serivce for setting module +* deleted comment for debug +* modified to prevent duplicate indirect address write +* added boost system dependencies +* fixed a bug that occure when handling bulk read item that does not exist. +* Contributors: Kayman, Zerom, Pyo + +0.2.8 (2018-03-20) +------------------ +* modified CMakeLists.txt for system dependencies (yaml-cpp) +* Contributors: Zerom, Pyo + +0.2.7 (2018-03-15) +------------------ +* changed the License and package format to version 2 +* changed all values read by bulk read are saved to dxl_state\_->bulk_read_table\_. +* Modified to prevent duplicate indirect address write +* Contributors: SCH, Zerom, Pyo + +0.2.6 (2017-08-09) +------------------ +* multi thread bug fixed. +* unnecessary debug print removed. +* OpenCR control table item name changed. (torque_enable -> dynamixel_power) +* fixed to not update update_time_stamp\_ if bulk read fails. +* Contributors: Zerom + +0.2.5 (2017-06-09) +------------------ +* updated for yaml-cpp dependencies +* Contributors: SCH + +0.2.4 (2017-06-07) +------------------ +* added cmake_modules in package.xml +* Contributors: SCH + +0.2.3 (2017-05-23) +------------------ +* updated the cmake file for ros install +* Contributors: SCH + +0.2.2 (2017-04-24) +------------------ +* updated humanoid_robot_intelligence_control_system_controller.cpp +* changed to read control cycle from .robot file +* Contributors: Zerom + +0.2.1 (2016-11-23) +------------------ +* Merge the changes and update +* - Direct Control Mode bug fixed. +* update +* - added writeControlTableCallback +* - added WriteControlTable msg callback +* mode change debugging +* - optimized cpu usage by spin loop (by astumpf) +* - humanoid_robot_intelligence_control_system_controller process() : processing order changed. + * 1st : packet communication + * 2nd : processing modules +* - dependencies fixed. (Pull requests `#26 `_) +* - make setJointCtrlModuleCallback() to the thread function & improved. +* - modified dependency problem. +* - reduce CPU consumption +* Contributors: Jay Song, Pyo, Zerom, SCH + +0.2.0 (2016-08-31) +------------------ +* bug fixed (position pid gain & velocity pid gain sync write). +* added velocity_to_value_ratio to DXL Pro-H series. +* changed some debug messages. +* added velocity p/i/d gain and position i/d gain sync_write code. +* SyncWriteItem bug fixed. +* add function / modified the code simple (using auto / range based for loop) +* added XM-430-W210 / XM-430-W350 device file. +* rename ControlMode(CurrentControl -> TorqueControl) +* rename (port_to_sync_write_torque\_ -> port_to_sync_write_current\_) +* rename (present_current\_ -> present_torque\_) +* modified torque control code +* fixed typos / changed ROS_INFO -> fprintf (for processing speed) +* startTimer() : after bulkread txpacket(), need some sleep() +* changed the order of processing in the Process() function. +* added missing mutex for gazebo +* fixed crash when running in gazebo simulation +* sync write bug fix. +* added position_p_gain sync write +* MotionModule/SensorModule member variable access changed (public -> protected). +* Contributors: Jay Song, Zerom, Pyo, SCH + +0.1.1 (2016-08-18) +------------------ +* updated the package information + +0.1.0 (2016-08-12) +------------------ +* first public release for Kinetic +* modified the package information for release +* develop branch -> master branch +* function name changed : DeviceInit() -> InitDevice() +* Fixed high CPU consumption due to busy waits +* add SensorState + add Singleton template +* XM-430 / CM-740 device file added. + Sensor device added. +* added code to support the gazebo simulator +* added first bulk read failure protection code +* renewal +* Contributors: Alexander Stumpf, Jay Song, Zerom, Pyo diff --git a/humanoid_robot_intelligence_control_system_controller/CMakeLists.txt b/humanoid_robot_intelligence_control_system_controller/CMakeLists.txt new file mode 100755 index 0000000..c69c671 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_controller/CMakeLists.txt @@ -0,0 +1,83 @@ +cmake_minimum_required(VERSION 3.8) +project(humanoid_robot_intelligence_control_system_controller) + +## Compile as C++11, supported in ROS Kinetic and newer +add_compile_options(-std=c++11) + + +if($ENV{ROS_VERSION} EQUAL 1) + find_package(catkin REQUIRED COMPONENTS + roscpp + roslib + std_msgs + sensor_msgs + humanoid_robot_intelligence_control_system_controller_msgs + dynamixel_sdk + humanoid_robot_intelligence_control_system_device + humanoid_robot_intelligence_control_system_framework_common + cmake_modules + ) + + find_package(Boost REQUIRED) + + find_package(PkgConfig REQUIRED) +else() + find_package(ament_cmake REQUIRED) +endif() + + +pkg_check_modules(YAML_CPP REQUIRED yaml-cpp) +find_path(YAML_CPP_INCLUDE_DIR + NAMES yaml_cpp.h + PATHS ${YAML_CPP_INCLUDE_DIRS} +) +find_library(YAML_CPP_LIBRARY + NAMES YAML_CPP + PATHS ${YAML_CPP_LIBRARY_DIRS} +) +link_directories(${YAML_CPP_LIBRARY_DIRS}) + +if(NOT ${YAML_CPP_VERSION} VERSION_LESS "0.5") +add_definitions(-DHAVE_NEW_YAMLCPP) +endif(NOT ${YAML_CPP_VERSION} VERSION_LESS "0.5") + + +if($ENV{ROS_VERSION} EQUAL 1) + catkin_package( + INCLUDE_DIRS include + LIBRARIES humanoid_robot_intelligence_control_system_controller + CATKIN_DEPENDS + roscpp + roslib + std_msgs + sensor_msgs + humanoid_robot_intelligence_control_system_controller_msgs + dynamixel_sdk + humanoid_robot_intelligence_control_system_device + humanoid_robot_intelligence_control_system_framework_common + cmake_modules + DEPENDS Boost + ) +endif() + + +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} + ${YAML_CPP_INCLUDE_DIRS} +) + +add_library(humanoid_robot_intelligence_control_system_controller src/humanoid_robot_intelligence_control_system_controller/humanoid_robot_intelligence_control_system_controller.cpp) +add_dependencies(humanoid_robot_intelligence_control_system_controller ${catkin_EXPORTED_TARGETS}) +target_link_libraries(humanoid_robot_intelligence_control_system_controller ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${YAML_CPP_LIBRARIES}) + +install(TARGETS humanoid_robot_intelligence_control_system_controller + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) diff --git a/humanoid_robot_intelligence_control_system_controller/include/humanoid_robot_intelligence_control_system_controller/humanoid_robot_intelligence_control_system_controller.h b/humanoid_robot_intelligence_control_system_controller/include/humanoid_robot_intelligence_control_system_controller/humanoid_robot_intelligence_control_system_controller.h new file mode 100755 index 0000000..019a9bb --- /dev/null +++ b/humanoid_robot_intelligence_control_system_controller/include/humanoid_robot_intelligence_control_system_controller/humanoid_robot_intelligence_control_system_controller.h @@ -0,0 +1,209 @@ +/******************************************************************************* + * Copyright 2018 ROBOTIS CO., LTD. + * + * 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. + *******************************************************************************/ + +/* + * humanoid_robot_intelligence_control_system_controller.h + * + * Created on: 2016. 1. 15. + * Author: zerom + */ + +#ifndef ROBOTIS_CONTROLLER_ROBOTIS_CONTROLLER_H_ +#define ROBOTIS_CONTROLLER_ROBOTIS_CONTROLLER_H_ + +#include +#include +#include +#include +#include +#include +#include + +#include "humanoid_robot_intelligence_control_system_controller_msgs/GetJointModule.h" +#include "humanoid_robot_intelligence_control_system_controller_msgs/JointCtrlModule.h" +#include "humanoid_robot_intelligence_control_system_controller_msgs/LoadOffset.h" +#include "humanoid_robot_intelligence_control_system_controller_msgs/SetJointModule.h" +#include "humanoid_robot_intelligence_control_system_controller_msgs/SetModule.h" +#include "humanoid_robot_intelligence_control_system_controller_msgs/SyncWriteItem.h" +#include "humanoid_robot_intelligence_control_system_controller_msgs/WriteControlTable.h" + +#include "dynamixel_sdk/group_bulk_read.h" +#include "dynamixel_sdk/group_sync_write.h" +#include "humanoid_robot_intelligence_control_system_device/robot.h" +#include "humanoid_robot_intelligence_control_system_framework_common/motion_module.h" +#include "humanoid_robot_intelligence_control_system_framework_common/sensor_module.h" + +namespace humanoid_robot_intelligence_control_system_framework { + +enum ControllerMode { MotionModuleMode, DirectControlMode }; + +class RobotisController : public Singleton { +private: + boost::thread queue_thread_; + boost::thread gazebo_thread_; + boost::thread set_module_thread_; + boost::mutex queue_mutex_; + + bool init_pose_loaded_; + bool is_timer_running_; + bool is_offset_enabled_; + double offset_ratio_; + bool stop_timer_; + pthread_t timer_thread_; + ControllerMode controller_mode_; + + std::list motion_modules_; + std::list sensor_modules_; + std::vector direct_sync_write_; + + std::map sensor_result_; + + void gazeboTimerThread(); + void msgQueueThread(); + void setCtrlModuleThread(std::string ctrl_module); + void setJointCtrlModuleThread( + const humanoid_robot_intelligence_control_system_controller_msgs::JointCtrlModule::ConstPtr &msg); + + bool isTimerStopped(); + void initializeSyncWrite(); + +public: + const int NONE_GAIN = 65535; + bool DEBUG_PRINT; + Robot *robot_; + + bool gazebo_mode_; + std::string gazebo_robot_name_; + + /* bulk read */ + std::map port_to_bulk_read_; + + /* sync write */ + std::map + port_to_sync_write_position_; + std::map + port_to_sync_write_velocity_; + std::map + port_to_sync_write_current_; + std::map + port_to_sync_write_position_p_gain_; + std::map + port_to_sync_write_position_i_gain_; + std::map + port_to_sync_write_position_d_gain_; + std::map + port_to_sync_write_velocity_p_gain_; + std::map + port_to_sync_write_velocity_i_gain_; + std::map + port_to_sync_write_velocity_d_gain_; + + /* publisher */ + ros::Publisher goal_joint_state_pub_; + ros::Publisher present_joint_state_pub_; + ros::Publisher current_module_pub_; + + std::map gazebo_joint_position_pub_; + std::map gazebo_joint_velocity_pub_; + std::map gazebo_joint_effort_pub_; + + static void *timerThread(void *param); + + RobotisController(); + + bool initialize(const std::string robot_file_path, + const std::string init_file_path); + void initializeDevice(const std::string init_file_path); + void process(); + + void addMotionModule(MotionModule *module); + void removeMotionModule(MotionModule *module); + void addSensorModule(SensorModule *module); + void removeSensorModule(SensorModule *module); + + void startTimer(); + void stopTimer(); + bool isTimerRunning(); + + void setCtrlModule(std::string module_name); + void loadOffset(const std::string path); + + /* ROS Topic Callback Functions */ + void writeControlTableCallback( + const humanoid_robot_intelligence_control_system_controller_msgs::WriteControlTable::ConstPtr &msg); + void syncWriteItemCallback( + const humanoid_robot_intelligence_control_system_controller_msgs::SyncWriteItem::ConstPtr &msg); + void setControllerModeCallback(const std_msgs::String::ConstPtr &msg); + void setJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg); + void setJointCtrlModuleCallback( + const humanoid_robot_intelligence_control_system_controller_msgs::JointCtrlModule::ConstPtr &msg); + void setCtrlModuleCallback(const std_msgs::String::ConstPtr &msg); + void enableOffsetCallback(const std_msgs::Bool::ConstPtr &msg); + bool getJointCtrlModuleService( + humanoid_robot_intelligence_control_system_controller_msgs::GetJointModule::Request &req, + humanoid_robot_intelligence_control_system_controller_msgs::GetJointModule::Response &res); + bool setJointCtrlModuleService( + humanoid_robot_intelligence_control_system_controller_msgs::SetJointModule::Request &req, + humanoid_robot_intelligence_control_system_controller_msgs::SetJointModule::Response &res); + bool setCtrlModuleService( + humanoid_robot_intelligence_control_system_controller_msgs::SetModule::Request &req, + humanoid_robot_intelligence_control_system_controller_msgs::SetModule::Response &res); + bool + loadOffsetService(humanoid_robot_intelligence_control_system_controller_msgs::LoadOffset::Request &req, + humanoid_robot_intelligence_control_system_controller_msgs::LoadOffset::Response &res); + + void gazeboJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg); + + int ping(const std::string joint_name, uint8_t *error = 0); + int ping(const std::string joint_name, uint16_t *model_number, + uint8_t *error = 0); + + int action(const std::string joint_name); + int reboot(const std::string joint_name, uint8_t *error = 0); + int factoryReset(const std::string joint_name, uint8_t option = 0, + uint8_t *error = 0); + + int read(const std::string joint_name, uint16_t address, uint16_t length, + uint8_t *data, uint8_t *error = 0); + int readCtrlItem(const std::string joint_name, const std::string item_name, + uint32_t *data, uint8_t *error = 0); + + int read1Byte(const std::string joint_name, uint16_t address, uint8_t *data, + uint8_t *error = 0); + int read2Byte(const std::string joint_name, uint16_t address, uint16_t *data, + uint8_t *error = 0); + int read4Byte(const std::string joint_name, uint16_t address, uint32_t *data, + uint8_t *error = 0); + + int write(const std::string joint_name, uint16_t address, uint16_t length, + uint8_t *data, uint8_t *error = 0); + int writeCtrlItem(const std::string joint_name, const std::string item_name, + uint32_t data, uint8_t *error = 0); + + int write1Byte(const std::string joint_name, uint16_t address, uint8_t data, + uint8_t *error = 0); + int write2Byte(const std::string joint_name, uint16_t address, uint16_t data, + uint8_t *error = 0); + int write4Byte(const std::string joint_name, uint16_t address, uint32_t data, + uint8_t *error = 0); + + int regWrite(const std::string joint_name, uint16_t address, uint16_t length, + uint8_t *data, uint8_t *error = 0); +}; + +} // namespace humanoid_robot_intelligence_control_system_framework + +#endif /* ROBOTIS_CONTROLLER_ROBOTIS_CONTROLLER_H_ */ diff --git a/humanoid_robot_intelligence_control_system_controller/package.xml b/humanoid_robot_intelligence_control_system_controller/package.xml new file mode 100755 index 0000000..302552d --- /dev/null +++ b/humanoid_robot_intelligence_control_system_controller/package.xml @@ -0,0 +1,81 @@ + + + humanoid_robot_intelligence_control_system_controller + 0.3.2 + humanoid_robot_intelligence_control_system_controller package for ROBOTIS's platform like Manipulator-H, THORMANG and OP series + Apache 2.0 + Ronaldson Bellande + + + catkin + + roscpp + roslib + std_msgs + sensor_msgs + humanoid_robot_intelligence_control_system_controller_msgs + dynamixel_sdk + humanoid_robot_intelligence_control_system_device + humanoid_robot_intelligence_control_system_framework_common + cmake_modules + yaml-cpp + + roscpp + roslib + std_msgs + sensor_msgs + humanoid_robot_intelligence_control_system_controller_msgs + dynamixel_sdk + humanoid_robot_intelligence_control_system_device + humanoid_robot_intelligence_control_system_framework_common + cmake_modules + yaml-cpp + + roscpp + roslib + std_msgs + sensor_msgs + humanoid_robot_intelligence_control_system_controller_msgs + dynamixel_sdk + humanoid_robot_intelligence_control_system_device + humanoid_robot_intelligence_control_system_framework_common + cmake_modules + yaml-cpp + + + ament_cmake + + roscpp + roslib + std_msgs + sensor_msgs + humanoid_robot_intelligence_control_system_controller_msgs + dynamixel_sdk + humanoid_robot_intelligence_control_system_device + humanoid_robot_intelligence_control_system_framework_common + cmake_modules + yaml-cpp + + roscpp + roslib + std_msgs + sensor_msgs + humanoid_robot_intelligence_control_system_controller_msgs + dynamixel_sdk + humanoid_robot_intelligence_control_system_device + humanoid_robot_intelligence_control_system_framework_common + cmake_modules + yaml-cpp + + roscpp + roslib + std_msgs + sensor_msgs + humanoid_robot_intelligence_control_system_controller_msgs + dynamixel_sdk + humanoid_robot_intelligence_control_system_device + humanoid_robot_intelligence_control_system_framework_common + cmake_modules + yaml-cpp + + diff --git a/humanoid_robot_intelligence_control_system_controller/src/humanoid_robot_intelligence_control_system_controller/humanoid_robot_intelligence_control_system_controller.cpp b/humanoid_robot_intelligence_control_system_controller/src/humanoid_robot_intelligence_control_system_controller/humanoid_robot_intelligence_control_system_controller.cpp new file mode 100755 index 0000000..1204a29 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_controller/src/humanoid_robot_intelligence_control_system_controller/humanoid_robot_intelligence_control_system_controller.cpp @@ -0,0 +1,2625 @@ +/******************************************************************************* + * Copyright 2018 ROBOTIS CO., LTD. + * + * 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. + *******************************************************************************/ + +/* + * humanoid_robot_intelligence_control_system_controller.cpp + * + * Created on: 2016. 1. 15. + * Author: zerom + */ + +#include +#include + +#include "humanoid_robot_intelligence_control_system_controller/humanoid_robot_intelligence_control_system_controller.h" + +using namespace humanoid_robot_intelligence_control_system_framework; + +RobotisController::RobotisController() + : is_timer_running_(false), is_offset_enabled_(true), offset_ratio_(1.0), + stop_timer_(false), init_pose_loaded_(false), timer_thread_(0), + controller_mode_(MotionModuleMode), DEBUG_PRINT(false), robot_(0), + gazebo_mode_(false), + gazebo_robot_name_("humanoid_robot_intelligence_control_system") { + direct_sync_write_.clear(); +} + +void RobotisController::initializeSyncWrite() { + if (gazebo_mode_ == true) + return; + + // ROS_INFO("FIRST BULKREAD"); + for (auto &it : port_to_bulk_read_) + it.second->txRxPacket(); + for (auto &it : port_to_bulk_read_) { + int error_count = 0; + int result = COMM_SUCCESS; + do { + if (++error_count > 10) { + ROS_ERROR("[RobotisController] first bulk read fail!!"); + exit(-1); + } + usleep(10 * 1000); + result = it.second->txRxPacket(); + } while (result != COMM_SUCCESS); + } + init_pose_loaded_ = true; + // ROS_INFO("FIRST BULKREAD END"); + + // clear syncwrite param setting + for (auto &it : port_to_sync_write_position_) { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto &it : port_to_sync_write_position_p_gain_) { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto &it : port_to_sync_write_position_i_gain_) { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto &it : port_to_sync_write_position_d_gain_) { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto &it : port_to_sync_write_velocity_) { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto &it : port_to_sync_write_velocity_p_gain_) { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto &it : port_to_sync_write_velocity_i_gain_) { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto &it : port_to_sync_write_velocity_d_gain_) { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto &it : port_to_sync_write_current_) { + if (it.second != NULL) + it.second->clearParam(); + } + + // set init syncwrite param(from data of bulkread) + for (auto &it : robot_->dxls_) { + std::string joint_name = it.first; + Dynamixel *dxl = it.second; + + for (int i = 0; i < dxl->bulk_read_items_.size(); i++) { + uint32_t read_data = 0; + uint8_t sync_write_data[4]; + + if (port_to_bulk_read_[dxl->port_name_]->isAvailable( + dxl->id_, dxl->bulk_read_items_[i]->address_, + dxl->bulk_read_items_[i]->data_length_) == true) { + read_data = port_to_bulk_read_[dxl->port_name_]->getData( + dxl->id_, dxl->bulk_read_items_[i]->address_, + dxl->bulk_read_items_[i]->data_length_); + + sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(read_data)); + sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(read_data)); + sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(read_data)); + sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(read_data)); + + if ((dxl->present_position_item_ != 0) && + (dxl->bulk_read_items_[i]->item_name_ == + dxl->present_position_item_->item_name_)) { + dxl->dxl_state_->present_position_ = + dxl->convertValue2Radian(read_data) - + dxl->dxl_state_->position_offset_ * offset_ratio_; + dxl->dxl_state_->goal_position_ = dxl->dxl_state_->present_position_; + + port_to_sync_write_position_[dxl->port_name_]->addParam( + dxl->id_, sync_write_data); + } else if ((dxl->position_p_gain_item_ != 0) && + (dxl->bulk_read_items_[i]->item_name_ == + dxl->position_p_gain_item_->item_name_)) { + dxl->dxl_state_->position_p_gain_ = read_data; + } else if ((dxl->position_i_gain_item_ != 0) && + (dxl->bulk_read_items_[i]->item_name_ == + dxl->position_i_gain_item_->item_name_)) { + dxl->dxl_state_->position_i_gain_ = read_data; + } else if ((dxl->position_d_gain_item_ != 0) && + (dxl->bulk_read_items_[i]->item_name_ == + dxl->position_d_gain_item_->item_name_)) { + dxl->dxl_state_->position_d_gain_ = read_data; + } else if ((dxl->present_velocity_item_ != 0) && + (dxl->bulk_read_items_[i]->item_name_ == + dxl->present_velocity_item_->item_name_)) { + dxl->dxl_state_->present_velocity_ = + dxl->convertValue2Velocity(read_data); + dxl->dxl_state_->goal_velocity_ = dxl->dxl_state_->present_velocity_; + } else if ((dxl->velocity_p_gain_item_ != 0) && + (dxl->bulk_read_items_[i]->item_name_ == + dxl->velocity_p_gain_item_->item_name_)) { + dxl->dxl_state_->velocity_p_gain_ = read_data; + } else if ((dxl->velocity_i_gain_item_ != 0) && + (dxl->bulk_read_items_[i]->item_name_ == + dxl->velocity_i_gain_item_->item_name_)) { + dxl->dxl_state_->velocity_i_gain_ = read_data; + } else if ((dxl->velocity_d_gain_item_ != 0) && + (dxl->bulk_read_items_[i]->item_name_ == + dxl->velocity_d_gain_item_->item_name_)) { + dxl->dxl_state_->velocity_d_gain_ = read_data; + } else if ((dxl->present_current_item_ != 0) && + (dxl->bulk_read_items_[i]->item_name_ == + dxl->present_current_item_->item_name_)) { + dxl->dxl_state_->present_torque_ = + dxl->convertValue2Torque(read_data); + dxl->dxl_state_->goal_torque_ = dxl->dxl_state_->present_torque_; + } + } + } + } +} + +bool RobotisController::initialize(const std::string robot_file_path, + const std::string init_file_path) { + std::string dev_desc_dir_path = + ros::package::getPath( + "humanoid_robot_intelligence_control_system_device") + + "/devices"; + + // load robot info : port , device + robot_ = new Robot(robot_file_path, dev_desc_dir_path); + + if (gazebo_mode_ == true) { + queue_thread_ = + boost::thread(boost::bind(&RobotisController::msgQueueThread, this)); + return true; + } + + for (auto &it : robot_->ports_) { + std::string port_name = it.first; + dynamixel::PortHandler *port = it.second; + dynamixel::PacketHandler *default_pkt_handler = + dynamixel::PacketHandler::getPacketHandler(2.0); + + if (port->setBaudRate(port->getBaudRate()) == false) { + ROS_ERROR("PORT [%s] SETUP ERROR! (baudrate: %d)", port_name.c_str(), + port->getBaudRate()); + exit(-1); + } + + // get the default device info of the port + std::string default_device_name = robot_->port_default_device_[port_name]; + auto dxl_it = robot_->dxls_.find(default_device_name); + auto sensor_it = robot_->sensors_.find(default_device_name); + if (dxl_it != robot_->dxls_.end()) { + Dynamixel *default_device = dxl_it->second; + default_pkt_handler = dynamixel::PacketHandler::getPacketHandler( + default_device->protocol_version_); + + if (default_device->goal_position_item_ != 0) { + port_to_sync_write_position_[port_name] = new dynamixel::GroupSyncWrite( + port, default_pkt_handler, + default_device->goal_position_item_->address_, + default_device->goal_position_item_->data_length_); + } + + if (default_device->position_p_gain_item_ != 0) { + port_to_sync_write_position_p_gain_[port_name] = + new dynamixel::GroupSyncWrite( + port, default_pkt_handler, + default_device->position_p_gain_item_->address_, + default_device->position_p_gain_item_->data_length_); + } + + if (default_device->position_i_gain_item_ != 0) { + port_to_sync_write_position_i_gain_[port_name] = + new dynamixel::GroupSyncWrite( + port, default_pkt_handler, + default_device->position_i_gain_item_->address_, + default_device->position_i_gain_item_->data_length_); + } + + if (default_device->position_d_gain_item_ != 0) { + port_to_sync_write_position_d_gain_[port_name] = + new dynamixel::GroupSyncWrite( + port, default_pkt_handler, + default_device->position_d_gain_item_->address_, + default_device->position_d_gain_item_->data_length_); + } + + if (default_device->goal_velocity_item_ != 0) { + port_to_sync_write_velocity_[port_name] = new dynamixel::GroupSyncWrite( + port, default_pkt_handler, + default_device->goal_velocity_item_->address_, + default_device->goal_velocity_item_->data_length_); + } + + if (default_device->velocity_p_gain_item_ != 0) { + port_to_sync_write_velocity_p_gain_[port_name] = + new dynamixel::GroupSyncWrite( + port, default_pkt_handler, + default_device->velocity_p_gain_item_->address_, + default_device->velocity_p_gain_item_->data_length_); + } + + if (default_device->velocity_i_gain_item_ != 0) { + port_to_sync_write_velocity_i_gain_[port_name] = + new dynamixel::GroupSyncWrite( + port, default_pkt_handler, + default_device->velocity_i_gain_item_->address_, + default_device->velocity_i_gain_item_->data_length_); + } + + if (default_device->velocity_d_gain_item_ != 0) { + port_to_sync_write_velocity_d_gain_[port_name] = + new dynamixel::GroupSyncWrite( + port, default_pkt_handler, + default_device->velocity_d_gain_item_->address_, + default_device->velocity_d_gain_item_->data_length_); + } + + if (default_device->goal_current_item_ != 0) { + port_to_sync_write_current_[port_name] = new dynamixel::GroupSyncWrite( + port, default_pkt_handler, + default_device->goal_current_item_->address_, + default_device->goal_current_item_->data_length_); + } + } else if (sensor_it != robot_->sensors_.end()) { + Sensor *_default_device = sensor_it->second; + default_pkt_handler = dynamixel::PacketHandler::getPacketHandler( + _default_device->protocol_version_); + } + + port_to_bulk_read_[port_name] = + new dynamixel::GroupBulkRead(port, default_pkt_handler); + } + + // (for loop) check all dxls are connected. + for (auto &it : robot_->dxls_) { + std::string joint_name = it.first; + Dynamixel *dxl = it.second; + + if (ping(joint_name) != 0) { + usleep(10 * 1000); + if (ping(joint_name) != 0) + ROS_ERROR("JOINT[%s] does NOT respond!!", joint_name.c_str()); + } + } + + initializeDevice(init_file_path); + + queue_thread_ = + boost::thread(boost::bind(&RobotisController::msgQueueThread, this)); + return true; +} + +void RobotisController::initializeDevice(const std::string init_file_path) { + // device initialize + if (DEBUG_PRINT) + ROS_WARN("INIT FILE LOAD"); + + YAML::Node doc; + try { + doc = YAML::LoadFile(init_file_path.c_str()); + + for (YAML::const_iterator it_doc = doc.begin(); it_doc != doc.end(); + it_doc++) { + std::string joint_name = it_doc->first.as(); + + YAML::Node joint_node = doc[joint_name]; + if (joint_node.size() == 0) + continue; + + Dynamixel *dxl = NULL; + auto dxl_it = robot_->dxls_.find(joint_name); + if (dxl_it != robot_->dxls_.end()) + dxl = dxl_it->second; + + if (dxl == NULL) { + ROS_WARN("Joint [%s] was not found.", joint_name.c_str()); + continue; + } + if (DEBUG_PRINT) + ROS_INFO("JOINT_NAME: %s", joint_name.c_str()); + + uint8_t torque_enabled = 0; + read1Byte(joint_name, dxl->torque_enable_item_->address_, + &torque_enabled); + + for (YAML::const_iterator it_joint = joint_node.begin(); + it_joint != joint_node.end(); it_joint++) { + std::string item_name = it_joint->first.as(); + + if (DEBUG_PRINT) + ROS_INFO(" ITEM_NAME: %s", item_name.c_str()); + + uint32_t value = it_joint->second.as(); + + ControlTableItem *item = dxl->ctrl_table_[item_name]; + if (item == NULL) { + ROS_WARN("Control Item [%s] was not found.", item_name.c_str()); + continue; + } + + if (item->memory_type_ == EEPROM) { + uint8_t data8 = 0; + uint16_t data16 = 0; + uint32_t data32 = 0; + + switch (item->data_length_) { + case 1: + read1Byte(joint_name, item->address_, &data8); + if (data8 == value) + continue; + break; + case 2: + read2Byte(joint_name, item->address_, &data16); + if (data16 == value) + continue; + break; + case 4: + read4Byte(joint_name, item->address_, &data32); + if (data32 == value) + continue; + break; + default: + break; + } + + if (torque_enabled == 1) { + ROS_ERROR( + "################\nThe initial value of the EEPROM area has " + "been changed. \nTurn off Torque Enable and try again."); + exit(-1); + } + } + + switch (item->data_length_) { + case 1: + write1Byte(joint_name, item->address_, (uint8_t)value); + break; + case 2: + write2Byte(joint_name, item->address_, (uint16_t)value); + break; + case 4: + write4Byte(joint_name, item->address_, value); + break; + default: + break; + } + + if (item->memory_type_ == EEPROM) { + // Write to EEPROM -> delay is required (max delay: 55 msec per byte) + usleep(item->data_length_ * 55 * 1000); + } + } + } + } catch (const std::exception &e) { + ROS_INFO("Dynamixel Init file not found."); + } + + // [ BulkRead ] StartAddress : Present Position , Length : 10 ( + // Position/Velocity/Current ) + for (auto &it : robot_->ports_) { + if (port_to_bulk_read_[it.first] != 0) + port_to_bulk_read_[it.first]->clearParam(); + } + for (auto &it : robot_->dxls_) { + std::string joint_name = it.first; + Dynamixel *dxl = it.second; + + if (dxl == NULL) + continue; + + int bulkread_start_addr = 0; + int bulkread_data_length = 0; + + // // bulk read default : present position + // if(dxl->present_position_item != 0) + // { + // bulkread_start_addr = dxl->present_position_item->address; + // bulkread_data_length = dxl->present_position_item->data_length; + // } + + uint8_t torque_enabled = 0; + read1Byte(joint_name, dxl->torque_enable_item_->address_, &torque_enabled); + + // calculate bulk read start address & data length + auto indirect_addr_it = dxl->ctrl_table_.find(INDIRECT_ADDRESS_1); + if (indirect_addr_it != dxl->ctrl_table_.end()) // INDIRECT_ADDRESS_1 exist + { + if (dxl->bulk_read_items_.size() != 0) { + uint16_t data16 = 0; + + bulkread_start_addr = dxl->bulk_read_items_[0]->address_; + bulkread_data_length = 0; + + // set indirect address + int indirect_addr = indirect_addr_it->second->address_; + for (int i = 0; i < dxl->bulk_read_items_.size(); i++) { + int addr_leng = dxl->bulk_read_items_[i]->data_length_; + + bulkread_data_length += addr_leng; + for (int l = 0; l < addr_leng; l++) { + // ROS_WARN("[%12s] INDIR_ADDR: %d, ITEM_ADDR: %d", + // joint_name.c_str(), indirect_addr, + // dxl->ctrl_table[dxl->bulk_read_items[i]->item_name]->address + + // _l); + + read2Byte(joint_name, indirect_addr, &data16); + if (data16 != dxl->ctrl_table_[dxl->bulk_read_items_[i]->item_name_] + ->address_ + + l) { + if (torque_enabled == 1) { + ROS_ERROR("################\nThe indirect address of the " + "EEPROM area has been changed. \nTurn off Torque " + "Enable and try again."); + exit(-1); + } + write2Byte(joint_name, indirect_addr, + dxl->ctrl_table_[dxl->bulk_read_items_[i]->item_name_] + ->address_ + + l); + } + indirect_addr += 2; + } + } + } + } else // INDIRECT_ADDRESS_1 NOT exist + { + if (dxl->bulk_read_items_.size() != 0) { + bulkread_start_addr = dxl->bulk_read_items_[0]->address_; + bulkread_data_length = 0; + + ControlTableItem *last_item = dxl->bulk_read_items_[0]; + + for (int i = 0; i < dxl->bulk_read_items_.size(); i++) { + int addr = dxl->bulk_read_items_[i]->address_; + if (addr < bulkread_start_addr) + bulkread_start_addr = addr; + else if (last_item->address_ < addr) + last_item = dxl->bulk_read_items_[i]; + } + + bulkread_data_length = + last_item->address_ - bulkread_start_addr + last_item->data_length_; + } + } + + // ROS_WARN("[%12s] start_addr: %d, data_length: %d", joint_name.c_str(), + // bulkread_start_addr, bulkread_data_length); + if (bulkread_start_addr != 0) + port_to_bulk_read_[dxl->port_name_]->addParam( + dxl->id_, bulkread_start_addr, bulkread_data_length); + + // Torque ON + if (writeCtrlItem(joint_name, dxl->torque_enable_item_->item_name_, 1) != + COMM_SUCCESS) + writeCtrlItem(joint_name, dxl->torque_enable_item_->item_name_, 1); + } + + for (auto &it : robot_->sensors_) { + std::string sensor_name = it.first; + Sensor *sensor = it.second; + + if (sensor == NULL) + continue; + + int bulkread_start_addr = 0; + int bulkread_data_length = 0; + + // calculate bulk read start address & data length + auto indirect_addr_it = sensor->ctrl_table_.find(INDIRECT_ADDRESS_1); + if (indirect_addr_it != + sensor->ctrl_table_.end()) // INDIRECT_ADDRESS_1 exist + { + if (sensor->bulk_read_items_.size() != 0) { + uint16_t data16 = 0; + + bulkread_start_addr = sensor->bulk_read_items_[0]->address_; + bulkread_data_length = 0; + + // set indirect address + int indirect_addr = indirect_addr_it->second->address_; + for (int i = 0; i < sensor->bulk_read_items_.size(); i++) { + int addr_leng = sensor->bulk_read_items_[i]->data_length_; + + bulkread_data_length += addr_leng; + for (int l = 0; l < addr_leng; l++) { + // ROS_WARN("[%12s] INDIR_ADDR: %d, ITEM_ADDR: %d", + // sensor_name.c_str(), indirect_addr, + // sensor->ctrl_table[sensor->bulk_read_items[i]->item_name]->address + // + _l); + read2Byte(sensor_name, indirect_addr, &data16); + if (data16 != + sensor->ctrl_table_[sensor->bulk_read_items_[i]->item_name_] + ->address_ + + l) { + write2Byte( + sensor_name, indirect_addr, + sensor->ctrl_table_[sensor->bulk_read_items_[i]->item_name_] + ->address_ + + l); + } + indirect_addr += 2; + } + } + } + } else // INDIRECT_ADDRESS_1 NOT exist + { + if (sensor->bulk_read_items_.size() != 0) { + bulkread_start_addr = sensor->bulk_read_items_[0]->address_; + bulkread_data_length = 0; + + ControlTableItem *last_item = sensor->bulk_read_items_[0]; + + for (int i = 0; i < sensor->bulk_read_items_.size(); i++) { + int addr = sensor->bulk_read_items_[i]->address_; + if (addr < bulkread_start_addr) + bulkread_start_addr = addr; + else if (last_item->address_ < addr) + last_item = sensor->bulk_read_items_[i]; + } + + bulkread_data_length = + last_item->address_ - bulkread_start_addr + last_item->data_length_; + } + } + + // ROS_WARN("[%12s] start_addr: %d, data_length: %d", sensor_name.c_str(), + // bulkread_start_addr, bulkread_data_length); + if (bulkread_start_addr != 0) + port_to_bulk_read_[sensor->port_name_]->addParam( + sensor->id_, bulkread_start_addr, bulkread_data_length); + } +} + +void RobotisController::gazeboTimerThread() { + ros::Rate gazebo_rate(1000 / robot_->getControlCycle()); + + while (!stop_timer_) { + if (init_pose_loaded_ == true) + process(); + gazebo_rate.sleep(); + } +} + +void RobotisController::msgQueueThread() { + ros::NodeHandle ros_node; + ros::CallbackQueue callback_queue; + + ros_node.setCallbackQueue(&callback_queue); + + /* subscriber */ + ros::Subscriber write_control_table_sub = ros_node.subscribe( + "/humanoid_robot_intelligence_control_system/write_control_table", 5, + &RobotisController::writeControlTableCallback, this); + ros::Subscriber sync_write_item_sub = ros_node.subscribe( + "/humanoid_robot_intelligence_control_system/sync_write_item", 10, + &RobotisController::syncWriteItemCallback, this); + ros::Subscriber joint_ctrl_modules_sub = ros_node.subscribe( + "/humanoid_robot_intelligence_control_system/set_joint_ctrl_modules", 10, + &RobotisController::setJointCtrlModuleCallback, this); + ros::Subscriber enable_ctrl_module_sub = ros_node.subscribe( + "/humanoid_robot_intelligence_control_system/enable_ctrl_module", 10, + &RobotisController::setCtrlModuleCallback, this); + ros::Subscriber control_mode_sub = ros_node.subscribe( + "/humanoid_robot_intelligence_control_system/set_control_mode", 10, + &RobotisController::setControllerModeCallback, this); + ros::Subscriber joint_states_sub = ros_node.subscribe( + "/humanoid_robot_intelligence_control_system/set_joint_states", 10, + &RobotisController::setJointStatesCallback, this); + ros::Subscriber enable_offset_sub = ros_node.subscribe( + "/humanoid_robot_intelligence_control_system/enable_offset", 10, + &RobotisController::enableOffsetCallback, this); + + ros::Subscriber gazebo_joint_states_sub; + if (gazebo_mode_ == true) + gazebo_joint_states_sub = + ros_node.subscribe("/" + gazebo_robot_name_ + "/joint_states", 10, + &RobotisController::gazeboJointStatesCallback, this); + + /* publisher */ + goal_joint_state_pub_ = ros_node.advertise( + "/humanoid_robot_intelligence_control_system/goal_joint_states", 10); + present_joint_state_pub_ = ros_node.advertise( + "/humanoid_robot_intelligence_control_system/present_joint_states", 10); + current_module_pub_ = ros_node.advertise< + humanoid_robot_intelligence_control_system_controller_msgs:: + JointCtrlModule>( + "/humanoid_robot_intelligence_control_system/present_joint_ctrl_modules", + 10); + + if (gazebo_mode_ == true) { + for (auto &it : robot_->dxls_) { + gazebo_joint_position_pub_[it.first] = + ros_node.advertise( + "/" + gazebo_robot_name_ + "/" + it.first + "_position/command", + 1); + gazebo_joint_velocity_pub_[it.first] = + ros_node.advertise( + "/" + gazebo_robot_name_ + "/" + it.first + "_velocity/command", + 1); + gazebo_joint_effort_pub_[it.first] = + ros_node.advertise( + "/" + gazebo_robot_name_ + "/" + it.first + "_effort/command", 1); + } + } + + /* service */ + ros::ServiceServer get_joint_module_server = ros_node.advertiseService( + "/humanoid_robot_intelligence_control_system/" + "get_present_joint_ctrl_modules", + &RobotisController::getJointCtrlModuleService, this); + ros::ServiceServer set_joint_module_server = ros_node.advertiseService( + "/humanoid_robot_intelligence_control_system/" + "set_present_joint_ctrl_modules", + &RobotisController::setJointCtrlModuleService, this); + ros::ServiceServer set_module_server = ros_node.advertiseService( + "/humanoid_robot_intelligence_control_system/set_present_ctrl_modules", + &RobotisController::setCtrlModuleService, this); + ros::ServiceServer load_offset_server = ros_node.advertiseService( + "/humanoid_robot_intelligence_control_system/load_offset", + &RobotisController::loadOffsetService, this); + + ros::WallDuration duration(robot_->getControlCycle() / 1000.0); + while (ros_node.ok()) + callback_queue.callAvailable(duration); +} + +void *RobotisController::timerThread(void *param) { + RobotisController *controller = (RobotisController *)param; + static struct timespec next_time; + static struct timespec curr_time; + + ROS_DEBUG("controller::thread_proc started"); + + clock_gettime(CLOCK_MONOTONIC, &next_time); + + while (!controller->stop_timer_) { + next_time.tv_sec += + (next_time.tv_nsec + controller->robot_->getControlCycle() * 1000000) / + 1000000000; + next_time.tv_nsec = + (next_time.tv_nsec + controller->robot_->getControlCycle() * 1000000) % + 1000000000; + + controller->process(); + + clock_gettime(CLOCK_MONOTONIC, &curr_time); + long delta_nsec = (next_time.tv_sec - curr_time.tv_sec) * 1000000000 + + (next_time.tv_nsec - curr_time.tv_nsec); + if (delta_nsec < -100000) { + if (controller->DEBUG_PRINT == true) { + fprintf(stderr, + "[RobotisController::ThreadProc] NEXT TIME < CURR TIME.. " + "(%f)[%ld.%09ld / %ld.%09ld]", + delta_nsec / 1000000.0, (long)next_time.tv_sec, + (long)next_time.tv_nsec, (long)curr_time.tv_sec, + (long)curr_time.tv_nsec); + } + + // next_time = curr_time + 3 msec + next_time.tv_sec = + curr_time.tv_sec + (curr_time.tv_nsec + 3000000) / 1000000000; + next_time.tv_nsec = (curr_time.tv_nsec + 3000000) % 1000000000; + } + + clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &next_time, NULL); + } + return 0; +} + +void RobotisController::startTimer() { + if (this->is_timer_running_ == true) + return; + + if (this->gazebo_mode_ == true) { + // create and start the thread + gazebo_thread_ = + boost::thread(boost::bind(&RobotisController::gazeboTimerThread, this)); + } else { + initializeSyncWrite(); + + for (auto &it : port_to_bulk_read_) { + it.second->txPacket(); + } + + usleep(8 * 1000); + + int error; + struct sched_param param; + pthread_attr_t attr; + + pthread_attr_init(&attr); + + error = pthread_attr_setschedpolicy(&attr, SCHED_RR); + if (error != 0) + ROS_ERROR("pthread_attr_setschedpolicy error = %d\n", error); + error = pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED); + if (error != 0) + ROS_ERROR("pthread_attr_setinheritsched error = %d\n", error); + + memset(¶m, 0, sizeof(param)); + param.sched_priority = 31; // RT + error = pthread_attr_setschedparam(&attr, ¶m); + if (error != 0) + ROS_ERROR("pthread_attr_setschedparam error = %d\n", error); + + // create and start the thread + if ((error = pthread_create(&this->timer_thread_, &attr, this->timerThread, + this)) != 0) { + ROS_ERROR("Creating timer thread failed!!"); + exit(-1); + } + } + + this->is_timer_running_ = true; +} + +void RobotisController::stopTimer() { + int error = 0; + + // set the flag to stop the thread + if (this->is_timer_running_) { + this->stop_timer_ = true; + + if (this->gazebo_mode_ == false) { + // wait until the thread is stopped. + if ((error = pthread_join(this->timer_thread_, NULL)) != 0) + exit(-1); + + for (auto &it : port_to_bulk_read_) { + if (it.second != NULL) + it.second->rxPacket(); + } + + for (auto &it : port_to_sync_write_position_) { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto &it : port_to_sync_write_position_p_gain_) { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto &it : port_to_sync_write_position_i_gain_) { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto &it : port_to_sync_write_position_d_gain_) { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto &it : port_to_sync_write_velocity_) { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto &it : port_to_sync_write_velocity_p_gain_) { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto &it : port_to_sync_write_velocity_i_gain_) { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto &it : port_to_sync_write_velocity_d_gain_) { + if (it.second != NULL) + it.second->clearParam(); + } + for (auto &it : port_to_sync_write_current_) { + if (it.second != NULL) + it.second->clearParam(); + } + } else { + // wait until the thread is stopped. + gazebo_thread_.join(); + } + + this->stop_timer_ = false; + this->is_timer_running_ = false; + } +} + +bool RobotisController::isTimerRunning() { return this->is_timer_running_; } + +void RobotisController::loadOffset(const std::string path) { + YAML::Node doc; + try { + doc = YAML::LoadFile(path.c_str()); + } catch (const std::exception &e) { + ROS_WARN("Fail to load offset yaml."); + return; + } + + YAML::Node offset_node = doc["offset"]; + if (offset_node.size() == 0) + return; + + ROS_INFO("Load offsets..."); + for (YAML::const_iterator it = offset_node.begin(); it != offset_node.end(); + it++) { + std::string joint_name = it->first.as(); + double offset = it->second.as(); + + auto dxl_it = robot_->dxls_.find(joint_name); + if (dxl_it != robot_->dxls_.end()) + dxl_it->second->dxl_state_->position_offset_ = offset; + } +} + +void RobotisController::process() { + // avoid duplicated function call + static bool is_process_running = false; + if (is_process_running == true) + return; + is_process_running = true; + + // ROS_INFO("Controller::Process()"); + // offset ratio + if (is_offset_enabled_) { + if (offset_ratio_ < 1.0) + offset_ratio_ += 0.01; + else + offset_ratio_ = 1.0; + } else { + if (offset_ratio_ > 0.0) + offset_ratio_ -= 0.01; + else + offset_ratio_ = 0.0; + } + + ros::Time start_time; + ros::Duration time_duration; + + if (DEBUG_PRINT) + start_time = ros::Time::now(); + + sensor_msgs::JointState goal_state; + sensor_msgs::JointState present_state; + + present_state.header.stamp = ros::Time::now(); + goal_state.header.stamp = present_state.header.stamp; + + if (controller_mode_ == MotionModuleMode) { + if (gazebo_mode_ == false) { + // BulkRead Rx + for (auto &it : port_to_bulk_read_) { + robot_->ports_[it.first]->setPacketTimeout(0.0); + if (DEBUG_PRINT) { + int result = it.second->rxPacket(); + if (result != COMM_SUCCESS) + ROS_ERROR_STREAM("Bulk Read Fail : " << it.first); + } else + it.second->rxPacket(); + } + + // -> save to robot->dxls_[]->dxl_state_ + if (robot_->dxls_.size() > 0) { + for (auto &dxl_it : robot_->dxls_) { + Dynamixel *dxl = dxl_it.second; + std::string port_name = dxl_it.second->port_name_; + std::string joint_name = dxl_it.first; + + if (dxl->bulk_read_items_.size() > 0) { + bool updated = false; + uint32_t data = 0; + for (int i = 0; i < dxl->bulk_read_items_.size(); i++) { + ControlTableItem *item = dxl->bulk_read_items_[i]; + if (port_to_bulk_read_[port_name]->isAvailable( + dxl->id_, item->address_, item->data_length_) == true) { + updated = true; + data = port_to_bulk_read_[port_name]->getData( + dxl->id_, item->address_, item->data_length_); + + // change dxl_state + if (dxl->present_position_item_ != 0 && + item->item_name_ == + dxl->present_position_item_->item_name_) { + dxl->dxl_state_->present_position_ = + dxl->convertValue2Radian(data) - + dxl->dxl_state_->position_offset_ * offset_ratio_; + } else if (dxl->present_velocity_item_ != 0 && + item->item_name_ == + dxl->present_velocity_item_->item_name_) + dxl->dxl_state_->present_velocity_ = + dxl->convertValue2Velocity(data); + else if (dxl->present_current_item_ != 0 && + item->item_name_ == + dxl->present_current_item_->item_name_) + dxl->dxl_state_->present_torque_ = + dxl->convertValue2Torque(data); + else if (dxl->goal_position_item_ != 0 && + item->item_name_ == + dxl->goal_position_item_->item_name_) { + dxl->dxl_state_->goal_position_ = + dxl->convertValue2Radian(data) - + dxl->dxl_state_->position_offset_ * offset_ratio_; + } else if (dxl->goal_velocity_item_ != 0 && + item->item_name_ == + dxl->goal_velocity_item_->item_name_) + dxl->dxl_state_->goal_velocity_ = + dxl->convertValue2Velocity(data); + else if (dxl->goal_current_item_ != 0 && + item->item_name_ == + dxl->goal_current_item_->item_name_) + dxl->dxl_state_->goal_torque_ = + dxl->convertValue2Torque(data); + + dxl->dxl_state_->bulk_read_table_[item->item_name_] = data; + } + } + + // -> update time stamp to + // Robot->dxls[]->dynamixel_state.update_time_stamp + if (updated == true) + dxl->dxl_state_->update_time_stamp_ = + TimeStamp(present_state.header.stamp.sec, + present_state.header.stamp.nsec); + } + } + } + + // -> save to robot->sensors_[]->sensor_state_ + if (robot_->sensors_.size() > 0) { + for (auto &sensor_it : robot_->sensors_) { + Sensor *sensor = sensor_it.second; + std::string port_name = sensor_it.second->port_name_; + std::string sensor_name = sensor_it.first; + + if (sensor->bulk_read_items_.size() > 0) { + bool updated = false; + uint32_t data = 0; + for (int i = 0; i < sensor->bulk_read_items_.size(); i++) { + ControlTableItem *item = sensor->bulk_read_items_[i]; + if (port_to_bulk_read_[port_name]->isAvailable( + sensor->id_, item->address_, item->data_length_) == + true) { + updated = true; + data = port_to_bulk_read_[port_name]->getData( + sensor->id_, item->address_, item->data_length_); + + // change sensor_state + sensor->sensor_state_->bulk_read_table_[item->item_name_] = + data; + } + } + + // -> update time stamp to + // Robot->dxls[]->dynamixel_state.update_time_stamp + if (updated == true) + sensor->sensor_state_->update_time_stamp_ = + TimeStamp(present_state.header.stamp.sec, + present_state.header.stamp.nsec); + } + } + } + + if (DEBUG_PRINT) { + time_duration = ros::Time::now() - start_time; + fprintf(stderr, "(%2.6f) BulkRead Rx & update state \n", + time_duration.nsec * 0.000001); + } + + // SyncWrite + queue_mutex_.lock(); + + if (direct_sync_write_.size() > 0) { + for (int i = 0; i < direct_sync_write_.size(); i++) { + direct_sync_write_[i]->txPacket(); + direct_sync_write_[i]->clearParam(); + } + direct_sync_write_.clear(); + } + + if (port_to_sync_write_position_p_gain_.size() > 0) { + for (auto &it : port_to_sync_write_position_p_gain_) { + it.second->txPacket(); + it.second->clearParam(); + } + } + if (port_to_sync_write_position_i_gain_.size() > 0) { + for (auto &it : port_to_sync_write_position_i_gain_) { + it.second->txPacket(); + it.second->clearParam(); + } + } + if (port_to_sync_write_position_d_gain_.size() > 0) { + for (auto &it : port_to_sync_write_position_d_gain_) { + it.second->txPacket(); + it.second->clearParam(); + } + } + if (port_to_sync_write_velocity_p_gain_.size() > 0) { + for (auto &it : port_to_sync_write_velocity_p_gain_) { + it.second->txPacket(); + it.second->clearParam(); + } + } + if (port_to_sync_write_velocity_i_gain_.size() > 0) { + for (auto &it : port_to_sync_write_velocity_i_gain_) { + it.second->txPacket(); + it.second->clearParam(); + } + } + if (port_to_sync_write_velocity_d_gain_.size() > 0) { + for (auto &it : port_to_sync_write_velocity_d_gain_) { + it.second->txPacket(); + it.second->clearParam(); + } + } + for (auto &it : port_to_sync_write_position_) { + if (it.second != NULL) + it.second->txPacket(); + } + for (auto &it : port_to_sync_write_velocity_) { + if (it.second != NULL) + it.second->txPacket(); + } + for (auto &it : port_to_sync_write_current_) { + if (it.second != NULL) + it.second->txPacket(); + } + + queue_mutex_.unlock(); + + // BulkRead Tx + for (auto &it : port_to_bulk_read_) + it.second->txPacket(); + + if (DEBUG_PRINT) { + time_duration = ros::Time::now() - start_time; + fprintf(stderr, "(%2.6f) SyncWrite & BulkRead Tx \n", + time_duration.nsec * 0.000001); + } + } else if (gazebo_mode_ == true) { + std_msgs::Float64 joint_msg; + + for (auto &dxl_it : robot_->dxls_) { + std::string joint_name = dxl_it.first; + Dynamixel *dxl = dxl_it.second; + DynamixelState *dxl_state = dxl_it.second->dxl_state_; + + if (dxl->ctrl_module_name_ == "none") { + joint_msg.data = dxl_state->goal_position_; + gazebo_joint_position_pub_[joint_name].publish(joint_msg); + } + } + + for (auto module_it = motion_modules_.begin(); + module_it != motion_modules_.end(); module_it++) { + if ((*module_it)->getModuleEnable() == false) + continue; + + for (auto &dxl_it : robot_->dxls_) { + std::string joint_name = dxl_it.first; + Dynamixel *dxl = dxl_it.second; + DynamixelState *dxl_state = dxl_it.second->dxl_state_; + + if (dxl->ctrl_module_name_ == (*module_it)->getModuleName()) { + if ((*module_it)->getControlMode() == PositionControl) { + joint_msg.data = dxl_state->goal_position_; + gazebo_joint_position_pub_[joint_name].publish(joint_msg); + } else if ((*module_it)->getControlMode() == VelocityControl) { + joint_msg.data = dxl_state->goal_velocity_; + gazebo_joint_velocity_pub_[joint_name].publish(joint_msg); + } else if ((*module_it)->getControlMode() == TorqueControl) { + joint_msg.data = dxl_state->goal_torque_; + gazebo_joint_effort_pub_[joint_name].publish(joint_msg); + } + } + } + } + } + } else if (controller_mode_ == DirectControlMode) { + if (gazebo_mode_ == false) { + // BulkRead Rx + for (auto &it : port_to_bulk_read_) { + robot_->ports_[it.first]->setPacketTimeout(0.0); + it.second->rxPacket(); + } + + // -> save to robot->dxls_[]->dxl_state_ + if (robot_->dxls_.size() > 0) { + for (auto &dxl_it : robot_->dxls_) { + Dynamixel *dxl = dxl_it.second; + std::string port_name = dxl_it.second->port_name_; + std::string joint_name = dxl_it.first; + + if (dxl->bulk_read_items_.size() > 0) { + uint32_t data = 0; + for (int i = 0; i < dxl->bulk_read_items_.size(); i++) { + ControlTableItem *item = dxl->bulk_read_items_[i]; + if (port_to_bulk_read_[port_name]->isAvailable( + dxl->id_, item->address_, item->data_length_) == true) { + data = port_to_bulk_read_[port_name]->getData( + dxl->id_, item->address_, item->data_length_); + + // change dxl_state + if (dxl->present_position_item_ != 0 && + item->item_name_ == + dxl->present_position_item_->item_name_) { + dxl->dxl_state_->present_position_ = + dxl->convertValue2Radian(data) - + dxl->dxl_state_->position_offset_ * offset_ratio_; + } else if (dxl->present_velocity_item_ != 0 && + item->item_name_ == + dxl->present_velocity_item_->item_name_) + dxl->dxl_state_->present_velocity_ = + dxl->convertValue2Velocity(data); + else if (dxl->present_current_item_ != 0 && + item->item_name_ == + dxl->present_current_item_->item_name_) + dxl->dxl_state_->present_torque_ = + dxl->convertValue2Torque(data); + else if (dxl->goal_position_item_ != 0 && + item->item_name_ == + dxl->goal_position_item_->item_name_) { + dxl->dxl_state_->goal_position_ = + dxl->convertValue2Radian(data) - + dxl->dxl_state_->position_offset_ * offset_ratio_; + } else if (dxl->goal_velocity_item_ != 0 && + item->item_name_ == + dxl->goal_velocity_item_->item_name_) + dxl->dxl_state_->goal_velocity_ = + dxl->convertValue2Velocity(data); + else if (dxl->goal_current_item_ != 0 && + item->item_name_ == + dxl->goal_current_item_->item_name_) + dxl->dxl_state_->goal_torque_ = + dxl->convertValue2Torque(data); + + dxl->dxl_state_->bulk_read_table_[item->item_name_] = data; + } + } + + // -> update time stamp to + // Robot->dxls[]->dynamixel_state.update_time_stamp + dxl->dxl_state_->update_time_stamp_ = + TimeStamp(present_state.header.stamp.sec, + present_state.header.stamp.nsec); + } + } + } + + queue_mutex_.lock(); + + // for (auto& it : port_to_sync_write_position_) + // { + // it.second->txPacket(); + // it.second->clearParam(); + // } + + if (direct_sync_write_.size() > 0) { + for (int i = 0; i < direct_sync_write_.size(); i++) { + direct_sync_write_[i]->txPacket(); + direct_sync_write_[i]->clearParam(); + } + direct_sync_write_.clear(); + } + + queue_mutex_.unlock(); + + // BulkRead Tx + for (auto &it : port_to_bulk_read_) + it.second->txPacket(); + } + } + + // Call SensorModule Process() + // -> for loop : call SensorModule list -> Process() + if (sensor_modules_.size() > 0) { + for (auto module_it = sensor_modules_.begin(); + module_it != sensor_modules_.end(); module_it++) { + (*module_it)->process(robot_->dxls_, robot_->sensors_); + + for (auto &it : (*module_it)->result_) + sensor_result_[it.first] = it.second; + } + } + + if (DEBUG_PRINT) { + time_duration = ros::Time::now() - start_time; + fprintf(stderr, "(%2.6f) SensorModule Process() & save result \n", + time_duration.nsec * 0.000001); + } + + if (controller_mode_ == MotionModuleMode) { + // Call MotionModule Process() + // -> for loop : call MotionModule list -> Process() + if (motion_modules_.size() > 0) { + queue_mutex_.lock(); + + for (auto module_it = motion_modules_.begin(); + module_it != motion_modules_.end(); module_it++) { + if ((*module_it)->getModuleEnable() == false) + continue; + + (*module_it)->process(robot_->dxls_, sensor_result_); + + // for loop : joint list + for (auto &dxl_it : robot_->dxls_) { + std::string joint_name = dxl_it.first; + Dynamixel *dxl = dxl_it.second; + DynamixelState *dxl_state = dxl_it.second->dxl_state_; + + if (dxl->ctrl_module_name_ == (*module_it)->getModuleName()) { + // do_sync_write = true; + DynamixelState *result_state = (*module_it)->result_[joint_name]; + + if (result_state == NULL) { + ROS_ERROR("[%s] %s ", (*module_it)->getModuleName().c_str(), + joint_name.c_str()); + continue; + } + + // TODO: check update time stamp ? + + if ((*module_it)->getControlMode() == PositionControl) { + dxl_state->goal_position_ = result_state->goal_position_; + + if (gazebo_mode_ == false) { + // add offset + uint32_t pos_data; + pos_data = dxl->convertRadian2Value( + dxl_state->goal_position_ + + dxl_state->position_offset_ * offset_ratio_); + + uint8_t sync_write_data[4] = {0}; + sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(pos_data)); + sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(pos_data)); + sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(pos_data)); + sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(pos_data)); + + if (port_to_sync_write_position_[dxl->port_name_] != NULL) + port_to_sync_write_position_[dxl->port_name_]->changeParam( + dxl->id_, sync_write_data); + + // if position p gain value is changed -> sync write + if (result_state->position_p_gain_ != NONE_GAIN && + dxl_state->position_p_gain_ != + result_state->position_p_gain_) { + dxl_state->position_p_gain_ = result_state->position_p_gain_; + uint8_t sync_write_data[4] = {0}; + sync_write_data[0] = + DXL_LOBYTE(DXL_LOWORD(dxl_state->position_p_gain_)); + sync_write_data[1] = + DXL_HIBYTE(DXL_LOWORD(dxl_state->position_p_gain_)); + sync_write_data[2] = + DXL_LOBYTE(DXL_HIWORD(dxl_state->position_p_gain_)); + sync_write_data[3] = + DXL_HIBYTE(DXL_HIWORD(dxl_state->position_p_gain_)); + + if (port_to_sync_write_position_p_gain_[dxl->port_name_] != + NULL) + port_to_sync_write_position_p_gain_[dxl->port_name_] + ->addParam(dxl->id_, sync_write_data); + } + + // if position i gain value is changed -> sync write + if (result_state->position_i_gain_ != NONE_GAIN && + dxl_state->position_i_gain_ != + result_state->position_i_gain_) { + dxl_state->position_i_gain_ = result_state->position_i_gain_; + uint8_t sync_write_data[4] = {0}; + sync_write_data[0] = + DXL_LOBYTE(DXL_LOWORD(dxl_state->position_i_gain_)); + sync_write_data[1] = + DXL_HIBYTE(DXL_LOWORD(dxl_state->position_i_gain_)); + sync_write_data[2] = + DXL_LOBYTE(DXL_HIWORD(dxl_state->position_i_gain_)); + sync_write_data[3] = + DXL_HIBYTE(DXL_HIWORD(dxl_state->position_i_gain_)); + + if (port_to_sync_write_position_i_gain_[dxl->port_name_] != + NULL) + port_to_sync_write_position_i_gain_[dxl->port_name_] + ->addParam(dxl->id_, sync_write_data); + } + + // if position d gain value is changed -> sync write + if (result_state->position_d_gain_ != NONE_GAIN && + dxl_state->position_d_gain_ != + result_state->position_d_gain_) { + dxl_state->position_d_gain_ = result_state->position_d_gain_; + uint8_t sync_write_data[4] = {0}; + sync_write_data[0] = + DXL_LOBYTE(DXL_LOWORD(dxl_state->position_d_gain_)); + sync_write_data[1] = + DXL_HIBYTE(DXL_LOWORD(dxl_state->position_d_gain_)); + sync_write_data[2] = + DXL_LOBYTE(DXL_HIWORD(dxl_state->position_d_gain_)); + sync_write_data[3] = + DXL_HIBYTE(DXL_HIWORD(dxl_state->position_d_gain_)); + + if (port_to_sync_write_position_d_gain_[dxl->port_name_] != + NULL) + port_to_sync_write_position_d_gain_[dxl->port_name_] + ->addParam(dxl->id_, sync_write_data); + } + + // if velocity p gain gain value is changed -> sync write + if (result_state->velocity_p_gain_ != NONE_GAIN && + dxl_state->velocity_p_gain_ != + result_state->velocity_p_gain_) { + dxl_state->velocity_p_gain_ = result_state->velocity_p_gain_; + uint8_t sync_write_data[4] = {0}; + sync_write_data[0] = + DXL_LOBYTE(DXL_LOWORD(dxl_state->velocity_p_gain_)); + sync_write_data[1] = + DXL_HIBYTE(DXL_LOWORD(dxl_state->velocity_p_gain_)); + sync_write_data[2] = + DXL_LOBYTE(DXL_HIWORD(dxl_state->velocity_p_gain_)); + sync_write_data[3] = + DXL_HIBYTE(DXL_HIWORD(dxl_state->velocity_p_gain_)); + + if (port_to_sync_write_velocity_p_gain_[dxl->port_name_] != + NULL) + port_to_sync_write_velocity_p_gain_[dxl->port_name_] + ->addParam(dxl->id_, sync_write_data); + } + + // if velocity i gain value is changed -> sync write + if (result_state->velocity_i_gain_ != NONE_GAIN && + dxl_state->velocity_i_gain_ != + result_state->velocity_i_gain_) { + dxl_state->velocity_i_gain_ = result_state->velocity_i_gain_; + uint8_t sync_write_data[4] = {0}; + sync_write_data[0] = + DXL_LOBYTE(DXL_LOWORD(dxl_state->velocity_i_gain_)); + sync_write_data[1] = + DXL_HIBYTE(DXL_LOWORD(dxl_state->velocity_i_gain_)); + sync_write_data[2] = + DXL_LOBYTE(DXL_HIWORD(dxl_state->velocity_i_gain_)); + sync_write_data[3] = + DXL_HIBYTE(DXL_HIWORD(dxl_state->velocity_i_gain_)); + + if (port_to_sync_write_velocity_i_gain_[dxl->port_name_] != + NULL) + port_to_sync_write_velocity_i_gain_[dxl->port_name_] + ->addParam(dxl->id_, sync_write_data); + } + } + } else if ((*module_it)->getControlMode() == VelocityControl) { + dxl_state->goal_velocity_ = result_state->goal_velocity_; + + if (gazebo_mode_ == false) { + uint32_t vel_data = + dxl->convertVelocity2Value(dxl_state->goal_velocity_); + uint8_t sync_write_data[4] = {0}; + sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(vel_data)); + sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(vel_data)); + sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(vel_data)); + sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(vel_data)); + + if (port_to_sync_write_velocity_[dxl->port_name_] != NULL) + port_to_sync_write_velocity_[dxl->port_name_]->changeParam( + dxl->id_, sync_write_data); + + // if velocity p gain gain value is changed -> sync write + if (result_state->velocity_p_gain_ != NONE_GAIN && + dxl_state->velocity_p_gain_ != + result_state->velocity_p_gain_) { + dxl_state->velocity_p_gain_ = result_state->velocity_p_gain_; + uint8_t sync_write_data[4] = {0}; + sync_write_data[0] = + DXL_LOBYTE(DXL_LOWORD(dxl_state->velocity_p_gain_)); + sync_write_data[1] = + DXL_HIBYTE(DXL_LOWORD(dxl_state->velocity_p_gain_)); + sync_write_data[2] = + DXL_LOBYTE(DXL_HIWORD(dxl_state->velocity_p_gain_)); + sync_write_data[3] = + DXL_HIBYTE(DXL_HIWORD(dxl_state->velocity_p_gain_)); + + if (port_to_sync_write_velocity_p_gain_[dxl->port_name_] != + NULL) + port_to_sync_write_velocity_p_gain_[dxl->port_name_] + ->addParam(dxl->id_, sync_write_data); + } + + // if velocity i gain value is changed -> sync write + if (result_state->velocity_i_gain_ != NONE_GAIN && + dxl_state->velocity_i_gain_ != + result_state->velocity_i_gain_) { + dxl_state->velocity_i_gain_ = result_state->velocity_i_gain_; + uint8_t sync_write_data[4] = {0}; + sync_write_data[0] = + DXL_LOBYTE(DXL_LOWORD(dxl_state->velocity_i_gain_)); + sync_write_data[1] = + DXL_HIBYTE(DXL_LOWORD(dxl_state->velocity_i_gain_)); + sync_write_data[2] = + DXL_LOBYTE(DXL_HIWORD(dxl_state->velocity_i_gain_)); + sync_write_data[3] = + DXL_HIBYTE(DXL_HIWORD(dxl_state->velocity_i_gain_)); + + if (port_to_sync_write_velocity_i_gain_[dxl->port_name_] != + NULL) + port_to_sync_write_velocity_i_gain_[dxl->port_name_] + ->addParam(dxl->id_, sync_write_data); + } + + // if velocity d gain value is changed -> sync write + if (result_state->velocity_d_gain_ != NONE_GAIN && + dxl_state->velocity_d_gain_ != + result_state->velocity_d_gain_) { + dxl_state->velocity_d_gain_ = result_state->velocity_d_gain_; + uint8_t sync_write_data[4] = {0}; + sync_write_data[0] = + DXL_LOBYTE(DXL_LOWORD(dxl_state->velocity_d_gain_)); + sync_write_data[1] = + DXL_HIBYTE(DXL_LOWORD(dxl_state->velocity_d_gain_)); + sync_write_data[2] = + DXL_LOBYTE(DXL_HIWORD(dxl_state->velocity_d_gain_)); + sync_write_data[3] = + DXL_HIBYTE(DXL_HIWORD(dxl_state->velocity_d_gain_)); + + if (port_to_sync_write_velocity_d_gain_[dxl->port_name_] != + NULL) + port_to_sync_write_velocity_d_gain_[dxl->port_name_] + ->addParam(dxl->id_, sync_write_data); + } + } + } else if ((*module_it)->getControlMode() == TorqueControl) { + dxl_state->goal_torque_ = result_state->goal_torque_; + + if (gazebo_mode_ == false) { + uint32_t curr_data = + dxl->convertTorque2Value(dxl_state->goal_torque_); + uint8_t sync_write_data[2] = {0}; + sync_write_data[0] = DXL_LOBYTE(curr_data); + sync_write_data[1] = DXL_HIBYTE(curr_data); + + if (port_to_sync_write_current_[dxl->port_name_] != NULL) + port_to_sync_write_current_[dxl->port_name_]->changeParam( + dxl->id_, sync_write_data); + } + } + } + } + } + + queue_mutex_.unlock(); + } + + if (DEBUG_PRINT) { + time_duration = ros::Time::now() - start_time; + fprintf(stderr, "(%2.6f) MotionModule Process() & save result \n", + time_duration.nsec * 0.000001); + } + } + + // publish present & goal position + for (auto &dxl_it : robot_->dxls_) { + std::string joint_name = dxl_it.first; + Dynamixel *dxl = dxl_it.second; + + present_state.name.push_back(joint_name); + present_state.position.push_back(dxl->dxl_state_->present_position_); + present_state.velocity.push_back(dxl->dxl_state_->present_velocity_); + present_state.effort.push_back(dxl->dxl_state_->present_torque_); + + goal_state.name.push_back(joint_name); + goal_state.position.push_back(dxl->dxl_state_->goal_position_); + goal_state.velocity.push_back(dxl->dxl_state_->goal_velocity_); + goal_state.effort.push_back(dxl->dxl_state_->goal_torque_); + } + + // -> publish present joint_states & goal joint states topic + present_joint_state_pub_.publish(present_state); + goal_joint_state_pub_.publish(goal_state); + + if (DEBUG_PRINT) { + time_duration = ros::Time::now() - start_time; + fprintf(stderr, "(%2.6f) Process() DONE \n", time_duration.nsec * 0.000001); + } + + is_process_running = false; +} + +void RobotisController::addMotionModule(MotionModule *module) { + // check whether the module name already exists + for (auto m_it = motion_modules_.begin(); m_it != motion_modules_.end(); + m_it++) { + if ((*m_it)->getModuleName() == module->getModuleName()) { + ROS_ERROR("Motion Module Name [%s] already exist !!", + module->getModuleName().c_str()); + return; + } + } + + module->initialize(robot_->getControlCycle(), robot_); + motion_modules_.push_back(module); + motion_modules_.unique(); +} + +void RobotisController::removeMotionModule(MotionModule *module) { + motion_modules_.remove(module); +} + +void RobotisController::addSensorModule(SensorModule *module) { + // check whether the module name already exists + for (auto m_it = sensor_modules_.begin(); m_it != sensor_modules_.end(); + m_it++) { + if ((*m_it)->getModuleName() == module->getModuleName()) { + ROS_ERROR("Sensor Module Name [%s] already exist !!", + module->getModuleName().c_str()); + return; + } + } + + module->initialize(robot_->getControlCycle(), robot_); + sensor_modules_.push_back(module); + sensor_modules_.unique(); +} + +void RobotisController::removeSensorModule(SensorModule *module) { + sensor_modules_.remove(module); +} + +void RobotisController::writeControlTableCallback( + const humanoid_robot_intelligence_control_system_controller_msgs:: + WriteControlTable::ConstPtr &msg) { + Device *device = NULL; + + if (DEBUG_PRINT) + fprintf(stderr, "[WriteControlTable] led control msg received\n"); + + auto dev_it1 = robot_->dxls_.find(msg->joint_name); + if (dev_it1 != robot_->dxls_.end()) { + device = dev_it1->second; + } else { + auto dev_it2 = robot_->sensors_.find(msg->joint_name); + if (dev_it2 != robot_->sensors_.end()) { + device = dev_it2->second; + } else { + ROS_WARN("[WriteControlTable] Unknown device : %s", + msg->joint_name.c_str()); + return; + } + } + + ControlTableItem *item = NULL; + auto item_it = device->ctrl_table_.find(msg->start_item_name); + if (item_it != device->ctrl_table_.end()) { + item = item_it->second; + } else { + ROS_WARN("[WriteControlTable] Unknown item : %s", + msg->start_item_name.c_str()); + return; + } + + dynamixel::PortHandler *port = robot_->ports_[device->port_name_]; + dynamixel::PacketHandler *packet_handler = + dynamixel::PacketHandler::getPacketHandler(device->protocol_version_); + + if (item->access_type_ == Read) + return; + + queue_mutex_.lock(); + + direct_sync_write_.push_back(new dynamixel::GroupSyncWrite( + port, packet_handler, item->address_, msg->data_length)); + direct_sync_write_[direct_sync_write_.size() - 1]->addParam( + device->id_, (uint8_t *)(msg->data.data())); + + // fprintf(stderr, "[WriteControlTable] %s -> %s : ", + // msg->joint_name.c_str(), msg->start_item_name.c_str()); for (auto &dt : + // msg->data) + // fprintf(stderr, "%02X ", dt); + // fprintf(stderr, "\n"); + + queue_mutex_.unlock(); +} + +void RobotisController::syncWriteItemCallback( + const humanoid_robot_intelligence_control_system_controller_msgs:: + SyncWriteItem::ConstPtr &msg) { + for (int i = 0; i < msg->joint_name.size(); i++) { + Device *device; + + auto d_it1 = robot_->dxls_.find(msg->joint_name[i]); + if (d_it1 != robot_->dxls_.end()) { + device = d_it1->second; + } else { + auto d_it2 = robot_->sensors_.find(msg->joint_name[i]); + if (d_it2 != robot_->sensors_.end()) { + device = d_it2->second; + } else { + ROS_WARN("[SyncWriteItem] Unknown device : %s", + msg->joint_name[i].c_str()); + continue; + } + } + + // ControlTableItem *item = device->ctrl_table_[msg->item_name]; + ControlTableItem *item = NULL; + auto item_it = device->ctrl_table_.find(msg->item_name); + if (item_it != device->ctrl_table_.end()) { + item = item_it->second; + } else { + ROS_WARN("SyncWriteItem] Unknown item : %s", msg->item_name.c_str()); + continue; + } + + dynamixel::PortHandler *port = robot_->ports_[device->port_name_]; + dynamixel::PacketHandler *packet_handler = + dynamixel::PacketHandler::getPacketHandler(device->protocol_version_); + + if (item->access_type_ == Read) + continue; + + queue_mutex_.lock(); + + int idx = 0; + if (direct_sync_write_.size() == 0) { + direct_sync_write_.push_back(new dynamixel::GroupSyncWrite( + port, packet_handler, item->address_, item->data_length_)); + idx = 0; + } else { + for (idx = 0; idx < direct_sync_write_.size(); idx++) { + if (direct_sync_write_[idx]->getPortHandler() == port && + direct_sync_write_[idx]->getPacketHandler() == packet_handler) + break; + } + + if (idx == direct_sync_write_.size()) + direct_sync_write_.push_back(new dynamixel::GroupSyncWrite( + port, packet_handler, item->address_, item->data_length_)); + } + + uint8_t *data = new uint8_t[item->data_length_]; + if (item->data_length_ == 1) + data[0] = (uint8_t)msg->value[i]; + else if (item->data_length_ == 2) { + data[0] = DXL_LOBYTE((uint16_t)msg->value[i]); + data[1] = DXL_HIBYTE((uint16_t)msg->value[i]); + } else if (item->data_length_ == 4) { + data[0] = DXL_LOBYTE(DXL_LOWORD((uint32_t)msg->value[i])); + data[1] = DXL_HIBYTE(DXL_LOWORD((uint32_t)msg->value[i])); + data[2] = DXL_LOBYTE(DXL_HIWORD((uint32_t)msg->value[i])); + data[3] = DXL_HIBYTE(DXL_HIWORD((uint32_t)msg->value[i])); + } + direct_sync_write_[idx]->addParam(device->id_, data); + delete[] data; + + queue_mutex_.unlock(); + } +} + +void RobotisController::setControllerModeCallback( + const std_msgs::String::ConstPtr &msg) { + if (msg->data == "DirectControlMode") { + for (auto &it : port_to_bulk_read_) { + robot_->ports_[it.first]->setPacketTimeout(0.0); + it.second->rxPacket(); + } + controller_mode_ = DirectControlMode; + } else if (msg->data == "MotionModuleMode") { + for (auto &it : port_to_bulk_read_) { + it.second->txPacket(); + } + controller_mode_ = MotionModuleMode; + } +} + +void RobotisController::setJointStatesCallback( + const sensor_msgs::JointState::ConstPtr &msg) { + queue_mutex_.lock(); + + for (int i = 0; i < msg->name.size(); i++) { + Dynamixel *dxl = robot_->dxls_[msg->name[i]]; + if (dxl == NULL) + continue; + + if ((controller_mode_ == DirectControlMode) || + (controller_mode_ == MotionModuleMode && + dxl->ctrl_module_name_ == "none")) { + dxl->dxl_state_->goal_position_ = (double)msg->position[i]; + + if (gazebo_mode_ == false) { + // add offset + uint32_t pos_data; + pos_data = dxl->convertRadian2Value(dxl->dxl_state_->goal_position_ + + dxl->dxl_state_->position_offset_ * + offset_ratio_); + + uint8_t sync_write_data[4] = {0}; + sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(pos_data)); + sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(pos_data)); + sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(pos_data)); + sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(pos_data)); + + if (port_to_sync_write_position_[dxl->port_name_] != NULL) + port_to_sync_write_position_[dxl->port_name_]->changeParam( + dxl->id_, sync_write_data); + } + } + } + + queue_mutex_.unlock(); +} + +void RobotisController::setCtrlModuleCallback( + const std_msgs::String::ConstPtr &msg) { + if (set_module_thread_.joinable()) + set_module_thread_.join(); + + std::string _module_name_to_set = msg->data; + + set_module_thread_ = boost::thread(boost::bind( + &RobotisController::setCtrlModuleThread, this, _module_name_to_set)); +} + +void RobotisController::setCtrlModule(std::string module_name) { + if (set_module_thread_.joinable()) + set_module_thread_.join(); + + set_module_thread_ = boost::thread( + boost::bind(&RobotisController::setCtrlModuleThread, this, module_name)); +} +void RobotisController::setJointCtrlModuleCallback( + const humanoid_robot_intelligence_control_system_controller_msgs:: + JointCtrlModule::ConstPtr &msg) { + if (msg->joint_name.size() != msg->module_name.size()) + return; + + if (set_module_thread_.joinable()) + set_module_thread_.join(); + + set_module_thread_ = boost::thread( + boost::bind(&RobotisController::setJointCtrlModuleThread, this, msg)); +} + +void RobotisController::enableOffsetCallback( + const std_msgs::Bool::ConstPtr &msg) { + is_offset_enabled_ = (bool)msg->data; + if (is_offset_enabled_) + offset_ratio_ = 0.0; + else + offset_ratio_ = 1.0; +} + +bool RobotisController::getJointCtrlModuleService( + humanoid_robot_intelligence_control_system_controller_msgs::GetJointModule:: + Request &req, + humanoid_robot_intelligence_control_system_controller_msgs::GetJointModule:: + Response &res) { + for (unsigned int idx = 0; idx < req.joint_name.size(); idx++) { + auto d_it = robot_->dxls_.find((std::string)(req.joint_name[idx])); + if (d_it != robot_->dxls_.end()) { + res.joint_name.push_back(req.joint_name[idx]); + res.module_name.push_back(d_it->second->ctrl_module_name_); + } + } + + if (res.joint_name.size() == 0) + return false; + + return true; +} + +bool RobotisController::setJointCtrlModuleService( + humanoid_robot_intelligence_control_system_controller_msgs::SetJointModule:: + Request &req, + humanoid_robot_intelligence_control_system_controller_msgs::SetJointModule:: + Response &res) { + if (set_module_thread_.joinable()) + set_module_thread_.join(); + + humanoid_robot_intelligence_control_system_controller_msgs::JointCtrlModule + modules; + modules.joint_name = req.joint_name; + modules.module_name = req.module_name; + + humanoid_robot_intelligence_control_system_controller_msgs::JointCtrlModule:: + ConstPtr msg_ptr( + new humanoid_robot_intelligence_control_system_controller_msgs:: + JointCtrlModule(modules)); + + if (modules.joint_name.size() != modules.module_name.size()) + return false; + + set_module_thread_ = boost::thread( + boost::bind(&RobotisController::setJointCtrlModuleThread, this, msg_ptr)); + + set_module_thread_.join(); + + return true; +} + +bool RobotisController::setCtrlModuleService( + humanoid_robot_intelligence_control_system_controller_msgs::SetModule:: + Request &req, + humanoid_robot_intelligence_control_system_controller_msgs::SetModule:: + Response &res) { + if (set_module_thread_.joinable()) + set_module_thread_.join(); + + std::string _module_name_to_set = req.module_name; + + set_module_thread_ = boost::thread(boost::bind( + &RobotisController::setCtrlModuleThread, this, _module_name_to_set)); + + set_module_thread_.join(); + + res.result = true; + return true; +} + +bool RobotisController::loadOffsetService( + humanoid_robot_intelligence_control_system_controller_msgs::LoadOffset:: + Request &req, + humanoid_robot_intelligence_control_system_controller_msgs::LoadOffset:: + Response &res) { + loadOffset((std::string)req.file_path); + res.result = true; + return true; +} + +void RobotisController::setJointCtrlModuleThread( + const humanoid_robot_intelligence_control_system_controller_msgs:: + JointCtrlModule::ConstPtr &msg) { + // stop module list + std::list _stop_modules; + std::list _enable_modules; + + for (unsigned int idx = 0; idx < msg->joint_name.size(); idx++) { + Dynamixel *_dxl = NULL; + std::map::iterator _dxl_it = + robot_->dxls_.find((std::string)(msg->joint_name[idx])); + if (_dxl_it != robot_->dxls_.end()) + _dxl = _dxl_it->second; + else + continue; + + // enqueue + if (_dxl->ctrl_module_name_ != msg->module_name[idx]) { + for (std::list::iterator _stop_m_it = + motion_modules_.begin(); + _stop_m_it != motion_modules_.end(); _stop_m_it++) { + if ((*_stop_m_it)->getModuleName() == _dxl->ctrl_module_name_ && + (*_stop_m_it)->getModuleEnable() == true) + _stop_modules.push_back(*_stop_m_it); + } + } + } + + // stop the module + _stop_modules.unique(); + for (std::list::iterator _stop_m_it = _stop_modules.begin(); + _stop_m_it != _stop_modules.end(); _stop_m_it++) { + (*_stop_m_it)->stop(); + } + + // wait to stop + for (std::list::iterator _stop_m_it = _stop_modules.begin(); + _stop_m_it != _stop_modules.end(); _stop_m_it++) { + while ((*_stop_m_it)->isRunning()) + usleep(robot_->getControlCycle() * 1000); + } + + // disable module(s) + for (std::list::iterator _stop_m_it = _stop_modules.begin(); + _stop_m_it != _stop_modules.end(); _stop_m_it++) { + (*_stop_m_it)->setModuleEnable(false); + } + + // set ctrl module + queue_mutex_.lock(); + + for (unsigned int idx = 0; idx < msg->joint_name.size(); idx++) { + std::string ctrl_module = msg->module_name[idx]; + std::string joint_name = msg->joint_name[idx]; + + Dynamixel *_dxl = NULL; + std::map::iterator _dxl_it = + robot_->dxls_.find(joint_name); + if (_dxl_it != robot_->dxls_.end()) + _dxl = _dxl_it->second; + else + continue; + + // none + if (ctrl_module == "" || ctrl_module == "none") { + _dxl->ctrl_module_name_ = "none"; + + if (gazebo_mode_ == true) + continue; + + uint32_t _pos_data; + _pos_data = _dxl->convertRadian2Value(_dxl->dxl_state_->goal_position_ + + _dxl->dxl_state_->position_offset_ * + offset_ratio_); + + uint8_t _sync_write_data[4]; + _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_pos_data)); + _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_pos_data)); + _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_pos_data)); + _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_pos_data)); + + if (port_to_sync_write_position_[_dxl->port_name_] != NULL) + port_to_sync_write_position_[_dxl->port_name_]->addParam( + _dxl->id_, _sync_write_data); + + if (port_to_sync_write_current_[_dxl->port_name_] != NULL) + port_to_sync_write_current_[_dxl->port_name_]->removeParam(_dxl->id_); + if (port_to_sync_write_velocity_[_dxl->port_name_] != NULL) + port_to_sync_write_velocity_[_dxl->port_name_]->removeParam(_dxl->id_); + } else { + // check whether the module exist + for (std::list::iterator _m_it = motion_modules_.begin(); + _m_it != motion_modules_.end(); _m_it++) { + // if it exist + if ((*_m_it)->getModuleName() == ctrl_module) { + std::map::iterator _result_it = + (*_m_it)->result_.find(joint_name); + if (_result_it == (*_m_it)->result_.end()) + break; + + _dxl->ctrl_module_name_ = ctrl_module; + + // enqueue enable module list + _enable_modules.push_back(*_m_it); + ControlMode _mode = (*_m_it)->getControlMode(); + + if (gazebo_mode_ == true) + break; + + if (_mode == PositionControl) { + uint32_t _pos_data; + _pos_data = _dxl->convertRadian2Value( + _dxl->dxl_state_->goal_position_ + + _dxl->dxl_state_->position_offset_ * offset_ratio_); + + uint8_t _sync_write_data[4]; + _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_pos_data)); + _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_pos_data)); + _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_pos_data)); + _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_pos_data)); + + if (port_to_sync_write_position_[_dxl->port_name_] != NULL) + port_to_sync_write_position_[_dxl->port_name_]->addParam( + _dxl->id_, _sync_write_data); + + if (port_to_sync_write_current_[_dxl->port_name_] != NULL) + port_to_sync_write_current_[_dxl->port_name_]->removeParam( + _dxl->id_); + if (port_to_sync_write_velocity_[_dxl->port_name_] != NULL) + port_to_sync_write_velocity_[_dxl->port_name_]->removeParam( + _dxl->id_); + } else if (_mode == VelocityControl) { + uint32_t _vel_data = + _dxl->convertVelocity2Value(_dxl->dxl_state_->goal_velocity_); + uint8_t _sync_write_data[4]; + _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_vel_data)); + _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_vel_data)); + _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_vel_data)); + _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_vel_data)); + + if (port_to_sync_write_velocity_[_dxl->port_name_] != NULL) + port_to_sync_write_velocity_[_dxl->port_name_]->addParam( + _dxl->id_, _sync_write_data); + + if (port_to_sync_write_current_[_dxl->port_name_] != NULL) + port_to_sync_write_current_[_dxl->port_name_]->removeParam( + _dxl->id_); + if (port_to_sync_write_position_[_dxl->port_name_] != NULL) + port_to_sync_write_position_[_dxl->port_name_]->removeParam( + _dxl->id_); + } else if (_mode == TorqueControl) { + uint32_t _curr_data = + _dxl->convertTorque2Value(_dxl->dxl_state_->goal_torque_); + uint8_t _sync_write_data[4]; + _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_curr_data)); + _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_curr_data)); + _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_curr_data)); + _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_curr_data)); + + if (port_to_sync_write_current_[_dxl->port_name_] != NULL) + port_to_sync_write_current_[_dxl->port_name_]->addParam( + _dxl->id_, _sync_write_data); + + if (port_to_sync_write_velocity_[_dxl->port_name_] != NULL) + port_to_sync_write_velocity_[_dxl->port_name_]->removeParam( + _dxl->id_); + if (port_to_sync_write_position_[_dxl->port_name_] != NULL) + port_to_sync_write_position_[_dxl->port_name_]->removeParam( + _dxl->id_); + } + break; + } + } + } + } + + // enable module(s) + _enable_modules.unique(); + for (std::list::iterator _m_it = _enable_modules.begin(); + _m_it != _enable_modules.end(); _m_it++) { + (*_m_it)->setModuleEnable(true); + } + + // TODO: set indirect address + // -> check module's control_mode + + queue_mutex_.unlock(); + + // publish current module + humanoid_robot_intelligence_control_system_controller_msgs::JointCtrlModule + _current_module_msg; + for (std::map::iterator _dxl_iter = + robot_->dxls_.begin(); + _dxl_iter != robot_->dxls_.end(); ++_dxl_iter) { + _current_module_msg.joint_name.push_back(_dxl_iter->first); + _current_module_msg.module_name.push_back( + _dxl_iter->second->ctrl_module_name_); + } + + if (_current_module_msg.joint_name.size() == + _current_module_msg.module_name.size()) + current_module_pub_.publish(_current_module_msg); +} + +void RobotisController::setCtrlModuleThread(std::string ctrl_module) { + // stop module + std::list stop_modules; + + if (ctrl_module == "" || ctrl_module == "none") { + // enqueue all modules in order to stop + for (auto m_it = motion_modules_.begin(); m_it != motion_modules_.end(); + m_it++) { + if ((*m_it)->getModuleEnable() == true) + stop_modules.push_back(*m_it); + } + } else { + for (auto m_it = motion_modules_.begin(); m_it != motion_modules_.end(); + m_it++) { + // if it exist + if ((*m_it)->getModuleName() == ctrl_module) { + // enqueue the module which lost control of joint in order to stop + for (auto &result_it : (*m_it)->result_) { + auto d_it = robot_->dxls_.find(result_it.first); + + if (d_it != robot_->dxls_.end()) { + // enqueue + if (d_it->second->ctrl_module_name_ != ctrl_module) { + for (auto stop_m_it = motion_modules_.begin(); + stop_m_it != motion_modules_.end(); stop_m_it++) { + if (((*stop_m_it)->getModuleName() == + d_it->second->ctrl_module_name_) && + ((*stop_m_it)->getModuleEnable() == true)) { + stop_modules.push_back(*stop_m_it); + } + } + } + } + } + + break; + } + } + } + + // stop the module + stop_modules.unique(); + for (auto stop_m_it = stop_modules.begin(); stop_m_it != stop_modules.end(); + stop_m_it++) { + (*stop_m_it)->stop(); + } + + // wait to stop + for (auto stop_m_it = stop_modules.begin(); stop_m_it != stop_modules.end(); + stop_m_it++) { + while ((*stop_m_it)->isRunning()) + usleep(robot_->getControlCycle() * 1000); + } + + // disable module(s) + for (std::list::iterator _stop_m_it = stop_modules.begin(); + _stop_m_it != stop_modules.end(); _stop_m_it++) { + (*_stop_m_it)->setModuleEnable(false); + } + + // set ctrl module + queue_mutex_.lock(); + + if (DEBUG_PRINT) + ROS_INFO_STREAM("set module : " << ctrl_module); + + // none + if ((ctrl_module == "") || (ctrl_module == "none")) { + // set dxl's control module to "none" + for (auto &d_it : robot_->dxls_) { + Dynamixel *dxl = d_it.second; + dxl->ctrl_module_name_ = "none"; + + if (gazebo_mode_ == true) + continue; + + uint32_t pos_data; + pos_data = dxl->convertRadian2Value(dxl->dxl_state_->goal_position_ + + dxl->dxl_state_->position_offset_ * + offset_ratio_); + + uint8_t sync_write_data[4] = {0}; + sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(pos_data)); + sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(pos_data)); + sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(pos_data)); + sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(pos_data)); + + if (port_to_sync_write_position_[dxl->port_name_] != NULL) + port_to_sync_write_position_[dxl->port_name_]->addParam( + dxl->id_, sync_write_data); + + if (port_to_sync_write_current_[dxl->port_name_] != NULL) + port_to_sync_write_current_[dxl->port_name_]->removeParam(dxl->id_); + if (port_to_sync_write_velocity_[dxl->port_name_] != NULL) + port_to_sync_write_velocity_[dxl->port_name_]->removeParam(dxl->id_); + } + } else { + // check whether the module exist + for (auto m_it = motion_modules_.begin(); m_it != motion_modules_.end(); + m_it++) { + // if it exist + if ((*m_it)->getModuleName() == ctrl_module) { + ControlMode mode = (*m_it)->getControlMode(); + for (auto &result_it : (*m_it)->result_) { + auto d_it = robot_->dxls_.find(result_it.first); + if (d_it != robot_->dxls_.end()) { + Dynamixel *dxl = d_it->second; + dxl->ctrl_module_name_ = ctrl_module; + + if (gazebo_mode_ == true) + continue; + + if (mode == PositionControl) { + uint32_t pos_data; + pos_data = dxl->convertRadian2Value( + dxl->dxl_state_->goal_position_ + + dxl->dxl_state_->position_offset_ * offset_ratio_); + + uint8_t sync_write_data[4] = {0}; + sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(pos_data)); + sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(pos_data)); + sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(pos_data)); + sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(pos_data)); + + if (port_to_sync_write_position_[dxl->port_name_] != NULL) + port_to_sync_write_position_[dxl->port_name_]->addParam( + dxl->id_, sync_write_data); + + if (port_to_sync_write_current_[dxl->port_name_] != NULL) + port_to_sync_write_current_[dxl->port_name_]->removeParam( + dxl->id_); + if (port_to_sync_write_velocity_[dxl->port_name_] != NULL) + port_to_sync_write_velocity_[dxl->port_name_]->removeParam( + dxl->id_); + } else if (mode == VelocityControl) { + uint32_t vel_data = + dxl->convertVelocity2Value(dxl->dxl_state_->goal_velocity_); + uint8_t sync_write_data[4] = {0}; + sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(vel_data)); + sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(vel_data)); + sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(vel_data)); + sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(vel_data)); + + if (port_to_sync_write_velocity_[dxl->port_name_] != NULL) + port_to_sync_write_velocity_[dxl->port_name_]->addParam( + dxl->id_, sync_write_data); + + if (port_to_sync_write_current_[dxl->port_name_] != NULL) + port_to_sync_write_current_[dxl->port_name_]->removeParam( + dxl->id_); + if (port_to_sync_write_position_[dxl->port_name_] != NULL) + port_to_sync_write_position_[dxl->port_name_]->removeParam( + dxl->id_); + } else if (mode == TorqueControl) { + uint32_t curr_data = + dxl->convertTorque2Value(dxl->dxl_state_->goal_torque_); + uint8_t sync_write_data[4] = {0}; + sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(curr_data)); + sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(curr_data)); + sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(curr_data)); + sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(curr_data)); + + if (port_to_sync_write_current_[dxl->port_name_] != NULL) + port_to_sync_write_current_[dxl->port_name_]->addParam( + dxl->id_, sync_write_data); + + if (port_to_sync_write_velocity_[dxl->port_name_] != NULL) + port_to_sync_write_velocity_[dxl->port_name_]->removeParam( + dxl->id_); + if (port_to_sync_write_position_[dxl->port_name_] != NULL) + port_to_sync_write_position_[dxl->port_name_]->removeParam( + dxl->id_); + } + } + } + + break; + } + } + } + + for (auto m_it = motion_modules_.begin(); m_it != motion_modules_.end(); + m_it++) { + // set all used modules -> enable + for (auto &d_it : robot_->dxls_) { + if (d_it.second->ctrl_module_name_ == (*m_it)->getModuleName()) { + (*m_it)->setModuleEnable(true); + break; + } + } + } + + // TODO: set indirect address + // -> check module's control_mode + + queue_mutex_.unlock(); + + // publish current module + humanoid_robot_intelligence_control_system_controller_msgs::JointCtrlModule + current_module_msg; + for (auto &dxl_iter : robot_->dxls_) { + current_module_msg.joint_name.push_back(dxl_iter.first); + current_module_msg.module_name.push_back( + dxl_iter.second->ctrl_module_name_); + } + + if (current_module_msg.joint_name.size() == + current_module_msg.module_name.size()) + current_module_pub_.publish(current_module_msg); +} + +void RobotisController::gazeboJointStatesCallback( + const sensor_msgs::JointState::ConstPtr &msg) { + queue_mutex_.lock(); + + for (unsigned int i = 0; i < msg->name.size(); i++) { + auto d_it = robot_->dxls_.find((std::string)msg->name[i]); + if (d_it != robot_->dxls_.end()) { + d_it->second->dxl_state_->present_position_ = msg->position[i]; + d_it->second->dxl_state_->present_velocity_ = msg->velocity[i]; + d_it->second->dxl_state_->present_torque_ = msg->effort[i]; + } + } + + if (init_pose_loaded_ == false) { + for (auto &it : robot_->dxls_) + it.second->dxl_state_->goal_position_ = + it.second->dxl_state_->present_position_; + init_pose_loaded_ = true; + } + + queue_mutex_.unlock(); +} + +bool RobotisController::isTimerStopped() { + if (this->is_timer_running_) { + if (DEBUG_PRINT == true) + ROS_WARN("Process Timer is running.. STOP the timer first."); + return false; + } + return true; +} + +int RobotisController::ping(const std::string joint_name, uint8_t *error) { + return ping(joint_name, 0, error); +} +int RobotisController::ping(const std::string joint_name, + uint16_t *model_number, uint8_t *error) { + if (isTimerStopped() == false) + return COMM_PORT_BUSY; + + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; + + dynamixel::PacketHandler *pkt_handler = + dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; + + return pkt_handler->ping(port_handler, dxl->id_, model_number, error); +} + +int RobotisController::action(const std::string joint_name) { + if (isTimerStopped() == false) + return COMM_PORT_BUSY; + + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; + + dynamixel::PacketHandler *pkt_handler = + dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; + + return pkt_handler->action(port_handler, dxl->id_); +} +int RobotisController::reboot(const std::string joint_name, uint8_t *error) { + if (isTimerStopped() == false) + return COMM_PORT_BUSY; + + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; + + dynamixel::PacketHandler *pkt_handler = + dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; + + return pkt_handler->reboot(port_handler, dxl->id_, error); +} +int RobotisController::factoryReset(const std::string joint_name, + uint8_t option, uint8_t *error) { + if (isTimerStopped() == false) + return COMM_PORT_BUSY; + + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; + + dynamixel::PacketHandler *pkt_handler = + dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; + + return pkt_handler->factoryReset(port_handler, dxl->id_, option, error); +} + +int RobotisController::read(const std::string joint_name, uint16_t address, + uint16_t length, uint8_t *data, uint8_t *error) { + if (isTimerStopped() == false) + return COMM_PORT_BUSY; + + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; + + dynamixel::PacketHandler *pkt_handler = + dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; + + return pkt_handler->readTxRx(port_handler, dxl->id_, address, length, data, + error); +} + +int RobotisController::readCtrlItem(const std::string joint_name, + const std::string item_name, uint32_t *data, + uint8_t *error) { + if (isTimerStopped() == false) + return COMM_PORT_BUSY; + + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; + + ControlTableItem *item = dxl->ctrl_table_[item_name]; + if (item == NULL) + return COMM_NOT_AVAILABLE; + + dynamixel::PacketHandler *pkt_handler = + dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; + + int result = COMM_NOT_AVAILABLE; + switch (item->data_length_) { + case 1: { + uint8_t read_data = 0; + result = pkt_handler->read1ByteTxRx(port_handler, dxl->id_, item->address_, + &read_data, error); + if (result == COMM_SUCCESS) + *data = read_data; + break; + } + case 2: { + uint16_t read_data = 0; + result = pkt_handler->read2ByteTxRx(port_handler, dxl->id_, item->address_, + &read_data, error); + if (result == COMM_SUCCESS) + *data = read_data; + break; + } + case 4: { + uint32_t read_data = 0; + result = pkt_handler->read4ByteTxRx(port_handler, dxl->id_, item->address_, + &read_data, error); + if (result == COMM_SUCCESS) + *data = read_data; + break; + } + default: + break; + } + return result; +} + +int RobotisController::read1Byte(const std::string joint_name, uint16_t address, + uint8_t *data, uint8_t *error) { + if (isTimerStopped() == false) + return COMM_PORT_BUSY; + + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; + + dynamixel::PacketHandler *pkt_handler = + dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; + + return pkt_handler->read1ByteTxRx(port_handler, dxl->id_, address, data, + error); +} + +int RobotisController::read2Byte(const std::string joint_name, uint16_t address, + uint16_t *data, uint8_t *error) { + if (isTimerStopped() == false) + return COMM_PORT_BUSY; + + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; + + dynamixel::PacketHandler *pkt_handler = + dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; + + return pkt_handler->read2ByteTxRx(port_handler, dxl->id_, address, data, + error); +} + +int RobotisController::read4Byte(const std::string joint_name, uint16_t address, + uint32_t *data, uint8_t *error) { + if (isTimerStopped() == false) + return COMM_PORT_BUSY; + + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; + + dynamixel::PacketHandler *pkt_handler = + dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; + + return pkt_handler->read4ByteTxRx(port_handler, dxl->id_, address, data, + error); +} + +int RobotisController::write(const std::string joint_name, uint16_t address, + uint16_t length, uint8_t *data, uint8_t *error) { + if (isTimerStopped() == false) + return COMM_PORT_BUSY; + + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; + + dynamixel::PacketHandler *pkt_handler = + dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; + + return pkt_handler->writeTxRx(port_handler, dxl->id_, address, length, data, + error); +} + +int RobotisController::writeCtrlItem(const std::string joint_name, + const std::string item_name, uint32_t data, + uint8_t *error) { + if (isTimerStopped() == false) + return COMM_PORT_BUSY; + + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; + + ControlTableItem *item = dxl->ctrl_table_[item_name]; + if (item == NULL) + return COMM_NOT_AVAILABLE; + + dynamixel::PacketHandler *pkt_handler = + dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; + + int result = COMM_NOT_AVAILABLE; + uint8_t *write_data = new uint8_t[item->data_length_]; + if (item->data_length_ == 1) { + write_data[0] = (uint8_t)data; + result = pkt_handler->write1ByteTxRx(port_handler, dxl->id_, item->address_, + data, error); + } else if (item->data_length_ == 2) { + write_data[0] = DXL_LOBYTE((uint16_t)data); + write_data[1] = DXL_HIBYTE((uint16_t)data); + result = pkt_handler->write2ByteTxRx(port_handler, dxl->id_, item->address_, + data, error); + } else if (item->data_length_ == 4) { + write_data[0] = DXL_LOBYTE(DXL_LOWORD((uint32_t)data)); + write_data[1] = DXL_HIBYTE(DXL_LOWORD((uint32_t)data)); + write_data[2] = DXL_LOBYTE(DXL_HIWORD((uint32_t)data)); + write_data[3] = DXL_HIBYTE(DXL_HIWORD((uint32_t)data)); + result = pkt_handler->write4ByteTxRx(port_handler, dxl->id_, item->address_, + data, error); + } + delete[] write_data; + return result; +} + +int RobotisController::write1Byte(const std::string joint_name, + uint16_t address, uint8_t data, + uint8_t *error) { + if (isTimerStopped() == false) + return COMM_PORT_BUSY; + + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; + + dynamixel::PacketHandler *pkt_handler = + dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; + + return pkt_handler->write1ByteTxRx(port_handler, dxl->id_, address, data, + error); +} + +int RobotisController::write2Byte(const std::string joint_name, + uint16_t address, uint16_t data, + uint8_t *error) { + if (isTimerStopped() == false) + return COMM_PORT_BUSY; + + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; + + dynamixel::PacketHandler *pkt_handler = + dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; + + return pkt_handler->write2ByteTxRx(port_handler, dxl->id_, address, data, + error); +} + +int RobotisController::write4Byte(const std::string joint_name, + uint16_t address, uint32_t data, + uint8_t *error) { + if (isTimerStopped() == false) + return COMM_PORT_BUSY; + + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; + + dynamixel::PacketHandler *pkt_handler = + dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; + + return pkt_handler->write4ByteTxRx(port_handler, dxl->id_, address, data, + error); +} + +int RobotisController::regWrite(const std::string joint_name, uint16_t address, + uint16_t length, uint8_t *data, + uint8_t *error) { + if (isTimerStopped() == false) + return COMM_PORT_BUSY; + + Dynamixel *dxl = robot_->dxls_[joint_name]; + if (dxl == NULL) + return COMM_NOT_AVAILABLE; + + dynamixel::PacketHandler *pkt_handler = + dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); + dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; + + return pkt_handler->regWriteTxRx(port_handler, dxl->id_, address, length, + data, error); +} diff --git a/humanoid_robot_intelligence_control_system_device/CHANGELOG.rst b/humanoid_robot_intelligence_control_system_device/CHANGELOG.rst new file mode 100755 index 0000000..10a041c --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/CHANGELOG.rst @@ -0,0 +1,110 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package humanoid_robot_intelligence_control_system_device +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.3.2 (2023-10-03) +------------------ +* Make it compatible for ROS1/ROS2 +* Fix bugs +* Update package.xml and CMakeList.txt for to the latest versions +* Contributors & Maintainer: Ronaldson Bellande + +0.3.1 (2023-09-27) +------------------ +* Starting from this point it under a new license +* Fix errors and Issues +* Rename Repository for a completely different purpose +* Make it compatible with ROS/ROS2 +* Upgrade version of all builds and make it more compatible +* Update package.xml and CMakeList.txt for to the latest versions +* Contributors & Maintainer: Ronaldson Bellande + +0.3.0 (2021-05-03) +------------------ +* Update package.xml and CMakeList.txt for noetic branch +* Contributors: Ronaldson Bellande + +0.2.9 (2018-03-22) +------------------ +* modified to prevent duplicate indirect address write +* fixed a bug that occure when handling bulk read item that does not exist +* Contributors: Zerom + +0.2.8 (2018-03-20) +------------------ +* added RH-P12-RN.device file +* Contributors: Zerom, Pyo + +0.2.7 (2018-03-15) +------------------ +* fixed a bug that occur when handling bulk read item that does not exist +* changed the License and package format to version 2 +* Contributors: SCH, Pyo + +0.2.6 (2017-08-09) +------------------ +* OpenCR control table item name changed. (torque_enable -> dynamixel_power) +* fixed to not update update_time_stamp\_ if bulk read fails. +* Contributors: Zerom + +0.2.5 (2017-06-09) +------------------ +* none + +0.2.4 (2017-06-07) +------------------ +* none + +0.2.3 (2017-05-23) +------------------ +* updated the cmake file for ros install +* Contributors: SCH + +0.2.2 (2017-04-24) +------------------ +* added a deivce: OpenCR +* changed to read control cycle from .robot file +* Contributors: Zerom, Kayman + +0.2.1 (2016-11-23) +------------------ +* Merge the changes and update +* mode change debugging +* - convertRadian2Value / convertValue2Radian : commented out the code that limits the maximum/minimum value. +* - modified dependency problem. +* Contributors: Jay Song, Pyo, Zerom, SCH + +0.2.0 (2016-08-31) +------------------ +* bug fixed (position pid gain & velocity pid gain sync write). +* added velocity_to_value_ratio to DXL Pro-H series. +* added velocity p/i/d gain and position i/d gain sync_write code. +* fixed humanoid_robot_intelligence_control_system_device build_depend. +* added XM-430-W210 / XM-430-W350 device file. +* rename (present_current\_ -> present_torque\_) +* modified torque control code +* added device file for MX-64 / MX-106 +* adjusted position min/max value. (MX-28, XM-430) +* Contributors: Zerom, Pyo + +0.1.1 (2016-08-18) +------------------ +* updated the package information +* Contributors: Zerom + +0.1.0 (2016-08-12) +------------------ +* first public release for Kinetic +* modified the package information for release +* develop branch -> master branch +* Setting the license to BSD. +* add SensorState + add Singleton template +* XM-430 / CM-740 device file added. + Sensor device added. +* modified. +* variable name changed. + ConvertRadian2Value / ConvertValue2Radian function bug fixed. +* added code to support the gazebo simulator +* renewal +* Contributors: Zerom diff --git a/humanoid_robot_intelligence_control_system_device/CMakeLists.txt b/humanoid_robot_intelligence_control_system_device/CMakeLists.txt new file mode 100755 index 0000000..a9c5f39 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/CMakeLists.txt @@ -0,0 +1,51 @@ +cmake_minimum_required(VERSION 3.8) +project(humanoid_robot_intelligence_control_system_device) + + +if($ENV{ROS_VERSION} EQUAL 1) + find_package(catkin REQUIRED COMPONENTS + roscpp + dynamixel_sdk + ) +else() + find_package(ament_cmake REQUIRED) +endif() + + +if($ENV{ROS_VERSION} EQUAL 1) + catkin_package( + INCLUDE_DIRS include + LIBRARIES humanoid_robot_intelligence_control_system_device + CATKIN_DEPENDS + roscpp + dynamixel_sdk + ) +endif() + + +include_directories( + include + ${catkin_INCLUDE_DIRS} +) + +add_library(humanoid_robot_intelligence_control_system_device + src/humanoid_robot_intelligence_control_system_device/robot.cpp + src/humanoid_robot_intelligence_control_system_device/sensor.cpp + src/humanoid_robot_intelligence_control_system_device/dynamixel.cpp +) +add_dependencies(humanoid_robot_intelligence_control_system_device ${catkin_EXPORTED_TARGETS}) +target_link_libraries(humanoid_robot_intelligence_control_system_device ${catkin_LIBRARIES}) + +install(TARGETS humanoid_robot_intelligence_control_system_device + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) + +install(DIRECTORY devices + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) diff --git a/humanoid_robot_intelligence_control_system_device/devices/dynamixel/GRIPPER.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/GRIPPER.device new file mode 100755 index 0000000..7e0846d --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/GRIPPER.device @@ -0,0 +1,73 @@ +[device info] +model_name = GRIPPER +device_type = dynamixel + +[type info] +value_of_0_radian_position = 0 +value_of_min_radian_position = 0 +value_of_max_radian_position = 750 +min_radian = 0 +max_radian = 1.150767 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_torque +position_d_gain_item_name = +position_i_gain_item_name = +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 30 | torque_limit | 2 | RW | EEPROM | 0 | 32767 | N + 32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 562 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 586 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N + 588 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N + 594 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N + 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y + 606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N + 610 | is_moving | 1 | R | RAM | 0 | 1 | N + 611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 621 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 623 | present_voltage | 2 | R | RAM | 0 | 500 | N + 625 | present_temperature | 1 | R | RAM | 0 | 200 | N + 626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 891 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N diff --git a/humanoid_robot_intelligence_control_system_device/devices/dynamixel/GRIPPER_TORQUE.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/GRIPPER_TORQUE.device new file mode 100755 index 0000000..208b7cb --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/GRIPPER_TORQUE.device @@ -0,0 +1,74 @@ +[device info] +model_name = GRIPPER +device_type = dynamixel + +[type info] +value_of_0_radian_position = 0 +value_of_min_radian_position = 0 +value_of_max_radian_position = 750 +min_radian = 0 +max_radian = 1.150767 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_torque +position_d_gain_item_name = +position_i_gain_item_name = +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 30 | torque_limit | 2 | RW | EEPROM | 0 | 32767 | N + 32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 562 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 586 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N + 588 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N + 590 | position_d_gain | 2 | RW | RAM | 0 | 2047 | N + 594 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N + 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y + 606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N + 610 | is_moving | 1 | R | RAM | 0 | 1 | N + 611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 621 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 623 | present_voltage | 2 | R | RAM | 0 | 500 | N + 625 | present_temperature | 1 | R | RAM | 0 | 200 | N + 626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 891 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N diff --git a/humanoid_robot_intelligence_control_system_device/devices/dynamixel/H42-20-S300-R(A).device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/H42-20-S300-R(A).device new file mode 100644 index 0000000..a6f523c --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/H42-20-S300-R(A).device @@ -0,0 +1,92 @@ +[device info] +model_name = H42-20-S300-R(A) +device_type = dynamixel + +[type info] +torque_to_current_value_ratio = 109.3747778 +velocity_to_value_ratio = 954.93 +value_of_0_radian_position = 0 +value_of_min_radian_position = -303750 +value_of_max_radian_position = 303750 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_current +position_d_gain_item_name = position_d_gain +position_i_gain_item_name = position_i_gain +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 10 | drive_mode | 1 | RW | EEPROM | 0 | 3 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 12 | secondary_id | 1 | RW | EEPROM | 0 | 255 | N + 13 | protocol_version | 1 | RW | EEPROM | 1 | 2 | N + 20 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 24 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 31 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 32 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 34 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 36 | pwm_limit | 2 | RW | EEPROM | 0 | 885 | N + 38 | current_limit | 2 | RW | EEPROM | 0 | 32767 | N + 40 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 44 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 48 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 52 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 56 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 57 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 58 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 59 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 63 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 168 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 512 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 513 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 514 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 515 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 516 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 517 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 518 | hardware_error_status | 1 | R | RAM | 0 | 31 | N + 524 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N + 526 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N + 528 | position_d_gain | 2 | RW | RAM | 0 | 2047 | N + 530 | position_i_gain | 2 | RW | RAM | 0 | 2047 | N + 532 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N + 536 | feedforward_2nd_gain | 2 | RW | RAM | 0 | 32767 | N + 538 | feedforward_1st_gain | 2 | RW | RAM | 0 | 32767 | N + 546 | bus_watchdog | 1 | RW | RAM | -1 | 127 | Y + 548 | goal_pwm | 2 | RW | RAM | -2100 | 2100 | Y + 550 | goal_current | 2 | RW | RAM | -32768 | 32767 | Y + 552 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 556 | profile_acceleration | 4 | RW | RAM | 0 | 32767 | N + 560 | profile_velocity | 4 | RW | RAM | 0 | 1023 | N + 564 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 568 | realtime_tick | 2 | R | RAM | 0 | 32767 | N + 570 | moving | 1 | R | RAM | 0 | 1 | N + 571 | moving_status | 1 | R | RAM | 0 | 255 | N + 572 | present_pwm | 2 | R | RAM | -2100 | 2100 | Y + 574 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 576 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 580 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 584 | velocity_trajectory | 4 | R | RAM | 0 | 1023 | N + 588 | position_trajectory | 4 | R | RAM | 0 | 4095 | N + 592 | present_voltage | 2 | R | RAM | 0 | 500 | N + 594 | present_temperature | 1 | R | RAM | 0 | 200 | N + 600 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 602 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 604 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 606 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N diff --git a/humanoid_robot_intelligence_control_system_device/devices/dynamixel/H42-20-S300-R.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/H42-20-S300-R.device new file mode 100755 index 0000000..ab7aecb --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/H42-20-S300-R.device @@ -0,0 +1,76 @@ +[device info] +model_name = H42-20-S300-R +device_type = dynamixel + +[type info] +torque_to_current_value_ratio = 27.15146 +velocity_to_value_ratio = 2900.59884 +value_of_0_radian_position = 0 +value_of_min_radian_position = -151900 +value_of_max_radian_position = 151900 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_torque +position_d_gain_item_name = +position_i_gain_item_name = +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 13 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 30 | torque_limit | 2 | RW | EEPROM | 0 | 32767 | N + 32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 562 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 586 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N + 588 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N + 594 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N + 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y + 606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N + 610 | is_moving | 1 | R | RAM | 0 | 1 | N + 611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 621 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 623 | present_voltage | 2 | R | RAM | 0 | 500 | N + 625 | present_temperature | 1 | R | RAM | 0 | 200 | N + 626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 891 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N diff --git a/humanoid_robot_intelligence_control_system_device/devices/dynamixel/H42P-020-S300-R.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/H42P-020-S300-R.device new file mode 100644 index 0000000..92d405a --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/H42P-020-S300-R.device @@ -0,0 +1,92 @@ +[device info] +model_name = H42P-020-S300-R +device_type = dynamixel + +[type info] +torque_to_current_value_ratio = 109.3747778 +velocity_to_value_ratio = 954.93 +value_of_0_radian_position = 0 +value_of_min_radian_position = -303750 +value_of_max_radian_position = 303750 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_current +position_d_gain_item_name = position_d_gain +position_i_gain_item_name = position_i_gain +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 10 | drive_mode | 1 | RW | EEPROM | 0 | 3 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 12 | secondary_id | 1 | RW | EEPROM | 0 | 255 | N + 13 | protocol_version | 1 | RW | EEPROM | 1 | 2 | N + 20 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 24 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 31 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 32 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 34 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 36 | pwm_limit | 2 | RW | EEPROM | 0 | 885 | N + 38 | current_limit | 2 | RW | EEPROM | 0 | 32767 | N + 40 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 44 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 48 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 52 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 56 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 57 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 58 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 59 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 63 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 168 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 512 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 513 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 514 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 515 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 516 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 517 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 518 | hardware_error_status | 1 | R | RAM | 0 | 31 | N + 524 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N + 526 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N + 528 | position_d_gain | 2 | RW | RAM | 0 | 2047 | N + 530 | position_i_gain | 2 | RW | RAM | 0 | 2047 | N + 532 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N + 536 | feedforward_2nd_gain | 2 | RW | RAM | 0 | 32767 | N + 538 | feedforward_1st_gain | 2 | RW | RAM | 0 | 32767 | N + 546 | bus_watchdog | 1 | RW | RAM | -1 | 127 | Y + 548 | goal_pwm | 2 | RW | RAM | -2100 | 2100 | Y + 550 | goal_current | 2 | RW | RAM | -32768 | 32767 | Y + 552 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 556 | profile_acceleration | 4 | RW | RAM | 0 | 32767 | N + 560 | profile_velocity | 4 | RW | RAM | 0 | 1023 | N + 564 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 568 | realtime_tick | 2 | R | RAM | 0 | 32767 | N + 570 | moving | 1 | R | RAM | 0 | 1 | N + 571 | moving_status | 1 | R | RAM | 0 | 255 | N + 572 | present_pwm | 2 | R | RAM | -2100 | 2100 | Y + 574 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 576 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 580 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 584 | velocity_trajectory | 4 | R | RAM | 0 | 1023 | N + 588 | position_trajectory | 4 | R | RAM | 0 | 4095 | N + 592 | present_voltage | 2 | R | RAM | 0 | 500 | N + 594 | present_temperature | 1 | R | RAM | 0 | 200 | N + 600 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 602 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 604 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 606 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N diff --git a/humanoid_robot_intelligence_control_system_device/devices/dynamixel/H54-100-B210-R-NR.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/H54-100-B210-R-NR.device new file mode 100755 index 0000000..42342ae --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/H54-100-B210-R-NR.device @@ -0,0 +1,76 @@ +[device info] +model_name = H54-100-B210-R-NR +device_type = dynamixel + +[type info] +torque_to_current_value_ratio = 9.09201 +velocity_to_value_ratio = 2046.2777 +value_of_0_radian_position = 0 +value_of_min_radian_position = -250950 +value_of_max_radian_position = 250950 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_torque +position_d_gain_item_name = +position_i_gain_item_name = +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 13 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 30 | torque_limit | 2 | RW | EEPROM | 0 | 32767 | N + 32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 562 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 586 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N + 588 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N + 594 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N + 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y + 606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N + 610 | is_moving | 1 | R | RAM | 0 | 1 | N + 611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 621 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 623 | present_voltage | 2 | R | RAM | 0 | 500 | N + 625 | present_temperature | 1 | R | RAM | 0 | 200 | N + 626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 891 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N diff --git a/humanoid_robot_intelligence_control_system_device/devices/dynamixel/H54-100-S500-R(A).device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/H54-100-S500-R(A).device new file mode 100644 index 0000000..fe706cc --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/H54-100-S500-R(A).device @@ -0,0 +1,92 @@ +[device info] +model_name = H54-100-S500-R(A) +device_type = dynamixel + +[type info] +torque_to_current_value_ratio = 155.658486 +velocity_to_value_ratio = 954.93 +value_of_0_radian_position = 0 +value_of_min_radian_position = -501923 +value_of_max_radian_position = 501923 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_current +position_d_gain_item_name = position_d_gain +position_i_gain_item_name = position_i_gain +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 10 | drive_mode | 1 | RW | EEPROM | 0 | 3 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 12 | secondary_id | 1 | RW | EEPROM | 0 | 255 | N + 13 | protocol_version | 1 | RW | EEPROM | 1 | 2 | N + 20 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 24 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 31 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 32 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 34 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 36 | pwm_limit | 2 | RW | EEPROM | 0 | 885 | N + 38 | current_limit | 2 | RW | EEPROM | 0 | 32767 | N + 40 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 44 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 48 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 52 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 56 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 57 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 58 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 59 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 63 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 168 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 512 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 513 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 514 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 515 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 516 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 517 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 518 | hardware_error_status | 1 | R | RAM | 0 | 31 | N + 524 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N + 526 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N + 528 | position_d_gain | 2 | RW | RAM | 0 | 2047 | N + 530 | position_i_gain | 2 | RW | RAM | 0 | 2047 | N + 532 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N + 536 | feedforward_2nd_gain | 2 | RW | RAM | 0 | 32767 | N + 538 | feedforward_1st_gain | 2 | RW | RAM | 0 | 32767 | N + 546 | bus_watchdog | 1 | RW | RAM | -1 | 127 | Y + 548 | goal_pwm | 2 | RW | RAM | -2100 | 2100 | Y + 550 | goal_current | 2 | RW | RAM | -32768 | 32767 | Y + 552 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 556 | profile_acceleration | 4 | RW | RAM | 0 | 32767 | N + 560 | profile_velocity | 4 | RW | RAM | 0 | 1023 | N + 564 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 568 | realtime_tick | 2 | R | RAM | 0 | 32767 | N + 570 | moving | 1 | R | RAM | 0 | 1 | N + 571 | moving_status | 1 | R | RAM | 0 | 255 | N + 572 | present_pwm | 2 | R | RAM | -2100 | 2100 | Y + 574 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 576 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 580 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 584 | velocity_trajectory | 4 | R | RAM | 0 | 1023 | N + 588 | position_trajectory | 4 | R | RAM | 0 | 4095 | N + 592 | present_voltage | 2 | R | RAM | 0 | 500 | N + 594 | present_temperature | 1 | R | RAM | 0 | 200 | N + 600 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 602 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 604 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 606 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N diff --git a/humanoid_robot_intelligence_control_system_device/devices/dynamixel/H54-100-S500-R.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/H54-100-S500-R.device new file mode 100755 index 0000000..b042f27 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/H54-100-S500-R.device @@ -0,0 +1,76 @@ +[device info] +model_name = H54-100-S500-R +device_type = dynamixel + +[type info] +torque_to_current_value_ratio = 9.66026 +velocity_to_value_ratio = 4793.01226 +value_of_0_radian_position = 0 +value_of_min_radian_position = -250950 +value_of_max_radian_position = 250950 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_torque +position_d_gain_item_name = +position_i_gain_item_name = +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 13 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 30 | torque_limit | 2 | RW | EEPROM | 0 | 32767 | N + 32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 562 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 586 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N + 588 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N + 594 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N + 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y + 606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N + 610 | is_moving | 1 | R | RAM | 0 | 1 | N + 611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 621 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 623 | present_voltage | 2 | R | RAM | 0 | 500 | N + 625 | present_temperature | 1 | R | RAM | 0 | 200 | N + 626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 891 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N diff --git a/humanoid_robot_intelligence_control_system_device/devices/dynamixel/H54-200-B500-R.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/H54-200-B500-R.device new file mode 100755 index 0000000..ca52162 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/H54-200-B500-R.device @@ -0,0 +1,76 @@ +[device info] +model_name = H54-200-B500-R +device_type = dynamixel + +[type info] +torque_to_current_value_ratio = 9.09201 +velocity_to_value_ratio = 4793.01226 +value_of_0_radian_position = 0 +value_of_min_radian_position = -250950 +value_of_max_radian_position = 250950 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_torque +position_d_gain_item_name = +position_i_gain_item_name = +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 13 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 30 | torque_limit | 2 | RW | EEPROM | 0 | 32767 | N + 32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 562 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 586 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N + 588 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N + 594 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N + 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y + 606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N + 610 | is_moving | 1 | R | RAM | 0 | 1 | N + 611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 621 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 623 | present_voltage | 2 | R | RAM | 0 | 500 | N + 625 | present_temperature | 1 | R | RAM | 0 | 200 | N + 626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 891 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N diff --git a/humanoid_robot_intelligence_control_system_device/devices/dynamixel/H54-200-S500-R(A).device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/H54-200-S500-R(A).device new file mode 100644 index 0000000..3cfb758 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/H54-200-S500-R(A).device @@ -0,0 +1,92 @@ +[device info] +model_name = H54-200-S500-R(A) +device_type = dynamixel + +[type info] +torque_to_current_value_ratio = 146.502114 +velocity_to_value_ratio = 954.93 +value_of_0_radian_position = 0 +value_of_min_radian_position = -501923 +value_of_max_radian_position = 501923 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_current +position_d_gain_item_name = position_d_gain +position_i_gain_item_name = position_i_gain +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 10 | drive_mode | 1 | RW | EEPROM | 0 | 3 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 12 | secondary_id | 1 | RW | EEPROM | 0 | 255 | N + 13 | protocol_version | 1 | RW | EEPROM | 1 | 2 | N + 20 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 24 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 31 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 32 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 34 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 36 | pwm_limit | 2 | RW | EEPROM | 0 | 885 | N + 38 | current_limit | 2 | RW | EEPROM | 0 | 32767 | N + 40 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 44 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 48 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 52 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 56 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 57 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 58 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 59 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 63 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 168 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 512 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 513 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 514 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 515 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 516 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 517 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 518 | hardware_error_status | 1 | R | RAM | 0 | 31 | N + 524 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N + 526 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N + 528 | position_d_gain | 2 | RW | RAM | 0 | 2047 | N + 530 | position_i_gain | 2 | RW | RAM | 0 | 2047 | N + 532 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N + 536 | feedforward_2nd_gain | 2 | RW | RAM | 0 | 32767 | N + 538 | feedforward_1st_gain | 2 | RW | RAM | 0 | 32767 | N + 546 | bus_watchdog | 1 | RW | RAM | -1 | 127 | Y + 548 | goal_pwm | 2 | RW | RAM | -2100 | 2100 | Y + 550 | goal_current | 2 | RW | RAM | -32768 | 32767 | Y + 552 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 556 | profile_acceleration | 4 | RW | RAM | 0 | 32767 | N + 560 | profile_velocity | 4 | RW | RAM | 0 | 1023 | N + 564 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 568 | realtime_tick | 2 | R | RAM | 0 | 32767 | N + 570 | moving | 1 | R | RAM | 0 | 1 | N + 571 | moving_status | 1 | R | RAM | 0 | 255 | N + 572 | present_pwm | 2 | R | RAM | -2100 | 2100 | Y + 574 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 576 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 580 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 584 | velocity_trajectory | 4 | R | RAM | 0 | 1023 | N + 588 | position_trajectory | 4 | R | RAM | 0 | 4095 | N + 592 | present_voltage | 2 | R | RAM | 0 | 500 | N + 594 | present_temperature | 1 | R | RAM | 0 | 200 | N + 600 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 602 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 604 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 606 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N diff --git a/humanoid_robot_intelligence_control_system_device/devices/dynamixel/H54-200-S500-R.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/H54-200-S500-R.device new file mode 100755 index 0000000..4f81222 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/H54-200-S500-R.device @@ -0,0 +1,76 @@ +[device info] +model_name = H54-200-S500-R +device_type = dynamixel + +[type info] +torque_to_current_value_ratio = 9.09201 +velocity_to_value_ratio = 4793.01226 +value_of_0_radian_position = 0 +value_of_min_radian_position = -250950 +value_of_max_radian_position = 250950 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_torque +position_d_gain_item_name = +position_i_gain_item_name = +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 13 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 30 | torque_limit | 2 | RW | EEPROM | 0 | 32767 | N + 32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 562 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 586 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N + 588 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N + 594 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N + 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y + 606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N + 610 | is_moving | 1 | R | RAM | 0 | 1 | N + 611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 621 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 623 | present_voltage | 2 | R | RAM | 0 | 500 | N + 625 | present_temperature | 1 | R | RAM | 0 | 200 | N + 626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 891 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N diff --git a/humanoid_robot_intelligence_control_system_device/devices/dynamixel/H54P-100-S500-R.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/H54P-100-S500-R.device new file mode 100644 index 0000000..e9661e0 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/H54P-100-S500-R.device @@ -0,0 +1,92 @@ +[device info] +model_name = H54P-100-S500-R +device_type = dynamixel + +[type info] +torque_to_current_value_ratio = 155.658486 +velocity_to_value_ratio = 954.93 +value_of_0_radian_position = 0 +value_of_min_radian_position = -501923 +value_of_max_radian_position = 501923 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_current +position_d_gain_item_name = position_d_gain +position_i_gain_item_name = position_i_gain +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 10 | drive_mode | 1 | RW | EEPROM | 0 | 3 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 12 | secondary_id | 1 | RW | EEPROM | 0 | 255 | N + 13 | protocol_version | 1 | RW | EEPROM | 1 | 2 | N + 20 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 24 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 31 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 32 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 34 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 36 | pwm_limit | 2 | RW | EEPROM | 0 | 885 | N + 38 | current_limit | 2 | RW | EEPROM | 0 | 32767 | N + 40 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 44 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 48 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 52 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 56 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 57 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 58 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 59 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 63 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 168 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 512 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 513 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 514 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 515 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 516 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 517 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 518 | hardware_error_status | 1 | R | RAM | 0 | 31 | N + 524 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N + 526 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N + 528 | position_d_gain | 2 | RW | RAM | 0 | 2047 | N + 530 | position_i_gain | 2 | RW | RAM | 0 | 2047 | N + 532 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N + 536 | feedforward_2nd_gain | 2 | RW | RAM | 0 | 32767 | N + 538 | feedforward_1st_gain | 2 | RW | RAM | 0 | 32767 | N + 546 | bus_watchdog | 1 | RW | RAM | -1 | 127 | Y + 548 | goal_pwm | 2 | RW | RAM | -2100 | 2100 | Y + 550 | goal_current | 2 | RW | RAM | -32768 | 32767 | Y + 552 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 556 | profile_acceleration | 4 | RW | RAM | 0 | 32767 | N + 560 | profile_velocity | 4 | RW | RAM | 0 | 1023 | N + 564 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 568 | realtime_tick | 2 | R | RAM | 0 | 32767 | N + 570 | moving | 1 | R | RAM | 0 | 1 | N + 571 | moving_status | 1 | R | RAM | 0 | 255 | N + 572 | present_pwm | 2 | R | RAM | -2100 | 2100 | Y + 574 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 576 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 580 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 584 | velocity_trajectory | 4 | R | RAM | 0 | 1023 | N + 588 | position_trajectory | 4 | R | RAM | 0 | 4095 | N + 592 | present_voltage | 2 | R | RAM | 0 | 500 | N + 594 | present_temperature | 1 | R | RAM | 0 | 200 | N + 600 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 602 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 604 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 606 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N diff --git a/humanoid_robot_intelligence_control_system_device/devices/dynamixel/H54P-200-B500-R.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/H54P-200-B500-R.device new file mode 100644 index 0000000..97f44c5 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/H54P-200-B500-R.device @@ -0,0 +1,92 @@ +[device info] +model_name = H54P-200-B500-R +device_type = dynamixel + +[type info] +torque_to_current_value_ratio = 146.502114 +velocity_to_value_ratio = 954.93 +value_of_0_radian_position = 0 +value_of_min_radian_position = -501923 +value_of_max_radian_position = 501923 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_current +position_d_gain_item_name = position_d_gain +position_i_gain_item_name = position_i_gain +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 10 | drive_mode | 1 | RW | EEPROM | 0 | 3 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 12 | secondary_id | 1 | RW | EEPROM | 0 | 255 | N + 13 | protocol_version | 1 | RW | EEPROM | 1 | 2 | N + 20 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 24 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 31 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 32 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 34 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 36 | pwm_limit | 2 | RW | EEPROM | 0 | 885 | N + 38 | current_limit | 2 | RW | EEPROM | 0 | 32767 | N + 40 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 44 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 48 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 52 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 56 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 57 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 58 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 59 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 63 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 168 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 512 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 513 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 514 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 515 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 516 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 517 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 518 | hardware_error_status | 1 | R | RAM | 0 | 31 | N + 524 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N + 526 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N + 528 | position_d_gain | 2 | RW | RAM | 0 | 2047 | N + 530 | position_i_gain | 2 | RW | RAM | 0 | 2047 | N + 532 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N + 536 | feedforward_2nd_gain | 2 | RW | RAM | 0 | 32767 | N + 538 | feedforward_1st_gain | 2 | RW | RAM | 0 | 32767 | N + 546 | bus_watchdog | 1 | RW | RAM | -1 | 127 | Y + 548 | goal_pwm | 2 | RW | RAM | -2100 | 2100 | Y + 550 | goal_current | 2 | RW | RAM | -32768 | 32767 | Y + 552 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 556 | profile_acceleration | 4 | RW | RAM | 0 | 32767 | N + 560 | profile_velocity | 4 | RW | RAM | 0 | 1023 | N + 564 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 568 | realtime_tick | 2 | R | RAM | 0 | 32767 | N + 570 | moving | 1 | R | RAM | 0 | 1 | N + 571 | moving_status | 1 | R | RAM | 0 | 255 | N + 572 | present_pwm | 2 | R | RAM | -2100 | 2100 | Y + 574 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 576 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 580 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 584 | velocity_trajectory | 4 | R | RAM | 0 | 1023 | N + 588 | position_trajectory | 4 | R | RAM | 0 | 4095 | N + 592 | present_voltage | 2 | R | RAM | 0 | 500 | N + 594 | present_temperature | 1 | R | RAM | 0 | 200 | N + 600 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 602 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 604 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 606 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N diff --git a/humanoid_robot_intelligence_control_system_device/devices/dynamixel/H54P-200-S500-R.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/H54P-200-S500-R.device new file mode 100644 index 0000000..c58460e --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/H54P-200-S500-R.device @@ -0,0 +1,92 @@ +[device info] +model_name = H54P-200-S500-R +device_type = dynamixel + +[type info] +torque_to_current_value_ratio = 146.502114 +velocity_to_value_ratio = 954.93 +value_of_0_radian_position = 0 +value_of_min_radian_position = -501923 +value_of_max_radian_position = 501923 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_current +position_d_gain_item_name = position_d_gain +position_i_gain_item_name = position_i_gain +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 10 | drive_mode | 1 | RW | EEPROM | 0 | 3 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 12 | secondary_id | 1 | RW | EEPROM | 0 | 255 | N + 13 | protocol_version | 1 | RW | EEPROM | 1 | 2 | N + 20 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 24 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 31 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 32 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 34 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 36 | pwm_limit | 2 | RW | EEPROM | 0 | 885 | N + 38 | current_limit | 2 | RW | EEPROM | 0 | 32767 | N + 40 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 44 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 48 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 52 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 56 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 57 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 58 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 59 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 63 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 168 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 512 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 513 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 514 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 515 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 516 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 517 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 518 | hardware_error_status | 1 | R | RAM | 0 | 31 | N + 524 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N + 526 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N + 528 | position_d_gain | 2 | RW | RAM | 0 | 2047 | N + 530 | position_i_gain | 2 | RW | RAM | 0 | 2047 | N + 532 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N + 536 | feedforward_2nd_gain | 2 | RW | RAM | 0 | 32767 | N + 538 | feedforward_1st_gain | 2 | RW | RAM | 0 | 32767 | N + 546 | bus_watchdog | 1 | RW | RAM | -1 | 127 | Y + 548 | goal_pwm | 2 | RW | RAM | -2100 | 2100 | Y + 550 | goal_current | 2 | RW | RAM | -32768 | 32767 | Y + 552 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 556 | profile_acceleration | 4 | RW | RAM | 0 | 32767 | N + 560 | profile_velocity | 4 | RW | RAM | 0 | 1023 | N + 564 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 568 | realtime_tick | 2 | R | RAM | 0 | 32767 | N + 570 | moving | 1 | R | RAM | 0 | 1 | N + 571 | moving_status | 1 | R | RAM | 0 | 255 | N + 572 | present_pwm | 2 | R | RAM | -2100 | 2100 | Y + 574 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 576 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 580 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 584 | velocity_trajectory | 4 | R | RAM | 0 | 1023 | N + 588 | position_trajectory | 4 | R | RAM | 0 | 4095 | N + 592 | present_voltage | 2 | R | RAM | 0 | 500 | N + 594 | present_temperature | 1 | R | RAM | 0 | 200 | N + 600 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 602 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 604 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 606 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N diff --git a/humanoid_robot_intelligence_control_system_device/devices/dynamixel/L42-10-S300-R.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/L42-10-S300-R.device new file mode 100755 index 0000000..8d861f1 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/L42-10-S300-R.device @@ -0,0 +1,73 @@ +[device info] +model_name = L42-10-S300-R +device_type = dynamixel + +[type info] +value_of_0_radian_position = 0 +value_of_min_radian_position = -2047 +value_of_max_radian_position = 2048 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_torque +position_d_gain_item_name = +position_i_gain_item_name = +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 30 | torque_limit | 2 | RW | EEPROM | 0 | 32767 | N + 32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 562 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 586 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N + 588 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N + 594 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N + 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y + 606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N + 610 | is_moving | 1 | R | RAM | 0 | 1 | N + 611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 621 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 623 | present_voltage | 2 | R | RAM | 0 | 500 | N + 625 | present_temperature | 1 | R | RAM | 0 | 200 | N + 626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 891 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N diff --git a/humanoid_robot_intelligence_control_system_device/devices/dynamixel/L54-30-S400-R.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/L54-30-S400-R.device new file mode 100755 index 0000000..71fa09d --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/L54-30-S400-R.device @@ -0,0 +1,74 @@ +[device info] +model_name = L54-30-S400-R +device_type = dynamixel + +[type info] +value_of_0_radian_position = 0 +value_of_min_radian_position = -144198 +value_of_max_radian_position = 144198 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_torque +position_d_gain_item_name = +position_i_gain_item_name = +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 13 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 30 | torque_limit | 2 | RW | EEPROM | 0 | 32767 | N + 32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 562 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 586 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N + 588 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N + 594 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N + 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y + 606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N + 610 | is_moving | 1 | R | RAM | 0 | 1 | N + 611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 621 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 623 | present_voltage | 2 | R | RAM | 0 | 500 | N + 625 | present_temperature | 1 | R | RAM | 0 | 200 | N + 626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 891 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N diff --git a/humanoid_robot_intelligence_control_system_device/devices/dynamixel/L54-30-S500-R.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/L54-30-S500-R.device new file mode 100755 index 0000000..5a8add7 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/L54-30-S500-R.device @@ -0,0 +1,74 @@ +[device info] +model_name = L54-30-S500-R +device_type = dynamixel + +[type info] +value_of_0_radian_position = 0 +value_of_min_radian_position = -180684 +value_of_max_radian_position = 180684 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_torque +position_d_gain_item_name = +position_i_gain_item_name = +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 13 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 30 | torque_limit | 2 | RW | EEPROM | 0 | 32767 | N + 32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 562 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 586 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N + 588 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N + 594 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N + 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y + 606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N + 610 | is_moving | 1 | R | RAM | 0 | 1 | N + 611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 621 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 623 | present_voltage | 2 | R | RAM | 0 | 500 | N + 625 | present_temperature | 1 | R | RAM | 0 | 200 | N + 626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 891 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N diff --git a/humanoid_robot_intelligence_control_system_device/devices/dynamixel/L54-50-S290-R.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/L54-50-S290-R.device new file mode 100755 index 0000000..7af9c22 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/L54-50-S290-R.device @@ -0,0 +1,74 @@ +[device info] +model_name = L54-50-S290-R +device_type = dynamixel + +[type info] +value_of_0_radian_position = 0 +value_of_min_radian_position = -103860 +value_of_max_radian_position = 103860 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_torque +position_d_gain_item_name = +position_i_gain_item_name = +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 13 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 30 | torque_limit | 2 | RW | EEPROM | 0 | 32767 | N + 32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 562 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 586 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N + 588 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N + 594 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N + 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y + 606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N + 610 | is_moving | 1 | R | RAM | 0 | 1 | N + 611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 621 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 623 | present_voltage | 2 | R | RAM | 0 | 500 | N + 625 | present_temperature | 1 | R | RAM | 0 | 200 | N + 626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 891 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N diff --git a/humanoid_robot_intelligence_control_system_device/devices/dynamixel/L54-50-S500-R.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/L54-50-S500-R.device new file mode 100755 index 0000000..11d63b3 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/L54-50-S500-R.device @@ -0,0 +1,74 @@ +[device info] +model_name = L54-50-S500-R +device_type = dynamixel + +[type info] +value_of_0_radian_position = 0 +value_of_min_radian_position = -180684 +value_of_max_radian_position = 180684 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_torque +position_d_gain_item_name = +position_i_gain_item_name = +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 13 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 30 | torque_limit | 2 | RW | EEPROM | 0 | 32767 | N + 32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 562 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 586 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N + 588 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N + 594 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N + 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y + 606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N + 610 | is_moving | 1 | R | RAM | 0 | 1 | N + 611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 621 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 623 | present_voltage | 2 | R | RAM | 0 | 500 | N + 625 | present_temperature | 1 | R | RAM | 0 | 200 | N + 626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 891 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N diff --git a/humanoid_robot_intelligence_control_system_device/devices/dynamixel/M42-10-S260-R.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/M42-10-S260-R.device new file mode 100755 index 0000000..ba205bb --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/M42-10-S260-R.device @@ -0,0 +1,74 @@ +[device info] +model_name = M42-10-S260-R +device_type = dynamixel + +[type info] +value_of_0_radian_position = 0 +value_of_min_radian_position = -131584 +value_of_max_radian_position = 131584 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_torque +position_d_gain_item_name = +position_i_gain_item_name = +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 13 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 30 | torque_limit | 2 | RW | EEPROM | 0 | 32767 | N + 32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 562 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 586 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N + 588 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N + 594 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N + 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y + 606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N + 610 | is_moving | 1 | R | RAM | 0 | 1 | N + 611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 621 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 623 | present_voltage | 2 | R | RAM | 0 | 500 | N + 625 | present_temperature | 1 | R | RAM | 0 | 200 | N + 626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 891 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N diff --git a/humanoid_robot_intelligence_control_system_device/devices/dynamixel/M54-40-S250-R.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/M54-40-S250-R.device new file mode 100755 index 0000000..62f9990 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/M54-40-S250-R.device @@ -0,0 +1,74 @@ +[device info] +model_name = M54-40-S250-R +device_type = dynamixel + +[type info] +value_of_0_radian_position = 0 +value_of_min_radian_position = -125700 +value_of_max_radian_position = 125700 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_torque +position_d_gain_item_name = +position_i_gain_item_name = +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 13 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 30 | torque_limit | 2 | RW | EEPROM | 0 | 32767 | N + 32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 562 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 586 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N + 588 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N + 594 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N + 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y + 606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N + 610 | is_moving | 1 | R | RAM | 0 | 1 | N + 611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 621 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 623 | present_voltage | 2 | R | RAM | 0 | 500 | N + 625 | present_temperature | 1 | R | RAM | 0 | 200 | N + 626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 891 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N diff --git a/humanoid_robot_intelligence_control_system_device/devices/dynamixel/M54-60-S250-R.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/M54-60-S250-R.device new file mode 100755 index 0000000..dd2efde --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/M54-60-S250-R.device @@ -0,0 +1,74 @@ +[device info] +model_name = M54-60-S250-R +device_type = dynamixel + +[type info] +value_of_0_radian_position = 0 +value_of_min_radian_position = -125700 +value_of_max_radian_position = 125700 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_torque +position_d_gain_item_name = +position_i_gain_item_name = +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 13 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 30 | torque_limit | 2 | RW | EEPROM | 0 | 32767 | N + 32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 562 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 586 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N + 588 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N + 594 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N + 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y + 606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N + 610 | is_moving | 1 | R | RAM | 0 | 1 | N + 611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 621 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 623 | present_voltage | 2 | R | RAM | 0 | 500 | N + 625 | present_temperature | 1 | R | RAM | 0 | 200 | N + 626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 891 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N diff --git a/humanoid_robot_intelligence_control_system_device/devices/dynamixel/MX-106.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/MX-106.device new file mode 100755 index 0000000..a122831 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/MX-106.device @@ -0,0 +1,66 @@ +[device info] +model_name = MX-106 +device_type = dynamixel + +[type info] +value_of_0_radian_position = 2048 +value_of_min_radian_position = 0 +value_of_max_radian_position = 4095 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = +position_d_gain_item_name = position_d_gain +position_i_gain_item_name = position_i_gain +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = +velocity_p_gain_item_name = + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 2 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 3 | ID | 1 | RW | EEPROM | 0 | 252 | N + 4 | baudrate | 1 | RW | EEPROM | 0 | 252 | N + 5 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 6 | CW_angle_limit | 2 | RW | EEPROM | 0 | 4095 | N + 8 | CCW_angle_limit | 2 | RW | EEPROM | 0 | 4095 | N + 10 | drive_mode | 1 | RW | EEPROM | 0 | 3 | N + 11 | max_temperature_limit | 1 | RW | EEPROM | 0 | 99 | N + 12 | min_voltage_limit | 1 | RW | EEPROM | 0 | 250 | N + 13 | max_voltage_limit | 1 | RW | EEPROM | 0 | 250 | N + 14 | max_torque | 2 | RW | EEPROM | 0 | 1023 | N + 16 | status_return_level | 1 | RW | EEPROM | 0 | 2 | N + 17 | alarm_LED | 1 | RW | EEPROM | 0 | 127 | N + 18 | alarm_shutdown | 1 | RW | EEPROM | 0 | 127 | N + 20 | multi_turn_offset | 2 | RW | EEPROM | -26624 | 26624 | Y + 22 | resolution_dividor | 1 | RW | EEPROM | 1 | 255 | N + 24 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 25 | LED | 1 | RW | RAM | 0 | 1 | N + 26 | position_d_gain | 1 | RW | RAM | 0 | 254 | N + 27 | position_i_gain | 1 | RW | RAM | 0 | 254 | N + 28 | position_p_gain | 1 | RW | RAM | 0 | 254 | N + 30 | goal_position | 2 | RW | RAM | -28672 | 28672 | Y + 32 | goal_velocity | 2 | RW | RAM | 0 | 1023 | N + 34 | goal_torque | 2 | RW | RAM | 0 | 1023 | N + 36 | present_position | 2 | R | RAM | -32768 | 32767 | Y + 38 | present_velocity | 2 | R | RAM | 0 | 2048 | N + 40 | present_load | 2 | R | RAM | 0 | 2048 | N + 42 | present_voltage | 1 | R | RAM | 50 | 250 | N + 43 | present_temperature | 1 | R | RAM | 0 | 99 | N + 44 | registered_instruction | 1 | R | RAM | 0 | 1 | N + 46 | is_moving | 1 | R | RAM | 0 | 1 | N + 47 | EEPROM_lock | 1 | RW | RAM | 0 | 1 | N + 48 | punch | 2 | RW | RAM | 0 | 1023 | N + 68 | current_consumption | 2 | RW | RAM | 0 | 4095 | N + 70 | torque_control_mode | 1 | RW | RAM | 0 | 1 | N + 71 | torque_control_goal | 2 | RW | RAM | 0 | 2047 | N + 73 | goal_acceleration | 1 | RW | RAM | 0 | 254 | N + diff --git a/humanoid_robot_intelligence_control_system_device/devices/dynamixel/MX-28.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/MX-28.device new file mode 100755 index 0000000..ae1f877 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/MX-28.device @@ -0,0 +1,62 @@ +[device info] +model_name = MX-28 +device_type = dynamixel + +[type info] +value_of_0_radian_position = 2048 +value_of_min_radian_position = 0 +value_of_max_radian_position = 4095 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = +position_d_gain_item_name = position_d_gain +position_i_gain_item_name = position_i_gain +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = +velocity_p_gain_item_name = + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 2 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 3 | ID | 1 | RW | EEPROM | 0 | 252 | N + 4 | baudrate | 1 | RW | EEPROM | 0 | 252 | N + 5 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 6 | CW_angle_limit | 2 | RW | EEPROM | 0 | 4095 | N + 8 | CCW_angle_limit | 2 | RW | EEPROM | 0 | 4095 | N + 10 | drive_mode | 1 | RW | EEPROM | 0 | 3 | N + 11 | max_temperature_limit | 1 | RW | EEPROM | 0 | 99 | N + 12 | min_voltage_limit | 1 | RW | EEPROM | 0 | 250 | N + 13 | max_voltage_limit | 1 | RW | EEPROM | 0 | 250 | N + 14 | max_torque | 2 | RW | EEPROM | 0 | 1023 | N + 16 | status_return_level | 1 | RW | EEPROM | 0 | 2 | N + 17 | alarm_LED | 1 | RW | EEPROM | 0 | 127 | N + 18 | alarm_shutdown | 1 | RW | EEPROM | 0 | 127 | N + 20 | multi_turn_offset | 2 | RW | EEPROM | -26624 | 26624 | Y + 22 | resolution_dividor | 1 | RW | EEPROM | 1 | 255 | N + 24 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 25 | LED | 1 | RW | RAM | 0 | 1 | N + 26 | position_d_gain | 1 | RW | RAM | 0 | 254 | N + 27 | position_i_gain | 1 | RW | RAM | 0 | 254 | N + 28 | position_p_gain | 1 | RW | RAM | 0 | 254 | N + 30 | goal_position | 2 | RW | RAM | -28672 | 28672 | Y + 32 | goal_velocity | 2 | RW | RAM | 0 | 1023 | N + 34 | goal_torque | 2 | RW | RAM | 0 | 1023 | N + 36 | present_position | 2 | R | RAM | -32768 | 32767 | Y + 38 | present_velocity | 2 | R | RAM | 0 | 2048 | N + 40 | present_load | 2 | R | RAM | 0 | 2048 | N + 42 | present_voltage | 1 | R | RAM | 50 | 250 | N + 43 | present_temperature | 1 | R | RAM | 0 | 99 | N + 44 | registered_instruction | 1 | R | RAM | 0 | 1 | N + 46 | is_moving | 1 | R | RAM | 0 | 1 | N + 47 | EEPROM_lock | 1 | RW | RAM | 0 | 1 | N + 48 | punch | 2 | RW | RAM | 0 | 1023 | N + 73 | goal_acceleration | 1 | RW | RAM | 0 | 254 | N diff --git a/humanoid_robot_intelligence_control_system_device/devices/dynamixel/MX-64.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/MX-64.device new file mode 100755 index 0000000..c6a50de --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/MX-64.device @@ -0,0 +1,66 @@ +[device info] +model_name = MX-64 +device_type = dynamixel + +[type info] +value_of_0_radian_position = 2048 +value_of_min_radian_position = 0 +value_of_max_radian_position = 4095 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = +position_d_gain_item_name = position_d_gain +position_i_gain_item_name = position_i_gain +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = +velocity_p_gain_item_name = + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 2 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 3 | ID | 1 | RW | EEPROM | 0 | 252 | N + 4 | baudrate | 1 | RW | EEPROM | 0 | 252 | N + 5 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 6 | CW_angle_limit | 2 | RW | EEPROM | 0 | 4095 | N + 8 | CCW_angle_limit | 2 | RW | EEPROM | 0 | 4095 | N + 10 | drive_mode | 1 | RW | EEPROM | 0 | 3 | N + 11 | max_temperature_limit | 1 | RW | EEPROM | 0 | 99 | N + 12 | min_voltage_limit | 1 | RW | EEPROM | 0 | 250 | N + 13 | max_voltage_limit | 1 | RW | EEPROM | 0 | 250 | N + 14 | max_torque | 2 | RW | EEPROM | 0 | 1023 | N + 16 | status_return_level | 1 | RW | EEPROM | 0 | 2 | N + 17 | alarm_LED | 1 | RW | EEPROM | 0 | 127 | N + 18 | alarm_shutdown | 1 | RW | EEPROM | 0 | 127 | N + 20 | multi_turn_offset | 2 | RW | EEPROM | -26624 | 26624 | Y + 22 | resolution_dividor | 1 | RW | EEPROM | 1 | 255 | N + 24 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 25 | LED | 1 | RW | RAM | 0 | 1 | N + 26 | position_d_gain | 1 | RW | RAM | 0 | 254 | N + 27 | position_i_gain | 1 | RW | RAM | 0 | 254 | N + 28 | position_p_gain | 1 | RW | RAM | 0 | 254 | N + 30 | goal_position | 2 | RW | RAM | -28672 | 28672 | Y + 32 | goal_velocity | 2 | RW | RAM | 0 | 1023 | N + 34 | goal_torque | 2 | RW | RAM | 0 | 1023 | N + 36 | present_position | 2 | R | RAM | -32768 | 32767 | Y + 38 | present_velocity | 2 | R | RAM | 0 | 2048 | N + 40 | present_load | 2 | R | RAM | 0 | 2048 | N + 42 | present_voltage | 1 | R | RAM | 50 | 250 | N + 43 | present_temperature | 1 | R | RAM | 0 | 99 | N + 44 | registered_instruction | 1 | R | RAM | 0 | 1 | N + 46 | is_moving | 1 | R | RAM | 0 | 1 | N + 47 | EEPROM_lock | 1 | RW | RAM | 0 | 1 | N + 48 | punch | 2 | RW | RAM | 0 | 1023 | N + 68 | current_consumption | 2 | RW | RAM | 0 | 4095 | N + 70 | torque_control_mode | 1 | RW | RAM | 0 | 1 | N + 71 | torque_control_goal | 2 | RW | RAM | 0 | 2047 | N + 73 | goal_acceleration | 1 | RW | RAM | 0 | 254 | N + diff --git a/humanoid_robot_intelligence_control_system_device/devices/dynamixel/PH42-020-S300-R.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/PH42-020-S300-R.device new file mode 100644 index 0000000..92e9df6 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/PH42-020-S300-R.device @@ -0,0 +1,92 @@ +[device info] +model_name = PH42-020-S300-R +device_type = dynamixel + +[type info] +torque_to_current_value_ratio = 109.3747778 +velocity_to_value_ratio = 954.93 +value_of_0_radian_position = 0 +value_of_min_radian_position = -303750 +value_of_max_radian_position = 303750 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_current +position_d_gain_item_name = position_d_gain +position_i_gain_item_name = position_i_gain +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 10 | drive_mode | 1 | RW | EEPROM | 0 | 3 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 12 | secondary_id | 1 | RW | EEPROM | 0 | 255 | N + 13 | protocol_version | 1 | RW | EEPROM | 1 | 2 | N + 20 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 24 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 31 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 32 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 34 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 36 | pwm_limit | 2 | RW | EEPROM | 0 | 885 | N + 38 | current_limit | 2 | RW | EEPROM | 0 | 32767 | N + 40 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 44 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 48 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 52 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 56 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 57 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 58 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 59 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 63 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 168 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 512 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 513 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 514 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 515 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 516 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 517 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 518 | hardware_error_status | 1 | R | RAM | 0 | 31 | N + 524 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N + 526 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N + 528 | position_d_gain | 2 | RW | RAM | 0 | 2047 | N + 530 | position_i_gain | 2 | RW | RAM | 0 | 2047 | N + 532 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N + 536 | feedforward_2nd_gain | 2 | RW | RAM | 0 | 32767 | N + 538 | feedforward_1st_gain | 2 | RW | RAM | 0 | 32767 | N + 546 | bus_watchdog | 1 | RW | RAM | -1 | 127 | Y + 548 | goal_pwm | 2 | RW | RAM | -2100 | 2100 | Y + 550 | goal_current | 2 | RW | RAM | -32768 | 32767 | Y + 552 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 556 | profile_acceleration | 4 | RW | RAM | 0 | 32767 | N + 560 | profile_velocity | 4 | RW | RAM | 0 | 1023 | N + 564 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 568 | realtime_tick | 2 | R | RAM | 0 | 32767 | N + 570 | moving | 1 | R | RAM | 0 | 1 | N + 571 | moving_status | 1 | R | RAM | 0 | 255 | N + 572 | present_pwm | 2 | R | RAM | -2100 | 2100 | Y + 574 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 576 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 580 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 584 | velocity_trajectory | 4 | R | RAM | 0 | 1023 | N + 588 | position_trajectory | 4 | R | RAM | 0 | 4095 | N + 592 | present_voltage | 2 | R | RAM | 0 | 500 | N + 594 | present_temperature | 1 | R | RAM | 0 | 200 | N + 600 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 602 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 604 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 606 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N diff --git a/humanoid_robot_intelligence_control_system_device/devices/dynamixel/PH54-100-S500-R.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/PH54-100-S500-R.device new file mode 100644 index 0000000..6dd1ece --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/PH54-100-S500-R.device @@ -0,0 +1,92 @@ +[device info] +model_name = PH54-100-S500-R +device_type = dynamixel + +[type info] +torque_to_current_value_ratio = 155.658486 +velocity_to_value_ratio = 954.93 +value_of_0_radian_position = 0 +value_of_min_radian_position = -501923 +value_of_max_radian_position = 501923 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_current +position_d_gain_item_name = position_d_gain +position_i_gain_item_name = position_i_gain +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 10 | drive_mode | 1 | RW | EEPROM | 0 | 3 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 12 | secondary_id | 1 | RW | EEPROM | 0 | 255 | N + 13 | protocol_version | 1 | RW | EEPROM | 1 | 2 | N + 20 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 24 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 31 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 32 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 34 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 36 | pwm_limit | 2 | RW | EEPROM | 0 | 885 | N + 38 | current_limit | 2 | RW | EEPROM | 0 | 32767 | N + 40 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 44 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 48 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 52 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 56 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 57 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 58 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 59 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 63 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 168 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 512 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 513 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 514 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 515 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 516 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 517 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 518 | hardware_error_status | 1 | R | RAM | 0 | 31 | N + 524 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N + 526 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N + 528 | position_d_gain | 2 | RW | RAM | 0 | 2047 | N + 530 | position_i_gain | 2 | RW | RAM | 0 | 2047 | N + 532 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N + 536 | feedforward_2nd_gain | 2 | RW | RAM | 0 | 32767 | N + 538 | feedforward_1st_gain | 2 | RW | RAM | 0 | 32767 | N + 546 | bus_watchdog | 1 | RW | RAM | -1 | 127 | Y + 548 | goal_pwm | 2 | RW | RAM | -2100 | 2100 | Y + 550 | goal_current | 2 | RW | RAM | -32768 | 32767 | Y + 552 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 556 | profile_acceleration | 4 | RW | RAM | 0 | 32767 | N + 560 | profile_velocity | 4 | RW | RAM | 0 | 1023 | N + 564 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 568 | realtime_tick | 2 | R | RAM | 0 | 32767 | N + 570 | moving | 1 | R | RAM | 0 | 1 | N + 571 | moving_status | 1 | R | RAM | 0 | 255 | N + 572 | present_pwm | 2 | R | RAM | -2100 | 2100 | Y + 574 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 576 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 580 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 584 | velocity_trajectory | 4 | R | RAM | 0 | 1023 | N + 588 | position_trajectory | 4 | R | RAM | 0 | 4095 | N + 592 | present_voltage | 2 | R | RAM | 0 | 500 | N + 594 | present_temperature | 1 | R | RAM | 0 | 200 | N + 600 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 602 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 604 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 606 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N diff --git a/humanoid_robot_intelligence_control_system_device/devices/dynamixel/PH54-200-S500-R.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/PH54-200-S500-R.device new file mode 100644 index 0000000..b758315 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/PH54-200-S500-R.device @@ -0,0 +1,92 @@ +[device info] +model_name = PH54-200-S500-R +device_type = dynamixel + +[type info] +torque_to_current_value_ratio = 146.502114 +velocity_to_value_ratio = 954.93 +value_of_0_radian_position = 0 +value_of_min_radian_position = -501923 +value_of_max_radian_position = 501923 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_current +position_d_gain_item_name = position_d_gain +position_i_gain_item_name = position_i_gain +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 10 | drive_mode | 1 | RW | EEPROM | 0 | 3 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 12 | secondary_id | 1 | RW | EEPROM | 0 | 255 | N + 13 | protocol_version | 1 | RW | EEPROM | 1 | 2 | N + 20 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 24 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 31 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 32 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 34 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 36 | pwm_limit | 2 | RW | EEPROM | 0 | 885 | N + 38 | current_limit | 2 | RW | EEPROM | 0 | 32767 | N + 40 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 44 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 48 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 52 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 56 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 57 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 58 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 59 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 63 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 168 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 512 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 513 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 514 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 515 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 516 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 517 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 518 | hardware_error_status | 1 | R | RAM | 0 | 31 | N + 524 | velocity_i_gain | 2 | RW | RAM | 0 | 2047 | N + 526 | velocity_p_gain | 2 | RW | RAM | 0 | 2047 | N + 528 | position_d_gain | 2 | RW | RAM | 0 | 2047 | N + 530 | position_i_gain | 2 | RW | RAM | 0 | 2047 | N + 532 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N + 536 | feedforward_2nd_gain | 2 | RW | RAM | 0 | 32767 | N + 538 | feedforward_1st_gain | 2 | RW | RAM | 0 | 32767 | N + 546 | bus_watchdog | 1 | RW | RAM | -1 | 127 | Y + 548 | goal_pwm | 2 | RW | RAM | -2100 | 2100 | Y + 550 | goal_current | 2 | RW | RAM | -32768 | 32767 | Y + 552 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 556 | profile_acceleration | 4 | RW | RAM | 0 | 32767 | N + 560 | profile_velocity | 4 | RW | RAM | 0 | 1023 | N + 564 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 568 | realtime_tick | 2 | R | RAM | 0 | 32767 | N + 570 | moving | 1 | R | RAM | 0 | 1 | N + 571 | moving_status | 1 | R | RAM | 0 | 255 | N + 572 | present_pwm | 2 | R | RAM | -2100 | 2100 | Y + 574 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 576 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 580 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 584 | velocity_trajectory | 4 | R | RAM | 0 | 1023 | N + 588 | position_trajectory | 4 | R | RAM | 0 | 4095 | N + 592 | present_voltage | 2 | R | RAM | 0 | 500 | N + 594 | present_temperature | 1 | R | RAM | 0 | 200 | N + 600 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 602 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 604 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 606 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N diff --git a/humanoid_robot_intelligence_control_system_device/devices/dynamixel/RH-P12-RN(A).device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/RH-P12-RN(A).device new file mode 100644 index 0000000..31d253b --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/RH-P12-RN(A).device @@ -0,0 +1,90 @@ +[device info] +model_name = RH-P12-RN(A) +device_type = dynamixel + +[type info] +value_of_0_radian_position = 0 +value_of_min_radian_position = 0 +value_of_max_radian_position = 740 +min_radian = 0 +max_radian = 1.150767 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_current +position_d_gain_item_name = position_d_gain +position_i_gain_item_name = position_i_gain +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 9 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 10 | drive_mode | 1 | RW | EEPROM | 0 | 1 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 12 | secondary_ID | 1 | RW | EEPROM | 0 | 255 | N + 20 | homing_offset | 4 | RW | EEPROM | -1150 | 1150 | Y + 24 | moving_threshold | 4 | RW | EEPROM | 0 | 2970 | N + 31 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 32 | max_voltage_limit | 2 | RW | EEPROM | 0 | 350 | N + 34 | min_voltage_limit | 2 | RW | EEPROM | 0 | 350 | N + 36 | pwm_limit | 2 | RW | EEPROM | 0 | 2009 | N + 38 | current_limit | 2 | RW | EEPROM | 0 | 1984 | N + 40 | acceleration_limit | 4 | RW | EEPROM | 0 | 1378788 | N + 44 | velocity_limit | 4 | RW | EEPROM | 0 | 2970 | N + 48 | max_position_limit | 4 | RW | EEPROM | 0 | 1150 | N + 52 | min_position_limit | 4 | RW | EEPROM | 0 | 1150 | N + 56 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 57 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 58 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 59 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 63 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 168 | indirect_address_1 | 2 | RW | EEPROM | 512 | 1023 | N + 512 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 513 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 514 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 515 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 516 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 517 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 518 | hardware_error_status | 2 | R | RAM | 0 | 63 | N + 524 | velocity_i_gain | 2 | RW | RAM | 0 | 32767 | N + 526 | velocity_p_gain | 2 | RW | RAM | 0 | 32767 | N + 528 | position_d_gain | 2 | RW | RAM | 0 | 32767 | N + 530 | position_i_gain | 2 | RW | RAM | 0 | 32767 | N + 532 | position_p_gain | 2 | RW | RAM | 0 | 32767 | N + 536 | feedforward_2nd_gain | 2 | RW | RAM | 0 | 32767 | N + 538 | feedforward_1st_gain | 2 | RW | RAM | 0 | 32767 | N + 546 | bus_watchdog | 1 | RW | RAM | -1 | 127 | Y + 548 | goal_pwm | 2 | RW | RAM | -2009 | 2009 | Y + 550 | goal_current | 2 | RW | RAM | -1984 | 1984 | Y + 552 | goal_velocity | 4 | RW | RAM | -2970 | 2970 | Y + 556 | profile_acceleration | 4 | RW | RAM | 0 | 1378788 | N + 560 | profile_velocity | 4 | RW | RAM | 0 | 2970 | N + 564 | goal_position | 4 | RW | RAM | 0 | 1150 | N + 568 | realtime_tick | 2 | R | RAM | 0 | 32767 | N + 570 | is_moving | 1 | R | RAM | 0 | 1 | N + 571 | moving_status | 1 | R | RAM | 0 | 255 | N + 572 | present_pwm | 2 | R | RAM | -2009 | 2009 | Y + 574 | present_current | 2 | R | RAM | -1984 | 1984 | Y + 576 | present_velocity | 4 | R | RAM | -2970 | 2970 | Y + 580 | present_position | 4 | R | RAM | -1150 | 1150 | Y + 584 | velocity_trajectory | 4 | R | RAM | -2147483648 | 2147483648 | Y + 588 | position_trajectory | 4 | R | RAM | -2147483648 | 2147483648 | Y + 592 | present_voltage | 2 | R | RAM | 0 | 350 | N + 594 | present_temperature | 1 | R | RAM | 0 | 100 | N + 595 | grip_detection | 1 | R | RAM | 0 | 1 | N + 600 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 602 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 604 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 606 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N diff --git a/humanoid_robot_intelligence_control_system_device/devices/dynamixel/RH-P12-RN.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/RH-P12-RN.device new file mode 100755 index 0000000..14ed73b --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/RH-P12-RN.device @@ -0,0 +1,73 @@ +[device info] +model_name = RH-P12-RN +device_type = dynamixel + +[type info] +value_of_0_radian_position = 0 +value_of_min_radian_position = 0 +value_of_max_radian_position = 740 +min_radian = 0 +max_radian = 1.150767 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_current +position_d_gain_item_name = position_d_gain +position_i_gain_item_name = position_i_gain +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = +velocity_p_gain_item_name = + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 5 | N + 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 30 | current_limit | 2 | RW | EEPROM | 0 | 32767 | N + 32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 562 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 590 | position_d_gain | 2 | RW | RAM | 0 | 2047 | N + 592 | position_i_gain | 2 | RW | RAM | 0 | 2047 | N + 594 | position_p_gain | 2 | RW | RAM | 0 | 2047 | N + 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 604 | goal_current | 2 | RW | RAM | -32768 | 32767 | Y + 606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N + 610 | is_moving | 1 | R | RAM | 0 | 1 | N + 611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 621 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 623 | present_voltage | 2 | R | RAM | 0 | 500 | N + 625 | present_temperature | 1 | R | RAM | 0 | 200 | N + 626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 891 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N diff --git a/humanoid_robot_intelligence_control_system_device/devices/dynamixel/XM-430.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/XM-430.device new file mode 100755 index 0000000..7dfe91d --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/XM-430.device @@ -0,0 +1,82 @@ +[device info] +model_name = XM-430 +device_type = dynamixel + +[type info] +torque_to_current_value_ratio = 149.795386991 + +value_of_0_radian_position = 2048 +value_of_min_radian_position = 0 +value_of_max_radian_position = 4095 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_current +position_d_gain_item_name = position_d_gain +position_i_gain_item_name = position_i_gain +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 7 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 10 | drive_mode | 1 | RW | EEPROM | 0 | 1 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 16 | N + 12 | secondary_id | 1 | RW | EEPROM | 0 | 255 | N + 20 | homing_offset | 4 | RW | EEPROM | -1044479 | 1044479 | Y + 24 | moving_threshold | 4 | RW | EEPROM | 0 | 1023 | N + 31 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 32 | max_voltage_limit | 2 | RW | EEPROM | 95 | 160 | N + 34 | min_voltage_limit | 2 | RW | EEPROM | 95 | 160 | N + 36 | pwm_limit | 2 | RW | EEPROM | 0 | 885 | N + 38 | current_limit | 2 | RW | EEPROM | 0 | 1193 | N + 44 | velocity_limit | 4 | RW | EEPROM | 0 | 1023 | N + 48 | max_position_limit | 4 | RW | EEPROM | 0 | 4095 | N + 52 | min_position_limit | 4 | RW | EEPROM | 0 | 4095 | N + 63 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 64 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 65 | LED | 1 | RW | RAM | 0 | 1 | N + 68 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 69 | registered_instruction | 1 | R | RAM | 0 | 1 | N + 70 | hardware_error_status | 1 | R | RAM | 0 | 255 | N + 76 | velocity_i_gain | 2 | RW | RAM | 0 | 32767 | N + 78 | velocity_p_gain | 2 | RW | RAM | 0 | 32767 | N + 80 | position_d_gain | 2 | RW | RAM | 0 | 32767 | N + 82 | position_i_gain | 2 | RW | RAM | 0 | 32767 | N + 84 | position_p_gain | 2 | RW | RAM | 0 | 32767 | N + 88 | feedforward_2nd_gain | 2 | RW | RAM | 0 | 32767 | N + 90 | feedforward_1st_gain | 2 | RW | RAM | 0 | 32767 | N + 98 | bus_watchdog | 1 | RW | RAM | -1 | 127 | Y + 100 | goal_pwm | 2 | RW | RAM | 0 | 885 | N + 102 | goal_current | 2 | RW | RAM | 0 | 1193 | N + 104 | goal_velocity | 4 | RW | RAM | 0 | 1023 | N + 108 | profile_acceleration | 4 | RW | RAM | 0 | 32767 | N + 112 | profile_velocity | 4 | RW | RAM | 0 | 1023 | N + 116 | goal_position | 4 | RW | RAM | -1048575 | 1048575 | Y + 120 | realtime_tick | 2 | R | RAM | 0 | 32767 | N + 122 | moving | 1 | R | RAM | 0 | 1 | N + 123 | moving_status | 1 | R | RAM | 0 | 255 | N + 124 | present_pwm | 2 | R | RAM | 0 | 885 | N + 126 | present_current | 2 | R | RAM | 0 | 1193 | N + 128 | present_velocity | 4 | R | RAM | 0 | 1023 | N + 132 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 136 | velocity_trajectory | 4 | R | RAM | 0 | 1023 | N + 140 | position_trajectory | 4 | R | RAM | 0 | 4095 | N + 144 | present_input_voltage | 2 | R | RAM | 95 | 160 | N + 146 | present_temperature | 1 | R | RAM | 0 | 100 | N + 168 | indirect_address_1 | 2 | RW | RAM | 0 | 65535 | N + 224 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 578 | indirect_address_29 | 2 | RW | RAM | 0 | 65535 | N + 634 | indirect_data_29 | 1 | RW | RAM | 0 | 255 | N diff --git a/humanoid_robot_intelligence_control_system_device/devices/dynamixel/XM430-W210.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/XM430-W210.device new file mode 100644 index 0000000..6113e04 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/XM430-W210.device @@ -0,0 +1,83 @@ +[device info] +model_name = XM430-W210 +device_type = dynamixel + +[type info] +torque_to_current_value_ratio = 235.53647082 +velocity_to_value_ratio = 41.707853 + +value_of_0_radian_position = 2048 +value_of_min_radian_position = 0 +value_of_max_radian_position = 4095 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_current +position_d_gain_item_name = position_d_gain +position_i_gain_item_name = position_i_gain +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 7 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 10 | drive_mode | 1 | RW | EEPROM | 0 | 1 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 16 | N + 12 | secondary_id | 1 | RW | EEPROM | 0 | 255 | N + 20 | homing_offset | 4 | RW | EEPROM | -1044479 | 1044479 | Y + 24 | moving_threshold | 4 | RW | EEPROM | 0 | 1023 | N + 31 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 32 | max_voltage_limit | 2 | RW | EEPROM | 95 | 160 | N + 34 | min_voltage_limit | 2 | RW | EEPROM | 95 | 160 | N + 36 | pwm_limit | 2 | RW | EEPROM | 0 | 885 | N + 38 | current_limit | 2 | RW | EEPROM | 0 | 1193 | N + 44 | velocity_limit | 4 | RW | EEPROM | 0 | 1023 | N + 48 | max_position_limit | 4 | RW | EEPROM | 0 | 4095 | N + 52 | min_position_limit | 4 | RW | EEPROM | 0 | 4095 | N + 63 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 64 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 65 | LED | 1 | RW | RAM | 0 | 1 | N + 68 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 69 | registered_instruction | 1 | R | RAM | 0 | 1 | N + 70 | hardware_error_status | 1 | R | RAM | 0 | 255 | N + 76 | velocity_i_gain | 2 | RW | RAM | 0 | 32767 | N + 78 | velocity_p_gain | 2 | RW | RAM | 0 | 32767 | N + 80 | position_d_gain | 2 | RW | RAM | 0 | 32767 | N + 82 | position_i_gain | 2 | RW | RAM | 0 | 32767 | N + 84 | position_p_gain | 2 | RW | RAM | 0 | 32767 | N + 88 | feedforward_2nd_gain | 2 | RW | RAM | 0 | 32767 | N + 90 | feedforward_1st_gain | 2 | RW | RAM | 0 | 32767 | N + 98 | bus_watchdog | 1 | RW | RAM | -1 | 127 | Y + 100 | goal_pwm | 2 | RW | RAM | 0 | 885 | N + 102 | goal_current | 2 | RW | RAM | 0 | 1193 | N + 104 | goal_velocity | 4 | RW | RAM | 0 | 1023 | N + 108 | profile_acceleration | 4 | RW | RAM | 0 | 32767 | N + 112 | profile_velocity | 4 | RW | RAM | 0 | 1023 | N + 116 | goal_position | 4 | RW | RAM | -1048575 | 1048575 | Y + 120 | realtime_tick | 2 | R | RAM | 0 | 32767 | N + 122 | moving | 1 | R | RAM | 0 | 1 | N + 123 | moving_status | 1 | R | RAM | 0 | 255 | N + 124 | present_pwm | 2 | R | RAM | 0 | 885 | N + 126 | present_current | 2 | R | RAM | 0 | 1193 | N + 128 | present_velocity | 4 | R | RAM | 0 | 1023 | N + 132 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 136 | velocity_trajectory | 4 | R | RAM | 0 | 1023 | N + 140 | position_trajectory | 4 | R | RAM | 0 | 4095 | N + 144 | present_input_voltage | 2 | R | RAM | 95 | 160 | N + 146 | present_temperature | 1 | R | RAM | 0 | 100 | N + 168 | indirect_address_1 | 2 | RW | RAM | 0 | 65535 | N + 224 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 578 | indirect_address_29 | 2 | RW | RAM | 0 | 65535 | N + 634 | indirect_data_29 | 1 | RW | RAM | 0 | 255 | N diff --git a/humanoid_robot_intelligence_control_system_device/devices/dynamixel/XM430-W350.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/XM430-W350.device new file mode 100644 index 0000000..e1be362 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/XM430-W350.device @@ -0,0 +1,83 @@ +[device info] +model_name = XM430-W350 +device_type = dynamixel + +[type info] +torque_to_current_value_ratio = 149.795386991 +velocity_to_value_ratio = 41.707853 + +value_of_0_radian_position = 2048 +value_of_min_radian_position = 0 +value_of_max_radian_position = 4095 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_current +position_d_gain_item_name = position_d_gain +position_i_gain_item_name = position_i_gain +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 7 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 10 | drive_mode | 1 | RW | EEPROM | 0 | 1 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 16 | N + 12 | secondary_id | 1 | RW | EEPROM | 0 | 255 | N + 20 | homing_offset | 4 | RW | EEPROM | -1044479 | 1044479 | Y + 24 | moving_threshold | 4 | RW | EEPROM | 0 | 1023 | N + 31 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 32 | max_voltage_limit | 2 | RW | EEPROM | 95 | 160 | N + 34 | min_voltage_limit | 2 | RW | EEPROM | 95 | 160 | N + 36 | pwm_limit | 2 | RW | EEPROM | 0 | 885 | N + 38 | current_limit | 2 | RW | EEPROM | 0 | 1193 | N + 44 | velocity_limit | 4 | RW | EEPROM | 0 | 1023 | N + 48 | max_position_limit | 4 | RW | EEPROM | 0 | 4095 | N + 52 | min_position_limit | 4 | RW | EEPROM | 0 | 4095 | N + 63 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 64 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 65 | LED | 1 | RW | RAM | 0 | 1 | N + 68 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 69 | registered_instruction | 1 | R | RAM | 0 | 1 | N + 70 | hardware_error_status | 1 | R | RAM | 0 | 255 | N + 76 | velocity_i_gain | 2 | RW | RAM | 0 | 32767 | N + 78 | velocity_p_gain | 2 | RW | RAM | 0 | 32767 | N + 80 | position_d_gain | 2 | RW | RAM | 0 | 32767 | N + 82 | position_i_gain | 2 | RW | RAM | 0 | 32767 | N + 84 | position_p_gain | 2 | RW | RAM | 0 | 32767 | N + 88 | feedforward_2nd_gain | 2 | RW | RAM | 0 | 32767 | N + 90 | feedforward_1st_gain | 2 | RW | RAM | 0 | 32767 | N + 98 | bus_watchdog | 1 | RW | RAM | -1 | 127 | Y + 100 | goal_pwm | 2 | RW | RAM | 0 | 885 | N + 102 | goal_current | 2 | RW | RAM | 0 | 1193 | N + 104 | goal_velocity | 4 | RW | RAM | 0 | 1023 | N + 108 | profile_acceleration | 4 | RW | RAM | 0 | 32767 | N + 112 | profile_velocity | 4 | RW | RAM | 0 | 1023 | N + 116 | goal_position | 4 | RW | RAM | -1048575 | 1048575 | Y + 120 | realtime_tick | 2 | R | RAM | 0 | 32767 | N + 122 | moving | 1 | R | RAM | 0 | 1 | N + 123 | moving_status | 1 | R | RAM | 0 | 255 | N + 124 | present_pwm | 2 | R | RAM | 0 | 885 | N + 126 | present_current | 2 | R | RAM | 0 | 1193 | N + 128 | present_velocity | 4 | R | RAM | 0 | 1023 | N + 132 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 136 | velocity_trajectory | 4 | R | RAM | 0 | 1023 | N + 140 | position_trajectory | 4 | R | RAM | 0 | 4095 | N + 144 | present_input_voltage | 2 | R | RAM | 95 | 160 | N + 146 | present_temperature | 1 | R | RAM | 0 | 100 | N + 168 | indirect_address_1 | 2 | RW | RAM | 0 | 65535 | N + 224 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 578 | indirect_address_29 | 2 | RW | RAM | 0 | 65535 | N + 634 | indirect_data_29 | 1 | RW | RAM | 0 | 255 | N diff --git a/humanoid_robot_intelligence_control_system_device/devices/dynamixel/XM540-W150.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/XM540-W150.device new file mode 100755 index 0000000..64fcf3b --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/XM540-W150.device @@ -0,0 +1,89 @@ +[device info] +model_name = XM540-W150 +device_type = dynamixel + +[type info] +torque_to_current_value_ratio = 289.13672036 +velocity_to_value_ratio = 41.707853 + +value_of_0_radian_position = 2048 +value_of_min_radian_position = 0 +value_of_max_radian_position = 4095 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_current +position_d_gain_item_name = position_d_gain +position_i_gain_item_name = position_i_gain +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 7 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 10 | drive_mode | 1 | RW | EEPROM | 0 | 1 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 16 | N + 12 | secondary_id | 1 | RW | EEPROM | 0 | 255 | N + 20 | homing_offset | 4 | RW | EEPROM | -1044479 | 1044479 | Y + 24 | moving_threshold | 4 | RW | EEPROM | 0 | 1023 | N + 31 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 32 | max_voltage_limit | 2 | RW | EEPROM | 95 | 160 | N + 34 | min_voltage_limit | 2 | RW | EEPROM | 95 | 160 | N + 36 | pwm_limit | 2 | RW | EEPROM | 0 | 885 | N + 38 | current_limit | 2 | RW | EEPROM | 0 | 1193 | N + 44 | velocity_limit | 4 | RW | EEPROM | 0 | 1023 | N + 48 | max_position_limit | 4 | RW | EEPROM | 0 | 4095 | N + 52 | min_position_limit | 4 | RW | EEPROM | 0 | 4095 | N + 56 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 57 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 58 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 63 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 64 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 65 | LED | 1 | RW | RAM | 0 | 1 | N + 68 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 69 | registered_instruction | 1 | R | RAM | 0 | 1 | N + 70 | hardware_error_status | 1 | R | RAM | 0 | 255 | N + 76 | velocity_i_gain | 2 | RW | RAM | 0 | 32767 | N + 78 | velocity_p_gain | 2 | RW | RAM | 0 | 32767 | N + 80 | position_d_gain | 2 | RW | RAM | 0 | 32767 | N + 82 | position_i_gain | 2 | RW | RAM | 0 | 32767 | N + 84 | position_p_gain | 2 | RW | RAM | 0 | 32767 | N + 88 | feedforward_2nd_gain | 2 | RW | RAM | 0 | 32767 | N + 90 | feedforward_1st_gain | 2 | RW | RAM | 0 | 32767 | N + 98 | bus_watchdog | 1 | RW | RAM | -1 | 127 | Y + 100 | goal_pwm | 2 | RW | RAM | 0 | 885 | N + 102 | goal_current | 2 | RW | RAM | 0 | 1193 | N + 104 | goal_velocity | 4 | RW | RAM | 0 | 1023 | N + 108 | profile_acceleration | 4 | RW | RAM | 0 | 32767 | N + 112 | profile_velocity | 4 | RW | RAM | 0 | 1023 | N + 116 | goal_position | 4 | RW | RAM | -1048575 | 1048575 | Y + 120 | realtime_tick | 2 | R | RAM | 0 | 32767 | N + 122 | moving | 1 | R | RAM | 0 | 1 | N + 123 | moving_status | 1 | R | RAM | 0 | 255 | N + 124 | present_pwm | 2 | R | RAM | 0 | 885 | N + 126 | present_current | 2 | R | RAM | 0 | 1193 | N + 128 | present_velocity | 4 | R | RAM | 0 | 1023 | N + 132 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 136 | velocity_trajectory | 4 | R | RAM | 0 | 1023 | N + 140 | position_trajectory | 4 | R | RAM | 0 | 4095 | N + 144 | present_input_voltage | 2 | R | RAM | 95 | 160 | N + 146 | present_temperature | 1 | R | RAM | 0 | 100 | N + 152 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 154 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 156 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 168 | indirect_address_1 | 2 | RW | RAM | 0 | 65535 | N + 224 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 578 | indirect_address_29 | 2 | RW | RAM | 0 | 65535 | N + 634 | indirect_data_29 | 1 | RW | RAM | 0 | 255 | N diff --git a/humanoid_robot_intelligence_control_system_device/devices/dynamixel/XM540-W270.device b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/XM540-W270.device new file mode 100755 index 0000000..c787cba --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/devices/dynamixel/XM540-W270.device @@ -0,0 +1,89 @@ +[device info] +model_name = XM540-W270 +device_type = dynamixel + +[type info] +torque_to_current_value_ratio = 156.133829 +velocity_to_value_ratio = 41.707853 + +value_of_0_radian_position = 2048 +value_of_min_radian_position = 0 +value_of_max_radian_position = 4095 +min_radian = -3.14159265 +max_radian = 3.14159265 + +torque_enable_item_name = torque_enable +present_position_item_name = present_position +present_velocity_item_name = present_velocity +present_current_item_name = present_current +goal_position_item_name = goal_position +goal_velocity_item_name = goal_velocity +goal_current_item_name = goal_current +position_d_gain_item_name = position_d_gain +position_i_gain_item_name = position_i_gain +position_p_gain_item_name = position_p_gain +velocity_d_gain_item_name = +velocity_i_gain_item_name = velocity_i_gain +velocity_p_gain_item_name = velocity_p_gain + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 7 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 10 | drive_mode | 1 | RW | EEPROM | 0 | 1 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 16 | N + 12 | secondary_id | 1 | RW | EEPROM | 0 | 255 | N + 20 | homing_offset | 4 | RW | EEPROM | -1044479 | 1044479 | Y + 24 | moving_threshold | 4 | RW | EEPROM | 0 | 1023 | N + 31 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 32 | max_voltage_limit | 2 | RW | EEPROM | 95 | 160 | N + 34 | min_voltage_limit | 2 | RW | EEPROM | 95 | 160 | N + 36 | pwm_limit | 2 | RW | EEPROM | 0 | 885 | N + 38 | current_limit | 2 | RW | EEPROM | 0 | 1193 | N + 44 | velocity_limit | 4 | RW | EEPROM | 0 | 1023 | N + 48 | max_position_limit | 4 | RW | EEPROM | 0 | 4095 | N + 52 | min_position_limit | 4 | RW | EEPROM | 0 | 4095 | N + 56 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 57 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 58 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 63 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 64 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 65 | LED | 1 | RW | RAM | 0 | 1 | N + 68 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 69 | registered_instruction | 1 | R | RAM | 0 | 1 | N + 70 | hardware_error_status | 1 | R | RAM | 0 | 255 | N + 76 | velocity_i_gain | 2 | RW | RAM | 0 | 32767 | N + 78 | velocity_p_gain | 2 | RW | RAM | 0 | 32767 | N + 80 | position_d_gain | 2 | RW | RAM | 0 | 32767 | N + 82 | position_i_gain | 2 | RW | RAM | 0 | 32767 | N + 84 | position_p_gain | 2 | RW | RAM | 0 | 32767 | N + 88 | feedforward_2nd_gain | 2 | RW | RAM | 0 | 32767 | N + 90 | feedforward_1st_gain | 2 | RW | RAM | 0 | 32767 | N + 98 | bus_watchdog | 1 | RW | RAM | -1 | 127 | Y + 100 | goal_pwm | 2 | RW | RAM | 0 | 885 | N + 102 | goal_current | 2 | RW | RAM | 0 | 1193 | N + 104 | goal_velocity | 4 | RW | RAM | 0 | 1023 | N + 108 | profile_acceleration | 4 | RW | RAM | 0 | 32767 | N + 112 | profile_velocity | 4 | RW | RAM | 0 | 1023 | N + 116 | goal_position | 4 | RW | RAM | -1048575 | 1048575 | Y + 120 | realtime_tick | 2 | R | RAM | 0 | 32767 | N + 122 | moving | 1 | R | RAM | 0 | 1 | N + 123 | moving_status | 1 | R | RAM | 0 | 255 | N + 124 | present_pwm | 2 | R | RAM | 0 | 885 | N + 126 | present_current | 2 | R | RAM | 0 | 1193 | N + 128 | present_velocity | 4 | R | RAM | 0 | 1023 | N + 132 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 136 | velocity_trajectory | 4 | R | RAM | 0 | 1023 | N + 140 | position_trajectory | 4 | R | RAM | 0 | 4095 | N + 144 | present_input_voltage | 2 | R | RAM | 95 | 160 | N + 146 | present_temperature | 1 | R | RAM | 0 | 100 | N + 152 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 154 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 156 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 168 | indirect_address_1 | 2 | RW | RAM | 0 | 65535 | N + 224 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 578 | indirect_address_29 | 2 | RW | RAM | 0 | 65535 | N + 634 | indirect_data_29 | 1 | RW | RAM | 0 | 255 | N diff --git a/humanoid_robot_intelligence_control_system_device/devices/sensor/CM-740.device b/humanoid_robot_intelligence_control_system_device/devices/sensor/CM-740.device new file mode 100755 index 0000000..5cdf45f --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/devices/sensor/CM-740.device @@ -0,0 +1,25 @@ +[device info] +model_name = CM-740 +device_type = sensor + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 2 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 3 | ID | 1 | RW | EEPROM | 0 | 252 | N + 4 | baudrate | 1 | RW | EEPROM | 0 | 252 | N + 5 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 16 | status_return_level | 1 | RW | EEPROM | 0 | 2 | N + 24 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 25 | LED | 1 | RW | RAM | 0 | 7 | N + 26 | LED_5 | 2 | RW | RAM | 0 | 32767 | N + 28 | LED_6 | 2 | RW | RAM | 0 | 32767 | N + 30 | button | 1 | RW | RAM | 0 | 3 | N + 38 | gyro_z | 2 | R | RAM | 0 | 1023 | N + 40 | gyro_y | 2 | R | RAM | 0 | 1023 | N + 42 | gyro_x | 2 | R | RAM | 0 | 1023 | N + 44 | acc_x | 2 | R | RAM | 0 | 1023 | N + 46 | acc_y | 2 | R | RAM | 0 | 1023 | N + 48 | acc_z | 2 | R | RAM | 0 | 1023 | N + 50 | present_voltage | 1 | R | RAM | 50 | 250 | N + \ No newline at end of file diff --git a/humanoid_robot_intelligence_control_system_device/devices/sensor/OPEN-CR.device b/humanoid_robot_intelligence_control_system_device/devices/sensor/OPEN-CR.device new file mode 100755 index 0000000..71cee8e --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/devices/sensor/OPEN-CR.device @@ -0,0 +1,30 @@ +[device info] +model_name = OPEN-CR +device_type = sensor + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 2 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 3 | ID | 1 | RW | EEPROM | 0 | 252 | N + 4 | baudrate | 1 | RW | EEPROM | 0 | 252 | N + 5 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 16 | status_return_level | 1 | RW | EEPROM | 0 | 2 | N + 24 | dynamixel_power | 1 | RW | RAM | 0 | 1 | N + 25 | LED | 1 | RW | RAM | 0 | 7 | N + 26 | LED_RGB | 2 | RW | RAM | 0 | 32767 | N + 28 | buzzer | 2 | RW | RAM | 0 | 65535 | N + 30 | button | 1 | R | RAM | 0 | 15 | N + 31 | present_voltage | 1 | R | RAM | 50 | 250 | N + 32 | gyro_x | 2 | R | RAM | -32800 | 32800 | Y + 34 | gyro_y | 2 | R | RAM | -32800 | 32800 | Y + 36 | gyro_z | 2 | R | RAM | -32800 | 32800 | Y + 38 | acc_x | 2 | R | RAM | -32768 | 32768 | Y + 40 | acc_y | 2 | R | RAM | -32768 | 32768 | Y + 42 | acc_z | 2 | R | RAM | -32768 | 32768 | Y + 44 | roll | 2 | R | RAM | 0 | 4096 | N + 46 | pitch | 2 | R | RAM | 0 | 4096 | N + 48 | yaw | 2 | R | RAM | 0 | 4096 | N + 50 | imu_control | 1 | RW | RAM | 0 | 255 | N + + \ No newline at end of file diff --git a/humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device/control_table_item.h b/humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device/control_table_item.h new file mode 100755 index 0000000..9b0a878 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device/control_table_item.h @@ -0,0 +1,54 @@ +/******************************************************************************* + * Copyright 2018 ROBOTIS CO., LTD. + * + * 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. + *******************************************************************************/ + +/* + * control_table_item.h + * + * Created on: 2015. 12. 16. + * Author: zerom + */ + +#ifndef ROBOTIS_DEVICE_CONTROL_TABLE_ITEM_H_ +#define ROBOTIS_DEVICE_CONTROL_TABLE_ITEM_H_ + +#include + +namespace humanoid_robot_intelligence_control_system_framework { + +enum AccessType { Read, ReadWrite }; + +enum MemoryType { EEPROM, RAM }; + +class ControlTableItem { +public: + std::string item_name_; + uint16_t address_; + AccessType access_type_; + MemoryType memory_type_; + uint8_t data_length_; + int32_t data_min_value_; + int32_t data_max_value_; + bool is_signed_; + + ControlTableItem() + : item_name_(""), address_(0), access_type_(Read), memory_type_(RAM), + data_length_(0), data_min_value_(0), data_max_value_(0), + is_signed_(false) {} +}; + +} // namespace humanoid_robot_intelligence_control_system_framework + +#endif /* ROBOTIS_DEVICE_CONTROL_TABLE_ITEM_H_ */ diff --git a/humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device/device.h b/humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device/device.h new file mode 100755 index 0000000..c14deb0 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device/device.h @@ -0,0 +1,54 @@ +/******************************************************************************* +* Copyright 2018 ROBOTIS CO., LTD. +* +* 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. +*******************************************************************************/ + +/* + * device.h + * + * Created on: 2016. 5. 12. + * Author: zerom + */ + +#ifndef ROBOTIS_DEVICE_DEVICE_H_ +#define ROBOTIS_DEVICE_DEVICE_H_ + + +#include +#include +#include + +#include "control_table_item.h" + +namespace humanoid_robot_intelligence_control_system_framework +{ + +class Device +{ +public: + uint8_t id_; + float protocol_version_; + std::string model_name_; + std::string port_name_; + + std::map ctrl_table_; + std::vector bulk_read_items_; + + virtual ~Device() { } +}; + +} + + +#endif /* ROBOTIS_DEVICE_DEVICE_H_ */ diff --git a/humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device/dynamixel.h b/humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device/dynamixel.h new file mode 100755 index 0000000..329b626 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device/dynamixel.h @@ -0,0 +1,83 @@ +/******************************************************************************* +* Copyright 2018 ROBOTIS CO., LTD. +* +* 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. +*******************************************************************************/ + +/* + * dynamixel.h + * + * Created on: 2015. 12. 8. + * Author: zerom + */ + +#ifndef ROBOTIS_DEVICE_DYNAMIXEL_H_ +#define ROBOTIS_DEVICE_DYNAMIXEL_H_ + + +#include +#include +#include + +#include "control_table_item.h" +#include "device.h" +#include "dynamixel_state.h" + +namespace humanoid_robot_intelligence_control_system_framework +{ + +class Dynamixel : public Device +{ +public: + std::string ctrl_module_name_; + DynamixelState *dxl_state_; + + double velocity_to_value_ratio_; + double torque_to_current_value_ratio_; + + int32_t value_of_0_radian_position_; + int32_t value_of_min_radian_position_; + int32_t value_of_max_radian_position_; + double min_radian_; + double max_radian_; + + ControlTableItem *torque_enable_item_; + ControlTableItem *present_position_item_; + ControlTableItem *present_velocity_item_; + ControlTableItem *present_current_item_; + ControlTableItem *goal_position_item_; + ControlTableItem *goal_velocity_item_; + ControlTableItem *goal_current_item_; + ControlTableItem *position_p_gain_item_; + ControlTableItem *position_i_gain_item_; + ControlTableItem *position_d_gain_item_; + ControlTableItem *velocity_p_gain_item_; + ControlTableItem *velocity_i_gain_item_; + ControlTableItem *velocity_d_gain_item_; + + Dynamixel(int id, std::string model_name, float protocol_version); + + double convertValue2Radian(int32_t value); + int32_t convertRadian2Value(double radian); + + double convertValue2Velocity(int32_t value); + int32_t convertVelocity2Value(double velocity); + + double convertValue2Torque(int16_t value); + int16_t convertTorque2Value(double torque); +}; + +} + + +#endif /* ROBOTIS_DEVICE_DYNAMIXEL_H_ */ diff --git a/humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device/dynamixel_state.h b/humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device/dynamixel_state.h new file mode 100755 index 0000000..b915824 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device/dynamixel_state.h @@ -0,0 +1,82 @@ +/******************************************************************************* +* Copyright 2018 ROBOTIS CO., LTD. +* +* 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. +*******************************************************************************/ + +/* + * dynamixel_state.h + * + * Created on: 2015. 12. 8. + * Author: zerom + */ + +#ifndef ROBOTIS_DEVICE_DYNAMIXEL_STATE_H_ +#define ROBOTIS_DEVICE_DYNAMIXEL_STATE_H_ + +#include + +#include "time_stamp.h" + +#define INDIRECT_DATA_1 "indirect_data_1" +#define INDIRECT_ADDRESS_1 "indirect_address_1" + +namespace humanoid_robot_intelligence_control_system_framework +{ + +class DynamixelState +{ +public: + TimeStamp update_time_stamp_; + + double present_position_; + double present_velocity_; + double present_torque_; + double goal_position_; + double goal_velocity_; + double goal_torque_; + int position_p_gain_; + int position_i_gain_; + int position_d_gain_; + int velocity_p_gain_; + int velocity_i_gain_; + int velocity_d_gain_; + + std::map bulk_read_table_; + + double position_offset_; + + DynamixelState() + : update_time_stamp_(0, 0), + present_position_(0.0), + present_velocity_(0.0), + present_torque_(0.0), + goal_position_(0.0), + goal_velocity_(0.0), + goal_torque_(0.0), + position_p_gain_(65535), + position_i_gain_(65535), + position_d_gain_(65535), + velocity_p_gain_(65535), + velocity_i_gain_(65535), + velocity_d_gain_(65535), + position_offset_(0) + { + bulk_read_table_.clear(); + } +}; + +} + + +#endif /* ROBOTIS_DEVICE_DYNAMIXEL_STATE_H_ */ diff --git a/humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device/robot.h b/humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device/robot.h new file mode 100755 index 0000000..15350bd --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device/robot.h @@ -0,0 +1,72 @@ +/******************************************************************************* +* Copyright 2018 ROBOTIS CO., LTD. +* +* 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. +*******************************************************************************/ + +/* + * robot.h + * + * Created on: 2015. 12. 11. + * Author: zerom + */ + +#ifndef ROBOTIS_DEVICE_ROBOT_H_ +#define ROBOTIS_DEVICE_ROBOT_H_ + + +#include + +#include "sensor.h" +#include "dynamixel.h" +#include "dynamixel_sdk/port_handler.h" + +#define DYNAMIXEL "dynamixel" +#define SENSOR "sensor" + +#define SESSION_CONTROL_INFO "control info" +#define SESSION_PORT_INFO "port info" +#define SESSION_DEVICE_INFO "device info" + +#define SESSION_TYPE_INFO "type info" +#define SESSION_CONTROL_TABLE "control table" + +#define DEFAULT_CONTROL_CYCLE 8 // milliseconds + +namespace humanoid_robot_intelligence_control_system_framework +{ + +class Robot +{ +private: + int control_cycle_msec_; + +public: + std::map ports_; // string: port name + std::map port_default_device_; // port name, default device name + + std::map dxls_; // string: joint name + std::map sensors_; // string: sensor name + + Robot(std::string robot_file_path, std::string dev_desc_dir_path); + + Sensor *getSensor(std::string path, int id, std::string port, float protocol_version); + Dynamixel *getDynamixel(std::string path, int id, std::string port, float protocol_version); + + int getControlCycle(); +}; + +} + + +#endif /* ROBOTIS_DEVICE_ROBOT_H_ */ diff --git a/humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device/sensor.h b/humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device/sensor.h new file mode 100755 index 0000000..32abe6c --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device/sensor.h @@ -0,0 +1,49 @@ +/******************************************************************************* +* Copyright 2018 ROBOTIS CO., LTD. +* +* 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. +*******************************************************************************/ + +/* + * sensor.h + * + * Created on: 2015. 12. 16. + * Author: zerom + */ + +#ifndef ROBOTIS_DEVICE_SENSOR_H_ +#define ROBOTIS_DEVICE_SENSOR_H_ + +#include +#include +#include + +#include "device.h" +#include "sensor_state.h" +#include "control_table_item.h" + +namespace humanoid_robot_intelligence_control_system_framework +{ + +class Sensor : public Device +{ +public: + SensorState *sensor_state_; + + Sensor(int id, std::string model_name, float protocol_version); +}; + +} + + +#endif /* ROBOTIS_DEVICE_SENSOR_H_ */ diff --git a/humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device/sensor_state.h b/humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device/sensor_state.h new file mode 100755 index 0000000..08f61ba --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device/sensor_state.h @@ -0,0 +1,50 @@ +/******************************************************************************* +* Copyright 2018 ROBOTIS CO., LTD. +* +* 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. +*******************************************************************************/ + +/* + * sensor_state.h + * + * Created on: 2016. 5. 16. + * Author: zerom + */ + +#ifndef ROBOTIS_DEVICE_SENSOR_STATE_H_ +#define ROBOTIS_DEVICE_SENSOR_STATE_H_ + + +#include "time_stamp.h" + +namespace humanoid_robot_intelligence_control_system_framework +{ + +class SensorState +{ +public: + TimeStamp update_time_stamp_; + + std::map bulk_read_table_; + + SensorState() + : update_time_stamp_(0, 0) + { + bulk_read_table_.clear(); + } +}; + +} + + +#endif /* ROBOTIS_DEVICE_SENSOR_STATE_H_ */ diff --git a/humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device/time_stamp.h b/humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device/time_stamp.h new file mode 100755 index 0000000..87f59f4 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/include/humanoid_robot_intelligence_control_system_device/time_stamp.h @@ -0,0 +1,39 @@ +/******************************************************************************* + * Copyright 2018 ROBOTIS CO., LTD. + * + * 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. + *******************************************************************************/ + +/* + * time_stamp.h + * + * Created on: 2016. 5. 16. + * Author: zerom + */ + +#ifndef ROBOTIS_DEVICE_TIME_STAMP_H_ +#define ROBOTIS_DEVICE_TIME_STAMP_H_ + +namespace humanoid_robot_intelligence_control_system_framework { + +class TimeStamp { +public: + long sec_; + long nsec_; + + TimeStamp(long sec, long nsec) : sec_(sec), nsec_(nsec) {} +}; + +} // namespace humanoid_robot_intelligence_control_system_framework + +#endif /* ROBOTIS_DEVICE_TIME_STAMP_H_ */ diff --git a/humanoid_robot_intelligence_control_system_device/package.xml b/humanoid_robot_intelligence_control_system_device/package.xml new file mode 100755 index 0000000..b21c17d --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/package.xml @@ -0,0 +1,38 @@ + + + humanoid_robot_intelligence_control_system_device + 0.3.2 + + The package that manages device information of ROBOTIS robots. + This package is used when reading device information with the robot information file + from the humanoid_robot_intelligence_control_system_controller package. + + Apache 2.0 + + Ronaldson Bellande + + + catkin + + roscpp + dynamixel_sdk + + roscpp + dynamixel_sdk + + roscpp + dynamixel_sdk + + + ament_cmake + + roscpp + dynamixel_sdk + + roscpp + dynamixel_sdk + + roscpp + dynamixel_sdk + + diff --git a/humanoid_robot_intelligence_control_system_device/src/humanoid_robot_intelligence_control_system_device/dynamixel.cpp b/humanoid_robot_intelligence_control_system_device/src/humanoid_robot_intelligence_control_system_device/dynamixel.cpp new file mode 100755 index 0000000..95a01ca --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/src/humanoid_robot_intelligence_control_system_device/dynamixel.cpp @@ -0,0 +1,119 @@ +/******************************************************************************* + * Copyright 2018 ROBOTIS CO., LTD. + * + * 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. + *******************************************************************************/ + +/* + * dynamixel.cpp + * + * Created on: 2015. 12. 8. + * Author: zerom + */ + +#include "humanoid_robot_intelligence_control_system_device/dynamixel.h" + +using namespace humanoid_robot_intelligence_control_system_framework; + +Dynamixel::Dynamixel(int id, std::string model_name, float protocol_version) + : ctrl_module_name_("none"), torque_to_current_value_ratio_(1.0), + velocity_to_value_ratio_(1.0), value_of_0_radian_position_(0), + value_of_min_radian_position_(0), value_of_max_radian_position_(0), + min_radian_(-3.14159265), max_radian_(3.14159265), torque_enable_item_(0), + present_position_item_(0), present_velocity_item_(0), + present_current_item_(0), goal_position_item_(0), goal_velocity_item_(0), + goal_current_item_(0), position_p_gain_item_(0), position_i_gain_item_(0), + position_d_gain_item_(0), velocity_p_gain_item_(0), + velocity_i_gain_item_(0), velocity_d_gain_item_(0) { + this->id_ = id; + this->model_name_ = model_name; + this->port_name_ = ""; + this->protocol_version_ = protocol_version; + + ctrl_table_.clear(); + dxl_state_ = new DynamixelState(); + + bulk_read_items_.clear(); +} + +double Dynamixel::convertValue2Radian(int32_t value) { + double radian = 0.0; + if (value > value_of_0_radian_position_) { + if (max_radian_ <= 0) + return max_radian_; + + radian = + (double)(value - value_of_0_radian_position_) * max_radian_ / + (double)(value_of_max_radian_position_ - value_of_0_radian_position_); + } else if (value < value_of_0_radian_position_) { + if (min_radian_ >= 0) + return min_radian_; + + radian = + (double)(value - value_of_0_radian_position_) * min_radian_ / + (double)(value_of_min_radian_position_ - value_of_0_radian_position_); + } + + // if (radian > max_radian_) + // return max_radian_; + // else if (radian < min_radian_) + // return min_radian_; + + return radian; +} + +int32_t Dynamixel::convertRadian2Value(double radian) { + int32_t value = 0; + if (radian > 0) { + if (value_of_max_radian_position_ <= value_of_0_radian_position_) + return value_of_max_radian_position_; + + value = (radian * + (value_of_max_radian_position_ - value_of_0_radian_position_) / + max_radian_) + + value_of_0_radian_position_; + } else if (radian < 0) { + if (value_of_min_radian_position_ >= value_of_0_radian_position_) + return value_of_min_radian_position_; + + value = (radian * + (value_of_min_radian_position_ - value_of_0_radian_position_) / + min_radian_) + + value_of_0_radian_position_; + } else + value = value_of_0_radian_position_; + + // if (value > value_of_max_radian_position_) + // return value_of_max_radian_position_; + // else if (value < value_of_min_radian_position_) + // return value_of_min_radian_position_; + + return value; +} + +double Dynamixel::convertValue2Velocity(int32_t value) { + return (double)value / velocity_to_value_ratio_; +} + +int32_t Dynamixel::convertVelocity2Value(double velocity) { + return (int32_t)(velocity * velocity_to_value_ratio_); + ; +} + +double Dynamixel::convertValue2Torque(int16_t value) { + return (double)value / torque_to_current_value_ratio_; +} + +int16_t Dynamixel::convertTorque2Value(double torque) { + return (int16_t)(torque * torque_to_current_value_ratio_); +} diff --git a/humanoid_robot_intelligence_control_system_device/src/humanoid_robot_intelligence_control_system_device/robot.cpp b/humanoid_robot_intelligence_control_system_device/src/humanoid_robot_intelligence_control_system_device/robot.cpp new file mode 100755 index 0000000..c930e86 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/src/humanoid_robot_intelligence_control_system_device/robot.cpp @@ -0,0 +1,480 @@ +/******************************************************************************* + * Copyright 2018 ROBOTIS CO., LTD. + * + * 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. + *******************************************************************************/ + +/* + * robot.cpp + * + * Created on: 2015. 12. 11. + * Author: zerom + */ + +#include +#include +#include + +#include "humanoid_robot_intelligence_control_system_device/robot.h" + +using namespace humanoid_robot_intelligence_control_system_framework; + +static inline std::string <rim(std::string &s) { + s.erase(s.begin(), + std::find_if(s.begin(), s.end(), + std::not1(std::ptr_fun(std::isspace)))); + return s; +} +static inline std::string &rtrim(std::string &s) { + s.erase(std::find_if(s.rbegin(), s.rend(), + std::not1(std::ptr_fun(std::isspace))) + .base(), + s.end()); + return s; +} +static inline std::string &trim(std::string &s) { return ltrim(rtrim(s)); } + +static inline std::vector split(const std::string &text, + char sep) { + std::vector tokens; + std::size_t start = 0, end = 0; + + while ((end = text.find(sep, start)) != (std::string::npos)) { + tokens.push_back(text.substr(start, end - start)); + trim(tokens.back()); + start = end + 1; + } + tokens.push_back(text.substr(start)); + trim(tokens.back()); + + return tokens; +} + +Robot::Robot(std::string robot_file_path, std::string dev_desc_dir_path) + : control_cycle_msec_(DEFAULT_CONTROL_CYCLE) { + if (dev_desc_dir_path.compare(dev_desc_dir_path.size() - 1, 1, "/") != 0) + dev_desc_dir_path += "/"; + + std::ifstream file(robot_file_path.c_str()); + if (file.is_open()) { + std::string session = ""; + std::string input_str; + while (!file.eof()) { + std::getline(file, input_str); + + // remove comment ( # ) + std::size_t pos = input_str.find("#"); + if (pos != std::string::npos) + input_str = input_str.substr(0, pos); + + // trim + input_str = trim(input_str); + + // find session + if (!input_str.compare(0, 1, "[") && + !input_str.compare(input_str.size() - 1, 1, "]")) { + input_str = input_str.substr(1, input_str.size() - 2); + std::transform(input_str.begin(), input_str.end(), input_str.begin(), + ::tolower); + session = trim(input_str); + continue; + } + + if (session == SESSION_CONTROL_INFO) { + std::vector tokens = split(input_str, '='); + if (tokens.size() != 2) + continue; + + if (tokens[0] == "control_cycle") + control_cycle_msec_ = std::atoi(tokens[1].c_str()); + } else if (session == SESSION_PORT_INFO) { + std::vector tokens = split(input_str, '|'); + if (tokens.size() != 3) + continue; + + std::cout << tokens[0] << " added. (baudrate: " << tokens[1] << ")" + << std::endl; + + ports_[tokens[0]] = + dynamixel::PortHandler::getPortHandler(tokens[0].c_str()); + ports_[tokens[0]]->setBaudRate(std::atoi(tokens[1].c_str())); + port_default_device_[tokens[0]] = tokens[2]; + } else if (session == SESSION_DEVICE_INFO) { + std::vector tokens = split(input_str, '|'); + if (tokens.size() != 7) + continue; + + if (tokens[0] == DYNAMIXEL) { + std::string file_path = + dev_desc_dir_path + tokens[0] + "/" + tokens[3] + ".device"; + int id = std::atoi(tokens[2].c_str()); + std::string port = tokens[1]; + float protocol = std::atof(tokens[4].c_str()); + std::string dev_name = tokens[5]; + + dxls_[dev_name] = getDynamixel(file_path, id, port, protocol); + + Dynamixel *dxl = dxls_[dev_name]; + std::vector sub_tokens = split(tokens[6], ','); + if (sub_tokens.size() > 0 && sub_tokens[0] != "") { + std::map::iterator indirect_it = + dxl->ctrl_table_.find(INDIRECT_ADDRESS_1); + if (indirect_it != + dxl->ctrl_table_.end()) // INDIRECT_ADDRESS_1 exist + { + uint16_t indirect_data_addr = + dxl->ctrl_table_[INDIRECT_DATA_1]->address_; + for (int _i = 0; _i < sub_tokens.size(); _i++) { + std::map::iterator + bulkread_it = dxl->ctrl_table_.find(sub_tokens[_i]); + if (bulkread_it == dxl->ctrl_table_.end()) { + fprintf( + stderr, + "\n ##### BULK READ ITEM [ %s ] NOT FOUND!! #####\n\n", + sub_tokens[_i].c_str()); + continue; + } + + dxl->bulk_read_items_.push_back(new ControlTableItem()); + ControlTableItem *dest_item = dxl->bulk_read_items_[_i]; + ControlTableItem *src_item = dxl->ctrl_table_[sub_tokens[_i]]; + + dest_item->item_name_ = src_item->item_name_; + dest_item->address_ = indirect_data_addr; + dest_item->access_type_ = src_item->access_type_; + dest_item->memory_type_ = src_item->memory_type_; + dest_item->data_length_ = src_item->data_length_; + dest_item->data_min_value_ = src_item->data_min_value_; + dest_item->data_max_value_ = src_item->data_max_value_; + dest_item->is_signed_ = src_item->is_signed_; + + indirect_data_addr += dest_item->data_length_; + } + } else // INDIRECT_ADDRESS_1 not exist + { + for (int i = 0; i < sub_tokens.size(); i++) { + if (dxl->ctrl_table_[sub_tokens[i]] != NULL) + dxl->bulk_read_items_.push_back( + dxl->ctrl_table_[sub_tokens[i]]); + } + } + } + } else if (tokens[0] == SENSOR) { + std::string file_path = + dev_desc_dir_path + tokens[0] + "/" + tokens[3] + ".device"; + int id = std::atoi(tokens[2].c_str()); + std::string port = tokens[1]; + float protocol = std::atof(tokens[4].c_str()); + std::string dev_name = tokens[5]; + + sensors_[dev_name] = getSensor(file_path, id, port, protocol); + + Sensor *sensor = sensors_[dev_name]; + std::vector sub_tokens = split(tokens[6], ','); + if (sub_tokens.size() > 0 && sub_tokens[0] != "") { + std::map::iterator indirect_it = + sensor->ctrl_table_.find(INDIRECT_ADDRESS_1); + if (indirect_it != + sensor->ctrl_table_.end()) // INDIRECT_ADDRESS_1 exist + { + uint16_t indirect_data_addr = + sensor->ctrl_table_[INDIRECT_DATA_1]->address_; + for (int i = 0; i < sub_tokens.size(); i++) { + sensor->bulk_read_items_.push_back(new ControlTableItem()); + ControlTableItem *dest_item = sensor->bulk_read_items_[i]; + ControlTableItem *src_item = sensor->ctrl_table_[sub_tokens[i]]; + + dest_item->item_name_ = src_item->item_name_; + dest_item->address_ = indirect_data_addr; + dest_item->access_type_ = src_item->access_type_; + dest_item->memory_type_ = src_item->memory_type_; + dest_item->data_length_ = src_item->data_length_; + dest_item->data_min_value_ = src_item->data_min_value_; + dest_item->data_max_value_ = src_item->data_max_value_; + dest_item->is_signed_ = src_item->is_signed_; + + indirect_data_addr += dest_item->data_length_; + } + } else // INDIRECT_ADDRESS_1 exist + { + for (int i = 0; i < sub_tokens.size(); i++) + sensor->bulk_read_items_.push_back( + sensor->ctrl_table_[sub_tokens[i]]); + } + } + } + } + } + file.close(); + } else { + std::cout << "Unable to open file : " + robot_file_path << std::endl; + } +} + +Sensor *Robot::getSensor(std::string path, int id, std::string port, + float protocol_version) { + Sensor *sensor; + + // load file model_name.device + std::ifstream file(path.c_str()); + if (file.is_open()) { + std::string session = ""; + std::string input_str; + + while (!file.eof()) { + std::getline(file, input_str); + + // remove comment ( # ) + std::size_t pos = input_str.find("#"); + if (pos != std::string::npos) + input_str = input_str.substr(0, pos); + + // trim + input_str = trim(input_str); + if (input_str == "") + continue; + + // find _session + if (!input_str.compare(0, 1, "[") && + !input_str.compare(input_str.size() - 1, 1, "]")) { + input_str = input_str.substr(1, input_str.size() - 2); + std::transform(input_str.begin(), input_str.end(), input_str.begin(), + ::tolower); + session = trim(input_str); + continue; + } + + if (session == SESSION_DEVICE_INFO) { + std::vector tokens = split(input_str, '='); + if (tokens.size() != 2) + continue; + + if (tokens[0] == "model_name") + sensor = new Sensor(id, tokens[1], protocol_version); + } else if (session == SESSION_CONTROL_TABLE) { + std::vector tokens = split(input_str, '|'); + if (tokens.size() != 8) + continue; + + ControlTableItem *item = new ControlTableItem(); + item->item_name_ = tokens[1]; + item->address_ = std::atoi(tokens[0].c_str()); + item->data_length_ = std::atoi(tokens[2].c_str()); + + if (tokens[3] == "R") + item->access_type_ = Read; + else if (tokens[3] == "RW") + item->access_type_ = ReadWrite; + + if (tokens[4] == "EEPROM") + item->memory_type_ = EEPROM; + else if (tokens[4] == "RAM") + item->memory_type_ = RAM; + + item->data_min_value_ = std::atol(tokens[5].c_str()); + item->data_max_value_ = std::atol(tokens[6].c_str()); + + if (tokens[7] == "Y") + item->is_signed_ = true; + else if (tokens[7] == "N") + item->is_signed_ = false; + sensor->ctrl_table_[tokens[1]] = item; + } + } + sensor->port_name_ = port; + + fprintf(stderr, "(%s) [ID:%3d] %14s added. \n", port.c_str(), sensor->id_, + sensor->model_name_.c_str()); + // std::cout << "[ID:" << (int)(_sensor->id) << "] " << _sensor->model_name + // << " added. (" << port << ")" << std::endl; + file.close(); + } else + std::cout << "Unable to open file : " + path << std::endl; + + return sensor; +} + +Dynamixel *Robot::getDynamixel(std::string path, int id, std::string port, + float protocol_version) { + Dynamixel *dxl; + + // load file model_name.device + std::ifstream file(path.c_str()); + if (file.is_open()) { + std::string session = ""; + std::string input_str; + + std::string torque_enable_item_name = ""; + std::string present_position_item_name = ""; + std::string present_velocity_item_name = ""; + std::string present_current_item_name = ""; + std::string goal_position_item_name = ""; + std::string goal_velocity_item_name = ""; + std::string goal_current_item_name = ""; + std::string position_d_gain_item_name = ""; + std::string position_i_gain_item_name = ""; + std::string position_p_gain_item_name = ""; + std::string velocity_d_gain_item_name = ""; + std::string velocity_i_gain_item_name = ""; + std::string velocity_p_gain_item_name = ""; + + while (!file.eof()) { + std::getline(file, input_str); + + // remove comment ( # ) + std::size_t pos = input_str.find("#"); + if (pos != std::string::npos) + input_str = input_str.substr(0, pos); + + // trim + input_str = trim(input_str); + if (input_str == "") + continue; + + // find session + if (!input_str.compare(0, 1, "[") && + !input_str.compare(input_str.size() - 1, 1, "]")) { + input_str = input_str.substr(1, input_str.size() - 2); + std::transform(input_str.begin(), input_str.end(), input_str.begin(), + ::tolower); + session = trim(input_str); + continue; + } + + if (session == SESSION_DEVICE_INFO) { + std::vector tokens = split(input_str, '='); + if (tokens.size() != 2) + continue; + + if (tokens[0] == "model_name") + dxl = new Dynamixel(id, tokens[1], protocol_version); + } else if (session == SESSION_TYPE_INFO) { + std::vector tokens = split(input_str, '='); + if (tokens.size() != 2) + continue; + + if (tokens[0] == "torque_to_current_value_ratio") + dxl->torque_to_current_value_ratio_ = std::atof(tokens[1].c_str()); + else if (tokens[0] == "velocity_to_value_ratio") + dxl->velocity_to_value_ratio_ = std::atof(tokens[1].c_str()); + + else if (tokens[0] == "value_of_0_radian_position") + dxl->value_of_0_radian_position_ = std::atoi(tokens[1].c_str()); + else if (tokens[0] == "value_of_min_radian_position") + dxl->value_of_min_radian_position_ = std::atoi(tokens[1].c_str()); + else if (tokens[0] == "value_of_max_radian_position") + dxl->value_of_max_radian_position_ = std::atoi(tokens[1].c_str()); + else if (tokens[0] == "min_radian") + dxl->min_radian_ = std::atof(tokens[1].c_str()); + else if (tokens[0] == "max_radian") + dxl->max_radian_ = std::atof(tokens[1].c_str()); + + else if (tokens[0] == "torque_enable_item_name") + torque_enable_item_name = tokens[1]; + else if (tokens[0] == "present_position_item_name") + present_position_item_name = tokens[1]; + else if (tokens[0] == "present_velocity_item_name") + present_velocity_item_name = tokens[1]; + else if (tokens[0] == "present_current_item_name") + present_current_item_name = tokens[1]; + else if (tokens[0] == "goal_position_item_name") + goal_position_item_name = tokens[1]; + else if (tokens[0] == "goal_velocity_item_name") + goal_velocity_item_name = tokens[1]; + else if (tokens[0] == "goal_current_item_name") + goal_current_item_name = tokens[1]; + else if (tokens[0] == "position_d_gain_item_name") + position_d_gain_item_name = tokens[1]; + else if (tokens[0] == "position_i_gain_item_name") + position_i_gain_item_name = tokens[1]; + else if (tokens[0] == "position_p_gain_item_name") + position_p_gain_item_name = tokens[1]; + else if (tokens[0] == "velocity_d_gain_item_name") + velocity_d_gain_item_name = tokens[1]; + else if (tokens[0] == "velocity_i_gain_item_name") + velocity_i_gain_item_name = tokens[1]; + else if (tokens[0] == "velocity_p_gain_item_name") + velocity_p_gain_item_name = tokens[1]; + } else if (session == SESSION_CONTROL_TABLE) { + std::vector tokens = split(input_str, '|'); + if (tokens.size() != 8) + continue; + + ControlTableItem *item = new ControlTableItem(); + item->item_name_ = tokens[1]; + item->address_ = std::atoi(tokens[0].c_str()); + item->data_length_ = std::atoi(tokens[2].c_str()); + + if (tokens[3] == "R") + item->access_type_ = Read; + else if (tokens[3] == "RW") + item->access_type_ = ReadWrite; + + if (tokens[4] == "EEPROM") + item->memory_type_ = EEPROM; + else if (tokens[4] == "RAM") + item->memory_type_ = RAM; + + item->data_min_value_ = std::atol(tokens[5].c_str()); + item->data_max_value_ = std::atol(tokens[6].c_str()); + + if (tokens[7] == "Y") + item->is_signed_ = true; + else if (tokens[7] == "N") + item->is_signed_ = false; + dxl->ctrl_table_[tokens[1]] = item; + } + } + dxl->port_name_ = port; + + if (dxl->ctrl_table_[torque_enable_item_name] != NULL) + dxl->torque_enable_item_ = dxl->ctrl_table_[torque_enable_item_name]; + if (dxl->ctrl_table_[present_position_item_name] != NULL) + dxl->present_position_item_ = + dxl->ctrl_table_[present_position_item_name]; + if (dxl->ctrl_table_[present_velocity_item_name] != NULL) + dxl->present_velocity_item_ = + dxl->ctrl_table_[present_velocity_item_name]; + if (dxl->ctrl_table_[present_current_item_name] != NULL) + dxl->present_current_item_ = dxl->ctrl_table_[present_current_item_name]; + if (dxl->ctrl_table_[goal_position_item_name] != NULL) + dxl->goal_position_item_ = dxl->ctrl_table_[goal_position_item_name]; + if (dxl->ctrl_table_[goal_velocity_item_name] != NULL) + dxl->goal_velocity_item_ = dxl->ctrl_table_[goal_velocity_item_name]; + if (dxl->ctrl_table_[goal_current_item_name] != NULL) + dxl->goal_current_item_ = dxl->ctrl_table_[goal_current_item_name]; + if (dxl->ctrl_table_[position_d_gain_item_name] != NULL) + dxl->position_d_gain_item_ = dxl->ctrl_table_[position_d_gain_item_name]; + if (dxl->ctrl_table_[position_i_gain_item_name] != NULL) + dxl->position_i_gain_item_ = dxl->ctrl_table_[position_i_gain_item_name]; + if (dxl->ctrl_table_[position_p_gain_item_name] != NULL) + dxl->position_p_gain_item_ = dxl->ctrl_table_[position_p_gain_item_name]; + if (dxl->ctrl_table_[velocity_d_gain_item_name] != NULL) + dxl->velocity_d_gain_item_ = dxl->ctrl_table_[velocity_d_gain_item_name]; + if (dxl->ctrl_table_[velocity_i_gain_item_name] != NULL) + dxl->velocity_i_gain_item_ = dxl->ctrl_table_[velocity_i_gain_item_name]; + if (dxl->ctrl_table_[velocity_p_gain_item_name] != NULL) + dxl->velocity_p_gain_item_ = dxl->ctrl_table_[velocity_p_gain_item_name]; + + fprintf(stderr, "(%s) [ID:%3d] %14s added. \n", port.c_str(), dxl->id_, + dxl->model_name_.c_str()); + // std::cout << "[ID:" << (int)(_dxl->id) << "] " << _dxl->model_name << " + // added. (" << port << ")" << std::endl; + file.close(); + } else + std::cout << "Unable to open file : " + path << std::endl; + + return dxl; +} + +int Robot::getControlCycle() { return control_cycle_msec_; } diff --git a/humanoid_robot_intelligence_control_system_device/src/humanoid_robot_intelligence_control_system_device/sensor.cpp b/humanoid_robot_intelligence_control_system_device/src/humanoid_robot_intelligence_control_system_device/sensor.cpp new file mode 100755 index 0000000..6d5e619 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_device/src/humanoid_robot_intelligence_control_system_device/sensor.cpp @@ -0,0 +1,39 @@ +/******************************************************************************* + * Copyright 2018 ROBOTIS CO., LTD. + * + * 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. + *******************************************************************************/ + +/* + * sensor.cpp + * + * Created on: 2016. 5. 11. + * Author: zerom + */ + +#include "humanoid_robot_intelligence_control_system_device/sensor.h" + +using namespace humanoid_robot_intelligence_control_system_framework; + +Sensor::Sensor(int id, std::string model_name, float protocol_version) { + + this->id_ = id; + this->model_name_ = model_name; + this->port_name_ = ""; + this->protocol_version_ = protocol_version; + ctrl_table_.clear(); + + sensor_state_ = new SensorState(); + + bulk_read_items_.clear(); +} diff --git a/humanoid_robot_intelligence_control_system_framework/CHANGELOG.rst b/humanoid_robot_intelligence_control_system_framework/CHANGELOG.rst new file mode 100755 index 0000000..5376392 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_framework/CHANGELOG.rst @@ -0,0 +1,89 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package humanoid_robot_intelligence_control_system_framework +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.3.2 (2023-10-03) +------------------ +* Make it compatible for ROS1/ROS2 +* Fix bugs +* Update package.xml and CMakeList.txt for to the latest versions +* Contributors & Maintainer: Ronaldson Bellande + +0.3.1 (2023-09-27) +------------------ +* Starting from this point it under a new license +* Fix errors and Issues +* Rename Repository for a completely different purpose +* Make it compatible with ROS/ROS2 +* Upgrade version of all builds and make it more compatible +* Update package.xml and CMakeList.txt for to the latest versions +* Contributors & Maintainer: Ronaldson Bellande + +0.2.9 (2018-03-22) +------------------ +* added serivce for setting module +* deleted comment for debug +* modified to prevent duplicate indirect address write +* added boost system dependencies +* fixed a bug that occure when handling bulk read item that does not exist +* Contributors: Kayman, Zerom, Pyo + +0.2.8 (2018-03-20) +------------------ +* added RH-P12-RN.device file +* modified CMakeLists.txt for system dependencies (yaml-cpp) +* Contributors: Zerom, Pyo + +0.2.7 (2018-03-15) +------------------ +* changed all values read by bulk read are saved to dxl_state\_->bulk_read_table\_. +* Modified to prevent duplicate indirect address write +* fixed a bug that occur when handling bulk read item that does not exist +* changed the License and package format to version 2 +* Contributors: SCH, Zerom, Pyo + +0.2.6 (2017-08-09) +------------------ +* multi thread bug fixed. +* unnecessary debug print removed. +* OpenCR control table item name changed. (torque_enable -> dynamixel_power) +* fixed to not update update_time_stamp\_ if bulk read fails. +* Contributors: Zerom + +0.2.5 (2017-06-09) +------------------ +* updated for yaml-cpp dependencies (humanoid_robot_intelligence_control_system_controller) +* Contributors: SCH + +0.2.4 (2017-06-07) +------------------ +* added cmake_modules in package.xml +* Contributors: SCH + +0.2.3 (2017-05-23) +------------------ +* updated the cmake file for ros install +* Contributors: SCH + +0.2.2 (2017-04-24) +------------------ +* added a deivce: OpenCR +* updated humanoid_robot_intelligence_control_system_controller.cpp +* changed to read control cycle from .robot file +* Contributors: Zerom, Kayman + +0.2.1 (2016-11-23) +------------------ +* Merge the changes and update + +0.2.0 (2016-08-31) +------------------ +* updated CHANGLOG.rst for minor release + +0.1.1 (2016-08-18) +------------------ +* updated the package information + +0.1.0 (2016-08-12) +------------------ +* make a meta-package diff --git a/humanoid_robot_intelligence_control_system_framework/CMakeLists.txt b/humanoid_robot_intelligence_control_system_framework/CMakeLists.txt new file mode 100644 index 0000000..dda922c --- /dev/null +++ b/humanoid_robot_intelligence_control_system_framework/CMakeLists.txt @@ -0,0 +1,12 @@ +cmake_minimum_required(VERSION 3.8) +project(humanoid_robot_intelligence_control_system_framework) + +if($ENV{ROS_VERSION} EQUAL 1) + find_package(catkin REQUIRED) +else() + find_package(ament_cmake REQUIRED) +endif() + +if($ENV{ROS_VERSION} EQUAL 1) + catkin_package() +endif() diff --git a/humanoid_robot_intelligence_control_system_framework/package.xml b/humanoid_robot_intelligence_control_system_framework/package.xml new file mode 100755 index 0000000..39be137 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_framework/package.xml @@ -0,0 +1,40 @@ + + + humanoid_robot_intelligence_control_system_framework + 0.3.2 + ROS packages for the humanoid_robot_intelligence_control_system_framework (meta package) + Apache 2.0 + Ronaldson Bellande + + + catkin + + humanoid_robot_intelligence_control_system_controller + humanoid_robot_intelligence_control_system_device + humanoid_robot_intelligence_control_system_framework_common + + humanoid_robot_intelligence_control_system_controller + humanoid_robot_intelligence_control_system_device + humanoid_robot_intelligence_control_system_framework_common + + humanoid_robot_intelligence_control_system_controller + humanoid_robot_intelligence_control_system_device + humanoid_robot_intelligence_control_system_framework_common + + + ament_cmake + + humanoid_robot_intelligence_control_system_controller + humanoid_robot_intelligence_control_system_device + humanoid_robot_intelligence_control_system_framework_common + + humanoid_robot_intelligence_control_system_controller + humanoid_robot_intelligence_control_system_device + humanoid_robot_intelligence_control_system_framework_common + + humanoid_robot_intelligence_control_system_controller + humanoid_robot_intelligence_control_system_device + humanoid_robot_intelligence_control_system_framework_common + + + diff --git a/humanoid_robot_intelligence_control_system_framework_common/CHANGELOG.rst b/humanoid_robot_intelligence_control_system_framework_common/CHANGELOG.rst new file mode 100755 index 0000000..0af7c89 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_framework_common/CHANGELOG.rst @@ -0,0 +1,82 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package humanoid_robot_intelligence_control_system_framework_common +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.3.2 (2023-10-03) +------------------ +* Make it compatible for ROS1/ROS2 +* Fix bugs +* Update package.xml and CMakeList.txt for to the latest versions +* Contributors & Maintainer: Ronaldson Bellande + +0.3.1 (2023-09-27) +------------------ +* Starting from this point it under a new license +* Fix errors and Issues +* Rename Repository for a completely different purpose +* Make it compatible with ROS/ROS2 +* Upgrade version of all builds and make it more compatible +* Update package.xml and CMakeList.txt for to the latest versions +* Contributors & Maintainer: Ronaldson Bellande + +0.2.9 (2018-03-22) +------------------ +* none + +0.2.8 (2018-03-20) +------------------ +* tested for system dependencies +* Contributors: Pyo + +0.2.7 (2018-03-15) +------------------ +* change the License and package format to version 2 +* Contributors: Pyo + +0.2.6 (2017-08-09) +------------------ +* none + +0.2.5 (2017-06-09) +------------------ +* none + +0.2.4 (2017-06-07) +------------------ +* none + +0.2.3 (2017-05-23) +------------------ +* updated the cmake file for ros install +* Contributors: SCH + +0.2.2 (2017-04-24) +------------------ +* updated for other packages +* Contributors: Zerom, Kayman + +0.2.1 (2016-11-23) +------------------ +* Merge the changes and update +* - dependencies fixed. (Pull requests `#26 `_) +* - modified dependency problem. +* Contributors: Jay Song, Pyo, Zerom + +0.2.0 (2016-08-31) +------------------ +* updated CHANGLOG.rst for minor release +* rename ControlMode(CurrentControl -> TorqueControl) +* rename (port_to_sync_write_torque\_ -> port_to_sync_write_current\_) +* Contributors: Zerom, Pyo + +0.1.1 (2016-08-18) +------------------ +* updated the package information + +0.1.0 (2016-08-12) +------------------ +* modified the package information for release +* Setting the license to BSD. +* add SensorState + add Singleton template +* Contributors: Jay Song, Zerom, Pyo diff --git a/humanoid_robot_intelligence_control_system_framework_common/CMakeLists.txt b/humanoid_robot_intelligence_control_system_framework_common/CMakeLists.txt new file mode 100755 index 0000000..d90d27c --- /dev/null +++ b/humanoid_robot_intelligence_control_system_framework_common/CMakeLists.txt @@ -0,0 +1,46 @@ +cmake_minimum_required(VERSION 3.8) +project(humanoid_robot_intelligence_control_system_framework_common) + + +if($ENV{ROS_VERSION} EQUAL 1) + find_package(catkin REQUIRED COMPONENTS + roscpp + humanoid_robot_intelligence_control_system_device + ) +else() + find_package(ament_cmake REQUIRED) +endif() + + +if($ENV{ROS_VERSION} EQUAL 1) + catkin_package( + INCLUDE_DIRS include + LIBRARIES ${PROJECT_NAME} + CATKIN_DEPENDS + roscpp + humanoid_robot_intelligence_control_system_device + ) +endif() + + +include_directories( + include + ${catkin_INCLUDE_DIRS} +) + +add_library(${PROJECT_NAME} + include/${PROJECT_NAME}/motion_module.h + include/${PROJECT_NAME}/sensor_module.h + include/${PROJECT_NAME}/singleton.h +) +set_target_properties(${PROJECT_NAME} PROPERTIES LINKER_LANGUAGE CXX) + +install(TARGETS humanoid_robot_intelligence_control_system_framework_common + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) diff --git a/humanoid_robot_intelligence_control_system_framework_common/include/humanoid_robot_intelligence_control_system_framework_common/motion_module.h b/humanoid_robot_intelligence_control_system_framework_common/include/humanoid_robot_intelligence_control_system_framework_common/motion_module.h new file mode 100755 index 0000000..9df7326 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_framework_common/include/humanoid_robot_intelligence_control_system_framework_common/motion_module.h @@ -0,0 +1,87 @@ +/******************************************************************************* +* Copyright 2018 ROBOTIS CO., LTD. +* +* 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. +*******************************************************************************/ + +/* + * motion_module.h + * + * Created on: 2016. 1. 15. + * Author: zerom + */ + +#ifndef ROBOTIS_FRAMEWORK_COMMON_MOTION_MODULE_H_ +#define ROBOTIS_FRAMEWORK_COMMON_MOTION_MODULE_H_ + + +#include +#include + +#include "singleton.h" +#include "humanoid_robot_intelligence_control_system_device/robot.h" +#include "humanoid_robot_intelligence_control_system_device/dynamixel.h" + +namespace humanoid_robot_intelligence_control_system_framework +{ + +enum ControlMode +{ + PositionControl, + VelocityControl, + TorqueControl +}; + +class MotionModule +{ +protected: + bool enable_; + std::string module_name_; + ControlMode control_mode_; + +public: + std::map result_; + + virtual ~MotionModule() { } + + std::string getModuleName() { return module_name_; } + ControlMode getControlMode() { return control_mode_; } + + void setModuleEnable(bool enable) + { + if(this->enable_ == enable) + return; + + this->enable_ = enable; + if(enable) + onModuleEnable(); + else + onModuleDisable(); + } + bool getModuleEnable() { return enable_; } + + virtual void onModuleEnable() { } + virtual void onModuleDisable() { } + + virtual void initialize(const int control_cycle_msec, Robot *robot) = 0; + virtual void process(std::map dxls, std::map sensors) = 0; + + virtual void stop() = 0; + virtual bool isRunning() = 0; +}; + + +} + + +#endif /* ROBOTIS_FRAMEWORK_COMMON_MOTION_MODULE_H_ */ diff --git a/humanoid_robot_intelligence_control_system_framework_common/include/humanoid_robot_intelligence_control_system_framework_common/sensor_module.h b/humanoid_robot_intelligence_control_system_framework_common/include/humanoid_robot_intelligence_control_system_framework_common/sensor_module.h new file mode 100755 index 0000000..23e6292 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_framework_common/include/humanoid_robot_intelligence_control_system_framework_common/sensor_module.h @@ -0,0 +1,57 @@ +/******************************************************************************* +* Copyright 2018 ROBOTIS CO., LTD. +* +* 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. +*******************************************************************************/ + +/* + * sensor_module.h + * + * Created on: 2016. 3. 30. + * Author: zerom + */ + +#ifndef ROBOTIS_FRAMEWORK_COMMON_SENSOR_MODULE_H_ +#define ROBOTIS_FRAMEWORK_COMMON_SENSOR_MODULE_H_ + + +#include +#include + +#include "singleton.h" +#include "humanoid_robot_intelligence_control_system_device/robot.h" +#include "humanoid_robot_intelligence_control_system_device/dynamixel.h" + +namespace humanoid_robot_intelligence_control_system_framework +{ + +class SensorModule +{ +protected: + std::string module_name_; + +public: + std::map result_; + + virtual ~SensorModule() { } + + std::string getModuleName() { return module_name_; } + + virtual void initialize(const int control_cycle_msec, Robot *robot) = 0; + virtual void process(std::map dxls, std::map sensors) = 0; +}; + +} + + +#endif /* ROBOTIS_FRAMEWORK_COMMON_SENSOR_MODULE_H_ */ diff --git a/humanoid_robot_intelligence_control_system_framework_common/include/humanoid_robot_intelligence_control_system_framework_common/singleton.h b/humanoid_robot_intelligence_control_system_framework_common/include/humanoid_robot_intelligence_control_system_framework_common/singleton.h new file mode 100755 index 0000000..5f1c4ab --- /dev/null +++ b/humanoid_robot_intelligence_control_system_framework_common/include/humanoid_robot_intelligence_control_system_framework_common/singleton.h @@ -0,0 +1,65 @@ +/******************************************************************************* +* Copyright 2018 ROBOTIS CO., LTD. +* +* 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. +*******************************************************************************/ + +/* + * singleton.h + * + * Created on: 2016. 5. 17. + * Author: zerom + */ + +#ifndef ROBOTIS_FRAMEWORK_COMMON_SINGLETON_H_ +#define ROBOTIS_FRAMEWORK_COMMON_SINGLETON_H_ + + +namespace humanoid_robot_intelligence_control_system_framework +{ + +template +class Singleton +{ +private: + static T *unique_instance_; + +protected: + Singleton() { } + Singleton(Singleton const&) { } + Singleton& operator=(Singleton const&) { return *this; } + +public: + static T* getInstance() + { + if(unique_instance_ == NULL) + unique_instance_ = new T; + return unique_instance_; + } + + static void destroyInstance() + { + if(unique_instance_) + { + delete unique_instance_; + unique_instance_ = NULL; + } + } +}; + +template T* Singleton::unique_instance_ = NULL; + +} + + +#endif /* ROBOTIS_FRAMEWORK_COMMON_SINGLETON_H_ */ diff --git a/humanoid_robot_intelligence_control_system_framework_common/package.xml b/humanoid_robot_intelligence_control_system_framework_common/package.xml new file mode 100755 index 0000000..c12c3b2 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_framework_common/package.xml @@ -0,0 +1,33 @@ + + + humanoid_robot_intelligence_control_system_framework_common + 0.3.2 + The package contains commonly used headers for the humanoid_robot_intelligence_control_system_framework + Apache 2.0 + Ronaldson Bellande + + + catkin + + roscpp + humanoid_robot_intelligence_control_system_device + + roscpp + humanoid_robot_intelligence_control_system_device + + roscpp + humanoid_robot_intelligence_control_system_device + + + ament_cmake + + roscpp + humanoid_robot_intelligence_control_system_device + + roscpp + humanoid_robot_intelligence_control_system_device + + roscpp + humanoid_robot_intelligence_control_system_device + + From d7cfd1c504062e3827a9ed47286c6d546009d370 Mon Sep 17 00:00:00 2001 From: Ronaldson Bellande <47253433+RonaldsonBellande@users.noreply.github.com> Date: Thu, 30 Nov 2023 20:17:06 -0500 Subject: [PATCH 214/238] Update README.md --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 8b0d5d4..0e48dcd 100755 --- a/README.md +++ b/README.md @@ -1,4 +1,4 @@ -# ROS/ROS2 Humanoid Robot Framework +# ROS/ROS2 Humanoid Robot Intelligence Control System Framework [![Website](https://img.shields.io/badge/Visit%20our-Website-0099cc?style=for-the-badge)](https://robotics-sensors.github.io) [![Discord](https://img.shields.io/badge/Join%20our-Discord-7289DA?logo=discord&style=for-the-badge)](https://discord.gg/Yc72nd4w) From dcbc776b0b5d15992a3660ab9faf5a119d0dc2c4 Mon Sep 17 00:00:00 2001 From: Ronaldson Bellande <47253433+RonaldsonBellande@users.noreply.github.com> Date: Thu, 30 Nov 2023 20:17:06 -0500 Subject: [PATCH 215/238] Update README.md --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 8b0d5d4..0e48dcd 100755 --- a/README.md +++ b/README.md @@ -1,4 +1,4 @@ -# ROS/ROS2 Humanoid Robot Framework +# ROS/ROS2 Humanoid Robot Intelligence Control System Framework [![Website](https://img.shields.io/badge/Visit%20our-Website-0099cc?style=for-the-badge)](https://robotics-sensors.github.io) [![Discord](https://img.shields.io/badge/Join%20our-Discord-7289DA?logo=discord&style=for-the-badge)](https://discord.gg/Yc72nd4w) From 02354c9517e9bd13682246fad75c623632123e73 Mon Sep 17 00:00:00 2001 From: Ronaldson Bellande <47253433+RonaldsonBellande@users.noreply.github.com> Date: Thu, 30 Nov 2023 21:19:56 -0500 Subject: [PATCH 216/238] Update README.md --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 0e48dcd..76173d3 100755 --- a/README.md +++ b/README.md @@ -1,4 +1,4 @@ -# ROS/ROS2 Humanoid Robot Intelligence Control System Framework +# ROS/ROS2 {BR&SRI} Humanoid Robot Intelligence Control System Framework [![Website](https://img.shields.io/badge/Visit%20our-Website-0099cc?style=for-the-badge)](https://robotics-sensors.github.io) [![Discord](https://img.shields.io/badge/Join%20our-Discord-7289DA?logo=discord&style=for-the-badge)](https://discord.gg/Yc72nd4w) From 270b49467b071444be301d507e3dcfff555ec9df Mon Sep 17 00:00:00 2001 From: Ronaldson Bellande <47253433+RonaldsonBellande@users.noreply.github.com> Date: Thu, 30 Nov 2023 21:19:56 -0500 Subject: [PATCH 217/238] Update README.md --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 0e48dcd..76173d3 100755 --- a/README.md +++ b/README.md @@ -1,4 +1,4 @@ -# ROS/ROS2 Humanoid Robot Intelligence Control System Framework +# ROS/ROS2 {BR&SRI} Humanoid Robot Intelligence Control System Framework [![Website](https://img.shields.io/badge/Visit%20our-Website-0099cc?style=for-the-badge)](https://robotics-sensors.github.io) [![Discord](https://img.shields.io/badge/Join%20our-Discord-7289DA?logo=discord&style=for-the-badge)](https://discord.gg/Yc72nd4w) From 1f1047f2eb7885c915df9deee5127c72a93f52f3 Mon Sep 17 00:00:00 2001 From: Ronaldson Bellande <47253433+RonaldsonBellande@users.noreply.github.com> Date: Wed, 6 Dec 2023 13:32:39 -0500 Subject: [PATCH 218/238] Update README.md --- README.md | 78 +++++++++++++++++++++++++++++++------------------------ 1 file changed, 44 insertions(+), 34 deletions(-) diff --git a/README.md b/README.md index 76173d3..9195865 100755 --- a/README.md +++ b/README.md @@ -1,51 +1,61 @@ -# ROS/ROS2 {BR&SRI} Humanoid Robot Intelligence Control System Framework +# Welcome to the {BR&SRI} Humanoid Robot Intelligence Control System Demos [![Website](https://img.shields.io/badge/Visit%20our-Website-0099cc?style=for-the-badge)](https://robotics-sensors.github.io) [![Discord](https://img.shields.io/badge/Join%20our-Discord-7289DA?logo=discord&style=for-the-badge)](https://discord.gg/Yc72nd4w) [![Sponsor](https://img.shields.io/badge/Sponsor-Robotics%20Sensors%20Research-red?style=for-the-badge&logo=github)](https://github.com/sponsors/Robotics-Sensors) -# Stats -[![GitHub stars](https://img.shields.io/github/stars/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework.svg?style=social)](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework/stargazers) -[![GitHub forks](https://img.shields.io/github/forks/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework.svg?style=social)](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework/network) -[![GitHub watchers](https://img.shields.io/github/watchers/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework.svg?style=social)](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework/watchers) +## 🤖 Explore Humanoid Robot Intelligence with Us! -[![GitHub issues](https://img.shields.io/github/issues/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework.svg)](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework/issues) -[![GitHub pull requests](https://img.shields.io/github/issues-pr/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework.svg)](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework/pulls) -[![GitHub license](https://img.shields.io/github/license/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework.svg)](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework/blob/main/LICENSE) +Welcome to the {BR&SRI} Humanoid Robot Intelligence Control System Demos repository! Here, we invite you to delve into the fascinating world of humanoid robotics, showcasing the innovative capabilities of our intelligent control system. -[![GitHub last commit](https://img.shields.io/github/last-commit/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework.svg)](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework/commits) -[![Visitor & Github Clones](https://img.shields.io/badge/dynamic/json?color=2e8b57&label=Visitor%20%26%20GitHub%20Clones&query=$.count&url=https://api.github.com/repos/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework/traffic)](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework) +### 🚀 Key Repository Stats --------------------------------------------------------------------------------------------------------- -# Repository Website -https://robotics-sensors.github.io/humanoid_robot_intelligence_control_system_framework +- ![GitHub stars](https://img.shields.io/github/stars/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos.svg?style=social) Starred by the community +- ![GitHub forks](https://img.shields.io/github/forks/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos.svg?style=social) Forked by enthusiasts +- ![GitHub watchers](https://img.shields.io/github/watchers/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos.svg?style=social) Watched by curious minds --------------------------------------------------------------------------------------------------------- -Updated Version [humanoid_robot_intelligence_control_system_framework](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework) readme. -Old Version/Previous Used for Different Context [ROBOTIS-Framework](https://github.com/ROBOTIS-GIT/ROBOTIS-Framework) readme. +- ![GitHub issues](https://img.shields.io/github/issues/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos.svg) Actively addressing issues +- ![GitHub pull requests](https://img.shields.io/github/issues-pr/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos.svg) Welcoming contributions +- ![GitHub license](https://img.shields.io/github/license/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos.svg) Licensed under Apache 2.0 -# Release -[![Latest Release](https://img.shields.io/github/v/release/Robotics-Sensors/humanoid_robot_intelligence_control_system_tools?style=for-the-badge&color=yellow)](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework/releases/) +- ![GitHub last commit](https://img.shields.io/github/last-commit/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos.svg) Latest updates +- ![Visitor & Github Clones](https://img.shields.io/badge/dynamic/json?color=2e8b57&label=Visitor%20%26%20GitHub%20Clones&query=$.count&url=https://api.github.com/repos/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos/traffic) Engaging with the community -# Contact -Depeding on the repository, If you are interested in accessing the complete version or other repository related to this repository, we kindly request that you reach out to the organization's director or the company behind this project. We invite you to explore the USE CASE to understand the specific terms and conditions for usage. To utilize this repository, it is imperative that you adhere to the guidelines outlined in the USE CASE. For those interested in showing their support, becoming a sponsor of the organization is an option, and detailed information can be found within the USE CASE and License documents. Furthermore, we encourage you to join our official Discord community, where you can engage with like-minded individuals, contribute to the project, and stay up-to-date with the latest developments. It's worth noting that while a fraction of research is publicly accessible, the majority remains private. To gain insights into the wealth of knowledge held by the Company or the organization's director, we recommend direct contact for more information. +--- --------------------------------------------------------------------------------------------------------- -## Important -The repository has diverged, as the old commits and codes are under the previous License and -the new commits and codes are under New License +## 🌐 Repository Website --------------------------------------------------------------------------------------------------------- -Latest versions and Maintainer is on organization https://github.com/Robotics-Sensors +Explore our [repository website](https://robotics-sensors.github.io/humanoid_robot_intelligence_control_system_demos) for detailed documentation, tutorials, and additional information about our humanoid robot intelligence control system. +--- -# USE CASE --------------------------------------------------------------------------------------------------------- -* Every repository within our organization is a valuable resource that can be utilized for educational purposes by individuals who actively contribute to the repository or choose to become sponsors of the organization. For those who wish to use our services conversationally, the acquisition of a license and subscription from our company website is mandatory. This ensures that the services are appropriately compensated for their use. Additionally, for the 90% of services designated as private, acquiring a license and subscription can be facilitated through our company website. We encourage interested parties to visit our website to explore and procure the necessary licenses and subscriptions for the diverse range of services and products we offer. Your support and commitment enable us to maintain and enhance the quality of our offerings, contributing to a thriving collaborative environment. --------------------------------------------------------------------------------------------------------- +### 🔄 Updates and Versions -### Maintainer -* Ronaldson Bellande +- **Updated Version:** [humanoid_robot_intelligence_control_system_module](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos) +- **Old Version/Previous Used for Different Context:** [ROBOTIS-OP3-Demo](https://github.com/ROBOTIS-GIT/ROBOTIS-OP3-Demo) -## License -This SDK is distributed under the [Apache License, Version 2.0](https://www.apache.org/licenses/LICENSE-2.0), see [LICENSE](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework/blob/main/LICENSE) and [NOTICE](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework/blob/main/LICENSE) for more information. +--- + +# 🎉 Latest Release + +[![Latest Release](https://img.shields.io/github/v/release/Robotics-Sensors/humanoid_robot_intelligence_control_system_tools?style=for-the-badge&color=yellow)](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos/releases/) + +--- + +## 📢 Important Announcement + +The repository has recently undergone significant updates. Older commits and codes are covered under the previous license, while new commits and codes fall under the new license. + +--- + +🚀 For the latest versions and updates, visit our organization: [Robotics-Sensors](https://github.com/Robotics-Sensors). + +### 🧑‍💼 Maintainer + +Meet our dedicated maintainer, **Ronaldson Bellande**. + +--- + +## 📄 License + +This SDK is distributed under the [Apache License, Version 2.0](https://www.apache.org/licenses/LICENSE-2.0). For detailed information, refer to [LICENSE](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos/blob/main/LICENSE) and [NOTICE](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos/blob/main/LICENSE). From f55986ac82844163c3a75d3235b82a939b7a5c92 Mon Sep 17 00:00:00 2001 From: Ronaldson Bellande <47253433+RonaldsonBellande@users.noreply.github.com> Date: Wed, 6 Dec 2023 13:32:39 -0500 Subject: [PATCH 219/238] Update README.md --- README.md | 78 +++++++++++++++++++++++++++++++------------------------ 1 file changed, 44 insertions(+), 34 deletions(-) diff --git a/README.md b/README.md index 76173d3..9195865 100755 --- a/README.md +++ b/README.md @@ -1,51 +1,61 @@ -# ROS/ROS2 {BR&SRI} Humanoid Robot Intelligence Control System Framework +# Welcome to the {BR&SRI} Humanoid Robot Intelligence Control System Demos [![Website](https://img.shields.io/badge/Visit%20our-Website-0099cc?style=for-the-badge)](https://robotics-sensors.github.io) [![Discord](https://img.shields.io/badge/Join%20our-Discord-7289DA?logo=discord&style=for-the-badge)](https://discord.gg/Yc72nd4w) [![Sponsor](https://img.shields.io/badge/Sponsor-Robotics%20Sensors%20Research-red?style=for-the-badge&logo=github)](https://github.com/sponsors/Robotics-Sensors) -# Stats -[![GitHub stars](https://img.shields.io/github/stars/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework.svg?style=social)](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework/stargazers) -[![GitHub forks](https://img.shields.io/github/forks/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework.svg?style=social)](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework/network) -[![GitHub watchers](https://img.shields.io/github/watchers/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework.svg?style=social)](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework/watchers) +## 🤖 Explore Humanoid Robot Intelligence with Us! -[![GitHub issues](https://img.shields.io/github/issues/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework.svg)](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework/issues) -[![GitHub pull requests](https://img.shields.io/github/issues-pr/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework.svg)](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework/pulls) -[![GitHub license](https://img.shields.io/github/license/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework.svg)](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework/blob/main/LICENSE) +Welcome to the {BR&SRI} Humanoid Robot Intelligence Control System Demos repository! Here, we invite you to delve into the fascinating world of humanoid robotics, showcasing the innovative capabilities of our intelligent control system. -[![GitHub last commit](https://img.shields.io/github/last-commit/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework.svg)](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework/commits) -[![Visitor & Github Clones](https://img.shields.io/badge/dynamic/json?color=2e8b57&label=Visitor%20%26%20GitHub%20Clones&query=$.count&url=https://api.github.com/repos/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework/traffic)](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework) +### 🚀 Key Repository Stats --------------------------------------------------------------------------------------------------------- -# Repository Website -https://robotics-sensors.github.io/humanoid_robot_intelligence_control_system_framework +- ![GitHub stars](https://img.shields.io/github/stars/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos.svg?style=social) Starred by the community +- ![GitHub forks](https://img.shields.io/github/forks/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos.svg?style=social) Forked by enthusiasts +- ![GitHub watchers](https://img.shields.io/github/watchers/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos.svg?style=social) Watched by curious minds --------------------------------------------------------------------------------------------------------- -Updated Version [humanoid_robot_intelligence_control_system_framework](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework) readme. -Old Version/Previous Used for Different Context [ROBOTIS-Framework](https://github.com/ROBOTIS-GIT/ROBOTIS-Framework) readme. +- ![GitHub issues](https://img.shields.io/github/issues/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos.svg) Actively addressing issues +- ![GitHub pull requests](https://img.shields.io/github/issues-pr/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos.svg) Welcoming contributions +- ![GitHub license](https://img.shields.io/github/license/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos.svg) Licensed under Apache 2.0 -# Release -[![Latest Release](https://img.shields.io/github/v/release/Robotics-Sensors/humanoid_robot_intelligence_control_system_tools?style=for-the-badge&color=yellow)](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework/releases/) +- ![GitHub last commit](https://img.shields.io/github/last-commit/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos.svg) Latest updates +- ![Visitor & Github Clones](https://img.shields.io/badge/dynamic/json?color=2e8b57&label=Visitor%20%26%20GitHub%20Clones&query=$.count&url=https://api.github.com/repos/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos/traffic) Engaging with the community -# Contact -Depeding on the repository, If you are interested in accessing the complete version or other repository related to this repository, we kindly request that you reach out to the organization's director or the company behind this project. We invite you to explore the USE CASE to understand the specific terms and conditions for usage. To utilize this repository, it is imperative that you adhere to the guidelines outlined in the USE CASE. For those interested in showing their support, becoming a sponsor of the organization is an option, and detailed information can be found within the USE CASE and License documents. Furthermore, we encourage you to join our official Discord community, where you can engage with like-minded individuals, contribute to the project, and stay up-to-date with the latest developments. It's worth noting that while a fraction of research is publicly accessible, the majority remains private. To gain insights into the wealth of knowledge held by the Company or the organization's director, we recommend direct contact for more information. +--- --------------------------------------------------------------------------------------------------------- -## Important -The repository has diverged, as the old commits and codes are under the previous License and -the new commits and codes are under New License +## 🌐 Repository Website --------------------------------------------------------------------------------------------------------- -Latest versions and Maintainer is on organization https://github.com/Robotics-Sensors +Explore our [repository website](https://robotics-sensors.github.io/humanoid_robot_intelligence_control_system_demos) for detailed documentation, tutorials, and additional information about our humanoid robot intelligence control system. +--- -# USE CASE --------------------------------------------------------------------------------------------------------- -* Every repository within our organization is a valuable resource that can be utilized for educational purposes by individuals who actively contribute to the repository or choose to become sponsors of the organization. For those who wish to use our services conversationally, the acquisition of a license and subscription from our company website is mandatory. This ensures that the services are appropriately compensated for their use. Additionally, for the 90% of services designated as private, acquiring a license and subscription can be facilitated through our company website. We encourage interested parties to visit our website to explore and procure the necessary licenses and subscriptions for the diverse range of services and products we offer. Your support and commitment enable us to maintain and enhance the quality of our offerings, contributing to a thriving collaborative environment. --------------------------------------------------------------------------------------------------------- +### 🔄 Updates and Versions -### Maintainer -* Ronaldson Bellande +- **Updated Version:** [humanoid_robot_intelligence_control_system_module](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos) +- **Old Version/Previous Used for Different Context:** [ROBOTIS-OP3-Demo](https://github.com/ROBOTIS-GIT/ROBOTIS-OP3-Demo) -## License -This SDK is distributed under the [Apache License, Version 2.0](https://www.apache.org/licenses/LICENSE-2.0), see [LICENSE](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework/blob/main/LICENSE) and [NOTICE](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework/blob/main/LICENSE) for more information. +--- + +# 🎉 Latest Release + +[![Latest Release](https://img.shields.io/github/v/release/Robotics-Sensors/humanoid_robot_intelligence_control_system_tools?style=for-the-badge&color=yellow)](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos/releases/) + +--- + +## 📢 Important Announcement + +The repository has recently undergone significant updates. Older commits and codes are covered under the previous license, while new commits and codes fall under the new license. + +--- + +🚀 For the latest versions and updates, visit our organization: [Robotics-Sensors](https://github.com/Robotics-Sensors). + +### 🧑‍💼 Maintainer + +Meet our dedicated maintainer, **Ronaldson Bellande**. + +--- + +## 📄 License + +This SDK is distributed under the [Apache License, Version 2.0](https://www.apache.org/licenses/LICENSE-2.0). For detailed information, refer to [LICENSE](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos/blob/main/LICENSE) and [NOTICE](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos/blob/main/LICENSE). From 7c85535c514894249046cd9ff7a14ee187c158c9 Mon Sep 17 00:00:00 2001 From: Ronaldson Bellande <47253433+RonaldsonBellande@users.noreply.github.com> Date: Wed, 6 Dec 2023 13:33:44 -0500 Subject: [PATCH 220/238] Update README.md --- README.md | 30 +++++++++++++++--------------- 1 file changed, 15 insertions(+), 15 deletions(-) diff --git a/README.md b/README.md index 9195865..347179d 100755 --- a/README.md +++ b/README.md @@ -1,4 +1,4 @@ -# Welcome to the {BR&SRI} Humanoid Robot Intelligence Control System Demos +# Welcome to the {BR&SRI} Humanoid Robot Intelligence Control System framework [![Website](https://img.shields.io/badge/Visit%20our-Website-0099cc?style=for-the-badge)](https://robotics-sensors.github.io) [![Discord](https://img.shields.io/badge/Join%20our-Discord-7289DA?logo=discord&style=for-the-badge)](https://discord.gg/Yc72nd4w) @@ -6,39 +6,39 @@ ## 🤖 Explore Humanoid Robot Intelligence with Us! -Welcome to the {BR&SRI} Humanoid Robot Intelligence Control System Demos repository! Here, we invite you to delve into the fascinating world of humanoid robotics, showcasing the innovative capabilities of our intelligent control system. +Welcome to the {BR&SRI} Humanoid Robot Intelligence Control System framework repository! Here, we invite you to delve into the fascinating world of humanoid robotics, showcasing the innovative capabilities of our intelligent control system. ### 🚀 Key Repository Stats -- ![GitHub stars](https://img.shields.io/github/stars/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos.svg?style=social) Starred by the community -- ![GitHub forks](https://img.shields.io/github/forks/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos.svg?style=social) Forked by enthusiasts -- ![GitHub watchers](https://img.shields.io/github/watchers/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos.svg?style=social) Watched by curious minds +- ![GitHub stars](https://img.shields.io/github/stars/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework.svg?style=social) Starred by the community +- ![GitHub forks](https://img.shields.io/github/forks/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework.svg?style=social) Forked by enthusiasts +- ![GitHub watchers](https://img.shields.io/github/watchers/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework.svg?style=social) Watched by curious minds -- ![GitHub issues](https://img.shields.io/github/issues/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos.svg) Actively addressing issues -- ![GitHub pull requests](https://img.shields.io/github/issues-pr/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos.svg) Welcoming contributions -- ![GitHub license](https://img.shields.io/github/license/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos.svg) Licensed under Apache 2.0 +- ![GitHub issues](https://img.shields.io/github/issues/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework.svg) Actively addressing issues +- ![GitHub pull requests](https://img.shields.io/github/issues-pr/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework.svg) Welcoming contributions +- ![GitHub license](https://img.shields.io/github/license/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework.svg) Licensed under Apache 2.0 -- ![GitHub last commit](https://img.shields.io/github/last-commit/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos.svg) Latest updates -- ![Visitor & Github Clones](https://img.shields.io/badge/dynamic/json?color=2e8b57&label=Visitor%20%26%20GitHub%20Clones&query=$.count&url=https://api.github.com/repos/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos/traffic) Engaging with the community +- ![GitHub last commit](https://img.shields.io/github/last-commit/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework.svg) Latest updates +- ![Visitor & Github Clones](https://img.shields.io/badge/dynamic/json?color=2e8b57&label=Visitor%20%26%20GitHub%20Clones&query=$.count&url=https://api.github.com/repos/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework/traffic) Engaging with the community --- ## 🌐 Repository Website -Explore our [repository website](https://robotics-sensors.github.io/humanoid_robot_intelligence_control_system_demos) for detailed documentation, tutorials, and additional information about our humanoid robot intelligence control system. +Explore our [repository website](https://robotics-sensors.github.io/humanoid_robot_intelligence_control_system_framework) for detailed documentation, tutorials, and additional information about our humanoid robot intelligence control system. --- ### 🔄 Updates and Versions -- **Updated Version:** [humanoid_robot_intelligence_control_system_module](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos) -- **Old Version/Previous Used for Different Context:** [ROBOTIS-OP3-Demo](https://github.com/ROBOTIS-GIT/ROBOTIS-OP3-Demo) +- **Updated Version:** [humanoid_robot_intelligence_control_system_module](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework) +- **Old Version/Previous Used for Different Context:** [ROBOTIS-Framework](https://github.com/ROBOTIS-GIT/ROBOTIS-Framework) --- # 🎉 Latest Release -[![Latest Release](https://img.shields.io/github/v/release/Robotics-Sensors/humanoid_robot_intelligence_control_system_tools?style=for-the-badge&color=yellow)](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos/releases/) +[![Latest Release](https://img.shields.io/github/v/release/Robotics-Sensors/humanoid_robot_intelligence_control_system_tools?style=for-the-badge&color=yellow)](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework/releases/) --- @@ -58,4 +58,4 @@ Meet our dedicated maintainer, **Ronaldson Bellande**. ## 📄 License -This SDK is distributed under the [Apache License, Version 2.0](https://www.apache.org/licenses/LICENSE-2.0). For detailed information, refer to [LICENSE](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos/blob/main/LICENSE) and [NOTICE](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos/blob/main/LICENSE). +This SDK is distributed under the [Apache License, Version 2.0](https://www.apache.org/licenses/LICENSE-2.0). For detailed information, refer to [LICENSE](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework/blob/main/LICENSE) and [NOTICE](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework/blob/main/LICENSE). From 1a30048449317ae122145d84f02d9b3559394009 Mon Sep 17 00:00:00 2001 From: Ronaldson Bellande <47253433+RonaldsonBellande@users.noreply.github.com> Date: Wed, 6 Dec 2023 13:33:44 -0500 Subject: [PATCH 221/238] Update README.md --- README.md | 30 +++++++++++++++--------------- 1 file changed, 15 insertions(+), 15 deletions(-) diff --git a/README.md b/README.md index 9195865..347179d 100755 --- a/README.md +++ b/README.md @@ -1,4 +1,4 @@ -# Welcome to the {BR&SRI} Humanoid Robot Intelligence Control System Demos +# Welcome to the {BR&SRI} Humanoid Robot Intelligence Control System framework [![Website](https://img.shields.io/badge/Visit%20our-Website-0099cc?style=for-the-badge)](https://robotics-sensors.github.io) [![Discord](https://img.shields.io/badge/Join%20our-Discord-7289DA?logo=discord&style=for-the-badge)](https://discord.gg/Yc72nd4w) @@ -6,39 +6,39 @@ ## 🤖 Explore Humanoid Robot Intelligence with Us! -Welcome to the {BR&SRI} Humanoid Robot Intelligence Control System Demos repository! Here, we invite you to delve into the fascinating world of humanoid robotics, showcasing the innovative capabilities of our intelligent control system. +Welcome to the {BR&SRI} Humanoid Robot Intelligence Control System framework repository! Here, we invite you to delve into the fascinating world of humanoid robotics, showcasing the innovative capabilities of our intelligent control system. ### 🚀 Key Repository Stats -- ![GitHub stars](https://img.shields.io/github/stars/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos.svg?style=social) Starred by the community -- ![GitHub forks](https://img.shields.io/github/forks/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos.svg?style=social) Forked by enthusiasts -- ![GitHub watchers](https://img.shields.io/github/watchers/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos.svg?style=social) Watched by curious minds +- ![GitHub stars](https://img.shields.io/github/stars/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework.svg?style=social) Starred by the community +- ![GitHub forks](https://img.shields.io/github/forks/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework.svg?style=social) Forked by enthusiasts +- ![GitHub watchers](https://img.shields.io/github/watchers/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework.svg?style=social) Watched by curious minds -- ![GitHub issues](https://img.shields.io/github/issues/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos.svg) Actively addressing issues -- ![GitHub pull requests](https://img.shields.io/github/issues-pr/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos.svg) Welcoming contributions -- ![GitHub license](https://img.shields.io/github/license/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos.svg) Licensed under Apache 2.0 +- ![GitHub issues](https://img.shields.io/github/issues/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework.svg) Actively addressing issues +- ![GitHub pull requests](https://img.shields.io/github/issues-pr/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework.svg) Welcoming contributions +- ![GitHub license](https://img.shields.io/github/license/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework.svg) Licensed under Apache 2.0 -- ![GitHub last commit](https://img.shields.io/github/last-commit/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos.svg) Latest updates -- ![Visitor & Github Clones](https://img.shields.io/badge/dynamic/json?color=2e8b57&label=Visitor%20%26%20GitHub%20Clones&query=$.count&url=https://api.github.com/repos/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos/traffic) Engaging with the community +- ![GitHub last commit](https://img.shields.io/github/last-commit/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework.svg) Latest updates +- ![Visitor & Github Clones](https://img.shields.io/badge/dynamic/json?color=2e8b57&label=Visitor%20%26%20GitHub%20Clones&query=$.count&url=https://api.github.com/repos/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework/traffic) Engaging with the community --- ## 🌐 Repository Website -Explore our [repository website](https://robotics-sensors.github.io/humanoid_robot_intelligence_control_system_demos) for detailed documentation, tutorials, and additional information about our humanoid robot intelligence control system. +Explore our [repository website](https://robotics-sensors.github.io/humanoid_robot_intelligence_control_system_framework) for detailed documentation, tutorials, and additional information about our humanoid robot intelligence control system. --- ### 🔄 Updates and Versions -- **Updated Version:** [humanoid_robot_intelligence_control_system_module](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos) -- **Old Version/Previous Used for Different Context:** [ROBOTIS-OP3-Demo](https://github.com/ROBOTIS-GIT/ROBOTIS-OP3-Demo) +- **Updated Version:** [humanoid_robot_intelligence_control_system_module](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework) +- **Old Version/Previous Used for Different Context:** [ROBOTIS-Framework](https://github.com/ROBOTIS-GIT/ROBOTIS-Framework) --- # 🎉 Latest Release -[![Latest Release](https://img.shields.io/github/v/release/Robotics-Sensors/humanoid_robot_intelligence_control_system_tools?style=for-the-badge&color=yellow)](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos/releases/) +[![Latest Release](https://img.shields.io/github/v/release/Robotics-Sensors/humanoid_robot_intelligence_control_system_tools?style=for-the-badge&color=yellow)](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework/releases/) --- @@ -58,4 +58,4 @@ Meet our dedicated maintainer, **Ronaldson Bellande**. ## 📄 License -This SDK is distributed under the [Apache License, Version 2.0](https://www.apache.org/licenses/LICENSE-2.0). For detailed information, refer to [LICENSE](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos/blob/main/LICENSE) and [NOTICE](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos/blob/main/LICENSE). +This SDK is distributed under the [Apache License, Version 2.0](https://www.apache.org/licenses/LICENSE-2.0). For detailed information, refer to [LICENSE](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework/blob/main/LICENSE) and [NOTICE](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework/blob/main/LICENSE). From f5f7eb421352722c5c82ed2d50992825c3e7083b Mon Sep 17 00:00:00 2001 From: Ronaldson Bellande <47253433+RonaldsonBellande@users.noreply.github.com> Date: Thu, 8 Feb 2024 02:25:24 -0500 Subject: [PATCH 222/238] Update README.md --- README.md | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/README.md b/README.md index 347179d..9c2bc38 100755 --- a/README.md +++ b/README.md @@ -8,6 +8,10 @@ Welcome to the {BR&SRI} Humanoid Robot Intelligence Control System framework repository! Here, we invite you to delve into the fascinating world of humanoid robotics, showcasing the innovative capabilities of our intelligent control system. +## 💻 Functionality To Switch from ROS to ROS2 Checkout The Below Repository + +[![Toggle ROS Versions](https://img.shields.io/badge/Toggle%20ROS%20Versions-Explore%20ROS%20and%20ROS2%20migration-blue?style=for-the-badge&logo=ros&color=blue)](https://github.com/Robotics-Sensors/ros_extension/) + ### 🚀 Key Repository Stats - ![GitHub stars](https://img.shields.io/github/stars/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework.svg?style=social) Starred by the community From 1de1de19baa6f4a760c6f32c6714655bdd2b8a00 Mon Sep 17 00:00:00 2001 From: Ronaldson Bellande <47253433+RonaldsonBellande@users.noreply.github.com> Date: Thu, 8 Feb 2024 02:25:24 -0500 Subject: [PATCH 223/238] Update README.md --- README.md | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/README.md b/README.md index 347179d..9c2bc38 100755 --- a/README.md +++ b/README.md @@ -8,6 +8,10 @@ Welcome to the {BR&SRI} Humanoid Robot Intelligence Control System framework repository! Here, we invite you to delve into the fascinating world of humanoid robotics, showcasing the innovative capabilities of our intelligent control system. +## 💻 Functionality To Switch from ROS to ROS2 Checkout The Below Repository + +[![Toggle ROS Versions](https://img.shields.io/badge/Toggle%20ROS%20Versions-Explore%20ROS%20and%20ROS2%20migration-blue?style=for-the-badge&logo=ros&color=blue)](https://github.com/Robotics-Sensors/ros_extension/) + ### 🚀 Key Repository Stats - ![GitHub stars](https://img.shields.io/github/stars/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework.svg?style=social) Starred by the community From 0917688144a3d834c01ab4c0cac090e345c2858d Mon Sep 17 00:00:00 2001 From: RonaldsonBellande Date: Mon, 29 Apr 2024 15:54:09 -0400 Subject: [PATCH 224/238] latest pushes --- .../package.xml | 61 ++++++++++++++----- .../package.xml | 16 +++++ .../package.xml | 49 +++++++++++---- .../package.xml | 25 +++++++- 4 files changed, 122 insertions(+), 29 deletions(-) diff --git a/humanoid_robot_intelligence_control_system_controller/package.xml b/humanoid_robot_intelligence_control_system_controller/package.xml index 302552d..d2e9ca0 100755 --- a/humanoid_robot_intelligence_control_system_controller/package.xml +++ b/humanoid_robot_intelligence_control_system_controller/package.xml @@ -1,8 +1,25 @@ + + humanoid_robot_intelligence_control_system_controller 0.3.2 - humanoid_robot_intelligence_control_system_controller package for ROBOTIS's platform like Manipulator-H, THORMANG and OP series + humanoid_robot_intelligence_control_system_controller package for ROBOTIS's platform + like Manipulator-H, THORMANG and OP series Apache 2.0 Ronaldson Bellande @@ -13,10 +30,12 @@ roslib std_msgs sensor_msgs - humanoid_robot_intelligence_control_system_controller_msgs + + humanoid_robot_intelligence_control_system_controller_msgs dynamixel_sdk humanoid_robot_intelligence_control_system_device - humanoid_robot_intelligence_control_system_framework_common + + humanoid_robot_intelligence_control_system_framework_common cmake_modules yaml-cpp @@ -24,10 +43,13 @@ roslib std_msgs sensor_msgs - humanoid_robot_intelligence_control_system_controller_msgs + + humanoid_robot_intelligence_control_system_controller_msgs dynamixel_sdk - humanoid_robot_intelligence_control_system_device - humanoid_robot_intelligence_control_system_framework_common + + humanoid_robot_intelligence_control_system_device + + humanoid_robot_intelligence_control_system_framework_common cmake_modules yaml-cpp @@ -35,10 +57,12 @@ roslib std_msgs sensor_msgs - humanoid_robot_intelligence_control_system_controller_msgs + + humanoid_robot_intelligence_control_system_controller_msgs dynamixel_sdk humanoid_robot_intelligence_control_system_device - humanoid_robot_intelligence_control_system_framework_common + + humanoid_robot_intelligence_control_system_framework_common cmake_modules yaml-cpp @@ -49,10 +73,12 @@ roslib std_msgs sensor_msgs - humanoid_robot_intelligence_control_system_controller_msgs + + humanoid_robot_intelligence_control_system_controller_msgs dynamixel_sdk humanoid_robot_intelligence_control_system_device - humanoid_robot_intelligence_control_system_framework_common + + humanoid_robot_intelligence_control_system_framework_common cmake_modules yaml-cpp @@ -60,10 +86,13 @@ roslib std_msgs sensor_msgs - humanoid_robot_intelligence_control_system_controller_msgs + + humanoid_robot_intelligence_control_system_controller_msgs dynamixel_sdk - humanoid_robot_intelligence_control_system_device - humanoid_robot_intelligence_control_system_framework_common + + humanoid_robot_intelligence_control_system_device + + humanoid_robot_intelligence_control_system_framework_common cmake_modules yaml-cpp @@ -71,10 +100,12 @@ roslib std_msgs sensor_msgs - humanoid_robot_intelligence_control_system_controller_msgs + + humanoid_robot_intelligence_control_system_controller_msgs dynamixel_sdk humanoid_robot_intelligence_control_system_device - humanoid_robot_intelligence_control_system_framework_common + + humanoid_robot_intelligence_control_system_framework_common cmake_modules yaml-cpp diff --git a/humanoid_robot_intelligence_control_system_device/package.xml b/humanoid_robot_intelligence_control_system_device/package.xml index b21c17d..397f854 100755 --- a/humanoid_robot_intelligence_control_system_device/package.xml +++ b/humanoid_robot_intelligence_control_system_device/package.xml @@ -1,3 +1,19 @@ + + humanoid_robot_intelligence_control_system_device diff --git a/humanoid_robot_intelligence_control_system_framework/package.xml b/humanoid_robot_intelligence_control_system_framework/package.xml index 39be137..d23a668 100755 --- a/humanoid_robot_intelligence_control_system_framework/package.xml +++ b/humanoid_robot_intelligence_control_system_framework/package.xml @@ -1,8 +1,25 @@ + + humanoid_robot_intelligence_control_system_framework 0.3.2 - ROS packages for the humanoid_robot_intelligence_control_system_framework (meta package) + ROS packages for the humanoid_robot_intelligence_control_system_framework (meta + package) Apache 2.0 Ronaldson Bellande @@ -11,30 +28,40 @@ humanoid_robot_intelligence_control_system_controller humanoid_robot_intelligence_control_system_device - humanoid_robot_intelligence_control_system_framework_common + + humanoid_robot_intelligence_control_system_framework_common - humanoid_robot_intelligence_control_system_controller - humanoid_robot_intelligence_control_system_device - humanoid_robot_intelligence_control_system_framework_common + + humanoid_robot_intelligence_control_system_controller + + humanoid_robot_intelligence_control_system_device + + humanoid_robot_intelligence_control_system_framework_common humanoid_robot_intelligence_control_system_controller humanoid_robot_intelligence_control_system_device - humanoid_robot_intelligence_control_system_framework_common + + humanoid_robot_intelligence_control_system_framework_common ament_cmake humanoid_robot_intelligence_control_system_controller humanoid_robot_intelligence_control_system_device - humanoid_robot_intelligence_control_system_framework_common + + humanoid_robot_intelligence_control_system_framework_common - humanoid_robot_intelligence_control_system_controller - humanoid_robot_intelligence_control_system_device - humanoid_robot_intelligence_control_system_framework_common + + humanoid_robot_intelligence_control_system_controller + + humanoid_robot_intelligence_control_system_device + + humanoid_robot_intelligence_control_system_framework_common humanoid_robot_intelligence_control_system_controller humanoid_robot_intelligence_control_system_device - humanoid_robot_intelligence_control_system_framework_common + + humanoid_robot_intelligence_control_system_framework_common diff --git a/humanoid_robot_intelligence_control_system_framework_common/package.xml b/humanoid_robot_intelligence_control_system_framework_common/package.xml index c12c3b2..aed0500 100755 --- a/humanoid_robot_intelligence_control_system_framework_common/package.xml +++ b/humanoid_robot_intelligence_control_system_framework_common/package.xml @@ -1,8 +1,25 @@ + + humanoid_robot_intelligence_control_system_framework_common 0.3.2 - The package contains commonly used headers for the humanoid_robot_intelligence_control_system_framework + The package contains commonly used headers for the + humanoid_robot_intelligence_control_system_framework Apache 2.0 Ronaldson Bellande @@ -13,7 +30,8 @@ humanoid_robot_intelligence_control_system_device roscpp - humanoid_robot_intelligence_control_system_device + + humanoid_robot_intelligence_control_system_device roscpp humanoid_robot_intelligence_control_system_device @@ -25,7 +43,8 @@ humanoid_robot_intelligence_control_system_device roscpp - humanoid_robot_intelligence_control_system_device + + humanoid_robot_intelligence_control_system_device roscpp humanoid_robot_intelligence_control_system_device From 43587dfeefb9b0f15834081e5ebfa1079caa276f Mon Sep 17 00:00:00 2001 From: RonaldsonBellande Date: Mon, 29 Apr 2024 15:54:09 -0400 Subject: [PATCH 225/238] latest pushes --- .../package.xml | 61 ++++++++++++++----- .../package.xml | 16 +++++ .../package.xml | 25 +++++++- 3 files changed, 84 insertions(+), 18 deletions(-) diff --git a/humanoid_robot_intelligence_control_system_controller/package.xml b/humanoid_robot_intelligence_control_system_controller/package.xml index 302552d..d2e9ca0 100755 --- a/humanoid_robot_intelligence_control_system_controller/package.xml +++ b/humanoid_robot_intelligence_control_system_controller/package.xml @@ -1,8 +1,25 @@ + + humanoid_robot_intelligence_control_system_controller 0.3.2 - humanoid_robot_intelligence_control_system_controller package for ROBOTIS's platform like Manipulator-H, THORMANG and OP series + humanoid_robot_intelligence_control_system_controller package for ROBOTIS's platform + like Manipulator-H, THORMANG and OP series Apache 2.0 Ronaldson Bellande @@ -13,10 +30,12 @@ roslib std_msgs sensor_msgs - humanoid_robot_intelligence_control_system_controller_msgs + + humanoid_robot_intelligence_control_system_controller_msgs dynamixel_sdk humanoid_robot_intelligence_control_system_device - humanoid_robot_intelligence_control_system_framework_common + + humanoid_robot_intelligence_control_system_framework_common cmake_modules yaml-cpp @@ -24,10 +43,13 @@ roslib std_msgs sensor_msgs - humanoid_robot_intelligence_control_system_controller_msgs + + humanoid_robot_intelligence_control_system_controller_msgs dynamixel_sdk - humanoid_robot_intelligence_control_system_device - humanoid_robot_intelligence_control_system_framework_common + + humanoid_robot_intelligence_control_system_device + + humanoid_robot_intelligence_control_system_framework_common cmake_modules yaml-cpp @@ -35,10 +57,12 @@ roslib std_msgs sensor_msgs - humanoid_robot_intelligence_control_system_controller_msgs + + humanoid_robot_intelligence_control_system_controller_msgs dynamixel_sdk humanoid_robot_intelligence_control_system_device - humanoid_robot_intelligence_control_system_framework_common + + humanoid_robot_intelligence_control_system_framework_common cmake_modules yaml-cpp @@ -49,10 +73,12 @@ roslib std_msgs sensor_msgs - humanoid_robot_intelligence_control_system_controller_msgs + + humanoid_robot_intelligence_control_system_controller_msgs dynamixel_sdk humanoid_robot_intelligence_control_system_device - humanoid_robot_intelligence_control_system_framework_common + + humanoid_robot_intelligence_control_system_framework_common cmake_modules yaml-cpp @@ -60,10 +86,13 @@ roslib std_msgs sensor_msgs - humanoid_robot_intelligence_control_system_controller_msgs + + humanoid_robot_intelligence_control_system_controller_msgs dynamixel_sdk - humanoid_robot_intelligence_control_system_device - humanoid_robot_intelligence_control_system_framework_common + + humanoid_robot_intelligence_control_system_device + + humanoid_robot_intelligence_control_system_framework_common cmake_modules yaml-cpp @@ -71,10 +100,12 @@ roslib std_msgs sensor_msgs - humanoid_robot_intelligence_control_system_controller_msgs + + humanoid_robot_intelligence_control_system_controller_msgs dynamixel_sdk humanoid_robot_intelligence_control_system_device - humanoid_robot_intelligence_control_system_framework_common + + humanoid_robot_intelligence_control_system_framework_common cmake_modules yaml-cpp diff --git a/humanoid_robot_intelligence_control_system_device/package.xml b/humanoid_robot_intelligence_control_system_device/package.xml index b21c17d..397f854 100755 --- a/humanoid_robot_intelligence_control_system_device/package.xml +++ b/humanoid_robot_intelligence_control_system_device/package.xml @@ -1,3 +1,19 @@ + + humanoid_robot_intelligence_control_system_device diff --git a/humanoid_robot_intelligence_control_system_framework_common/package.xml b/humanoid_robot_intelligence_control_system_framework_common/package.xml index c12c3b2..aed0500 100755 --- a/humanoid_robot_intelligence_control_system_framework_common/package.xml +++ b/humanoid_robot_intelligence_control_system_framework_common/package.xml @@ -1,8 +1,25 @@ + + humanoid_robot_intelligence_control_system_framework_common 0.3.2 - The package contains commonly used headers for the humanoid_robot_intelligence_control_system_framework + The package contains commonly used headers for the + humanoid_robot_intelligence_control_system_framework Apache 2.0 Ronaldson Bellande @@ -13,7 +30,8 @@ humanoid_robot_intelligence_control_system_device roscpp - humanoid_robot_intelligence_control_system_device + + humanoid_robot_intelligence_control_system_device roscpp humanoid_robot_intelligence_control_system_device @@ -25,7 +43,8 @@ humanoid_robot_intelligence_control_system_device roscpp - humanoid_robot_intelligence_control_system_device + + humanoid_robot_intelligence_control_system_device roscpp humanoid_robot_intelligence_control_system_device From 28fbc293eacda8d3ea8092b2c2d6f265ec4f645b Mon Sep 17 00:00:00 2001 From: RonaldsonBellande Date: Mon, 29 Apr 2024 16:04:18 -0400 Subject: [PATCH 226/238] latest pushes --- .../CMakeLists.txt | 14 ++++++++++++++ .../CMakeLists.txt | 14 ++++++++++++++ .../CMakeLists.txt | 14 ++++++++++++++ .../CMakeLists.txt | 14 ++++++++++++++ 4 files changed, 56 insertions(+) diff --git a/humanoid_robot_intelligence_control_system_controller/CMakeLists.txt b/humanoid_robot_intelligence_control_system_controller/CMakeLists.txt index c69c671..d90c748 100755 --- a/humanoid_robot_intelligence_control_system_controller/CMakeLists.txt +++ b/humanoid_robot_intelligence_control_system_controller/CMakeLists.txt @@ -1,3 +1,17 @@ +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# 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. + cmake_minimum_required(VERSION 3.8) project(humanoid_robot_intelligence_control_system_controller) diff --git a/humanoid_robot_intelligence_control_system_device/CMakeLists.txt b/humanoid_robot_intelligence_control_system_device/CMakeLists.txt index a9c5f39..906a25f 100755 --- a/humanoid_robot_intelligence_control_system_device/CMakeLists.txt +++ b/humanoid_robot_intelligence_control_system_device/CMakeLists.txt @@ -1,3 +1,17 @@ +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# 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. + cmake_minimum_required(VERSION 3.8) project(humanoid_robot_intelligence_control_system_device) diff --git a/humanoid_robot_intelligence_control_system_framework/CMakeLists.txt b/humanoid_robot_intelligence_control_system_framework/CMakeLists.txt index dda922c..929650b 100644 --- a/humanoid_robot_intelligence_control_system_framework/CMakeLists.txt +++ b/humanoid_robot_intelligence_control_system_framework/CMakeLists.txt @@ -1,3 +1,17 @@ +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# 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. + cmake_minimum_required(VERSION 3.8) project(humanoid_robot_intelligence_control_system_framework) diff --git a/humanoid_robot_intelligence_control_system_framework_common/CMakeLists.txt b/humanoid_robot_intelligence_control_system_framework_common/CMakeLists.txt index d90d27c..bc1e839 100755 --- a/humanoid_robot_intelligence_control_system_framework_common/CMakeLists.txt +++ b/humanoid_robot_intelligence_control_system_framework_common/CMakeLists.txt @@ -1,3 +1,17 @@ +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# 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. + cmake_minimum_required(VERSION 3.8) project(humanoid_robot_intelligence_control_system_framework_common) From 046719d76b467bc20705a3e728057a55ec3e0e46 Mon Sep 17 00:00:00 2001 From: RonaldsonBellande Date: Mon, 29 Apr 2024 16:04:18 -0400 Subject: [PATCH 227/238] latest pushes --- .../CMakeLists.txt | 14 ++++++++++++++ .../CMakeLists.txt | 14 ++++++++++++++ .../CMakeLists.txt | 14 ++++++++++++++ 3 files changed, 42 insertions(+) diff --git a/humanoid_robot_intelligence_control_system_controller/CMakeLists.txt b/humanoid_robot_intelligence_control_system_controller/CMakeLists.txt index c69c671..d90c748 100755 --- a/humanoid_robot_intelligence_control_system_controller/CMakeLists.txt +++ b/humanoid_robot_intelligence_control_system_controller/CMakeLists.txt @@ -1,3 +1,17 @@ +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# 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. + cmake_minimum_required(VERSION 3.8) project(humanoid_robot_intelligence_control_system_controller) diff --git a/humanoid_robot_intelligence_control_system_device/CMakeLists.txt b/humanoid_robot_intelligence_control_system_device/CMakeLists.txt index a9c5f39..906a25f 100755 --- a/humanoid_robot_intelligence_control_system_device/CMakeLists.txt +++ b/humanoid_robot_intelligence_control_system_device/CMakeLists.txt @@ -1,3 +1,17 @@ +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# 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. + cmake_minimum_required(VERSION 3.8) project(humanoid_robot_intelligence_control_system_device) diff --git a/humanoid_robot_intelligence_control_system_framework_common/CMakeLists.txt b/humanoid_robot_intelligence_control_system_framework_common/CMakeLists.txt index d90d27c..bc1e839 100755 --- a/humanoid_robot_intelligence_control_system_framework_common/CMakeLists.txt +++ b/humanoid_robot_intelligence_control_system_framework_common/CMakeLists.txt @@ -1,3 +1,17 @@ +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# 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. + cmake_minimum_required(VERSION 3.8) project(humanoid_robot_intelligence_control_system_framework_common) From 043227e64e467ddfefde3df730fc510bacf0eae6 Mon Sep 17 00:00:00 2001 From: RonaldsonBellande Date: Tue, 7 May 2024 00:54:10 -0400 Subject: [PATCH 228/238] latest pushes --- .../package.xml | 4 ++-- humanoid_robot_intelligence_control_system_device/package.xml | 4 ++-- .../package.xml | 4 ++-- .../package.xml | 4 ++-- 4 files changed, 8 insertions(+), 8 deletions(-) diff --git a/humanoid_robot_intelligence_control_system_controller/package.xml b/humanoid_robot_intelligence_control_system_controller/package.xml index d2e9ca0..63452d9 100755 --- a/humanoid_robot_intelligence_control_system_controller/package.xml +++ b/humanoid_robot_intelligence_control_system_controller/package.xml @@ -1,5 +1,6 @@ + - humanoid_robot_intelligence_control_system_controller 0.3.2 diff --git a/humanoid_robot_intelligence_control_system_device/package.xml b/humanoid_robot_intelligence_control_system_device/package.xml index 397f854..fcd6b2d 100755 --- a/humanoid_robot_intelligence_control_system_device/package.xml +++ b/humanoid_robot_intelligence_control_system_device/package.xml @@ -1,5 +1,6 @@ + - humanoid_robot_intelligence_control_system_device 0.3.2 diff --git a/humanoid_robot_intelligence_control_system_framework/package.xml b/humanoid_robot_intelligence_control_system_framework/package.xml index d23a668..ec1e4f3 100755 --- a/humanoid_robot_intelligence_control_system_framework/package.xml +++ b/humanoid_robot_intelligence_control_system_framework/package.xml @@ -1,5 +1,6 @@ + - humanoid_robot_intelligence_control_system_framework 0.3.2 diff --git a/humanoid_robot_intelligence_control_system_framework_common/package.xml b/humanoid_robot_intelligence_control_system_framework_common/package.xml index aed0500..258c36e 100755 --- a/humanoid_robot_intelligence_control_system_framework_common/package.xml +++ b/humanoid_robot_intelligence_control_system_framework_common/package.xml @@ -1,5 +1,6 @@ + - humanoid_robot_intelligence_control_system_framework_common 0.3.2 From 4dfde53052cd11b3fe7e5f6cf7b107fced63e992 Mon Sep 17 00:00:00 2001 From: RonaldsonBellande Date: Tue, 7 May 2024 00:54:10 -0400 Subject: [PATCH 229/238] latest pushes --- .../package.xml | 4 ++-- humanoid_robot_intelligence_control_system_device/package.xml | 4 ++-- .../package.xml | 4 ++-- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/humanoid_robot_intelligence_control_system_controller/package.xml b/humanoid_robot_intelligence_control_system_controller/package.xml index d2e9ca0..63452d9 100755 --- a/humanoid_robot_intelligence_control_system_controller/package.xml +++ b/humanoid_robot_intelligence_control_system_controller/package.xml @@ -1,5 +1,6 @@ + - humanoid_robot_intelligence_control_system_controller 0.3.2 diff --git a/humanoid_robot_intelligence_control_system_device/package.xml b/humanoid_robot_intelligence_control_system_device/package.xml index 397f854..fcd6b2d 100755 --- a/humanoid_robot_intelligence_control_system_device/package.xml +++ b/humanoid_robot_intelligence_control_system_device/package.xml @@ -1,5 +1,6 @@ + - humanoid_robot_intelligence_control_system_device 0.3.2 diff --git a/humanoid_robot_intelligence_control_system_framework_common/package.xml b/humanoid_robot_intelligence_control_system_framework_common/package.xml index aed0500..258c36e 100755 --- a/humanoid_robot_intelligence_control_system_framework_common/package.xml +++ b/humanoid_robot_intelligence_control_system_framework_common/package.xml @@ -1,5 +1,6 @@ + - humanoid_robot_intelligence_control_system_framework_common 0.3.2 From ba76cf32ce18598a0b35313a5bf0de2cfb5b9c1d Mon Sep 17 00:00:00 2001 From: Ronaldson Bellande <47253433+RonaldsonBellande@users.noreply.github.com> Date: Tue, 7 May 2024 17:15:19 -0400 Subject: [PATCH 230/238] Update README.md --- README.md | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index 9c2bc38..7a802c5 100755 --- a/README.md +++ b/README.md @@ -8,10 +8,18 @@ Welcome to the {BR&SRI} Humanoid Robot Intelligence Control System framework repository! Here, we invite you to delve into the fascinating world of humanoid robotics, showcasing the innovative capabilities of our intelligent control system. +## 📢 Work with Bellande Algorithms and Models through Bellande's API! + +[![Bellande's Algorithm through Bellande API](https://img.shields.io/badge/Bellande's%20Algorithm%20through%20Bellande's%20API-Bellande%20API-blue?style=for-the-badge&logo=ros&color=blue)](https://github.com/Robotics-Sensors/Web-ROS-API) + +--- + ## 💻 Functionality To Switch from ROS to ROS2 Checkout The Below Repository [![Toggle ROS Versions](https://img.shields.io/badge/Toggle%20ROS%20Versions-Explore%20ROS%20and%20ROS2%20migration-blue?style=for-the-badge&logo=ros&color=blue)](https://github.com/Robotics-Sensors/ros_extension/) +--- + ### 🚀 Key Repository Stats - ![GitHub stars](https://img.shields.io/github/stars/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework.svg?style=social) Starred by the community @@ -56,7 +64,7 @@ The repository has recently undergone significant updates. Older commits and cod ### 🧑‍💼 Maintainer -Meet our dedicated maintainer, **Ronaldson Bellande**. +Meet our dedicated author & maintainer, **Ronaldson Bellande**. --- From 6c473f60320f2e35d465814cff9e568e5ba2ab11 Mon Sep 17 00:00:00 2001 From: Ronaldson Bellande <47253433+RonaldsonBellande@users.noreply.github.com> Date: Tue, 7 May 2024 17:15:19 -0400 Subject: [PATCH 231/238] Update README.md --- README.md | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index 9c2bc38..7a802c5 100755 --- a/README.md +++ b/README.md @@ -8,10 +8,18 @@ Welcome to the {BR&SRI} Humanoid Robot Intelligence Control System framework repository! Here, we invite you to delve into the fascinating world of humanoid robotics, showcasing the innovative capabilities of our intelligent control system. +## 📢 Work with Bellande Algorithms and Models through Bellande's API! + +[![Bellande's Algorithm through Bellande API](https://img.shields.io/badge/Bellande's%20Algorithm%20through%20Bellande's%20API-Bellande%20API-blue?style=for-the-badge&logo=ros&color=blue)](https://github.com/Robotics-Sensors/Web-ROS-API) + +--- + ## 💻 Functionality To Switch from ROS to ROS2 Checkout The Below Repository [![Toggle ROS Versions](https://img.shields.io/badge/Toggle%20ROS%20Versions-Explore%20ROS%20and%20ROS2%20migration-blue?style=for-the-badge&logo=ros&color=blue)](https://github.com/Robotics-Sensors/ros_extension/) +--- + ### 🚀 Key Repository Stats - ![GitHub stars](https://img.shields.io/github/stars/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework.svg?style=social) Starred by the community @@ -56,7 +64,7 @@ The repository has recently undergone significant updates. Older commits and cod ### 🧑‍💼 Maintainer -Meet our dedicated maintainer, **Ronaldson Bellande**. +Meet our dedicated author & maintainer, **Ronaldson Bellande**. --- From bc0194c3d8813eb8d7a438a5394857c183477edb Mon Sep 17 00:00:00 2001 From: Ronaldson Bellande <47253433+RonaldsonBellande@users.noreply.github.com> Date: Wed, 8 May 2024 23:44:32 -0400 Subject: [PATCH 232/238] Update README.md --- README.md | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/README.md b/README.md index 7a802c5..ab4394d 100755 --- a/README.md +++ b/README.md @@ -14,6 +14,12 @@ Welcome to the {BR&SRI} Humanoid Robot Intelligence Control System framework rep --- +## 🧑‍💼 Work with Bellande Models through Bellande Framework! + +[![Bellande's Algorithm through Bellande API](https://img.shields.io/badge/Bellande's%20Models%20through%20Bellande's%20Framework-Bellande%20Framework-blue?style=for-the-badge&logo=ros&color=blue)](https://github.com/Robotics-Sensors/ROS-MODELS) + +--- + ## 💻 Functionality To Switch from ROS to ROS2 Checkout The Below Repository [![Toggle ROS Versions](https://img.shields.io/badge/Toggle%20ROS%20Versions-Explore%20ROS%20and%20ROS2%20migration-blue?style=for-the-badge&logo=ros&color=blue)](https://github.com/Robotics-Sensors/ros_extension/) From bc7fd1547517378d678f5ebc212b2f99301d61d8 Mon Sep 17 00:00:00 2001 From: Ronaldson Bellande <47253433+RonaldsonBellande@users.noreply.github.com> Date: Wed, 8 May 2024 23:44:32 -0400 Subject: [PATCH 233/238] Update README.md --- README.md | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/README.md b/README.md index 7a802c5..ab4394d 100755 --- a/README.md +++ b/README.md @@ -14,6 +14,12 @@ Welcome to the {BR&SRI} Humanoid Robot Intelligence Control System framework rep --- +## 🧑‍💼 Work with Bellande Models through Bellande Framework! + +[![Bellande's Algorithm through Bellande API](https://img.shields.io/badge/Bellande's%20Models%20through%20Bellande's%20Framework-Bellande%20Framework-blue?style=for-the-badge&logo=ros&color=blue)](https://github.com/Robotics-Sensors/ROS-MODELS) + +--- + ## 💻 Functionality To Switch from ROS to ROS2 Checkout The Below Repository [![Toggle ROS Versions](https://img.shields.io/badge/Toggle%20ROS%20Versions-Explore%20ROS%20and%20ROS2%20migration-blue?style=for-the-badge&logo=ros&color=blue)](https://github.com/Robotics-Sensors/ros_extension/) From 58cf10ff6c625cbdf1387c37972fe6aa896c092b Mon Sep 17 00:00:00 2001 From: Ronaldson Bellande <47253433+RonaldsonBellande@users.noreply.github.com> Date: Tue, 4 Jun 2024 21:40:55 -0400 Subject: [PATCH 234/238] Update README.md --- README.md | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/README.md b/README.md index ab4394d..b1db628 100755 --- a/README.md +++ b/README.md @@ -8,6 +8,11 @@ Welcome to the {BR&SRI} Humanoid Robot Intelligence Control System framework repository! Here, we invite you to delve into the fascinating world of humanoid robotics, showcasing the innovative capabilities of our intelligent control system. +## 🔄 Run and Experiment with the build Bellande's Humanoid Robot + +[![Bellande's Humanoid](https://img.shields.io/badge/Bellande's-Humanoid%20Robot%20Package-blue?style=for-the-badge&logo=ros&color=blue)](https://github.com/Robotics-Sensors/bellande_humanoid_robotics_package) + + ## 📢 Work with Bellande Algorithms and Models through Bellande's API! [![Bellande's Algorithm through Bellande API](https://img.shields.io/badge/Bellande's%20Algorithm%20through%20Bellande's%20API-Bellande%20API-blue?style=for-the-badge&logo=ros&color=blue)](https://github.com/Robotics-Sensors/Web-ROS-API) From c19b7789c4bb2cb11bdb22bd4672e31cd6917d0b Mon Sep 17 00:00:00 2001 From: Ronaldson Bellande <47253433+RonaldsonBellande@users.noreply.github.com> Date: Tue, 4 Jun 2024 21:40:55 -0400 Subject: [PATCH 235/238] Update README.md --- README.md | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/README.md b/README.md index ab4394d..b1db628 100755 --- a/README.md +++ b/README.md @@ -8,6 +8,11 @@ Welcome to the {BR&SRI} Humanoid Robot Intelligence Control System framework repository! Here, we invite you to delve into the fascinating world of humanoid robotics, showcasing the innovative capabilities of our intelligent control system. +## 🔄 Run and Experiment with the build Bellande's Humanoid Robot + +[![Bellande's Humanoid](https://img.shields.io/badge/Bellande's-Humanoid%20Robot%20Package-blue?style=for-the-badge&logo=ros&color=blue)](https://github.com/Robotics-Sensors/bellande_humanoid_robotics_package) + + ## 📢 Work with Bellande Algorithms and Models through Bellande's API! [![Bellande's Algorithm through Bellande API](https://img.shields.io/badge/Bellande's%20Algorithm%20through%20Bellande's%20API-Bellande%20API-blue?style=for-the-badge&logo=ros&color=blue)](https://github.com/Robotics-Sensors/Web-ROS-API) From c1abce41ade8527d4010d86df54da8a385215062 Mon Sep 17 00:00:00 2001 From: RonaldsonBellande Date: Sun, 9 Jun 2024 16:59:47 -0400 Subject: [PATCH 236/238] latest pushes --- .../CHANGELOG.rst | 89 ------------------- .../CMakeLists.txt | 26 ------ .../package.xml | 67 -------------- 3 files changed, 182 deletions(-) delete mode 100755 humanoid_robot_intelligence_control_system_framework/CHANGELOG.rst delete mode 100644 humanoid_robot_intelligence_control_system_framework/CMakeLists.txt delete mode 100755 humanoid_robot_intelligence_control_system_framework/package.xml diff --git a/humanoid_robot_intelligence_control_system_framework/CHANGELOG.rst b/humanoid_robot_intelligence_control_system_framework/CHANGELOG.rst deleted file mode 100755 index 5376392..0000000 --- a/humanoid_robot_intelligence_control_system_framework/CHANGELOG.rst +++ /dev/null @@ -1,89 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package humanoid_robot_intelligence_control_system_framework -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.3.2 (2023-10-03) ------------------- -* Make it compatible for ROS1/ROS2 -* Fix bugs -* Update package.xml and CMakeList.txt for to the latest versions -* Contributors & Maintainer: Ronaldson Bellande - -0.3.1 (2023-09-27) ------------------- -* Starting from this point it under a new license -* Fix errors and Issues -* Rename Repository for a completely different purpose -* Make it compatible with ROS/ROS2 -* Upgrade version of all builds and make it more compatible -* Update package.xml and CMakeList.txt for to the latest versions -* Contributors & Maintainer: Ronaldson Bellande - -0.2.9 (2018-03-22) ------------------- -* added serivce for setting module -* deleted comment for debug -* modified to prevent duplicate indirect address write -* added boost system dependencies -* fixed a bug that occure when handling bulk read item that does not exist -* Contributors: Kayman, Zerom, Pyo - -0.2.8 (2018-03-20) ------------------- -* added RH-P12-RN.device file -* modified CMakeLists.txt for system dependencies (yaml-cpp) -* Contributors: Zerom, Pyo - -0.2.7 (2018-03-15) ------------------- -* changed all values read by bulk read are saved to dxl_state\_->bulk_read_table\_. -* Modified to prevent duplicate indirect address write -* fixed a bug that occur when handling bulk read item that does not exist -* changed the License and package format to version 2 -* Contributors: SCH, Zerom, Pyo - -0.2.6 (2017-08-09) ------------------- -* multi thread bug fixed. -* unnecessary debug print removed. -* OpenCR control table item name changed. (torque_enable -> dynamixel_power) -* fixed to not update update_time_stamp\_ if bulk read fails. -* Contributors: Zerom - -0.2.5 (2017-06-09) ------------------- -* updated for yaml-cpp dependencies (humanoid_robot_intelligence_control_system_controller) -* Contributors: SCH - -0.2.4 (2017-06-07) ------------------- -* added cmake_modules in package.xml -* Contributors: SCH - -0.2.3 (2017-05-23) ------------------- -* updated the cmake file for ros install -* Contributors: SCH - -0.2.2 (2017-04-24) ------------------- -* added a deivce: OpenCR -* updated humanoid_robot_intelligence_control_system_controller.cpp -* changed to read control cycle from .robot file -* Contributors: Zerom, Kayman - -0.2.1 (2016-11-23) ------------------- -* Merge the changes and update - -0.2.0 (2016-08-31) ------------------- -* updated CHANGLOG.rst for minor release - -0.1.1 (2016-08-18) ------------------- -* updated the package information - -0.1.0 (2016-08-12) ------------------- -* make a meta-package diff --git a/humanoid_robot_intelligence_control_system_framework/CMakeLists.txt b/humanoid_robot_intelligence_control_system_framework/CMakeLists.txt deleted file mode 100644 index 929650b..0000000 --- a/humanoid_robot_intelligence_control_system_framework/CMakeLists.txt +++ /dev/null @@ -1,26 +0,0 @@ -# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande -# -# 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. - -cmake_minimum_required(VERSION 3.8) -project(humanoid_robot_intelligence_control_system_framework) - -if($ENV{ROS_VERSION} EQUAL 1) - find_package(catkin REQUIRED) -else() - find_package(ament_cmake REQUIRED) -endif() - -if($ENV{ROS_VERSION} EQUAL 1) - catkin_package() -endif() diff --git a/humanoid_robot_intelligence_control_system_framework/package.xml b/humanoid_robot_intelligence_control_system_framework/package.xml deleted file mode 100755 index ec1e4f3..0000000 --- a/humanoid_robot_intelligence_control_system_framework/package.xml +++ /dev/null @@ -1,67 +0,0 @@ - - - - - humanoid_robot_intelligence_control_system_framework - 0.3.2 - ROS packages for the humanoid_robot_intelligence_control_system_framework (meta - package) - Apache 2.0 - Ronaldson Bellande - - - catkin - - humanoid_robot_intelligence_control_system_controller - humanoid_robot_intelligence_control_system_device - - humanoid_robot_intelligence_control_system_framework_common - - - humanoid_robot_intelligence_control_system_controller - - humanoid_robot_intelligence_control_system_device - - humanoid_robot_intelligence_control_system_framework_common - - humanoid_robot_intelligence_control_system_controller - humanoid_robot_intelligence_control_system_device - - humanoid_robot_intelligence_control_system_framework_common - - - ament_cmake - - humanoid_robot_intelligence_control_system_controller - humanoid_robot_intelligence_control_system_device - - humanoid_robot_intelligence_control_system_framework_common - - - humanoid_robot_intelligence_control_system_controller - - humanoid_robot_intelligence_control_system_device - - humanoid_robot_intelligence_control_system_framework_common - - humanoid_robot_intelligence_control_system_controller - humanoid_robot_intelligence_control_system_device - - humanoid_robot_intelligence_control_system_framework_common - - - From bb68e1519ac8a61b6c33fe8588504a6ac5fa4145 Mon Sep 17 00:00:00 2001 From: Ronaldson Bellande <47253433+RonaldsonBellande@users.noreply.github.com> Date: Wed, 12 Jun 2024 00:05:54 -0400 Subject: [PATCH 237/238] Update README.md Signed-off-by: Ronaldson Bellande <47253433+RonaldsonBellande@users.noreply.github.com> --- README.md | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/README.md b/README.md index b1db628..3490366 100755 --- a/README.md +++ b/README.md @@ -42,26 +42,26 @@ Welcome to the {BR&SRI} Humanoid Robot Intelligence Control System framework rep - ![GitHub license](https://img.shields.io/github/license/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework.svg) Licensed under Apache 2.0 - ![GitHub last commit](https://img.shields.io/github/last-commit/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework.svg) Latest updates -- ![Visitor & Github Clones](https://img.shields.io/badge/dynamic/json?color=2e8b57&label=Visitor%20%26%20GitHub%20Clones&query=$.count&url=https://api.github.com/repos/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework/traffic) Engaging with the community +- ![Visitor & Github Clones](https://img.shields.io/badge/dynamic/json?color=2e8b57&label=Visitor%20%26%20GitHub%20Clones&query=$.count&url=https://api.github.com/repos/Robotics-Sensors/bellande_humanoid_robot_intelligence_control_system_framework/traffic) Engaging with the community --- ## 🌐 Repository Website -Explore our [repository website](https://robotics-sensors.github.io/humanoid_robot_intelligence_control_system_framework) for detailed documentation, tutorials, and additional information about our humanoid robot intelligence control system. +Explore our [repository website](https://robotics-sensors.github.io/bellande_humanoid_robot_intelligence_control_system_framework) for detailed documentation, tutorials, and additional information about our humanoid robot intelligence control system. --- ### 🔄 Updates and Versions -- **Updated Version:** [humanoid_robot_intelligence_control_system_module](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework) +- **Updated Version:** [humanoid_robot_intelligence_control_system_module](https://github.com/Robotics-Sensors/bellande_humanoid_robot_intelligence_control_system_framework) - **Old Version/Previous Used for Different Context:** [ROBOTIS-Framework](https://github.com/ROBOTIS-GIT/ROBOTIS-Framework) --- # 🎉 Latest Release -[![Latest Release](https://img.shields.io/github/v/release/Robotics-Sensors/humanoid_robot_intelligence_control_system_tools?style=for-the-badge&color=yellow)](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework/releases/) +[![Latest Release](https://img.shields.io/github/v/release/Robotics-Sensors/bellande_humanoid_robot_intelligence_control_system_tools?style=for-the-badge&color=yellow)](https://github.com/Robotics-Sensors/bellande_humanoid_robot_intelligence_control_system_framework/releases/) --- @@ -81,4 +81,4 @@ Meet our dedicated author & maintainer, **Ronaldson Bellande**. ## 📄 License -This SDK is distributed under the [Apache License, Version 2.0](https://www.apache.org/licenses/LICENSE-2.0). For detailed information, refer to [LICENSE](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework/blob/main/LICENSE) and [NOTICE](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework/blob/main/LICENSE). +This SDK is distributed under the [Apache License, Version 2.0](https://www.apache.org/licenses/LICENSE-2.0). For detailed information, refer to [LICENSE](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_framework/blob/main/LICENSE) and [NOTICE](https://github.com/Robotics-Sensors/bellande_humanoid_robot_intelligence_control_system_framework/blob/main/LICENSE). From 9b5ff10fa4a499b1576c3b2df2a7d6695614e735 Mon Sep 17 00:00:00 2001 From: RonaldsonBellande Date: Fri, 26 Jul 2024 15:17:09 -0400 Subject: [PATCH 238/238] latest pushes --- .travis.yml | 30 - .../CHANGELOG.rst | 136 - .../CMakeLists.txt | 157 +- ...t_intelligence_control_system_controller.h | 209 -- .../package.xml | 100 +- .../setup.py | 26 + .../src/PIDController.py | 50 + ...intelligence_control_system_controller.cpp | 2625 ----------------- .../CMakeLists.txt | 60 + .../package.xml | 46 + .../setup.py | 26 + .../src/PIDController.py | 50 + .../CMakeLists.txt | 60 + .../package.xml | 46 + .../setup.py | 26 + .../src/PIDController.py | 50 + .../CMakeLists.txt | 71 + .../object_detector_processor.launch.py | 108 + .../ros1/object_detector_processor.launch | 41 + .../package.xml | 61 + .../setup.py | 26 + .../src/object_detection_processor.py | 111 + .../CMakeLists.txt | 71 + .../ros1/speech_detector_processor.launch | 41 + .../speech_detector_processor.launch.py | 108 + .../package.xml | 61 + .../setup.py | 26 + .../src/speech_detection_processor.py | 111 + 28 files changed, 1353 insertions(+), 3180 deletions(-) delete mode 100644 .travis.yml delete mode 100755 humanoid_robot_intelligence_control_system_controller/CHANGELOG.rst mode change 100755 => 100644 humanoid_robot_intelligence_control_system_controller/CMakeLists.txt delete mode 100755 humanoid_robot_intelligence_control_system_controller/include/humanoid_robot_intelligence_control_system_controller/humanoid_robot_intelligence_control_system_controller.h mode change 100755 => 100644 humanoid_robot_intelligence_control_system_controller/package.xml create mode 100644 humanoid_robot_intelligence_control_system_controller/setup.py create mode 100644 humanoid_robot_intelligence_control_system_controller/src/PIDController.py delete mode 100755 humanoid_robot_intelligence_control_system_controller/src/humanoid_robot_intelligence_control_system_controller/humanoid_robot_intelligence_control_system_controller.cpp create mode 100644 humanoid_robot_intelligence_control_system_external_sensors/CMakeLists.txt create mode 100644 humanoid_robot_intelligence_control_system_external_sensors/package.xml create mode 100644 humanoid_robot_intelligence_control_system_external_sensors/setup.py create mode 100644 humanoid_robot_intelligence_control_system_external_sensors/src/PIDController.py create mode 100644 humanoid_robot_intelligence_control_system_internal_sensors/CMakeLists.txt create mode 100644 humanoid_robot_intelligence_control_system_internal_sensors/package.xml create mode 100644 humanoid_robot_intelligence_control_system_internal_sensors/setup.py create mode 100644 humanoid_robot_intelligence_control_system_internal_sensors/src/PIDController.py create mode 100644 humanoid_robot_intelligence_control_system_object_detector/CMakeLists.txt create mode 100644 humanoid_robot_intelligence_control_system_object_detector/launch/object_detector_processor.launch.py create mode 100644 humanoid_robot_intelligence_control_system_object_detector/launch/ros1/object_detector_processor.launch create mode 100644 humanoid_robot_intelligence_control_system_object_detector/package.xml create mode 100644 humanoid_robot_intelligence_control_system_object_detector/setup.py create mode 100644 humanoid_robot_intelligence_control_system_object_detector/src/object_detection_processor.py create mode 100644 humanoid_robot_intelligence_control_system_speech_detector/CMakeLists.txt create mode 100644 humanoid_robot_intelligence_control_system_speech_detector/launch/ros1/speech_detector_processor.launch create mode 100644 humanoid_robot_intelligence_control_system_speech_detector/launch/speech_detector_processor.launch.py create mode 100644 humanoid_robot_intelligence_control_system_speech_detector/package.xml create mode 100644 humanoid_robot_intelligence_control_system_speech_detector/setup.py create mode 100644 humanoid_robot_intelligence_control_system_speech_detector/src/speech_detection_processor.py diff --git a/.travis.yml b/.travis.yml deleted file mode 100644 index 43bc280..0000000 --- a/.travis.yml +++ /dev/null @@ -1,30 +0,0 @@ -# This config file for Travis CI utilizes ros-industrial/industrial_ci package. -# For more info for the package, see https://github.com/ros-industrial/industrial_ci/blob/master/README.rst - -sudo: required -dist: trusty -services: - - docker -language: generic -python: - - "3.8" -compiler: - - gcc -notifications: - email: - on_success: change - on_failure: always - recipients: - - ronaldsonbellande@gmail.com -env: - matrix: - - ROS_DISTRO=noetic ROS_REPO=ros-shadow-fixed UPSTREAM_WORKSPACE=file OS_NAME=ubuntu OS_CODE_NAME=focal $ROSINSTALL_FILENAME=".humanoid_robot_intelligence_control_system_framework.rosinstall" -branches: - only: - - master - - noetic -install: - - git clone https://github.com/ros-industrial/industrial_ci.git .ci_config -script: - - source .ci_config/travis.sh - diff --git a/humanoid_robot_intelligence_control_system_controller/CHANGELOG.rst b/humanoid_robot_intelligence_control_system_controller/CHANGELOG.rst deleted file mode 100755 index 0b85c9e..0000000 --- a/humanoid_robot_intelligence_control_system_controller/CHANGELOG.rst +++ /dev/null @@ -1,136 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package humanoid_robot_intelligence_control_system_controller -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.3.2 (2023-10-03) ------------------- -* Make it compatible for ROS1/ROS2 -* Fix bugs -* Update package.xml and CMakeList.txt for to the latest versions -* Contributors & Maintainer: Ronaldson Bellande - -0.3.1 (2023-09-27) ------------------- -* Starting from this point it under a new license -* Fix errors and Issues -* Rename Repository for a completely different purpose -* Make it compatible with ROS/ROS2 -* Upgrade version of all builds and make it more compatible -* Update package.xml and CMakeList.txt for to the latest versions -* Contributors & Maintainer: Ronaldson Bellande - -0.3.0 (2021-05-03) ------------------- -* Update package.xml and CMakeList.txt for noetic branch -* Contributors: Ronaldson Bellande - -0.2.9 (2018-03-22) ------------------- -* added serivce for setting module -* deleted comment for debug -* modified to prevent duplicate indirect address write -* added boost system dependencies -* fixed a bug that occure when handling bulk read item that does not exist. -* Contributors: Kayman, Zerom, Pyo - -0.2.8 (2018-03-20) ------------------- -* modified CMakeLists.txt for system dependencies (yaml-cpp) -* Contributors: Zerom, Pyo - -0.2.7 (2018-03-15) ------------------- -* changed the License and package format to version 2 -* changed all values read by bulk read are saved to dxl_state\_->bulk_read_table\_. -* Modified to prevent duplicate indirect address write -* Contributors: SCH, Zerom, Pyo - -0.2.6 (2017-08-09) ------------------- -* multi thread bug fixed. -* unnecessary debug print removed. -* OpenCR control table item name changed. (torque_enable -> dynamixel_power) -* fixed to not update update_time_stamp\_ if bulk read fails. -* Contributors: Zerom - -0.2.5 (2017-06-09) ------------------- -* updated for yaml-cpp dependencies -* Contributors: SCH - -0.2.4 (2017-06-07) ------------------- -* added cmake_modules in package.xml -* Contributors: SCH - -0.2.3 (2017-05-23) ------------------- -* updated the cmake file for ros install -* Contributors: SCH - -0.2.2 (2017-04-24) ------------------- -* updated humanoid_robot_intelligence_control_system_controller.cpp -* changed to read control cycle from .robot file -* Contributors: Zerom - -0.2.1 (2016-11-23) ------------------- -* Merge the changes and update -* - Direct Control Mode bug fixed. -* update -* - added writeControlTableCallback -* - added WriteControlTable msg callback -* mode change debugging -* - optimized cpu usage by spin loop (by astumpf) -* - humanoid_robot_intelligence_control_system_controller process() : processing order changed. - * 1st : packet communication - * 2nd : processing modules -* - dependencies fixed. (Pull requests `#26 `_) -* - make setJointCtrlModuleCallback() to the thread function & improved. -* - modified dependency problem. -* - reduce CPU consumption -* Contributors: Jay Song, Pyo, Zerom, SCH - -0.2.0 (2016-08-31) ------------------- -* bug fixed (position pid gain & velocity pid gain sync write). -* added velocity_to_value_ratio to DXL Pro-H series. -* changed some debug messages. -* added velocity p/i/d gain and position i/d gain sync_write code. -* SyncWriteItem bug fixed. -* add function / modified the code simple (using auto / range based for loop) -* added XM-430-W210 / XM-430-W350 device file. -* rename ControlMode(CurrentControl -> TorqueControl) -* rename (port_to_sync_write_torque\_ -> port_to_sync_write_current\_) -* rename (present_current\_ -> present_torque\_) -* modified torque control code -* fixed typos / changed ROS_INFO -> fprintf (for processing speed) -* startTimer() : after bulkread txpacket(), need some sleep() -* changed the order of processing in the Process() function. -* added missing mutex for gazebo -* fixed crash when running in gazebo simulation -* sync write bug fix. -* added position_p_gain sync write -* MotionModule/SensorModule member variable access changed (public -> protected). -* Contributors: Jay Song, Zerom, Pyo, SCH - -0.1.1 (2016-08-18) ------------------- -* updated the package information - -0.1.0 (2016-08-12) ------------------- -* first public release for Kinetic -* modified the package information for release -* develop branch -> master branch -* function name changed : DeviceInit() -> InitDevice() -* Fixed high CPU consumption due to busy waits -* add SensorState - add Singleton template -* XM-430 / CM-740 device file added. - Sensor device added. -* added code to support the gazebo simulator -* added first bulk read failure protection code -* renewal -* Contributors: Alexander Stumpf, Jay Song, Zerom, Pyo diff --git a/humanoid_robot_intelligence_control_system_controller/CMakeLists.txt b/humanoid_robot_intelligence_control_system_controller/CMakeLists.txt old mode 100755 new mode 100644 index d90c748..e7de26d --- a/humanoid_robot_intelligence_control_system_controller/CMakeLists.txt +++ b/humanoid_robot_intelligence_control_system_controller/CMakeLists.txt @@ -1,97 +1,60 @@ -# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande -# -# 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. - -cmake_minimum_required(VERSION 3.8) -project(humanoid_robot_intelligence_control_system_controller) - -## Compile as C++11, supported in ROS Kinetic and newer -add_compile_options(-std=c++11) - - -if($ENV{ROS_VERSION} EQUAL 1) - find_package(catkin REQUIRED COMPONENTS - roscpp - roslib - std_msgs - sensor_msgs - humanoid_robot_intelligence_control_system_controller_msgs - dynamixel_sdk - humanoid_robot_intelligence_control_system_device - humanoid_robot_intelligence_control_system_framework_common - cmake_modules - ) - - find_package(Boost REQUIRED) - - find_package(PkgConfig REQUIRED) -else() - find_package(ament_cmake REQUIRED) -endif() - - -pkg_check_modules(YAML_CPP REQUIRED yaml-cpp) -find_path(YAML_CPP_INCLUDE_DIR - NAMES yaml_cpp.h - PATHS ${YAML_CPP_INCLUDE_DIRS} -) -find_library(YAML_CPP_LIBRARY - NAMES YAML_CPP - PATHS ${YAML_CPP_LIBRARY_DIRS} -) -link_directories(${YAML_CPP_LIBRARY_DIRS}) - -if(NOT ${YAML_CPP_VERSION} VERSION_LESS "0.5") -add_definitions(-DHAVE_NEW_YAMLCPP) -endif(NOT ${YAML_CPP_VERSION} VERSION_LESS "0.5") - - -if($ENV{ROS_VERSION} EQUAL 1) - catkin_package( - INCLUDE_DIRS include - LIBRARIES humanoid_robot_intelligence_control_system_controller - CATKIN_DEPENDS - roscpp - roslib - std_msgs - sensor_msgs - humanoid_robot_intelligence_control_system_controller_msgs - dynamixel_sdk - humanoid_robot_intelligence_control_system_device - humanoid_robot_intelligence_control_system_framework_common - cmake_modules - DEPENDS Boost - ) -endif() - - -include_directories( - include - ${catkin_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS} - ${YAML_CPP_INCLUDE_DIRS} -) - -add_library(humanoid_robot_intelligence_control_system_controller src/humanoid_robot_intelligence_control_system_controller/humanoid_robot_intelligence_control_system_controller.cpp) -add_dependencies(humanoid_robot_intelligence_control_system_controller ${catkin_EXPORTED_TARGETS}) -target_link_libraries(humanoid_robot_intelligence_control_system_controller ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${YAML_CPP_LIBRARIES}) - -install(TARGETS humanoid_robot_intelligence_control_system_controller - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -) +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# 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. + +cmake_minimum_required(VERSION 3.0.2) +project(humanoid_robot_intelligence_control_system_controller) + +if($ENV{ROS_VERSION} EQUAL 1) + find_package(catkin REQUIRED COMPONENTS + rospy + ) + + catkin_package( + CATKIN_DEPENDS + rospy + ) + +else() + find_package(ament_cmake REQUIRED) + find_package(ament_cmake_python REQUIRED) + find_package(rclpy REQUIRED) + find_package(std_msgs REQUIRED) +endif() + +if($ENV{ROS_VERSION} EQUAL 1) + catkin_python_setup() + + catkin_install_python(PROGRAMS + src/PIDController.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ) + + install(DIRECTORY config launch rviz + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + ) + +else() + ament_python_install_package(${PROJECT_NAME}) + + install(PROGRAMS + src/PIDController.py + DESTINATION lib/${PROJECT_NAME} + ) + + install(DIRECTORY config launch rviz + DESTINATION share/${PROJECT_NAME} + ) + + ament_package() +endif() diff --git a/humanoid_robot_intelligence_control_system_controller/include/humanoid_robot_intelligence_control_system_controller/humanoid_robot_intelligence_control_system_controller.h b/humanoid_robot_intelligence_control_system_controller/include/humanoid_robot_intelligence_control_system_controller/humanoid_robot_intelligence_control_system_controller.h deleted file mode 100755 index 019a9bb..0000000 --- a/humanoid_robot_intelligence_control_system_controller/include/humanoid_robot_intelligence_control_system_controller/humanoid_robot_intelligence_control_system_controller.h +++ /dev/null @@ -1,209 +0,0 @@ -/******************************************************************************* - * Copyright 2018 ROBOTIS CO., LTD. - * - * 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. - *******************************************************************************/ - -/* - * humanoid_robot_intelligence_control_system_controller.h - * - * Created on: 2016. 1. 15. - * Author: zerom - */ - -#ifndef ROBOTIS_CONTROLLER_ROBOTIS_CONTROLLER_H_ -#define ROBOTIS_CONTROLLER_ROBOTIS_CONTROLLER_H_ - -#include -#include -#include -#include -#include -#include -#include - -#include "humanoid_robot_intelligence_control_system_controller_msgs/GetJointModule.h" -#include "humanoid_robot_intelligence_control_system_controller_msgs/JointCtrlModule.h" -#include "humanoid_robot_intelligence_control_system_controller_msgs/LoadOffset.h" -#include "humanoid_robot_intelligence_control_system_controller_msgs/SetJointModule.h" -#include "humanoid_robot_intelligence_control_system_controller_msgs/SetModule.h" -#include "humanoid_robot_intelligence_control_system_controller_msgs/SyncWriteItem.h" -#include "humanoid_robot_intelligence_control_system_controller_msgs/WriteControlTable.h" - -#include "dynamixel_sdk/group_bulk_read.h" -#include "dynamixel_sdk/group_sync_write.h" -#include "humanoid_robot_intelligence_control_system_device/robot.h" -#include "humanoid_robot_intelligence_control_system_framework_common/motion_module.h" -#include "humanoid_robot_intelligence_control_system_framework_common/sensor_module.h" - -namespace humanoid_robot_intelligence_control_system_framework { - -enum ControllerMode { MotionModuleMode, DirectControlMode }; - -class RobotisController : public Singleton { -private: - boost::thread queue_thread_; - boost::thread gazebo_thread_; - boost::thread set_module_thread_; - boost::mutex queue_mutex_; - - bool init_pose_loaded_; - bool is_timer_running_; - bool is_offset_enabled_; - double offset_ratio_; - bool stop_timer_; - pthread_t timer_thread_; - ControllerMode controller_mode_; - - std::list motion_modules_; - std::list sensor_modules_; - std::vector direct_sync_write_; - - std::map sensor_result_; - - void gazeboTimerThread(); - void msgQueueThread(); - void setCtrlModuleThread(std::string ctrl_module); - void setJointCtrlModuleThread( - const humanoid_robot_intelligence_control_system_controller_msgs::JointCtrlModule::ConstPtr &msg); - - bool isTimerStopped(); - void initializeSyncWrite(); - -public: - const int NONE_GAIN = 65535; - bool DEBUG_PRINT; - Robot *robot_; - - bool gazebo_mode_; - std::string gazebo_robot_name_; - - /* bulk read */ - std::map port_to_bulk_read_; - - /* sync write */ - std::map - port_to_sync_write_position_; - std::map - port_to_sync_write_velocity_; - std::map - port_to_sync_write_current_; - std::map - port_to_sync_write_position_p_gain_; - std::map - port_to_sync_write_position_i_gain_; - std::map - port_to_sync_write_position_d_gain_; - std::map - port_to_sync_write_velocity_p_gain_; - std::map - port_to_sync_write_velocity_i_gain_; - std::map - port_to_sync_write_velocity_d_gain_; - - /* publisher */ - ros::Publisher goal_joint_state_pub_; - ros::Publisher present_joint_state_pub_; - ros::Publisher current_module_pub_; - - std::map gazebo_joint_position_pub_; - std::map gazebo_joint_velocity_pub_; - std::map gazebo_joint_effort_pub_; - - static void *timerThread(void *param); - - RobotisController(); - - bool initialize(const std::string robot_file_path, - const std::string init_file_path); - void initializeDevice(const std::string init_file_path); - void process(); - - void addMotionModule(MotionModule *module); - void removeMotionModule(MotionModule *module); - void addSensorModule(SensorModule *module); - void removeSensorModule(SensorModule *module); - - void startTimer(); - void stopTimer(); - bool isTimerRunning(); - - void setCtrlModule(std::string module_name); - void loadOffset(const std::string path); - - /* ROS Topic Callback Functions */ - void writeControlTableCallback( - const humanoid_robot_intelligence_control_system_controller_msgs::WriteControlTable::ConstPtr &msg); - void syncWriteItemCallback( - const humanoid_robot_intelligence_control_system_controller_msgs::SyncWriteItem::ConstPtr &msg); - void setControllerModeCallback(const std_msgs::String::ConstPtr &msg); - void setJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg); - void setJointCtrlModuleCallback( - const humanoid_robot_intelligence_control_system_controller_msgs::JointCtrlModule::ConstPtr &msg); - void setCtrlModuleCallback(const std_msgs::String::ConstPtr &msg); - void enableOffsetCallback(const std_msgs::Bool::ConstPtr &msg); - bool getJointCtrlModuleService( - humanoid_robot_intelligence_control_system_controller_msgs::GetJointModule::Request &req, - humanoid_robot_intelligence_control_system_controller_msgs::GetJointModule::Response &res); - bool setJointCtrlModuleService( - humanoid_robot_intelligence_control_system_controller_msgs::SetJointModule::Request &req, - humanoid_robot_intelligence_control_system_controller_msgs::SetJointModule::Response &res); - bool setCtrlModuleService( - humanoid_robot_intelligence_control_system_controller_msgs::SetModule::Request &req, - humanoid_robot_intelligence_control_system_controller_msgs::SetModule::Response &res); - bool - loadOffsetService(humanoid_robot_intelligence_control_system_controller_msgs::LoadOffset::Request &req, - humanoid_robot_intelligence_control_system_controller_msgs::LoadOffset::Response &res); - - void gazeboJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg); - - int ping(const std::string joint_name, uint8_t *error = 0); - int ping(const std::string joint_name, uint16_t *model_number, - uint8_t *error = 0); - - int action(const std::string joint_name); - int reboot(const std::string joint_name, uint8_t *error = 0); - int factoryReset(const std::string joint_name, uint8_t option = 0, - uint8_t *error = 0); - - int read(const std::string joint_name, uint16_t address, uint16_t length, - uint8_t *data, uint8_t *error = 0); - int readCtrlItem(const std::string joint_name, const std::string item_name, - uint32_t *data, uint8_t *error = 0); - - int read1Byte(const std::string joint_name, uint16_t address, uint8_t *data, - uint8_t *error = 0); - int read2Byte(const std::string joint_name, uint16_t address, uint16_t *data, - uint8_t *error = 0); - int read4Byte(const std::string joint_name, uint16_t address, uint32_t *data, - uint8_t *error = 0); - - int write(const std::string joint_name, uint16_t address, uint16_t length, - uint8_t *data, uint8_t *error = 0); - int writeCtrlItem(const std::string joint_name, const std::string item_name, - uint32_t data, uint8_t *error = 0); - - int write1Byte(const std::string joint_name, uint16_t address, uint8_t data, - uint8_t *error = 0); - int write2Byte(const std::string joint_name, uint16_t address, uint16_t data, - uint8_t *error = 0); - int write4Byte(const std::string joint_name, uint16_t address, uint32_t data, - uint8_t *error = 0); - - int regWrite(const std::string joint_name, uint16_t address, uint16_t length, - uint8_t *data, uint8_t *error = 0); -}; - -} // namespace humanoid_robot_intelligence_control_system_framework - -#endif /* ROBOTIS_CONTROLLER_ROBOTIS_CONTROLLER_H_ */ diff --git a/humanoid_robot_intelligence_control_system_controller/package.xml b/humanoid_robot_intelligence_control_system_controller/package.xml old mode 100755 new mode 100644 index 63452d9..0ab0b27 --- a/humanoid_robot_intelligence_control_system_controller/package.xml +++ b/humanoid_robot_intelligence_control_system_controller/package.xml @@ -17,96 +17,30 @@ the License. humanoid_robot_intelligence_control_system_controller - 0.3.2 - humanoid_robot_intelligence_control_system_controller package for ROBOTIS's platform - like Manipulator-H, THORMANG and OP series + 0.0.1 + + This Package is for Detection Math + Apache 2.0 Ronaldson Bellande - catkin - - roscpp - roslib - std_msgs - sensor_msgs - - humanoid_robot_intelligence_control_system_controller_msgs - dynamixel_sdk - humanoid_robot_intelligence_control_system_device - - humanoid_robot_intelligence_control_system_framework_common - cmake_modules - yaml-cpp - - roscpp - roslib - std_msgs - sensor_msgs - - humanoid_robot_intelligence_control_system_controller_msgs - dynamixel_sdk - - humanoid_robot_intelligence_control_system_device - - humanoid_robot_intelligence_control_system_framework_common - cmake_modules - yaml-cpp - - roscpp - roslib - std_msgs - sensor_msgs - - humanoid_robot_intelligence_control_system_controller_msgs - dynamixel_sdk - humanoid_robot_intelligence_control_system_device - - humanoid_robot_intelligence_control_system_framework_common - cmake_modules - yaml-cpp - - ament_cmake + ament_cmake_python - roscpp - roslib - std_msgs - sensor_msgs - - humanoid_robot_intelligence_control_system_controller_msgs - dynamixel_sdk - humanoid_robot_intelligence_control_system_device - - humanoid_robot_intelligence_control_system_framework_common - cmake_modules - yaml-cpp + + rospy - roscpp - roslib - std_msgs - sensor_msgs - - humanoid_robot_intelligence_control_system_controller_msgs - dynamixel_sdk - - humanoid_robot_intelligence_control_system_device - - humanoid_robot_intelligence_control_system_framework_common - cmake_modules - yaml-cpp + + rclpy - roscpp - roslib - std_msgs - sensor_msgs - - humanoid_robot_intelligence_control_system_controller_msgs - dynamixel_sdk - humanoid_robot_intelligence_control_system_device - - humanoid_robot_intelligence_control_system_framework_common - cmake_modules - yaml-cpp + + python3-opencv + python3-yaml + usb_cam + + catkin + ament_cmake + diff --git a/humanoid_robot_intelligence_control_system_controller/setup.py b/humanoid_robot_intelligence_control_system_controller/setup.py new file mode 100644 index 0000000..cbd6630 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_controller/setup.py @@ -0,0 +1,26 @@ +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +# fetch values from package.xml +setup_args = generate_distutils_setup( + scripts=['src/PIDController.py'], + packages=['humanoid_robot_intelligence_control_system_controller'], + package_dir={'': 'src'}, +) + +setup(**setup_args) diff --git a/humanoid_robot_intelligence_control_system_controller/src/PIDController.py b/humanoid_robot_intelligence_control_system_controller/src/PIDController.py new file mode 100644 index 0000000..0a87c58 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_controller/src/PIDController.py @@ -0,0 +1,50 @@ +#!/usr/bin/env python3 + +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +import time +from typing import Dict, List, Tuple + +class PIDController: + def __init__(self, gains: Tuple[float, float, float], name: str, output_limits: Tuple[float, float] = (-float('inf'), float('inf'))): + self.kp, self.ki, self.kd = gains + self.name = name + self.output_limits = output_limits + self.reset() + + def reset(self): + self.last_error = 0 + self.integral = 0 + self.last_time = time.time() + + def compute(self, setpoint: float, process_variable: float) -> float: + current_time = time.time() + dt = current_time - self.last_time + error = setpoint - process_variable + + self.integral += error * dt + derivative = (error - self.last_error) / dt if dt > 0 else 0 + + output = self.kp * error + self.ki * self.integral + self.kd * derivative + output = max(min(output, self.output_limits[1]), self.output_limits[0]) + + self.last_error = error + self.last_time = current_time + + return output + + def update_config(self, gains: Tuple[float, float, float]): + self.kp, self.ki, self.kd = gains diff --git a/humanoid_robot_intelligence_control_system_controller/src/humanoid_robot_intelligence_control_system_controller/humanoid_robot_intelligence_control_system_controller.cpp b/humanoid_robot_intelligence_control_system_controller/src/humanoid_robot_intelligence_control_system_controller/humanoid_robot_intelligence_control_system_controller.cpp deleted file mode 100755 index 1204a29..0000000 --- a/humanoid_robot_intelligence_control_system_controller/src/humanoid_robot_intelligence_control_system_controller/humanoid_robot_intelligence_control_system_controller.cpp +++ /dev/null @@ -1,2625 +0,0 @@ -/******************************************************************************* - * Copyright 2018 ROBOTIS CO., LTD. - * - * 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. - *******************************************************************************/ - -/* - * humanoid_robot_intelligence_control_system_controller.cpp - * - * Created on: 2016. 1. 15. - * Author: zerom - */ - -#include -#include - -#include "humanoid_robot_intelligence_control_system_controller/humanoid_robot_intelligence_control_system_controller.h" - -using namespace humanoid_robot_intelligence_control_system_framework; - -RobotisController::RobotisController() - : is_timer_running_(false), is_offset_enabled_(true), offset_ratio_(1.0), - stop_timer_(false), init_pose_loaded_(false), timer_thread_(0), - controller_mode_(MotionModuleMode), DEBUG_PRINT(false), robot_(0), - gazebo_mode_(false), - gazebo_robot_name_("humanoid_robot_intelligence_control_system") { - direct_sync_write_.clear(); -} - -void RobotisController::initializeSyncWrite() { - if (gazebo_mode_ == true) - return; - - // ROS_INFO("FIRST BULKREAD"); - for (auto &it : port_to_bulk_read_) - it.second->txRxPacket(); - for (auto &it : port_to_bulk_read_) { - int error_count = 0; - int result = COMM_SUCCESS; - do { - if (++error_count > 10) { - ROS_ERROR("[RobotisController] first bulk read fail!!"); - exit(-1); - } - usleep(10 * 1000); - result = it.second->txRxPacket(); - } while (result != COMM_SUCCESS); - } - init_pose_loaded_ = true; - // ROS_INFO("FIRST BULKREAD END"); - - // clear syncwrite param setting - for (auto &it : port_to_sync_write_position_) { - if (it.second != NULL) - it.second->clearParam(); - } - for (auto &it : port_to_sync_write_position_p_gain_) { - if (it.second != NULL) - it.second->clearParam(); - } - for (auto &it : port_to_sync_write_position_i_gain_) { - if (it.second != NULL) - it.second->clearParam(); - } - for (auto &it : port_to_sync_write_position_d_gain_) { - if (it.second != NULL) - it.second->clearParam(); - } - for (auto &it : port_to_sync_write_velocity_) { - if (it.second != NULL) - it.second->clearParam(); - } - for (auto &it : port_to_sync_write_velocity_p_gain_) { - if (it.second != NULL) - it.second->clearParam(); - } - for (auto &it : port_to_sync_write_velocity_i_gain_) { - if (it.second != NULL) - it.second->clearParam(); - } - for (auto &it : port_to_sync_write_velocity_d_gain_) { - if (it.second != NULL) - it.second->clearParam(); - } - for (auto &it : port_to_sync_write_current_) { - if (it.second != NULL) - it.second->clearParam(); - } - - // set init syncwrite param(from data of bulkread) - for (auto &it : robot_->dxls_) { - std::string joint_name = it.first; - Dynamixel *dxl = it.second; - - for (int i = 0; i < dxl->bulk_read_items_.size(); i++) { - uint32_t read_data = 0; - uint8_t sync_write_data[4]; - - if (port_to_bulk_read_[dxl->port_name_]->isAvailable( - dxl->id_, dxl->bulk_read_items_[i]->address_, - dxl->bulk_read_items_[i]->data_length_) == true) { - read_data = port_to_bulk_read_[dxl->port_name_]->getData( - dxl->id_, dxl->bulk_read_items_[i]->address_, - dxl->bulk_read_items_[i]->data_length_); - - sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(read_data)); - sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(read_data)); - sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(read_data)); - sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(read_data)); - - if ((dxl->present_position_item_ != 0) && - (dxl->bulk_read_items_[i]->item_name_ == - dxl->present_position_item_->item_name_)) { - dxl->dxl_state_->present_position_ = - dxl->convertValue2Radian(read_data) - - dxl->dxl_state_->position_offset_ * offset_ratio_; - dxl->dxl_state_->goal_position_ = dxl->dxl_state_->present_position_; - - port_to_sync_write_position_[dxl->port_name_]->addParam( - dxl->id_, sync_write_data); - } else if ((dxl->position_p_gain_item_ != 0) && - (dxl->bulk_read_items_[i]->item_name_ == - dxl->position_p_gain_item_->item_name_)) { - dxl->dxl_state_->position_p_gain_ = read_data; - } else if ((dxl->position_i_gain_item_ != 0) && - (dxl->bulk_read_items_[i]->item_name_ == - dxl->position_i_gain_item_->item_name_)) { - dxl->dxl_state_->position_i_gain_ = read_data; - } else if ((dxl->position_d_gain_item_ != 0) && - (dxl->bulk_read_items_[i]->item_name_ == - dxl->position_d_gain_item_->item_name_)) { - dxl->dxl_state_->position_d_gain_ = read_data; - } else if ((dxl->present_velocity_item_ != 0) && - (dxl->bulk_read_items_[i]->item_name_ == - dxl->present_velocity_item_->item_name_)) { - dxl->dxl_state_->present_velocity_ = - dxl->convertValue2Velocity(read_data); - dxl->dxl_state_->goal_velocity_ = dxl->dxl_state_->present_velocity_; - } else if ((dxl->velocity_p_gain_item_ != 0) && - (dxl->bulk_read_items_[i]->item_name_ == - dxl->velocity_p_gain_item_->item_name_)) { - dxl->dxl_state_->velocity_p_gain_ = read_data; - } else if ((dxl->velocity_i_gain_item_ != 0) && - (dxl->bulk_read_items_[i]->item_name_ == - dxl->velocity_i_gain_item_->item_name_)) { - dxl->dxl_state_->velocity_i_gain_ = read_data; - } else if ((dxl->velocity_d_gain_item_ != 0) && - (dxl->bulk_read_items_[i]->item_name_ == - dxl->velocity_d_gain_item_->item_name_)) { - dxl->dxl_state_->velocity_d_gain_ = read_data; - } else if ((dxl->present_current_item_ != 0) && - (dxl->bulk_read_items_[i]->item_name_ == - dxl->present_current_item_->item_name_)) { - dxl->dxl_state_->present_torque_ = - dxl->convertValue2Torque(read_data); - dxl->dxl_state_->goal_torque_ = dxl->dxl_state_->present_torque_; - } - } - } - } -} - -bool RobotisController::initialize(const std::string robot_file_path, - const std::string init_file_path) { - std::string dev_desc_dir_path = - ros::package::getPath( - "humanoid_robot_intelligence_control_system_device") + - "/devices"; - - // load robot info : port , device - robot_ = new Robot(robot_file_path, dev_desc_dir_path); - - if (gazebo_mode_ == true) { - queue_thread_ = - boost::thread(boost::bind(&RobotisController::msgQueueThread, this)); - return true; - } - - for (auto &it : robot_->ports_) { - std::string port_name = it.first; - dynamixel::PortHandler *port = it.second; - dynamixel::PacketHandler *default_pkt_handler = - dynamixel::PacketHandler::getPacketHandler(2.0); - - if (port->setBaudRate(port->getBaudRate()) == false) { - ROS_ERROR("PORT [%s] SETUP ERROR! (baudrate: %d)", port_name.c_str(), - port->getBaudRate()); - exit(-1); - } - - // get the default device info of the port - std::string default_device_name = robot_->port_default_device_[port_name]; - auto dxl_it = robot_->dxls_.find(default_device_name); - auto sensor_it = robot_->sensors_.find(default_device_name); - if (dxl_it != robot_->dxls_.end()) { - Dynamixel *default_device = dxl_it->second; - default_pkt_handler = dynamixel::PacketHandler::getPacketHandler( - default_device->protocol_version_); - - if (default_device->goal_position_item_ != 0) { - port_to_sync_write_position_[port_name] = new dynamixel::GroupSyncWrite( - port, default_pkt_handler, - default_device->goal_position_item_->address_, - default_device->goal_position_item_->data_length_); - } - - if (default_device->position_p_gain_item_ != 0) { - port_to_sync_write_position_p_gain_[port_name] = - new dynamixel::GroupSyncWrite( - port, default_pkt_handler, - default_device->position_p_gain_item_->address_, - default_device->position_p_gain_item_->data_length_); - } - - if (default_device->position_i_gain_item_ != 0) { - port_to_sync_write_position_i_gain_[port_name] = - new dynamixel::GroupSyncWrite( - port, default_pkt_handler, - default_device->position_i_gain_item_->address_, - default_device->position_i_gain_item_->data_length_); - } - - if (default_device->position_d_gain_item_ != 0) { - port_to_sync_write_position_d_gain_[port_name] = - new dynamixel::GroupSyncWrite( - port, default_pkt_handler, - default_device->position_d_gain_item_->address_, - default_device->position_d_gain_item_->data_length_); - } - - if (default_device->goal_velocity_item_ != 0) { - port_to_sync_write_velocity_[port_name] = new dynamixel::GroupSyncWrite( - port, default_pkt_handler, - default_device->goal_velocity_item_->address_, - default_device->goal_velocity_item_->data_length_); - } - - if (default_device->velocity_p_gain_item_ != 0) { - port_to_sync_write_velocity_p_gain_[port_name] = - new dynamixel::GroupSyncWrite( - port, default_pkt_handler, - default_device->velocity_p_gain_item_->address_, - default_device->velocity_p_gain_item_->data_length_); - } - - if (default_device->velocity_i_gain_item_ != 0) { - port_to_sync_write_velocity_i_gain_[port_name] = - new dynamixel::GroupSyncWrite( - port, default_pkt_handler, - default_device->velocity_i_gain_item_->address_, - default_device->velocity_i_gain_item_->data_length_); - } - - if (default_device->velocity_d_gain_item_ != 0) { - port_to_sync_write_velocity_d_gain_[port_name] = - new dynamixel::GroupSyncWrite( - port, default_pkt_handler, - default_device->velocity_d_gain_item_->address_, - default_device->velocity_d_gain_item_->data_length_); - } - - if (default_device->goal_current_item_ != 0) { - port_to_sync_write_current_[port_name] = new dynamixel::GroupSyncWrite( - port, default_pkt_handler, - default_device->goal_current_item_->address_, - default_device->goal_current_item_->data_length_); - } - } else if (sensor_it != robot_->sensors_.end()) { - Sensor *_default_device = sensor_it->second; - default_pkt_handler = dynamixel::PacketHandler::getPacketHandler( - _default_device->protocol_version_); - } - - port_to_bulk_read_[port_name] = - new dynamixel::GroupBulkRead(port, default_pkt_handler); - } - - // (for loop) check all dxls are connected. - for (auto &it : robot_->dxls_) { - std::string joint_name = it.first; - Dynamixel *dxl = it.second; - - if (ping(joint_name) != 0) { - usleep(10 * 1000); - if (ping(joint_name) != 0) - ROS_ERROR("JOINT[%s] does NOT respond!!", joint_name.c_str()); - } - } - - initializeDevice(init_file_path); - - queue_thread_ = - boost::thread(boost::bind(&RobotisController::msgQueueThread, this)); - return true; -} - -void RobotisController::initializeDevice(const std::string init_file_path) { - // device initialize - if (DEBUG_PRINT) - ROS_WARN("INIT FILE LOAD"); - - YAML::Node doc; - try { - doc = YAML::LoadFile(init_file_path.c_str()); - - for (YAML::const_iterator it_doc = doc.begin(); it_doc != doc.end(); - it_doc++) { - std::string joint_name = it_doc->first.as(); - - YAML::Node joint_node = doc[joint_name]; - if (joint_node.size() == 0) - continue; - - Dynamixel *dxl = NULL; - auto dxl_it = robot_->dxls_.find(joint_name); - if (dxl_it != robot_->dxls_.end()) - dxl = dxl_it->second; - - if (dxl == NULL) { - ROS_WARN("Joint [%s] was not found.", joint_name.c_str()); - continue; - } - if (DEBUG_PRINT) - ROS_INFO("JOINT_NAME: %s", joint_name.c_str()); - - uint8_t torque_enabled = 0; - read1Byte(joint_name, dxl->torque_enable_item_->address_, - &torque_enabled); - - for (YAML::const_iterator it_joint = joint_node.begin(); - it_joint != joint_node.end(); it_joint++) { - std::string item_name = it_joint->first.as(); - - if (DEBUG_PRINT) - ROS_INFO(" ITEM_NAME: %s", item_name.c_str()); - - uint32_t value = it_joint->second.as(); - - ControlTableItem *item = dxl->ctrl_table_[item_name]; - if (item == NULL) { - ROS_WARN("Control Item [%s] was not found.", item_name.c_str()); - continue; - } - - if (item->memory_type_ == EEPROM) { - uint8_t data8 = 0; - uint16_t data16 = 0; - uint32_t data32 = 0; - - switch (item->data_length_) { - case 1: - read1Byte(joint_name, item->address_, &data8); - if (data8 == value) - continue; - break; - case 2: - read2Byte(joint_name, item->address_, &data16); - if (data16 == value) - continue; - break; - case 4: - read4Byte(joint_name, item->address_, &data32); - if (data32 == value) - continue; - break; - default: - break; - } - - if (torque_enabled == 1) { - ROS_ERROR( - "################\nThe initial value of the EEPROM area has " - "been changed. \nTurn off Torque Enable and try again."); - exit(-1); - } - } - - switch (item->data_length_) { - case 1: - write1Byte(joint_name, item->address_, (uint8_t)value); - break; - case 2: - write2Byte(joint_name, item->address_, (uint16_t)value); - break; - case 4: - write4Byte(joint_name, item->address_, value); - break; - default: - break; - } - - if (item->memory_type_ == EEPROM) { - // Write to EEPROM -> delay is required (max delay: 55 msec per byte) - usleep(item->data_length_ * 55 * 1000); - } - } - } - } catch (const std::exception &e) { - ROS_INFO("Dynamixel Init file not found."); - } - - // [ BulkRead ] StartAddress : Present Position , Length : 10 ( - // Position/Velocity/Current ) - for (auto &it : robot_->ports_) { - if (port_to_bulk_read_[it.first] != 0) - port_to_bulk_read_[it.first]->clearParam(); - } - for (auto &it : robot_->dxls_) { - std::string joint_name = it.first; - Dynamixel *dxl = it.second; - - if (dxl == NULL) - continue; - - int bulkread_start_addr = 0; - int bulkread_data_length = 0; - - // // bulk read default : present position - // if(dxl->present_position_item != 0) - // { - // bulkread_start_addr = dxl->present_position_item->address; - // bulkread_data_length = dxl->present_position_item->data_length; - // } - - uint8_t torque_enabled = 0; - read1Byte(joint_name, dxl->torque_enable_item_->address_, &torque_enabled); - - // calculate bulk read start address & data length - auto indirect_addr_it = dxl->ctrl_table_.find(INDIRECT_ADDRESS_1); - if (indirect_addr_it != dxl->ctrl_table_.end()) // INDIRECT_ADDRESS_1 exist - { - if (dxl->bulk_read_items_.size() != 0) { - uint16_t data16 = 0; - - bulkread_start_addr = dxl->bulk_read_items_[0]->address_; - bulkread_data_length = 0; - - // set indirect address - int indirect_addr = indirect_addr_it->second->address_; - for (int i = 0; i < dxl->bulk_read_items_.size(); i++) { - int addr_leng = dxl->bulk_read_items_[i]->data_length_; - - bulkread_data_length += addr_leng; - for (int l = 0; l < addr_leng; l++) { - // ROS_WARN("[%12s] INDIR_ADDR: %d, ITEM_ADDR: %d", - // joint_name.c_str(), indirect_addr, - // dxl->ctrl_table[dxl->bulk_read_items[i]->item_name]->address + - // _l); - - read2Byte(joint_name, indirect_addr, &data16); - if (data16 != dxl->ctrl_table_[dxl->bulk_read_items_[i]->item_name_] - ->address_ + - l) { - if (torque_enabled == 1) { - ROS_ERROR("################\nThe indirect address of the " - "EEPROM area has been changed. \nTurn off Torque " - "Enable and try again."); - exit(-1); - } - write2Byte(joint_name, indirect_addr, - dxl->ctrl_table_[dxl->bulk_read_items_[i]->item_name_] - ->address_ + - l); - } - indirect_addr += 2; - } - } - } - } else // INDIRECT_ADDRESS_1 NOT exist - { - if (dxl->bulk_read_items_.size() != 0) { - bulkread_start_addr = dxl->bulk_read_items_[0]->address_; - bulkread_data_length = 0; - - ControlTableItem *last_item = dxl->bulk_read_items_[0]; - - for (int i = 0; i < dxl->bulk_read_items_.size(); i++) { - int addr = dxl->bulk_read_items_[i]->address_; - if (addr < bulkread_start_addr) - bulkread_start_addr = addr; - else if (last_item->address_ < addr) - last_item = dxl->bulk_read_items_[i]; - } - - bulkread_data_length = - last_item->address_ - bulkread_start_addr + last_item->data_length_; - } - } - - // ROS_WARN("[%12s] start_addr: %d, data_length: %d", joint_name.c_str(), - // bulkread_start_addr, bulkread_data_length); - if (bulkread_start_addr != 0) - port_to_bulk_read_[dxl->port_name_]->addParam( - dxl->id_, bulkread_start_addr, bulkread_data_length); - - // Torque ON - if (writeCtrlItem(joint_name, dxl->torque_enable_item_->item_name_, 1) != - COMM_SUCCESS) - writeCtrlItem(joint_name, dxl->torque_enable_item_->item_name_, 1); - } - - for (auto &it : robot_->sensors_) { - std::string sensor_name = it.first; - Sensor *sensor = it.second; - - if (sensor == NULL) - continue; - - int bulkread_start_addr = 0; - int bulkread_data_length = 0; - - // calculate bulk read start address & data length - auto indirect_addr_it = sensor->ctrl_table_.find(INDIRECT_ADDRESS_1); - if (indirect_addr_it != - sensor->ctrl_table_.end()) // INDIRECT_ADDRESS_1 exist - { - if (sensor->bulk_read_items_.size() != 0) { - uint16_t data16 = 0; - - bulkread_start_addr = sensor->bulk_read_items_[0]->address_; - bulkread_data_length = 0; - - // set indirect address - int indirect_addr = indirect_addr_it->second->address_; - for (int i = 0; i < sensor->bulk_read_items_.size(); i++) { - int addr_leng = sensor->bulk_read_items_[i]->data_length_; - - bulkread_data_length += addr_leng; - for (int l = 0; l < addr_leng; l++) { - // ROS_WARN("[%12s] INDIR_ADDR: %d, ITEM_ADDR: %d", - // sensor_name.c_str(), indirect_addr, - // sensor->ctrl_table[sensor->bulk_read_items[i]->item_name]->address - // + _l); - read2Byte(sensor_name, indirect_addr, &data16); - if (data16 != - sensor->ctrl_table_[sensor->bulk_read_items_[i]->item_name_] - ->address_ + - l) { - write2Byte( - sensor_name, indirect_addr, - sensor->ctrl_table_[sensor->bulk_read_items_[i]->item_name_] - ->address_ + - l); - } - indirect_addr += 2; - } - } - } - } else // INDIRECT_ADDRESS_1 NOT exist - { - if (sensor->bulk_read_items_.size() != 0) { - bulkread_start_addr = sensor->bulk_read_items_[0]->address_; - bulkread_data_length = 0; - - ControlTableItem *last_item = sensor->bulk_read_items_[0]; - - for (int i = 0; i < sensor->bulk_read_items_.size(); i++) { - int addr = sensor->bulk_read_items_[i]->address_; - if (addr < bulkread_start_addr) - bulkread_start_addr = addr; - else if (last_item->address_ < addr) - last_item = sensor->bulk_read_items_[i]; - } - - bulkread_data_length = - last_item->address_ - bulkread_start_addr + last_item->data_length_; - } - } - - // ROS_WARN("[%12s] start_addr: %d, data_length: %d", sensor_name.c_str(), - // bulkread_start_addr, bulkread_data_length); - if (bulkread_start_addr != 0) - port_to_bulk_read_[sensor->port_name_]->addParam( - sensor->id_, bulkread_start_addr, bulkread_data_length); - } -} - -void RobotisController::gazeboTimerThread() { - ros::Rate gazebo_rate(1000 / robot_->getControlCycle()); - - while (!stop_timer_) { - if (init_pose_loaded_ == true) - process(); - gazebo_rate.sleep(); - } -} - -void RobotisController::msgQueueThread() { - ros::NodeHandle ros_node; - ros::CallbackQueue callback_queue; - - ros_node.setCallbackQueue(&callback_queue); - - /* subscriber */ - ros::Subscriber write_control_table_sub = ros_node.subscribe( - "/humanoid_robot_intelligence_control_system/write_control_table", 5, - &RobotisController::writeControlTableCallback, this); - ros::Subscriber sync_write_item_sub = ros_node.subscribe( - "/humanoid_robot_intelligence_control_system/sync_write_item", 10, - &RobotisController::syncWriteItemCallback, this); - ros::Subscriber joint_ctrl_modules_sub = ros_node.subscribe( - "/humanoid_robot_intelligence_control_system/set_joint_ctrl_modules", 10, - &RobotisController::setJointCtrlModuleCallback, this); - ros::Subscriber enable_ctrl_module_sub = ros_node.subscribe( - "/humanoid_robot_intelligence_control_system/enable_ctrl_module", 10, - &RobotisController::setCtrlModuleCallback, this); - ros::Subscriber control_mode_sub = ros_node.subscribe( - "/humanoid_robot_intelligence_control_system/set_control_mode", 10, - &RobotisController::setControllerModeCallback, this); - ros::Subscriber joint_states_sub = ros_node.subscribe( - "/humanoid_robot_intelligence_control_system/set_joint_states", 10, - &RobotisController::setJointStatesCallback, this); - ros::Subscriber enable_offset_sub = ros_node.subscribe( - "/humanoid_robot_intelligence_control_system/enable_offset", 10, - &RobotisController::enableOffsetCallback, this); - - ros::Subscriber gazebo_joint_states_sub; - if (gazebo_mode_ == true) - gazebo_joint_states_sub = - ros_node.subscribe("/" + gazebo_robot_name_ + "/joint_states", 10, - &RobotisController::gazeboJointStatesCallback, this); - - /* publisher */ - goal_joint_state_pub_ = ros_node.advertise( - "/humanoid_robot_intelligence_control_system/goal_joint_states", 10); - present_joint_state_pub_ = ros_node.advertise( - "/humanoid_robot_intelligence_control_system/present_joint_states", 10); - current_module_pub_ = ros_node.advertise< - humanoid_robot_intelligence_control_system_controller_msgs:: - JointCtrlModule>( - "/humanoid_robot_intelligence_control_system/present_joint_ctrl_modules", - 10); - - if (gazebo_mode_ == true) { - for (auto &it : robot_->dxls_) { - gazebo_joint_position_pub_[it.first] = - ros_node.advertise( - "/" + gazebo_robot_name_ + "/" + it.first + "_position/command", - 1); - gazebo_joint_velocity_pub_[it.first] = - ros_node.advertise( - "/" + gazebo_robot_name_ + "/" + it.first + "_velocity/command", - 1); - gazebo_joint_effort_pub_[it.first] = - ros_node.advertise( - "/" + gazebo_robot_name_ + "/" + it.first + "_effort/command", 1); - } - } - - /* service */ - ros::ServiceServer get_joint_module_server = ros_node.advertiseService( - "/humanoid_robot_intelligence_control_system/" - "get_present_joint_ctrl_modules", - &RobotisController::getJointCtrlModuleService, this); - ros::ServiceServer set_joint_module_server = ros_node.advertiseService( - "/humanoid_robot_intelligence_control_system/" - "set_present_joint_ctrl_modules", - &RobotisController::setJointCtrlModuleService, this); - ros::ServiceServer set_module_server = ros_node.advertiseService( - "/humanoid_robot_intelligence_control_system/set_present_ctrl_modules", - &RobotisController::setCtrlModuleService, this); - ros::ServiceServer load_offset_server = ros_node.advertiseService( - "/humanoid_robot_intelligence_control_system/load_offset", - &RobotisController::loadOffsetService, this); - - ros::WallDuration duration(robot_->getControlCycle() / 1000.0); - while (ros_node.ok()) - callback_queue.callAvailable(duration); -} - -void *RobotisController::timerThread(void *param) { - RobotisController *controller = (RobotisController *)param; - static struct timespec next_time; - static struct timespec curr_time; - - ROS_DEBUG("controller::thread_proc started"); - - clock_gettime(CLOCK_MONOTONIC, &next_time); - - while (!controller->stop_timer_) { - next_time.tv_sec += - (next_time.tv_nsec + controller->robot_->getControlCycle() * 1000000) / - 1000000000; - next_time.tv_nsec = - (next_time.tv_nsec + controller->robot_->getControlCycle() * 1000000) % - 1000000000; - - controller->process(); - - clock_gettime(CLOCK_MONOTONIC, &curr_time); - long delta_nsec = (next_time.tv_sec - curr_time.tv_sec) * 1000000000 + - (next_time.tv_nsec - curr_time.tv_nsec); - if (delta_nsec < -100000) { - if (controller->DEBUG_PRINT == true) { - fprintf(stderr, - "[RobotisController::ThreadProc] NEXT TIME < CURR TIME.. " - "(%f)[%ld.%09ld / %ld.%09ld]", - delta_nsec / 1000000.0, (long)next_time.tv_sec, - (long)next_time.tv_nsec, (long)curr_time.tv_sec, - (long)curr_time.tv_nsec); - } - - // next_time = curr_time + 3 msec - next_time.tv_sec = - curr_time.tv_sec + (curr_time.tv_nsec + 3000000) / 1000000000; - next_time.tv_nsec = (curr_time.tv_nsec + 3000000) % 1000000000; - } - - clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &next_time, NULL); - } - return 0; -} - -void RobotisController::startTimer() { - if (this->is_timer_running_ == true) - return; - - if (this->gazebo_mode_ == true) { - // create and start the thread - gazebo_thread_ = - boost::thread(boost::bind(&RobotisController::gazeboTimerThread, this)); - } else { - initializeSyncWrite(); - - for (auto &it : port_to_bulk_read_) { - it.second->txPacket(); - } - - usleep(8 * 1000); - - int error; - struct sched_param param; - pthread_attr_t attr; - - pthread_attr_init(&attr); - - error = pthread_attr_setschedpolicy(&attr, SCHED_RR); - if (error != 0) - ROS_ERROR("pthread_attr_setschedpolicy error = %d\n", error); - error = pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED); - if (error != 0) - ROS_ERROR("pthread_attr_setinheritsched error = %d\n", error); - - memset(¶m, 0, sizeof(param)); - param.sched_priority = 31; // RT - error = pthread_attr_setschedparam(&attr, ¶m); - if (error != 0) - ROS_ERROR("pthread_attr_setschedparam error = %d\n", error); - - // create and start the thread - if ((error = pthread_create(&this->timer_thread_, &attr, this->timerThread, - this)) != 0) { - ROS_ERROR("Creating timer thread failed!!"); - exit(-1); - } - } - - this->is_timer_running_ = true; -} - -void RobotisController::stopTimer() { - int error = 0; - - // set the flag to stop the thread - if (this->is_timer_running_) { - this->stop_timer_ = true; - - if (this->gazebo_mode_ == false) { - // wait until the thread is stopped. - if ((error = pthread_join(this->timer_thread_, NULL)) != 0) - exit(-1); - - for (auto &it : port_to_bulk_read_) { - if (it.second != NULL) - it.second->rxPacket(); - } - - for (auto &it : port_to_sync_write_position_) { - if (it.second != NULL) - it.second->clearParam(); - } - for (auto &it : port_to_sync_write_position_p_gain_) { - if (it.second != NULL) - it.second->clearParam(); - } - for (auto &it : port_to_sync_write_position_i_gain_) { - if (it.second != NULL) - it.second->clearParam(); - } - for (auto &it : port_to_sync_write_position_d_gain_) { - if (it.second != NULL) - it.second->clearParam(); - } - for (auto &it : port_to_sync_write_velocity_) { - if (it.second != NULL) - it.second->clearParam(); - } - for (auto &it : port_to_sync_write_velocity_p_gain_) { - if (it.second != NULL) - it.second->clearParam(); - } - for (auto &it : port_to_sync_write_velocity_i_gain_) { - if (it.second != NULL) - it.second->clearParam(); - } - for (auto &it : port_to_sync_write_velocity_d_gain_) { - if (it.second != NULL) - it.second->clearParam(); - } - for (auto &it : port_to_sync_write_current_) { - if (it.second != NULL) - it.second->clearParam(); - } - } else { - // wait until the thread is stopped. - gazebo_thread_.join(); - } - - this->stop_timer_ = false; - this->is_timer_running_ = false; - } -} - -bool RobotisController::isTimerRunning() { return this->is_timer_running_; } - -void RobotisController::loadOffset(const std::string path) { - YAML::Node doc; - try { - doc = YAML::LoadFile(path.c_str()); - } catch (const std::exception &e) { - ROS_WARN("Fail to load offset yaml."); - return; - } - - YAML::Node offset_node = doc["offset"]; - if (offset_node.size() == 0) - return; - - ROS_INFO("Load offsets..."); - for (YAML::const_iterator it = offset_node.begin(); it != offset_node.end(); - it++) { - std::string joint_name = it->first.as(); - double offset = it->second.as(); - - auto dxl_it = robot_->dxls_.find(joint_name); - if (dxl_it != robot_->dxls_.end()) - dxl_it->second->dxl_state_->position_offset_ = offset; - } -} - -void RobotisController::process() { - // avoid duplicated function call - static bool is_process_running = false; - if (is_process_running == true) - return; - is_process_running = true; - - // ROS_INFO("Controller::Process()"); - // offset ratio - if (is_offset_enabled_) { - if (offset_ratio_ < 1.0) - offset_ratio_ += 0.01; - else - offset_ratio_ = 1.0; - } else { - if (offset_ratio_ > 0.0) - offset_ratio_ -= 0.01; - else - offset_ratio_ = 0.0; - } - - ros::Time start_time; - ros::Duration time_duration; - - if (DEBUG_PRINT) - start_time = ros::Time::now(); - - sensor_msgs::JointState goal_state; - sensor_msgs::JointState present_state; - - present_state.header.stamp = ros::Time::now(); - goal_state.header.stamp = present_state.header.stamp; - - if (controller_mode_ == MotionModuleMode) { - if (gazebo_mode_ == false) { - // BulkRead Rx - for (auto &it : port_to_bulk_read_) { - robot_->ports_[it.first]->setPacketTimeout(0.0); - if (DEBUG_PRINT) { - int result = it.second->rxPacket(); - if (result != COMM_SUCCESS) - ROS_ERROR_STREAM("Bulk Read Fail : " << it.first); - } else - it.second->rxPacket(); - } - - // -> save to robot->dxls_[]->dxl_state_ - if (robot_->dxls_.size() > 0) { - for (auto &dxl_it : robot_->dxls_) { - Dynamixel *dxl = dxl_it.second; - std::string port_name = dxl_it.second->port_name_; - std::string joint_name = dxl_it.first; - - if (dxl->bulk_read_items_.size() > 0) { - bool updated = false; - uint32_t data = 0; - for (int i = 0; i < dxl->bulk_read_items_.size(); i++) { - ControlTableItem *item = dxl->bulk_read_items_[i]; - if (port_to_bulk_read_[port_name]->isAvailable( - dxl->id_, item->address_, item->data_length_) == true) { - updated = true; - data = port_to_bulk_read_[port_name]->getData( - dxl->id_, item->address_, item->data_length_); - - // change dxl_state - if (dxl->present_position_item_ != 0 && - item->item_name_ == - dxl->present_position_item_->item_name_) { - dxl->dxl_state_->present_position_ = - dxl->convertValue2Radian(data) - - dxl->dxl_state_->position_offset_ * offset_ratio_; - } else if (dxl->present_velocity_item_ != 0 && - item->item_name_ == - dxl->present_velocity_item_->item_name_) - dxl->dxl_state_->present_velocity_ = - dxl->convertValue2Velocity(data); - else if (dxl->present_current_item_ != 0 && - item->item_name_ == - dxl->present_current_item_->item_name_) - dxl->dxl_state_->present_torque_ = - dxl->convertValue2Torque(data); - else if (dxl->goal_position_item_ != 0 && - item->item_name_ == - dxl->goal_position_item_->item_name_) { - dxl->dxl_state_->goal_position_ = - dxl->convertValue2Radian(data) - - dxl->dxl_state_->position_offset_ * offset_ratio_; - } else if (dxl->goal_velocity_item_ != 0 && - item->item_name_ == - dxl->goal_velocity_item_->item_name_) - dxl->dxl_state_->goal_velocity_ = - dxl->convertValue2Velocity(data); - else if (dxl->goal_current_item_ != 0 && - item->item_name_ == - dxl->goal_current_item_->item_name_) - dxl->dxl_state_->goal_torque_ = - dxl->convertValue2Torque(data); - - dxl->dxl_state_->bulk_read_table_[item->item_name_] = data; - } - } - - // -> update time stamp to - // Robot->dxls[]->dynamixel_state.update_time_stamp - if (updated == true) - dxl->dxl_state_->update_time_stamp_ = - TimeStamp(present_state.header.stamp.sec, - present_state.header.stamp.nsec); - } - } - } - - // -> save to robot->sensors_[]->sensor_state_ - if (robot_->sensors_.size() > 0) { - for (auto &sensor_it : robot_->sensors_) { - Sensor *sensor = sensor_it.second; - std::string port_name = sensor_it.second->port_name_; - std::string sensor_name = sensor_it.first; - - if (sensor->bulk_read_items_.size() > 0) { - bool updated = false; - uint32_t data = 0; - for (int i = 0; i < sensor->bulk_read_items_.size(); i++) { - ControlTableItem *item = sensor->bulk_read_items_[i]; - if (port_to_bulk_read_[port_name]->isAvailable( - sensor->id_, item->address_, item->data_length_) == - true) { - updated = true; - data = port_to_bulk_read_[port_name]->getData( - sensor->id_, item->address_, item->data_length_); - - // change sensor_state - sensor->sensor_state_->bulk_read_table_[item->item_name_] = - data; - } - } - - // -> update time stamp to - // Robot->dxls[]->dynamixel_state.update_time_stamp - if (updated == true) - sensor->sensor_state_->update_time_stamp_ = - TimeStamp(present_state.header.stamp.sec, - present_state.header.stamp.nsec); - } - } - } - - if (DEBUG_PRINT) { - time_duration = ros::Time::now() - start_time; - fprintf(stderr, "(%2.6f) BulkRead Rx & update state \n", - time_duration.nsec * 0.000001); - } - - // SyncWrite - queue_mutex_.lock(); - - if (direct_sync_write_.size() > 0) { - for (int i = 0; i < direct_sync_write_.size(); i++) { - direct_sync_write_[i]->txPacket(); - direct_sync_write_[i]->clearParam(); - } - direct_sync_write_.clear(); - } - - if (port_to_sync_write_position_p_gain_.size() > 0) { - for (auto &it : port_to_sync_write_position_p_gain_) { - it.second->txPacket(); - it.second->clearParam(); - } - } - if (port_to_sync_write_position_i_gain_.size() > 0) { - for (auto &it : port_to_sync_write_position_i_gain_) { - it.second->txPacket(); - it.second->clearParam(); - } - } - if (port_to_sync_write_position_d_gain_.size() > 0) { - for (auto &it : port_to_sync_write_position_d_gain_) { - it.second->txPacket(); - it.second->clearParam(); - } - } - if (port_to_sync_write_velocity_p_gain_.size() > 0) { - for (auto &it : port_to_sync_write_velocity_p_gain_) { - it.second->txPacket(); - it.second->clearParam(); - } - } - if (port_to_sync_write_velocity_i_gain_.size() > 0) { - for (auto &it : port_to_sync_write_velocity_i_gain_) { - it.second->txPacket(); - it.second->clearParam(); - } - } - if (port_to_sync_write_velocity_d_gain_.size() > 0) { - for (auto &it : port_to_sync_write_velocity_d_gain_) { - it.second->txPacket(); - it.second->clearParam(); - } - } - for (auto &it : port_to_sync_write_position_) { - if (it.second != NULL) - it.second->txPacket(); - } - for (auto &it : port_to_sync_write_velocity_) { - if (it.second != NULL) - it.second->txPacket(); - } - for (auto &it : port_to_sync_write_current_) { - if (it.second != NULL) - it.second->txPacket(); - } - - queue_mutex_.unlock(); - - // BulkRead Tx - for (auto &it : port_to_bulk_read_) - it.second->txPacket(); - - if (DEBUG_PRINT) { - time_duration = ros::Time::now() - start_time; - fprintf(stderr, "(%2.6f) SyncWrite & BulkRead Tx \n", - time_duration.nsec * 0.000001); - } - } else if (gazebo_mode_ == true) { - std_msgs::Float64 joint_msg; - - for (auto &dxl_it : robot_->dxls_) { - std::string joint_name = dxl_it.first; - Dynamixel *dxl = dxl_it.second; - DynamixelState *dxl_state = dxl_it.second->dxl_state_; - - if (dxl->ctrl_module_name_ == "none") { - joint_msg.data = dxl_state->goal_position_; - gazebo_joint_position_pub_[joint_name].publish(joint_msg); - } - } - - for (auto module_it = motion_modules_.begin(); - module_it != motion_modules_.end(); module_it++) { - if ((*module_it)->getModuleEnable() == false) - continue; - - for (auto &dxl_it : robot_->dxls_) { - std::string joint_name = dxl_it.first; - Dynamixel *dxl = dxl_it.second; - DynamixelState *dxl_state = dxl_it.second->dxl_state_; - - if (dxl->ctrl_module_name_ == (*module_it)->getModuleName()) { - if ((*module_it)->getControlMode() == PositionControl) { - joint_msg.data = dxl_state->goal_position_; - gazebo_joint_position_pub_[joint_name].publish(joint_msg); - } else if ((*module_it)->getControlMode() == VelocityControl) { - joint_msg.data = dxl_state->goal_velocity_; - gazebo_joint_velocity_pub_[joint_name].publish(joint_msg); - } else if ((*module_it)->getControlMode() == TorqueControl) { - joint_msg.data = dxl_state->goal_torque_; - gazebo_joint_effort_pub_[joint_name].publish(joint_msg); - } - } - } - } - } - } else if (controller_mode_ == DirectControlMode) { - if (gazebo_mode_ == false) { - // BulkRead Rx - for (auto &it : port_to_bulk_read_) { - robot_->ports_[it.first]->setPacketTimeout(0.0); - it.second->rxPacket(); - } - - // -> save to robot->dxls_[]->dxl_state_ - if (robot_->dxls_.size() > 0) { - for (auto &dxl_it : robot_->dxls_) { - Dynamixel *dxl = dxl_it.second; - std::string port_name = dxl_it.second->port_name_; - std::string joint_name = dxl_it.first; - - if (dxl->bulk_read_items_.size() > 0) { - uint32_t data = 0; - for (int i = 0; i < dxl->bulk_read_items_.size(); i++) { - ControlTableItem *item = dxl->bulk_read_items_[i]; - if (port_to_bulk_read_[port_name]->isAvailable( - dxl->id_, item->address_, item->data_length_) == true) { - data = port_to_bulk_read_[port_name]->getData( - dxl->id_, item->address_, item->data_length_); - - // change dxl_state - if (dxl->present_position_item_ != 0 && - item->item_name_ == - dxl->present_position_item_->item_name_) { - dxl->dxl_state_->present_position_ = - dxl->convertValue2Radian(data) - - dxl->dxl_state_->position_offset_ * offset_ratio_; - } else if (dxl->present_velocity_item_ != 0 && - item->item_name_ == - dxl->present_velocity_item_->item_name_) - dxl->dxl_state_->present_velocity_ = - dxl->convertValue2Velocity(data); - else if (dxl->present_current_item_ != 0 && - item->item_name_ == - dxl->present_current_item_->item_name_) - dxl->dxl_state_->present_torque_ = - dxl->convertValue2Torque(data); - else if (dxl->goal_position_item_ != 0 && - item->item_name_ == - dxl->goal_position_item_->item_name_) { - dxl->dxl_state_->goal_position_ = - dxl->convertValue2Radian(data) - - dxl->dxl_state_->position_offset_ * offset_ratio_; - } else if (dxl->goal_velocity_item_ != 0 && - item->item_name_ == - dxl->goal_velocity_item_->item_name_) - dxl->dxl_state_->goal_velocity_ = - dxl->convertValue2Velocity(data); - else if (dxl->goal_current_item_ != 0 && - item->item_name_ == - dxl->goal_current_item_->item_name_) - dxl->dxl_state_->goal_torque_ = - dxl->convertValue2Torque(data); - - dxl->dxl_state_->bulk_read_table_[item->item_name_] = data; - } - } - - // -> update time stamp to - // Robot->dxls[]->dynamixel_state.update_time_stamp - dxl->dxl_state_->update_time_stamp_ = - TimeStamp(present_state.header.stamp.sec, - present_state.header.stamp.nsec); - } - } - } - - queue_mutex_.lock(); - - // for (auto& it : port_to_sync_write_position_) - // { - // it.second->txPacket(); - // it.second->clearParam(); - // } - - if (direct_sync_write_.size() > 0) { - for (int i = 0; i < direct_sync_write_.size(); i++) { - direct_sync_write_[i]->txPacket(); - direct_sync_write_[i]->clearParam(); - } - direct_sync_write_.clear(); - } - - queue_mutex_.unlock(); - - // BulkRead Tx - for (auto &it : port_to_bulk_read_) - it.second->txPacket(); - } - } - - // Call SensorModule Process() - // -> for loop : call SensorModule list -> Process() - if (sensor_modules_.size() > 0) { - for (auto module_it = sensor_modules_.begin(); - module_it != sensor_modules_.end(); module_it++) { - (*module_it)->process(robot_->dxls_, robot_->sensors_); - - for (auto &it : (*module_it)->result_) - sensor_result_[it.first] = it.second; - } - } - - if (DEBUG_PRINT) { - time_duration = ros::Time::now() - start_time; - fprintf(stderr, "(%2.6f) SensorModule Process() & save result \n", - time_duration.nsec * 0.000001); - } - - if (controller_mode_ == MotionModuleMode) { - // Call MotionModule Process() - // -> for loop : call MotionModule list -> Process() - if (motion_modules_.size() > 0) { - queue_mutex_.lock(); - - for (auto module_it = motion_modules_.begin(); - module_it != motion_modules_.end(); module_it++) { - if ((*module_it)->getModuleEnable() == false) - continue; - - (*module_it)->process(robot_->dxls_, sensor_result_); - - // for loop : joint list - for (auto &dxl_it : robot_->dxls_) { - std::string joint_name = dxl_it.first; - Dynamixel *dxl = dxl_it.second; - DynamixelState *dxl_state = dxl_it.second->dxl_state_; - - if (dxl->ctrl_module_name_ == (*module_it)->getModuleName()) { - // do_sync_write = true; - DynamixelState *result_state = (*module_it)->result_[joint_name]; - - if (result_state == NULL) { - ROS_ERROR("[%s] %s ", (*module_it)->getModuleName().c_str(), - joint_name.c_str()); - continue; - } - - // TODO: check update time stamp ? - - if ((*module_it)->getControlMode() == PositionControl) { - dxl_state->goal_position_ = result_state->goal_position_; - - if (gazebo_mode_ == false) { - // add offset - uint32_t pos_data; - pos_data = dxl->convertRadian2Value( - dxl_state->goal_position_ + - dxl_state->position_offset_ * offset_ratio_); - - uint8_t sync_write_data[4] = {0}; - sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(pos_data)); - sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(pos_data)); - sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(pos_data)); - sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(pos_data)); - - if (port_to_sync_write_position_[dxl->port_name_] != NULL) - port_to_sync_write_position_[dxl->port_name_]->changeParam( - dxl->id_, sync_write_data); - - // if position p gain value is changed -> sync write - if (result_state->position_p_gain_ != NONE_GAIN && - dxl_state->position_p_gain_ != - result_state->position_p_gain_) { - dxl_state->position_p_gain_ = result_state->position_p_gain_; - uint8_t sync_write_data[4] = {0}; - sync_write_data[0] = - DXL_LOBYTE(DXL_LOWORD(dxl_state->position_p_gain_)); - sync_write_data[1] = - DXL_HIBYTE(DXL_LOWORD(dxl_state->position_p_gain_)); - sync_write_data[2] = - DXL_LOBYTE(DXL_HIWORD(dxl_state->position_p_gain_)); - sync_write_data[3] = - DXL_HIBYTE(DXL_HIWORD(dxl_state->position_p_gain_)); - - if (port_to_sync_write_position_p_gain_[dxl->port_name_] != - NULL) - port_to_sync_write_position_p_gain_[dxl->port_name_] - ->addParam(dxl->id_, sync_write_data); - } - - // if position i gain value is changed -> sync write - if (result_state->position_i_gain_ != NONE_GAIN && - dxl_state->position_i_gain_ != - result_state->position_i_gain_) { - dxl_state->position_i_gain_ = result_state->position_i_gain_; - uint8_t sync_write_data[4] = {0}; - sync_write_data[0] = - DXL_LOBYTE(DXL_LOWORD(dxl_state->position_i_gain_)); - sync_write_data[1] = - DXL_HIBYTE(DXL_LOWORD(dxl_state->position_i_gain_)); - sync_write_data[2] = - DXL_LOBYTE(DXL_HIWORD(dxl_state->position_i_gain_)); - sync_write_data[3] = - DXL_HIBYTE(DXL_HIWORD(dxl_state->position_i_gain_)); - - if (port_to_sync_write_position_i_gain_[dxl->port_name_] != - NULL) - port_to_sync_write_position_i_gain_[dxl->port_name_] - ->addParam(dxl->id_, sync_write_data); - } - - // if position d gain value is changed -> sync write - if (result_state->position_d_gain_ != NONE_GAIN && - dxl_state->position_d_gain_ != - result_state->position_d_gain_) { - dxl_state->position_d_gain_ = result_state->position_d_gain_; - uint8_t sync_write_data[4] = {0}; - sync_write_data[0] = - DXL_LOBYTE(DXL_LOWORD(dxl_state->position_d_gain_)); - sync_write_data[1] = - DXL_HIBYTE(DXL_LOWORD(dxl_state->position_d_gain_)); - sync_write_data[2] = - DXL_LOBYTE(DXL_HIWORD(dxl_state->position_d_gain_)); - sync_write_data[3] = - DXL_HIBYTE(DXL_HIWORD(dxl_state->position_d_gain_)); - - if (port_to_sync_write_position_d_gain_[dxl->port_name_] != - NULL) - port_to_sync_write_position_d_gain_[dxl->port_name_] - ->addParam(dxl->id_, sync_write_data); - } - - // if velocity p gain gain value is changed -> sync write - if (result_state->velocity_p_gain_ != NONE_GAIN && - dxl_state->velocity_p_gain_ != - result_state->velocity_p_gain_) { - dxl_state->velocity_p_gain_ = result_state->velocity_p_gain_; - uint8_t sync_write_data[4] = {0}; - sync_write_data[0] = - DXL_LOBYTE(DXL_LOWORD(dxl_state->velocity_p_gain_)); - sync_write_data[1] = - DXL_HIBYTE(DXL_LOWORD(dxl_state->velocity_p_gain_)); - sync_write_data[2] = - DXL_LOBYTE(DXL_HIWORD(dxl_state->velocity_p_gain_)); - sync_write_data[3] = - DXL_HIBYTE(DXL_HIWORD(dxl_state->velocity_p_gain_)); - - if (port_to_sync_write_velocity_p_gain_[dxl->port_name_] != - NULL) - port_to_sync_write_velocity_p_gain_[dxl->port_name_] - ->addParam(dxl->id_, sync_write_data); - } - - // if velocity i gain value is changed -> sync write - if (result_state->velocity_i_gain_ != NONE_GAIN && - dxl_state->velocity_i_gain_ != - result_state->velocity_i_gain_) { - dxl_state->velocity_i_gain_ = result_state->velocity_i_gain_; - uint8_t sync_write_data[4] = {0}; - sync_write_data[0] = - DXL_LOBYTE(DXL_LOWORD(dxl_state->velocity_i_gain_)); - sync_write_data[1] = - DXL_HIBYTE(DXL_LOWORD(dxl_state->velocity_i_gain_)); - sync_write_data[2] = - DXL_LOBYTE(DXL_HIWORD(dxl_state->velocity_i_gain_)); - sync_write_data[3] = - DXL_HIBYTE(DXL_HIWORD(dxl_state->velocity_i_gain_)); - - if (port_to_sync_write_velocity_i_gain_[dxl->port_name_] != - NULL) - port_to_sync_write_velocity_i_gain_[dxl->port_name_] - ->addParam(dxl->id_, sync_write_data); - } - } - } else if ((*module_it)->getControlMode() == VelocityControl) { - dxl_state->goal_velocity_ = result_state->goal_velocity_; - - if (gazebo_mode_ == false) { - uint32_t vel_data = - dxl->convertVelocity2Value(dxl_state->goal_velocity_); - uint8_t sync_write_data[4] = {0}; - sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(vel_data)); - sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(vel_data)); - sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(vel_data)); - sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(vel_data)); - - if (port_to_sync_write_velocity_[dxl->port_name_] != NULL) - port_to_sync_write_velocity_[dxl->port_name_]->changeParam( - dxl->id_, sync_write_data); - - // if velocity p gain gain value is changed -> sync write - if (result_state->velocity_p_gain_ != NONE_GAIN && - dxl_state->velocity_p_gain_ != - result_state->velocity_p_gain_) { - dxl_state->velocity_p_gain_ = result_state->velocity_p_gain_; - uint8_t sync_write_data[4] = {0}; - sync_write_data[0] = - DXL_LOBYTE(DXL_LOWORD(dxl_state->velocity_p_gain_)); - sync_write_data[1] = - DXL_HIBYTE(DXL_LOWORD(dxl_state->velocity_p_gain_)); - sync_write_data[2] = - DXL_LOBYTE(DXL_HIWORD(dxl_state->velocity_p_gain_)); - sync_write_data[3] = - DXL_HIBYTE(DXL_HIWORD(dxl_state->velocity_p_gain_)); - - if (port_to_sync_write_velocity_p_gain_[dxl->port_name_] != - NULL) - port_to_sync_write_velocity_p_gain_[dxl->port_name_] - ->addParam(dxl->id_, sync_write_data); - } - - // if velocity i gain value is changed -> sync write - if (result_state->velocity_i_gain_ != NONE_GAIN && - dxl_state->velocity_i_gain_ != - result_state->velocity_i_gain_) { - dxl_state->velocity_i_gain_ = result_state->velocity_i_gain_; - uint8_t sync_write_data[4] = {0}; - sync_write_data[0] = - DXL_LOBYTE(DXL_LOWORD(dxl_state->velocity_i_gain_)); - sync_write_data[1] = - DXL_HIBYTE(DXL_LOWORD(dxl_state->velocity_i_gain_)); - sync_write_data[2] = - DXL_LOBYTE(DXL_HIWORD(dxl_state->velocity_i_gain_)); - sync_write_data[3] = - DXL_HIBYTE(DXL_HIWORD(dxl_state->velocity_i_gain_)); - - if (port_to_sync_write_velocity_i_gain_[dxl->port_name_] != - NULL) - port_to_sync_write_velocity_i_gain_[dxl->port_name_] - ->addParam(dxl->id_, sync_write_data); - } - - // if velocity d gain value is changed -> sync write - if (result_state->velocity_d_gain_ != NONE_GAIN && - dxl_state->velocity_d_gain_ != - result_state->velocity_d_gain_) { - dxl_state->velocity_d_gain_ = result_state->velocity_d_gain_; - uint8_t sync_write_data[4] = {0}; - sync_write_data[0] = - DXL_LOBYTE(DXL_LOWORD(dxl_state->velocity_d_gain_)); - sync_write_data[1] = - DXL_HIBYTE(DXL_LOWORD(dxl_state->velocity_d_gain_)); - sync_write_data[2] = - DXL_LOBYTE(DXL_HIWORD(dxl_state->velocity_d_gain_)); - sync_write_data[3] = - DXL_HIBYTE(DXL_HIWORD(dxl_state->velocity_d_gain_)); - - if (port_to_sync_write_velocity_d_gain_[dxl->port_name_] != - NULL) - port_to_sync_write_velocity_d_gain_[dxl->port_name_] - ->addParam(dxl->id_, sync_write_data); - } - } - } else if ((*module_it)->getControlMode() == TorqueControl) { - dxl_state->goal_torque_ = result_state->goal_torque_; - - if (gazebo_mode_ == false) { - uint32_t curr_data = - dxl->convertTorque2Value(dxl_state->goal_torque_); - uint8_t sync_write_data[2] = {0}; - sync_write_data[0] = DXL_LOBYTE(curr_data); - sync_write_data[1] = DXL_HIBYTE(curr_data); - - if (port_to_sync_write_current_[dxl->port_name_] != NULL) - port_to_sync_write_current_[dxl->port_name_]->changeParam( - dxl->id_, sync_write_data); - } - } - } - } - } - - queue_mutex_.unlock(); - } - - if (DEBUG_PRINT) { - time_duration = ros::Time::now() - start_time; - fprintf(stderr, "(%2.6f) MotionModule Process() & save result \n", - time_duration.nsec * 0.000001); - } - } - - // publish present & goal position - for (auto &dxl_it : robot_->dxls_) { - std::string joint_name = dxl_it.first; - Dynamixel *dxl = dxl_it.second; - - present_state.name.push_back(joint_name); - present_state.position.push_back(dxl->dxl_state_->present_position_); - present_state.velocity.push_back(dxl->dxl_state_->present_velocity_); - present_state.effort.push_back(dxl->dxl_state_->present_torque_); - - goal_state.name.push_back(joint_name); - goal_state.position.push_back(dxl->dxl_state_->goal_position_); - goal_state.velocity.push_back(dxl->dxl_state_->goal_velocity_); - goal_state.effort.push_back(dxl->dxl_state_->goal_torque_); - } - - // -> publish present joint_states & goal joint states topic - present_joint_state_pub_.publish(present_state); - goal_joint_state_pub_.publish(goal_state); - - if (DEBUG_PRINT) { - time_duration = ros::Time::now() - start_time; - fprintf(stderr, "(%2.6f) Process() DONE \n", time_duration.nsec * 0.000001); - } - - is_process_running = false; -} - -void RobotisController::addMotionModule(MotionModule *module) { - // check whether the module name already exists - for (auto m_it = motion_modules_.begin(); m_it != motion_modules_.end(); - m_it++) { - if ((*m_it)->getModuleName() == module->getModuleName()) { - ROS_ERROR("Motion Module Name [%s] already exist !!", - module->getModuleName().c_str()); - return; - } - } - - module->initialize(robot_->getControlCycle(), robot_); - motion_modules_.push_back(module); - motion_modules_.unique(); -} - -void RobotisController::removeMotionModule(MotionModule *module) { - motion_modules_.remove(module); -} - -void RobotisController::addSensorModule(SensorModule *module) { - // check whether the module name already exists - for (auto m_it = sensor_modules_.begin(); m_it != sensor_modules_.end(); - m_it++) { - if ((*m_it)->getModuleName() == module->getModuleName()) { - ROS_ERROR("Sensor Module Name [%s] already exist !!", - module->getModuleName().c_str()); - return; - } - } - - module->initialize(robot_->getControlCycle(), robot_); - sensor_modules_.push_back(module); - sensor_modules_.unique(); -} - -void RobotisController::removeSensorModule(SensorModule *module) { - sensor_modules_.remove(module); -} - -void RobotisController::writeControlTableCallback( - const humanoid_robot_intelligence_control_system_controller_msgs:: - WriteControlTable::ConstPtr &msg) { - Device *device = NULL; - - if (DEBUG_PRINT) - fprintf(stderr, "[WriteControlTable] led control msg received\n"); - - auto dev_it1 = robot_->dxls_.find(msg->joint_name); - if (dev_it1 != robot_->dxls_.end()) { - device = dev_it1->second; - } else { - auto dev_it2 = robot_->sensors_.find(msg->joint_name); - if (dev_it2 != robot_->sensors_.end()) { - device = dev_it2->second; - } else { - ROS_WARN("[WriteControlTable] Unknown device : %s", - msg->joint_name.c_str()); - return; - } - } - - ControlTableItem *item = NULL; - auto item_it = device->ctrl_table_.find(msg->start_item_name); - if (item_it != device->ctrl_table_.end()) { - item = item_it->second; - } else { - ROS_WARN("[WriteControlTable] Unknown item : %s", - msg->start_item_name.c_str()); - return; - } - - dynamixel::PortHandler *port = robot_->ports_[device->port_name_]; - dynamixel::PacketHandler *packet_handler = - dynamixel::PacketHandler::getPacketHandler(device->protocol_version_); - - if (item->access_type_ == Read) - return; - - queue_mutex_.lock(); - - direct_sync_write_.push_back(new dynamixel::GroupSyncWrite( - port, packet_handler, item->address_, msg->data_length)); - direct_sync_write_[direct_sync_write_.size() - 1]->addParam( - device->id_, (uint8_t *)(msg->data.data())); - - // fprintf(stderr, "[WriteControlTable] %s -> %s : ", - // msg->joint_name.c_str(), msg->start_item_name.c_str()); for (auto &dt : - // msg->data) - // fprintf(stderr, "%02X ", dt); - // fprintf(stderr, "\n"); - - queue_mutex_.unlock(); -} - -void RobotisController::syncWriteItemCallback( - const humanoid_robot_intelligence_control_system_controller_msgs:: - SyncWriteItem::ConstPtr &msg) { - for (int i = 0; i < msg->joint_name.size(); i++) { - Device *device; - - auto d_it1 = robot_->dxls_.find(msg->joint_name[i]); - if (d_it1 != robot_->dxls_.end()) { - device = d_it1->second; - } else { - auto d_it2 = robot_->sensors_.find(msg->joint_name[i]); - if (d_it2 != robot_->sensors_.end()) { - device = d_it2->second; - } else { - ROS_WARN("[SyncWriteItem] Unknown device : %s", - msg->joint_name[i].c_str()); - continue; - } - } - - // ControlTableItem *item = device->ctrl_table_[msg->item_name]; - ControlTableItem *item = NULL; - auto item_it = device->ctrl_table_.find(msg->item_name); - if (item_it != device->ctrl_table_.end()) { - item = item_it->second; - } else { - ROS_WARN("SyncWriteItem] Unknown item : %s", msg->item_name.c_str()); - continue; - } - - dynamixel::PortHandler *port = robot_->ports_[device->port_name_]; - dynamixel::PacketHandler *packet_handler = - dynamixel::PacketHandler::getPacketHandler(device->protocol_version_); - - if (item->access_type_ == Read) - continue; - - queue_mutex_.lock(); - - int idx = 0; - if (direct_sync_write_.size() == 0) { - direct_sync_write_.push_back(new dynamixel::GroupSyncWrite( - port, packet_handler, item->address_, item->data_length_)); - idx = 0; - } else { - for (idx = 0; idx < direct_sync_write_.size(); idx++) { - if (direct_sync_write_[idx]->getPortHandler() == port && - direct_sync_write_[idx]->getPacketHandler() == packet_handler) - break; - } - - if (idx == direct_sync_write_.size()) - direct_sync_write_.push_back(new dynamixel::GroupSyncWrite( - port, packet_handler, item->address_, item->data_length_)); - } - - uint8_t *data = new uint8_t[item->data_length_]; - if (item->data_length_ == 1) - data[0] = (uint8_t)msg->value[i]; - else if (item->data_length_ == 2) { - data[0] = DXL_LOBYTE((uint16_t)msg->value[i]); - data[1] = DXL_HIBYTE((uint16_t)msg->value[i]); - } else if (item->data_length_ == 4) { - data[0] = DXL_LOBYTE(DXL_LOWORD((uint32_t)msg->value[i])); - data[1] = DXL_HIBYTE(DXL_LOWORD((uint32_t)msg->value[i])); - data[2] = DXL_LOBYTE(DXL_HIWORD((uint32_t)msg->value[i])); - data[3] = DXL_HIBYTE(DXL_HIWORD((uint32_t)msg->value[i])); - } - direct_sync_write_[idx]->addParam(device->id_, data); - delete[] data; - - queue_mutex_.unlock(); - } -} - -void RobotisController::setControllerModeCallback( - const std_msgs::String::ConstPtr &msg) { - if (msg->data == "DirectControlMode") { - for (auto &it : port_to_bulk_read_) { - robot_->ports_[it.first]->setPacketTimeout(0.0); - it.second->rxPacket(); - } - controller_mode_ = DirectControlMode; - } else if (msg->data == "MotionModuleMode") { - for (auto &it : port_to_bulk_read_) { - it.second->txPacket(); - } - controller_mode_ = MotionModuleMode; - } -} - -void RobotisController::setJointStatesCallback( - const sensor_msgs::JointState::ConstPtr &msg) { - queue_mutex_.lock(); - - for (int i = 0; i < msg->name.size(); i++) { - Dynamixel *dxl = robot_->dxls_[msg->name[i]]; - if (dxl == NULL) - continue; - - if ((controller_mode_ == DirectControlMode) || - (controller_mode_ == MotionModuleMode && - dxl->ctrl_module_name_ == "none")) { - dxl->dxl_state_->goal_position_ = (double)msg->position[i]; - - if (gazebo_mode_ == false) { - // add offset - uint32_t pos_data; - pos_data = dxl->convertRadian2Value(dxl->dxl_state_->goal_position_ + - dxl->dxl_state_->position_offset_ * - offset_ratio_); - - uint8_t sync_write_data[4] = {0}; - sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(pos_data)); - sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(pos_data)); - sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(pos_data)); - sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(pos_data)); - - if (port_to_sync_write_position_[dxl->port_name_] != NULL) - port_to_sync_write_position_[dxl->port_name_]->changeParam( - dxl->id_, sync_write_data); - } - } - } - - queue_mutex_.unlock(); -} - -void RobotisController::setCtrlModuleCallback( - const std_msgs::String::ConstPtr &msg) { - if (set_module_thread_.joinable()) - set_module_thread_.join(); - - std::string _module_name_to_set = msg->data; - - set_module_thread_ = boost::thread(boost::bind( - &RobotisController::setCtrlModuleThread, this, _module_name_to_set)); -} - -void RobotisController::setCtrlModule(std::string module_name) { - if (set_module_thread_.joinable()) - set_module_thread_.join(); - - set_module_thread_ = boost::thread( - boost::bind(&RobotisController::setCtrlModuleThread, this, module_name)); -} -void RobotisController::setJointCtrlModuleCallback( - const humanoid_robot_intelligence_control_system_controller_msgs:: - JointCtrlModule::ConstPtr &msg) { - if (msg->joint_name.size() != msg->module_name.size()) - return; - - if (set_module_thread_.joinable()) - set_module_thread_.join(); - - set_module_thread_ = boost::thread( - boost::bind(&RobotisController::setJointCtrlModuleThread, this, msg)); -} - -void RobotisController::enableOffsetCallback( - const std_msgs::Bool::ConstPtr &msg) { - is_offset_enabled_ = (bool)msg->data; - if (is_offset_enabled_) - offset_ratio_ = 0.0; - else - offset_ratio_ = 1.0; -} - -bool RobotisController::getJointCtrlModuleService( - humanoid_robot_intelligence_control_system_controller_msgs::GetJointModule:: - Request &req, - humanoid_robot_intelligence_control_system_controller_msgs::GetJointModule:: - Response &res) { - for (unsigned int idx = 0; idx < req.joint_name.size(); idx++) { - auto d_it = robot_->dxls_.find((std::string)(req.joint_name[idx])); - if (d_it != robot_->dxls_.end()) { - res.joint_name.push_back(req.joint_name[idx]); - res.module_name.push_back(d_it->second->ctrl_module_name_); - } - } - - if (res.joint_name.size() == 0) - return false; - - return true; -} - -bool RobotisController::setJointCtrlModuleService( - humanoid_robot_intelligence_control_system_controller_msgs::SetJointModule:: - Request &req, - humanoid_robot_intelligence_control_system_controller_msgs::SetJointModule:: - Response &res) { - if (set_module_thread_.joinable()) - set_module_thread_.join(); - - humanoid_robot_intelligence_control_system_controller_msgs::JointCtrlModule - modules; - modules.joint_name = req.joint_name; - modules.module_name = req.module_name; - - humanoid_robot_intelligence_control_system_controller_msgs::JointCtrlModule:: - ConstPtr msg_ptr( - new humanoid_robot_intelligence_control_system_controller_msgs:: - JointCtrlModule(modules)); - - if (modules.joint_name.size() != modules.module_name.size()) - return false; - - set_module_thread_ = boost::thread( - boost::bind(&RobotisController::setJointCtrlModuleThread, this, msg_ptr)); - - set_module_thread_.join(); - - return true; -} - -bool RobotisController::setCtrlModuleService( - humanoid_robot_intelligence_control_system_controller_msgs::SetModule:: - Request &req, - humanoid_robot_intelligence_control_system_controller_msgs::SetModule:: - Response &res) { - if (set_module_thread_.joinable()) - set_module_thread_.join(); - - std::string _module_name_to_set = req.module_name; - - set_module_thread_ = boost::thread(boost::bind( - &RobotisController::setCtrlModuleThread, this, _module_name_to_set)); - - set_module_thread_.join(); - - res.result = true; - return true; -} - -bool RobotisController::loadOffsetService( - humanoid_robot_intelligence_control_system_controller_msgs::LoadOffset:: - Request &req, - humanoid_robot_intelligence_control_system_controller_msgs::LoadOffset:: - Response &res) { - loadOffset((std::string)req.file_path); - res.result = true; - return true; -} - -void RobotisController::setJointCtrlModuleThread( - const humanoid_robot_intelligence_control_system_controller_msgs:: - JointCtrlModule::ConstPtr &msg) { - // stop module list - std::list _stop_modules; - std::list _enable_modules; - - for (unsigned int idx = 0; idx < msg->joint_name.size(); idx++) { - Dynamixel *_dxl = NULL; - std::map::iterator _dxl_it = - robot_->dxls_.find((std::string)(msg->joint_name[idx])); - if (_dxl_it != robot_->dxls_.end()) - _dxl = _dxl_it->second; - else - continue; - - // enqueue - if (_dxl->ctrl_module_name_ != msg->module_name[idx]) { - for (std::list::iterator _stop_m_it = - motion_modules_.begin(); - _stop_m_it != motion_modules_.end(); _stop_m_it++) { - if ((*_stop_m_it)->getModuleName() == _dxl->ctrl_module_name_ && - (*_stop_m_it)->getModuleEnable() == true) - _stop_modules.push_back(*_stop_m_it); - } - } - } - - // stop the module - _stop_modules.unique(); - for (std::list::iterator _stop_m_it = _stop_modules.begin(); - _stop_m_it != _stop_modules.end(); _stop_m_it++) { - (*_stop_m_it)->stop(); - } - - // wait to stop - for (std::list::iterator _stop_m_it = _stop_modules.begin(); - _stop_m_it != _stop_modules.end(); _stop_m_it++) { - while ((*_stop_m_it)->isRunning()) - usleep(robot_->getControlCycle() * 1000); - } - - // disable module(s) - for (std::list::iterator _stop_m_it = _stop_modules.begin(); - _stop_m_it != _stop_modules.end(); _stop_m_it++) { - (*_stop_m_it)->setModuleEnable(false); - } - - // set ctrl module - queue_mutex_.lock(); - - for (unsigned int idx = 0; idx < msg->joint_name.size(); idx++) { - std::string ctrl_module = msg->module_name[idx]; - std::string joint_name = msg->joint_name[idx]; - - Dynamixel *_dxl = NULL; - std::map::iterator _dxl_it = - robot_->dxls_.find(joint_name); - if (_dxl_it != robot_->dxls_.end()) - _dxl = _dxl_it->second; - else - continue; - - // none - if (ctrl_module == "" || ctrl_module == "none") { - _dxl->ctrl_module_name_ = "none"; - - if (gazebo_mode_ == true) - continue; - - uint32_t _pos_data; - _pos_data = _dxl->convertRadian2Value(_dxl->dxl_state_->goal_position_ + - _dxl->dxl_state_->position_offset_ * - offset_ratio_); - - uint8_t _sync_write_data[4]; - _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_pos_data)); - _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_pos_data)); - _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_pos_data)); - _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_pos_data)); - - if (port_to_sync_write_position_[_dxl->port_name_] != NULL) - port_to_sync_write_position_[_dxl->port_name_]->addParam( - _dxl->id_, _sync_write_data); - - if (port_to_sync_write_current_[_dxl->port_name_] != NULL) - port_to_sync_write_current_[_dxl->port_name_]->removeParam(_dxl->id_); - if (port_to_sync_write_velocity_[_dxl->port_name_] != NULL) - port_to_sync_write_velocity_[_dxl->port_name_]->removeParam(_dxl->id_); - } else { - // check whether the module exist - for (std::list::iterator _m_it = motion_modules_.begin(); - _m_it != motion_modules_.end(); _m_it++) { - // if it exist - if ((*_m_it)->getModuleName() == ctrl_module) { - std::map::iterator _result_it = - (*_m_it)->result_.find(joint_name); - if (_result_it == (*_m_it)->result_.end()) - break; - - _dxl->ctrl_module_name_ = ctrl_module; - - // enqueue enable module list - _enable_modules.push_back(*_m_it); - ControlMode _mode = (*_m_it)->getControlMode(); - - if (gazebo_mode_ == true) - break; - - if (_mode == PositionControl) { - uint32_t _pos_data; - _pos_data = _dxl->convertRadian2Value( - _dxl->dxl_state_->goal_position_ + - _dxl->dxl_state_->position_offset_ * offset_ratio_); - - uint8_t _sync_write_data[4]; - _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_pos_data)); - _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_pos_data)); - _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_pos_data)); - _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_pos_data)); - - if (port_to_sync_write_position_[_dxl->port_name_] != NULL) - port_to_sync_write_position_[_dxl->port_name_]->addParam( - _dxl->id_, _sync_write_data); - - if (port_to_sync_write_current_[_dxl->port_name_] != NULL) - port_to_sync_write_current_[_dxl->port_name_]->removeParam( - _dxl->id_); - if (port_to_sync_write_velocity_[_dxl->port_name_] != NULL) - port_to_sync_write_velocity_[_dxl->port_name_]->removeParam( - _dxl->id_); - } else if (_mode == VelocityControl) { - uint32_t _vel_data = - _dxl->convertVelocity2Value(_dxl->dxl_state_->goal_velocity_); - uint8_t _sync_write_data[4]; - _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_vel_data)); - _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_vel_data)); - _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_vel_data)); - _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_vel_data)); - - if (port_to_sync_write_velocity_[_dxl->port_name_] != NULL) - port_to_sync_write_velocity_[_dxl->port_name_]->addParam( - _dxl->id_, _sync_write_data); - - if (port_to_sync_write_current_[_dxl->port_name_] != NULL) - port_to_sync_write_current_[_dxl->port_name_]->removeParam( - _dxl->id_); - if (port_to_sync_write_position_[_dxl->port_name_] != NULL) - port_to_sync_write_position_[_dxl->port_name_]->removeParam( - _dxl->id_); - } else if (_mode == TorqueControl) { - uint32_t _curr_data = - _dxl->convertTorque2Value(_dxl->dxl_state_->goal_torque_); - uint8_t _sync_write_data[4]; - _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_curr_data)); - _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_curr_data)); - _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_curr_data)); - _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_curr_data)); - - if (port_to_sync_write_current_[_dxl->port_name_] != NULL) - port_to_sync_write_current_[_dxl->port_name_]->addParam( - _dxl->id_, _sync_write_data); - - if (port_to_sync_write_velocity_[_dxl->port_name_] != NULL) - port_to_sync_write_velocity_[_dxl->port_name_]->removeParam( - _dxl->id_); - if (port_to_sync_write_position_[_dxl->port_name_] != NULL) - port_to_sync_write_position_[_dxl->port_name_]->removeParam( - _dxl->id_); - } - break; - } - } - } - } - - // enable module(s) - _enable_modules.unique(); - for (std::list::iterator _m_it = _enable_modules.begin(); - _m_it != _enable_modules.end(); _m_it++) { - (*_m_it)->setModuleEnable(true); - } - - // TODO: set indirect address - // -> check module's control_mode - - queue_mutex_.unlock(); - - // publish current module - humanoid_robot_intelligence_control_system_controller_msgs::JointCtrlModule - _current_module_msg; - for (std::map::iterator _dxl_iter = - robot_->dxls_.begin(); - _dxl_iter != robot_->dxls_.end(); ++_dxl_iter) { - _current_module_msg.joint_name.push_back(_dxl_iter->first); - _current_module_msg.module_name.push_back( - _dxl_iter->second->ctrl_module_name_); - } - - if (_current_module_msg.joint_name.size() == - _current_module_msg.module_name.size()) - current_module_pub_.publish(_current_module_msg); -} - -void RobotisController::setCtrlModuleThread(std::string ctrl_module) { - // stop module - std::list stop_modules; - - if (ctrl_module == "" || ctrl_module == "none") { - // enqueue all modules in order to stop - for (auto m_it = motion_modules_.begin(); m_it != motion_modules_.end(); - m_it++) { - if ((*m_it)->getModuleEnable() == true) - stop_modules.push_back(*m_it); - } - } else { - for (auto m_it = motion_modules_.begin(); m_it != motion_modules_.end(); - m_it++) { - // if it exist - if ((*m_it)->getModuleName() == ctrl_module) { - // enqueue the module which lost control of joint in order to stop - for (auto &result_it : (*m_it)->result_) { - auto d_it = robot_->dxls_.find(result_it.first); - - if (d_it != robot_->dxls_.end()) { - // enqueue - if (d_it->second->ctrl_module_name_ != ctrl_module) { - for (auto stop_m_it = motion_modules_.begin(); - stop_m_it != motion_modules_.end(); stop_m_it++) { - if (((*stop_m_it)->getModuleName() == - d_it->second->ctrl_module_name_) && - ((*stop_m_it)->getModuleEnable() == true)) { - stop_modules.push_back(*stop_m_it); - } - } - } - } - } - - break; - } - } - } - - // stop the module - stop_modules.unique(); - for (auto stop_m_it = stop_modules.begin(); stop_m_it != stop_modules.end(); - stop_m_it++) { - (*stop_m_it)->stop(); - } - - // wait to stop - for (auto stop_m_it = stop_modules.begin(); stop_m_it != stop_modules.end(); - stop_m_it++) { - while ((*stop_m_it)->isRunning()) - usleep(robot_->getControlCycle() * 1000); - } - - // disable module(s) - for (std::list::iterator _stop_m_it = stop_modules.begin(); - _stop_m_it != stop_modules.end(); _stop_m_it++) { - (*_stop_m_it)->setModuleEnable(false); - } - - // set ctrl module - queue_mutex_.lock(); - - if (DEBUG_PRINT) - ROS_INFO_STREAM("set module : " << ctrl_module); - - // none - if ((ctrl_module == "") || (ctrl_module == "none")) { - // set dxl's control module to "none" - for (auto &d_it : robot_->dxls_) { - Dynamixel *dxl = d_it.second; - dxl->ctrl_module_name_ = "none"; - - if (gazebo_mode_ == true) - continue; - - uint32_t pos_data; - pos_data = dxl->convertRadian2Value(dxl->dxl_state_->goal_position_ + - dxl->dxl_state_->position_offset_ * - offset_ratio_); - - uint8_t sync_write_data[4] = {0}; - sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(pos_data)); - sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(pos_data)); - sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(pos_data)); - sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(pos_data)); - - if (port_to_sync_write_position_[dxl->port_name_] != NULL) - port_to_sync_write_position_[dxl->port_name_]->addParam( - dxl->id_, sync_write_data); - - if (port_to_sync_write_current_[dxl->port_name_] != NULL) - port_to_sync_write_current_[dxl->port_name_]->removeParam(dxl->id_); - if (port_to_sync_write_velocity_[dxl->port_name_] != NULL) - port_to_sync_write_velocity_[dxl->port_name_]->removeParam(dxl->id_); - } - } else { - // check whether the module exist - for (auto m_it = motion_modules_.begin(); m_it != motion_modules_.end(); - m_it++) { - // if it exist - if ((*m_it)->getModuleName() == ctrl_module) { - ControlMode mode = (*m_it)->getControlMode(); - for (auto &result_it : (*m_it)->result_) { - auto d_it = robot_->dxls_.find(result_it.first); - if (d_it != robot_->dxls_.end()) { - Dynamixel *dxl = d_it->second; - dxl->ctrl_module_name_ = ctrl_module; - - if (gazebo_mode_ == true) - continue; - - if (mode == PositionControl) { - uint32_t pos_data; - pos_data = dxl->convertRadian2Value( - dxl->dxl_state_->goal_position_ + - dxl->dxl_state_->position_offset_ * offset_ratio_); - - uint8_t sync_write_data[4] = {0}; - sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(pos_data)); - sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(pos_data)); - sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(pos_data)); - sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(pos_data)); - - if (port_to_sync_write_position_[dxl->port_name_] != NULL) - port_to_sync_write_position_[dxl->port_name_]->addParam( - dxl->id_, sync_write_data); - - if (port_to_sync_write_current_[dxl->port_name_] != NULL) - port_to_sync_write_current_[dxl->port_name_]->removeParam( - dxl->id_); - if (port_to_sync_write_velocity_[dxl->port_name_] != NULL) - port_to_sync_write_velocity_[dxl->port_name_]->removeParam( - dxl->id_); - } else if (mode == VelocityControl) { - uint32_t vel_data = - dxl->convertVelocity2Value(dxl->dxl_state_->goal_velocity_); - uint8_t sync_write_data[4] = {0}; - sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(vel_data)); - sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(vel_data)); - sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(vel_data)); - sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(vel_data)); - - if (port_to_sync_write_velocity_[dxl->port_name_] != NULL) - port_to_sync_write_velocity_[dxl->port_name_]->addParam( - dxl->id_, sync_write_data); - - if (port_to_sync_write_current_[dxl->port_name_] != NULL) - port_to_sync_write_current_[dxl->port_name_]->removeParam( - dxl->id_); - if (port_to_sync_write_position_[dxl->port_name_] != NULL) - port_to_sync_write_position_[dxl->port_name_]->removeParam( - dxl->id_); - } else if (mode == TorqueControl) { - uint32_t curr_data = - dxl->convertTorque2Value(dxl->dxl_state_->goal_torque_); - uint8_t sync_write_data[4] = {0}; - sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(curr_data)); - sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(curr_data)); - sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(curr_data)); - sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(curr_data)); - - if (port_to_sync_write_current_[dxl->port_name_] != NULL) - port_to_sync_write_current_[dxl->port_name_]->addParam( - dxl->id_, sync_write_data); - - if (port_to_sync_write_velocity_[dxl->port_name_] != NULL) - port_to_sync_write_velocity_[dxl->port_name_]->removeParam( - dxl->id_); - if (port_to_sync_write_position_[dxl->port_name_] != NULL) - port_to_sync_write_position_[dxl->port_name_]->removeParam( - dxl->id_); - } - } - } - - break; - } - } - } - - for (auto m_it = motion_modules_.begin(); m_it != motion_modules_.end(); - m_it++) { - // set all used modules -> enable - for (auto &d_it : robot_->dxls_) { - if (d_it.second->ctrl_module_name_ == (*m_it)->getModuleName()) { - (*m_it)->setModuleEnable(true); - break; - } - } - } - - // TODO: set indirect address - // -> check module's control_mode - - queue_mutex_.unlock(); - - // publish current module - humanoid_robot_intelligence_control_system_controller_msgs::JointCtrlModule - current_module_msg; - for (auto &dxl_iter : robot_->dxls_) { - current_module_msg.joint_name.push_back(dxl_iter.first); - current_module_msg.module_name.push_back( - dxl_iter.second->ctrl_module_name_); - } - - if (current_module_msg.joint_name.size() == - current_module_msg.module_name.size()) - current_module_pub_.publish(current_module_msg); -} - -void RobotisController::gazeboJointStatesCallback( - const sensor_msgs::JointState::ConstPtr &msg) { - queue_mutex_.lock(); - - for (unsigned int i = 0; i < msg->name.size(); i++) { - auto d_it = robot_->dxls_.find((std::string)msg->name[i]); - if (d_it != robot_->dxls_.end()) { - d_it->second->dxl_state_->present_position_ = msg->position[i]; - d_it->second->dxl_state_->present_velocity_ = msg->velocity[i]; - d_it->second->dxl_state_->present_torque_ = msg->effort[i]; - } - } - - if (init_pose_loaded_ == false) { - for (auto &it : robot_->dxls_) - it.second->dxl_state_->goal_position_ = - it.second->dxl_state_->present_position_; - init_pose_loaded_ = true; - } - - queue_mutex_.unlock(); -} - -bool RobotisController::isTimerStopped() { - if (this->is_timer_running_) { - if (DEBUG_PRINT == true) - ROS_WARN("Process Timer is running.. STOP the timer first."); - return false; - } - return true; -} - -int RobotisController::ping(const std::string joint_name, uint8_t *error) { - return ping(joint_name, 0, error); -} -int RobotisController::ping(const std::string joint_name, - uint16_t *model_number, uint8_t *error) { - if (isTimerStopped() == false) - return COMM_PORT_BUSY; - - Dynamixel *dxl = robot_->dxls_[joint_name]; - if (dxl == NULL) - return COMM_NOT_AVAILABLE; - - dynamixel::PacketHandler *pkt_handler = - dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); - dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; - - return pkt_handler->ping(port_handler, dxl->id_, model_number, error); -} - -int RobotisController::action(const std::string joint_name) { - if (isTimerStopped() == false) - return COMM_PORT_BUSY; - - Dynamixel *dxl = robot_->dxls_[joint_name]; - if (dxl == NULL) - return COMM_NOT_AVAILABLE; - - dynamixel::PacketHandler *pkt_handler = - dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); - dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; - - return pkt_handler->action(port_handler, dxl->id_); -} -int RobotisController::reboot(const std::string joint_name, uint8_t *error) { - if (isTimerStopped() == false) - return COMM_PORT_BUSY; - - Dynamixel *dxl = robot_->dxls_[joint_name]; - if (dxl == NULL) - return COMM_NOT_AVAILABLE; - - dynamixel::PacketHandler *pkt_handler = - dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); - dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; - - return pkt_handler->reboot(port_handler, dxl->id_, error); -} -int RobotisController::factoryReset(const std::string joint_name, - uint8_t option, uint8_t *error) { - if (isTimerStopped() == false) - return COMM_PORT_BUSY; - - Dynamixel *dxl = robot_->dxls_[joint_name]; - if (dxl == NULL) - return COMM_NOT_AVAILABLE; - - dynamixel::PacketHandler *pkt_handler = - dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); - dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; - - return pkt_handler->factoryReset(port_handler, dxl->id_, option, error); -} - -int RobotisController::read(const std::string joint_name, uint16_t address, - uint16_t length, uint8_t *data, uint8_t *error) { - if (isTimerStopped() == false) - return COMM_PORT_BUSY; - - Dynamixel *dxl = robot_->dxls_[joint_name]; - if (dxl == NULL) - return COMM_NOT_AVAILABLE; - - dynamixel::PacketHandler *pkt_handler = - dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); - dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; - - return pkt_handler->readTxRx(port_handler, dxl->id_, address, length, data, - error); -} - -int RobotisController::readCtrlItem(const std::string joint_name, - const std::string item_name, uint32_t *data, - uint8_t *error) { - if (isTimerStopped() == false) - return COMM_PORT_BUSY; - - Dynamixel *dxl = robot_->dxls_[joint_name]; - if (dxl == NULL) - return COMM_NOT_AVAILABLE; - - ControlTableItem *item = dxl->ctrl_table_[item_name]; - if (item == NULL) - return COMM_NOT_AVAILABLE; - - dynamixel::PacketHandler *pkt_handler = - dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); - dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; - - int result = COMM_NOT_AVAILABLE; - switch (item->data_length_) { - case 1: { - uint8_t read_data = 0; - result = pkt_handler->read1ByteTxRx(port_handler, dxl->id_, item->address_, - &read_data, error); - if (result == COMM_SUCCESS) - *data = read_data; - break; - } - case 2: { - uint16_t read_data = 0; - result = pkt_handler->read2ByteTxRx(port_handler, dxl->id_, item->address_, - &read_data, error); - if (result == COMM_SUCCESS) - *data = read_data; - break; - } - case 4: { - uint32_t read_data = 0; - result = pkt_handler->read4ByteTxRx(port_handler, dxl->id_, item->address_, - &read_data, error); - if (result == COMM_SUCCESS) - *data = read_data; - break; - } - default: - break; - } - return result; -} - -int RobotisController::read1Byte(const std::string joint_name, uint16_t address, - uint8_t *data, uint8_t *error) { - if (isTimerStopped() == false) - return COMM_PORT_BUSY; - - Dynamixel *dxl = robot_->dxls_[joint_name]; - if (dxl == NULL) - return COMM_NOT_AVAILABLE; - - dynamixel::PacketHandler *pkt_handler = - dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); - dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; - - return pkt_handler->read1ByteTxRx(port_handler, dxl->id_, address, data, - error); -} - -int RobotisController::read2Byte(const std::string joint_name, uint16_t address, - uint16_t *data, uint8_t *error) { - if (isTimerStopped() == false) - return COMM_PORT_BUSY; - - Dynamixel *dxl = robot_->dxls_[joint_name]; - if (dxl == NULL) - return COMM_NOT_AVAILABLE; - - dynamixel::PacketHandler *pkt_handler = - dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); - dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; - - return pkt_handler->read2ByteTxRx(port_handler, dxl->id_, address, data, - error); -} - -int RobotisController::read4Byte(const std::string joint_name, uint16_t address, - uint32_t *data, uint8_t *error) { - if (isTimerStopped() == false) - return COMM_PORT_BUSY; - - Dynamixel *dxl = robot_->dxls_[joint_name]; - if (dxl == NULL) - return COMM_NOT_AVAILABLE; - - dynamixel::PacketHandler *pkt_handler = - dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); - dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; - - return pkt_handler->read4ByteTxRx(port_handler, dxl->id_, address, data, - error); -} - -int RobotisController::write(const std::string joint_name, uint16_t address, - uint16_t length, uint8_t *data, uint8_t *error) { - if (isTimerStopped() == false) - return COMM_PORT_BUSY; - - Dynamixel *dxl = robot_->dxls_[joint_name]; - if (dxl == NULL) - return COMM_NOT_AVAILABLE; - - dynamixel::PacketHandler *pkt_handler = - dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); - dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; - - return pkt_handler->writeTxRx(port_handler, dxl->id_, address, length, data, - error); -} - -int RobotisController::writeCtrlItem(const std::string joint_name, - const std::string item_name, uint32_t data, - uint8_t *error) { - if (isTimerStopped() == false) - return COMM_PORT_BUSY; - - Dynamixel *dxl = robot_->dxls_[joint_name]; - if (dxl == NULL) - return COMM_NOT_AVAILABLE; - - ControlTableItem *item = dxl->ctrl_table_[item_name]; - if (item == NULL) - return COMM_NOT_AVAILABLE; - - dynamixel::PacketHandler *pkt_handler = - dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); - dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; - - int result = COMM_NOT_AVAILABLE; - uint8_t *write_data = new uint8_t[item->data_length_]; - if (item->data_length_ == 1) { - write_data[0] = (uint8_t)data; - result = pkt_handler->write1ByteTxRx(port_handler, dxl->id_, item->address_, - data, error); - } else if (item->data_length_ == 2) { - write_data[0] = DXL_LOBYTE((uint16_t)data); - write_data[1] = DXL_HIBYTE((uint16_t)data); - result = pkt_handler->write2ByteTxRx(port_handler, dxl->id_, item->address_, - data, error); - } else if (item->data_length_ == 4) { - write_data[0] = DXL_LOBYTE(DXL_LOWORD((uint32_t)data)); - write_data[1] = DXL_HIBYTE(DXL_LOWORD((uint32_t)data)); - write_data[2] = DXL_LOBYTE(DXL_HIWORD((uint32_t)data)); - write_data[3] = DXL_HIBYTE(DXL_HIWORD((uint32_t)data)); - result = pkt_handler->write4ByteTxRx(port_handler, dxl->id_, item->address_, - data, error); - } - delete[] write_data; - return result; -} - -int RobotisController::write1Byte(const std::string joint_name, - uint16_t address, uint8_t data, - uint8_t *error) { - if (isTimerStopped() == false) - return COMM_PORT_BUSY; - - Dynamixel *dxl = robot_->dxls_[joint_name]; - if (dxl == NULL) - return COMM_NOT_AVAILABLE; - - dynamixel::PacketHandler *pkt_handler = - dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); - dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; - - return pkt_handler->write1ByteTxRx(port_handler, dxl->id_, address, data, - error); -} - -int RobotisController::write2Byte(const std::string joint_name, - uint16_t address, uint16_t data, - uint8_t *error) { - if (isTimerStopped() == false) - return COMM_PORT_BUSY; - - Dynamixel *dxl = robot_->dxls_[joint_name]; - if (dxl == NULL) - return COMM_NOT_AVAILABLE; - - dynamixel::PacketHandler *pkt_handler = - dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); - dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; - - return pkt_handler->write2ByteTxRx(port_handler, dxl->id_, address, data, - error); -} - -int RobotisController::write4Byte(const std::string joint_name, - uint16_t address, uint32_t data, - uint8_t *error) { - if (isTimerStopped() == false) - return COMM_PORT_BUSY; - - Dynamixel *dxl = robot_->dxls_[joint_name]; - if (dxl == NULL) - return COMM_NOT_AVAILABLE; - - dynamixel::PacketHandler *pkt_handler = - dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); - dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; - - return pkt_handler->write4ByteTxRx(port_handler, dxl->id_, address, data, - error); -} - -int RobotisController::regWrite(const std::string joint_name, uint16_t address, - uint16_t length, uint8_t *data, - uint8_t *error) { - if (isTimerStopped() == false) - return COMM_PORT_BUSY; - - Dynamixel *dxl = robot_->dxls_[joint_name]; - if (dxl == NULL) - return COMM_NOT_AVAILABLE; - - dynamixel::PacketHandler *pkt_handler = - dynamixel::PacketHandler::getPacketHandler(dxl->protocol_version_); - dynamixel::PortHandler *port_handler = robot_->ports_[dxl->port_name_]; - - return pkt_handler->regWriteTxRx(port_handler, dxl->id_, address, length, - data, error); -} diff --git a/humanoid_robot_intelligence_control_system_external_sensors/CMakeLists.txt b/humanoid_robot_intelligence_control_system_external_sensors/CMakeLists.txt new file mode 100644 index 0000000..e7de26d --- /dev/null +++ b/humanoid_robot_intelligence_control_system_external_sensors/CMakeLists.txt @@ -0,0 +1,60 @@ +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# 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. + +cmake_minimum_required(VERSION 3.0.2) +project(humanoid_robot_intelligence_control_system_controller) + +if($ENV{ROS_VERSION} EQUAL 1) + find_package(catkin REQUIRED COMPONENTS + rospy + ) + + catkin_package( + CATKIN_DEPENDS + rospy + ) + +else() + find_package(ament_cmake REQUIRED) + find_package(ament_cmake_python REQUIRED) + find_package(rclpy REQUIRED) + find_package(std_msgs REQUIRED) +endif() + +if($ENV{ROS_VERSION} EQUAL 1) + catkin_python_setup() + + catkin_install_python(PROGRAMS + src/PIDController.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ) + + install(DIRECTORY config launch rviz + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + ) + +else() + ament_python_install_package(${PROJECT_NAME}) + + install(PROGRAMS + src/PIDController.py + DESTINATION lib/${PROJECT_NAME} + ) + + install(DIRECTORY config launch rviz + DESTINATION share/${PROJECT_NAME} + ) + + ament_package() +endif() diff --git a/humanoid_robot_intelligence_control_system_external_sensors/package.xml b/humanoid_robot_intelligence_control_system_external_sensors/package.xml new file mode 100644 index 0000000..0ab0b27 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_external_sensors/package.xml @@ -0,0 +1,46 @@ + + + + + humanoid_robot_intelligence_control_system_controller + 0.0.1 + + This Package is for Detection Math + + Apache 2.0 + Ronaldson Bellande + + catkin + ament_cmake + ament_cmake_python + + + rospy + + + rclpy + + + python3-opencv + python3-yaml + usb_cam + + + catkin + ament_cmake + + diff --git a/humanoid_robot_intelligence_control_system_external_sensors/setup.py b/humanoid_robot_intelligence_control_system_external_sensors/setup.py new file mode 100644 index 0000000..cbd6630 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_external_sensors/setup.py @@ -0,0 +1,26 @@ +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +# fetch values from package.xml +setup_args = generate_distutils_setup( + scripts=['src/PIDController.py'], + packages=['humanoid_robot_intelligence_control_system_controller'], + package_dir={'': 'src'}, +) + +setup(**setup_args) diff --git a/humanoid_robot_intelligence_control_system_external_sensors/src/PIDController.py b/humanoid_robot_intelligence_control_system_external_sensors/src/PIDController.py new file mode 100644 index 0000000..0a87c58 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_external_sensors/src/PIDController.py @@ -0,0 +1,50 @@ +#!/usr/bin/env python3 + +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +import time +from typing import Dict, List, Tuple + +class PIDController: + def __init__(self, gains: Tuple[float, float, float], name: str, output_limits: Tuple[float, float] = (-float('inf'), float('inf'))): + self.kp, self.ki, self.kd = gains + self.name = name + self.output_limits = output_limits + self.reset() + + def reset(self): + self.last_error = 0 + self.integral = 0 + self.last_time = time.time() + + def compute(self, setpoint: float, process_variable: float) -> float: + current_time = time.time() + dt = current_time - self.last_time + error = setpoint - process_variable + + self.integral += error * dt + derivative = (error - self.last_error) / dt if dt > 0 else 0 + + output = self.kp * error + self.ki * self.integral + self.kd * derivative + output = max(min(output, self.output_limits[1]), self.output_limits[0]) + + self.last_error = error + self.last_time = current_time + + return output + + def update_config(self, gains: Tuple[float, float, float]): + self.kp, self.ki, self.kd = gains diff --git a/humanoid_robot_intelligence_control_system_internal_sensors/CMakeLists.txt b/humanoid_robot_intelligence_control_system_internal_sensors/CMakeLists.txt new file mode 100644 index 0000000..e7de26d --- /dev/null +++ b/humanoid_robot_intelligence_control_system_internal_sensors/CMakeLists.txt @@ -0,0 +1,60 @@ +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# 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. + +cmake_minimum_required(VERSION 3.0.2) +project(humanoid_robot_intelligence_control_system_controller) + +if($ENV{ROS_VERSION} EQUAL 1) + find_package(catkin REQUIRED COMPONENTS + rospy + ) + + catkin_package( + CATKIN_DEPENDS + rospy + ) + +else() + find_package(ament_cmake REQUIRED) + find_package(ament_cmake_python REQUIRED) + find_package(rclpy REQUIRED) + find_package(std_msgs REQUIRED) +endif() + +if($ENV{ROS_VERSION} EQUAL 1) + catkin_python_setup() + + catkin_install_python(PROGRAMS + src/PIDController.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ) + + install(DIRECTORY config launch rviz + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + ) + +else() + ament_python_install_package(${PROJECT_NAME}) + + install(PROGRAMS + src/PIDController.py + DESTINATION lib/${PROJECT_NAME} + ) + + install(DIRECTORY config launch rviz + DESTINATION share/${PROJECT_NAME} + ) + + ament_package() +endif() diff --git a/humanoid_robot_intelligence_control_system_internal_sensors/package.xml b/humanoid_robot_intelligence_control_system_internal_sensors/package.xml new file mode 100644 index 0000000..0ab0b27 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_internal_sensors/package.xml @@ -0,0 +1,46 @@ + + + + + humanoid_robot_intelligence_control_system_controller + 0.0.1 + + This Package is for Detection Math + + Apache 2.0 + Ronaldson Bellande + + catkin + ament_cmake + ament_cmake_python + + + rospy + + + rclpy + + + python3-opencv + python3-yaml + usb_cam + + + catkin + ament_cmake + + diff --git a/humanoid_robot_intelligence_control_system_internal_sensors/setup.py b/humanoid_robot_intelligence_control_system_internal_sensors/setup.py new file mode 100644 index 0000000..cbd6630 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_internal_sensors/setup.py @@ -0,0 +1,26 @@ +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +# fetch values from package.xml +setup_args = generate_distutils_setup( + scripts=['src/PIDController.py'], + packages=['humanoid_robot_intelligence_control_system_controller'], + package_dir={'': 'src'}, +) + +setup(**setup_args) diff --git a/humanoid_robot_intelligence_control_system_internal_sensors/src/PIDController.py b/humanoid_robot_intelligence_control_system_internal_sensors/src/PIDController.py new file mode 100644 index 0000000..0a87c58 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_internal_sensors/src/PIDController.py @@ -0,0 +1,50 @@ +#!/usr/bin/env python3 + +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +import time +from typing import Dict, List, Tuple + +class PIDController: + def __init__(self, gains: Tuple[float, float, float], name: str, output_limits: Tuple[float, float] = (-float('inf'), float('inf'))): + self.kp, self.ki, self.kd = gains + self.name = name + self.output_limits = output_limits + self.reset() + + def reset(self): + self.last_error = 0 + self.integral = 0 + self.last_time = time.time() + + def compute(self, setpoint: float, process_variable: float) -> float: + current_time = time.time() + dt = current_time - self.last_time + error = setpoint - process_variable + + self.integral += error * dt + derivative = (error - self.last_error) / dt if dt > 0 else 0 + + output = self.kp * error + self.ki * self.integral + self.kd * derivative + output = max(min(output, self.output_limits[1]), self.output_limits[0]) + + self.last_error = error + self.last_time = current_time + + return output + + def update_config(self, gains: Tuple[float, float, float]): + self.kp, self.ki, self.kd = gains diff --git a/humanoid_robot_intelligence_control_system_object_detector/CMakeLists.txt b/humanoid_robot_intelligence_control_system_object_detector/CMakeLists.txt new file mode 100644 index 0000000..55bba24 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_object_detector/CMakeLists.txt @@ -0,0 +1,71 @@ +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# 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. + +cmake_minimum_required(VERSION 3.0.2) +project(humanoid_robot_intelligence_control_system_object_detector) + +if($ENV{ROS_VERSION} EQUAL 1) + find_package(catkin REQUIRED COMPONENTS + rospy + std_msgs + sensor_msgs + geometry_msgs + cv_bridge + ) + + catkin_package( + CATKIN_DEPENDS + rospy + std_msgs + sensor_msgs + geometry_msgs + cv_bridge + ) + +else() + find_package(ament_cmake REQUIRED) + find_package(ament_cmake_python REQUIRED) + find_package(rclpy REQUIRED) + find_package(std_msgs REQUIRED) + find_package(sensor_msgs REQUIRED) + find_package(geometry_msgs REQUIRED) + find_package(cv_bridge REQUIRED) +endif() + +if($ENV{ROS_VERSION} EQUAL 1) + catkin_python_setup() + + catkin_install_python(PROGRAMS + src/object_detection_processor.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ) + + install(DIRECTORY config launch rviz + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + ) + +else() + ament_python_install_package(${PROJECT_NAME}) + + install(PROGRAMS + src/object_detection_processor.py + DESTINATION lib/${PROJECT_NAME} + ) + + install(DIRECTORY config launch rviz + DESTINATION share/${PROJECT_NAME} + ) + + ament_package() +endif() diff --git a/humanoid_robot_intelligence_control_system_object_detector/launch/object_detector_processor.launch.py b/humanoid_robot_intelligence_control_system_object_detector/launch/object_detector_processor.launch.py new file mode 100644 index 0000000..64ac1dd --- /dev/null +++ b/humanoid_robot_intelligence_control_system_object_detector/launch/object_detector_processor.launch.py @@ -0,0 +1,108 @@ +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +import os +import sys +import subprocess +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration + +def ros1_launch_description(): + # Get command-line arguments + args = sys.argv[1:] + + # Construct the ROS 1 launch command + roslaunch_command = ["roslaunch", "humanoid_robot_intelligence_control_system_object_detector", "object_detector_processor.launch"] + args + + roslaunch_command.extend([ + "usb_cam", "usb_cam_node", "name:=camera", + "video_device:=/dev/video0", + "image_width:=640", + "image_height:=480", + "pixel_format:=yuyv", + "camera_frame_id:=usb_cam", + "io_method:=mmap" + ]) + + roslaunch_command.extend([ + "ros_web_api_bellande_2d_computer_vision", "bellande_2d_computer_vision_object_detection.py", "name:=object_detection_node" + ]) + + roslaunch_command.extend([ + "humanoid_robot_intelligence_control_system_detector", "object_detection_processor.py", "name:=object_detection_processor_node" + ]) + + roslaunch_command.extend([ + "rviz", "rviz", "name:=rviz", + "args:=-d $(find ros_web_api_bellande_2d_computer_vision)/rviz/visualization.rviz" + ]) + + # Execute the launch command + subprocess.call(roslaunch_command) + +def ros2_launch_description(): + nodes_to_launch = [] + + nodes_to_launch.append(Node( + package='usb_cam', + executable='usb_cam_node', + name='camera', + output='screen', + parameters=[{ + 'video_device': '/dev/video0', + 'image_width': 640, + 'image_height': 480, + 'pixel_format': 'yuyv', + 'camera_frame_id': 'usb_cam', + 'io_method': 'mmap' + }] + )) + + nodes_to_launch.append(Node( + package='ros_web_api_bellande_2d_computer_vision', + executable='bellande_2d_computer_vision_object_detection.py', + name='object_detection_node', + output='screen', + remappings=[('camera/image_raw', '/usb_cam/image_raw')] + )) + + nodes_to_launch.append(Node( + package='humanoid_robot_intelligence_control_system_object_detector', + executable='object_detection_processor.py', + name='object_detection_processor_node', + output='screen', + parameters=[{'yaml_path': '$(find ros_web_api_bellande_2d_computer_vision)/yaml/object_detection_params.yaml'}] + )) + + nodes_to_launch.append(Node( + package='rviz2', + executable='rviz2', + name='rviz', + arguments=['-d', '$(find ros_web_api_bellande_2d_computer_vision)/rviz/visualization.rviz'] + )) + + return LaunchDescription(nodes_to_launch) + +if __name__ == "__main__": + ros_version = os.getenv("ROS_VERSION") + if ros_version == "1": + ros1_launch_description() + elif ros_version == "2": + ros2_launch_description() + else: + print("Unsupported ROS version. Please set the ROS_VERSION environment variable to '1' for ROS 1 or '2' for ROS 2.") + sys.exit(1) diff --git a/humanoid_robot_intelligence_control_system_object_detector/launch/ros1/object_detector_processor.launch b/humanoid_robot_intelligence_control_system_object_detector/launch/ros1/object_detector_processor.launch new file mode 100644 index 0000000..6799e95 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_object_detector/launch/ros1/object_detector_processor.launch @@ -0,0 +1,41 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/humanoid_robot_intelligence_control_system_object_detector/package.xml b/humanoid_robot_intelligence_control_system_object_detector/package.xml new file mode 100644 index 0000000..706fe9e --- /dev/null +++ b/humanoid_robot_intelligence_control_system_object_detector/package.xml @@ -0,0 +1,61 @@ + + + + + humanoid_robot_intelligence_control_system_object_detector + 0.0.1 + + This Package is for Object Detection, detecting objects like tools, or utilities + + Apache 2.0 + Ronaldson Bellande + + catkin + ament_cmake + ament_cmake_python + + + rospy + std_msgs + sensor_msgs + geometry_msgs + cv_bridge + image_transport + dynamic_reconfigure + message_generation + message_runtime + + + rclpy + std_msgs + sensor_msgs + geometry_msgs + cv_bridge + image_transport + rosidl_default_generators + rosidl_default_runtime + + + python3-opencv + python3-yaml + usb_cam + + + catkin + ament_cmake + + diff --git a/humanoid_robot_intelligence_control_system_object_detector/setup.py b/humanoid_robot_intelligence_control_system_object_detector/setup.py new file mode 100644 index 0000000..a2eac39 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_object_detector/setup.py @@ -0,0 +1,26 @@ +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +# fetch values from package.xml +setup_args = generate_distutils_setup( + scripts=['src/object_detection_processor.py'], + packages=['humanoid_robot_intelligence_control_system_object_detector'], + package_dir={'': 'src'}, +) + +setup(**setup_args) diff --git a/humanoid_robot_intelligence_control_system_object_detector/src/object_detection_processor.py b/humanoid_robot_intelligence_control_system_object_detector/src/object_detection_processor.py new file mode 100644 index 0000000..6f44d68 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_object_detector/src/object_detection_processor.py @@ -0,0 +1,111 @@ +#!/usr/bin/env python3 + +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +import rospy +import cv2 +import numpy as np +from sensor_msgs.msg import Image, CameraInfo +from std_msgs.msg import Bool, String +from cv_bridge import CvBridge +import yaml + +class ObjectDetectionProcessor: + def __init__(self): + rospy.init_node('object_detection_processor') + self.bridge = CvBridge() + self.enable = True + self.new_image_flag = False + self.load_params() + self.setup_ros() + + def load_params(self): + param_path = rospy.get_param('~yaml_path', '') + if param_path: + with open(param_path, 'r') as file: + self.params = yaml.safe_load(file) + else: + self.set_default_params() + + def set_default_params(self): + self.params = { + 'debug': False, + 'ellipse_size': 2, + # Add other default parameters as needed + } + + def setup_ros(self): + self.image_pub = rospy.Publisher('image_out', Image, queue_size=10) + self.camera_info_pub = rospy.Publisher('camera_info', CameraInfo, queue_size=10) + + rospy.Subscriber('enable', Bool, self.enable_callback) + rospy.Subscriber('image_in', Image, self.image_callback) + rospy.Subscriber('cameraInfo_in', CameraInfo, self.camera_info_callback) + rospy.Subscriber('object_detection_result', String, self.object_detection_callback) + + def enable_callback(self, msg): + self.enable = msg.data + + def image_callback(self, msg): + if not self.enable: + return + self.cv_image = self.bridge.imgmsg_to_cv2(msg, "bgr8") + self.new_image_flag = True + self.image_header = msg.header + + def camera_info_callback(self, msg): + if not self.enable: + return + self.camera_info_msg = msg + + def object_detection_callback(self, msg): + if not self.enable or not hasattr(self, 'cv_image'): + return + + objects = eval(msg.data) # Assuming the data is a string representation of a list + self.process_detected_objects(objects) + + def process_detected_objects(self, objects): + output_image = self.cv_image.copy() + for obj in objects: + x, y, w, h = obj['bbox'] + cv2.rectangle(output_image, (int(x), int(y)), (int(x+w), int(y+h)), (0, 255, 0), 2) + cv2.putText(output_image, f"{obj['label']}: {obj['confidence']:.2f}", + (int(x), int(y-10)), cv2.FONT_HERSHEY_SIMPLEX, 0.9, (0, 255, 0), 2) + + self.publish_image(output_image) + + def publish_image(self, image): + img_msg = self.bridge.cv2_to_imgmsg(image, encoding="bgr8") + img_msg.header = self.image_header + self.image_pub.publish(img_msg) + if hasattr(self, 'camera_info_msg'): + self.camera_info_pub.publish(self.camera_info_msg) + + def run(self): + rate = rospy.Rate(30) # 30 Hz + while not rospy.is_shutdown(): + if self.new_image_flag: + # The processing is done in object_detection_callback + self.new_image_flag = False + rate.sleep() + +if __name__ == '__main__': + try: + processor = ObjectDetectionProcessor() + processor.run() + except rospy.ROSInterruptException: + pass diff --git a/humanoid_robot_intelligence_control_system_speech_detector/CMakeLists.txt b/humanoid_robot_intelligence_control_system_speech_detector/CMakeLists.txt new file mode 100644 index 0000000..78abc20 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_speech_detector/CMakeLists.txt @@ -0,0 +1,71 @@ +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# 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. + +cmake_minimum_required(VERSION 3.0.2) +project(humanoid_robot_intelligence_control_system_speech_detector) + +if($ENV{ROS_VERSION} EQUAL 1) + find_package(catkin REQUIRED COMPONENTS + rospy + std_msgs + sensor_msgs + geometry_msgs + cv_bridge + ) + + catkin_package( + CATKIN_DEPENDS + rospy + std_msgs + sensor_msgs + geometry_msgs + cv_bridge + ) + +else() + find_package(ament_cmake REQUIRED) + find_package(ament_cmake_python REQUIRED) + find_package(rclpy REQUIRED) + find_package(std_msgs REQUIRED) + find_package(sensor_msgs REQUIRED) + find_package(geometry_msgs REQUIRED) + find_package(cv_bridge REQUIRED) +endif() + +if($ENV{ROS_VERSION} EQUAL 1) + catkin_python_setup() + + catkin_install_python(PROGRAMS + src/speech_detection_processor.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ) + + install(DIRECTORY config launch rviz + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + ) + +else() + ament_python_install_package(${PROJECT_NAME}) + + install(PROGRAMS + src/speech_detection_processor.py + DESTINATION lib/${PROJECT_NAME} + ) + + install(DIRECTORY config launch rviz + DESTINATION share/${PROJECT_NAME} + ) + + ament_package() +endif() diff --git a/humanoid_robot_intelligence_control_system_speech_detector/launch/ros1/speech_detector_processor.launch b/humanoid_robot_intelligence_control_system_speech_detector/launch/ros1/speech_detector_processor.launch new file mode 100644 index 0000000..014521f --- /dev/null +++ b/humanoid_robot_intelligence_control_system_speech_detector/launch/ros1/speech_detector_processor.launch @@ -0,0 +1,41 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/humanoid_robot_intelligence_control_system_speech_detector/launch/speech_detector_processor.launch.py b/humanoid_robot_intelligence_control_system_speech_detector/launch/speech_detector_processor.launch.py new file mode 100644 index 0000000..c11ca70 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_speech_detector/launch/speech_detector_processor.launch.py @@ -0,0 +1,108 @@ +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +import os +import sys +import subprocess +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration + +def ros1_launch_description(): + # Get command-line arguments + args = sys.argv[1:] + + # Construct the ROS 1 launch command + roslaunch_command = ["roslaunch", "humanoid_robot_intelligence_control_system_speech_detector", "speech_detector_processor.launch"] + args + + roslaunch_command.extend([ + "usb_cam", "usb_cam_node", "name:=camera", + "video_device:=/dev/video0", + "image_width:=640", + "image_height:=480", + "pixel_format:=yuyv", + "camera_frame_id:=usb_cam", + "io_method:=mmap" + ]) + + roslaunch_command.extend([ + "ros_web_api_bellande_2d_computer_vision", "bellande_2d_computer_vision_speech_detection.py", "name:=speech_detection_node" + ]) + + roslaunch_command.extend([ + "humanoid_robot_intelligence_control_system_ball_detector", "speech_detection_processor.py", "name:=speech_detection_processor_node" + ]) + + roslaunch_command.extend([ + "rviz", "rviz", "name:=rviz", + "args:=-d $(find ros_web_api_bellande_2d_computer_vision)/rviz/visualization.rviz" + ]) + + # Execute the launch command + subprocess.call(roslaunch_command) + +def ros2_launch_description(): + nodes_to_launch = [] + + nodes_to_launch.append(Node( + package='usb_cam', + executable='usb_cam_node', + name='camera', + output='screen', + parameters=[{ + 'video_device': '/dev/video0', + 'image_width': 640, + 'image_height': 480, + 'pixel_format': 'yuyv', + 'camera_frame_id': 'usb_cam', + 'io_method': 'mmap' + }] + )) + + nodes_to_launch.append(Node( + package='ros_web_api_bellande_2d_computer_vision', + executable='bellande_2d_computer_vision_speech_detection.py', + name='speech_detection_node', + output='screen', + remappings=[('camera/image_raw', '/usb_cam/image_raw')] + )) + + nodes_to_launch.append(Node( + package='humanoid_robot_intelligence_control_system_speech_detector', + executable='speech_detection_processor.py', + name='speech_detection_processor_node', + output='screen', + parameters=[{'yaml_path': '$(find ros_web_api_bellande_2d_computer_vision)/yaml/speech_detection_params.yaml'}] + )) + + nodes_to_launch.append(Node( + package='rviz2', + executable='rviz2', + name='rviz', + arguments=['-d', '$(find ros_web_api_bellande_2d_computer_vision)/rviz/visualization.rviz'] + )) + + return LaunchDescription(nodes_to_launch) + +if __name__ == "__main__": + ros_version = os.getenv("ROS_VERSION") + if ros_version == "1": + ros1_launch_description() + elif ros_version == "2": + ros2_launch_description() + else: + print("Unsupported ROS version. Please set the ROS_VERSION environment variable to '1' for ROS 1 or '2' for ROS 2.") + sys.exit(1) diff --git a/humanoid_robot_intelligence_control_system_speech_detector/package.xml b/humanoid_robot_intelligence_control_system_speech_detector/package.xml new file mode 100644 index 0000000..416194b --- /dev/null +++ b/humanoid_robot_intelligence_control_system_speech_detector/package.xml @@ -0,0 +1,61 @@ + + + + + humanoid_robot_intelligence_control_system_speech_detector + 0.0.1 + + This Package is for Object Detection, detecting speechs like tools, or utilities + + Apache 2.0 + Ronaldson Bellande + + catkin + ament_cmake + ament_cmake_python + + + rospy + std_msgs + sensor_msgs + geometry_msgs + cv_bridge + image_transport + dynamic_reconfigure + message_generation + message_runtime + + + rclpy + std_msgs + sensor_msgs + geometry_msgs + cv_bridge + image_transport + rosidl_default_generators + rosidl_default_runtime + + + python3-opencv + python3-yaml + usb_cam + + + catkin + ament_cmake + + diff --git a/humanoid_robot_intelligence_control_system_speech_detector/setup.py b/humanoid_robot_intelligence_control_system_speech_detector/setup.py new file mode 100644 index 0000000..f4a55e3 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_speech_detector/setup.py @@ -0,0 +1,26 @@ +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +# fetch values from package.xml +setup_args = generate_distutils_setup( + scripts=['src/speech_detection_processor.py'], + packages=['humanoid_robot_intelligence_control_system_speech_detector'], + package_dir={'': 'src'}, +) + +setup(**setup_args) diff --git a/humanoid_robot_intelligence_control_system_speech_detector/src/speech_detection_processor.py b/humanoid_robot_intelligence_control_system_speech_detector/src/speech_detection_processor.py new file mode 100644 index 0000000..fcc60d2 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_speech_detector/src/speech_detection_processor.py @@ -0,0 +1,111 @@ +#!/usr/bin/env python3 + +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +import rospy +import cv2 +import numpy as np +from sensor_msgs.msg import Image, CameraInfo +from std_msgs.msg import Bool, String +from cv_bridge import CvBridge +import yaml + +class ObjectDetectionProcessor: + def __init__(self): + rospy.init_node('speech_detection_processor') + self.bridge = CvBridge() + self.enable = True + self.new_image_flag = False + self.load_params() + self.setup_ros() + + def load_params(self): + param_path = rospy.get_param('~yaml_path', '') + if param_path: + with open(param_path, 'r') as file: + self.params = yaml.safe_load(file) + else: + self.set_default_params() + + def set_default_params(self): + self.params = { + 'debug': False, + 'ellipse_size': 2, + # Add other default parameters as needed + } + + def setup_ros(self): + self.image_pub = rospy.Publisher('image_out', Image, queue_size=10) + self.camera_info_pub = rospy.Publisher('camera_info', CameraInfo, queue_size=10) + + rospy.Subscriber('enable', Bool, self.enable_callback) + rospy.Subscriber('image_in', Image, self.image_callback) + rospy.Subscriber('cameraInfo_in', CameraInfo, self.camera_info_callback) + rospy.Subscriber('speech_detection_result', String, self.speech_detection_callback) + + def enable_callback(self, msg): + self.enable = msg.data + + def image_callback(self, msg): + if not self.enable: + return + self.cv_image = self.bridge.imgmsg_to_cv2(msg, "bgr8") + self.new_image_flag = True + self.image_header = msg.header + + def camera_info_callback(self, msg): + if not self.enable: + return + self.camera_info_msg = msg + + def speech_detection_callback(self, msg): + if not self.enable or not hasattr(self, 'cv_image'): + return + + speechs = eval(msg.data) # Assuming the data is a string representation of a list + self.process_detected_speechs(speechs) + + def process_detected_speechs(self, speechs): + output_image = self.cv_image.copy() + for obj in speechs: + x, y, w, h = obj['bbox'] + cv2.rectangle(output_image, (int(x), int(y)), (int(x+w), int(y+h)), (0, 255, 0), 2) + cv2.putText(output_image, f"{obj['label']}: {obj['confidence']:.2f}", + (int(x), int(y-10)), cv2.FONT_HERSHEY_SIMPLEX, 0.9, (0, 255, 0), 2) + + self.publish_image(output_image) + + def publish_image(self, image): + img_msg = self.bridge.cv2_to_imgmsg(image, encoding="bgr8") + img_msg.header = self.image_header + self.image_pub.publish(img_msg) + if hasattr(self, 'camera_info_msg'): + self.camera_info_pub.publish(self.camera_info_msg) + + def run(self): + rate = rospy.Rate(30) # 30 Hz + while not rospy.is_shutdown(): + if self.new_image_flag: + # The processing is done in speech_detection_callback + self.new_image_flag = False + rate.sleep() + +if __name__ == '__main__': + try: + processor = ObjectDetectionProcessor() + processor.run() + except rospy.ROSInterruptException: + pass