BRSRI/bellande_humanoid_robot_intelligence_control_system_framework
1
0
Fork 0
Code Issues Pull Requests Actions 1 Packages Projects Releases Wiki Activity

modified.

Browse Source
This commit is contained in:
ROBOTIS-zerom 2016-04-29 16:18:25 +09:00
parent 738b68b6e4
commit c72bab42ba
59 changed files with 1987 additions and 789 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
dynamixel_sdk/CMakeLists.txt
View File

@ -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
)

2
dynamixel_sdk/include/DynamixelSDK.h
View File

@ -2,7 +2,7 @@
* DynamixelSDK.h
*
* Created on: 2016. 3. 8.
* Author: zerom
* Author: zerom, leon
*/
#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_DYNAMIXELSDK_H_

9
dynamixel_sdk/include/dynamixel_sdk/GroupBulkRead.h
View File

@ -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);
};
}

4
dynamixel_sdk/include/dynamixel_sdk/GroupBulkWrite.h
View File

@ -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_;

9
dynamixel_sdk/include/dynamixel_sdk/GroupSyncRead.h
View File

@ -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);
};
}

4
dynamixel_sdk/include/dynamixel_sdk/GroupSyncWrite.h
View File

@ -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_;

4
dynamixel_sdk/include/dynamixel_sdk/PacketHandler.h
View File

@ -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() { }

13
dynamixel_sdk/include/dynamixel_sdk/PortHandler.h
View File

@ -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;

4
dynamixel_sdk/include/dynamixel_sdk/Protocol1PacketHandler.h
View File

@ -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_;

4
dynamixel_sdk/include/dynamixel_sdk/Protocol2PacketHandler.h
View File

@ -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_;

2
dynamixel_sdk/include/dynamixel_sdk/RobotisDef.h
View File

@ -2,7 +2,7 @@
* RobotisDef.h
*
* Created on: 2016. 1. 27.
* Author: zerom
* Author: zerom, leon
*/
#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_ROBOTISDEF_H_

2
dynamixel_sdk/include/dynamixel_sdk_linux/PortHandlerLinux.h
View File

@ -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_

57
dynamixel_sdk/src/dynamixel_sdk/GroupBulkRead.cpp
View File

@ -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 <stdio.h>
#include <algorithm>
@ -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;
}

5
dynamixel_sdk/src/dynamixel_sdk/GroupBulkWrite.cpp
View File

@ -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 <algorithm>
#include "dynamixel_sdk/GroupBulkWrite.h"

68
dynamixel_sdk/src/dynamixel_sdk/GroupSyncRead.cpp
View File

@ -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 <algorithm>
#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;
}
}

5
dynamixel_sdk/src/dynamixel_sdk/GroupSyncWrite.cpp
View File

@ -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 <algorithm>
#include "dynamixel_sdk/GroupSyncWrite.h"

5
dynamixel_sdk/src/dynamixel_sdk/PacketHandler.cpp
View File

@ -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"

7
dynamixel_sdk/src/dynamixel_sdk/PortHandler.cpp
View File

@ -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
}

13
dynamixel_sdk/src/dynamixel_sdk/Protocol1PacketHandler.cpp
View File

@ -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 <string.h>
#include <stdlib.h>
@ -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;
}

13
dynamixel_sdk/src/dynamixel_sdk/Protocol2PacketHandler.cpp
View File

@ -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 <stdio.h>
#include <string.h>
#include <stdlib.h>
@ -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

2
dynamixel_sdk/src/dynamixel_sdk_linux/PortHandlerLinux.cpp
View File

@ -2,7 +2,7 @@
* PortHandlerLinux.cpp
*
* Created on: 2016. 1. 26.
* Author: zerom
* Author: zerom, leon
*/
#include <stdio.h>

4
robotis_controller/CMakeLists.txt
View File

@ -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}
)

22
robotis_controller/include/robotis_controller/RobotisController.h
View File

