diff --git a/.clang-format b/.clang-format index c6ba5550..8d8dc719 100644 --- a/.clang-format +++ b/.clang-format @@ -4,4 +4,6 @@ AccessModifierOffset: -4 CommentPragmas: 'NOLINT' PackConstructorInitializers: CurrentLine AlignOperands: AlignAfterOperator -AlignTrailingComments: true \ No newline at end of file +AlignTrailingComments: true +ColumnLimit: 0 +SeparateDefinitionBlocks: Always \ No newline at end of file diff --git a/src/wr_control_drive_arm/CMakeLists.txt b/src/wr_control_drive_arm/CMakeLists.txt index 19ef7fde..a122dafe 100644 --- a/src/wr_control_drive_arm/CMakeLists.txt +++ b/src/wr_control_drive_arm/CMakeLists.txt @@ -1,12 +1,12 @@ cmake_minimum_required(VERSION 3.8) project(wr_control_drive_arm) -## Compile as C++11, supported in ROS Kinetic and newer -# add_compile_options(-std=c++11) +# # Compile as C++11, supported in ROS Kinetic and newer +add_compile_options(-std=c++17) -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages +# # Find catkin macros and libraries +# # if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +# # is used, also find other catkin packages find_package(catkin REQUIRED COMPONENTS roscpp rospy @@ -15,201 +15,213 @@ find_package(catkin REQUIRED COMPONENTS sensor_msgs ) -## System dependencies are found with CMake's conventions +# # System dependencies are found with CMake's conventions # find_package(Boost REQUIRED COMPONENTS system) - -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# # Uncomment this if the package has a setup.py. This macro ensures +# # modules and global scripts declared therein get installed +# # See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html # catkin_python_setup() -################################################ -## Declare ROS messages, services and actions ## -################################################ - -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend tag for "message_generation" -## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependency has been pulled in -## but can be declared for certainty nonetheless: -## * add a exec_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) - -## Generate messages in the 'msg' folder +# ############################################### +# # Declare ROS messages, services and actions ## +# ############################################### + +# # To declare and build messages, services or actions from within this +# # package, follow these steps: +# # * Let MSG_DEP_SET be the set of packages whose message types you use in +# # your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +# # * In the file package.xml: +# # * add a build_depend tag for "message_generation" +# # * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET +# # * If MSG_DEP_SET isn't empty the following dependency has been pulled in +# # but can be declared for certainty nonetheless: +# # * add a exec_depend tag for "message_runtime" +# # * In this file (CMakeLists.txt): +# # * add "message_generation" and every package in MSG_DEP_SET to +# # find_package(catkin REQUIRED COMPONENTS ...) +# # * add "message_runtime" and every package in MSG_DEP_SET to +# # catkin_package(CATKIN_DEPENDS ...) +# # * uncomment the add_*_files sections below as needed +# # and list every .msg/.srv/.action file to be processed +# # * uncomment the generate_messages entry below +# # * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +# # Generate messages in the 'msg' folder # add_message_files( -# FILES -# Message1.msg -# Message2.msg +# FILES +# Message1.msg +# Message2.msg # ) -## Generate services in the 'srv' folder +# # Generate services in the 'srv' folder # add_service_files( -# FILES -# Service1.srv -# Service2.srv +# FILES +# Service1.srv +# Service2.srv # ) -## Generate actions in the 'action' folder +# # Generate actions in the 'action' folder # add_action_files( -# FILES -# Action1.action -# Action2.action +# FILES +# Action1.action +# Action2.action # ) -## Generate added messages and services with any dependencies listed here +# # Generate added messages and services with any dependencies listed here # generate_messages( -# DEPENDENCIES -# std_msgs # Or other packages containing msgs +# DEPENDENCIES +# std_msgs # Or other packages containing msgs # ) -################################################ -## Declare ROS dynamic reconfigure parameters ## -################################################ - -## To declare and build dynamic reconfigure parameters within this -## package, follow these steps: -## * In the file package.xml: -## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" -## * In this file (CMakeLists.txt): -## * add "dynamic_reconfigure" to -## find_package(catkin REQUIRED COMPONENTS ...) -## * uncomment the "generate_dynamic_reconfigure_options" section below -## and list every .cfg file to be processed - -## Generate dynamic reconfigure parameters in the 'cfg' folder +# ############################################### +# # Declare ROS dynamic reconfigure parameters ## +# ############################################### + +# # To declare and build dynamic reconfigure parameters within this +# # package, follow these steps: +# # * In the file package.xml: +# # * add a build_depend and a exec_depend tag for "dynamic_reconfigure" +# # * In this file (CMakeLists.txt): +# # * add "dynamic_reconfigure" to +# # find_package(catkin REQUIRED COMPONENTS ...) +# # * uncomment the "generate_dynamic_reconfigure_options" section below +# # and list every .cfg file to be processed + +# # Generate dynamic reconfigure parameters in the 'cfg' folder # generate_dynamic_reconfigure_options( -# cfg/DynReconf1.cfg -# cfg/DynReconf2.cfg +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg # ) -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if your package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need +# ################################## +# # catkin specific configuration ## +# ################################## +# # The catkin_package macro generates cmake config files for your package +# # Declare things to be passed to dependent projects +# # INCLUDE_DIRS: uncomment this if your package contains header files +# # LIBRARIES: libraries you create in this project that dependent projects also need +# # CATKIN_DEPENDS: catkin_packages dependent projects also need +# # DEPENDS: system dependencies of this project that dependent projects also need catkin_package( -# INCLUDE_DIRS include -# LIBRARIES wr_control_drive_science -# CATKIN_DEPENDS roscpp rospy -# DEPENDS system_lib + + # INCLUDE_DIRS include + # LIBRARIES wr_control_drive_science + # CATKIN_DEPENDS roscpp rospy + # DEPENDS system_lib ) -########### -## Build ## -########### +# ########## +# # Build ## +# ########## -## Specify additional locations of header files -## Your package locations should be listed before other locations +# # Specify additional locations of header files +# # Your package locations should be listed before other locations include_directories( -# include + + # include ${catkin_INCLUDE_DIRS} ) -## Declare a C++ library - add_library( ${PROJECT_NAME} - src/ArmMotor.cpp - src/AbstractJoint.cpp - src/DifferentialJoint.cpp - src/SimpleJoint.cpp - ) - -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure +# # Declare a C++ library +add_library(${PROJECT_NAME} + src/DifferentialJointToMotorSpeedConverter.cpp + src/DirectJointToMotorSpeedConverter.cpp + src/Joint.cpp + src/MathUtil.cpp + src/Motor.cpp + src/SingleEncoderJointPositionMonitor.cpp +) + +# # Add cmake target dependencies of the library +# # as an example, code may need to be generated before libraries +# # either from message generation or dynamic reconfigure # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -## Declare a C++ executable -## With catkin_make all packages are built within a single CMake context -## The recommended prefix ensures that target names across packages don't collide +# # Declare a C++ executable +# # With catkin_make all packages are built within a single CMake context +# # The recommended prefix ensures that target names across packages don't collide # add_executable(${PROJECT_NAME}_node src/wr_control_drive_arm_node.cpp) add_executable(ArmControlActionServer src/ArmControlActionServer.cpp) target_link_libraries(ArmControlActionServer ${catkin_LIBRARIES} ${PROJECT_NAME}) add_executable(JointStatePublisher src/JointStatePublisher.cpp) target_link_libraries(JointStatePublisher ${catkin_LIBRARIES} ${PROJECT_NAME}) - -## Rename C++ executable without prefix -## The above recommended prefix causes long target names, the following renames the -## target back to the shorter version for ease of user use -## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +add_executable(testDifferential src/testDifferential.cpp) +target_link_libraries(testDifferential ${catkin_LIBRARIES} ${PROJECT_NAME}) + +set_property(TARGET ArmControlActionServer PROPERTY CXX_STANDARD 17) +set_property(TARGET JointStatePublisher PROPERTY CXX_STANDARD 17) +set_property(TARGET ArmControlActionServer PROPERTY CXX_STANDARD_REQUIRED true) +set_property(TARGET JointStatePublisher PROPERTY CXX_STANDARD_REQUIRED true) +set_property(TARGET testDifferential PROPERTY CXX_STANDARD 17) +set_property(TARGET testDifferential PROPERTY CXX_STANDARD_REQUIRED true) + +# # Rename C++ executable without prefix +# # The above recommended prefix causes long target names, the following renames the +# # target back to the shorter version for ease of user use +# # e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") -## Add cmake target dependencies of the executable -## same as for the library above +# # Add cmake target dependencies of the executable +# # same as for the library above # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -## Specify libraries to link a library or executable target against +# # Specify libraries to link a library or executable target against # target_link_libraries(${PROJECT_NAME}_node -# ${catkin_LIBRARIES} +# ${catkin_LIBRARIES} # ) -############# -## Install ## -############# +# ############ +# # Install ## +# ############ # all install targets should use catkin DESTINATION variables # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination +# # Mark executable scripts (Python etc.) for installation +# # in contrast to setup.py, you can choose the destination # catkin_install_python(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} # ) -## Mark executables for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html +# # Mark executables for installation +# # See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html # install(TARGETS ${PROJECT_NAME}_node -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} # ) -## Mark libraries for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html +# # Mark libraries for installation +# # See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html # install(TARGETS ${PROJECT_NAME} -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} # ) -## Mark cpp header files for installation +# # Mark cpp header files for installation # install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE # ) -## Mark other files for installation (e.g. launch and bag files, etc.) +# # Mark other files for installation (e.g. launch and bag files, etc.) # install(FILES -# # myfile1 -# # myfile2 -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} # ) -############# -## Testing ## -############# +# ############ +# # Testing ## +# ############ -## Add gtest based cpp test target and link libraries +# # Add gtest based cpp test target and link libraries # catkin_add_gtest(${PROJECT_NAME}-test test/test_wr_control_drive_science.cpp) # if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) # endif() -## Add folders to be run by python nosetests +# # Add folders to be run by python nosetests # catkin_add_nosetests(test) diff --git a/src/wr_control_drive_arm/config/arm_motor_PID_mock.yaml b/src/wr_control_drive_arm/config/arm_motor_PID_mock.yaml index 88d33a6e..e59fd363 100644 --- a/src/wr_control_drive_arm/config/arm_motor_PID_mock.yaml +++ b/src/wr_control_drive_arm/config/arm_motor_PID_mock.yaml @@ -1,64 +1,55 @@ rate: 50 controllers: - - setpointTopic: "/control/arm/00/setpoint" - feedbackTopic: "/control/arm/00/feedback" - outputTopic: "/control/arm/00/output" - P: 100 - I: 0 + - setpointTopic: "/control/arm/pid/turntable/setpoint" + feedbackTopic: "/control/arm/pid/turntable/feedback" + outputTopic: "/control/arm/pid/turntable/output" + P: 5 + I: 0.5 D: 0 min: -1 max: 1 - - setpointTopic: "/control/arm/01/setpoint" - feedbackTopic: "/control/arm/01/feedback" - outputTopic: "/control/arm/01/output" - P: 100 - I: 0 + - setpointTopic: "/control/arm/pid/shoulder/setpoint" + feedbackTopic: "/control/arm/pid/shoulder/feedback" + outputTopic: "/control/arm/pid/shoulder/output" + P: 5 + I: 0.5 D: 0 min: -1 max: 1 - - setpointTopic: "/control/arm/10/setpoint" - feedbackTopic: "/control/arm/10/feedback" - outputTopic: "/control/arm/10/output" - P: 100 - I: 0 + - setpointTopic: "/control/arm/pid/elbow/setpoint" + feedbackTopic: "/control/arm/pid/elbow/feedback" + outputTopic: "/control/arm/pid/elbow/output" + P: 5 + I: 0.5 D: 0 min: -1 max: 1 - - setpointTopic: "/control/arm/11/setpoint" - feedbackTopic: "/control/arm/11/feedback" - outputTopic: "/control/arm/11/output" - P: 100 - I: 0 + - setpointTopic: "/control/arm/pid/forearmRoll/setpoint" + feedbackTopic: "/control/arm/pid/forearmRoll/feedback" + outputTopic: "/control/arm/pid/forearmRoll/output" + P: 5 + I: 0.5 D: 0 min: -1 max: 1 - - setpointTopic: "/control/arm/20/setpoint" - feedbackTopic: "/control/arm/20/feedback" - outputTopic: "/control/arm/20/output" - P: 100 - I: 0 + - setpointTopic: "/control/arm/pid/wristPitch/setpoint" + feedbackTopic: "/control/arm/pid/wristPitch/feedback" + outputTopic: "/control/arm/pid/wristPitch/output" + P: 5 + I: 0.5 D: 0 min: -1 max: 1 - - setpointTopic: "/control/arm/21/setpoint" - feedbackTopic: "/control/arm/21/feedback" - outputTopic: "/control/arm/21/output" - P: 100 - I: 0 - D: 0 - min: -1 - max: 1 - - - setpointTopic: "/control/arm/3000/setpoint" - feedbackTopic: "/control/arm/3000/feedback" - outputTopic: "/control/arm/3000/output" - P: 100 - I: 0 + - setpointTopic: "/control/arm/pid/wristRoll/setpoint" + feedbackTopic: "/control/arm/pid/wristRoll/feedback" + outputTopic: "/control/arm/pid/wristRoll/output" + P: 5 + I: 0.5 D: 0 min: -1 max: 1 diff --git a/src/wr_control_drive_arm/config/arm_motor_PID_real.yaml b/src/wr_control_drive_arm/config/arm_motor_PID_real.yaml index 108c62f4..a0da7801 100644 --- a/src/wr_control_drive_arm/config/arm_motor_PID_real.yaml +++ b/src/wr_control_drive_arm/config/arm_motor_PID_real.yaml @@ -1,63 +1,54 @@ rate: 50 controllers: - - setpointTopic: "/control/arm/00/setpoint" - feedbackTopic: "/control/arm/00/feedback" - outputTopic: "/control/arm/00/output" - P: 5 + - setpointTopic: "/control/arm/pid/turntable/setpoint" + feedbackTopic: "/control/arm/pid/turntable/feedback" + outputTopic: "/control/arm/pid/turntable/output" + P: 10 I: 0 D: 0 min: -1 max: 1 - - setpointTopic: "/control/arm/01/setpoint" - feedbackTopic: "/control/arm/01/feedback" - outputTopic: "/control/arm/01/output" - P: 5 + - setpointTopic: "/control/arm/pid/shoulder/setpoint" + feedbackTopic: "/control/arm/pid/shoulder/feedback" + outputTopic: "/control/arm/pid/shoulder/output" + P: 10 I: 0 D: 0 min: -1 max: 1 - - setpointTopic: "/control/arm/10/setpoint" - feedbackTopic: "/control/arm/10/feedback" - outputTopic: "/control/arm/10/output" - P: 5 + - setpointTopic: "/control/arm/pid/elbow/setpoint" + feedbackTopic: "/control/arm/pid/elbow/feedback" + outputTopic: "/control/arm/pid/elbow/output" + P: 10 I: 0 D: 0 min: -1 max: 1 - - setpointTopic: "/control/arm/11/setpoint" - feedbackTopic: "/control/arm/11/feedback" - outputTopic: "/control/arm/11/output" - P: 5 + - setpointTopic: "/control/arm/pid/forearmRoll/setpoint" + feedbackTopic: "/control/arm/pid/forearmRoll/feedback" + outputTopic: "/control/arm/pid/forearmRoll/output" + P: 10 I: 0 D: 0 min: -1 max: 1 - - setpointTopic: "/control/arm/20/setpoint" - feedbackTopic: "/control/arm/20/feedback" - outputTopic: "/control/arm/20/output" - P: 5 + - setpointTopic: "/control/arm/pid/wristPitch/setpoint" + feedbackTopic: "/control/arm/pid/wristPitch/feedback" + outputTopic: "/control/arm/pid/wristPitch/output" + P: 10 I: 0 D: 0 min: -1 max: 1 - - setpointTopic: "/control/arm/21/setpoint" - feedbackTopic: "/control/arm/21/feedback" - outputTopic: "/control/arm/21/output" - P: 5 - I: 0 - D: 0 - min: -1 - max: 1 - - - setpointTopic: "/control/arm/3000/setpoint" - feedbackTopic: "/control/arm/3000/feedback" - outputTopic: "/control/arm/3000/output" - P: 5 + - setpointTopic: "/control/arm/pid/wristRoll/setpoint" + feedbackTopic: "/control/arm/pid/wristRoll/feedback" + outputTopic: "/control/arm/pid/wristRoll/output" + P: 10 I: 0 D: 0 min: -1 diff --git a/src/wr_control_drive_arm/config/encoder_parameters_mock.yaml b/src/wr_control_drive_arm/config/encoder_parameters_mock.yaml index 5c855815..91e14a26 100644 --- a/src/wr_control_drive_arm/config/encoder_parameters_mock.yaml +++ b/src/wr_control_drive_arm/config/encoder_parameters_mock.yaml @@ -1,19 +1,19 @@ encoder_parameters: -# elbow - - counts_per_rotation: 1000 + elbow: + counts_per_rotation: 1000 offset: 500 -# forearm_roll - - counts_per_rotation: 1000 + forearmRoll: + counts_per_rotation: 1000 offset: 500 -# shoulder - - counts_per_rotation: 1000 + shoulder: + counts_per_rotation: 1000 offset: 500 -# turntable - - counts_per_rotation: 1000 + turntable: + counts_per_rotation: 1000 offset: 500 -# wrist_pitch - - counts_per_rotation: 1000 + wristPitch: + counts_per_rotation: 1000 offset: 500 -# wrist_roll - - counts_per_rotation: 1000 + wristRoll: + counts_per_rotation: 1000 offset: 500 diff --git a/src/wr_control_drive_arm/config/encoder_parameters_real.yaml b/src/wr_control_drive_arm/config/encoder_parameters_real.yaml index 1fce6c2e..b4d31f7c 100644 --- a/src/wr_control_drive_arm/config/encoder_parameters_real.yaml +++ b/src/wr_control_drive_arm/config/encoder_parameters_real.yaml @@ -1,19 +1,19 @@ encoder_parameters: -# elbow - - counts_per_rotation: -2048 - offset: 610 -# forearm_roll - - counts_per_rotation: -2048 - offset: 540 -# shoulder - - counts_per_rotation: -6144 - offset: 1129 -# turntable - - counts_per_rotation: 2048 - offset: 985 -# wrist_pitch - - counts_per_rotation: 2048 + elbow: + counts_per_rotation: 1850 + offset: 546 + forearmRoll: + counts_per_rotation: -1850 + offset: 576 + shoulder: + counts_per_rotation: 5550 + offset: 1281 + turntable: + counts_per_rotation: 6900 #7200 + offset: 90 + wristPitch: + counts_per_rotation: -1850 offset: 886 -# wrist_roll - - counts_per_rotation: 2048 - offset: 1050 + wristRoll: + counts_per_rotation: -1850 + offset: 981 diff --git a/src/wr_control_drive_arm/launch/jointstatepublisher.launch b/src/wr_control_drive_arm/launch/jointstatepublisher.launch index eb85aac1..8aa7f82c 100644 --- a/src/wr_control_drive_arm/launch/jointstatepublisher.launch +++ b/src/wr_control_drive_arm/launch/jointstatepublisher.launch @@ -1,9 +1,12 @@ - - - + + + \ No newline at end of file diff --git a/src/wr_control_drive_arm/launch/testDifferential.launch b/src/wr_control_drive_arm/launch/testDifferential.launch new file mode 100644 index 00000000..b6632b1a --- /dev/null +++ b/src/wr_control_drive_arm/launch/testDifferential.launch @@ -0,0 +1,3 @@ + + + \ No newline at end of file diff --git a/src/wr_control_drive_arm/src/AbstractJoint.cpp b/src/wr_control_drive_arm/src/AbstractJoint.cpp deleted file mode 100644 index e88e5afa..00000000 --- a/src/wr_control_drive_arm/src/AbstractJoint.cpp +++ /dev/null @@ -1,39 +0,0 @@ -/** - * @file AbstractJoint.cpp - * @author Nichols Underwood - * @brief Implementation of the AbstractJoint class - * @date 2021-10-25 - */ - -#include "AbstractJoint.hpp" - -AbstractJoint::AbstractJoint(ros::NodeHandle &n, int numMotors){ - this->motors.resize(numMotors); -} - -auto AbstractJoint::getDegreesOfFreedom() const -> unsigned int{ - return this->motors.size(); -} - -auto AbstractJoint::getMotor(int motorIndex) const -> const std::unique_ptr&{ - return this->motors.at(motorIndex).motor; -} - -void AbstractJoint::configSetpoint(int degreeIndex, double position, double velocity){ - - this->motors.at(degreeIndex).position = position; - this->motors.at(degreeIndex).velocity = velocity; -} - - -void AbstractJoint::exectute(){ - for(auto &motorHandle : motors){ - motorHandle.motor->runToTarget(motorHandle.position, motorHandle.velocity); - } -} - -void AbstractJoint::stopJoint(){ - for(auto &motorHandle : motors){ - motorHandle.motor->setPower(0.F); - } -} \ No newline at end of file diff --git a/src/wr_control_drive_arm/src/AbstractJoint.hpp b/src/wr_control_drive_arm/src/AbstractJoint.hpp deleted file mode 100644 index b395aa61..00000000 --- a/src/wr_control_drive_arm/src/AbstractJoint.hpp +++ /dev/null @@ -1,56 +0,0 @@ -/** - * @file AbstractJoint.hpp - * @author Nichols Underwood - * @brief Header file of the AbstractJoint class - * @date 2021-10-25 - */ - -#ifndef ABSTRACT_JOINT_GUARD -#define ABSTRACT_JOINT_GUARD - -#include "ArmMotor.hpp" -using std::vector; - -class AbstractJoint { -public: - struct MotorHandler{ - std::unique_ptr motor; - double position; - double velocity; - std::string jointTopicName; - std::string motorTopicName; - bool newVelocity; - }; - - AbstractJoint(ros::NodeHandle &n, int numMotors); - - // never used, need to be defined for compiler v-table - virtual auto getMotorPositions(const vector &jointPositions) -> vector = 0; - virtual auto getMotorVelocities(const vector &joinVelocities) -> vector = 0; - virtual auto getJointPositions(const vector &motorPositions) -> vector = 0; - - auto getDegreesOfFreedom() const -> unsigned int; - - auto getMotor(int motorIndex) const -> const std::unique_ptr&; - - void configSetpoint(int degreeIndex, double position, double velocity); - - void exectute(); - - void stopJoint(); - - virtual ~AbstractJoint() = default; - - AbstractJoint(const AbstractJoint&) = delete; - - auto operator=(const AbstractJoint&) -> AbstractJoint& = delete; - - AbstractJoint(AbstractJoint&&) = delete; - - auto operator=(AbstractJoint&&) -> AbstractJoint& = delete; - -protected: - vector motors; -}; - -#endif \ No newline at end of file diff --git a/src/wr_control_drive_arm/src/ArmControlActionServer.cpp b/src/wr_control_drive_arm/src/ArmControlActionServer.cpp index 9b660b61..14892cd3 100644 --- a/src/wr_control_drive_arm/src/ArmControlActionServer.cpp +++ b/src/wr_control_drive_arm/src/ArmControlActionServer.cpp @@ -4,61 +4,63 @@ * @brief The exeutable file to run the Arm Control Action Server * @date 2022-12-05 */ +#include "DifferentialJointToMotorSpeedConverter.hpp" +#include "DirectJointToMotorSpeedConverter.hpp" +#include "Joint.hpp" +#include "Motor.hpp" +#include "RoboclawChannel.hpp" +#include "SingleEncoderJointPositionMonitor.hpp" #include "XmlRpcValue.h" +#include "ros/init.h" -#include "DifferentialJoint.hpp" -#include "SimpleJoint.hpp" -#include "ros/console.h" -#include "ros/ros.h" #include #include #include +#include #include #include #include +#include #include #include #include #include #include +#include +#include +#include using XmlRpc::XmlRpcValue; /** * @brief Refresh rate of ros::Rate */ -constexpr float CLOCK_RATE = 50; +constexpr float CLOCK_RATE{50}; -constexpr double IK_WARN_RATE = 1.0 / 2; +constexpr double IK_WARN_RATE{1.0 / 2}; -constexpr double JOINT_SAFETY_MAX_SPEED = 0.5; -constexpr double JOINT_SAFETY_HOLD_SPEED = 0.15; +constexpr double JOINT_SAFETY_MAX_SPEED{0.5}; +constexpr double JOINT_SAFETY_HOLD_SPEED{0.3}; /** * @brief Nessage cache size of publisher */ -constexpr std::uint32_t MESSAGE_CACHE_SIZE = 1000; +constexpr std::uint32_t MESSAGE_CACHE_SIZE{10}; /** * @brief Period between timer callback */ -constexpr float TIMER_CALLBACK_DURATION = 1.0 / 50.0; +constexpr float TIMER_CALLBACK_DURATION{1.0 / 50.0}; /** * @brief Defines space for all Joint references */ -constexpr int NUM_JOINTS = 5; -std::array, NUM_JOINTS> joints; -// AbstractJoint *joints[numJoints]; -/** - * @brief The Joint State Publisher for MoveIt - */ -ros::Publisher jointStatePublisher; +std::unordered_map> namedJointMap; + /** * @brief Simplify the SimpleActionServer reference name */ -typedef actionlib::SimpleActionServer - Server; +using Server = actionlib::SimpleActionServer; /** * @brief The service server for enabling IK */ @@ -80,158 +82,92 @@ ros::ServiceClient enableServiceClient; * @param as The Action Server this is occuring on */ void execute(const control_msgs::FollowJointTrajectoryGoalConstPtr &goal, - Server *as) { + Server *server) { if (!IKEnabled) { - as->setAborted(); - ROS_WARN_THROTTLE( + server->setAborted(); + ROS_WARN_THROTTLE( // NOLINT(hicpp-no-array-decay,hicpp,hicpp-vararg,cppcoreguidelines-pro-bounds-array-to-pointer-decay, cppcoreguidelines-pro-type-vararg) IK_WARN_RATE, - "IK is disabled"); // NOLINT(hicpp-no-array-decay,hicpp,hicpp-vararg,cppcoreguidelines-pro-bounds-array-to-pointer-decay, cppcoreguidelines-pro-type-vararg) + "IK is disabled"); return; } - int currPoint = 1; - - std::cout << "start exec: " << goal->trajectory.points.size() << std::endl; - // For each point in the trajectory execution sequence... - for (const auto &name : goal->trajectory.joint_names) { - std::cout << name << "\t"; + for (const auto &jointName : goal->trajectory.joint_names) { + std::cout << jointName << "\t"; } std::cout << std::endl; - for (const auto &currTargetPosition : goal->trajectory.points) { - for (double pos : currTargetPosition.positions) { - std::cout << std::round(pos * 100) / 100 << "\t"; + for (const auto &waypoint : goal->trajectory.points) { + for (const auto &jointVal : waypoint.positions) { + std::cout << jointVal << "\t"; } std::cout << std::endl; } + for (const auto &currTargetPosition : goal->trajectory.points) { - // Track whether or not the current position is done - bool hasPositionFinished = false; - // Keep max loop rate at 50 Hz - ros::Rate loop{CLOCK_RATE}; + + if (!IKEnabled) { + server->setAborted(); + std::cout << "Over current fault!" << std::endl; + return; + } const double VELOCITY_MAX = abs(*std::max_element( currTargetPosition.velocities.begin(), currTargetPosition.velocities.end(), - [](double a, double b) -> bool { return abs(a) < abs(b); })); - - ArmMotor *currmotor = NULL; - int currItr = 0; - // std::cout << currPoint << " / " << goal->trajectory.points.size() << - // std::endl; - currPoint++; - for (const auto &joint : joints) { - for (int i = 0; i < joint->getDegreesOfFreedom(); i++) { - double velocity = - VELOCITY_MAX == 0.F - ? JOINT_SAFETY_HOLD_SPEED - : JOINT_SAFETY_MAX_SPEED * currTargetPosition.velocities[currItr] / VELOCITY_MAX; - std::cout << "config setpoint: " << - currTargetPosition.positions[currItr] << ":" << velocity << - std::endl; - joint->configSetpoint(i, currTargetPosition.positions[currItr], - velocity); - currItr++; - } - joint->exectute(); + [](double lhs, double rhs) -> bool { return abs(lhs) < abs(rhs); })); + + for (uint32_t i = 0; i < goal->trajectory.joint_names.size(); ++i) { + auto jointVelocity{JOINT_SAFETY_HOLD_SPEED}; + if (VELOCITY_MAX != 0) + jointVelocity = currTargetPosition.velocities.at(i) / VELOCITY_MAX * JOINT_SAFETY_MAX_SPEED; + + namedJointMap.at(goal->trajectory.joint_names.at(i))->setTarget(currTargetPosition.positions.at(i), jointVelocity); } + } - // While the current position is not complete yet... - while (!hasPositionFinished && ros::ok()) { - // Assume the current action is done until proven otherwise - hasPositionFinished = true; - // Create the Joint State message for the current update cycle - - // For each joint specified in the currTargetPosition... - for (const auto &joint : joints) { - - for (int k = 0; k < joint->getDegreesOfFreedom(); k++) { - // if (joint->getMotor(k)->getMotorState() == - // MotorState::MOVING) { - // std::cout << "Moving" << std::endl; - // } else if (joint->getMotor(k)->getMotorState() == - // MotorState::RUN_TO_TARGET) { - // std::cout << "Run to target" << std::endl; - // } else if (joint->getMotor(k)->getMotorState() == - // MotorState::STALLING) { - // std::cout << "Stalling" << std::endl; - // } else if (joint->getMotor(k)->getMotorState() == - // MotorState::STOP) { - // std::cout << "Stop" << std::endl; - // } else { - // std::cout << "Error" << std::endl; - // } - - if (joint->getMotor(k)->getMotorState() == - MotorState::STALLING) { - std::cout << "ACS stall detected" << std::endl; - IKEnabled = false; - std_srvs::Trigger srv; - if (enableServiceClient.call(srv)) { - ROS_WARN( - "%s", - (std::string{"PLACEHOLDER_NAME: "} + - srv.response.message) - .data()); // NOLINT(cppcoreguidelines-pro-bounds-array-to-pointer-decay, cppcoreguidelines-pro-type-vararg) - } else { - ROS_WARN( - "Error: failed to call service " - "PLACEHOLDER_NAME"); // NOLINT(cppcoreguidelines-pro-bounds-array-to-pointer-decay, cppcoreguidelines-pro-type-vararg) - } - } else { - hasPositionFinished &= - joint->getMotor(k)->getMotorState() == - MotorState::STOP; - std::cout << joint->getMotor(k)->getMotorName() << " state: " << (joint->getMotor(k)->getMotorState() == MotorState::STOP) << std::endl; - } - // DEBUGGING OUTPUT: Print each motor's name, radian - // position, encoder position, and power - // std::cout << std::setw(30) << joint->getMotor(k)->getRads() - // << std::endl; - } - - joint->exectute(); - - // if (!joint->exectute()) { - // IKEnabled = false; - // std_srvs::Trigger srv; - // if (enableServiceClient.call(srv)) { - // ROS_WARN("%s", (std::string{"PLACEHOLDER_NAME: "} + srv.response.message).data()); // NOLINT(cppcoreguidelines-pro-bounds-array-to-pointer-decay, cppcoreguidelines-pro-type-vararg) - // } else { - // ROS_WARN("Error: failed to call service PLACEHOLDER_NAME"); // NOLINT(cppcoreguidelines-pro-bounds-array-to-pointer-decay, cppcoreguidelines-pro-type-vararg) - // } - // return; - // } - } - // Sleep until the next update cycle - loop.sleep(); + auto waypointComplete{false}; + ros::Rate updateRate{CLOCK_RATE}; + + while (!waypointComplete && ros::ok() && !server->isNewGoalAvailable()) { + + if (!IKEnabled) { + server->setAborted(); + std::cout << "Over current fault!" << std::endl; + return; + } + + waypointComplete = true; + for (const auto &[_, joint] : namedJointMap) { + waypointComplete &= joint->hasReachedTarget(); } + updateRate.sleep(); } + // Report preemption if it occurred + if (server->isNewGoalAvailable()) + server->setPreempted(); // When all positions have been reached, set the current task as succeeded + else + server->setSucceeded(); + std::cout << "Action Complete!" << std::endl; +} - as->setSucceeded(); +auto getEncoderConfigFromParams(const XmlRpcValue ¶ms, const std::string &jointName) -> EncoderConfiguration { + return {.countsPerRotation = static_cast(params[jointName]["counts_per_rotation"]), + .offset = static_cast(params[jointName]["offset"])}; } -/** - * @brief publishes the arm's position - */ -void publishJointStates(const ros::TimerEvent &event) { - std::vector names; - std::vector positions; - sensor_msgs::JointState js_msg; - - for (const auto &joint : joints) { - for (int i = 0; i < joint->getDegreesOfFreedom(); i++) { - names.push_back(joint->getMotor(i)->getMotorName()); - positions.push_back(joint->getMotor(i)->getRads()); +void checkOverCurrentFaults(const std::vector> &motors){ + for(const auto& motor : motors){ + if (motor->isOverCurrent()) { + IKEnabled = false; + std::cout << "Over current fault!" << std::endl; + + for (const auto& joint : namedJointMap) { + joint.second->stop(); + } } + // TODO: arbitrate control to old arm driver } - - js_msg.name = names; - js_msg.position = positions; - js_msg.header.stamp = ros::Time::now(); - // Publish the Joint State message - jointStatePublisher.publish(js_msg); } /** @@ -255,47 +191,105 @@ auto main(int argc, char **argv) -> int { // Initialize all motors with their MoveIt name, WRoboclaw initialization, // and reference to the current node - auto elbowPitch_joint = std::make_unique( - "elbowPitch_joint", 1, 0, - static_cast(encParams[0]["counts_per_rotation"]), - static_cast(encParams[0]["offset"]), n); - auto elbowRoll_joint = std::make_unique( - "elbowRoll_joint", 1, 1, - static_cast(encParams[1]["counts_per_rotation"]), - static_cast(encParams[1]["offset"]), n); - auto shoulder_joint = std::make_unique( - "shoulder_joint", 0, 1, - static_cast(encParams[2]["counts_per_rotation"]), - static_cast(encParams[2]["offset"]), n); - auto turntable_joint = std::make_unique( - "turntable_joint", 0, 0, - static_cast(encParams[3]["counts_per_rotation"]), - static_cast(encParams[3]["offset"]), n); - auto wristPitch_joint = std::make_unique( - "wristPitch_joint", 2, 0, - static_cast(encParams[4]["counts_per_rotation"]), - static_cast(encParams[4]["offset"]), n); - auto wristRoll_link = std::make_unique( - "wristRoll_link", 2, 1, - static_cast(encParams[5]["counts_per_rotation"]), - static_cast(encParams[5]["offset"]), n); std::cout << "init motors" << std::endl; + /** + * @brief The list of motors + */ + std::vector> motors{}; + + using std::literals::string_literals::operator""s; + + const auto turntableMotor{std::make_shared("aux0"s, RoboclawChannel::A, n)}; + const auto shoulderMotor{std::make_shared("aux0"s, RoboclawChannel::B, n)}; + const auto elbowMotor{std::make_shared("aux1"s, RoboclawChannel::A, n)}; + const auto forearmRollMotor{std::make_shared("aux1"s, RoboclawChannel::B, n)}; + const auto wristLeftMotor{std::make_shared("aux2"s, RoboclawChannel::A, n)}; + const auto wristRightMotor{std::make_shared("aux2"s, RoboclawChannel::B, n)}; + + motors.push_back(turntableMotor); + motors.push_back(shoulderMotor); + motors.push_back(elbowMotor); + motors.push_back(forearmRollMotor); + motors.push_back(wristLeftMotor); + motors.push_back(wristRightMotor); + + const auto turntablePositionMonitor{std::make_shared( + "aux0"s, + RoboclawChannel::A, + getEncoderConfigFromParams(encParams, "turntable"), + n)}; + const auto shoulderPositionMonitor{std::make_shared( + "aux0"s, + RoboclawChannel::B, + getEncoderConfigFromParams(encParams, "shoulder"), + n)}; + const auto elbowPositionMonitor{std::make_shared( + "aux1"s, + RoboclawChannel::A, + getEncoderConfigFromParams(encParams, "elbow"), + n)}; + const auto forearmRollPositionMonitor{std::make_shared( + "aux1"s, + RoboclawChannel::B, + getEncoderConfigFromParams(encParams, "forearmRoll"), + n)}; + const auto wristPitchPositionMonitor{std::make_shared( + "aux2"s, + RoboclawChannel::A, + getEncoderConfigFromParams(encParams, "wristPitch"), + n)}; + const auto wristRollPositionMonitor{std::make_shared( + "aux2"s, + RoboclawChannel::B, + getEncoderConfigFromParams(encParams, "wristRoll"), + n)}; + + const auto turntableSpeedConverter{std::make_shared(turntableMotor, MotorSpeedDirection::REVERSE)}; + const auto shoulderSpeedConverter{std::make_shared(shoulderMotor, MotorSpeedDirection::REVERSE)}; + const auto elbowSpeedConverter{std::make_shared(elbowMotor, MotorSpeedDirection::REVERSE)}; + const auto forearmRollSpeedConverter{std::make_shared(forearmRollMotor, MotorSpeedDirection::REVERSE)}; + const auto differentialSpeedConverter{std::make_shared(wristLeftMotor, wristRightMotor)}; + // Initialize all Joints - joints.at(0) = - std::make_unique(std::move(elbowPitch_joint), n); - joints.at(1) = std::make_unique(std::move(elbowRoll_joint), n); - joints.at(2) = std::make_unique(std::move(shoulder_joint), n); - joints.at(3) = std::make_unique(std::move(turntable_joint), n); - joints.at(4) = std::make_unique( - std::move(wristPitch_joint), std::move(wristRoll_link), n, - "/control/arm/5/pitch", "/control/arm/5/roll", "/control/arm/20/", - "/control/arm/21/"); + std::cout << "init joints" << std::endl; + namedJointMap.insert({"turntable_joint", std::make_unique( + "turntable"s, + [turntablePositionMonitor]() -> double { return (*turntablePositionMonitor)(); }, + [turntableSpeedConverter](double speed) { (*turntableSpeedConverter)(speed); }, + n)}); + namedJointMap.insert({"shoulder_joint", std::make_unique( + "shoulder", + [shoulderPositionMonitor]() -> double { return (*shoulderPositionMonitor)(); }, + [shoulderSpeedConverter](double speed) { (*shoulderSpeedConverter)(speed); }, + n)}); + namedJointMap.insert({"elbowPitch_joint", std::make_unique( + "elbow", + [elbowPositionMonitor]() -> double { return (*elbowPositionMonitor)(); }, + [elbowSpeedConverter](double speed) { (*elbowSpeedConverter)(speed); }, + n)}); + namedJointMap.insert({"elbowRoll_joint", std::make_unique( + "forearmRoll", + [forearmRollPositionMonitor]() -> double { return (*forearmRollPositionMonitor)(); }, + [forearmRollSpeedConverter](double speed) { (*forearmRollSpeedConverter)(speed); }, + n)}); + namedJointMap.insert({"wristPitch_joint", std::make_unique( + "wristPitch", + [wristPitchPositionMonitor]() -> double { return (*wristPitchPositionMonitor)(); }, + [converter = differentialSpeedConverter](double speed) { converter->setPitchSpeed(speed); }, + n)}); + namedJointMap.insert({"wristRoll_link", std::make_unique( + "wristRoll", + [wristRollPositionMonitor]() -> double { return (*wristRollPositionMonitor)(); }, + [converter = differentialSpeedConverter](double speed) { converter->setRollSpeed(speed); }, + n)}); + // Initialize the Action Server - Server server(n, "/arm_controller/follow_joint_trajectory", - boost::bind(&execute, _1, &server), false); + Server server( + n, "/arm_controller/follow_joint_trajectory", + [&server](auto goal) { execute(goal, &server); }, false); // Start the Action Server server.start(); std::cout << "server started" << std::endl; @@ -315,6 +309,8 @@ auto main(int argc, char **argv) -> int { enableServiceClient = n.serviceClient("PLACEHOLDER_NAME"); + ros::Timer currentTimer = n.createTimer(ros::Duration{TIMER_CALLBACK_DURATION}, [&motors](const ros::TimerEvent& event) { checkOverCurrentFaults(motors); }); + std::cout << "entering ROS spin..." << std::endl; // ROS spin for communication with other nodes ros::spin(); diff --git a/src/wr_control_drive_arm/src/ArmMotor.cpp b/src/wr_control_drive_arm/src/ArmMotor.cpp deleted file mode 100644 index 10f25e05..00000000 --- a/src/wr_control_drive_arm/src/ArmMotor.cpp +++ /dev/null @@ -1,229 +0,0 @@ -/** - * @file ArmMotor.cpp - * @author Ben Nowotny - * @brief The implementation of the ArmMotor class - * @date 2021-04-05 - */ -#include "ArmMotor.hpp" -#include -#include -#include -#include - -/// Allow for referencing the UInt32 message type easier -using Std_UInt32 = std_msgs::UInt32::ConstPtr; -using Std_Float64 = std_msgs::Float64::ConstPtr; -using Std_Bool = std_msgs::Bool::ConstPtr; - -auto ArmMotor::corrMod(double i, double j) -> double { - // Stem i%j by j, which in modular arithmetic is the same as adding 0. - return std::fmod(std::fmod(std::abs(j) * i / j, std::abs(j)) + j, std::abs(j)); -} - -/// Currently consistent with the rad->enc equation as specified here. -auto ArmMotor::radToEnc(double rads) const -> uint32_t { - double remappedEnc = ArmMotor::corrMod(rads, 2 * M_PI) / (2 * M_PI); - return ArmMotor::corrMod((this->COUNTS_PER_ROTATION * remappedEnc) + this->ENCODER_OFFSET, abs(this->COUNTS_PER_ROTATION)); -} - -auto ArmMotor::encToRad(uint32_t enc) const -> double { - double remappedEnc = ArmMotor::corrMod(static_cast(enc - this->ENCODER_OFFSET), abs(static_cast(this->COUNTS_PER_ROTATION))) / this->COUNTS_PER_ROTATION; - return ArmMotor::corrMod(remappedEnc * 2 * M_PI + M_PI, 2 * M_PI) - M_PI; -} - -/// Currently consistent with the enc->rad equation as specified here. -auto ArmMotor::getRads() const -> double { - return ArmMotor::encToRad(this->getEncoderCounts()); -} - -void ArmMotor::storeEncoderVals(const Std_UInt32 &msg) { - // Store the message value in this ArmMotor's encoderVal variable - this->encoderVal = msg->data; - - // Send feedback - std_msgs::Float64 feedbackMsg; - feedbackMsg.data = ArmMotor::encToRad(msg->data); - if (!readOnly) - this->feedbackPub.publish(feedbackMsg); - - if (this->currState == MotorState::RUN_TO_TARGET) { - // std::cout << "[2] motor " << motorName << " position " << (hasReachedTarget(this->target) ? "at target " : "not at target ") << this->target << ":" << this->encoderVal << std::endl; - if (hasReachedTarget(this->target)) { - // std::cout << "[1] stop motor" << std::endl; - this->setPower(0.F, MotorState::STOP); - } - } -} - -void ArmMotor::redirectPowerOutput(const Std_Float64 &msg) { - // Set the speed to be the contained data - if (abs(msg->data) > 1) - std::cout << "Received power " << msg->data << " from PID for motor " << motorName << std::endl; - if (this->getMotorState() == MotorState::RUN_TO_TARGET) - this->setPower(static_cast(msg->data) * this->maxPower, MotorState::RUN_TO_TARGET); -} - -void ArmMotor::storeStallStatus(const Std_Bool &msg) { - if (static_cast(msg->data)) { - if ((ros::Time::now() - beginStallTime).toSec() >= STALL_THRESHOLD_TIME) { - std::cout << "over lim" << std::endl; - this->setPower(0.0F, MotorState::STALLING); - } - } else { - beginStallTime = ros::Time::now(); - } -} - -/// controllerID is constrained between [0,3] -/// motorID is constrained between [0,1] -ArmMotor::ArmMotor( - std::string motor_name, - unsigned int controllerID, - unsigned int motorID, - int64_t countsPerRotation, - int64_t offset, - ros::NodeHandle &n, - bool readOnly) : COUNTS_PER_ROTATION{countsPerRotation}, - ENCODER_BOUNDS{0, std::abs(countsPerRotation)}, - ENCODER_OFFSET{offset}, - motorName{std::move(motor_name)}, - controllerID{controllerID}, - motorID{motorID}, - currState{MotorState::STOP}, - power{0.F}, - maxPower{0.F}, - encoderVal{static_cast(offset)}, - readOnly{readOnly} { - - // Check validity of WRoboclaw and motor IDs - if (controllerID > 3) - throw std::invalid_argument{std::string{"Controller ID "} + std::to_string(controllerID) + "is only valid on [0,3]"}; - if (motorID > 1) - throw std::invalid_argument{std::string{"Motor ID "} + std::to_string(motorID) + " is only valid on [0,1]"}; - - // Create the topic string prefix for WRoboclaw controllers - std::string tpString = std::string{"/hsi/roboclaw/aux"} + std::to_string(controllerID); - std::string controlString = "/control/arm/" + std::to_string(controllerID) + std::to_string(motorID); - - // Create the appropriate encoder-reading and speed-publishing subscribers and advertisers, respectfully - this->encRead = n.subscribe(tpString + "/enc/" + (motorID == 0 ? "left" : "right"), ArmMotor::MESSAGE_CACHE_SIZE, &ArmMotor::storeEncoderVals, this); - if (!readOnly) { - this->speedPub = n.advertise(tpString + "/cmd/" + (motorID == 0 ? "left" : "right"), ArmMotor::MESSAGE_CACHE_SIZE); - this->targetPub = n.advertise(controlString + "/setpoint", ArmMotor::MESSAGE_CACHE_SIZE); - this->feedbackPub = n.advertise(controlString + "/feedback", ArmMotor::MESSAGE_CACHE_SIZE); - } - this->outputRead = n.subscribe(controlString + "/output", ArmMotor::MESSAGE_CACHE_SIZE, &ArmMotor::redirectPowerOutput, this); - this->stallRead = n.subscribe(tpString + "/curr/over_lim/" + (motorID == 0 ? "left" : "right"), ArmMotor::MESSAGE_CACHE_SIZE, &ArmMotor::storeStallStatus, this); - - std::cout << this->motorName << ": " << this->COUNTS_PER_ROTATION << std::endl; -} - -auto ArmMotor::getEncoderCounts() const -> uint32_t { - return this->encoderVal; -} - -void ArmMotor::runToTarget(uint32_t targetCounts, float power) { - // std::cout << "run to enc: " << targetCounts << std::endl; - this->runToTarget(targetCounts, power, false); -} - -auto ArmMotor::hasReachedTarget(uint32_t targetCounts, uint32_t tolerance) const -> bool { - // std::cout << "TOLERANCE: " << tolerance << std::endl; - // Compute the upper and lower bounds in the finite encoder space - int32_t lBound = ArmMotor::corrMod(static_cast(targetCounts - tolerance), static_cast(ArmMotor::ENCODER_BOUNDS[1])); - int32_t uBound = ArmMotor::corrMod(static_cast(targetCounts + tolerance), static_cast(ArmMotor::ENCODER_BOUNDS[1])); - // std::cout << "LBOUND: " << (lBound) << " UBOUND: " << (uBound) << std::endl; - auto position = ArmMotor::corrMod(getEncoderCounts(), ENCODER_BOUNDS[1]); - // std::cout << "POSITION RAW: " << encToRad(getEncoderCounts()) << "/" << getEncoderCounts() << std::endl; - // std::cout << "POSITION: " << encToRad(position) << "/" << position << std::endl; - // If the computed lower bound is lower than the upper bound, perform the computation normally - if (lBound < uBound) - return position <= uBound && position >= lBound; - // Otherwise, check if the value is outside either bound and negate the response - return position <= uBound || position >= lBound; -} - -/// Current tolerance is ±0.1 degree w.r.t. the current number of counts per rotation -auto ArmMotor::hasReachedTarget(uint32_t targetCounts) const -> bool { - auto tol = ArmMotor::TOLERANCE_RATIO * static_cast(std::abs(this->COUNTS_PER_ROTATION)); - return ArmMotor::hasReachedTarget(targetCounts, std::max(10.0, tol)); -} - -auto ArmMotor::getMotorState() const -> MotorState { - return this->currState; -} - -void ArmMotor::setPower(float power) { - this->setPower(power, power == 0.F ? MotorState::STOP : MotorState::MOVING); -} - -/// This method auto-publishes the speed command to the WRoboclaws -void ArmMotor::setPower(float power, MotorState state) { - // Check the bounds of the parameter - if (abs(power) > 1) - throw std::invalid_argument{std::string{"Power "} + std::to_string(power) + " is not on the interval [-1, 1]"}; - - if (!readOnly) { - // Set up and send the speed message - this->power = power; - this->powerMsg.data = power * INT16_MAX; - this->speedPub.publish(this->powerMsg); - // Update the cur.nt motor state based on the power command input - this->currState = state; - } -} - -void ArmMotor::runToTarget(uint32_t targetCounts, float power, bool block) { - this->target = targetCounts; - this->maxPower = abs(power); - - std_msgs::Float64 setpointMsg; - setpointMsg.data = ArmMotor::encToRad(targetCounts); - if (!readOnly) - this->targetPub.publish(setpointMsg); - - // If we are not at our target... - if (!this->hasReachedTarget(targetCounts)) { - // std::cout << "has not reached target" << std::endl; - // Set the power in the correct direction and continue running to the target - this->currState = MotorState::RUN_TO_TARGET; - - // long int direction = targetCounts - this->getEncoderCounts(); - // power = abs(power) * (corrMod(direction, ((long int)this->COUNTS_PER_ROTATION)) < corrMod(-direction, ((long int)this->COUNTS_PER_ROTATION)) ? 1 : -1); - // this->setPower(power, MotorState::RUN_TO_TARGET); - - // Otherwise, stop the motor - } else { - // std::cout << "has reached target" << std::endl; - this->setPower(0.F, MotorState::STOP); - } - // If this is a blocking call... - if (block) { - // Wait until the motor has reached the target, then stop - while (!this->hasReachedTarget(targetCounts)) - ; - this->setPower(0.F, MotorState::RUN_TO_TARGET); - } -} - -void ArmMotor::runToTarget(double rads, float power) { - // std::cout << "run to target: " << rads << ":" << this->radToEnc(rads) << ":" << this->getEncoderCounts() << std::endl; - - runToTarget(this->radToEnc(rads), power, false); -} - -auto ArmMotor::getMotorName() const -> std::string { - return this->motorName; -} - -auto ArmMotor::getMotorID() const -> unsigned int { - return this->motorID; -} - -auto ArmMotor::getControllerID() const -> unsigned int { - return this->controllerID; -} - -auto ArmMotor::getPower() const -> float { - return this->power; -} \ No newline at end of file diff --git a/src/wr_control_drive_arm/src/ArmMotor.hpp b/src/wr_control_drive_arm/src/ArmMotor.hpp deleted file mode 100644 index 9ad871c2..00000000 --- a/src/wr_control_drive_arm/src/ArmMotor.hpp +++ /dev/null @@ -1,259 +0,0 @@ -/** - * @file ArmMotor.hpp - * @author Ben Nowotny - * @brief Header file of the ArmMotor class - * @date 2021-04-05 - */ - -#include "ros/ros.h" -#include "std_msgs/Bool.h" -#include "std_msgs/Float64.h" -#include "std_msgs/Int16.h" -#include "std_msgs/UInt32.h" -#include -#include -#include - -/** - * @brief An enumeration of states for a motor to be in. - */ -enum class MotorState { - /// A Motor is stopped (not moving, 0 power command) - STOP, - /// A Motor is moving (non-0 power command) - MOVING, - /// A Motor is running to a given target - RUN_TO_TARGET, - /// A Motor is stalling (over safety current limit) - STALLING -}; - -/** - * @brief A way to control arm motors with WRoboclaw - */ -class ArmMotor { -private: - /// Time tolerance of over-current detection before a stall fault is - /// triggered - static constexpr float STALL_THRESHOLD_TIME = 0.5; - /// Tolerance ratio w.r.t the counts per rotation for encoder - /// run-to-position motion - static constexpr double TOLERANCE_RATIO = 0.1 / 360; - /// Size of ROS Topic message caches - static constexpr uint32_t MESSAGE_CACHE_SIZE = 10; - /// The number of encoder counts per rotation - int64_t const COUNTS_PER_ROTATION; - /// The upper and lower bounds of encoder rotation for absolute encoders - /// (index 0 is lower, index 1 is upper) - std::array const ENCODER_BOUNDS; - /// zero position of motor - int64_t const ENCODER_OFFSET; - /// The current state of the motor - std::atomic currState; - /// The joint name of the current motor - std::string motorName; - /// The ID of the WRoboclaw controller - unsigned int controllerID; - /// The ID of the motor within the WRoboclaw controller - unsigned int motorID; - /// The current encoder value - uint32_t encoderVal; - /// Target encoder counts for run to target - uint32_t target; - /// The ROS Subscriber that reads the encoders - ros::Subscriber encRead; - /// The ROS Publisher that sets the encoder targets - ros::Publisher targetPub; - /// The ROS Publisher that sets encoder feedback data - ros::Publisher feedbackPub; - /// The ROS Subscriber that reads controlled output data - ros::Subscriber outputRead; - /// The ROS Publisher that publishes motor speed commands - ros::Publisher speedPub; - /// The most recent power message sent - std_msgs::Int16 powerMsg; - /// The ROS Subscriber that reads stall status data - ros::Subscriber stallRead; - /// The time when the motor began stalling - ros::Time beginStallTime; - /// Motor power - float power; - /// Maximum absolute motor power in RUN_TO_POSITION mode. - std::atomic maxPower; - /// Whether or not this motor is read-only - bool readOnly; - - /** - * @brief A static conversion from radians to encoder counts - * - * @param rad The input number of radians - * @return uint32_t The corresponding encoder count bounded by - * ENCODER_BOUNDS - */ - auto radToEnc(double rad) const -> uint32_t; - - /** - * @brief A static conversion from encoder counts to radians - * - * @param enc The input number of encoder counts - * @return double The corresponding radian measure - */ - auto encToRad(uint32_t enc) const -> double; - - /** - * @brief Subscriber callback for encRead, captures the encoder value of the - * current motor - * - * @param msg The encoder value message as captured by encRead - */ - void storeEncoderVals(const std_msgs::UInt32::ConstPtr &msg); - - /** - * @brief Subscriber callback for outputRead, captures the PID output and - * sets the speed directly - * - * @param msg The PID output as captured by outputRead - */ - void redirectPowerOutput(const std_msgs::Float64::ConstPtr &msg); - - /** - * @brief Subscriber callback for stallRead, captures the stall status of - * the current motor - * - * @param msg The stall status of the current motor - */ - void storeStallStatus(const std_msgs::Bool::ConstPtr &msg); - - void setPower(float power, MotorState state); - - /** - * @brief Performs Euclidean correct modulus between two inputs of the same - * type - * - * @param i The dividend of the modulus - * @param j The divisor of the modulus - * @return double The Euclidean-correct remainder bounded on [0, j) - */ - static auto corrMod(double i, double j) -> double; - -public: - /** - * @brief Constructs a new ArmMotor - * - * @param motorName The joint name of the motor - * @param controllerID The WRoboclaw controller ID for this motor - * @param motorID The motor ID within its WRoboclaw controller - * @param n A NodeHandle reference to the constructing Node - * @param readOnly whether this motor will inhibit control messages - */ - ArmMotor(std::string motorName, unsigned int controllerID, - unsigned int motorID, int64_t countsPerRotation, int64_t offset, - ros::NodeHandle &n, bool readOnly = false); - - /** - * @brief Gets the encoder value of the motor - * - * @return uint32_t The current encoder value of the motor - */ - auto getEncoderCounts() const -> uint32_t; - - /** - * @brief Sends the motor to run to a target encoder value at a given power - * without blocking - * - * @param targetCounts The target encoder value for the motor - * @param power The power to move the motor at (Bounded between [-1, 1]) - */ - void runToTarget(uint32_t targetCounts, float power); - - /** - * @brief Sends the motor to run to a target encoder value at a given power - * - * @param targetCounts The target encoder value for the motor - * @param power The power to move the motor at (Bounded between [-1, 1]) - * @param block Specifies whether or not this action should block until it - * is complete - * @return True if the motor had stalled, and false otherwise - */ - void runToTarget(uint32_t targetCounts, float power, bool block); - - /** - * @brief Sends the motor to run to a specified position at a given power - * - * @param rads The position to send the motor to (specified in radians) - * @param power The power to move the motor at (Bounded between [-1, 1]) - * @return True if the motor had stalled, and false otherwise - */ - void runToTarget(double rads, float power); - - /** - * @brief Get the current state of the ArmMotor - * - * @return MotorState The current state of the ArmMotor - */ - auto getMotorState() const -> MotorState; - - /** - * @brief Set the motor power - * - * @param power The power to set the motor at (Bounded between [-1, 1]) - */ - void setPower(float power); - - /** - * @brief Get the radian measure of the current motor - * - * @return double The radian measure of the current motor's position - */ - auto getRads() const -> double; - - /** - * @brief Get the name of the ArmMotor - * - * @return std::string The name of the ArmMotor - */ - auto getMotorName() const -> std::string; - - /** - * @brief Get the name of the ArmMotor - * - * @return std::string The controller ID of the ArmMotor - */ - auto getControllerID() const -> unsigned int; - - /** - * @brief Get the name of the ArmMotor - * - * @return std::string The motor ID of the ArmMotor - */ - auto getMotorID() const -> unsigned int; - - /** - * @brief Checks if the motor is currently within a pre-specified tolerance - * of a target - * - * @param targetCounts The target to test against - * @return true The motor was within the target tolerance - * @return false The motor was outside of the target tolerance - */ - auto hasReachedTarget(uint32_t targetCounts) const -> bool; - - /** - * @brief Checks if the motor is currently within a given tolerance of a - * target - * - * @param targetCounts The target to test against - * @param tolerance The tolerance to give when testing the target - * @return true The motor was within the target tolerance - * @return false The motor was outside the target tolerance - */ - auto hasReachedTarget(uint32_t targetCounts, uint32_t tolerance) const - -> bool; - - /** - * @brief Get the most recently set motor power - * - * @return float Last motor power setting - */ - auto getPower() const -> float; -}; diff --git a/src/wr_control_drive_arm/src/DifferentialJoint.cpp b/src/wr_control_drive_arm/src/DifferentialJoint.cpp deleted file mode 100644 index 809a5772..00000000 --- a/src/wr_control_drive_arm/src/DifferentialJoint.cpp +++ /dev/null @@ -1,129 +0,0 @@ -/** - * @file DifferentialJoint.cpp - * @author Nicholas Underwood - * @brief Header file of the DifferentialJoint class - * @date 2021-10-25 - */ - -#include "DifferentialJoint.hpp" -#include -using std::vector; - -constexpr std::array, 2> DifferentialJoint::MOTOR_TO_JOINT_MATRIX; -constexpr std::array, 2> DifferentialJoint::JOINT_TO_MOTOR_MATRIX; - -DifferentialJoint::DifferentialJoint(std::unique_ptr leftMotor, std::unique_ptr rightMotor, ros::NodeHandle &n, - const std::string &pitchTopicName, const std::string &rollTopicName, - const std::string &leftTopicName, const std::string &rightTopicName) - : AbstractJoint(n, DEGREES_OF_FREEDOM) { - this->motors.at(0) = {std::move(leftMotor), 0, 0, "", "", false}; - this->motors.at(1) = {std::move(rightMotor), 0, 0, "", "", false}; - - // TODO: This should NOT be bypassing the motors in handling PID. This should do frame conversions and pass the results into the motors to handle the actual ROS comms - this->pitchOutputSubscriber = n.subscribe(pitchTopicName + "/output", MESSAGE_CACHE_SIZE, &DifferentialJoint::handoffPitchOutput, this); - this->rollOutputSubscriber = n.subscribe(rollTopicName + "/output", MESSAGE_CACHE_SIZE, &DifferentialJoint::handoffRollOutput, this); - this->leftOutputPublisher = n.advertise(leftTopicName + "/output", MESSAGE_CACHE_SIZE); - this->rightOutputPublisher = n.advertise(rightTopicName + "/output", MESSAGE_CACHE_SIZE); - this->leftFeedbackSubscriber = n.subscribe(leftTopicName + "/feeback", MESSAGE_CACHE_SIZE, &DifferentialJoint::handoffLeftFeedback, this); - this->rightFeedbackSubscriber = n.subscribe(rightTopicName + "/feeback", MESSAGE_CACHE_SIZE, &DifferentialJoint::handoffRightFeedback, this); - this->pitchFeedbackPublisher = n.advertise(pitchTopicName + "/feedback", MESSAGE_CACHE_SIZE); - this->rollFeedbackPublisher = n.advertise(rollTopicName + "/feedback", MESSAGE_CACHE_SIZE); -} - -auto DifferentialJoint::getJointPositions(const vector &motorPositions) -> vector{ - // vector positions; - // target->reserve(2); - - double pitch = motorPositions.at(0) * MOTOR_TO_JOINT_MATRIX.at(0).at(0) + motorPositions.at(1) * MOTOR_TO_JOINT_MATRIX.at(0).at(1); - double roll = motorPositions.at(0) * MOTOR_TO_JOINT_MATRIX.at(1).at(0) + motorPositions.at(1) * MOTOR_TO_JOINT_MATRIX.at(1).at(1); - - //TODO: Why not return a new vector? Wouldn't this constant resizing be more inefficient than just making/returning a new vector via copy ellision? - return {pitch, roll}; - - // return positions; -} - -auto DifferentialJoint::getMotorPositions(const vector &jointPositions) -> vector{ - - // std::unique_ptr> positions = std::make_unique>(2); - // target->reserve(2); - - double left = jointPositions.at(0) * JOINT_TO_MOTOR_MATRIX.at(0).at(0) + jointPositions.at(1) * JOINT_TO_MOTOR_MATRIX.at(0).at(1); - double right = jointPositions.at(0) * JOINT_TO_MOTOR_MATRIX.at(1).at(0) + jointPositions.at(1) * JOINT_TO_MOTOR_MATRIX.at(1).at(1); - - return {left, right}; - - // return std::move(positions); -} - -auto DifferentialJoint::getMotorVelocities(const vector &jointPositions) -> vector{ - return getMotorPositions(jointPositions); //deritivate of linear transformation is itself -} - -void DifferentialJoint::handoffPitchOutput(const std_msgs::Float64::ConstPtr &msg){ - this->cachedPitchOutput = msg->data; - this->hasNewPitchOutput = true; - handOffAllOutput(); -} -void DifferentialJoint::handoffRollOutput(const std_msgs::Float64::ConstPtr &msg){ - this->cachedRollOutput = msg->data; - this->hasNewRollOutput = true; - handOffAllOutput(); -} - -// TODO: Do these ouputs need to be synced? We're just referring to the motors individually, maybe we can transform and send the data right away -void DifferentialJoint::handOffAllOutput(){ - if(!this->hasNewPitchOutput || !this->hasNewRollOutput){ return; } - - vector outputs; - outputs.resize(2); - outputs.at(0) = this->cachedPitchOutput; - outputs.at(1) = this->cachedRollOutput; - //TODO: Still confused as to the purpose of this family of functions - outputs = getMotorVelocities(outputs); - - std_msgs::Float64 msg1; - std_msgs::Float64 msg2; - msg1.data = outputs.at(0); - msg2.data = outputs.at(1); - pitchFeedbackPublisher.publish(msg1); - rollFeedbackPublisher.publish(msg2); - - leftOutputPublisher.publish(msg1); - rightOutputPublisher.publish(msg2); - - this->hasNewPitchOutput = false; - this->hasNewRollOutput = false; - -} - -void DifferentialJoint::handoffRightFeedback(const std_msgs::Float64::ConstPtr &msg){ - this->cachedRightFeedback = msg->data; - this->hasNewRightFeedback = true; - handOffAllFeedback(); -} -void DifferentialJoint::handoffLeftFeedback(const std_msgs::Float64::ConstPtr &msg){ - this->cachedLeftFeedback = msg->data; - this->hasNewLeftFeedback = true; - handOffAllFeedback(); -} - -void DifferentialJoint::handOffAllFeedback(){ - if(!this->hasNewLeftFeedback || !this->hasNewRightFeedback){ return; } - - vector outputs; - outputs.at(0) = this->cachedLeftFeedback; - outputs.at(1) = this->cachedRightFeedback; - outputs = getMotorVelocities(outputs); - - std_msgs::Float64 msg1; - std_msgs::Float64 msg2; - msg1.data = outputs.at(0); - msg2.data = outputs.at(1); - pitchFeedbackPublisher.publish(msg1); - rollFeedbackPublisher.publish(msg2); - - this->hasNewLeftFeedback = false; - this->hasNewRightFeedback = false; - -} diff --git a/src/wr_control_drive_arm/src/DifferentialJoint.hpp b/src/wr_control_drive_arm/src/DifferentialJoint.hpp deleted file mode 100644 index 7bfaec4f..00000000 --- a/src/wr_control_drive_arm/src/DifferentialJoint.hpp +++ /dev/null @@ -1,56 +0,0 @@ -/** - * @file DifferentialJoint.hpp - * @author Nichols Underwood - * @brief Header file of the DifferentialJoint class - * @date 2021-10-25 - */ - -#include "AbstractJoint.hpp" - -class DifferentialJoint : public AbstractJoint { - public: - DifferentialJoint(std::unique_ptr leftMotor, std::unique_ptr rightMotor, ros::NodeHandle &n, - const std::string &pitchTopicName, const std::string &rollTopicName, - const std::string &leftTopicName, const std::string &rightTopicName); - - auto getMotorPositions(const vector &jointPositions) -> vector override; - auto getMotorVelocities(const vector &jointVelocities) -> vector override; - auto getJointPositions(const vector &motorPositions) -> vector override; - - private: - static constexpr uint32_t DEGREES_OF_FREEDOM = 2; - static constexpr uint32_t MESSAGE_CACHE_SIZE = 10; - // linear transformations works for position and velocity - // [0.5 0.5] [left motor ] [pitch] - // [ -1 1 ] * [right motor] = [roll ] - // double motorToJointMatrix[2][2] = {{0.5, 0.5}, {-1, 1}}; - static constexpr std::array, 2> MOTOR_TO_JOINT_MATRIX{{{1, 0}, {0, 1}}}; - // [1 -0.5] [pitch] [left motor ] - // [1 0.5] * [roll ] = [right motor] - // double jointToMotorMatrix[2][2] = {{1, 0.5}, {1, -0.5}}; - static constexpr std::array, 2> JOINT_TO_MOTOR_MATRIX{{{1, 0}, {0, 1}}}; - - void handoffPitchOutput(const std_msgs::Float64::ConstPtr&); - void handoffRollOutput(const std_msgs::Float64::ConstPtr&); - void handOffAllOutput(); - ros::Subscriber pitchOutputSubscriber; - ros::Subscriber rollOutputSubscriber; - ros::Publisher leftOutputPublisher; - ros::Publisher rightOutputPublisher; - float cachedPitchOutput = 0.0; - float cachedRollOutput = 0.0; - bool hasNewPitchOutput = false; - bool hasNewRollOutput = false; - - void handoffLeftFeedback(const std_msgs::Float64::ConstPtr&); - void handoffRightFeedback(const std_msgs::Float64::ConstPtr&); - void handOffAllFeedback(); - ros::Subscriber leftFeedbackSubscriber; - ros::Subscriber rightFeedbackSubscriber; - ros::Publisher pitchFeedbackPublisher; - ros::Publisher rollFeedbackPublisher; - float cachedLeftFeedback = 0.0; - float cachedRightFeedback = 0.0; - bool hasNewLeftFeedback = false; - bool hasNewRightFeedback = false; -}; \ No newline at end of file diff --git a/src/wr_control_drive_arm/src/DifferentialJointToMotorSpeedConverter.cpp b/src/wr_control_drive_arm/src/DifferentialJointToMotorSpeedConverter.cpp new file mode 100644 index 00000000..dfe28450 --- /dev/null +++ b/src/wr_control_drive_arm/src/DifferentialJointToMotorSpeedConverter.cpp @@ -0,0 +1,43 @@ +#include "DifferentialJointToMotorSpeedConverter.hpp" +#include + +DifferentialJointToMotorSpeedConverter::DifferentialJointToMotorSpeedConverter( + std::shared_ptr leftMotor, + std::shared_ptr rightMotor) + : cachedPitchSpeed{0}, + cachedRollSpeed{0}, + leftMotor{std::move(leftMotor)}, + rightMotor{std::move(rightMotor)} {} + +DifferentialJointToMotorSpeedConverter::DifferentialJointToMotorSpeedConverter( + const DifferentialJointToMotorSpeedConverter &other) + : cachedPitchSpeed{other.cachedPitchSpeed.load()}, + cachedRollSpeed{other.cachedRollSpeed.load()}, + leftMotor{other.leftMotor}, + rightMotor{other.rightMotor} {} + +DifferentialJointToMotorSpeedConverter::DifferentialJointToMotorSpeedConverter( + DifferentialJointToMotorSpeedConverter &&other) noexcept + : cachedPitchSpeed{other.cachedPitchSpeed.load()}, + cachedRollSpeed{other.cachedRollSpeed.load()}, + leftMotor{other.leftMotor}, + rightMotor{other.rightMotor} {} + +void DifferentialJointToMotorSpeedConverter::setPitchSpeed(double speed) { + cachedPitchSpeed = speed; + dispatchDifferentialSpeed(); +} + +void DifferentialJointToMotorSpeedConverter::setRollSpeed(double speed) { + cachedRollSpeed = speed; + dispatchDifferentialSpeed(); +} + +void DifferentialJointToMotorSpeedConverter::dispatchDifferentialSpeed() { + // const std::lock_guard guard{mutex}; + auto m1Speed{-cachedPitchSpeed + (cachedRollSpeed * AVERAGE_SCALING_FACTOR)}; + auto m2Speed{-cachedPitchSpeed - (cachedRollSpeed * AVERAGE_SCALING_FACTOR)}; + leftMotor->setSpeed(m1Speed); + rightMotor->setSpeed(m2Speed); + // std::cout << "ptch: " << cachedPitchSpeed.load() << "\troll: " << cachedRollSpeed.load() << "\tlspd: " << m1Speed << "\trspd: " << m2Speed << std::endl; +} \ No newline at end of file diff --git a/src/wr_control_drive_arm/src/DifferentialJointToMotorSpeedConverter.hpp b/src/wr_control_drive_arm/src/DifferentialJointToMotorSpeedConverter.hpp new file mode 100644 index 00000000..5d096add --- /dev/null +++ b/src/wr_control_drive_arm/src/DifferentialJointToMotorSpeedConverter.hpp @@ -0,0 +1,31 @@ +#ifndef DIFFERENTIAL_JOINT_TO_MOTOR_SPEED_CONVERTER_H +#define DIFFERENTIAL_JOINT_TO_MOTOR_SPEED_CONVERTER_H + +#include "Motor.hpp" + +class DifferentialJointToMotorSpeedConverter { +public: + DifferentialJointToMotorSpeedConverter(std::shared_ptr leftMotor, std::shared_ptr rightMotor); + void setPitchSpeed(double speed); + void setRollSpeed(double speed); + + DifferentialJointToMotorSpeedConverter(const DifferentialJointToMotorSpeedConverter &); + auto operator=(const DifferentialJointToMotorSpeedConverter &) -> DifferentialJointToMotorSpeedConverter & = delete; + DifferentialJointToMotorSpeedConverter(DifferentialJointToMotorSpeedConverter &&) noexcept; + auto operator=(DifferentialJointToMotorSpeedConverter &&) -> DifferentialJointToMotorSpeedConverter & = delete; + ~DifferentialJointToMotorSpeedConverter() = default; + +private: + std::atomic cachedPitchSpeed; + std::atomic cachedRollSpeed; + std::recursive_mutex mutex; + + const std::shared_ptr leftMotor; + const std::shared_ptr rightMotor; + + static constexpr double AVERAGE_SCALING_FACTOR{1}; + + void dispatchDifferentialSpeed(); +}; + +#endif \ No newline at end of file diff --git a/src/wr_control_drive_arm/src/DirectJointToMotorSpeedConverter.cpp b/src/wr_control_drive_arm/src/DirectJointToMotorSpeedConverter.cpp new file mode 100644 index 00000000..67fd0568 --- /dev/null +++ b/src/wr_control_drive_arm/src/DirectJointToMotorSpeedConverter.cpp @@ -0,0 +1,18 @@ +#include "DirectJointToMotorSpeedConverter.hpp" + +DirectJointToMotorSpeedConverter::DirectJointToMotorSpeedConverter(std::shared_ptr outputMotor, MotorSpeedDirection direction) + : outputMotor{std::move(outputMotor)}, + direction{direction} {} + +void DirectJointToMotorSpeedConverter::operator()(double speed) { + double actualSpeed{0}; // In event of enum error, stop the motor + switch (direction) { + case MotorSpeedDirection::FORWARD: + actualSpeed = speed; + break; + case MotorSpeedDirection::REVERSE: + actualSpeed = -speed; + break; + } + outputMotor->setSpeed(actualSpeed); +} diff --git a/src/wr_control_drive_arm/src/DirectJointToMotorSpeedConverter.hpp b/src/wr_control_drive_arm/src/DirectJointToMotorSpeedConverter.hpp new file mode 100644 index 00000000..6a117435 --- /dev/null +++ b/src/wr_control_drive_arm/src/DirectJointToMotorSpeedConverter.hpp @@ -0,0 +1,28 @@ +#ifndef DIRECT_JOINT_TO_MOTOR_SPEED_CONVERTER_H +#define DIRECT_JOINT_TO_MOTOR_SPEED_CONVERTER_H + +#include "Motor.hpp" +#include + +enum class MotorSpeedDirection { + FORWARD, + REVERSE +}; + +class DirectJointToMotorSpeedConverter { +public: + DirectJointToMotorSpeedConverter(std::shared_ptr outputMotor, MotorSpeedDirection direction); + void operator()(double speed); + + DirectJointToMotorSpeedConverter(const DirectJointToMotorSpeedConverter &) = default; + auto operator=(const DirectJointToMotorSpeedConverter &) -> DirectJointToMotorSpeedConverter & = delete; + DirectJointToMotorSpeedConverter(DirectJointToMotorSpeedConverter &&) = default; + auto operator=(DirectJointToMotorSpeedConverter &&) -> DirectJointToMotorSpeedConverter & = delete; + ~DirectJointToMotorSpeedConverter() = default; + +private: + const MotorSpeedDirection direction; + const std::shared_ptr outputMotor; +}; + +#endif \ No newline at end of file diff --git a/src/wr_control_drive_arm/src/Joint.cpp b/src/wr_control_drive_arm/src/Joint.cpp new file mode 100644 index 00000000..08b7c8a3 --- /dev/null +++ b/src/wr_control_drive_arm/src/Joint.cpp @@ -0,0 +1,72 @@ +#include "Joint.hpp" +#include "MathUtil.hpp" +#include +#include + +using std::literals::string_literals::operator""s; +using MathUtil::RADIANS_PER_ROTATION; + +Joint::Joint(std::string name, + std::function positionMonitor, + std::function motorSpeedDispatcher, + ros::NodeHandle node) + : name{std::move(name)}, + positionMonitor{std::move(positionMonitor)}, + motorSpeedDispatcher{std::move(motorSpeedDispatcher)}, + executeMotion{false}, + controlLoopUpdateTimer{node.createTimer(ros::Rate{FEEDBACK_UPDATE_FREQUENCY_HZ}, &Joint::onFeedbackUpdateEvent, this, false, false)}, + controlLoopOutputSubscriber{node.subscribe("/control/arm/pid/"s + this->name + "/output", 1, &Joint::onControlLoopOutput, this)}, + controlLoopSetpointPublisher{node.advertise("/control/arm/pid/"s + this->name + "/setpoint", 1)}, + controlLoopFeedbackPublisher{node.advertise("/control/arm/pid/"s + this->name + "/feedback", 1)} {} + +void Joint::setTarget(double target, double maxSpeed) { + this->target = target; + this->maxSpeed = maxSpeed; + executeMotion = true; + controlLoopUpdateTimer.start(); +} + +auto Joint::hasReachedTarget() const -> bool { + // Copy-on-read to avoid inconsistencies on asynchronous change + auto currentTarget = target.load(); + + // Compute the upper and lower bounds in the finite encoder space + auto lBound = MathUtil::corrMod(currentTarget - JOINT_TOLERANCE_RADIANS, RADIANS_PER_ROTATION); + auto uBound = MathUtil::corrMod(currentTarget + JOINT_TOLERANCE_RADIANS, RADIANS_PER_ROTATION); + auto position = MathUtil::corrMod(positionMonitor(), RADIANS_PER_ROTATION); + // If the computed lower bound is lower than the upper bound, perform the computation normally + if (lBound < uBound) + return position <= uBound && position >= lBound; + // Otherwise, check if the value is outside either bound and negate the response + return position <= uBound || position >= lBound; +} + +auto Joint::getName() const -> std::string { + return name; +} + +void Joint::stop() { + executeMotion = false; + controlLoopUpdateTimer.stop(); + motorSpeedDispatcher(0); +} + +void Joint::onControlLoopOutput(const std_msgs::Float64::ConstPtr &msg) { + auto cappedPowerUnsigned{std::min(std::abs(msg->data), std::abs(maxSpeed))}; + double cappedPower{0}; + if (msg->data != 0) + cappedPower = (std::abs(msg->data) / msg->data) * cappedPowerUnsigned; + motorSpeedDispatcher(executeMotion ? cappedPower : 0); +} + +void Joint::onFeedbackUpdateEvent(const ros::TimerEvent &event) { + // Setpoint + std_msgs::Float64 setpointMsg; + setpointMsg.data = target; + controlLoopSetpointPublisher.publish(setpointMsg); + + // Feedback + std_msgs::Float64 feedbackMsg; + feedbackMsg.data = positionMonitor(); + controlLoopFeedbackPublisher.publish(feedbackMsg); +} \ No newline at end of file diff --git a/src/wr_control_drive_arm/src/Joint.hpp b/src/wr_control_drive_arm/src/Joint.hpp new file mode 100644 index 00000000..659a4f98 --- /dev/null +++ b/src/wr_control_drive_arm/src/Joint.hpp @@ -0,0 +1,39 @@ +#ifndef JOINT_H +#define JOINT_H + +#include "ros/publisher.h" +#include "ros/subscriber.h" +#include "ros/timer.h" +#include "std_msgs/Float64.h" +#include +#include + +class Joint { +public: + explicit Joint(std::string name, std::function positionMonitor, std::function motorSpeedDispatcher, ros::NodeHandle node); + void setTarget(double target, double maxSpeed); + [[nodiscard]] auto hasReachedTarget() const -> bool; + [[nodiscard]] auto getName() const -> std::string; + void stop(); + +private: + static constexpr double FEEDBACK_UPDATE_FREQUENCY_HZ{50}; + static constexpr double JOINT_TOLERANCE_RADIANS{3 * M_PI / 180}; + + const std::string name; + const std::function positionMonitor; + const std::function motorSpeedDispatcher; + + void onControlLoopOutput(const std_msgs::Float64::ConstPtr &msg); + void onFeedbackUpdateEvent(const ros::TimerEvent &event); + + std::atomic target; + std::atomic maxSpeed; + std::atomic executeMotion; + ros::Timer controlLoopUpdateTimer; + ros::Subscriber controlLoopOutputSubscriber; + ros::Publisher controlLoopSetpointPublisher; + ros::Publisher controlLoopFeedbackPublisher; +}; + +#endif \ No newline at end of file diff --git a/src/wr_control_drive_arm/src/JointStatePublisher.cpp b/src/wr_control_drive_arm/src/JointStatePublisher.cpp index 3638c4ff..3a93cba6 100644 --- a/src/wr_control_drive_arm/src/JointStatePublisher.cpp +++ b/src/wr_control_drive_arm/src/JointStatePublisher.cpp @@ -5,12 +5,10 @@ * @brief The executable file to run the joint state publisher * @date 2022-12-05 */ + +#include "SingleEncoderJointPositionMonitor.hpp" #include "XmlRpcValue.h" -#include "DifferentialJoint.hpp" -#include "SimpleJoint.hpp" -#include "ros/console.h" -#include "ros/ros.h" #include #include #include @@ -28,34 +26,28 @@ using XmlRpc::XmlRpcValue; /** * @brief Nessage cache size of publisher */ -constexpr std::uint32_t MESSAGE_CACHE_SIZE = 1000; +constexpr std::uint32_t MESSAGE_CACHE_SIZE = 1; /** * @brief Period between timer callback */ constexpr float TIMER_CALLBACK_DURATION = 1.0 / 50.0; -/** - * @brief Defines space for all Joint references - */ -constexpr int NUM_JOINTS = 5; -std::array, NUM_JOINTS> joints; - /** * @brief The joint state publisher for MoveIt */ ros::Publisher jointStatePublisher; +std::map namedJointPositionMonitors; + void publishJointStates(const ros::TimerEvent &event) { std::vector names; std::vector positions; sensor_msgs::JointState js_msg; - for (const auto &joint : joints) { - for (int i = 0; i < joint->getDegreesOfFreedom(); i++) { - names.push_back(joint->getMotor(i)->getMotorName()); - positions.push_back(joint->getMotor(i)->getRads()); - } + for (auto &[name, monitor] : namedJointPositionMonitors) { + names.push_back(name); + positions.push_back(monitor()); } js_msg.name = names; @@ -65,6 +57,11 @@ void publishJointStates(const ros::TimerEvent &event) { jointStatePublisher.publish(js_msg); } +auto getEncoderConfigFromParams(const XmlRpcValue ¶ms, const std::string &jointName) -> EncoderConfiguration { + return {.countsPerRotation = static_cast(params[jointName]["counts_per_rotation"]), + .offset = static_cast(params[jointName]["offset"])}; +} + /** * @brief The main executable method of the node. Starts the ROS node * @@ -75,31 +72,50 @@ void publishJointStates(const ros::TimerEvent &event) { auto main(int argc, char **argv) -> int { std::cout << "start main" << std::endl; - // Initialize the current node as JointStatePublisherApplication + // // Initialize the current node as JointStatePublisherApplication ros::init(argc, argv, "JointStatePublisher"); - // Create the NodeHandle to the current ROS node + // // Create the NodeHandle to the current ROS node ros::NodeHandle n; ros::NodeHandle pn{"~"}; XmlRpcValue encParams; pn.getParam("encoder_parameters", encParams); - // Initialize all motors with their MoveIt name, WRoboclaw initialization, and reference to the current node - auto elbowPitch_joint = std::make_unique("elbowPitch_joint", 1, 0, static_cast(encParams[0]["counts_per_rotation"]), static_cast(encParams[0]["offset"]), n, true); - auto elbowRoll_joint = std::make_unique("elbowRoll_joint", 1, 1, static_cast(encParams[1]["counts_per_rotation"]), static_cast(encParams[1]["offset"]), n, true); - auto shoulder_joint = std::make_unique("shoulder_joint", 0, 1, static_cast(encParams[2]["counts_per_rotation"]), static_cast(encParams[2]["offset"]), n, true); - auto turntable_joint = std::make_unique("turntable_joint", 0, 0, static_cast(encParams[3]["counts_per_rotation"]), static_cast(encParams[3]["offset"]), n, true); - auto wristPitch_joint = std::make_unique("wristPitch_joint", 2, 0, static_cast(encParams[4]["counts_per_rotation"]), static_cast(encParams[4]["offset"]), n, true); - auto wristRoll_link = std::make_unique("wristRoll_link", 2, 1, static_cast(encParams[5]["counts_per_rotation"]), static_cast(encParams[5]["offset"]), n, true); - std::cout << "init motors" << std::endl; - - // Initialize all Joints - joints.at(0) = std::make_unique(std::move(elbowPitch_joint), n); - joints.at(1) = std::make_unique(std::move(elbowRoll_joint), n); - joints.at(2) = std::make_unique(std::move(shoulder_joint), n); - joints.at(3) = std::make_unique(std::move(turntable_joint), n); - joints.at(4) = std::make_unique(std::move(wristPitch_joint), std::move(wristRoll_link), n, "/control/arm/5/pitch", "/control/arm/5/roll", "/control/arm/20/", "/control/arm/21/"); - std::cout << "init joints" << std::endl; + namedJointPositionMonitors.try_emplace("elbowPitch_joint", + + "aux1", + RoboclawChannel::A, + getEncoderConfigFromParams(encParams, "elbow"), + n); + namedJointPositionMonitors.try_emplace("elbowRoll_joint", + + "aux1", + RoboclawChannel::B, + getEncoderConfigFromParams(encParams, "forearmRoll"), + n); + namedJointPositionMonitors.try_emplace("shoulder_joint", + + "aux0", + RoboclawChannel::B, + getEncoderConfigFromParams(encParams, "shoulder"), + n); + namedJointPositionMonitors.try_emplace("turntable_joint", + + "aux0", + RoboclawChannel::A, + getEncoderConfigFromParams(encParams, "turntable"), + n); + namedJointPositionMonitors.try_emplace("wristPitch_joint", + + "aux2", + RoboclawChannel::A, + getEncoderConfigFromParams(encParams, "wristPitch"), + n); + namedJointPositionMonitors.try_emplace("wristRoll_link", + "aux2", + RoboclawChannel::B, + getEncoderConfigFromParams(encParams, "wristRoll"), + n); // Initialize the Joint State Data Publisher jointStatePublisher = n.advertise("/joint_states", MESSAGE_CACHE_SIZE); diff --git a/src/wr_control_drive_arm/src/MathUtil.cpp b/src/wr_control_drive_arm/src/MathUtil.cpp new file mode 100644 index 00000000..682b0bad --- /dev/null +++ b/src/wr_control_drive_arm/src/MathUtil.cpp @@ -0,0 +1,8 @@ +#include "MathUtil.hpp" +#include + +namespace MathUtil { +auto corrMod(double dividend, double divisor) -> double { + return std::fmod(std::fmod(dividend, divisor) + divisor, std::abs(divisor)); +} +} // namespace MathUtil \ No newline at end of file diff --git a/src/wr_control_drive_arm/src/MathUtil.hpp b/src/wr_control_drive_arm/src/MathUtil.hpp new file mode 100644 index 00000000..07e7b0d0 --- /dev/null +++ b/src/wr_control_drive_arm/src/MathUtil.hpp @@ -0,0 +1,11 @@ +#ifndef MATH_UTIL_H +#define MATH_UTIL_H + +#include + +namespace MathUtil { +auto corrMod(double dividend, double divisor) -> double; +static constexpr double RADIANS_PER_ROTATION{2 * M_PI}; +} // namespace MathUtil + +#endif \ No newline at end of file diff --git a/src/wr_control_drive_arm/src/Motor.cpp b/src/wr_control_drive_arm/src/Motor.cpp new file mode 100644 index 00000000..bc96cd0f --- /dev/null +++ b/src/wr_control_drive_arm/src/Motor.cpp @@ -0,0 +1,45 @@ +#include "Motor.hpp" +#include "RoboclawChannel.hpp" +#include "ros/node_handle.h" +#include "ros/time.h" +#include "std_msgs/Int16.h" +#include +#include +#include +#include + +using std::literals::string_literals::operator""s; + +Motor::Motor(const std::string &controllerName, RoboclawChannel channel, ros::NodeHandle node) + : motorSpeedPublisher{ + node.advertise( + "/hsi/roboclaw/"s + controllerName + "/cmd/"s + (channel == RoboclawChannel::A ? "left" : "right"), + 1)}, + currentOverLimitSubscriber{ + node.subscribe("/hsi/roboclaw/"s + controllerName + "/curr/over_lim/" + (channel == RoboclawChannel::A ? "left" : "right"), + OVER_CURRENT_QUEUE_SIZE, &Motor::setCurrentStatus, this)} {} + +void Motor::setSpeed(double speed) { + if (abs(speed) > 1) + throw std::invalid_argument("speed must be between -1 and 1"); + + std_msgs::Int16 powerMsg{}; + powerMsg.data = static_cast(speed * INT16_MAX); + motorSpeedPublisher.publish(powerMsg); +} + +void Motor::setCurrentStatus(const std_msgs::Bool::ConstPtr &msg) { + bool overCurrent = static_cast(msg->data); + if (overCurrent && !this->beginStallTime.has_value()) { + this->beginStallTime = ros::Time::now(); + } else if (!overCurrent && this->beginStallTime.has_value()) { + this->beginStallTime = std::nullopt; + } +} + +auto Motor::isOverCurrent() const -> bool { + if (this->beginStallTime.has_value()) { + return static_cast((ros::Time::now() - this->beginStallTime.value()).toSec() >= STALL_THRESHOLD_TIME); + } + return false; +} diff --git a/src/wr_control_drive_arm/src/Motor.hpp b/src/wr_control_drive_arm/src/Motor.hpp new file mode 100644 index 00000000..ff3a6174 --- /dev/null +++ b/src/wr_control_drive_arm/src/Motor.hpp @@ -0,0 +1,30 @@ +#ifndef MOTOR_H +#define MOTOR_H + +#include "RoboclawChannel.hpp" +#include "ros/publisher.h" +#include "ros/subscriber.h" +#include "std_msgs/Bool.h" +#include +#include + +class Motor { +public: + Motor(const std::string &controllerName, RoboclawChannel channel, ros::NodeHandle node); + void setSpeed(double speed); + auto isOverCurrent() const -> bool; + +private: + + static constexpr float STALL_THRESHOLD_TIME{0.5F}; + static constexpr int OVER_CURRENT_QUEUE_SIZE{25}; + + + void setCurrentStatus(const std_msgs::Bool::ConstPtr &msg); + + ros::Publisher motorSpeedPublisher; + ros::Subscriber currentOverLimitSubscriber; + std::optional beginStallTime; +}; + +#endif diff --git a/src/wr_control_drive_arm/src/RoboclawChannel.hpp b/src/wr_control_drive_arm/src/RoboclawChannel.hpp new file mode 100644 index 00000000..fbf7de00 --- /dev/null +++ b/src/wr_control_drive_arm/src/RoboclawChannel.hpp @@ -0,0 +1,6 @@ +#ifndef ROBOCLAW_CHANNEL_H +#define ROBOCLAW_CHANNEL_H + +enum class RoboclawChannel { A, B }; + +#endif \ No newline at end of file diff --git a/src/wr_control_drive_arm/src/SimpleJoint.cpp b/src/wr_control_drive_arm/src/SimpleJoint.cpp deleted file mode 100644 index d7ca2ee2..00000000 --- a/src/wr_control_drive_arm/src/SimpleJoint.cpp +++ /dev/null @@ -1,24 +0,0 @@ -/** - * @file SimpleJoint.cpp - * @author Nichols Underwood - * @brief Implementation of the SimpleJoint class - * @date 2021-10-25 - */ - -#include "SimpleJoint.hpp" - -SimpleJoint::SimpleJoint(std::unique_ptr motor, ros::NodeHandle &n) : AbstractJoint(n, 1) { - this->motors.at(0) = MotorHandler{std::move(motor), 0, 0, "", "", false}; -} - -auto SimpleJoint::getMotorPositions(const vector &jointPositions) -> vector { - return {jointPositions[0]}; -} - -auto SimpleJoint::getMotorVelocities(const vector &jointVelocities) -> vector { - return {jointVelocities[0]}; -} - -auto SimpleJoint::getJointPositions(const vector &jointPositions) -> vector { - return {jointPositions[0]}; -} \ No newline at end of file diff --git a/src/wr_control_drive_arm/src/SimpleJoint.hpp b/src/wr_control_drive_arm/src/SimpleJoint.hpp deleted file mode 100644 index 2c1bf76b..00000000 --- a/src/wr_control_drive_arm/src/SimpleJoint.hpp +++ /dev/null @@ -1,18 +0,0 @@ -/** - * @file SimpleJoint.hpp - * @author Nichols Underwood - * @brief Header file of the SimpleJoint class - * @date 2021-10-25 - */ - -#include "AbstractJoint.hpp" - -using std::vector; - -class SimpleJoint : public AbstractJoint { - public: - SimpleJoint(std::unique_ptr motor, ros::NodeHandle& n); - auto getMotorPositions(const vector &jointPositions) -> vector override; - auto getMotorVelocities(const vector &joinVelocities) -> vector override; - auto getJointPositions(const vector &motorPositions) -> vector override; -}; diff --git a/src/wr_control_drive_arm/src/SingleEncoderJointPositionMonitor.cpp b/src/wr_control_drive_arm/src/SingleEncoderJointPositionMonitor.cpp new file mode 100644 index 00000000..9c179dc8 --- /dev/null +++ b/src/wr_control_drive_arm/src/SingleEncoderJointPositionMonitor.cpp @@ -0,0 +1,46 @@ +#include "SingleEncoderJointPositionMonitor.hpp" +#include "MathUtil.hpp" +#include "RoboclawChannel.hpp" +#include "ros/node_handle.h" + +using std::literals::string_literals::operator""s; +using MathUtil::RADIANS_PER_ROTATION; + +SingleEncoderJointPositionMonitor::SingleEncoderJointPositionMonitor( + const std::string &controllerName, + RoboclawChannel channel, + EncoderConfiguration config, + ros::NodeHandle node) + : countsPerRotation{config.countsPerRotation}, + offset{config.offset}, + position{0}, + encoderSubscriber{ + std::make_shared(node.subscribe( + "/hsi/roboclaw/"s + controllerName + "/enc/" + (channel == RoboclawChannel::A ? "left" : "right"), + 1, + &SingleEncoderJointPositionMonitor::onEncoderReceived, + this))} {} + +SingleEncoderJointPositionMonitor::SingleEncoderJointPositionMonitor( + const SingleEncoderJointPositionMonitor &other) + : countsPerRotation(other.countsPerRotation), + offset(other.offset), + encoderSubscriber{other.encoderSubscriber}, + position{other.position.load()} {} + +SingleEncoderJointPositionMonitor::SingleEncoderJointPositionMonitor( + SingleEncoderJointPositionMonitor &&other) noexcept + : countsPerRotation(other.countsPerRotation), + offset(other.offset), + encoderSubscriber{std::move(other.encoderSubscriber)}, + position{other.position.load()} {} + +auto SingleEncoderJointPositionMonitor::operator()() -> double { + return position; +} + +void SingleEncoderJointPositionMonitor::onEncoderReceived(const std_msgs::UInt32::ConstPtr &msg) { + auto enc = static_cast(msg->data); + auto rotations = MathUtil::corrMod(enc - offset, countsPerRotation) / countsPerRotation; + position = MathUtil::corrMod(rotations * RADIANS_PER_ROTATION + M_PI, RADIANS_PER_ROTATION) - M_PI; +} diff --git a/src/wr_control_drive_arm/src/SingleEncoderJointPositionMonitor.hpp b/src/wr_control_drive_arm/src/SingleEncoderJointPositionMonitor.hpp new file mode 100644 index 00000000..cdf9938e --- /dev/null +++ b/src/wr_control_drive_arm/src/SingleEncoderJointPositionMonitor.hpp @@ -0,0 +1,36 @@ +#ifndef SINGLE_ENCODER_JOINT_POSITION_MONITOR_H +#define SINGLE_ENCODER_JOINT_POSITION_MONITOR_H + +#include "RoboclawChannel.hpp" +#include +#include +#include +#include +#include + +struct EncoderConfiguration { + int32_t countsPerRotation; + int32_t offset; +}; + +class SingleEncoderJointPositionMonitor { +public: + SingleEncoderJointPositionMonitor(const std::string &controllerName, RoboclawChannel channel, EncoderConfiguration config, ros::NodeHandle node); + auto operator()() -> double; + + SingleEncoderJointPositionMonitor(const SingleEncoderJointPositionMonitor &); + auto operator=(const SingleEncoderJointPositionMonitor &) -> SingleEncoderJointPositionMonitor & = delete; + SingleEncoderJointPositionMonitor(SingleEncoderJointPositionMonitor &&) noexcept; + auto operator=(SingleEncoderJointPositionMonitor &&) -> SingleEncoderJointPositionMonitor & = delete; + ~SingleEncoderJointPositionMonitor() = default; + +private: + void onEncoderReceived(const std_msgs::UInt32::ConstPtr &msg); + + std::atomic position; + std::shared_ptr encoderSubscriber; + const int32_t countsPerRotation; + const int32_t offset; +}; + +#endif \ No newline at end of file diff --git a/src/wr_control_drive_arm/src/testDifferential.cpp b/src/wr_control_drive_arm/src/testDifferential.cpp new file mode 100644 index 00000000..5b2d31a6 --- /dev/null +++ b/src/wr_control_drive_arm/src/testDifferential.cpp @@ -0,0 +1,26 @@ +#include +#include "Motor.hpp" +#include "DifferentialJointToMotorSpeedConverter.hpp" +#include "ros/init.h" +#include + +auto main(int32_t argc, char** argv) -> int32_t{ + ros::init(argc, argv, "testNode"); + + ros::NodeHandle n{}; + using std::literals::string_literals::operator""s; + + const auto wristLeftMotor{std::make_shared("aux2"s, RoboclawChannel::A, n)}; + const auto wristRightMotor{std::make_shared("aux2"s, RoboclawChannel::B, n)}; + + const auto differentialSpeedConverter{std::make_shared(wristLeftMotor, wristRightMotor)}; + + ros::Rate loopRate{50}; + while(ros::ok()){ + differentialSpeedConverter->setPitchSpeed(0.3); + differentialSpeedConverter->setRollSpeed(0); + loopRate.sleep(); + } + + return 0; +} \ No newline at end of file diff --git a/src/wr_entry_point/launch/test/mock_arm.launch b/src/wr_entry_point/launch/test/mock_arm.launch index 67abb1df..6dfbb8b1 100644 --- a/src/wr_entry_point/launch/test/mock_arm.launch +++ b/src/wr_entry_point/launch/test/mock_arm.launch @@ -1,21 +1,24 @@ - - - - + + + + + + + - - - - + + + + - - - - - - - - + + + + + + + + \ No newline at end of file diff --git a/src/wr_logic_teleop_arm/src/ArmTeleopLogic.cpp b/src/wr_logic_teleop_arm/src/ArmTeleopLogic.cpp index 5c1b684e..9cdbb880 100644 --- a/src/wr_logic_teleop_arm/src/ArmTeleopLogic.cpp +++ b/src/wr_logic_teleop_arm/src/ArmTeleopLogic.cpp @@ -1,3 +1,4 @@ +#include "boost/function.hpp" #include "geometry_msgs/PoseStamped.h" #include "geometry_msgs/Quaternion.h" #include "moveit_msgs/RobotTrajectory.h" @@ -5,26 +6,25 @@ #include "ros/subscriber.h" #include "std_msgs/Bool.h" #include "std_msgs/Float32.h" -#include "boost/function.hpp" #include "tf2/LinearMath/Quaternion.h" #include "tf2/LinearMath/Vector3.h" #include "tf2_geometry_msgs/tf2_geometry_msgs.h" #include #include #include -#include #include #include +#include #include -using Std_Bool = const std_msgs::BoolConstPtr&; -using Std_Float32 = const std_msgs::Float32ConstPtr&; +using Std_Bool = const std_msgs::BoolConstPtr &; +using Std_Float32 = const std_msgs::Float32ConstPtr &; -std::atomic_bool isNewPath { true }; -const tf2::Quaternion WORLD_OFFSET {0, sin(M_PI/2), 0, cos(M_PI/2)}; +std::atomic_bool isNewPath{true}; +const tf2::Quaternion WORLD_OFFSET{0, sin(M_PI / 2), 0, cos(M_PI / 2)}; -auto updateTarget(float x_pos, float y_pos, float z_pos, tf2::Quaternion orientation, ros::Publisher &pub) -> void{ - geometry_msgs::PoseStamped p {}; +auto updateTarget(float x_pos, float y_pos, float z_pos, tf2::Quaternion orientation, ros::Publisher &pub) -> void { + geometry_msgs::PoseStamped p{}; p.pose.position.x = x_pos; p.pose.position.y = y_pos; p.pose.position.z = z_pos; @@ -34,15 +34,15 @@ auto updateTarget(float x_pos, float y_pos, float z_pos, tf2::Quaternion orienta pub.publish(p); } -auto main(int argc, char** argv) -> int{ +auto main(int argc, char **argv) -> int { ros::init(argc, argv, "ArmTeleopLogic"); ros::AsyncSpinner spin(1); spin.start(); - ros::NodeHandle np("~"); + ros::NodeHandle np("~"); constexpr float PLANNING_TIME = 0.05; constexpr float CLOCK_RATE = 2; - constexpr uint32_t MESSAGE_QUEUE_LENGTH = 1000; + constexpr uint32_t MESSAGE_QUEUE_LENGTH = 1000; constexpr float TRIGGER_PRESSED = 0.5; constexpr float JOYSTICK_DEADBAND = 0.1; constexpr float MINIMUM_PATH_ACCURACY = 0.0; @@ -55,7 +55,6 @@ auto main(int argc, char** argv) -> int{ constexpr float HOME_Y = -0.7; constexpr float HOME_Z = 0.2; - float accel = 1.0; float deaccel = 1.0; float step_mult = 1.0; @@ -63,171 +62,160 @@ auto main(int argc, char** argv) -> int{ float y_pos = HOME_Y; float z_pos = HOME_Z; - tf2::Quaternion orientation {sin(M_PI/4), 0, 0, cos(-M_PI/4)}; + tf2::Quaternion orientation{sin(M_PI / 4), 0, 0, cos(-M_PI / 4)}; orientation = orientation; - const tf2::Quaternion SPIN_X {sin(2*M_PI/1000), 0, 0, cos(2*M_PI/1000)}; - const tf2::Quaternion SPIN_Y {0, sin(2*M_PI/1000), 0, cos(2*M_PI/1000)}; - const tf2::Quaternion SPIN_Z {0, 0, sin(2*M_PI/1000), cos(2*M_PI/1000)}; + const tf2::Quaternion SPIN_X{sin(2 * M_PI / 1000), 0, 0, cos(2 * M_PI / 1000)}; + const tf2::Quaternion SPIN_Y{0, sin(2 * M_PI / 1000), 0, cos(2 * M_PI / 1000)}; + const tf2::Quaternion SPIN_Z{0, 0, sin(2 * M_PI / 1000), cos(2 * M_PI / 1000)}; moveit::planning_interface::MoveGroupInterface move("arm"); // move.setPlannerId("RRTStar"); move.setPlanningTime(PLANNING_TIME); - const moveit::core::JointModelGroup* joint_model_group = move.getCurrentState()->getJointModelGroup("arm"); + const moveit::core::JointModelGroup *joint_model_group = move.getCurrentState()->getJointModelGroup("arm"); robot_state::RobotState start_state(*move.getCurrentState()); moveit_visual_tools::MoveItVisualTools visual_tools("arm"); + ros::Rate loop{CLOCK_RATE}; - ros::Rate loop {CLOCK_RATE}; + ros::Publisher nextTarget = np.advertise("/logic/arm_teleop/next_target", + MESSAGE_QUEUE_LENGTH); - ros::Publisher nextTarget = np.advertise("/logic/arm_teleop/next_target", - MESSAGE_QUEUE_LENGTH); - - ros::Publisher trajectoryPub = np.advertise("/logic/arm_teleop/trajectory", - MESSAGE_QUEUE_LENGTH); - + ros::Publisher trajectoryPub = np.advertise("/logic/arm_teleop/trajectory", + MESSAGE_QUEUE_LENGTH); // y axis - ros::Subscriber yAxis = np.subscribe("/hci/arm/gamepad/axis/stick_left_y", - MESSAGE_QUEUE_LENGTH, - static_cast>([&](Std_Float32 msg) -> void { - if(abs(msg->data) >= JOYSTICK_DEADBAND){ - y_pos += msg->data * STEP_Y; - updateTarget(x_pos, y_pos, z_pos, orientation, nextTarget); - isNewPath.store(true); - - } - })); + ros::Subscriber yAxis = np.subscribe("/hci/arm/gamepad/axis/stick_left_y", + MESSAGE_QUEUE_LENGTH, + static_cast>([&](Std_Float32 msg) -> void { + if (abs(msg->data) >= JOYSTICK_DEADBAND) { + y_pos += msg->data * STEP_Y; + updateTarget(x_pos, y_pos, z_pos, orientation, nextTarget); + isNewPath.store(true); + } + })); // x axis - ros::Subscriber xAxis = np.subscribe("/hci/arm/gamepad/axis/stick_left_x", - MESSAGE_QUEUE_LENGTH, - static_cast>([&](Std_Float32 msg) -> void { - if(abs(msg->data) >= JOYSTICK_DEADBAND){ - x_pos += msg->data * STEP_X; - updateTarget(x_pos, y_pos, z_pos, orientation, nextTarget); - isNewPath.store(true); - - } - })); + ros::Subscriber xAxis = np.subscribe("/hci/arm/gamepad/axis/stick_left_x", + MESSAGE_QUEUE_LENGTH, + static_cast>([&](Std_Float32 msg) -> void { + if (abs(msg->data) >= JOYSTICK_DEADBAND) { + x_pos += msg->data * STEP_X; + updateTarget(x_pos, y_pos, z_pos, orientation, nextTarget); + isNewPath.store(true); + } + })); // y up ros::Subscriber zUp = np.subscribe("/hci/arm/gamepad/button/shoulder_l", - MESSAGE_QUEUE_LENGTH, - static_cast>([&](Std_Bool msg) -> void { - if(msg->data){ - z_pos += STEP_Y * step_mult; - updateTarget(x_pos, y_pos, z_pos, orientation, nextTarget); - isNewPath.store(true); - - } - })); + MESSAGE_QUEUE_LENGTH, + static_cast>([&](Std_Bool msg) -> void { + if (msg->data) { + z_pos += STEP_Y * step_mult; + updateTarget(x_pos, y_pos, z_pos, orientation, nextTarget); + isNewPath.store(true); + } + })); // y down ros::Subscriber zDown = np.subscribe("/hci/arm/gamepad/button/shoulder_r", - MESSAGE_QUEUE_LENGTH, - static_cast>([&](Std_Bool msg) -> void { - if(msg->data){ - z_pos -= STEP_Y * step_mult; - updateTarget(x_pos, y_pos, z_pos, orientation, nextTarget); - isNewPath.store(true); - - } - })); - - //roll counter clockwise + MESSAGE_QUEUE_LENGTH, + static_cast>([&](Std_Bool msg) -> void { + if (msg->data) { + z_pos -= STEP_Y * step_mult; + updateTarget(x_pos, y_pos, z_pos, orientation, nextTarget); + isNewPath.store(true); + } + })); + + // roll counter clockwise ros::Subscriber rollUp = np.subscribe("/hci/arm/gamepad/button/y", - MESSAGE_QUEUE_LENGTH, - static_cast>([&](Std_Bool msg) -> void { - if(msg->data){ - orientation *= SPIN_Z * step_mult; - updateTarget(x_pos, y_pos, z_pos, orientation, nextTarget); - isNewPath.store(true); - - } - })); + MESSAGE_QUEUE_LENGTH, + static_cast>([&](Std_Bool msg) -> void { + if (msg->data) { + orientation *= SPIN_Z * step_mult; + updateTarget(x_pos, y_pos, z_pos, orientation, nextTarget); + isNewPath.store(true); + } + })); // roll clockwise ros::Subscriber rollDown = np.subscribe("/hci/arm/gamepad/button/a", - MESSAGE_QUEUE_LENGTH, - static_cast>([&](Std_Bool msg) -> void { - if(msg->data){ - orientation *= SPIN_Z.inverse(); - updateTarget(x_pos, y_pos, z_pos, orientation, nextTarget); - isNewPath.store(true); - - } - })); + MESSAGE_QUEUE_LENGTH, + static_cast>([&](Std_Bool msg) -> void { + if (msg->data) { + orientation *= SPIN_Z.inverse(); + updateTarget(x_pos, y_pos, z_pos, orientation, nextTarget); + isNewPath.store(true); + } + })); // pitch ros::Subscriber pitch = np.subscribe("/hci/arm/gamepad/axis/stick_right_y", - MESSAGE_QUEUE_LENGTH, - static_cast>([&](Std_Float32 msg) -> void { - if(abs(msg->data) >= 0.5){ - orientation *= (msg->data > 0 ? SPIN_Y : SPIN_Y.inverse()); - updateTarget(x_pos, y_pos, z_pos, orientation, nextTarget); - isNewPath.store(true); - - } - })); + MESSAGE_QUEUE_LENGTH, + static_cast>([&](Std_Float32 msg) -> void { + if (abs(msg->data) >= 0.5) { + orientation *= (msg->data > 0 ? SPIN_Y : SPIN_Y.inverse()); + updateTarget(x_pos, y_pos, z_pos, orientation, nextTarget); + isNewPath.store(true); + } + })); // yaw ros::Subscriber yaw = np.subscribe("/hci/arm/gamepad/axis/stick_right_x", - MESSAGE_QUEUE_LENGTH, - static_cast>([&](Std_Float32 msg) -> void { - if(abs(msg->data) >= 0.5){ - orientation *= (msg->data > 0 ? SPIN_X : SPIN_X.inverse()); - updateTarget(x_pos, y_pos, z_pos, orientation, nextTarget); - isNewPath.store(true); - - } - })); + MESSAGE_QUEUE_LENGTH, + static_cast>([&](Std_Float32 msg) -> void { + if (abs(msg->data) >= 0.5) { + orientation *= (msg->data > 0 ? SPIN_X : SPIN_X.inverse()); + updateTarget(x_pos, y_pos, z_pos, orientation, nextTarget); + isNewPath.store(true); + } + })); ros::Subscriber speedUp = np.subscribe("/hci/arm/gamepad/axis/trigger_right", - MESSAGE_QUEUE_LENGTH, - static_cast>([&](Std_Float32 msg) -> void { - accel = msg->data; - step_mult = std::pow(10,accel-deaccel); - })); + MESSAGE_QUEUE_LENGTH, + static_cast>([&](Std_Float32 msg) -> void { + accel = msg->data; + step_mult = std::pow(10, accel - deaccel); + })); ros::Subscriber speedDown = np.subscribe("/hci/arm/gamepad/axis/trigger_right", - MESSAGE_QUEUE_LENGTH, - static_cast>([&](Std_Float32 msg) -> void { - deaccel = msg->data; - step_mult = std::pow(10,accel-deaccel); - })); + MESSAGE_QUEUE_LENGTH, + static_cast>([&](Std_Float32 msg) -> void { + deaccel = msg->data; + step_mult = std::pow(10, accel - deaccel); + })); // override path execution ros::Subscriber execPath = np.subscribe("/hci/arm/gamepad/button/x", - MESSAGE_QUEUE_LENGTH, - static_cast>([&](Std_Bool msg) -> void { - if(msg->data){ - isNewPath.store(true); - } - })); + MESSAGE_QUEUE_LENGTH, + static_cast>([&](Std_Bool msg) -> void { + if (msg->data) { + isNewPath.store(true); + } + })); // reset target/cancel path ros::Subscriber resetPose = np.subscribe("/hci/arm/gamepad/button/start", - MESSAGE_QUEUE_LENGTH, - static_cast>([&](Std_Bool msg) -> void { - if(msg->data){ - - geometry_msgs::Pose pose = move.getCurrentPose().pose; - x_pos = pose.position.x; - y_pos = pose.position.y; - z_pos = pose.position.z; - tf2::convert(pose.orientation, orientation); - updateTarget(x_pos, y_pos, z_pos, orientation, nextTarget); - - isNewPath.store(true); //override path - } - })); + MESSAGE_QUEUE_LENGTH, + static_cast>([&](Std_Bool msg) -> void { + if (msg->data) { + + geometry_msgs::Pose pose = move.getCurrentPose().pose; + x_pos = pose.position.x; + y_pos = pose.position.y; + z_pos = pose.position.z; + tf2::convert(pose.orientation, orientation); + updateTarget(x_pos, y_pos, z_pos, orientation, nextTarget); + isNewPath.store(true); // override path + } + })); - while(ros::ok()){ + while (ros::ok()) { updateTarget(x_pos, y_pos, z_pos, orientation, nextTarget); - if(!isNewPath.load()){ + if (!isNewPath.load()) { loop.sleep(); continue; } @@ -237,7 +225,7 @@ auto main(int argc, char** argv) -> int{ move.stop(); // configure target pose - geometry_msgs::PoseStamped p {}; + geometry_msgs::PoseStamped p{}; p.pose.position.x = x_pos; p.pose.position.y = y_pos; p.pose.position.z = z_pos; @@ -247,25 +235,25 @@ auto main(int argc, char** argv) -> int{ move.setPoseTarget(p); move.setStartStateToCurrentState(); - //plan and execute path + // plan and execute path move.asyncMove(); - while(ros::ok()){ + while (ros::ok()) { updateTarget(x_pos, y_pos, z_pos, orientation, nextTarget); - if(move.getMoveGroupClient().getState().isDone()){ + if (move.getMoveGroupClient().getState().isDone()) { std::cout << "[INFO] [" << ros::Time::now() << "]: path finished: " << move.getMoveGroupClient().getState().getText() << std::endl; break; - } - - else if(isNewPath.load()){ + } + + else if (isNewPath.load()) { std::cout << "[INFO] [" << ros::Time::now() << "]: path overridden" << std::endl; move.stop(); break; } loop.sleep(); - } + } } return 0; diff --git a/src/wroboarm_23/config/joint_limits.yaml b/src/wroboarm_23/config/joint_limits.yaml index 1935df40..ae579b8e 100644 --- a/src/wroboarm_23/config/joint_limits.yaml +++ b/src/wroboarm_23/config/joint_limits.yaml @@ -13,36 +13,36 @@ joint_limits: max_velocity: 10 has_acceleration_limits: true max_acceleration: 10 - max_position: !radians pi - min_position: !radians 0 + max_position: !radians 2.2 + min_position: !radians -0.2 elbowRoll_joint: has_velocity_limits: true max_velocity: 10 has_acceleration_limits: true max_acceleration: 10 - max_position: !radians pi / 2 - min_position: !radians -pi / 2 + max_position: !radians 0.7 + min_position: !radians -0.7 shoulder_joint: has_velocity_limits: true max_velocity: 10 has_acceleration_limits: true max_acceleration: 10 max_position: !radians 0 - min_position: !radians -pi + min_position: !radians -1.3 turntable_joint: has_velocity_limits: true max_velocity: 10 has_acceleration_limits: true max_acceleration: 10 - max_position: !radians pi - min_position: !radians -pi + max_position: !radians pi / 2 + min_position: !radians 0 wristPitch_joint: has_velocity_limits: true max_velocity: 10 has_acceleration_limits: true max_acceleration: 10 - max_position: !radians pi - min_position: !radians 0 + max_position: !radians 2.5 + min_position: !radians -2.5 wristRoll_link: has_velocity_limits: true max_velocity: 10 diff --git a/testscrpt.py b/testscrpt.py new file mode 100755 index 00000000..dd6cb4c3 --- /dev/null +++ b/testscrpt.py @@ -0,0 +1,41 @@ +#!/usr/bin/env python3 +import rospy +import sensor_msgs.msg as sensor_msgs +import matplotlib.pyplot as plt +from threading import Thread + +positions = None +names = None + +def newJS(jsMsg: sensor_msgs.JointState): + global positions, names + if positions is None: + positions = [] + names = [name for name in jsMsg.name] + for pos in jsMsg.position: + positions.append([pos]) + else: + for l, pos in zip(positions, jsMsg.position): + l.append(pos) + if len(l) > 500: + del l[0] + +def plot(): + global positions, names + while not rospy.is_shutdown(): + if positions is not None and names is not None: + for l in positions: + plt.plot(l) + plt.legend(names) + plt.pause(0.05) + plt.clf() + + +if __name__=="__main__": + rospy.init_node("testNode") + sub = rospy.Subscriber("/joint_states", sensor_msgs.JointState, newJS) + t1 = Thread(target=plot) + t1.start() + t1.join() + rospy.spin() + \ No newline at end of file