From 9426f2787bd0a1844416e09d17182cc88de2ae07 Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Thu, 2 Feb 2023 20:12:41 -0600 Subject: [PATCH 01/18] Starting to implement new design --- .clang-format | 4 +- .../src/AbstractJoint.cpp | 39 ------ .../src/AbstractJoint.hpp | 56 -------- .../src/DifferentialJoint.cpp | 129 ------------------ .../src/DifferentialJoint.hpp | 56 -------- src/wr_control_drive_arm/src/Motor.cpp | 27 ++++ src/wr_control_drive_arm/src/Motor.hpp | 18 +++ .../src/RoboclawChannel.hpp | 6 + src/wr_control_drive_arm/src/SimpleJoint.cpp | 24 ---- src/wr_control_drive_arm/src/SimpleJoint.hpp | 18 --- 10 files changed, 54 insertions(+), 323 deletions(-) delete mode 100644 src/wr_control_drive_arm/src/AbstractJoint.cpp delete mode 100644 src/wr_control_drive_arm/src/AbstractJoint.hpp delete mode 100644 src/wr_control_drive_arm/src/DifferentialJoint.cpp delete mode 100644 src/wr_control_drive_arm/src/DifferentialJoint.hpp create mode 100644 src/wr_control_drive_arm/src/Motor.cpp create mode 100644 src/wr_control_drive_arm/src/Motor.hpp create mode 100644 src/wr_control_drive_arm/src/RoboclawChannel.hpp delete mode 100644 src/wr_control_drive_arm/src/SimpleJoint.cpp delete mode 100644 src/wr_control_drive_arm/src/SimpleJoint.hpp 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/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/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/Motor.cpp b/src/wr_control_drive_arm/src/Motor.cpp new file mode 100644 index 00000000..3310ee5a --- /dev/null +++ b/src/wr_control_drive_arm/src/Motor.cpp @@ -0,0 +1,27 @@ +#include "Motor.hpp" +#include "RoboclawChannel.hpp" +#include "ros/node_handle.h" +#include "std_msgs/Int16.h" +#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)} {} + +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); +} + +auto Motor::isOverCurrent() -> bool { // NOLINT(readability-convert-member-functions-to-static) + return false; // TODO: IMPLEMENT ME +} \ No newline at end of file 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..a78cf332 --- /dev/null +++ b/src/wr_control_drive_arm/src/Motor.hpp @@ -0,0 +1,18 @@ +#ifndef MOTOR_H +#define MOTOR_H + +#include "RoboclawChannel.hpp" +#include "ros/publisher.h" +#include + +class Motor { +public: + Motor(const std::string &controllerName, RoboclawChannel channel, ros::NodeHandle node); + void setSpeed(double speed); + auto isOverCurrent() -> bool; + +private: + ros::Publisher motorSpeedPublisher; +}; + +#endif \ No newline at end of file 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; -}; From 623113e43af5548a3dbd6e8c941a5bb872a2849b Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Thu, 2 Feb 2023 23:06:00 -0600 Subject: [PATCH 02/18] Continuing implementation, doing simple parts first to remove big issues --- src/wr_control_drive_arm/src/Joint.cpp | 52 ++++++++++++++++++++++++++ src/wr_control_drive_arm/src/Joint.hpp | 37 ++++++++++++++++++ 2 files changed, 89 insertions(+) create mode 100644 src/wr_control_drive_arm/src/Joint.cpp create mode 100644 src/wr_control_drive_arm/src/Joint.hpp 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..39f38cb2 --- /dev/null +++ b/src/wr_control_drive_arm/src/Joint.cpp @@ -0,0 +1,52 @@ +#include "Joint.hpp" +#include "ros/node_handle.h" +#include "std_msgs/Float64.h" + +using std::literals::string_literals::operator""s; + +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/pid/"s + name + "/output", 1, &Joint::onControlLoopOutput, this)}, + controlLoopSetpointPublisher{node.advertise("/control/pid"s + name + "/setpoint", 1)}, + controlLoopFeedbackPublisher{node.advertise("/control/pid"s + name + "/feedback", 1)} {} + +void Joint::setTarget(double target) { + this->target = target; + executeMotion = true; + controlLoopUpdateTimer.start(); +} + +auto Joint::hasReachedTarget() const -> bool {} // TODO: IMPLEMENT ME + +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) { + motorSpeedDispatcher(executeMotion ? msg->data : 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..c139b709 --- /dev/null +++ b/src/wr_control_drive_arm/src/Joint.hpp @@ -0,0 +1,37 @@ +#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); + [[nodiscard]] auto hasReachedTarget() const -> bool; + [[nodiscard]] auto getName() const -> std::string; + void stop(); + +private: + static constexpr double FEEDBACK_UPDATE_FREQUENCY_HZ{50}; + + 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 executeMotion; + ros::Timer controlLoopUpdateTimer; + ros::Subscriber controlLoopOutputSubscriber; + ros::Publisher controlLoopSetpointPublisher; + ros::Publisher controlLoopFeedbackPublisher; +}; + +#endif \ No newline at end of file From 0a0f2207823483e90328bf1e40810b1c750ec4a7 Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Fri, 3 Feb 2023 08:08:29 -0600 Subject: [PATCH 03/18] Further implementation --- src/wr_control_drive_arm/src/Joint.cpp | 22 +++++++++++-- src/wr_control_drive_arm/src/Joint.hpp | 1 + src/wr_control_drive_arm/src/MathUtil.cpp | 8 +++++ src/wr_control_drive_arm/src/MathUtil.hpp | 11 +++++++ .../src/SingleEncoderJointPositionMonitor.cpp | 31 +++++++++++++++++++ .../src/SingleEncoderJointPositionMonitor.hpp | 30 ++++++++++++++++++ 6 files changed, 100 insertions(+), 3 deletions(-) create mode 100644 src/wr_control_drive_arm/src/MathUtil.cpp create mode 100644 src/wr_control_drive_arm/src/MathUtil.hpp create mode 100644 src/wr_control_drive_arm/src/SingleEncoderJointPositionMonitor.cpp create mode 100644 src/wr_control_drive_arm/src/SingleEncoderJointPositionMonitor.hpp diff --git a/src/wr_control_drive_arm/src/Joint.cpp b/src/wr_control_drive_arm/src/Joint.cpp index 39f38cb2..0efcda89 100644 --- a/src/wr_control_drive_arm/src/Joint.cpp +++ b/src/wr_control_drive_arm/src/Joint.cpp @@ -1,8 +1,11 @@ #include "Joint.hpp" -#include "ros/node_handle.h" -#include "std_msgs/Float64.h" +#include "MathUtil.hpp" +#include +#include +#include using std::literals::string_literals::operator""s; +using MathUtil::RADIANS_PER_ROTATION; Joint::Joint(std::string name, std::function positionMonitor, @@ -23,7 +26,20 @@ void Joint::setTarget(double target) { controlLoopUpdateTimer.start(); } -auto Joint::hasReachedTarget() const -> bool {} // TODO: IMPLEMENT ME +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; diff --git a/src/wr_control_drive_arm/src/Joint.hpp b/src/wr_control_drive_arm/src/Joint.hpp index c139b709..14e23628 100644 --- a/src/wr_control_drive_arm/src/Joint.hpp +++ b/src/wr_control_drive_arm/src/Joint.hpp @@ -18,6 +18,7 @@ class Joint { private: static constexpr double FEEDBACK_UPDATE_FREQUENCY_HZ{50}; + static constexpr double JOINT_TOLERANCE_RADIANS{std::numbers::pi / 180}; const std::string name; const std::function positionMonitor; 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..0ac1ffd4 --- /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 * std::numbers::pi}; +} // namespace MathUtil + +#endif \ No newline at end of file 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..0bc56f3c --- /dev/null +++ b/src/wr_control_drive_arm/src/SingleEncoderJointPositionMonitor.cpp @@ -0,0 +1,31 @@ +#include "SingleEncoderJointPositionMonitor.hpp" +#include "MathUtil.hpp" +#include "RoboclawChannel.hpp" +#include + +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}, + encoderSubscriber{ + node.subscribe( + "/hsi/roboclaw/"s + controllerName + "/enc/" + (channel == RoboclawChannel::A ? "left" : "right"), + 1, + &SingleEncoderJointPositionMonitor::onEncoderReceived, + this)} {} + +auto SingleEncoderJointPositionMonitor::operator()() -> double { + return position; +} + +void SingleEncoderJointPositionMonitor::onEncoderReceived(const std_msgs::UInt32::ConstPtr &msg) { + auto enc = msg->data; + double rotations = MathUtil::corrMod(static_cast(enc - offset), countsPerRotation) / countsPerRotation; + position = MathUtil::corrMod(rotations * RADIANS_PER_ROTATION, RADIANS_PER_ROTATION) - std::numbers::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..f25c8b56 --- /dev/null +++ b/src/wr_control_drive_arm/src/SingleEncoderJointPositionMonitor.hpp @@ -0,0 +1,30 @@ +#ifndef SINGLE_ENCODER_JOINT_POSITION_MONITOR_H +#define SINGLE_ENCODER_JOINT_POSITION_MONITOR_H + +#include "RoboclawChannel.hpp" +#include +#include +#include +#include +#include + +struct EncoderConfiguration { + uint32_t countsPerRotation; + uint32_t offset; +}; + +class SingleEncoderJointPositionMonitor { +public: + SingleEncoderJointPositionMonitor(const std::string &controllerName, RoboclawChannel channel, EncoderConfiguration config, ros::NodeHandle node); + auto operator()() -> double; + +private: + void onEncoderReceived(const std_msgs::UInt32::ConstPtr &msg); + + std::atomic position; + ros::Subscriber encoderSubscriber; + const uint32_t countsPerRotation; + const uint32_t offset; +}; + +#endif \ No newline at end of file From de5e37c6dafcce02b3a8f577ce4399b3fb686aed Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Fri, 3 Feb 2023 08:28:41 -0600 Subject: [PATCH 04/18] Only Differential stuff and mains left --- .../src/DirectJointToMotorSpeedConverter.cpp | 8 ++++++++ .../src/DirectJointToMotorSpeedConverter.hpp | 16 ++++++++++++++++ 2 files changed, 24 insertions(+) create mode 100644 src/wr_control_drive_arm/src/DirectJointToMotorSpeedConverter.cpp create mode 100644 src/wr_control_drive_arm/src/DirectJointToMotorSpeedConverter.hpp 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..9ab82e88 --- /dev/null +++ b/src/wr_control_drive_arm/src/DirectJointToMotorSpeedConverter.cpp @@ -0,0 +1,8 @@ +#include "DirectJointToMotorSpeedConverter.hpp" + +DirectJointToMotorSpeedConverter::DirectJointToMotorSpeedConverter(std::shared_ptr outputMotor) + : outputMotor{std::move(outputMotor)} {} + +void DirectJointToMotorSpeedConverter::operator()(double speed) { + outputMotor->setSpeed(speed); +} 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..ef3a2980 --- /dev/null +++ b/src/wr_control_drive_arm/src/DirectJointToMotorSpeedConverter.hpp @@ -0,0 +1,16 @@ +#ifndef DIRECT_JOINT_TO_MOTOR_SPEED_CONVERTER_H +#define DIRECT_JOINT_TO_MOTOR_SPEED_CONVERTER_H + +#include "Motor.hpp" +#include + +class DirectJointToMotorSpeedConverter { +public: + explicit DirectJointToMotorSpeedConverter(std::shared_ptr outputMotor); + void operator()(double speed); + +private: + const std::shared_ptr outputMotor; +}; + +#endif \ No newline at end of file From d03aff9987e45845bb3d3b3b852e5cdf364c240f Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Fri, 3 Feb 2023 10:43:41 -0600 Subject: [PATCH 05/18] Up to mains complete --- ...DifferentialJointToMotorSpeedConverter.cpp | 28 +++++++++++++++++++ ...DifferentialJointToMotorSpeedConverter.hpp | 25 +++++++++++++++++ 2 files changed, 53 insertions(+) create mode 100644 src/wr_control_drive_arm/src/DifferentialJointToMotorSpeedConverter.cpp create mode 100644 src/wr_control_drive_arm/src/DifferentialJointToMotorSpeedConverter.hpp 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..2fb72d6b --- /dev/null +++ b/src/wr_control_drive_arm/src/DifferentialJointToMotorSpeedConverter.cpp @@ -0,0 +1,28 @@ +#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)} {} + +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); +} \ 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..417e59de --- /dev/null +++ b/src/wr_control_drive_arm/src/DifferentialJointToMotorSpeedConverter.hpp @@ -0,0 +1,25 @@ +#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); + +private: + std::atomic cachedPitchSpeed; + std::atomic cachedRollSpeed; + std::mutex mutex; + + const std::shared_ptr leftMotor; + const std::shared_ptr rightMotor; + + static constexpr double AVERAGE_SCALING_FACTOR{0.5}; + + void dispatchDifferentialSpeed(); +}; + +#endif \ No newline at end of file From 2d1d047a03d905a89c50f122e7bad3e59026e587 Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Fri, 3 Feb 2023 10:44:18 -0600 Subject: [PATCH 06/18] Removing old arm motor files, still in VC for reference --- src/wr_control_drive_arm/src/ArmMotor.cpp | 229 ------------------- src/wr_control_drive_arm/src/ArmMotor.hpp | 259 ---------------------- 2 files changed, 488 deletions(-) delete mode 100644 src/wr_control_drive_arm/src/ArmMotor.cpp delete mode 100644 src/wr_control_drive_arm/src/ArmMotor.hpp 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; -}; From 3f1bf36ad9cee3e16333b1740dc3d49cd3b27a3f Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Sat, 4 Feb 2023 00:50:07 -0600 Subject: [PATCH 07/18] Fixed duplicating copies, need to resolve further --- src/wr_control_drive_arm/CMakeLists.txt | 273 ++++++++-------- .../config/arm_motor_PID_mock.yaml | 45 ++- .../config/arm_motor_PID_real.yaml | 45 ++- .../config/encoder_parameters_mock.yaml | 24 +- .../config/encoder_parameters_real.yaml | 24 +- .../launch/jointstatepublisher.launch | 9 +- .../src/ArmControlActionServer.cpp | 299 ++++++++---------- ...DifferentialJointToMotorSpeedConverter.cpp | 14 + ...DifferentialJointToMotorSpeedConverter.hpp | 6 + .../src/DirectJointToMotorSpeedConverter.hpp | 6 + src/wr_control_drive_arm/src/Joint.cpp | 15 +- src/wr_control_drive_arm/src/Joint.hpp | 5 +- .../src/JointStatePublisher.cpp | 117 +++++-- src/wr_control_drive_arm/src/MathUtil.hpp | 4 +- .../src/SingleEncoderJointPositionMonitor.cpp | 32 +- .../src/SingleEncoderJointPositionMonitor.hpp | 10 + .../launch/test/mock_arm.launch | 32 +- 17 files changed, 505 insertions(+), 455 deletions(-) diff --git a/src/wr_control_drive_arm/CMakeLists.txt b/src/wr_control_drive_arm/CMakeLists.txt index 19ef7fde..44e8c6b1 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 +# # Compile as C++11, supported in ROS Kinetic and newer # add_compile_options(-std=c++11) -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages +# # Find 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,204 @@ 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" +# # 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..a84ccbed 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,62 +1,53 @@ rate: 50 controllers: - - setpointTopic: "/control/arm/00/setpoint" - feedbackTopic: "/control/arm/00/feedback" - outputTopic: "/control/arm/00/output" + - setpointTopic: "/control/arm/pid/turntable/setpoint" + feedbackTopic: "/control/arm/pid/turntable/feedback" + outputTopic: "/control/arm/pid/turntable/output" P: 100 I: 0 D: 0 min: -1 max: 1 - - setpointTopic: "/control/arm/01/setpoint" - feedbackTopic: "/control/arm/01/feedback" - outputTopic: "/control/arm/01/output" + - setpointTopic: "/control/arm/pid/shoulder/setpoint" + feedbackTopic: "/control/arm/pid/shoulder/feedback" + outputTopic: "/control/arm/pid/shoulder/output" P: 100 I: 0 D: 0 min: -1 max: 1 - - setpointTopic: "/control/arm/10/setpoint" - feedbackTopic: "/control/arm/10/feedback" - outputTopic: "/control/arm/10/output" + - setpointTopic: "/control/arm/pid/elbow/setpoint" + feedbackTopic: "/control/arm/pid/elbow/feedback" + outputTopic: "/control/arm/pid/elbow/output" P: 100 I: 0 D: 0 min: -1 max: 1 - - setpointTopic: "/control/arm/11/setpoint" - feedbackTopic: "/control/arm/11/feedback" - outputTopic: "/control/arm/11/output" + - setpointTopic: "/control/arm/pid/forearmRoll/setpoint" + feedbackTopic: "/control/arm/pid/forearmRoll/feedback" + outputTopic: "/control/arm/pid/forearmRoll/output" P: 100 I: 0 D: 0 min: -1 max: 1 - - setpointTopic: "/control/arm/20/setpoint" - feedbackTopic: "/control/arm/20/feedback" - outputTopic: "/control/arm/20/output" + - setpointTopic: "/control/arm/pid/wristPitch/setpoint" + feedbackTopic: "/control/arm/pid/wristPitch/feedback" + outputTopic: "/control/arm/pid/wristPitch/output" P: 100 I: 0 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" + - setpointTopic: "/control/arm/pid/wristRoll/setpoint" + feedbackTopic: "/control/arm/pid/wristRoll/feedback" + outputTopic: "/control/arm/pid/wristRoll/output" P: 100 I: 0 D: 0 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..58c0b316 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,62 +1,53 @@ rate: 50 controllers: - - setpointTopic: "/control/arm/00/setpoint" - feedbackTopic: "/control/arm/00/feedback" - outputTopic: "/control/arm/00/output" + - setpointTopic: "/control/arm/pid/turntable/setpoint" + feedbackTopic: "/control/arm/pid/turntable/feedback" + outputTopic: "/control/arm/pid/turntable/output" P: 5 I: 0 D: 0 min: -1 max: 1 - - setpointTopic: "/control/arm/01/setpoint" - feedbackTopic: "/control/arm/01/feedback" - outputTopic: "/control/arm/01/output" + - setpointTopic: "/control/arm/pid/shoulder/setpoint" + feedbackTopic: "/control/arm/pid/shoulder/feedback" + outputTopic: "/control/arm/pid/shoulder/output" P: 5 I: 0 D: 0 min: -1 max: 1 - - setpointTopic: "/control/arm/10/setpoint" - feedbackTopic: "/control/arm/10/feedback" - outputTopic: "/control/arm/10/output" + - setpointTopic: "/control/arm/pid/elbow/setpoint" + feedbackTopic: "/control/arm/pid/elbow/feedback" + outputTopic: "/control/arm/pid/elbow/output" P: 5 I: 0 D: 0 min: -1 max: 1 - - setpointTopic: "/control/arm/11/setpoint" - feedbackTopic: "/control/arm/11/feedback" - outputTopic: "/control/arm/11/output" + - setpointTopic: "/control/arm/pid/forearmRoll/setpoint" + feedbackTopic: "/control/arm/pid/forearmRoll/feedback" + outputTopic: "/control/arm/pid/forearmRoll/output" P: 5 I: 0 D: 0 min: -1 max: 1 - - setpointTopic: "/control/arm/20/setpoint" - feedbackTopic: "/control/arm/20/feedback" - outputTopic: "/control/arm/20/output" + - setpointTopic: "/control/arm/pid/wristPitch/setpoint" + feedbackTopic: "/control/arm/pid/wristPitch/feedback" + outputTopic: "/control/arm/pid/wristPitch/output" P: 5 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" + - setpointTopic: "/control/arm/pid/wristRoll/setpoint" + feedbackTopic: "/control/arm/pid/wristRoll/feedback" + outputTopic: "/control/arm/pid/wristRoll/output" P: 5 I: 0 D: 0 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..4b84ad16 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 + elbow: + counts_per_rotation: -2048 offset: 610 -# forearm_roll - - counts_per_rotation: -2048 + forearmRoll: + counts_per_rotation: -2048 offset: 540 -# shoulder - - counts_per_rotation: -6144 + shoulder: + counts_per_rotation: -6144 offset: 1129 -# turntable - - counts_per_rotation: 2048 + turntable: + counts_per_rotation: 2048 offset: 985 -# wrist_pitch - - counts_per_rotation: 2048 + wristPitch: + counts_per_rotation: 2048 offset: 886 -# wrist_roll - - counts_per_rotation: 2048 + wristRoll: + counts_per_rotation: 2048 offset: 1050 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/src/ArmControlActionServer.cpp b/src/wr_control_drive_arm/src/ArmControlActionServer.cpp index 9b660b61..df77b025 100644 --- a/src/wr_control_drive_arm/src/ArmControlActionServer.cpp +++ b/src/wr_control_drive_arm/src/ArmControlActionServer.cpp @@ -4,61 +4,60 @@ * @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 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.15}; /** * @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,19 +79,17 @@ 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... + // DEBUG: Display every point in the movement sequence for (const auto &name : goal->trajectory.joint_names) { std::cout << name << "\t"; } @@ -103,135 +100,41 @@ void execute(const control_msgs::FollowJointTrajectoryGoalConstPtr &goal, } 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}; 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(); - } + 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; - // 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(); + namedJointMap.at(goal->trajectory.joint_names.at(i))->setTarget(currTargetPosition.positions.at(i), jointVelocity); } } - // When all positions have been reached, set the current task as succeeded - - as->setSucceeded(); -} + auto waypointComplete{false}; + ros::Rate updateRate{CLOCK_RATE}; -/** - * @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()); + while (!waypointComplete && ros::ok()) { + waypointComplete = true; + for (const auto &[_, joint] : namedJointMap) { + waypointComplete &= joint->hasReachedTarget(); } + updateRate.sleep(); } + // When all positions have been reached, set the current task as succeeded + + server->setSucceeded(); +} - js_msg.name = names; - js_msg.position = positions; - js_msg.header.stamp = ros::Time::now(); - // Publish the Joint State message - jointStatePublisher.publish(js_msg); +auto getEncoderConfigFromParams(const XmlRpcValue ¶ms, const std::string &jointName) -> EncoderConfiguration { + return {.countsPerRotation = static_cast(static_cast(params[jointName]["counts_per_rotation"])), + .offset = static_cast(static_cast(params[jointName]["offset"]))}; } /** @@ -255,47 +158,93 @@ 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; + 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)}; + + SingleEncoderJointPositionMonitor turntablePositionMonitor{ + "aux0"s, + RoboclawChannel::A, + getEncoderConfigFromParams(encParams, "turntable"), + n}; + SingleEncoderJointPositionMonitor shoulderPositionMonitor{ + "aux0"s, + RoboclawChannel::B, + getEncoderConfigFromParams(encParams, "shoulder"), + n}; + SingleEncoderJointPositionMonitor elbowPositionMonitor{ + "aux1"s, + RoboclawChannel::A, + getEncoderConfigFromParams(encParams, "elbow"), + n}; + SingleEncoderJointPositionMonitor forearmRollPositionMonitor{ + "aux1"s, + RoboclawChannel::B, + getEncoderConfigFromParams(encParams, "forearmRoll"), + n}; + SingleEncoderJointPositionMonitor wristPitchPositionMonitor{ + "aux2"s, + RoboclawChannel::A, + getEncoderConfigFromParams(encParams, "wristPitch"), + n}; + SingleEncoderJointPositionMonitor wristRollPositionMonitor{ + "aux2"s, + RoboclawChannel::B, + getEncoderConfigFromParams(encParams, "wristRoll"), + n}; + + DirectJointToMotorSpeedConverter turntableSpeedConverter{turntableMotor}; + DirectJointToMotorSpeedConverter shoulderSpeedConverter{shoulderMotor}; + DirectJointToMotorSpeedConverter elbowSpeedConverter{elbowMotor}; + DirectJointToMotorSpeedConverter forearmRollSpeedConverter{forearmRollMotor}; + DifferentialJointToMotorSpeedConverter differentialSpeedConverter{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, + turntableSpeedConverter, + n)}); + namedJointMap.insert({"shoulder_joint", std::make_unique( + "shoulder", + shoulderPositionMonitor, + shoulderSpeedConverter, + n)}); + namedJointMap.insert({"elbowPitch_joint", std::make_unique( + "elbow", + elbowPositionMonitor, + elbowSpeedConverter, + n)}); + namedJointMap.insert({"elbowRoll_joint", std::make_unique( + "forearmRoll", + forearmRollPositionMonitor, + forearmRollSpeedConverter, + n)}); + namedJointMap.insert({"wristPitch_joint", std::make_unique( + "wristPitch", + wristPitchPositionMonitor, + [&converter = differentialSpeedConverter](double speed) { converter.setPitchSpeed(speed); }, + n)}); + namedJointMap.insert({"wristRoll_link", std::make_unique( + "wristRoll", + 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; diff --git a/src/wr_control_drive_arm/src/DifferentialJointToMotorSpeedConverter.cpp b/src/wr_control_drive_arm/src/DifferentialJointToMotorSpeedConverter.cpp index 2fb72d6b..036008ed 100644 --- a/src/wr_control_drive_arm/src/DifferentialJointToMotorSpeedConverter.cpp +++ b/src/wr_control_drive_arm/src/DifferentialJointToMotorSpeedConverter.cpp @@ -9,6 +9,20 @@ DifferentialJointToMotorSpeedConverter::DifferentialJointToMotorSpeedConverter( 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(); diff --git a/src/wr_control_drive_arm/src/DifferentialJointToMotorSpeedConverter.hpp b/src/wr_control_drive_arm/src/DifferentialJointToMotorSpeedConverter.hpp index 417e59de..4c9e988c 100644 --- a/src/wr_control_drive_arm/src/DifferentialJointToMotorSpeedConverter.hpp +++ b/src/wr_control_drive_arm/src/DifferentialJointToMotorSpeedConverter.hpp @@ -9,6 +9,12 @@ class DifferentialJointToMotorSpeedConverter { 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; diff --git a/src/wr_control_drive_arm/src/DirectJointToMotorSpeedConverter.hpp b/src/wr_control_drive_arm/src/DirectJointToMotorSpeedConverter.hpp index ef3a2980..27c44363 100644 --- a/src/wr_control_drive_arm/src/DirectJointToMotorSpeedConverter.hpp +++ b/src/wr_control_drive_arm/src/DirectJointToMotorSpeedConverter.hpp @@ -9,6 +9,12 @@ class DirectJointToMotorSpeedConverter { explicit DirectJointToMotorSpeedConverter(std::shared_ptr outputMotor); 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 std::shared_ptr outputMotor; }; diff --git a/src/wr_control_drive_arm/src/Joint.cpp b/src/wr_control_drive_arm/src/Joint.cpp index 0efcda89..9db6b60c 100644 --- a/src/wr_control_drive_arm/src/Joint.cpp +++ b/src/wr_control_drive_arm/src/Joint.cpp @@ -16,12 +16,13 @@ Joint::Joint(std::string name, motorSpeedDispatcher{std::move(motorSpeedDispatcher)}, executeMotion{false}, controlLoopUpdateTimer{node.createTimer(ros::Rate{FEEDBACK_UPDATE_FREQUENCY_HZ}, &Joint::onFeedbackUpdateEvent, this, false, false)}, - controlLoopOutputSubscriber{node.subscribe("/control/pid/"s + name + "/output", 1, &Joint::onControlLoopOutput, this)}, - controlLoopSetpointPublisher{node.advertise("/control/pid"s + name + "/setpoint", 1)}, - controlLoopFeedbackPublisher{node.advertise("/control/pid"s + name + "/feedback", 1)} {} + 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) { +void Joint::setTarget(double target, double maxSpeed) { this->target = target; + this->maxSpeed = maxSpeed; executeMotion = true; controlLoopUpdateTimer.start(); } @@ -52,7 +53,11 @@ void Joint::stop() { } void Joint::onControlLoopOutput(const std_msgs::Float64::ConstPtr &msg) { - motorSpeedDispatcher(executeMotion ? msg->data : 0); + 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) { diff --git a/src/wr_control_drive_arm/src/Joint.hpp b/src/wr_control_drive_arm/src/Joint.hpp index 14e23628..8791165e 100644 --- a/src/wr_control_drive_arm/src/Joint.hpp +++ b/src/wr_control_drive_arm/src/Joint.hpp @@ -11,14 +11,14 @@ class Joint { public: explicit Joint(std::string name, std::function positionMonitor, std::function motorSpeedDispatcher, ros::NodeHandle node); - void setTarget(double target); + 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{std::numbers::pi / 180}; + static constexpr double JOINT_TOLERANCE_RADIANS{M_PI / 180}; const std::string name; const std::function positionMonitor; @@ -28,6 +28,7 @@ class Joint { void onFeedbackUpdateEvent(const ros::TimerEvent &event); std::atomic target; + std::atomic maxSpeed; std::atomic executeMotion; ros::Timer controlLoopUpdateTimer; ros::Subscriber controlLoopOutputSubscriber; diff --git a/src/wr_control_drive_arm/src/JointStatePublisher.cpp b/src/wr_control_drive_arm/src/JointStatePublisher.cpp index 3638c4ff..9ee5fe21 100644 --- a/src/wr_control_drive_arm/src/JointStatePublisher.cpp +++ b/src/wr_control_drive_arm/src/JointStatePublisher.cpp @@ -5,12 +5,11 @@ * @brief The executable file to run the joint state publisher * @date 2022-12-05 */ + +#include "SingleEncoderJointPositionMonitor.hpp" #include "XmlRpcValue.h" +#include "ros/spinner.h" -#include "DifferentialJoint.hpp" -#include "SimpleJoint.hpp" -#include "ros/console.h" -#include "ros/ros.h" #include #include #include @@ -28,34 +27,29 @@ 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()); + std::cout << name << " : " << monitor() << std::endl; } js_msg.name = names; @@ -65,6 +59,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(static_cast(params[jointName]["counts_per_rotation"])), + .offset = static_cast(static_cast(params[jointName]["offset"]))}; +} + /** * @brief The main executable method of the node. Starts the ROS node * @@ -75,31 +74,81 @@ 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; + // SingleEncoderJointPositionMonitor turntablePositionMonitor{ + // "aux0", + // RoboclawChannel::A, + // getEncoderConfigFromParams(encParams, "turntable"), + // n}; + // SingleEncoderJointPositionMonitor shoulderPositionMonitor{ + // "aux0", + // RoboclawChannel::B, + // getEncoderConfigFromParams(encParams, "shoulder"), + // n}; + // SingleEncoderJointPositionMonitor elbowPositionMonitor{ + // "aux1", + // RoboclawChannel::A, + // getEncoderConfigFromParams(encParams, "elbow"), + // n}; + // SingleEncoderJointPositionMonitor forearmRollPositionMonitor{ + // "aux1", + // RoboclawChannel::B, + // getEncoderConfigFromParams(encParams, "forearmRoll"), + // n}; + // SingleEncoderJointPositionMonitor wristPitchPositionMonitor{ + // "aux2", + // RoboclawChannel::A, + // getEncoderConfigFromParams(encParams, "wristPitch"), + // n}; + // SingleEncoderJointPositionMonitor wristRollPositionMonitor{ + // "aux2", + // RoboclawChannel::B, + // getEncoderConfigFromParams(encParams, "wristRoll"), + // n}; + + 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.hpp b/src/wr_control_drive_arm/src/MathUtil.hpp index 0ac1ffd4..07e7b0d0 100644 --- a/src/wr_control_drive_arm/src/MathUtil.hpp +++ b/src/wr_control_drive_arm/src/MathUtil.hpp @@ -1,11 +1,11 @@ #ifndef MATH_UTIL_H #define MATH_UTIL_H -#include +#include namespace MathUtil { auto corrMod(double dividend, double divisor) -> double; -static constexpr double RADIANS_PER_ROTATION{2 * std::numbers::pi}; +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/SingleEncoderJointPositionMonitor.cpp b/src/wr_control_drive_arm/src/SingleEncoderJointPositionMonitor.cpp index 0bc56f3c..4652ea0a 100644 --- a/src/wr_control_drive_arm/src/SingleEncoderJointPositionMonitor.cpp +++ b/src/wr_control_drive_arm/src/SingleEncoderJointPositionMonitor.cpp @@ -1,7 +1,8 @@ #include "SingleEncoderJointPositionMonitor.hpp" #include "MathUtil.hpp" #include "RoboclawChannel.hpp" -#include +#include "ros/node_handle.h" +#include "ros/this_node.h" using std::literals::string_literals::operator""s; using MathUtil::RADIANS_PER_ROTATION; @@ -11,21 +12,42 @@ SingleEncoderJointPositionMonitor::SingleEncoderJointPositionMonitor( RoboclawChannel channel, EncoderConfiguration config, ros::NodeHandle node) - : countsPerRotation{config.countsPerRotation}, + : my_counter{SingleEncoderJointPositionMonitor::counter}, + countsPerRotation{config.countsPerRotation}, offset{config.offset}, encoderSubscriber{ node.subscribe( "/hsi/roboclaw/"s + controllerName + "/enc/" + (channel == RoboclawChannel::A ? "left" : "right"), 1, &SingleEncoderJointPositionMonitor::onEncoderReceived, - this)} {} + this)} { + ++SingleEncoderJointPositionMonitor::counter; +} + +SingleEncoderJointPositionMonitor::SingleEncoderJointPositionMonitor( + const SingleEncoderJointPositionMonitor &other) + : my_counter{SingleEncoderJointPositionMonitor::counter}, + countsPerRotation(other.countsPerRotation), + offset(other.offset), + encoderSubscriber{other.encoderSubscriber}, + position{other.position.load()} { ++SingleEncoderJointPositionMonitor::counter; } + +SingleEncoderJointPositionMonitor::SingleEncoderJointPositionMonitor( + SingleEncoderJointPositionMonitor &&other) noexcept + : my_counter{SingleEncoderJointPositionMonitor::counter}, + countsPerRotation(other.countsPerRotation), + offset(other.offset), + encoderSubscriber{other.encoderSubscriber}, + position{other.position.load()} { ++SingleEncoderJointPositionMonitor::counter; } auto SingleEncoderJointPositionMonitor::operator()() -> double { + std::cout << ros::this_node::getName() << " (" << my_counter << ") READ POS : " << position << std::endl; return position; } void SingleEncoderJointPositionMonitor::onEncoderReceived(const std_msgs::UInt32::ConstPtr &msg) { auto enc = msg->data; - double rotations = MathUtil::corrMod(static_cast(enc - offset), countsPerRotation) / countsPerRotation; - position = MathUtil::corrMod(rotations * RADIANS_PER_ROTATION, RADIANS_PER_ROTATION) - std::numbers::pi; + auto rotations = MathUtil::corrMod(static_cast(enc - offset), countsPerRotation) / countsPerRotation; + position = MathUtil::corrMod(rotations * RADIANS_PER_ROTATION + M_PI, RADIANS_PER_ROTATION) - M_PI; + std::cout << ros::this_node::getName() << " (" << my_counter << ") READ POS : " << position << std::endl; } diff --git a/src/wr_control_drive_arm/src/SingleEncoderJointPositionMonitor.hpp b/src/wr_control_drive_arm/src/SingleEncoderJointPositionMonitor.hpp index f25c8b56..66a4c082 100644 --- a/src/wr_control_drive_arm/src/SingleEncoderJointPositionMonitor.hpp +++ b/src/wr_control_drive_arm/src/SingleEncoderJointPositionMonitor.hpp @@ -18,9 +18,19 @@ class SingleEncoderJointPositionMonitor { 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); + inline static std::size_t counter{0}; + + const std::size_t my_counter; + std::atomic position; ros::Subscriber encoderSubscriber; const uint32_t countsPerRotation; diff --git a/src/wr_entry_point/launch/test/mock_arm.launch b/src/wr_entry_point/launch/test/mock_arm.launch index 67abb1df..500b3ebe 100644 --- a/src/wr_entry_point/launch/test/mock_arm.launch +++ b/src/wr_entry_point/launch/test/mock_arm.launch @@ -1,21 +1,21 @@ - - - - + + + + - - - - + + + + - - - - - - - - + + + + + + + + \ No newline at end of file From 0bff2c076f1f4b99581ccc76a3b9119b715e8022 Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Sat, 4 Feb 2023 09:42:20 -0600 Subject: [PATCH 08/18] Fixed copying by allowing safer copying. Need to double check path planning --- .../src/ArmControlActionServer.cpp | 62 +++++++++---------- .../src/JointStatePublisher.cpp | 32 ---------- .../src/SingleEncoderJointPositionMonitor.cpp | 24 +++---- .../src/SingleEncoderJointPositionMonitor.hpp | 6 +- 4 files changed, 40 insertions(+), 84 deletions(-) diff --git a/src/wr_control_drive_arm/src/ArmControlActionServer.cpp b/src/wr_control_drive_arm/src/ArmControlActionServer.cpp index df77b025..b3a5569d 100644 --- a/src/wr_control_drive_arm/src/ArmControlActionServer.cpp +++ b/src/wr_control_drive_arm/src/ArmControlActionServer.cpp @@ -36,7 +36,7 @@ constexpr float CLOCK_RATE{50}; constexpr double IK_WARN_RATE{1.0 / 2}; -constexpr double JOINT_SAFETY_MAX_SPEED{0.5}; +constexpr double JOINT_SAFETY_MAX_SPEED{0.3}; constexpr double JOINT_SAFETY_HOLD_SPEED{0.15}; /** @@ -120,7 +120,7 @@ void execute(const control_msgs::FollowJointTrajectoryGoalConstPtr &goal, auto waypointComplete{false}; ros::Rate updateRate{CLOCK_RATE}; - while (!waypointComplete && ros::ok()) { + while (!waypointComplete && ros::ok() && !server->isNewGoalAvailable()) { waypointComplete = true; for (const auto &[_, joint] : namedJointMap) { waypointComplete &= joint->hasReachedTarget(); @@ -169,42 +169,42 @@ auto main(int argc, char **argv) -> int { const auto wristLeftMotor{std::make_shared("aux2"s, RoboclawChannel::A, n)}; const auto wristRightMotor{std::make_shared("aux2"s, RoboclawChannel::B, n)}; - SingleEncoderJointPositionMonitor turntablePositionMonitor{ + const auto turntablePositionMonitor{std::make_shared( "aux0"s, RoboclawChannel::A, getEncoderConfigFromParams(encParams, "turntable"), - n}; - SingleEncoderJointPositionMonitor shoulderPositionMonitor{ + n)}; + const auto shoulderPositionMonitor{std::make_shared( "aux0"s, RoboclawChannel::B, getEncoderConfigFromParams(encParams, "shoulder"), - n}; - SingleEncoderJointPositionMonitor elbowPositionMonitor{ + n)}; + const auto elbowPositionMonitor{std::make_shared( "aux1"s, RoboclawChannel::A, getEncoderConfigFromParams(encParams, "elbow"), - n}; - SingleEncoderJointPositionMonitor forearmRollPositionMonitor{ + n)}; + const auto forearmRollPositionMonitor{std::make_shared( "aux1"s, RoboclawChannel::B, getEncoderConfigFromParams(encParams, "forearmRoll"), - n}; - SingleEncoderJointPositionMonitor wristPitchPositionMonitor{ + n)}; + const auto wristPitchPositionMonitor{std::make_shared( "aux2"s, RoboclawChannel::A, getEncoderConfigFromParams(encParams, "wristPitch"), - n}; - SingleEncoderJointPositionMonitor wristRollPositionMonitor{ + n)}; + const auto wristRollPositionMonitor{std::make_shared( "aux2"s, RoboclawChannel::B, getEncoderConfigFromParams(encParams, "wristRoll"), - n}; + n)}; - DirectJointToMotorSpeedConverter turntableSpeedConverter{turntableMotor}; - DirectJointToMotorSpeedConverter shoulderSpeedConverter{shoulderMotor}; - DirectJointToMotorSpeedConverter elbowSpeedConverter{elbowMotor}; - DirectJointToMotorSpeedConverter forearmRollSpeedConverter{forearmRollMotor}; - DifferentialJointToMotorSpeedConverter differentialSpeedConverter{wristLeftMotor, wristRightMotor}; + const auto turntableSpeedConverter{std::make_shared(turntableMotor)}; + const auto shoulderSpeedConverter{std::make_shared(shoulderMotor)}; + const auto elbowSpeedConverter{std::make_shared(elbowMotor)}; + const auto forearmRollSpeedConverter{std::make_shared(forearmRollMotor)}; + const auto differentialSpeedConverter{std::make_shared(wristLeftMotor, wristRightMotor)}; // Initialize all Joints @@ -212,33 +212,33 @@ auto main(int argc, char **argv) -> int { namedJointMap.insert({"turntable_joint", std::make_unique( "turntable"s, - turntablePositionMonitor, - turntableSpeedConverter, + [turntablePositionMonitor]() -> double { return (*turntablePositionMonitor)(); }, + [turntableSpeedConverter](double speed) { (*turntableSpeedConverter)(speed); }, n)}); namedJointMap.insert({"shoulder_joint", std::make_unique( "shoulder", - shoulderPositionMonitor, - shoulderSpeedConverter, + [shoulderPositionMonitor]() -> double { return (*shoulderPositionMonitor)(); }, + [shoulderSpeedConverter](double speed) { (*shoulderSpeedConverter)(speed); }, n)}); namedJointMap.insert({"elbowPitch_joint", std::make_unique( "elbow", - elbowPositionMonitor, - elbowSpeedConverter, + [elbowPositionMonitor]() -> double { return (*elbowPositionMonitor)(); }, + [elbowSpeedConverter](double speed) { (*elbowSpeedConverter)(speed); }, n)}); namedJointMap.insert({"elbowRoll_joint", std::make_unique( "forearmRoll", - forearmRollPositionMonitor, - forearmRollSpeedConverter, + [forearmRollPositionMonitor]() -> double { return (*forearmRollPositionMonitor)(); }, + [forearmRollSpeedConverter](double speed) { (*forearmRollSpeedConverter)(speed); }, n)}); namedJointMap.insert({"wristPitch_joint", std::make_unique( "wristPitch", - wristPitchPositionMonitor, - [&converter = differentialSpeedConverter](double speed) { converter.setPitchSpeed(speed); }, + [wristPitchPositionMonitor]() -> double { return (*wristPitchPositionMonitor)(); }, + [converter = differentialSpeedConverter](double speed) { converter->setPitchSpeed(speed); }, n)}); namedJointMap.insert({"wristRoll_link", std::make_unique( "wristRoll", - wristRollPositionMonitor, - [&converter = differentialSpeedConverter](double speed) { converter.setRollSpeed(speed); }, + [wristRollPositionMonitor]() -> double { return (*wristRollPositionMonitor)(); }, + [converter = differentialSpeedConverter](double speed) { converter->setRollSpeed(speed); }, n)}); // Initialize the Action Server diff --git a/src/wr_control_drive_arm/src/JointStatePublisher.cpp b/src/wr_control_drive_arm/src/JointStatePublisher.cpp index 9ee5fe21..8fe56f88 100644 --- a/src/wr_control_drive_arm/src/JointStatePublisher.cpp +++ b/src/wr_control_drive_arm/src/JointStatePublisher.cpp @@ -49,7 +49,6 @@ void publishJointStates(const ros::TimerEvent &event) { for (auto &[name, monitor] : namedJointPositionMonitors) { names.push_back(name); positions.push_back(monitor()); - std::cout << name << " : " << monitor() << std::endl; } js_msg.name = names; @@ -83,37 +82,6 @@ auto main(int argc, char **argv) -> int { XmlRpcValue encParams; pn.getParam("encoder_parameters", encParams); - // SingleEncoderJointPositionMonitor turntablePositionMonitor{ - // "aux0", - // RoboclawChannel::A, - // getEncoderConfigFromParams(encParams, "turntable"), - // n}; - // SingleEncoderJointPositionMonitor shoulderPositionMonitor{ - // "aux0", - // RoboclawChannel::B, - // getEncoderConfigFromParams(encParams, "shoulder"), - // n}; - // SingleEncoderJointPositionMonitor elbowPositionMonitor{ - // "aux1", - // RoboclawChannel::A, - // getEncoderConfigFromParams(encParams, "elbow"), - // n}; - // SingleEncoderJointPositionMonitor forearmRollPositionMonitor{ - // "aux1", - // RoboclawChannel::B, - // getEncoderConfigFromParams(encParams, "forearmRoll"), - // n}; - // SingleEncoderJointPositionMonitor wristPitchPositionMonitor{ - // "aux2", - // RoboclawChannel::A, - // getEncoderConfigFromParams(encParams, "wristPitch"), - // n}; - // SingleEncoderJointPositionMonitor wristRollPositionMonitor{ - // "aux2", - // RoboclawChannel::B, - // getEncoderConfigFromParams(encParams, "wristRoll"), - // n}; - namedJointPositionMonitors.try_emplace("elbowPitch_joint", "aux1", diff --git a/src/wr_control_drive_arm/src/SingleEncoderJointPositionMonitor.cpp b/src/wr_control_drive_arm/src/SingleEncoderJointPositionMonitor.cpp index 4652ea0a..e66ee004 100644 --- a/src/wr_control_drive_arm/src/SingleEncoderJointPositionMonitor.cpp +++ b/src/wr_control_drive_arm/src/SingleEncoderJointPositionMonitor.cpp @@ -2,7 +2,6 @@ #include "MathUtil.hpp" #include "RoboclawChannel.hpp" #include "ros/node_handle.h" -#include "ros/this_node.h" using std::literals::string_literals::operator""s; using MathUtil::RADIANS_PER_ROTATION; @@ -12,36 +11,30 @@ SingleEncoderJointPositionMonitor::SingleEncoderJointPositionMonitor( RoboclawChannel channel, EncoderConfiguration config, ros::NodeHandle node) - : my_counter{SingleEncoderJointPositionMonitor::counter}, - countsPerRotation{config.countsPerRotation}, + : countsPerRotation{config.countsPerRotation}, offset{config.offset}, encoderSubscriber{ - node.subscribe( + std::make_shared(node.subscribe( "/hsi/roboclaw/"s + controllerName + "/enc/" + (channel == RoboclawChannel::A ? "left" : "right"), 1, &SingleEncoderJointPositionMonitor::onEncoderReceived, - this)} { - ++SingleEncoderJointPositionMonitor::counter; -} + this))} {} SingleEncoderJointPositionMonitor::SingleEncoderJointPositionMonitor( const SingleEncoderJointPositionMonitor &other) - : my_counter{SingleEncoderJointPositionMonitor::counter}, - countsPerRotation(other.countsPerRotation), + : countsPerRotation(other.countsPerRotation), offset(other.offset), encoderSubscriber{other.encoderSubscriber}, - position{other.position.load()} { ++SingleEncoderJointPositionMonitor::counter; } + position{other.position.load()} {} SingleEncoderJointPositionMonitor::SingleEncoderJointPositionMonitor( SingleEncoderJointPositionMonitor &&other) noexcept - : my_counter{SingleEncoderJointPositionMonitor::counter}, - countsPerRotation(other.countsPerRotation), + : countsPerRotation(other.countsPerRotation), offset(other.offset), - encoderSubscriber{other.encoderSubscriber}, - position{other.position.load()} { ++SingleEncoderJointPositionMonitor::counter; } + encoderSubscriber{std::move(other.encoderSubscriber)}, + position{other.position.load()} {} auto SingleEncoderJointPositionMonitor::operator()() -> double { - std::cout << ros::this_node::getName() << " (" << my_counter << ") READ POS : " << position << std::endl; return position; } @@ -49,5 +42,4 @@ void SingleEncoderJointPositionMonitor::onEncoderReceived(const std_msgs::UInt32 auto enc = msg->data; auto rotations = MathUtil::corrMod(static_cast(enc - offset), countsPerRotation) / countsPerRotation; position = MathUtil::corrMod(rotations * RADIANS_PER_ROTATION + M_PI, RADIANS_PER_ROTATION) - M_PI; - std::cout << ros::this_node::getName() << " (" << my_counter << ") READ POS : " << position << std::endl; } diff --git a/src/wr_control_drive_arm/src/SingleEncoderJointPositionMonitor.hpp b/src/wr_control_drive_arm/src/SingleEncoderJointPositionMonitor.hpp index 66a4c082..2ef74511 100644 --- a/src/wr_control_drive_arm/src/SingleEncoderJointPositionMonitor.hpp +++ b/src/wr_control_drive_arm/src/SingleEncoderJointPositionMonitor.hpp @@ -27,12 +27,8 @@ class SingleEncoderJointPositionMonitor { private: void onEncoderReceived(const std_msgs::UInt32::ConstPtr &msg); - inline static std::size_t counter{0}; - - const std::size_t my_counter; - std::atomic position; - ros::Subscriber encoderSubscriber; + std::shared_ptr encoderSubscriber; const uint32_t countsPerRotation; const uint32_t offset; }; From 98f957856606195fd3d0360b297d36817ff44077 Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Sat, 4 Feb 2023 14:05:44 -0600 Subject: [PATCH 09/18] All fixed up. Added test script --- .../config/arm_motor_PID_mock.yaml | 24 ++++++------ .../src/ArmControlActionServer.cpp | 25 +++++-------- .../src/JointStatePublisher.cpp | 1 - .../src/SingleEncoderJointPositionMonitor.cpp | 4 +- testscrpt.py | 37 +++++++++++++++++++ 5 files changed, 60 insertions(+), 31 deletions(-) create mode 100755 testscrpt.py 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 a84ccbed..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 @@ -3,8 +3,8 @@ controllers: - setpointTopic: "/control/arm/pid/turntable/setpoint" feedbackTopic: "/control/arm/pid/turntable/feedback" outputTopic: "/control/arm/pid/turntable/output" - P: 100 - I: 0 + P: 5 + I: 0.5 D: 0 min: -1 max: 1 @@ -12,8 +12,8 @@ controllers: - setpointTopic: "/control/arm/pid/shoulder/setpoint" feedbackTopic: "/control/arm/pid/shoulder/feedback" outputTopic: "/control/arm/pid/shoulder/output" - P: 100 - I: 0 + P: 5 + I: 0.5 D: 0 min: -1 max: 1 @@ -21,8 +21,8 @@ controllers: - setpointTopic: "/control/arm/pid/elbow/setpoint" feedbackTopic: "/control/arm/pid/elbow/feedback" outputTopic: "/control/arm/pid/elbow/output" - P: 100 - I: 0 + P: 5 + I: 0.5 D: 0 min: -1 max: 1 @@ -30,8 +30,8 @@ controllers: - setpointTopic: "/control/arm/pid/forearmRoll/setpoint" feedbackTopic: "/control/arm/pid/forearmRoll/feedback" outputTopic: "/control/arm/pid/forearmRoll/output" - P: 100 - I: 0 + P: 5 + I: 0.5 D: 0 min: -1 max: 1 @@ -39,8 +39,8 @@ controllers: - setpointTopic: "/control/arm/pid/wristPitch/setpoint" feedbackTopic: "/control/arm/pid/wristPitch/feedback" outputTopic: "/control/arm/pid/wristPitch/output" - P: 100 - I: 0 + P: 5 + I: 0.5 D: 0 min: -1 max: 1 @@ -48,8 +48,8 @@ controllers: - setpointTopic: "/control/arm/pid/wristRoll/setpoint" feedbackTopic: "/control/arm/pid/wristRoll/feedback" outputTopic: "/control/arm/pid/wristRoll/output" - P: 100 - I: 0 + P: 5 + I: 0.5 D: 0 min: -1 max: 1 diff --git a/src/wr_control_drive_arm/src/ArmControlActionServer.cpp b/src/wr_control_drive_arm/src/ArmControlActionServer.cpp index b3a5569d..f24cbfd9 100644 --- a/src/wr_control_drive_arm/src/ArmControlActionServer.cpp +++ b/src/wr_control_drive_arm/src/ArmControlActionServer.cpp @@ -16,6 +16,7 @@ #include #include #include +#include #include #include #include @@ -25,6 +26,7 @@ #include #include #include +#include #include using XmlRpc::XmlRpcValue; @@ -88,25 +90,12 @@ void execute(const control_msgs::FollowJointTrajectoryGoalConstPtr &goal, return; } - std::cout << "start exec: " << goal->trajectory.points.size() << std::endl; - // DEBUG: Display every point in the movement sequence - for (const auto &name : goal->trajectory.joint_names) { - std::cout << name << "\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"; - } - std::cout << std::endl; - } - for (const auto &currTargetPosition : goal->trajectory.points) { const double VELOCITY_MAX = abs(*std::max_element( currTargetPosition.velocities.begin(), currTargetPosition.velocities.end(), - [](double a, double b) -> bool { return abs(a) < abs(b); })); + [](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}; @@ -127,9 +116,13 @@ void execute(const control_msgs::FollowJointTrajectoryGoalConstPtr &goal, } updateRate.sleep(); } - // When all positions have been reached, set the current task as succeeded - server->setSucceeded(); + // 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(); } auto getEncoderConfigFromParams(const XmlRpcValue ¶ms, const std::string &jointName) -> EncoderConfiguration { diff --git a/src/wr_control_drive_arm/src/JointStatePublisher.cpp b/src/wr_control_drive_arm/src/JointStatePublisher.cpp index 8fe56f88..b1cda527 100644 --- a/src/wr_control_drive_arm/src/JointStatePublisher.cpp +++ b/src/wr_control_drive_arm/src/JointStatePublisher.cpp @@ -8,7 +8,6 @@ #include "SingleEncoderJointPositionMonitor.hpp" #include "XmlRpcValue.h" -#include "ros/spinner.h" #include #include diff --git a/src/wr_control_drive_arm/src/SingleEncoderJointPositionMonitor.cpp b/src/wr_control_drive_arm/src/SingleEncoderJointPositionMonitor.cpp index e66ee004..de13b0c8 100644 --- a/src/wr_control_drive_arm/src/SingleEncoderJointPositionMonitor.cpp +++ b/src/wr_control_drive_arm/src/SingleEncoderJointPositionMonitor.cpp @@ -39,7 +39,7 @@ auto SingleEncoderJointPositionMonitor::operator()() -> double { } void SingleEncoderJointPositionMonitor::onEncoderReceived(const std_msgs::UInt32::ConstPtr &msg) { - auto enc = msg->data; - auto rotations = MathUtil::corrMod(static_cast(enc - offset), countsPerRotation) / countsPerRotation; + 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/testscrpt.py b/testscrpt.py new file mode 100755 index 00000000..03b3df75 --- /dev/null +++ b/testscrpt.py @@ -0,0 +1,37 @@ +#!/usr/bin/env python3 +import rospy +import sensor_msgs.msg as sensor_msgs +import matplotlib.pyplot as plt +from threading import Thread + +positions = None + +def newJS(jsMsg: sensor_msgs.JointState): + global positions + if positions is None: + positions = [] + 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 + while not rospy.is_shutdown(): + if positions is not None: + for l in positions: + plt.plot(l) + 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 From 2813e95cf3e2fac69d7264dc11df23191e1c7fa3 Mon Sep 17 00:00:00 2001 From: WR-BaseStation Date: Sun, 5 Feb 2023 14:35:32 -0600 Subject: [PATCH 10/18] Additional changes, some motion planning is possible, need to refine motor/encoder directions --- src/wr_control_drive_arm/CMakeLists.txt | 5 ++++ .../config/arm_motor_PID_real.yaml | 12 +++++----- .../config/encoder_parameters_real.yaml | 18 +++++++-------- .../src/ArmControlActionServer.cpp | 23 ++++++++++++++----- ...DifferentialJointToMotorSpeedConverter.cpp | 2 +- src/wr_control_drive_arm/src/Joint.cpp | 1 - .../src/JointStatePublisher.cpp | 4 ++-- .../src/SingleEncoderJointPositionMonitor.hpp | 8 +++---- .../launch/test/mock_arm.launch | 2 +- src/wroboarm_23/config/joint_limits.yaml | 2 +- 10 files changed, 46 insertions(+), 31 deletions(-) diff --git a/src/wr_control_drive_arm/CMakeLists.txt b/src/wr_control_drive_arm/CMakeLists.txt index 44e8c6b1..347e0292 100644 --- a/src/wr_control_drive_arm/CMakeLists.txt +++ b/src/wr_control_drive_arm/CMakeLists.txt @@ -147,6 +147,11 @@ target_link_libraries(ArmControlActionServer ${catkin_LIBRARIES} ${PROJECT_NAME} add_executable(JointStatePublisher src/JointStatePublisher.cpp) target_link_libraries(JointStatePublisher ${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) + # # 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 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 58c0b316..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 @@ -3,7 +3,7 @@ controllers: - setpointTopic: "/control/arm/pid/turntable/setpoint" feedbackTopic: "/control/arm/pid/turntable/feedback" outputTopic: "/control/arm/pid/turntable/output" - P: 5 + P: 10 I: 0 D: 0 min: -1 @@ -12,7 +12,7 @@ controllers: - setpointTopic: "/control/arm/pid/shoulder/setpoint" feedbackTopic: "/control/arm/pid/shoulder/feedback" outputTopic: "/control/arm/pid/shoulder/output" - P: 5 + P: 10 I: 0 D: 0 min: -1 @@ -21,7 +21,7 @@ controllers: - setpointTopic: "/control/arm/pid/elbow/setpoint" feedbackTopic: "/control/arm/pid/elbow/feedback" outputTopic: "/control/arm/pid/elbow/output" - P: 5 + P: 10 I: 0 D: 0 min: -1 @@ -30,7 +30,7 @@ controllers: - setpointTopic: "/control/arm/pid/forearmRoll/setpoint" feedbackTopic: "/control/arm/pid/forearmRoll/feedback" outputTopic: "/control/arm/pid/forearmRoll/output" - P: 5 + P: 10 I: 0 D: 0 min: -1 @@ -39,7 +39,7 @@ controllers: - setpointTopic: "/control/arm/pid/wristPitch/setpoint" feedbackTopic: "/control/arm/pid/wristPitch/feedback" outputTopic: "/control/arm/pid/wristPitch/output" - P: 5 + P: 10 I: 0 D: 0 min: -1 @@ -48,7 +48,7 @@ controllers: - setpointTopic: "/control/arm/pid/wristRoll/setpoint" feedbackTopic: "/control/arm/pid/wristRoll/feedback" outputTopic: "/control/arm/pid/wristRoll/output" - P: 5 + P: 10 I: 0 D: 0 min: -1 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 4b84ad16..f069322a 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 + counts_per_rotation: 2048 + offset: 533 forearmRoll: - counts_per_rotation: -2048 + counts_per_rotation: 2048 offset: 540 shoulder: - counts_per_rotation: -6144 - offset: 1129 + counts_per_rotation: 6144 + offset: 1332 turntable: - counts_per_rotation: 2048 - offset: 985 + counts_per_rotation: 7200 + offset: 375 wristPitch: - counts_per_rotation: 2048 + counts_per_rotation: -2048 offset: 886 wristRoll: counts_per_rotation: 2048 - offset: 1050 + offset: 842 diff --git a/src/wr_control_drive_arm/src/ArmControlActionServer.cpp b/src/wr_control_drive_arm/src/ArmControlActionServer.cpp index f24cbfd9..1b273282 100644 --- a/src/wr_control_drive_arm/src/ArmControlActionServer.cpp +++ b/src/wr_control_drive_arm/src/ArmControlActionServer.cpp @@ -90,6 +90,17 @@ void execute(const control_msgs::FollowJointTrajectoryGoalConstPtr &goal, return; } + for(const auto& jointName : goal->trajectory.joint_names){ + std::cout << jointName << "\t"; + } + std::cout << std::endl; + 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) { const double VELOCITY_MAX = abs(*std::max_element( @@ -126,8 +137,8 @@ void execute(const control_msgs::FollowJointTrajectoryGoalConstPtr &goal, } auto getEncoderConfigFromParams(const XmlRpcValue ¶ms, const std::string &jointName) -> EncoderConfiguration { - return {.countsPerRotation = static_cast(static_cast(params[jointName]["counts_per_rotation"])), - .offset = static_cast(static_cast(params[jointName]["offset"]))}; + return {.countsPerRotation = static_cast(params[jointName]["counts_per_rotation"]), + .offset = static_cast(params[jointName]["offset"])}; } /** @@ -206,17 +217,17 @@ auto main(int argc, char **argv) -> int { namedJointMap.insert({"turntable_joint", std::make_unique( "turntable"s, [turntablePositionMonitor]() -> double { return (*turntablePositionMonitor)(); }, - [turntableSpeedConverter](double speed) { (*turntableSpeedConverter)(speed); }, + [turntableSpeedConverter](double speed) { (*turntableSpeedConverter)(-speed); }, n)}); namedJointMap.insert({"shoulder_joint", std::make_unique( "shoulder", [shoulderPositionMonitor]() -> double { return (*shoulderPositionMonitor)(); }, - [shoulderSpeedConverter](double speed) { (*shoulderSpeedConverter)(speed); }, + [shoulderSpeedConverter](double speed) { (*shoulderSpeedConverter)(-speed); }, n)}); namedJointMap.insert({"elbowPitch_joint", std::make_unique( "elbow", [elbowPositionMonitor]() -> double { return (*elbowPositionMonitor)(); }, - [elbowSpeedConverter](double speed) { (*elbowSpeedConverter)(speed); }, + [elbowSpeedConverter](double speed) { (*elbowSpeedConverter)(-speed); }, n)}); namedJointMap.insert({"elbowRoll_joint", std::make_unique( "forearmRoll", @@ -226,7 +237,7 @@ auto main(int argc, char **argv) -> int { namedJointMap.insert({"wristPitch_joint", std::make_unique( "wristPitch", [wristPitchPositionMonitor]() -> double { return (*wristPitchPositionMonitor)(); }, - [converter = differentialSpeedConverter](double speed) { converter->setPitchSpeed(speed); }, + [converter = differentialSpeedConverter](double speed) { converter->setPitchSpeed(-speed); }, n)}); namedJointMap.insert({"wristRoll_link", std::make_unique( "wristRoll", diff --git a/src/wr_control_drive_arm/src/DifferentialJointToMotorSpeedConverter.cpp b/src/wr_control_drive_arm/src/DifferentialJointToMotorSpeedConverter.cpp index 036008ed..c67a7477 100644 --- a/src/wr_control_drive_arm/src/DifferentialJointToMotorSpeedConverter.cpp +++ b/src/wr_control_drive_arm/src/DifferentialJointToMotorSpeedConverter.cpp @@ -34,7 +34,7 @@ void DifferentialJointToMotorSpeedConverter::setRollSpeed(double speed) { } void DifferentialJointToMotorSpeedConverter::dispatchDifferentialSpeed() { - const std::lock_guard guard{mutex}; + const std::lock_guard guard{mutex}; auto m1Speed{cachedPitchSpeed - (cachedRollSpeed * AVERAGE_SCALING_FACTOR)}; auto m2Speed{cachedPitchSpeed + (cachedRollSpeed * AVERAGE_SCALING_FACTOR)}; leftMotor->setSpeed(m1Speed); diff --git a/src/wr_control_drive_arm/src/Joint.cpp b/src/wr_control_drive_arm/src/Joint.cpp index 9db6b60c..08b7c8a3 100644 --- a/src/wr_control_drive_arm/src/Joint.cpp +++ b/src/wr_control_drive_arm/src/Joint.cpp @@ -1,6 +1,5 @@ #include "Joint.hpp" #include "MathUtil.hpp" -#include #include #include diff --git a/src/wr_control_drive_arm/src/JointStatePublisher.cpp b/src/wr_control_drive_arm/src/JointStatePublisher.cpp index b1cda527..3a93cba6 100644 --- a/src/wr_control_drive_arm/src/JointStatePublisher.cpp +++ b/src/wr_control_drive_arm/src/JointStatePublisher.cpp @@ -58,8 +58,8 @@ void publishJointStates(const ros::TimerEvent &event) { } auto getEncoderConfigFromParams(const XmlRpcValue ¶ms, const std::string &jointName) -> EncoderConfiguration { - return {.countsPerRotation = static_cast(static_cast(params[jointName]["counts_per_rotation"])), - .offset = static_cast(static_cast(params[jointName]["offset"]))}; + return {.countsPerRotation = static_cast(params[jointName]["counts_per_rotation"]), + .offset = static_cast(params[jointName]["offset"])}; } /** diff --git a/src/wr_control_drive_arm/src/SingleEncoderJointPositionMonitor.hpp b/src/wr_control_drive_arm/src/SingleEncoderJointPositionMonitor.hpp index 2ef74511..cdf9938e 100644 --- a/src/wr_control_drive_arm/src/SingleEncoderJointPositionMonitor.hpp +++ b/src/wr_control_drive_arm/src/SingleEncoderJointPositionMonitor.hpp @@ -9,8 +9,8 @@ #include struct EncoderConfiguration { - uint32_t countsPerRotation; - uint32_t offset; + int32_t countsPerRotation; + int32_t offset; }; class SingleEncoderJointPositionMonitor { @@ -29,8 +29,8 @@ class SingleEncoderJointPositionMonitor { std::atomic position; std::shared_ptr encoderSubscriber; - const uint32_t countsPerRotation; - const uint32_t offset; + const int32_t countsPerRotation; + const int32_t offset; }; #endif \ 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 500b3ebe..b89cc283 100644 --- a/src/wr_entry_point/launch/test/mock_arm.launch +++ b/src/wr_entry_point/launch/test/mock_arm.launch @@ -2,7 +2,7 @@ - + diff --git a/src/wroboarm_23/config/joint_limits.yaml b/src/wroboarm_23/config/joint_limits.yaml index 1935df40..61a78c2a 100644 --- a/src/wroboarm_23/config/joint_limits.yaml +++ b/src/wroboarm_23/config/joint_limits.yaml @@ -42,7 +42,7 @@ joint_limits: has_acceleration_limits: true max_acceleration: 10 max_position: !radians pi - min_position: !radians 0 + min_position: !radians -pi wristRoll_link: has_velocity_limits: true max_velocity: 10 From 33c024e759d83d07140991cb55f8e8240f86d16d Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Mon, 6 Feb 2023 01:40:21 -0600 Subject: [PATCH 11/18] Adding direction multipliers to formalize motor reversal in direct speed converters. Need equivalent for differential speed converters. --- .../src/ArmControlActionServer.cpp | 22 +- .../src/DirectJointToMotorSpeedConverter.cpp | 16 +- .../src/DirectJointToMotorSpeedConverter.hpp | 8 +- .../src/ArmTeleopLogic.cpp | 268 +++++++++--------- 4 files changed, 159 insertions(+), 155 deletions(-) diff --git a/src/wr_control_drive_arm/src/ArmControlActionServer.cpp b/src/wr_control_drive_arm/src/ArmControlActionServer.cpp index 1b273282..4e272e1a 100644 --- a/src/wr_control_drive_arm/src/ArmControlActionServer.cpp +++ b/src/wr_control_drive_arm/src/ArmControlActionServer.cpp @@ -90,12 +90,12 @@ void execute(const control_msgs::FollowJointTrajectoryGoalConstPtr &goal, return; } - for(const auto& jointName : goal->trajectory.joint_names){ + for (const auto &jointName : goal->trajectory.joint_names) { std::cout << jointName << "\t"; } std::cout << std::endl; - for(const auto& waypoint : goal->trajectory.points){ - for(const auto& jointVal : waypoint.positions){ + for (const auto &waypoint : goal->trajectory.points) { + for (const auto &jointVal : waypoint.positions) { std::cout << jointVal << "\t"; } std::cout << std::endl; @@ -204,10 +204,10 @@ auto main(int argc, char **argv) -> int { getEncoderConfigFromParams(encParams, "wristRoll"), n)}; - const auto turntableSpeedConverter{std::make_shared(turntableMotor)}; - const auto shoulderSpeedConverter{std::make_shared(shoulderMotor)}; - const auto elbowSpeedConverter{std::make_shared(elbowMotor)}; - const auto forearmRollSpeedConverter{std::make_shared(forearmRollMotor)}; + const auto turntableSpeedConverter{std::make_shared(turntableMotor, MotorSpeedDirection::FORWARD)}; + const auto shoulderSpeedConverter{std::make_shared(shoulderMotor, MotorSpeedDirection::FORWARD)}; + const auto elbowSpeedConverter{std::make_shared(elbowMotor, MotorSpeedDirection::FORWARD)}; + const auto forearmRollSpeedConverter{std::make_shared(forearmRollMotor, MotorSpeedDirection::FORWARD)}; const auto differentialSpeedConverter{std::make_shared(wristLeftMotor, wristRightMotor)}; // Initialize all Joints @@ -217,17 +217,17 @@ auto main(int argc, char **argv) -> int { namedJointMap.insert({"turntable_joint", std::make_unique( "turntable"s, [turntablePositionMonitor]() -> double { return (*turntablePositionMonitor)(); }, - [turntableSpeedConverter](double speed) { (*turntableSpeedConverter)(-speed); }, + [turntableSpeedConverter](double speed) { (*turntableSpeedConverter)(speed); }, n)}); namedJointMap.insert({"shoulder_joint", std::make_unique( "shoulder", [shoulderPositionMonitor]() -> double { return (*shoulderPositionMonitor)(); }, - [shoulderSpeedConverter](double speed) { (*shoulderSpeedConverter)(-speed); }, + [shoulderSpeedConverter](double speed) { (*shoulderSpeedConverter)(speed); }, n)}); namedJointMap.insert({"elbowPitch_joint", std::make_unique( "elbow", [elbowPositionMonitor]() -> double { return (*elbowPositionMonitor)(); }, - [elbowSpeedConverter](double speed) { (*elbowSpeedConverter)(-speed); }, + [elbowSpeedConverter](double speed) { (*elbowSpeedConverter)(speed); }, n)}); namedJointMap.insert({"elbowRoll_joint", std::make_unique( "forearmRoll", @@ -237,7 +237,7 @@ auto main(int argc, char **argv) -> int { namedJointMap.insert({"wristPitch_joint", std::make_unique( "wristPitch", [wristPitchPositionMonitor]() -> double { return (*wristPitchPositionMonitor)(); }, - [converter = differentialSpeedConverter](double speed) { converter->setPitchSpeed(-speed); }, + [converter = differentialSpeedConverter](double speed) { converter->setPitchSpeed(speed); }, n)}); namedJointMap.insert({"wristRoll_link", std::make_unique( "wristRoll", diff --git a/src/wr_control_drive_arm/src/DirectJointToMotorSpeedConverter.cpp b/src/wr_control_drive_arm/src/DirectJointToMotorSpeedConverter.cpp index 9ab82e88..67fd0568 100644 --- a/src/wr_control_drive_arm/src/DirectJointToMotorSpeedConverter.cpp +++ b/src/wr_control_drive_arm/src/DirectJointToMotorSpeedConverter.cpp @@ -1,8 +1,18 @@ #include "DirectJointToMotorSpeedConverter.hpp" -DirectJointToMotorSpeedConverter::DirectJointToMotorSpeedConverter(std::shared_ptr outputMotor) - : outputMotor{std::move(outputMotor)} {} +DirectJointToMotorSpeedConverter::DirectJointToMotorSpeedConverter(std::shared_ptr outputMotor, MotorSpeedDirection direction) + : outputMotor{std::move(outputMotor)}, + direction{direction} {} void DirectJointToMotorSpeedConverter::operator()(double speed) { - outputMotor->setSpeed(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 index 27c44363..6a117435 100644 --- a/src/wr_control_drive_arm/src/DirectJointToMotorSpeedConverter.hpp +++ b/src/wr_control_drive_arm/src/DirectJointToMotorSpeedConverter.hpp @@ -4,9 +4,14 @@ #include "Motor.hpp" #include +enum class MotorSpeedDirection { + FORWARD, + REVERSE +}; + class DirectJointToMotorSpeedConverter { public: - explicit DirectJointToMotorSpeedConverter(std::shared_ptr outputMotor); + DirectJointToMotorSpeedConverter(std::shared_ptr outputMotor, MotorSpeedDirection direction); void operator()(double speed); DirectJointToMotorSpeedConverter(const DirectJointToMotorSpeedConverter &) = default; @@ -16,6 +21,7 @@ class DirectJointToMotorSpeedConverter { ~DirectJointToMotorSpeedConverter() = default; private: + const MotorSpeedDirection direction; const std::shared_ptr outputMotor; }; 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; From dd77fbbf5f6e1f4116016b2a5bb289737f3a8356 Mon Sep 17 00:00:00 2001 From: WR-BaseStation Date: Thu, 9 Feb 2023 20:36:19 -0600 Subject: [PATCH 12/18] Updating constants --- .../config/encoder_parameters_real.yaml | 6 +++--- src/wr_control_drive_arm/src/ArmControlActionServer.cpp | 6 +++--- 2 files changed, 6 insertions(+), 6 deletions(-) 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 f069322a..dc35242e 100644 --- a/src/wr_control_drive_arm/config/encoder_parameters_real.yaml +++ b/src/wr_control_drive_arm/config/encoder_parameters_real.yaml @@ -3,7 +3,7 @@ encoder_parameters: counts_per_rotation: 2048 offset: 533 forearmRoll: - counts_per_rotation: 2048 + counts_per_rotation: -2048 offset: 540 shoulder: counts_per_rotation: 6144 @@ -15,5 +15,5 @@ encoder_parameters: counts_per_rotation: -2048 offset: 886 wristRoll: - counts_per_rotation: 2048 - offset: 842 + counts_per_rotation: -2048 + offset: 1011 diff --git a/src/wr_control_drive_arm/src/ArmControlActionServer.cpp b/src/wr_control_drive_arm/src/ArmControlActionServer.cpp index 4e272e1a..b79ab2de 100644 --- a/src/wr_control_drive_arm/src/ArmControlActionServer.cpp +++ b/src/wr_control_drive_arm/src/ArmControlActionServer.cpp @@ -204,9 +204,9 @@ auto main(int argc, char **argv) -> int { getEncoderConfigFromParams(encParams, "wristRoll"), n)}; - const auto turntableSpeedConverter{std::make_shared(turntableMotor, MotorSpeedDirection::FORWARD)}; - const auto shoulderSpeedConverter{std::make_shared(shoulderMotor, MotorSpeedDirection::FORWARD)}; - const auto elbowSpeedConverter{std::make_shared(elbowMotor, MotorSpeedDirection::FORWARD)}; + 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::FORWARD)}; const auto differentialSpeedConverter{std::make_shared(wristLeftMotor, wristRightMotor)}; From 5d93fca956c422279a3ff16889bbac38602ad337 Mon Sep 17 00:00:00 2001 From: WR-BaseStation Date: Fri, 10 Feb 2023 15:07:40 -0600 Subject: [PATCH 13/18] First autonomous actions on the arm --- src/wr_control_drive_arm/CMakeLists.txt | 4 +++ .../config/arm_motor_PID_real.yaml | 4 +-- .../launch/testDifferential.launch | 3 +++ .../src/ArmControlActionServer.cpp | 9 ++++--- ...DifferentialJointToMotorSpeedConverter.cpp | 7 ++--- ...DifferentialJointToMotorSpeedConverter.hpp | 4 +-- src/wr_control_drive_arm/src/Joint.hpp | 2 +- .../src/SingleEncoderJointPositionMonitor.cpp | 1 + .../src/testDifferential.cpp | 26 +++++++++++++++++++ 9 files changed, 49 insertions(+), 11 deletions(-) create mode 100644 src/wr_control_drive_arm/launch/testDifferential.launch create mode 100644 src/wr_control_drive_arm/src/testDifferential.cpp diff --git a/src/wr_control_drive_arm/CMakeLists.txt b/src/wr_control_drive_arm/CMakeLists.txt index 347e0292..60a88ed2 100644 --- a/src/wr_control_drive_arm/CMakeLists.txt +++ b/src/wr_control_drive_arm/CMakeLists.txt @@ -146,11 +146,15 @@ 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}) +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 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 a0da7801..73324809 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 @@ -4,7 +4,7 @@ controllers: feedbackTopic: "/control/arm/pid/turntable/feedback" outputTopic: "/control/arm/pid/turntable/output" P: 10 - I: 0 + I: 0.1 D: 0 min: -1 max: 1 @@ -13,7 +13,7 @@ controllers: feedbackTopic: "/control/arm/pid/shoulder/feedback" outputTopic: "/control/arm/pid/shoulder/output" P: 10 - I: 0 + I: 0.1 D: 0 min: -1 max: 1 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/ArmControlActionServer.cpp b/src/wr_control_drive_arm/src/ArmControlActionServer.cpp index b79ab2de..6c821b27 100644 --- a/src/wr_control_drive_arm/src/ArmControlActionServer.cpp +++ b/src/wr_control_drive_arm/src/ArmControlActionServer.cpp @@ -38,8 +38,8 @@ constexpr float CLOCK_RATE{50}; constexpr double IK_WARN_RATE{1.0 / 2}; -constexpr double JOINT_SAFETY_MAX_SPEED{0.3}; -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 @@ -124,6 +124,8 @@ void execute(const control_msgs::FollowJointTrajectoryGoalConstPtr &goal, waypointComplete = true; for (const auto &[_, joint] : namedJointMap) { waypointComplete &= joint->hasReachedTarget(); + if(!joint->hasReachedTarget()) + std::cout << "Waiting on joint " << _ << std::endl; } updateRate.sleep(); } @@ -134,6 +136,7 @@ void execute(const control_msgs::FollowJointTrajectoryGoalConstPtr &goal, // When all positions have been reached, set the current task as succeeded else server->setSucceeded(); + std::cout << "Action Complete!" << std::endl; } auto getEncoderConfigFromParams(const XmlRpcValue ¶ms, const std::string &jointName) -> EncoderConfiguration { @@ -207,7 +210,7 @@ auto main(int argc, char **argv) -> int { 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::FORWARD)}; + const auto forearmRollSpeedConverter{std::make_shared(forearmRollMotor, MotorSpeedDirection::REVERSE)}; const auto differentialSpeedConverter{std::make_shared(wristLeftMotor, wristRightMotor)}; // Initialize all Joints diff --git a/src/wr_control_drive_arm/src/DifferentialJointToMotorSpeedConverter.cpp b/src/wr_control_drive_arm/src/DifferentialJointToMotorSpeedConverter.cpp index c67a7477..dfe28450 100644 --- a/src/wr_control_drive_arm/src/DifferentialJointToMotorSpeedConverter.cpp +++ b/src/wr_control_drive_arm/src/DifferentialJointToMotorSpeedConverter.cpp @@ -34,9 +34,10 @@ void DifferentialJointToMotorSpeedConverter::setRollSpeed(double speed) { } void DifferentialJointToMotorSpeedConverter::dispatchDifferentialSpeed() { - const std::lock_guard guard{mutex}; - auto m1Speed{cachedPitchSpeed - (cachedRollSpeed * AVERAGE_SCALING_FACTOR)}; - auto m2Speed{cachedPitchSpeed + (cachedRollSpeed * AVERAGE_SCALING_FACTOR)}; + // 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 index 4c9e988c..5d096add 100644 --- a/src/wr_control_drive_arm/src/DifferentialJointToMotorSpeedConverter.hpp +++ b/src/wr_control_drive_arm/src/DifferentialJointToMotorSpeedConverter.hpp @@ -18,12 +18,12 @@ class DifferentialJointToMotorSpeedConverter { private: std::atomic cachedPitchSpeed; std::atomic cachedRollSpeed; - std::mutex mutex; + std::recursive_mutex mutex; const std::shared_ptr leftMotor; const std::shared_ptr rightMotor; - static constexpr double AVERAGE_SCALING_FACTOR{0.5}; + static constexpr double AVERAGE_SCALING_FACTOR{1}; void dispatchDifferentialSpeed(); }; diff --git a/src/wr_control_drive_arm/src/Joint.hpp b/src/wr_control_drive_arm/src/Joint.hpp index 8791165e..659a4f98 100644 --- a/src/wr_control_drive_arm/src/Joint.hpp +++ b/src/wr_control_drive_arm/src/Joint.hpp @@ -18,7 +18,7 @@ class Joint { private: static constexpr double FEEDBACK_UPDATE_FREQUENCY_HZ{50}; - static constexpr double JOINT_TOLERANCE_RADIANS{M_PI / 180}; + static constexpr double JOINT_TOLERANCE_RADIANS{3 * M_PI / 180}; const std::string name; const std::function positionMonitor; diff --git a/src/wr_control_drive_arm/src/SingleEncoderJointPositionMonitor.cpp b/src/wr_control_drive_arm/src/SingleEncoderJointPositionMonitor.cpp index de13b0c8..9c179dc8 100644 --- a/src/wr_control_drive_arm/src/SingleEncoderJointPositionMonitor.cpp +++ b/src/wr_control_drive_arm/src/SingleEncoderJointPositionMonitor.cpp @@ -13,6 +13,7 @@ SingleEncoderJointPositionMonitor::SingleEncoderJointPositionMonitor( 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"), 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 From ab4b4f6b1c52c51093cdffc91699f4c57951524a Mon Sep 17 00:00:00 2001 From: WR-BaseStation Date: Sat, 11 Feb 2023 14:28:57 -0600 Subject: [PATCH 14/18] Adding legend --- testscrpt.py | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/testscrpt.py b/testscrpt.py index 03b3df75..dd6cb4c3 100755 --- a/testscrpt.py +++ b/testscrpt.py @@ -5,11 +5,13 @@ from threading import Thread positions = None +names = None def newJS(jsMsg: sensor_msgs.JointState): - global positions + global positions, names if positions is None: positions = [] + names = [name for name in jsMsg.name] for pos in jsMsg.position: positions.append([pos]) else: @@ -19,14 +21,16 @@ def newJS(jsMsg: sensor_msgs.JointState): del l[0] def plot(): - global positions + global positions, names while not rospy.is_shutdown(): - if positions is not None: + 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) From 76fdadbd1220594f508b05fabb07bed57f72f2a9 Mon Sep 17 00:00:00 2001 From: WR-BaseStation Date: Sat, 11 Feb 2023 14:29:04 -0600 Subject: [PATCH 15/18] Adjusting parameters --- .../config/encoder_parameters_real.yaml | 22 +++++++++---------- 1 file changed, 11 insertions(+), 11 deletions(-) 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 dc35242e..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: 533 + counts_per_rotation: 1850 + offset: 546 forearmRoll: - counts_per_rotation: -2048 - offset: 540 + counts_per_rotation: -1850 + offset: 576 shoulder: - counts_per_rotation: 6144 - offset: 1332 + counts_per_rotation: 5550 + offset: 1281 turntable: - counts_per_rotation: 7200 - offset: 375 + counts_per_rotation: 6900 #7200 + offset: 90 wristPitch: - counts_per_rotation: -2048 + counts_per_rotation: -1850 offset: 886 wristRoll: - counts_per_rotation: -2048 - offset: 1011 + counts_per_rotation: -1850 + offset: 981 From d50c86334cefad7fc5e7fa6bcb32f640ab7c6f0a Mon Sep 17 00:00:00 2001 From: WR-BaseStation Date: Mon, 13 Feb 2023 10:37:06 -0600 Subject: [PATCH 16/18] Tuning joint limits --- src/wroboarm_23/config/joint_limits.yaml | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/src/wroboarm_23/config/joint_limits.yaml b/src/wroboarm_23/config/joint_limits.yaml index 61a78c2a..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 -pi + max_position: !radians 2.5 + min_position: !radians -2.5 wristRoll_link: has_velocity_limits: true max_velocity: 10 From 7ae36ef5ede6db2afab55ad13036867418c6c56e Mon Sep 17 00:00:00 2001 From: JackZautner <34869117+JackZautner@users.noreply.github.com> Date: Tue, 7 Mar 2023 18:26:32 -0600 Subject: [PATCH 17/18] Feature/over current fault implementation (#31) * Added functionality to retrieve over current status, updated build settings * Added functionality to stop all joints when a motor is over current * Added implementation for action server to stop when an over current fault is detected * set mock arm launch to launch the mock roboclaw * Added functionality to stop the action server when a motor is over current for too long * Fixed code in compliance with PR * Address fix in Motor.cpp * Corresponding const-change in Motor.hpp * Const-correctness in ArmControlActionServer.cpp --------- Co-authored-by: Ben Nowotny <44074469+bennowotny@users.noreply.github.com> --- src/wr_control_drive_arm/CMakeLists.txt | 2 +- .../src/ArmControlActionServer.cpp | 44 ++++++++++++++++++- src/wr_control_drive_arm/src/Motor.cpp | 26 +++++++++-- src/wr_control_drive_arm/src/Motor.hpp | 16 ++++++- .../launch/test/mock_arm.launch | 5 ++- 5 files changed, 83 insertions(+), 10 deletions(-) diff --git a/src/wr_control_drive_arm/CMakeLists.txt b/src/wr_control_drive_arm/CMakeLists.txt index 60a88ed2..a122dafe 100644 --- a/src/wr_control_drive_arm/CMakeLists.txt +++ b/src/wr_control_drive_arm/CMakeLists.txt @@ -2,7 +2,7 @@ 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) +add_compile_options(-std=c++17) # # Find catkin macros and libraries # # if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) diff --git a/src/wr_control_drive_arm/src/ArmControlActionServer.cpp b/src/wr_control_drive_arm/src/ArmControlActionServer.cpp index 6c821b27..14892cd3 100644 --- a/src/wr_control_drive_arm/src/ArmControlActionServer.cpp +++ b/src/wr_control_drive_arm/src/ArmControlActionServer.cpp @@ -28,6 +28,7 @@ #include #include #include +#include using XmlRpc::XmlRpcValue; @@ -103,6 +104,12 @@ void execute(const control_msgs::FollowJointTrajectoryGoalConstPtr &goal, for (const auto &currTargetPosition : goal->trajectory.points) { + 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(), @@ -121,11 +128,16 @@ void execute(const control_msgs::FollowJointTrajectoryGoalConstPtr &goal, 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(); - if(!joint->hasReachedTarget()) - std::cout << "Waiting on joint " << _ << std::endl; } updateRate.sleep(); } @@ -144,6 +156,20 @@ auto getEncoderConfigFromParams(const XmlRpcValue ¶ms, const std::string &jo .offset = static_cast(params[jointName]["offset"])}; } +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 + } +} + /** * @brief The main executable method of the node. Starts the ROS node and the * Action Server for processing Arm Control commands @@ -167,6 +193,11 @@ auto main(int argc, char **argv) -> int { // and reference to the current node 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)}; @@ -176,6 +207,13 @@ auto main(int argc, char **argv) -> int { 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, @@ -271,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/Motor.cpp b/src/wr_control_drive_arm/src/Motor.cpp index 3310ee5a..bc96cd0f 100644 --- a/src/wr_control_drive_arm/src/Motor.cpp +++ b/src/wr_control_drive_arm/src/Motor.cpp @@ -1,8 +1,11 @@ #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; @@ -11,7 +14,10 @@ Motor::Motor(const std::string &controllerName, RoboclawChannel channel, ros::No : motorSpeedPublisher{ node.advertise( "/hsi/roboclaw/"s + controllerName + "/cmd/"s + (channel == RoboclawChannel::A ? "left" : "right"), - 1)} {} + 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) @@ -22,6 +28,18 @@ void Motor::setSpeed(double speed) { motorSpeedPublisher.publish(powerMsg); } -auto Motor::isOverCurrent() -> bool { // NOLINT(readability-convert-member-functions-to-static) - return false; // TODO: IMPLEMENT ME -} \ No newline at end of file +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 index a78cf332..ff3a6174 100644 --- a/src/wr_control_drive_arm/src/Motor.hpp +++ b/src/wr_control_drive_arm/src/Motor.hpp @@ -3,16 +3,28 @@ #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() -> bool; + 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 \ No newline at end of file +#endif diff --git a/src/wr_entry_point/launch/test/mock_arm.launch b/src/wr_entry_point/launch/test/mock_arm.launch index b89cc283..6dfbb8b1 100644 --- a/src/wr_entry_point/launch/test/mock_arm.launch +++ b/src/wr_entry_point/launch/test/mock_arm.launch @@ -2,7 +2,10 @@ - + + + + From 62f533484b4c563af8158b6de02f54524b23e119 Mon Sep 17 00:00:00 2001 From: Ben Nowotny <44074469+bennowotny@users.noreply.github.com> Date: Tue, 7 Mar 2023 20:01:50 -0600 Subject: [PATCH 18/18] Removed I terms (need synchronization before we can do that) --- src/wr_control_drive_arm/config/arm_motor_PID_real.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 73324809..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 @@ -4,7 +4,7 @@ controllers: feedbackTopic: "/control/arm/pid/turntable/feedback" outputTopic: "/control/arm/pid/turntable/output" P: 10 - I: 0.1 + I: 0 D: 0 min: -1 max: 1 @@ -13,7 +13,7 @@ controllers: feedbackTopic: "/control/arm/pid/shoulder/feedback" outputTopic: "/control/arm/pid/shoulder/output" P: 10 - I: 0.1 + I: 0 D: 0 min: -1 max: 1