--- /dev/null
+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)
--- /dev/null
+arm_joint_state_controller:
+ type: "joint_state_controller/JointStateController"
+ publish_rate: 20
+
+arm_controller:
+ type: "position_controllers/JointTrajectoryController"
+ joints:
+ - joint1
+ - joint2
+ - joint3
+ - joint4
--- /dev/null
+#include <unistd.h>
+#include <sys/types.h>
+#include <sys/ioctl.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <mutex>
+#include <linux/i2c-dev.h>
+#include <hardware_interface/joint_command_interface.h>
+#include <hardware_interface/joint_state_interface.h>
+#include <hardware_interface/robot_hw.h>
+#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;
+ }
+};
--- /dev/null
+<?xml version="1.0"?>
+<launch>
+ <!-- Load controller configurations from YAML file to parameter server -->
+ <rosparam file="$(find wt_open_manipulator)/config/control.yaml" command="load" />
+
+ <node pkg="wt_open_manipulator" type="wt_open_manipulator_node" name="wt_open_manipulator" output="screen" required="true"/>
+
+ <!-- load the controllers -->
+ <node name="arm_controller_spawner" pkg="controller_manager" type="spawner" output="screen" args="arm_joint_state_controller arm_controller"/>
+</launch>
--- /dev/null
+<?xml version="1.0"?>
+<package format="2">
+ <name>wt_open_manipulator</name>
+ <version>0.0.0</version>
+ <description>The wt_open_manipulator package</description>
+
+ <!-- One maintainer tag required, multiple allowed, one person per tag -->
+ <!-- Example: -->
+ <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+ <maintainer email="erik@vontaene.de">erik</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 multiple are allowed, one per tag -->
+ <!-- Optional attribute type can be: website, bugtracker, or repository -->
+ <!-- Example: -->
+ <!-- <url type="website">http://wiki.ros.org/wt_open_manipulator</url> -->
+
+
+ <!-- Author tags are optional, multiple are allowed, one per tag -->
+ <!-- Authors do not have to be maintainers, but could be -->
+ <!-- Example: -->
+ <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+ <!-- The *depend tags are used to specify dependencies -->
+ <!-- Dependencies can be catkin packages or system dependencies -->
+ <!-- Examples: -->
+ <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+ <!-- <depend>roscpp</depend> -->
+ <!-- Note that this is equivalent to the following: -->
+ <!-- <build_depend>roscpp</build_depend> -->
+ <!-- <exec_depend>roscpp</exec_depend> -->
+ <!-- Use build_depend for packages you need at compile time: -->
+ <!-- <build_depend>message_generation</build_depend> -->
+ <!-- Use build_export_depend for packages you need in order to build against this package: -->
+ <!-- <build_export_depend>message_generation</build_export_depend> -->
+ <!-- Use buildtool_depend for build tool packages: -->
+ <!-- <buildtool_depend>catkin</buildtool_depend> -->
+ <!-- Use exec_depend for packages you need at runtime: -->
+ <!-- <exec_depend>message_runtime</exec_depend> -->
+ <!-- Use test_depend for packages you need only for testing: -->
+ <!-- <test_depend>gtest</test_depend> -->
+ <!-- Use doc_depend for packages you need only for building documentation: -->
+ <!-- <doc_depend>doxygen</doc_depend> -->
+ <buildtool_depend>catkin</buildtool_depend>
+ <build_depend>controller_manager</build_depend>
+ <build_depend>joint_trajectory_controller</build_depend>
+ <build_depend>libi2c-dev</build_depend>
+ <exec_depend>controller_manager</exec_depend>
+ <exec_depend>open_manipulator_description</exec_depend>
+
+ <!-- The export tag contains other, unspecified, tags -->
+ <export>
+ <!-- Other tools can request additional information be placed here -->
+
+ </export>
+</package>
--- /dev/null
+#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;
+}
projects / wt_open_manipulator.git / commitdiff