@ -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<MotionModule *> modules_;
std::list<MotionModule *> motion_modules_;
std::list<SensorModule *> sensor_modules_;
std::vector<GroupSyncWrite *> direct_sync_write_;
std::map<std::string, double> 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<std::string, GroupBulkRead *> port_to_bulk_read;
std::map<std::string, GroupSyncWrite *> port_to_sync_write;
std::map<std::string, GroupSyncWrite *> port_to_sync_write_position;
std::map<std::string, GroupSyncWrite *> port_to_sync_write_velocity;
std::map<std::string, GroupSyncWrite *> 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);

724
robotis_controller/src/robotis_controller/RobotisController.cpp
View File

@ -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<std::string, GroupSyncWrite *>::iterator _it = port_to_sync_write.begin(); _it != port_to_sync_write.end(); _it++)
for(std::map<std::string, GroupSyncWrite *>::iterator _it = port_to_sync_write_position.begin(); _it != port_to_sync_write_position.end(); _it++)
_it->second->ClearParam();
for(std::map<std::string, GroupSyncWrite *>::iterator _it = port_to_sync_write_velocity.begin(); _it != port_to_sync_write_velocity.end(); _it++)
_it->second->ClearParam();
for(std::map<std::string, GroupSyncWrite *>::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<std::string, Dynamixel*>::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<MotionModule *>::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<std::string, PortHandler *>::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<std::string>();
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<UINT32_T>();
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<int>();
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<std::string>().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<UINT32_T>();
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<std::string, ControlTableItem *>::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<std::string, PortHandler *>::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<sensor_msgs::JointState>("/robotis/goal_joint_states", 10);
present_joint_state_pub = _ros_node.advertise<sensor_msgs::JointState>("/robotis/present_joint_states", 10);
current_module_pub = _ros_node.advertise<robotis_controller_msgs::JointCtrlModule>("/robotis/current_ctrl_module", 10);
goal_joint_state_pub = _ros_node.advertise<sensor_msgs::JointState>("/robotis/goal_joint_states", 10);
present_joint_state_pub = _ros_node.advertise<sensor_msgs::JointState>("/robotis/present_joint_states", 10);
current_module_pub = _ros_node.advertise<robotis_controller_msgs::JointCtrlModule>("/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<std::string, GroupBulkRead *>::iterator _it = port_to_bulk_read.begin(); _it != port_to_bulk_read.end(); _it++)
_it->second->RxPacket();
for(std::map<std::string, GroupSyncWrite *>::iterator _it = port_to_sync_write.begin(); _it != port_to_sync_write.end(); _it++)
for(std::map<std::string, GroupSyncWrite *>::iterator _it = port_to_sync_write_position.begin(); _it != port_to_sync_write_position.end(); _it++)
_it->second->ClearParam();
for(std::map<std::string, GroupSyncWrite *>::iterator _it = port_to_sync_write_velocity.begin(); _it != port_to_sync_write_velocity.end(); _it++)
_it->second->ClearParam();
for(std::map<std::string, GroupSyncWrite *>::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<std::string, GroupBulkRead *>::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<std::string, Dynamixel *>::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<SensorModule*>::iterator _module_it = sensor_modules_.begin(); _module_it != sensor_modules_.end(); _module_it++)
{
(*_module_it)->Process(robot->dxls);
for(std::map<std::string, double>::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<MotionModule*>::iterator module_it = modules_.begin(); module_it != modules_.end(); module_it++)
for(std::list<MotionModule*>::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<std::string, Dynamixel *>::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<std::string, GroupSyncWrite *>::iterator _it = port_to_sync_write.begin(); _it != port_to_sync_write.end(); _it++)
for(std::map<std::string, GroupSyncWrite *>::iterator _it = port_to_sync_write_position.begin(); _it != port_to_sync_write_position.end(); _it++)
_it->second->TxPacket();
for(std::map<std::string, GroupSyncWrite *>::iterator _it = port_to_sync_write_velocity.begin(); _it != port_to_sync_write_velocity.end(); _it++)
_it->second->TxPacket();
for(std::map<std::string, GroupSyncWrite *>::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<std::string, GroupSyncWrite *>::iterator _it = port_to_sync_write.begin(); _it != port_to_sync_write.end(); _it++)
for(std::map<std::string, GroupSyncWrite *>::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<std::string, GroupBulkRead *>::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<std::string, Dynamixel *>::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<MotionModule *>::iterator _m_it = modules_.begin(); _m_it != modules_.end(); _m_it++)
for(std::list<MotionModule *>::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<MotionModule *>::iterator _m_it = modules_.begin(); _m_it != modules_.end(); _m_it++)
for(std::list<MotionModule *>::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<MotionModule *> _stop_modules;
if(ctrl_module == "" || ctrl_module == "none")
{
// enqueue all modules in order to stop
for(std::list<MotionModule *>::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<MotionModule *>::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<std::string, DynamixelState*>::iterator _result_it = (*_m_it)->result.begin(); _result_it != (*_m_it)->result.end(); _result_it++)
{
std::map<std::string, Dynamixel*>::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<MotionModule *>::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<MotionModule *>::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<MotionModule *>::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<MotionModule *>::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<std::string, Dynamixel*>::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<MotionModule *>::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<std::string, DynamixelState*>::iterator _result_it = (*_m_it)->result.begin(); _result_it != (*_m_it)->result.end(); _result_it++)
{
std::map<std::string, Dynamixel*>::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<MotionModule *>::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<std::string, Dynamixel*>::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<std::string, Dynamixel *>::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;
}

1
robotis_controller_msgs/CMakeLists.txt
View File

@ -15,6 +15,7 @@ add_message_files(
FILES
SyncWriteItem.msg
JointCtrlModule.msg
StatusMsg.msg
)
## Generate services in the 'srv' folder

10
robotis_controller_msgs/msg/StatusMsg.msg Normal file
View File

@ -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

3
robotis_device/CMakeLists.txt
View File

@ -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}
)

30
robotis_device/devices/dynamixel/GRIPPER.device
View File

@ -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

32
robotis_device/devices/dynamixel/H42-20-S300-R.device
View File

@ -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

32
robotis_device/devices/dynamixel/H54-100-S500-R.device
View File

@ -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

32
robotis_device/devices/dynamixel/H54-200-B500-R.device
View File

@ -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

32
robotis_device/devices/dynamixel/H54-200-S500-R.device
View File

@ -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

30
robotis_device/devices/dynamixel/L42-10-S300-R.device
View File

@ -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

32
robotis_device/devices/dynamixel/L54-30-S400-R.device
View File

@ -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

32
robotis_device/devices/dynamixel/L54-30-S500-R.device
View File

@ -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

32
robotis_device/devices/dynamixel/L54-50-S290-R.device
View File

@ -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

32
robotis_device/devices/dynamixel/L54-50-S500-R.device
View File

@ -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

32
robotis_device/devices/dynamixel/M42-10-S260-R.device
View File

@ -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

32
robotis_device/devices/dynamixel/M54-40-S250-R.device
View File

@ -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

32
robotis_device/devices/dynamixel/M54-60-S250-R.device
View File

@ -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

30
robotis_device/devices/dynamixel/MX-28.device
View File

@ -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

4
robotis_device/include/robotis_device/ControlTableItem.h
View File

@ -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),

31
robotis_device/include/robotis_device/Dynamixel.h
View File

@ -10,6 +10,7 @@
#include <map>
#include <vector>
#include <string>
#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<ControlTableItem *> bulk_read_items;
std::map<std::string, UINT16_T> 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);
};
}

