diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 0000000..217f765 --- /dev/null +++ b/.gitmodules @@ -0,0 +1,3 @@ +[submodule "DynamixelSDK"] + path = DynamixelSDK + url = git@github.com:ROBOTIS-GIT/DynamixelSDK.git diff --git a/Contributors/README.md b/Contributors/README.md index e87d0a8..f114a32 100644 --- a/Contributors/README.md +++ b/Contributors/README.md @@ -1,4 +1,20 @@ ## File system format: -Each of the subsystems has its own subdirectory, please name these folders in snake case. -All the files in the subdirectories are to be named in camel case. +Each of the subsystems has its own subdirectory, please name these folders in snake_case. +All the files in the subdirectories are to be named in lowerCamelCase. +ROS message and service definitions are to be named in UpperCamelCase. + + +## Contributing: +No one is allowed to push to the master branch. All the changes must be made in a separate branch and then a pull request must be created to the master branch. The pull request will be reviewed by the maintainers and then merged to the master branch. + +Create a branch with the name of the subsystem you are working on, if you are the only contributor working on that issue, use the format users//. If you are working on an issue with multiple contributors, use the format issues//. If you are working on a subsystem that is not an issue, use the format features/. + +When you are done with your work, create a pull request to the master branch. If you are working on an issue, please mention the issue number in the pull request description. If you are working on a feature, please mention the feature name in the pull request description. If you are working on a subsystem that is not an issue, please mention the subsystem name in the pull request description. Before merging, make sure that the current branch is up to date with the master branch. + +## Code style: + +To maintain a consistent code style, we use linters. The linters are configured to run automatically on every pull request. If the linters fail, the PR will not be allowed to merge. If you want to run the linters manually, run the following command in the root directory of the project: + +Please follow the [PEP 8](https://www.python.org/dev/peps/pep-0008/) style guide for python code. +Please follow for the [Google C++ Style Guide](https://google.github.io/styleguide/cppguide.html) for C++ code. diff --git a/DynamixelSDK b/DynamixelSDK new file mode 160000 index 0000000..3450c70 --- /dev/null +++ b/DynamixelSDK @@ -0,0 +1 @@ +Subproject commit 3450c7078917b262d9b36042c15444047aae226e diff --git a/README.md b/README.md index e776a66..5704d8e 100644 --- a/README.md +++ b/README.md @@ -1,12 +1,6 @@ # Flo-System-2 This is the master code repository for the second iteration of the Flo system. -It contains code used for the control of the humanoid robot, the control of the mobile telepresence platform and the autonomous navigation stack. -It contains repositories for the following components: -1. Control of the joints of second version of the flo humanoid -2. Control of the ohmni telepresence base -3. Camera and sensor data collection -4. Central systems to coordinate the above components ## License: MIT License. @@ -19,6 +13,22 @@ The above copyright notice and this permission notice shall be included in all c THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. +## Overview: + +This repository contains the following components: + +1. Control system of the humanoid robot (Dynamixel motors: 2 XL430-W250-T, 1 XC430-W240-T, 1 XM430-W350-T and 1 RX-64 motor per arm) +2. Control system of the mobile telepresence platform (Ohmni telepresence platform) +3. RGB-D cameras (2 Luxonis Oak-D cameras) +4. 2 dimensional lidar sensor (rp lidar a1) +5. object localization system (April tag 3) +6. Centeral user interface (web interface?) + +The system is designed to be modular and each component can be run independently, ROS is used as the communication framework between the components. + +Each component will be running as a seperate process in a docker container and interfacing with the ros master remotely. + + ## Connecting to the Ohmni telepresence robot and running docker 1. Clone this repository using git clone https://github.com/Rehab-Robotics-Lab/Flo-System-2.git 2. Download the private ssh key from penn box https://upenn.app.box.com/folder/206840565719; contact @anht-nguyen for access if necessary. diff --git a/compose.yaml b/compose.yaml new file mode 100644 index 0000000..507ca14 --- /dev/null +++ b/compose.yaml @@ -0,0 +1,9 @@ +services: + flo_humanoid: + build: /flo_humanoid + ports: + ohmni_base: + build: /ohmni_base + ports: + + \ No newline at end of file diff --git a/flo_humanoid/CMakeLists.txt b/flo_humanoid/CMakeLists.txt index a80ada7..9013807 100644 --- a/flo_humanoid/CMakeLists.txt +++ b/flo_humanoid/CMakeLists.txt @@ -22,20 +22,20 @@ find_package(catkin REQUIRED # Declare ROS messages, services and actions ################################################################################ ## Generate messages in the 'msg' folder -add_message_files( - FILES - SetPosition.msg - SyncSetPosition.msg - BulkSetItem.msg -) +# add_message_files( +# FILES +# SetPosition.msg +# SyncSetPosition.msg +# BulkSetItem.msg +# ) ## Generate services in the 'srv' folder -add_service_files( - FILES - GetPosition.srv - SyncGetPosition.srv - BulkGetItem.srv -) +# add_service_files( +# FILES +# GetPosition.srv +# SyncGetPosition.srv +# BulkGetItem.srv +# ) ## Generate added messages and services with any dependencies listed here generate_messages( @@ -63,27 +63,19 @@ include_directories( ${catkin_INCLUDE_DIRS} ) -add_executable(read_write_node src/read_write_node.cpp) -add_dependencies(read_write_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -target_link_libraries(read_write_node ${catkin_LIBRARIES}) - -add_executable(sync_read_write_node src/sync_read_write_node.cpp) -add_dependencies(sync_read_write_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -target_link_libraries(sync_read_write_node ${catkin_LIBRARIES}) - -add_executable(bulk_read_write_node src/bulk_read_write_node.cpp) -add_dependencies(bulk_read_write_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -target_link_libraries(bulk_read_write_node ${catkin_LIBRARIES}) +add_executable(readWriteArm src/readWriteArm.cpp) +add_dependencies(readWriteArm ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(readWriteArm ${catkin_LIBRARIES}) -add_executable(indirect_address_node src/indirect_address_node.cpp) -add_dependencies(indirect_address_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -target_link_libraries(indirect_address_node ${catkin_LIBRARIES}) +add_executable(readWriteJoint src/readWriteJoint.cpp) +add_dependencies(readWriteJoint ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(readWriteJoint ${catkin_LIBRARIES}) ################################################################################ # Install ################################################################################ ## Mark executables for installation ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html -install(TARGETS read_write_node sync_read_write_node bulk_read_write_node indirect_address_node +install(TARGETS readWriteArm readWriteJoint RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) diff --git a/flo_humanoid/Dockerfile b/flo_humanoid/Dockerfile index 2ca1fb9..f9c309c 100644 --- a/flo_humanoid/Dockerfile +++ b/flo_humanoid/Dockerfile @@ -1,44 +1,37 @@ -#base image from ohmnilabsvn/ohmni_ros:ohmni_ros_tbcontrol_0.0.13, preinstalled with ros-melodic + tb control ros packages -FROM ohmnilabsvn/ohmni_ros:ohmni_ros_tbcontrol_0.0.13 +FROM ros:melodic-robot-bionic #make super user RUN su +ARG ROS_DISTRO=melodic +ENV ROS_DISTRO="$ROS_DISTRO" #install essential tools -RUN apt-get update -y && sudo apt-get -y install \ +RUN apt-get update -y && apt-get install -y \ neovim \ tmux \ -git -# clear cache -RUN apt-get clean -#switch to ohmnidev directory, used for placing code. -WORKDIR /home/ohmnidev -RUN git clone https://github.com/Rehab-Robotics-Lab/FloSystemV2.git -WORKDIR /home/ohmnidev/FloSystemV2 -RUN git pull -WORKDIR /home/ohmnidev -RUN mkdir -p catkin_ws -WORKDIR /home/ohmnidev/catkin_ws -ARG ROS_DISTRO=melodic -ENV ROS_DISTRO="$ROS_DISTRO" -RUN apt-get update && apt-get install -y ros-$ROS_DISTRO-catkin \ -#install usb utilities to get info on connected devices - might not be as useful. comment next line if not needed. -#usbutils\ -#The following 2 lines download libraries for control of arduino via ros -#ros-$ROS_DISTRO-rosserial \ -#ros-$ROS_DISTRO-rosserial-arduino\ -#THE following line downloads the dynamixel sdk for ros. -ros-melodic-dynamixel-sdk\ -#The following line downloads the dynamixel sdk examples for ros. -ros-melodic-dynamixel-sdk-examples - +git \ +ros-$ROS_DISTRO-catkin \ +ros-$ROS_DISTRO-dynamixel-sdk\ +ros-$ROS_DISTRO-dynamixel-sdk-examples #Lidar libraries #RUN git clone https://github.com/Slamtec/rplidar_ros.git #Build the catkin_ws -#RUN catkin_make -j1 -l1 +# clear cache +RUN apt-get clean + +RUN mkdir -p home/git +WORKDIR /home/git +RUN git clone https://github.com/Rehab-Robotics-Lab/FloSystemV2.git +WORKDIR /home/git +RUN mkdir -p catkin_ws +RUN cp -r /FloSystemV2/flo_humanoid /catkin_ws/src catkin_ws/flo_humanoid +RUN cp -r /FloSystemV2/flo_humanoid_defs /catkin_ws/src catkin_ws/flo_humanoid_defs +WORKDIR /home/git/catkin_ws +RUN catkin_make -j1 -l1 +ENTRYPOINT ["./ros_entrypoint.sh"] +CMD ["bash"] -#add as neccessary. diff --git a/flo_humanoid/config/joints b/flo_humanoid/config/joints index 5803ca4..b010efd 100644 --- a/flo_humanoid/config/joints +++ b/flo_humanoid/config/joints @@ -1,13 +1,15 @@ #fill this table with the joints you want to control in the humanoid robot and their configurations. -<<<<<<< HEAD -#address refers to the motor address set in the dynamixel wizard -address name neutral min max inversion -#eg: -# 1 left_shoulder_flexionextension 765 10 500 1 -======= (dynamixel motor address and joint name for each motor) ->>>>>>> 6a1935e1108d90aad7556325ae2f6b80f855f72c -address name neutral min max inversion +address name type neutral min max inversion #eg: -# 1 left_shoulder_flexionextension 765 10 500 1 - +# 1 left_shoulder_flexionextension 765 10 500 1 +10 placeholder XC-430-W240-T 0 0 0 0 +11 placeholder XL-430-W250-T 0 0 0 0 +12 placeholder XL-430-W250-T 0 0 0 0 +13 placeholder XM-430-W350-T 0 0 0 0 +14 placeholder MX-64 0 0 0 0 +20 placeholder XC-430-W240-T 0 0 0 0 +21 placeholder XL-430-W250-T 0 0 0 0 +22 placeholder XL-430-W250-T 0 0 0 0 +23 placeholder XM-430-W350-T 0 0 0 0 +24 placeholder MX-64 0 0 0 0 diff --git a/flo_humanoid/package.xml b/flo_humanoid/package.xml new file mode 100644 index 0000000..b08c208 --- /dev/null +++ b/flo_humanoid/package.xml @@ -0,0 +1,19 @@ + + + flo_humanoid_defs + 0.0.0 + Contains code to pass commands to and read position information from the dynamixel motors used in the flo v2 humanoid + Ajay Anand + proprietery + catkin + message_generation + std_msgs + actionlib_msgs + std_msgs + actionlib_msgs + message_runtime + flo_humanoid_defs + + + + diff --git a/flo_humanoid/src/readWriteArm.cpp b/flo_humanoid/src/readWriteArm.cpp new file mode 100644 index 0000000..f99a91f --- /dev/null +++ b/flo_humanoid/src/readWriteArm.cpp @@ -0,0 +1,374 @@ +// This code is based on the bulk_read_write_node.cpp from the dynamixel_sdk_examples package; it has been expanded to control the 5 motors of a robot arm present in the FLO v2 humanoid robot. +// system() call can be used to run shell commands from within a C++ program. + +#include + +#include "std_msgs/String.h" +#include "dynamixel_sdk_examples/BulkGetItem.h" +#include "dynamixel_sdk_examples/BulkSetItem.h" +#include "dynamixel_sdk/dynamixel_sdk.h" + +using namespace dynamixel; + +// Control table address +#define ADDR_TORQUE_ENABLE 64 +#define ADDR_PRESENT_LED 65 +#define ADDR_PRESENT_POSITION 132 +#define ADDR_GOAL_POSITION 116 + +// Protocol version +#define PROTOCOL_VERSION 2.0 // Default Protocol version of DYNAMIXEL X series. + +// Default setting +// Modify the values below to fit the motor Id's assigned in the dynamixel wizard. +#define DXL1_ID 10 // DXL1 ID +#define DXL2_ID 11 // DXL2 ID +#define DXL3_ID 12 // DXL3 ID +#define DXL4_ID 13 // DXL4 ID +#define DXL5_ID 14 // DXL5 ID +#define BAUDRATE 57600 // Default Baudrate of DYNAMIXEL X series +//set up fixed mount point for the device, this is the same as the one set in the udev rules file. +#define DEVICE_NAME "/dev/ttyUSB0" // [Linux] To find assigned port, use "$ ls /dev/ttyUSB*" command + + +PortHandler * portHandler = PortHandler::getPortHandler(DEVICE_NAME); +PacketHandler * packetHandler = PacketHandler::getPacketHandler(PROTOCOL_VERSION); + +GroupBulkRead groupBulkRead(portHandler, packetHandler); +GroupBulkWrite groupBulkWrite(portHandler, packetHandler); +//This function was fully modified to work with the 5 motors of the robot arm. +// based on dynamixelSDK issue #196, it not possible to set multiple parameters for the same motor in a single groupBulkWrite() command. +bool getJointPositionsCallback( + flo_humanoid_defs::GetJointPositions::Request & req, + flo_humanoid_defs::GetJointPositions::Response & res) +{ + uint8_t dxl_error = 0; + int dxl_comm_result = COMM_TX_FAIL; + int dxl_addparam_result = false; + + // Position Value of X series is 4 byte data. For AX & MX(1.0) use 2 byte data(int16_t) for the Position Value. + int32_t position1 = 0; + int32_t position2 = 0; + + // Read Present Position (length : 4 bytes) and Convert uint32 -> int32 + // When reading 2 byte data from AX / MX(1.0), use read2ByteTxRx() instead. + if (req.item1 == "position") { + dxl_addparam_result = groupBulkRead.addParam((uint8_t)req.id1, ADDR_PRESENT_POSITION, 4); + } else if (req.item1 == "LED") { + dxl_addparam_result = groupBulkRead.addParam((uint8_t)req.id1, ADDR_PRESENT_LED, 1); + } + if (dxl_addparam_result != true) { + ROS_ERROR("Failed to addparam to groupBulkRead for Dynamixel ID: %d", req.id1); + return 0; + } + + if (req.item2 == "position") { + dxl_addparam_result = groupBulkRead.addParam((uint8_t)req.id2, ADDR_PRESENT_POSITION, 4); + } else if (req.item2 == "LED") { + dxl_addparam_result = groupBulkRead.addParam((uint8_t)req.id2, ADDR_PRESENT_LED, 1); + } + if (dxl_addparam_result != true) { + ROS_ERROR("Failed to addparam to groupBulkRead for Dynamixel ID %d", req.id2); + return 0; + } + + if (req.item3 == "position") { + dxl_addparam_result = groupBulkRead.addParam((uint8_t)req.id3, ADDR_PRESENT_POSITION, 4); + } else if (req.item3 == "LED") { + dxl_addparam_result = groupBulkRead.addParam((uint8_t)req.id3, ADDR_PRESENT_LED, 1); + } + if (dxl_addparam_result != true) { + ROS_ERROR("Failed to addparam to groupBulkRead for Dynamixel ID %d", req.id3); + return 0; + } + + if (req.item4 == "position") { + dxl_addparam_result = groupBulkRead.addParam((uint8_t)req.id4, ADDR_PRESENT_POSITION, 4); + } else if (req.item4 == "LED") { + dxl_addparam_result = groupBulkRead.addParam((uint8_t)req.id4, ADDR_PRESENT_LED, 1); + } + if (dxl_addparam_result != true) { + ROS_ERROR("Failed to addparam to groupBulkRead for Dynamixel ID %d", req.id4); + return 0; + } + + if (req.item5 == "position") { + dxl_addparam_result = groupBulkRead.addParam((uint8_t)req.id5, ADDR_PRESENT_POSITION, 4); + } else if (req.item5 == "LED") { + dxl_addparam_result = groupBulkRead.addParam((uint8_t)req.id5, ADDR_PRESENT_LED, 1); + } + if (dxl_addparam_result != true) { + ROS_ERROR("Failed to addparam to groupBulkRead for Dynamixel ID %d", req.id5); + return 0; + } + + uint32_t value1 = 0; + uint32_t value2 = 0; + dxl_comm_result = groupBulkRead.txRxPacket(); + + + + if (dxl_comm_result == COMM_SUCCESS) { + //having some issues understanding why the if statements below are necessary, test without the if statements to check if they are necessary. + if (req.item1 == "position") { + value1 = groupBulkRead.getData((uint8_t)req.id1, ADDR_PRESENT_POSITION, 4); + } else if (req.item2 == "LED") { + value1 = groupBulkRead.getData((uint8_t)req.id1, ADDR_PRESENT_POSITION, 4); + } + + if (req.item1 == "position") { + value2 = groupBulkRead.getData((uint8_t)req.id2, ADDR_PRESENT_POSITION, 4); + } else if (req.item2 == "LED") { + value2 = groupBulkRead.getData((uint8_t)req.id2, ADDR_PRESENT_POSITION, 4); + } + + if (req.item1 == "position") { + value2 = groupBulkRead.getData((uint8_t)req.id3, ADDR_PRESENT_POSITION, 4); + } else if (req.item2 == "LED") { + value2 = groupBulkRead.getData((uint8_t)req.id3, ADDR_PRESENT_POSITION, 4); + } + + if (req.item1 == "position") { + value2 = groupBulkRead.getData((uint8_t)req.id4, ADDR_PRESENT_POSITION, 4); + } else if (req.item2 == "LED") { + value2 = groupBulkRead.getData((uint8_t)req.id4, ADDR_PRESENT_POSITION, 4); + } + + if (req.item1 == "position") { + value2 = groupBulkRead.getData((uint8_t)req.id5, ADDR_PRESENT_POSITION, 4); + } else if (req.item2 == "LED") { + value2 = groupBulkRead.getData((uint8_t)req.id5, ADDR_PRESENT_POSITION, 4); + } + + ROS_INFO("getItem : [ID:%d] [%s: %d]", req.id1, req.item1.c_str(), value1); + ROS_INFO("getItem : [ID:%d] [%s: %d]", req.id2, req.item2.c_str(), value2); + ROS_INFO("getItem : [ID:%d] [%s: %d]", req.id3, req.item3.c_str(), value3); + ROS_INFO("getItem : [ID:%d] [%s: %d]", req.id4, req.item4.c_str(), value4); + ROS_INFO("getItem : [ID:%d] [%s: %d]", req.id5, req.item5.c_str(), value5); + res.value1 = value1; + res.value2 = value2; + res.value3 = value3; + res.value4 = value4; + res.value5 = value5; + groupBulkRead.clearParam(); + return true; + } else { + ROS_ERROR("Failed to get position! Result: %d", dxl_comm_result); + groupBulkRead.clearParam(); + return false; + } +} + +void setJointPositionsCallback(const flo_humanoid_defs::SetJointPositions::ConstPtr & msg) +{ + uint8_t dxl_error = 0; + int dxl_comm_result = COMM_TX_FAIL; + int dxl_addparam_result = false; + // uint8_t param_goal_position1[4]; + // uint8_t param_goal_position2[4]; + // uint8_t param_goal_led1[1]; + // uint8_t param_goal_led2[1]; + // uint8_t addr_goal_item[2]; + // uint8_t len_goal_item[2]; + uint8_t param_goal_position[5][4]; + uint8_t param_goal_led[5][1]; + uint8_t addr_goal_item[5]; + uint8_t len_goal_item[5]; + + // Position Value of X series is 4 byte data. For AX & MX(1.0) use 2 byte data(uint16_t) for the Position Value. + if (msg->item1 == "position") { + uint32_t position1 = (unsigned int)msg->value1; // Convert int32 -> uint32 + param_goal_position[0][0] = DXL_LOBYTE(DXL_LOWORD(position1)); + param_goal_position[0][1] = DXL_HIBYTE(DXL_LOWORD(position1)); + param_goal_position[0][2] = DXL_LOBYTE(DXL_HIWORD(position1)); + param_goal_position[0][3] = DXL_HIBYTE(DXL_HIWORD(position1)); + addr_goal_item[0] = ADDR_GOAL_POSITION; + len_goal_item[0] = 4; + } else if (msg->item1 == "LED") { + uint32_t led1 = (unsigned int)msg->value1; // Convert int32 -> uint32 + param_goal_led[0][0] = led1; + addr_goal_item[0] = ADDR_PRESENT_LED; + len_goal_item[0] = 1; + } + + if (msg->item2 == "position") { + uint32_t position2 = (unsigned int)msg->value2; // Convert int32 -> uint32 + param_goal_position[1][0] = DXL_LOBYTE(DXL_LOWORD(position2)); + param_goal_position[1][1] = DXL_HIBYTE(DXL_LOWORD(position2)); + param_goal_position[1][2] = DXL_LOBYTE(DXL_HIWORD(position2)); + param_goal_position[1][3] = DXL_HIBYTE(DXL_HIWORD(position2)); + addr_goal_item[1] = ADDR_GOAL_POSITION; + len_goal_item[1] = 4; + } else if (msg->item2 == "LED") { + uint32_t led2 = (unsigned int)msg->value2; // Convert int32 -> uint32 + param_goal_led[1][0] = led2; + addr_goal_item[1] = ADDR_PRESENT_LED; + len_goal_item[1] = 1; + } + + if (msg->item3 == "position") { + uint32_t position3 = (unsigned int)msg->value3; // Convert int32 -> uint32 + param_goal_position[2][0] = DXL_LOBYTE(DXL_LOWORD(position3)); + param_goal_position[2][1] = DXL_HIBYTE(DXL_LOWORD(position3)); + param_goal_position[2][2] = DXL_LOBYTE(DXL_HIWORD(position3)); + param_goal_position[2][3] = DXL_HIBYTE(DXL_HIWORD(position3)); + addr_goal_item[2] = ADDR_GOAL_POSITION; + len_goal_item[2] = 4; + } else if (msg->item3 == "LED") { + uint32_t led3 = (unsigned int)msg->value3; // Convert int32 -> uint32 + param_goal_led[2][0] = led3; + addr_goal_item[2] = ADDR_PRESENT_LED; + len_goal_item[2] = 1; + } + + if (msg->item4 == "position") { + uint32_t position4 = (unsigned int)msg->value4; // Convert int32 -> uint32 + param_goal_position[3][0] = DXL_LOBYTE(DXL_LOWORD(position4)); + param_goal_position[3][1] = DXL_HIBYTE(DXL_LOWORD(position4)); + param_goal_position[3][2] = DXL_LOBYTE(DXL_HIWORD(position4)); + param_goal_position[3][3] = DXL_HIBYTE(DXL_HIWORD(position4)); + addr_goal_item[3] = ADDR_GOAL_POSITION; + len_goal_item[3] = 4; + } else if (msg->item4 == "LED") { + uint32_t led4 = (unsigned int)msg->value4; // Convert int32 -> uint32 + param_goal_led[3][0] = led4; + addr_goal_item[3] = ADDR_PRESENT_LED; + len_goal_item[3] = 1; + } + + if (msg->item5 == "position") { + uint32_t position5 = (unsigned int)msg->value5; // Convert int32 -> uint32 + param_goal_position[4][0] = DXL_LOBYTE(DXL_LOWORD(position5)); + param_goal_position[4][1] = DXL_HIBYTE(DXL_LOWORD(position5)); + param_goal_position[4][2] = DXL_LOBYTE(DXL_HIWORD(position5)); + param_goal_position[4][3] = DXL_HIBYTE(DXL_HIWORD(position5)); + addr_goal_item[4] = ADDR_GOAL_POSITION; + len_goal_item[4] = 4; + } else if (msg->item5 == "LED") { + uint32_t led5 = (unsigned int)msg->value5; // Convert int32 -> uint32 + param_goal_led[4][0] = led5; + addr_goal_item[4] = ADDR_PRESENT_LED; + len_goal_item[4] = 1; + } + + // Write Goal Position (length : 4 bytes) + // When writing 2 byte data to AX / MX(1.0), use write2ByteTxRx() instead. + if (msg->item1 == "position") { + dxl_addparam_result = groupBulkWrite.addParam((uint8_t)msg->id1, addr_goal_item[0], len_goal_item[0], param_goal_position[0]); + } else if (msg->item1 == "LED") { + dxl_addparam_result = groupBulkWrite.addParam((uint8_t)msg->id1, addr_goal_item[0], len_goal_item[0], param_goal_led[0]); + } + if (dxl_addparam_result != true) { + ROS_ERROR("Failed to addparam to groupBulkWrite for Dynamixel ID: %d", msg->id1); + } + + if (msg->item2 == "position") { + dxl_addparam_result = groupBulkWrite.addParam((uint8_t)msg->id2, addr_goal_item[1], len_goal_item[1], param_goal_position[1]); + } else if (msg->item2 == "LED") { + dxl_addparam_result = groupBulkWrite.addParam((uint8_t)msg->id2, addr_goal_item[1], len_goal_item[1], param_goal_led[1]); + } + if (dxl_addparam_result != true) { + ROS_ERROR("Failed to addparam to groupBulkWrite for Dynamixel ID: %d", msg->id2); + } + + if (msg->item3 == "position") { + dxl_addparam_result = groupBulkWrite.addParam((uint8_t)msg->id3, addr_goal_item[2], len_goal_item[2], param_goal_position[2]); + } else if (msg->item3 == "LED") { + dxl_addparam_result = groupBulkWrite.addParam((uint8_t)msg->id3, addr_goal_item[2], len_goal_item[2], param_goal_led[2]); + } + if (dxl_addparam_result != true) { + ROS_ERROR("Failed to addparam to groupBulkWrite for Dynamixel ID: %d", msg->id3); + } + + if (msg->item4 == "position") { + dxl_addparam_result = groupBulkWrite.addParam((uint8_t)msg->id4, addr_goal_item[3], len_goal_item[3], param_goal_position[3]); + } else if (msg->item4 == "LED") { + dxl_addparam_result = groupBulkWrite.addParam((uint8_t)msg->id4, addr_goal_item[3], len_goal_item[3], param_goal_led[3]); + } + if (dxl_addparam_result != true) { + ROS_ERROR("Failed to addparam to groupBulkWrite for Dynamixel ID: %d", msg->id4); + } + + if (msg->item5 == "position") { + dxl_addparam_result = groupBulkWrite.addParam((uint8_t)msg->id5, addr_goal_item[4], len_goal_item[4], param_goal_position[4]); + } else if (msg->item5 == "LED") { + dxl_addparam_result = groupBulkWrite.addParam((uint8_t)msg->id5, addr_goal_item[4], len_goal_item[4], param_goal_led[4]); + } + if (dxl_addparam_result != true) { + ROS_ERROR("Failed to addparam to groupBulkWrite for Dynamixel ID: %d", msg->id5); + } + + dxl_comm_result = groupBulkWrite.txPacket(); + if (dxl_comm_result == COMM_SUCCESS) { + ROS_INFO("setItem : [ID:%d] [%s:%d]", msg->id1, msg->item1.c_str(), msg->value1); + ROS_INFO("setItem : [ID:%d] [%s:%d]", msg->id2, msg->item2.c_str(), msg->value2); + ROS_INFO("setItem : [ID:%d] [%s:%d]", msg->id3, msg->item3.c_str(), msg->value3); + ROS_INFO("setItem : [ID:%d] [%s:%d]", msg->id4, msg->item4.c_str(), msg->value4); + ROS_INFO("setItem : [ID:%d] [%s:%d]", msg->id5, msg->item5.c_str(), msg->value5); + } else { + ROS_INFO("Failed to set position! Result: %d", dxl_comm_result); + } + + groupBulkWrite.clearParam(); +} + +int main(int argc, char ** argv) +{ + uint8_t dxl_error = 0; + int dxl_comm_result = COMM_TX_FAIL; + + if (!portHandler->openPort()) { + ROS_ERROR("Failed to open the port!"); + return -1; + } + + if (!portHandler->setBaudRate(BAUDRATE)) { + ROS_ERROR("Failed to set the baudrate!"); + return -1; + } + + dxl_comm_result = packetHandler->write1ByteTxRx( + portHandler, DXL1_ID, ADDR_TORQUE_ENABLE, 1, &dxl_error); + if (dxl_comm_result != COMM_SUCCESS) { + ROS_ERROR("Failed to enable torque for Dynamixel ID: %d", DXL1_ID); + return -1; + } + + dxl_comm_result = packetHandler->write1ByteTxRx( + portHandler, DXL2_ID, ADDR_TORQUE_ENABLE, 1, &dxl_error); + if (dxl_comm_result != COMM_SUCCESS) { + ROS_ERROR("Failed to enable torque for Dynamixel ID: %d", DXL2_ID); + return -1; + } + + dxl_comm_result = packetHandler->write1ByteTxRx( + portHandler, DXL3_ID, ADDR_TORQUE_ENABLE, 1, &dxl_error); + if (dxl_comm_result != COMM_SUCCESS) { + ROS_ERROR("Failed to enable torque for Dynamixel ID: %d", DXL3_ID); + return -1; + } + + dxl_comm_result = packetHandler->write1ByteTxRx( + portHandler, DXL4_ID, ADDR_TORQUE_ENABLE, 1, &dxl_error); + if (dxl_comm_result != COMM_SUCCESS) { + ROS_ERROR("Failed to enable torque for Dynamixel ID: %d", DXL4_ID); + return -1; + } + + dxl_comm_result = packetHandler->write1ByteTxRx( + portHandler, DXL5_ID, ADDR_TORQUE_ENABLE, 1, &dxl_error); + if (dxl_comm_result != COMM_SUCCESS) { + ROS_ERROR("Failed to enable torque for Dynamixel ID: %d", DXL5_ID); + return -1; + } + + ros::init(argc, argv, "read_write_arm_node"); + ros::NodeHandle nh; + ros::ServiceServer get_joint_positions_srv = nh.advertiseService("/get_joint_positions", getJointPositionsCallback); + ros::Subscriber set_joint_positions_sub = nh.subscribe("/set_joint_positions", 10, setJointPositionsCallback); + ros::spin(); + + portHandler->closePort(); + return 0; +} + diff --git a/flo_humanoid/src/read_write_joint.cpp b/flo_humanoid/src/readWriteJoint.cpp similarity index 98% rename from flo_humanoid/src/read_write_joint.cpp rename to flo_humanoid/src/readWriteJoint.cpp index 2798071..8b6d811 100644 --- a/flo_humanoid/src/read_write_joint.cpp +++ b/flo_humanoid/src/readWriteJoint.cpp @@ -55,6 +55,7 @@ using namespace dynamixel; #define DXL1_ID 1 // DXL1 ID #define DXL2_ID 2 // DXL2 ID #define BAUDRATE 57600 // Default Baudrate of DYNAMIXEL X series +//set up fixed mount point for the device, this is the same as the one set in the udev rules file. #define DEVICE_NAME "/dev/ttyUSB0" // [Linux] To find assigned port, use "$ ls /dev/ttyUSB*" command PortHandler * portHandler; diff --git a/flo_humanoid/src/read_write_humanoid.cpp b/flo_humanoid/src/read_write_humanoid.cpp deleted file mode 100644 index d59be46..0000000 --- a/flo_humanoid/src/read_write_humanoid.cpp +++ /dev/null @@ -1,238 +0,0 @@ -// Copyright 2021 ROBOTIS CO., LTD. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/******************************************************************************* - * This example is written for DYNAMIXEL X(excluding XL-320) and MX(2.0) series with U2D2. - * For other series, please refer to the product eManual and modify the Control Table addresses and other definitions. - * To test this example, please follow the commands below. - * - * Open terminal #1 - * $ roscore - * - * Open terminal #2 - * $ rosrun dynamixel_sdk_examples bulk_read_write_node - * - * Open terminal #3 (run one of below commands at a time) - * $ rostopic pub -1 /bulk_set_item dynamixel_sdk_examples/BulkSetItem "{id1: 1, id2: 2, item1: 'position', item2: 'LED', value1: 1000, value2: 1}" - * $ rostopic pub -1 /bulk_set_item dynamixel_sdk_examples/BulkSetItem "{id1: 1, id2: 2, item1: 'LED', item2: 'position', value1: 1, value2: 1000}" - * $ rosservice call /bulk_get_item "{id1: 1, id2: 2, item1: 'position', item2: 'LED'}" - * - * Author: Jaehyun Shim -*******************************************************************************/ -// This code is based on the bulk_read_write_node.cpp from the dynamixel_sdk_examples package. -#include - -#include "std_msgs/String.h" -#include "dynamixel_sdk_examples/BulkGetItem.h" -#include "dynamixel_sdk_examples/BulkSetItem.h" -#include "dynamixel_sdk/dynamixel_sdk.h" - -using namespace dynamixel; - -// Control table address -#define ADDR_TORQUE_ENABLE 64 -#define ADDR_PRESENT_LED 65 -#define ADDR_PRESENT_POSITION 132 -#define ADDR_GOAL_POSITION 116 - -// Protocol version -#define PROTOCOL_VERSION 2.0 // Default Protocol version of DYNAMIXEL X series. - -// Default setting -#define DXL1_ID 1 // DXL1 ID -#define DXL2_ID 2 // DXL2 ID -#define BAUDRATE 57600 // Default Baudrate of DYNAMIXEL X series -#define DEVICE_NAME "/dev/ttyUSB0" // [Linux] To find assigned port, use "$ ls /dev/ttyUSB*" command - -PortHandler * portHandler = PortHandler::getPortHandler(DEVICE_NAME); -PacketHandler * packetHandler = PacketHandler::getPacketHandler(PROTOCOL_VERSION); - -GroupBulkRead groupBulkRead(portHandler, packetHandler); -GroupBulkWrite groupBulkWrite(portHandler, packetHandler); - -bool bulkGetItemCallback( - dynamixel_sdk_examples::BulkGetItem::Request & req, - dynamixel_sdk_examples::BulkGetItem::Response & res) -{ - uint8_t dxl_error = 0; - int dxl_comm_result = COMM_TX_FAIL; - int dxl_addparam_result = false; - - // Position Value of X series is 4 byte data. For AX & MX(1.0) use 2 byte data(int16_t) for the Position Value. - int32_t position1 = 0; - int32_t position2 = 0; - - // Read Present Position (length : 4 bytes) and Convert uint32 -> int32 - // When reading 2 byte data from AX / MX(1.0), use read2ByteTxRx() instead. - if (req.item1 == "position") { - dxl_addparam_result = groupBulkRead.addParam((uint8_t)req.id1, ADDR_PRESENT_POSITION, 4); - } else if (req.item1 == "LED") { - dxl_addparam_result = groupBulkRead.addParam((uint8_t)req.id1, ADDR_PRESENT_LED, 1); - } - if (dxl_addparam_result != true) { - ROS_ERROR("Failed to addparam to groupBulkRead for Dynamixel ID: %d", req.id1); - return 0; - } - - if (req.item2 == "position") { - dxl_addparam_result = groupBulkRead.addParam((uint8_t)req.id2, ADDR_PRESENT_POSITION, 4); - } else if (req.item2 == "LED") { - dxl_addparam_result = groupBulkRead.addParam((uint8_t)req.id2, ADDR_PRESENT_LED, 1); - } - if (dxl_addparam_result != true) { - ROS_ERROR("Failed to addparam to groupBulkRead for Dynamixel ID %d", req.id2); - return 0; - } - - uint32_t value1 = 0; - uint32_t value2 = 0; - dxl_comm_result = groupBulkRead.txRxPacket(); - if (dxl_comm_result == COMM_SUCCESS) { - if (req.item1 == "position") { - value1 = groupBulkRead.getData((uint8_t)req.id1, ADDR_PRESENT_POSITION, 4); - } else if (req.item2 == "LED") { - value1 = groupBulkRead.getData((uint8_t)req.id1, ADDR_PRESENT_POSITION, 4); - } - - if (req.item1 == "position") { - value2 = groupBulkRead.getData((uint8_t)req.id2, ADDR_PRESENT_POSITION, 4); - } else if (req.item2 == "LED") { - value2 = groupBulkRead.getData((uint8_t)req.id2, ADDR_PRESENT_POSITION, 4); - } - - ROS_INFO("getItem : [ID:%d] [%s: %d]", req.id1, req.item1.c_str(), value1); - ROS_INFO("getItem : [ID:%d] [%s: %d]", req.id2, req.item2.c_str(), value2); - res.value1 = value1; - res.value2 = value2; - groupBulkRead.clearParam(); - return true; - } else { - ROS_ERROR("Failed to get position! Result: %d", dxl_comm_result); - groupBulkRead.clearParam(); - return false; - } -} - -void bulkSetItemCallback(const dynamixel_sdk_examples::BulkSetItem::ConstPtr & msg) -{ - uint8_t dxl_error = 0; - int dxl_comm_result = COMM_TX_FAIL; - int dxl_addparam_result = false; - uint8_t param_goal_position1[4]; - uint8_t param_goal_position2[4]; - uint8_t param_goal_led1[1]; - uint8_t param_goal_led2[1]; - uint8_t addr_goal_item[2]; - uint8_t len_goal_item[2]; - - // Position Value of X series is 4 byte data. For AX & MX(1.0) use 2 byte data(uint16_t) for the Position Value. - if (msg->item1 == "position") { - uint32_t position1 = (unsigned int)msg->value1; // Convert int32 -> uint32 - param_goal_position1[0] = DXL_LOBYTE(DXL_LOWORD(position1)); - param_goal_position1[1] = DXL_HIBYTE(DXL_LOWORD(position1)); - param_goal_position1[2] = DXL_LOBYTE(DXL_HIWORD(position1)); - param_goal_position1[3] = DXL_HIBYTE(DXL_HIWORD(position1)); - addr_goal_item[0] = ADDR_GOAL_POSITION; - len_goal_item[0] = 4; - } else if (msg->item1 == "LED") { - uint32_t led1 = (unsigned int)msg->value1; // Convert int32 -> uint32 - param_goal_led1[0] = led1; - addr_goal_item[0] = ADDR_PRESENT_LED; - len_goal_item[0] = 1; - } - - if (msg->item2 == "position") { - uint32_t position2 = (unsigned int)msg->value2; // Convert int32 -> uint32 - param_goal_position2[0] = DXL_LOBYTE(DXL_LOWORD(position2)); - param_goal_position2[1] = DXL_HIBYTE(DXL_LOWORD(position2)); - param_goal_position2[2] = DXL_LOBYTE(DXL_HIWORD(position2)); - param_goal_position2[3] = DXL_HIBYTE(DXL_HIWORD(position2)); - addr_goal_item[1] = ADDR_GOAL_POSITION; - len_goal_item[1] = 4; - } else if (msg->item2 == "LED") { - uint32_t led2 = (unsigned int)msg->value2; // Convert int32 -> uint32 - param_goal_led2[0] = led2; - addr_goal_item[1] = ADDR_PRESENT_LED; - len_goal_item[1] = 1; - } - - // Write Goal Position (length : 4 bytes) - // When writing 2 byte data to AX / MX(1.0), use write2ByteTxRx() instead. - if (msg->item1 == "position") { - dxl_addparam_result = groupBulkWrite.addParam((uint8_t)msg->id1, addr_goal_item[0], len_goal_item[0], param_goal_position1); - } else if (msg->item1 == "LED") { - dxl_addparam_result = groupBulkWrite.addParam((uint8_t)msg->id1, addr_goal_item[0], len_goal_item[0], param_goal_led1); - } - if (dxl_addparam_result != true) { - ROS_ERROR("Failed to addparam to groupBulkWrite for Dynamixel ID: %d", msg->id1); - } - - if (msg->item2 == "position") { - dxl_addparam_result = groupBulkWrite.addParam((uint8_t)msg->id2, addr_goal_item[1], len_goal_item[1], param_goal_position2); - } else if (msg->item2 == "LED") { - dxl_addparam_result = groupBulkWrite.addParam((uint8_t)msg->id2, addr_goal_item[1], len_goal_item[1], param_goal_led2); - } - if (dxl_addparam_result != true) { - ROS_ERROR("Failed to addparam to groupBulkWrite for Dynamixel ID: %d", msg->id2); - } - - dxl_comm_result = groupBulkWrite.txPacket(); - if (dxl_comm_result == COMM_SUCCESS) { - ROS_INFO("setItem : [ID:%d] [%s:%d]", msg->id1, msg->item1.c_str(), msg->value1); - ROS_INFO("setItem : [ID:%d] [%s:%d]", msg->id2, msg->item2.c_str(), msg->value2); - } else { - ROS_INFO("Failed to set position! Result: %d", dxl_comm_result); - } - - groupBulkWrite.clearParam(); -} - -int main(int argc, char ** argv) -{ - uint8_t dxl_error = 0; - int dxl_comm_result = COMM_TX_FAIL; - - if (!portHandler->openPort()) { - ROS_ERROR("Failed to open the port!"); - return -1; - } - - if (!portHandler->setBaudRate(BAUDRATE)) { - ROS_ERROR("Failed to set the baudrate!"); - return -1; - } - - dxl_comm_result = packetHandler->write1ByteTxRx( - portHandler, DXL1_ID, ADDR_TORQUE_ENABLE, 1, &dxl_error); - if (dxl_comm_result != COMM_SUCCESS) { - ROS_ERROR("Failed to enable torque for Dynamixel ID: %d", DXL1_ID); - return -1; - } - - dxl_comm_result = packetHandler->write1ByteTxRx( - portHandler, DXL2_ID, ADDR_TORQUE_ENABLE, 1, &dxl_error); - if (dxl_comm_result != COMM_SUCCESS) { - ROS_ERROR("Failed to enable torque for Dynamixel ID: %d", DXL2_ID); - return -1; - } - - ros::init(argc, argv, "bulk_read_write_node"); - ros::NodeHandle nh; - ros::ServiceServer bulk_get_item_srv = nh.advertiseService("/bulk_get_item", bulkGetItemCallback); - ros::Subscriber bulk_set_item_sub = nh.subscribe("/bulk_set_item", 10, bulkSetItemCallback); - ros::spin(); - - portHandler->closePort(); - return 0; -} diff --git a/flo_humanoid/src/robot_controller.cpp b/flo_humanoid/src/robot_controller.cpp deleted file mode 100644 index e69de29..0000000 diff --git a/flo_humanoid_defs/msg/SetJointPositions.msg b/flo_humanoid_defs/msg/SetJointPositions.msg index 2608c06..06c6c5e 100644 --- a/flo_humanoid_defs/msg/SetJointPositions.msg +++ b/flo_humanoid_defs/msg/SetJointPositions.msg @@ -1,34 +1,22 @@ -#Message definition for passing motor commands to all the joints of the humanoid robot -#(10 joints, 2 at each shoulder, 2 at each elbow and 1 at the end effector) +#Message definition to get the position of all the joints of 1 arm in the humanoid robot +#(5 joints in each arm, 2 at the shoulder, 2 at the elbow and 1 at the end effector) uint8 id1 uint8 id2 uint8 id3 uint8 id4 uint8 id5 -uint8 id6 -uint8 id7 -uint8 id8 -uint8 id9 -uint8 id10 + string item1 string item2 string item3 string item4 string item5 -string item6 -string item7 -string item8 -string item9 -string item10 + int32 value1 int32 value2 int32 value3 int32 value4 int32 value5 -int32 value6 -int32 value7 -int32 value8 -int32 value9 -int32 value10 + diff --git a/flo_humanoid_defs/package.xml b/flo_humanoid_defs/package.xml index c55cfdd..8bffef5 100644 --- a/flo_humanoid_defs/package.xml +++ b/flo_humanoid_defs/package.xml @@ -2,7 +2,7 @@ flo_humanoid_defs 0.0.0 - The message and action definitions for the flo_humanoid package + Contains message and service definitions for the flo v2 humanoid Ajay Anand proprietery catkin diff --git a/flo_humanoid_defs/srv/GetJointPositions.srv b/flo_humanoid_defs/srv/GetJointPositions.srv index 67e8791..bb9c9eb 100644 --- a/flo_humanoid_defs/srv/GetJointPositions.srv +++ b/flo_humanoid_defs/srv/GetJointPositions.srv @@ -1,26 +1,18 @@ -#Service definition to get the position of all the joints in the humanoid robot -#(10 joints, 2 at each shoulder, 2 at each elbow and 1 at the end effector) +#Service definition to get the position of all the joints of 1 arm in the humanoid robot +#(5 joints in each arm, 2 at the shoulder, 2 at the elbow and 1 at the end effector) uint8 id1 uint8 id2 uint8 id3 uint8 id4 uint8 id5 -uint8 id6 -uint8 id7 -uint8 id8 -uint8 id9 -uint8 id10 + string item1 string item2 string item3 string item4 string item5 -string item6 -string item7 -string item8 -string item9 -string item10 + --- int32 value1 @@ -28,11 +20,7 @@ int32 value2 int32 value3 int32 value4 int32 value5 -int32 value6 -int32 value7 -int32 value8 -int32 value9 -int32 value10 + ``` diff --git a/flo_system/Dockerfile b/flo_system/Dockerfile new file mode 100644 index 0000000..2bca50d --- /dev/null +++ b/flo_system/Dockerfile @@ -0,0 +1,45 @@ +#base image from ohmnilabsvn/ohmni_ros:ohmni_ros_tbcontrol_0.0.13, preinstalled with ros-melodic + tb control ros packages +FROM ohmnilabsvn/ohmni_ros:ohmni_ros_tbcontrol_0.0.13 +#make super user +RUN su +#install essential tools +RUN apt-get update -y && sudo apt-get -y install \ +neovim \ +tmux \ +git +# clear cache +RUN apt-get clean +#switch to ohmnidev directory, used for placing code. +WORKDIR /home/ohmnidev +RUN git clone https://github.com/Rehab-Robotics-Lab/FloSystemV2.git +WORKDIR /home/ohmnidev/FloSystemV2 +RUN git pull +WORKDIR /home/ohmnidev +RUN mkdir -p catkin_ws +WORKDIR /home/ohmnidev/catkin_ws +ARG ROS_DISTRO=melodic +ENV ROS_DISTRO="$ROS_DISTRO" +RUN apt-get update && apt-get install -y ros-$ROS_DISTRO-catkin +#install usb utilities to get info on connected devices - might not be as useful. comment next line if not needed. +#usbutils\ +#The following 2 lines download libraries for control of arduino via ros +#ros-$ROS_DISTRO-rosserial \ +#ros-$ROS_DISTRO-rosserial-arduino\ +#THE following line downloads the dynamixel sdk for ros. +# ros-melodic-dynamixel-sdk\ +#The following line downloads the dynamixel sdk examples for ros. +# ros-melodic-dynamixel-sdk-examples + + +#Lidar libraries +#RUN git clone https://github.com/Slamtec/rplidar_ros.git + +#Build the catkin_ws +#RUN catkin_make -j1 -l1 + + +#add as neccessary. + + + + diff --git a/floSystem/README.md b/flo_system/README.md similarity index 100% rename from floSystem/README.md rename to flo_system/README.md