From 8298e8283e31062ff6fb34731120fde93ba02e80 Mon Sep 17 00:00:00 2001 From: Ajay Anand Date: Wed, 27 Sep 2023 15:10:33 -0400 Subject: [PATCH 01/22] create dockerfile --- flo_humanoid/Dockerfile | 26 +++++++++++--------------- 1 file changed, 11 insertions(+), 15 deletions(-) diff --git a/flo_humanoid/Dockerfile b/flo_humanoid/Dockerfile index 2ca1fb9..4354c48 100644 --- a/flo_humanoid/Dockerfile +++ b/flo_humanoid/Dockerfile @@ -1,22 +1,23 @@ #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 ohmnilabsvn/ohmni_ros:ohmni_ros_tbcontrol_0.0.13 +FROM ros:melodic-robot-bionic #make super user RUN su #install essential tools RUN apt-get update -y && sudo apt-get -y install \ -neovim \ -tmux \ +# neovim \ +# tmux \ git # clear cache RUN apt-get clean #switch to ohmnidev directory, used for placing code. -WORKDIR /home/ohmnidev +RUN mkdir -p home/git +WORKDIR /home/git/FloSystemV2 RUN git clone https://github.com/Rehab-Robotics-Lab/FloSystemV2.git -WORKDIR /home/ohmnidev/FloSystemV2 RUN git pull -WORKDIR /home/ohmnidev +WORKDIR /home/git RUN mkdir -p catkin_ws -WORKDIR /home/ohmnidev/catkin_ws +WORKDIR /home/git/catkin_ws ARG ROS_DISTRO=melodic ENV ROS_DISTRO="$ROS_DISTRO" RUN apt-get update && apt-get install -y ros-$ROS_DISTRO-catkin \ @@ -26,18 +27,13 @@ RUN apt-get update && apt-get install -y ros-$ROS_DISTRO-catkin \ #ros-$ROS_DISTRO-rosserial \ #ros-$ROS_DISTRO-rosserial-arduino\ #THE following line downloads the dynamixel sdk for ros. -ros-melodic-dynamixel-sdk\ +# ros-melodic-dynamixel-sdk\ #The following line downloads the dynamixel sdk examples for ros. -ros-melodic-dynamixel-sdk-examples - - +# 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 - - +RUN catkin_make -j1 -l1 #add as neccessary. From 0e373c87c060dd20c5eae8abcb80fcd3b0bdb0ac Mon Sep 17 00:00:00 2001 From: Ajay Anand Date: Wed, 27 Sep 2023 19:08:36 -0400 Subject: [PATCH 02/22] fix dockerfile --- flo_humanoid/Dockerfile | 32 +++++++++++++++----------------- 1 file changed, 15 insertions(+), 17 deletions(-) diff --git a/flo_humanoid/Dockerfile b/flo_humanoid/Dockerfile index 4354c48..99a8181 100644 --- a/flo_humanoid/Dockerfile +++ b/flo_humanoid/Dockerfile @@ -8,31 +8,29 @@ RUN apt-get update -y && sudo apt-get -y install \ # neovim \ # tmux \ git +RUN apt-get update && apt-get install -y ros-$ROS_DISTRO-catkin \ +#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 # clear cache RUN apt-get clean +ARG ROS_DISTRO=melodic +ENV ROS_DISTRO="$ROS_DISTRO" #switch to ohmnidev directory, used for placing code. RUN mkdir -p home/git -WORKDIR /home/git/FloSystemV2 +WORKDIR /home/git RUN git clone https://github.com/Rehab-Robotics-Lab/FloSystemV2.git -RUN git pull 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 -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. From 613f2354128b859731832f9902bfa29880b96b59 Mon Sep 17 00:00:00 2001 From: Ajay Anand Date: Wed, 27 Sep 2023 19:09:21 -0400 Subject: [PATCH 03/22] fix dockerfile --- flo_humanoid/Dockerfile | 1 - 1 file changed, 1 deletion(-) diff --git a/flo_humanoid/Dockerfile b/flo_humanoid/Dockerfile index 99a8181..d192ceb 100644 --- a/flo_humanoid/Dockerfile +++ b/flo_humanoid/Dockerfile @@ -30,7 +30,6 @@ 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 #add as neccessary. From 0c6189d78248bae3d38cf4661caefaf050a9790e Mon Sep 17 00:00:00 2001 From: Ajay Anand Date: Thu, 28 Sep 2023 12:59:02 -0400 Subject: [PATCH 04/22] edit dockerfile --- flo_humanoid/Dockerfile | 2 +- flo_humanoid/config/joints | 7 ------- 2 files changed, 1 insertion(+), 8 deletions(-) diff --git a/flo_humanoid/Dockerfile b/flo_humanoid/Dockerfile index d192ceb..997f2e0 100644 --- a/flo_humanoid/Dockerfile +++ b/flo_humanoid/Dockerfile @@ -31,7 +31,7 @@ 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 -#add as neccessary. + diff --git a/flo_humanoid/config/joints b/flo_humanoid/config/joints index 5803ca4..1572a9b 100644 --- a/flo_humanoid/config/joints +++ b/flo_humanoid/config/joints @@ -1,12 +1,5 @@ #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 #eg: # 1 left_shoulder_flexionextension 765 10 500 1 From cfe8c4b482dff47d410372b5039cde8f199cac28 Mon Sep 17 00:00:00 2001 From: Ajay Anand Date: Thu, 28 Sep 2023 13:40:48 -0400 Subject: [PATCH 05/22] edit config file - add new motor IDs and types --- flo_humanoid/Dockerfile | 22 +++++++++++----------- flo_humanoid/config/joints | 15 ++++++++++++--- 2 files changed, 23 insertions(+), 14 deletions(-) diff --git a/flo_humanoid/Dockerfile b/flo_humanoid/Dockerfile index 997f2e0..f023ca8 100644 --- a/flo_humanoid/Dockerfile +++ b/flo_humanoid/Dockerfile @@ -3,24 +3,24 @@ 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 \ -# neovim \ -# tmux \ -git -RUN apt-get update && apt-get install -y ros-$ROS_DISTRO-catkin \ -#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 +RUN apt-get update -y && apt-get install -y \ +neovim \ +tmux \ +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 # clear cache RUN apt-get clean -ARG ROS_DISTRO=melodic -ENV ROS_DISTRO="$ROS_DISTRO" + #switch to ohmnidev directory, used for placing code. RUN mkdir -p home/git WORKDIR /home/git diff --git a/flo_humanoid/config/joints b/flo_humanoid/config/joints index 1572a9b..b010efd 100644 --- a/flo_humanoid/config/joints +++ b/flo_humanoid/config/joints @@ -1,6 +1,15 @@ #fill this table with the joints you want to control in the humanoid robot and their configurations. (dynamixel motor address and joint name for each motor) -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 From e84ae1b424db3863740be9f7968e72102eed0814 Mon Sep 17 00:00:00 2001 From: Ajay Anand Date: Thu, 28 Sep 2023 13:52:59 -0400 Subject: [PATCH 06/22] edit nomenclature --- flo_humanoid/src/read_write_humanoid.cpp | 6 +++--- {floSystem => flo_system}/README.md | 0 2 files changed, 3 insertions(+), 3 deletions(-) rename {floSystem => flo_system}/README.md (100%) diff --git a/flo_humanoid/src/read_write_humanoid.cpp b/flo_humanoid/src/read_write_humanoid.cpp index d59be46..ad1e04a 100644 --- a/flo_humanoid/src/read_write_humanoid.cpp +++ b/flo_humanoid/src/read_write_humanoid.cpp @@ -50,10 +50,10 @@ using namespace dynamixel; #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 DXL1_ID 10 // DXL1 ID +#define DXL2_ID 11 // 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 +#define DEVICE_NAME "/dev/ttyUSB0" // [L-inux] To find assigned port, use "$ ls /dev/ttyUSB*" command PortHandler * portHandler = PortHandler::getPortHandler(DEVICE_NAME); PacketHandler * packetHandler = PacketHandler::getPacketHandler(PROTOCOL_VERSION); 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 From 756aca549bcfb2408ae42e0df4e8a5112f67e677 Mon Sep 17 00:00:00 2001 From: Ajay Anand Date: Fri, 29 Sep 2023 10:23:07 -0400 Subject: [PATCH 07/22] edit msg+srv defs humanoid --- flo_humanoid_defs/msg/SetJointPositions.msg | 22 +++++---------------- flo_humanoid_defs/srv/GetJointPositions.srv | 22 +++++---------------- 2 files changed, 10 insertions(+), 34 deletions(-) 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/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 + ``` From dae73d32d7093247184384988459e5706762f581 Mon Sep 17 00:00:00 2001 From: Ajay Anand Date: Fri, 29 Sep 2023 10:59:39 -0400 Subject: [PATCH 08/22] docker compose init, dockerfile edit --- compose.yaml | 9 +++++ flo_humanoid/Dockerfile | 8 ++--- flo_humanoid/src/read_write_humanoid.cpp | 22 +++--------- flo_system/Dockerfile | 45 ++++++++++++++++++++++++ 4 files changed, 63 insertions(+), 21 deletions(-) create mode 100644 compose.yaml create mode 100644 flo_system/Dockerfile 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/Dockerfile b/flo_humanoid/Dockerfile index f023ca8..f9c309c 100644 --- a/flo_humanoid/Dockerfile +++ b/flo_humanoid/Dockerfile @@ -1,5 +1,3 @@ -#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 @@ -20,8 +18,7 @@ ros-$ROS_DISTRO-dynamixel-sdk-examples #Build the catkin_ws # clear cache RUN apt-get clean - -#switch to ohmnidev directory, used for placing code. + RUN mkdir -p home/git WORKDIR /home/git RUN git clone https://github.com/Rehab-Robotics-Lab/FloSystemV2.git @@ -32,6 +29,9 @@ RUN cp -r /FloSystemV2/flo_humanoid_defs /catkin_ws/src catkin_ws/flo_humanoid_d WORKDIR /home/git/catkin_ws RUN catkin_make -j1 -l1 +ENTRYPOINT ["./ros_entrypoint.sh"] +CMD ["bash"] + diff --git a/flo_humanoid/src/read_write_humanoid.cpp b/flo_humanoid/src/read_write_humanoid.cpp index ad1e04a..082bd0d 100644 --- a/flo_humanoid/src/read_write_humanoid.cpp +++ b/flo_humanoid/src/read_write_humanoid.cpp @@ -1,16 +1,5 @@ -// 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 code is based on the bulk_read_write_node.cpp from the dynamixel_sdk_examples package. /******************************************************************************* * This example is written for DYNAMIXEL X(excluding XL-320) and MX(2.0) series with U2D2. @@ -30,7 +19,6 @@ * * 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" @@ -50,10 +38,10 @@ using namespace dynamixel; #define PROTOCOL_VERSION 2.0 // Default Protocol version of DYNAMIXEL X series. // Default setting -#define DXL1_ID 10 // DXL1 ID -#define DXL2_ID 11 // DXL2 ID +#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" // [L-inux] To find assigned port, use "$ ls /dev/ttyUSB*" command +// #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); 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. + + + + From 78eed8ff15a656d71ce0c262d70a4b0a1b30b7d3 Mon Sep 17 00:00:00 2001 From: Ajay Anand Date: Fri, 29 Sep 2023 11:29:02 -0400 Subject: [PATCH 09/22] edit package.xml --- flo_humanoid_defs/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flo_humanoid_defs/package.xml b/flo_humanoid_defs/package.xml index c55cfdd..013ef12 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_humanoid package Ajay Anand proprietery catkin From 28561457e3cf6d2fca93311de2a773c103e288cd Mon Sep 17 00:00:00 2001 From: Ajay Anand Date: Fri, 29 Sep 2023 11:40:30 -0400 Subject: [PATCH 10/22] edit package.xml --- flo_humanoid/package.xml | 19 +++++++++++++++++++ flo_humanoid_defs/package.xml | 2 +- 2 files changed, 20 insertions(+), 1 deletion(-) create mode 100644 flo_humanoid/package.xml 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_defs/package.xml b/flo_humanoid_defs/package.xml index 013ef12..8bffef5 100644 --- a/flo_humanoid_defs/package.xml +++ b/flo_humanoid_defs/package.xml @@ -2,7 +2,7 @@ flo_humanoid_defs 0.0.0 - Contains message and service definitions for the flo_humanoid package + Contains message and service definitions for the flo v2 humanoid Ajay Anand proprietery catkin From 1c4a8f4151bad7cd99fb8f2d175b5baf78dfbe83 Mon Sep 17 00:00:00 2001 From: Ajay Anand Date: Mon, 2 Oct 2023 12:47:19 -0400 Subject: [PATCH 11/22] edit executables --- flo_humanoid/CMakeLists.txt | 46 ++++++++----------- ...ad_write_humanoid.cpp => readWriteArm.cpp} | 5 +- ...ead_write_joint.cpp => readWriteJoint.cpp} | 1 + ...bot_controller.cpp => robotController.cpp} | 0 4 files changed, 23 insertions(+), 29 deletions(-) rename flo_humanoid/src/{read_write_humanoid.cpp => readWriteArm.cpp} (97%) rename flo_humanoid/src/{read_write_joint.cpp => readWriteJoint.cpp} (98%) rename flo_humanoid/src/{robot_controller.cpp => robotController.cpp} (100%) 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/src/read_write_humanoid.cpp b/flo_humanoid/src/readWriteArm.cpp similarity index 97% rename from flo_humanoid/src/read_write_humanoid.cpp rename to flo_humanoid/src/readWriteArm.cpp index ad1e04a..e6a1f2d 100644 --- a/flo_humanoid/src/read_write_humanoid.cpp +++ b/flo_humanoid/src/readWriteArm.cpp @@ -50,9 +50,10 @@ using namespace dynamixel; #define PROTOCOL_VERSION 2.0 // Default Protocol version of DYNAMIXEL X series. // Default setting -#define DXL1_ID 10 // DXL1 ID -#define DXL2_ID 11 // DXL2 ID +#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" // [L-inux] To find assigned port, use "$ ls /dev/ttyUSB*" command PortHandler * portHandler = PortHandler::getPortHandler(DEVICE_NAME); 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/robot_controller.cpp b/flo_humanoid/src/robotController.cpp similarity index 100% rename from flo_humanoid/src/robot_controller.cpp rename to flo_humanoid/src/robotController.cpp From f50ffeb533cc6cf3cb75f08d159a19635c90b69d Mon Sep 17 00:00:00 2001 From: Ajay Anand Date: Mon, 2 Oct 2023 12:57:20 -0400 Subject: [PATCH 12/22] edit contribution guidelines --- Contributors/README.md | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/Contributors/README.md b/Contributors/README.md index e87d0a8..4842e71 100644 --- a/Contributors/README.md +++ b/Contributors/README.md @@ -2,3 +2,18 @@ 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. + + +## 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. From bfd161dcdb814a12124fe5bbaff08c29f02b9aee Mon Sep 17 00:00:00 2001 From: Ajay Anand Date: Tue, 3 Oct 2023 12:40:16 -0400 Subject: [PATCH 13/22] add more motor control --- flo_humanoid/src/read_write_humanoid.cpp | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) diff --git a/flo_humanoid/src/read_write_humanoid.cpp b/flo_humanoid/src/read_write_humanoid.cpp index 082bd0d..1cbeeda 100644 --- a/flo_humanoid/src/read_write_humanoid.cpp +++ b/flo_humanoid/src/read_write_humanoid.cpp @@ -19,6 +19,9 @@ * * Author: Jaehyun Shim *******************************************************************************/ + +// system() call can be used to run shell commands from within a C++ program. + #include #include "std_msgs/String.h" @@ -38,10 +41,14 @@ using namespace dynamixel; #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 +// 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 +#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); @@ -224,3 +231,4 @@ int main(int argc, char ** argv) portHandler->closePort(); return 0; } + From 292e9d8fcb4ffa82101dd134a06a878a08ae7e29 Mon Sep 17 00:00:00 2001 From: Ajay Anand Date: Tue, 3 Oct 2023 12:48:41 -0400 Subject: [PATCH 14/22] add more motors --- flo_humanoid/src/readWriteArm.cpp | 34 +++++++++++++++++++++++++++++-- 1 file changed, 32 insertions(+), 2 deletions(-) diff --git a/flo_humanoid/src/readWriteArm.cpp b/flo_humanoid/src/readWriteArm.cpp index 01f5863..420ad8a 100644 --- a/flo_humanoid/src/readWriteArm.cpp +++ b/flo_humanoid/src/readWriteArm.cpp @@ -80,8 +80,8 @@ GroupBulkRead groupBulkRead(portHandler, packetHandler); GroupBulkWrite groupBulkWrite(portHandler, packetHandler); bool bulkGetItemCallback( - dynamixel_sdk_examples::BulkGetItem::Request & req, - dynamixel_sdk_examples::BulkGetItem::Response & res) + flo_humanoid_defs::GetJointPositions::Request & req, + flo_humanoid_defs::GetJointPositions::Response & res) { uint8_t dxl_error = 0; int dxl_comm_result = COMM_TX_FAIL; @@ -113,6 +113,36 @@ bool bulkGetItemCallback( 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(); From 416687e7eac0ee7787dd96f4725059deafcf7e7a Mon Sep 17 00:00:00 2001 From: Ajay Anand Date: Tue, 3 Oct 2023 12:53:30 -0400 Subject: [PATCH 15/22] rebase fix --- flo_humanoid/src/readWriteArm.cpp | 10 ++-------- 1 file changed, 2 insertions(+), 8 deletions(-) diff --git a/flo_humanoid/src/readWriteArm.cpp b/flo_humanoid/src/readWriteArm.cpp index 420ad8a..f685c73 100644 --- a/flo_humanoid/src/readWriteArm.cpp +++ b/flo_humanoid/src/readWriteArm.cpp @@ -56,7 +56,6 @@ using namespace dynamixel; #define PROTOCOL_VERSION 2.0 // Default Protocol version of DYNAMIXEL X series. // Default setting -<<<<<<< HEAD:flo_humanoid/src/read_write_humanoid.cpp // 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 @@ -64,14 +63,9 @@ using namespace dynamixel; #define DXL4_ID 13 // DXL4 ID #define DXL5_ID 14 // DXL5 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 -======= -#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" // [L-inux] To find assigned port, use "$ ls /dev/ttyUSB*" command ->>>>>>> f50ffeb533cc6cf3cb75f08d159a19635c90b69d:flo_humanoid/src/readWriteArm.cpp +#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); From 6fff39b3e9af0f7353746107f29dd64c0e7f9fff Mon Sep 17 00:00:00 2001 From: Ajay Anand Date: Tue, 3 Oct 2023 15:15:00 -0400 Subject: [PATCH 16/22] add multiple motors to read function --- flo_humanoid/src/readWriteArm.cpp | 60 ++++++++++++------------------- 1 file changed, 23 insertions(+), 37 deletions(-) diff --git a/flo_humanoid/src/readWriteArm.cpp b/flo_humanoid/src/readWriteArm.cpp index f685c73..676e897 100644 --- a/flo_humanoid/src/readWriteArm.cpp +++ b/flo_humanoid/src/readWriteArm.cpp @@ -1,42 +1,6 @@ -// 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 -*******************************************************************************/ -<<<<<<< HEAD:flo_humanoid/src/read_write_humanoid.cpp - +// 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. -======= -// This code is based on the bulk_read_write_node.cpp from the dynamixel_sdk_examples package. ->>>>>>> f50ffeb533cc6cf3cb75f08d159a19635c90b69d:flo_humanoid/src/readWriteArm.cpp #include #include "std_msgs/String.h" @@ -153,8 +117,30 @@ bool bulkGetItemCallback( 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; groupBulkRead.clearParam(); From 83282e70630ab6e0cc5f27c66df24f51efdd8442 Mon Sep 17 00:00:00 2001 From: Ajay Anand Date: Tue, 3 Oct 2023 15:29:41 -0400 Subject: [PATCH 17/22] BulkGetItem init complete --- flo_humanoid/src/readWriteArm.cpp | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/flo_humanoid/src/readWriteArm.cpp b/flo_humanoid/src/readWriteArm.cpp index 676e897..f824e2b 100644 --- a/flo_humanoid/src/readWriteArm.cpp +++ b/flo_humanoid/src/readWriteArm.cpp @@ -36,7 +36,8 @@ 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 bulkGetItemCallback( flo_humanoid_defs::GetJointPositions::Request & req, flo_humanoid_defs::GetJointPositions::Response & res) @@ -104,7 +105,11 @@ bool bulkGetItemCallback( 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") { @@ -135,7 +140,6 @@ bool bulkGetItemCallback( 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); @@ -143,6 +147,9 @@ bool bulkGetItemCallback( 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 { From d7e86dd17c0076e1a953b3dd5a1f2633854878af Mon Sep 17 00:00:00 2001 From: Ajay Anand Date: Tue, 3 Oct 2023 15:33:24 -0400 Subject: [PATCH 18/22] add addn formatting info --- Contributors/README.md | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/Contributors/README.md b/Contributors/README.md index 4842e71..f114a32 100644 --- a/Contributors/README.md +++ b/Contributors/README.md @@ -1,7 +1,8 @@ ## 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: From 66dec9ddc23ef43c6b79e68048974b6ab08938b7 Mon Sep 17 00:00:00 2001 From: Ajay Anand Date: Tue, 3 Oct 2023 16:41:56 -0400 Subject: [PATCH 19/22] final arm position control for testing --- flo_humanoid/src/readWriteArm.cpp | 150 +++++++++++++++++++++++++----- 1 file changed, 125 insertions(+), 25 deletions(-) diff --git a/flo_humanoid/src/readWriteArm.cpp b/flo_humanoid/src/readWriteArm.cpp index f824e2b..f99a91f 100644 --- a/flo_humanoid/src/readWriteArm.cpp +++ b/flo_humanoid/src/readWriteArm.cpp @@ -38,7 +38,7 @@ 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 bulkGetItemCallback( +bool getJointPositionsCallback( flo_humanoid_defs::GetJointPositions::Request & req, flo_humanoid_defs::GetJointPositions::Response & res) { @@ -159,73 +159,152 @@ bool bulkGetItemCallback( } } -void bulkSetItemCallback(const dynamixel_sdk_examples::BulkSetItem::ConstPtr & msg) +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_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_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)); + 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_led1[0] = led1; + 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_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)); + 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_led2[0] = led2; + 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_position1); + 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_led1); + 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_position2); + 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_led2); + 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); } @@ -262,10 +341,31 @@ int main(int argc, char ** argv) return -1; } - ros::init(argc, argv, "bulk_read_write_node"); + 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 bulk_get_item_srv = nh.advertiseService("/bulk_get_item", bulkGetItemCallback); - ros::Subscriber bulk_set_item_sub = nh.subscribe("/bulk_set_item", 10, bulkSetItemCallback); + 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(); From 0ebae86cc402664328457e7546a29e38491a60f1 Mon Sep 17 00:00:00 2001 From: Ajay Anand Date: Tue, 3 Oct 2023 16:50:47 -0400 Subject: [PATCH 20/22] delete unneeded file --- flo_humanoid/src/robotController.cpp | 0 1 file changed, 0 insertions(+), 0 deletions(-) delete mode 100644 flo_humanoid/src/robotController.cpp diff --git a/flo_humanoid/src/robotController.cpp b/flo_humanoid/src/robotController.cpp deleted file mode 100644 index e69de29..0000000 From c4259978bf1e9009f0a3d4d21f9cf97f3dba72b9 Mon Sep 17 00:00:00 2001 From: Ajay Anand Date: Wed, 4 Oct 2023 13:06:58 -0400 Subject: [PATCH 21/22] edit readme --- README.md | 22 ++++++++++++++++------ 1 file changed, 16 insertions(+), 6 deletions(-) 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. From aff8846c2e018f3f3b79eb166907e36cb4df36da Mon Sep 17 00:00:00 2001 From: Ajay Anand Date: Wed, 4 Oct 2023 13:11:40 -0400 Subject: [PATCH 22/22] add submodule dynamixelSDK --- .gitmodules | 3 +++ DynamixelSDK | 1 + 2 files changed, 4 insertions(+) create mode 100644 .gitmodules create mode 160000 DynamixelSDK 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/DynamixelSDK b/DynamixelSDK new file mode 160000 index 0000000..3450c70 --- /dev/null +++ b/DynamixelSDK @@ -0,0 +1 @@ +Subproject commit 3450c7078917b262d9b36042c15444047aae226e