42
robotis_device/include/robotis_device/DynamixelState.h
View File

@ -11,6 +11,9 @@
#include <stdint.h>
#include <robotis_framework_common/RobotisDef.h>
#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<std::string, UINT32_T> 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();
}
};

2
robotis_device/include/robotis_device/Robot.h
View File

@ -21,6 +21,8 @@ class Robot
{
public:
std::map<std::string, PortHandler *> ports; // string: port name
std::map<std::string, std::string> port_default_joint; // port name, default joint name
std::map<std::string, Dynamixel *> dxls; // string: joint name
std::map<std::string, Sensor *> sensors; // string: sensor name

47
robotis_device/src/robotis_device/Dynamixel.cpp
View File

@ -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);
}

143
robotis_device/src/robotis_device/Robot.cpp
View File

@ -71,18 +71,19 @@ Robot::Robot(std::string robot_file_path, std::string dev_desc_dir_path)
if(session == "port info")
{
std::vector<std::string> 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<std::string> 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<std::string> sub_tokens = split(tokens[6], ',');
if(sub_tokens.size() > 0)
{
std::map<std::string, ControlTableItem *>::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<std::string> tokens = split(input_str, '=');
std::vector<std::string> 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<std::string> tokens = split(input_str, '=');
std::vector<std::string> 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<std::string> tokens = split(input_str, '|');
std::vector<std::string> 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();

130
robotis_framework_common/CMakeLists.txt
View File

@ -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)

21
robotis_framework_common/include/robotis_framework_common/MotionModule.h
View File

@ -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 <map>
#include <string>
#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<std::string, Dynamixel *> dxls) = 0;
virtual void Initialize(const int control_cycle_msec, Robot *robot) = 0;
virtual void Process(std::map<std::string, Dynamixel *> dxls, std::map<std::string, double> 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_ */

37
robotis_framework_common/include/robotis_framework_common/SensorModule.h Normal file
View File

@ -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 <map>
#include <string>
#include "robotis_device/Robot.h"
#include "robotis_device/Dynamixel.h"
namespace ROBOTIS
{
class SensorModule
{
public:
std::string module_name;
std::map<std::string, double> result;
virtual ~SensorModule() { }
virtual void Initialize(const int control_cycle_msec, Robot *robot) = 0;
virtual void Process(std::map<std::string, Dynamixel *> dxls) = 0;
};
}
#endif /* ROBOTIS_FRAMEWORK_COMMON_INCLUDE_ROBOTIS_FRAMEWORK_COMMON_SENSORMODULE_H_ */

65
robotis_math/CMakeLists.txt Normal file
View File

@ -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}
)

