From 183c6cd62538abe4945aebdb2bcdccbbeb98888a Mon Sep 17 00:00:00 2001 From: Erik Andresen Date: Fri, 27 Dec 2019 22:43:24 +0100 Subject: [PATCH 1/1] Initial Commit --- CMakeLists.txt | 199 +++++++++++++++++++++++++++++++ config/control.yaml | 11 ++ include/open_manipulator_i2c.h | 194 ++++++++++++++++++++++++++++++ launch/open_manipulator.launch | 10 ++ package.xml | 63 ++++++++++ src/wt_open_manipulator_node.cpp | 32 +++++ 6 files changed, 509 insertions(+) create mode 100644 CMakeLists.txt create mode 100644 config/control.yaml create mode 100644 include/open_manipulator_i2c.h create mode 100644 launch/open_manipulator.launch create mode 100644 package.xml create mode 100644 src/wt_open_manipulator_node.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..0d17567 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,199 @@ +cmake_minimum_required(VERSION 2.8.3) +project(wt_open_manipulator) + +## Compile as C++11, supported in ROS Kinetic and newer +# add_compile_options(-std=c++11) + +## 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 + joint_trajectory_controller + controller_manager +) + +## 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 exec_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 exec_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 exec_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your 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 wt_open_manipulator +# CATKIN_DEPENDS other_catkin_pkg +# 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} +) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/wt_open_manipulator.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(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +add_executable(${PROJECT_NAME}_node src/wt_open_manipulator_node.cpp) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +target_link_libraries(${PROJECT_NAME}_node + ${catkin_LIBRARIES} +) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# install(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables and/or libraries for installation +# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_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_wt_open_manipulator.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/config/control.yaml b/config/control.yaml new file mode 100644 index 0000000..4aeee8c --- /dev/null +++ b/config/control.yaml @@ -0,0 +1,11 @@ +arm_joint_state_controller: + type: "joint_state_controller/JointStateController" + publish_rate: 20 + +arm_controller: + type: "position_controllers/JointTrajectoryController" + joints: + - joint1 + - joint2 + - joint3 + - joint4 diff --git a/include/open_manipulator_i2c.h b/include/open_manipulator_i2c.h new file mode 100644 index 0000000..80bfded --- /dev/null +++ b/include/open_manipulator_i2c.h @@ -0,0 +1,194 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "std_srvs/SetBool.h" + +/* http://wiki.ros.org/joint_trajectory_controller?distro=melodic + * + * arm: + * - Gripper Goal Current(102)=200, Operating Mode(11)=Current-based Position Control Mode, Profile_Acceleration=20, Profile_Velocity=200 + * - Read: Present Current(126), Present Velocity(128), Present Position(132) + * - Gripper joint limits: + * 0.010, // max gripper limit (0.01 m) + * -0.010, // min gripper limit (-0.01 m) + * -0.015, // Change unit from `meter` to `radian` + */ + +#define I2C_FILE "/dev/i2c-2" +#define I2C_ADDR (0x60>>1) +#define XM_TORQUE_ENABLE 64 +#define XM_GOAL_POSITION 116 +#define XM_PRESENT_POSITION 132 +#define CMD_EOF 0xaa + +class WtOpenManipulatorI2C : public hardware_interface::RobotHW +{ + public: + WtOpenManipulatorI2C(ros::NodeHandle nh) { + this->nh = nh; + torque_enable_service = nh.advertiseService("torque_enable", &WtOpenManipulatorI2C::cbTorqueEnable, this); + + // connect and register the joint state interface + hardware_interface::JointStateHandle state_handle_1("joint1", &pos[0], &vel[0], &eff[0]); + jnt_state_interface.registerHandle(state_handle_1); + hardware_interface::JointStateHandle state_handle_2("joint2", &pos[1], &vel[1], &eff[1]); + jnt_state_interface.registerHandle(state_handle_2); + hardware_interface::JointStateHandle state_handle_3("joint3", &pos[2], &vel[2], &eff[2]); + jnt_state_interface.registerHandle(state_handle_3); + hardware_interface::JointStateHandle state_handle_4("joint4", &pos[3], &vel[3], &eff[3]); + jnt_state_interface.registerHandle(state_handle_4); + registerInterface(&jnt_state_interface); + + // connect and register the joint position interface + hardware_interface::JointHandle pos_handle_1(state_handle_1, &cmd[0]); + jnt_pos_interface.registerHandle(pos_handle_1); + hardware_interface::JointHandle pos_handle_2(state_handle_2, &cmd[1]); + jnt_pos_interface.registerHandle(pos_handle_2); + hardware_interface::JointHandle pos_handle_3(state_handle_3, &cmd[2]); + jnt_pos_interface.registerHandle(pos_handle_3); + hardware_interface::JointHandle pos_handle_4(state_handle_4, &cmd[3]); + jnt_pos_interface.registerHandle(pos_handle_4); + registerInterface(&jnt_pos_interface); + + torque_enable(0x1); + } + + // Converts pos_encoder from wheels to wheel angles + void read(ros::Duration period) { + for(int i=0, id=11; id<=14; id++, i++) { + int32_t value; + if (dynamixel_readdword(id, XM_PRESENT_POSITION, &value) != 1) { + ROS_ERROR("I2C dynamixel id=%d read error", id); + return; + } + pos[i] = 0.088*M_PI/180*(value-2048); + } + ROS_DEBUG("pos: %.2f %.2f %.2f %.2f", pos[0]*180/M_PI, pos[1]*180/M_PI, pos[2]*180/M_PI, pos[3]*180/M_PI); + } + + // Writes current velocity command to hardware + void write() { + ROS_DEBUG("cmd: %.2f %.2f %.2f %.2f", cmd[0]*180/M_PI, cmd[1]*180/M_PI, cmd[2]*180/M_PI, cmd[3]*180/M_PI); + + for(int i=0, id=11; id<=14; id++, i++) { + if (isnan(cmd[i])) { + ROS_WARN("Desired angle for id=%d is NaN, skipping write.", id); + continue; + } + + int32_t value = cmd[i]/(0.088*M_PI/180) + 2048; + + if (dynamixel_writedword(id, XM_GOAL_POSITION, value) != 1) { + ROS_ERROR("I2C dynamixel id=%d write error", id); + } + } + } + + private: + hardware_interface::JointStateInterface jnt_state_interface; + hardware_interface::PositionJointInterface jnt_pos_interface; + ros::NodeHandle nh; + double cmd[4] = {NAN, NAN, NAN, NAN}; + double pos[4] = {0, 0, 0, 0}; + double vel[4] = {0, 0, 0, 0}; + double eff[4] = {0, 0, 0, 0}; + ros::ServiceServer torque_enable_service; + std::mutex comm_mutex; + + int dynamixel_write(uint8_t *write_data, int write_num, uint8_t *read_data, int read_num) { + int file; + uint8_t buf[32]; + int ret; + uint8_t read_buf[read_num+1]; // for unknown reason need to read one more byte with USI Slave + + comm_mutex.lock(); + if ((file = open(I2C_FILE, O_RDWR)) < 0) { + perror("open"); + goto error; + } + + if (ioctl(file, I2C_SLAVE, I2C_ADDR) < 0) { + perror("ioctl"); + goto error; + } + + if ((ret = i2c_smbus_write_i2c_block_data(file, 0x0, write_num, write_data)) != 0) { + perror("i2c_smbus_write_block_data"); + goto error; + } + + for(int i=0; i<10; i++) { + usleep(10*1000); + + if ((ret = i2c_smbus_read_i2c_block_data(file, 0x0, read_num+1, read_buf)) != read_num+1) { + perror("i2c_smbus_write_block_data"); + goto error; + } + if (read_data[0] == 1) { + break; + } + } + ret--; // remove extra byte + memcpy(read_data, read_buf, ret); + + error: + close(file); + comm_mutex.unlock(); + return ret; + } + + uint8_t dynamixel_writebyte(uint8_t id, uint8_t cmd, uint8_t value) { + uint8_t write_data[5] = {id, cmd, 1, value, CMD_EOF}; + uint8_t read_data[2]; + int ret = dynamixel_write(write_data, 5, read_data, 2); + if (ret == 2 && read_data[1] == id) { + return read_data[0]; + } + return 255; + } + + uint8_t dynamixel_writedword(uint8_t id, uint8_t cmd, int32_t value) { + uint8_t write_data[8] = {id, cmd, 4, (uint8_t)(value>>24), (uint8_t)(value>>16), (uint8_t)(value>>8), (uint8_t)value, CMD_EOF}; + uint8_t read_data[2]; + int ret = dynamixel_write(write_data, 8, read_data, 2); + if (ret == 2 && read_data[1] == id) { + return read_data[0]; + } + return 255; + } + + uint8_t dynamixel_readdword(uint8_t id, uint8_t cmd, int32_t *value) { + uint8_t write_data[4] = {id, cmd, 0x14, CMD_EOF}; + uint8_t read_data[6]; + int ret = dynamixel_write(write_data, 4, read_data, 6); + if (ret == 6 && read_data[1] == id) { + *value = (read_data[2]<<24) | (read_data[3]<<16) | (read_data[4]<<8) | read_data[5]; + return read_data[0]; + } + return 255; + } + + bool torque_enable(uint8_t enable) { + bool ret = true; + for(int id=11; id<=14; id++) { + if (dynamixel_writebyte(id, XM_TORQUE_ENABLE, enable) != 1) { + ROS_ERROR("I2C dynamixel id=%d torque change", id); + ret = false; + } + } + return ret; + } + + bool cbTorqueEnable(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res) { + ROS_INFO("Setting torque enable=%d", req.data); + res.success = torque_enable(req.data); + return true; + } +}; diff --git a/launch/open_manipulator.launch b/launch/open_manipulator.launch new file mode 100644 index 0000000..dcb2e7b --- /dev/null +++ b/launch/open_manipulator.launch @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..bb8c8a4 --- /dev/null +++ b/package.xml @@ -0,0 +1,63 @@ + + + wt_open_manipulator + 0.0.0 + The wt_open_manipulator package + + + + + erik + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + controller_manager + joint_trajectory_controller + libi2c-dev + controller_manager + open_manipulator_description + + + + + + + diff --git a/src/wt_open_manipulator_node.cpp b/src/wt_open_manipulator_node.cpp new file mode 100644 index 0000000..b20f13c --- /dev/null +++ b/src/wt_open_manipulator_node.cpp @@ -0,0 +1,32 @@ +#include "ros/ros.h" +#include "open_manipulator_i2c.h" +#include "controller_manager/controller_manager.h" + +int main(int argc, char **argv) { + ros::init(argc, argv, "wt_open_manipulator_i2c"); + + ros::NodeHandle n; + + WtOpenManipulatorI2C robot(n); + controller_manager::ControllerManager cm(&robot); + + ros::AsyncSpinner spinner(1); + spinner.start(); + + ros::Time prev_time = ros::Time::now(); + ros::Rate loop_rate(10); + while(ros::ok()) { + const ros::Time time = ros::Time::now(); + const ros::Duration period = time - prev_time; + + robot.read(period); + cm.update(time, period); + robot.write(); + + prev_time = time; + loop_rate.sleep(); + } + spinner.stop(); + + return 0; +} -- 2.39.5