52
robotis_math/include/robotis_math/RobotisLinearAlgebra.h Normal file
View File

@ -0,0 +1,52 @@
/*
* RobotisLinearAlgebra.h
*
* Created on: Mar 18, 2016
* Author: jay
*/
#ifndef ROBOTIS_LINEAR_ALGEBRA_H_
#define ROBOTIS_LINEAR_ALGEBRA_H_
#include <cmath>
#define EIGEN_NO_DEBUG
#define EIGEN_NO_STATIC_ASSERT
#include <Eigen/Dense>
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_ */

13
robotis_math/include/robotis_math/RobotisMath.h Normal file
View File

@ -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_ */

33
robotis_math/include/robotis_math/RobotisMathBase.h Normal file
View File

@ -0,0 +1,33 @@
/*
* RobotisMathBase.h
*
* Created on: 2016. 3. 28.
* Author: JaySong
*/
#ifndef ROBOTIS_MATH_BASE_H_
#define ROBOTIS_MATH_BASE_H_
#include <cmath>
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_ */

37
robotis_math/include/robotis_math/RobotisTrajectoryCalculator.h Normal file
View File

@ -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_ */

37
robotis_math/package.xml Normal file
View File

@ -0,0 +1,37 @@
<?xml version="1.0"?>
<package>
<name>robotis_math</name>
<version>0.0.0</version>
<description>The robotis_math package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="jay@todo.todo">jay</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>TODO</license>
<!-- Url tags are optional, but mutiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/robotis_math</url> -->
<!-- Author tags are optional, mutiple are allowed, one per tag -->
<!-- Authors do not have to be maintianers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
<build_depend>cmake_modules</build_depend>
<run_depend>roscpp</run_depend>
<run_depend>cmake_modules</run_depend>
</package>

291
robotis_math/src/RobotisLinearAlgebra.cpp Normal file
View File

@ -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);
}
}

26
robotis_math/src/RobotisMathBase.cpp Normal file
View File

@ -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;
}
}

325
robotis_math/src/RobotisTrajectoryCalculator.cpp Normal file
View File

@ -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<double,3,1> _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;
}
}