Added implementation for scaling joint speeds based on gear ratios

src/wr_control_drive_arm/config/arm_parameters_mock.yaml

@@ -0,0 +1,37 @@
+arm_parameters: 
+  elbow:
+    encoder_parameters:
+      counts_per_rotation: 1000
+      offset: 500
+    motor_parameters:
+      gear_ratio: 3
+  forearmRoll:
+    encoder_parameters:
+      counts_per_rotation: 1000
+      offset: 500
+    motor_parameters:
+      gear_ratio: 3
+  shoulder:
+    encoder_parameters:
+      counts_per_rotation: 1000
+      offset: 500
+    motor_parameters:
+      gear_ratio: 9
+  turntable:
+    encoder_parameters:
+      counts_per_rotation: 1000
+      offset: 500
+    motor_parameters:
+      gear_ratio: 3
+  wristPitch:
+    encoder_parameters:
+      counts_per_rotation: 1000
+      offset: 500
+    motor_parameters:
+      gear_ratio: 2
+  wristRoll:
+    encoder_parameters:
+      counts_per_rotation: 1000
+      offset: 500
+    motor_parameters:
+      gear_ratio: 2

src/wr_control_drive_arm/config/arm_parameters_real.yaml

@@ -0,0 +1,37 @@
+arm_parameters: 
+  elbow:
+    encoder_parameters:
+      counts_per_rotation: 1850
+      offset: 546
+    motor_parameters:
+      gear_ratio: 3
+  forearmRoll:
+    encoder_parameters:
+      counts_per_rotation: -1850
+      offset: 576
+    motor_parameters:
+      gear_ratio: 3
+  shoulder:
+    encoder_parameters:
+      counts_per_rotation: 5550
+      offset: 1281
+    motor_parameters:
+      gear_ratio: 9
+  turntable:
+    encoder_parameters:
+      counts_per_rotation: 6900 #7200
+      offset: 90
+    motor_parameters:
+      gear_ratio: 3
+  wristPitch:
+    encoder_parameters:
+      counts_per_rotation: -1850
+      offset: 886
+    motor_parameters:
+      gear_ratio: 4.875
+  wristRoll:
+    encoder_parameters:
+      counts_per_rotation: -1850
+      offset: 981
+    motor_parameters:
+      gear_ratio: 4.875

src/wr_control_drive_arm/launch/actionserver.launch

@@ -8,12 +8,12 @@
             type="ArmControlActionServer"
             output="screen">
             <rosparam
-                file="$(find wr_control_drive_arm)/config/encoder_parameters_real.yaml"
+                file="$(find wr_control_drive_arm)/config/arm_parameters_real.yaml"
                 if="$(eval
         env('WROVER_HW') == 'REAL')" />
             <rosparam
                 file="$(find
-        wr_control_drive_arm)/config/encoder_parameters_mock.yaml"
+        wr_control_drive_arm)/config/arm_parameters_mock.yaml"
                 if="$(eval
         env('WROVER_HW') == 'MOCK')" />
         </node>

src/wr_control_drive_arm/launch/jointstatepublisher.launch

@@ -3,9 +3,9 @@
     <group ns="control">
         <node name="JointStatePublisher" pkg="wr_control_drive_arm" type="JointStatePublisher"
             output="screen">
-            <rosparam file="$(find wr_control_drive_arm)/config/encoder_parameters_real.yaml"
+            <rosparam file="$(find wr_control_drive_arm)/config/arm_parameters_real.yaml"
                 if="$(eval env('WROVER_HW') == 'REAL')" />
-            <rosparam file="$(find wr_control_drive_arm)/config/encoder_parameters_mock.yaml"
+            <rosparam file="$(find wr_control_drive_arm)/config/arm_parameters_mock.yaml"
                 if="$(eval env('WROVER_HW') == 'MOCK')" />
         </node>
     </group>

src/wr_control_drive_arm/src/ArmControlActionServer.cpp

@@ -58,12 +58,16 @@ constexpr float TIMER_CALLBACK_DURATION{1.0 / 50.0};
  */
 std::unordered_map<std::string, std::unique_ptr<Joint>> namedJointMap;
 
+/**
+ * @brief Defines space for all joint position monitor references
+ */
 std::unordered_map<std::string, std::shared_ptr<SingleEncoderJointPositionMonitor>> namedPositionMonitors;
 
 /**
  * @brief Simplify the SimpleActionServer reference name
  */
 using Server = actionlib::SimpleActionServer<control_msgs::FollowJointTrajectoryAction>;
+
 /**
  * @brief The service server for enabling IK
  */
@@ -113,23 +117,37 @@ void execute(const control_msgs::FollowJointTrajectoryGoalConstPtr &goal,
             return;
         }
 
+        // Copy of the velocities received from MoveIt
         std::vector<double> velocityCopies{currTargetPosition.velocities};
 
         for (uint32_t i = 0; i < goal->trajectory.joint_names.size(); ++i) {
-            velocityCopies.at(i) *= abs(namedPositionMonitors.at(goal->trajectory.joint_names.at(i))->getCountsPerRotation());
+
+            // The joint that is currently being scaled
+            auto currJoint = goal->trajectory.joint_names.at(i);
+
+            // The position monitor whose velocity is currently being scaled
+            auto currPosMtr = namedPositionMonitors.at(currJoint);
+
+            // Scale by counts per rotation and gear ratio
+            velocityCopies.at(i) *= abs(currPosMtr->getCountsPerRotation()*currPosMtr->getGearRatio());
         }
 
+        // Get the maximum velocity assigned to any joint
         const double VELOCITY_MAX = abs(*std::max_element(
             velocityCopies.begin(),
             velocityCopies.end(),
             [](double lhs, double rhs) -> bool { return abs(lhs) < abs(rhs); }));
 
         for (uint32_t i = 0; i < goal->trajectory.joint_names.size(); ++i) {
+            // Set joint to hold speed in case the greatest velocity comes through as 0
             auto jointVelocity{JOINT_SAFETY_HOLD_SPEED};
+
+            // Scale all velocities by the safety max speed with respect to the maximum velocity given by MoveIt
             if (VELOCITY_MAX != 0)
                 jointVelocity = velocityCopies.at(i) / VELOCITY_MAX * JOINT_SAFETY_MAX_SPEED;
 
-            namedJointMap.at(goal->trajectory.joint_names.at(i))->setTarget(velocityCopies.at(i), jointVelocity);
+            // Set the joint's velocity
+            namedJointMap.at(goal->trajectory.joint_names.at(i))->setTarget(currTargetPosition.positions.at(i), jointVelocity);
         }
     }
 
@@ -161,8 +179,12 @@ void execute(const control_msgs::FollowJointTrajectoryGoalConstPtr &goal,
 }
 
 auto getEncoderConfigFromParams(const XmlRpcValue &params, const std::string &jointName) -> EncoderConfiguration {
-    return {.countsPerRotation = static_cast<int32_t>(params[jointName]["counts_per_rotation"]),
-            .offset = static_cast<int32_t>(params[jointName]["offset"])};
+    return {.countsPerRotation = static_cast<int32_t>(params[jointName]["encoder_parameters"]["counts_per_rotation"]),
+            .offset = static_cast<int32_t>(params[jointName]["encoder_parameters"]["offset"])};
+}
+
+auto getMotorConfigFromParams(const XmlRpcValue &params, const std::string &jointName) -> MotorConfiguration {
+    return {.gearRatio = static_cast<double>(params[jointName]["motor_configurations"]["gear_ratio"])};
 }
 
 void checkOverCurrentFaults(const std::vector<std::shared_ptr<Motor>> &motors){
@@ -195,8 +217,8 @@ auto main(int argc, char **argv) -> int {
     ros::NodeHandle n;
     ros::NodeHandle pn{"~"};
 
-    XmlRpcValue encParams;
-    pn.getParam("encoder_parameters", encParams);
+    XmlRpcValue armParams;
+    pn.getParam("arm_parameters", armParams);
 
     // Initialize all motors with their MoveIt name, WRoboclaw initialization,
     // and reference to the current node
@@ -226,37 +248,43 @@ auto main(int argc, char **argv) -> int {
     namedPositionMonitors.insert({"turntable_joint", std::make_shared<SingleEncoderJointPositionMonitor>(
                                                             "aux0"s,
                                                             RoboclawChannel::A,
-                                                            getEncoderConfigFromParams(encParams, "turntable"),
+                                                            getEncoderConfigFromParams(armParams, "turntable"),
+                                                            getMotorConfigFromParams(armParams, "turntable"),
                                                             n)});
 
     namedPositionMonitors.insert({"shoulder_joint", std::make_shared<SingleEncoderJointPositionMonitor>(
                                                             "aux0"s,
                                                             RoboclawChannel::B,
-                                                            getEncoderConfigFromParams(encParams, "shoulder"),
+                                                            getEncoderConfigFromParams(armParams, "shoulder"),
+                                                            getMotorConfigFromParams(armParams, "shoulder"),
                                                             n)});
 
     namedPositionMonitors.insert({"elbowPitch_joint", std::make_shared<SingleEncoderJointPositionMonitor>(
                                                             "aux1"s,
                                                             RoboclawChannel::A,
-                                                            getEncoderConfigFromParams(encParams, "elbow"),
+                                                            getEncoderConfigFromParams(armParams, "elbow"),
+                                                            getMotorConfigFromParams(armParams, "elbow"),
                                                             n)});
 
     namedPositionMonitors.insert({"elbowRoll_joint", std::make_shared<SingleEncoderJointPositionMonitor>(
                                                             "aux1"s,
                                                             RoboclawChannel::B,
-                                                            getEncoderConfigFromParams(encParams, "forearmRoll"),
+                                                            getEncoderConfigFromParams(armParams, "forearmRoll"),
+                                                            getMotorConfigFromParams(armParams, "forearmRoll"),
                                                             n)});
 
     namedPositionMonitors.insert({"wristPitch_joint", std::make_shared<SingleEncoderJointPositionMonitor>(
                                                             "aux2"s,
                                                             RoboclawChannel::A,
-                                                            getEncoderConfigFromParams(encParams, "wristPitch"),
+                                                            getEncoderConfigFromParams(armParams, "wristPitch"),
+                                                            getMotorConfigFromParams(armParams, "wristPitch"),
                                                             n)});
 
     namedPositionMonitors.insert({"wristRoll_link", std::make_shared<SingleEncoderJointPositionMonitor>(
                                                             "aux2"s,
                                                             RoboclawChannel::B,
-                                                            getEncoderConfigFromParams(encParams, "wristRoll"),
+                                                            getEncoderConfigFromParams(armParams, "wristRoll"),
+                                                            getMotorConfigFromParams(armParams, "wristRoll"),
                                                             n)});
 
     const auto turntableSpeedConverter{std::make_shared<DirectJointToMotorSpeedConverter>(turntableMotor, MotorSpeedDirection::REVERSE)};

src/wr_control_drive_arm/src/DirectJointToMotorSpeedConverter.hpp

@@ -2,6 +2,7 @@
 #define DIRECT_JOINT_TO_MOTOR_SPEED_CONVERTER_H
 
 #include "Motor.hpp"
+#include "SingleEncoderJointPositionMonitor.hpp"
 #include <memory>
 
 enum class MotorSpeedDirection {

src/wr_control_drive_arm/src/JointStatePublisher.cpp

@@ -58,8 +58,12 @@ void publishJointStates(const ros::TimerEvent &event) {
 }
 
 auto getEncoderConfigFromParams(const XmlRpcValue &params, const std::string &jointName) -> EncoderConfiguration {
-    return {.countsPerRotation = static_cast<int32_t>(params[jointName]["counts_per_rotation"]),
-            .offset = static_cast<int32_t>(params[jointName]["offset"])};
+    return {.countsPerRotation = static_cast<int32_t>(params[jointName]["encoder_parameters"]["counts_per_rotation"]),
+            .offset = static_cast<int32_t>(params[jointName]["encoder_parameters"]["offset"])};
+}
+
+auto getMotorConfigFromParams(const XmlRpcValue &params, const std::string &jointName) -> MotorConfiguration {
+    return {.gearRatio = static_cast<double>(params[jointName]["motor_configurations"]["gear_ratio"])};
 }
 
 /**
@@ -78,43 +82,49 @@ auto main(int argc, char **argv) -> int {
     ros::NodeHandle n;
     ros::NodeHandle pn{"~"};
 
-    XmlRpcValue encParams;
-    pn.getParam("encoder_parameters", encParams);
+    XmlRpcValue armParams;
+    pn.getParam("arm_parameters", armParams);
 
     namedJointPositionMonitors.try_emplace("elbowPitch_joint",
 
                                            "aux1",
                                            RoboclawChannel::A,
-                                           getEncoderConfigFromParams(encParams, "elbow"),
+                                           getEncoderConfigFromParams(armParams, "elbow"),
+                                           getMotorConfigFromParams(armParams, "elbow"),
                                            n);
     namedJointPositionMonitors.try_emplace("elbowRoll_joint",
 
                                            "aux1",
                                            RoboclawChannel::B,
-                                           getEncoderConfigFromParams(encParams, "forearmRoll"),
+                                           getEncoderConfigFromParams(armParams, "forearmRoll"),
+                                           getMotorConfigFromParams(armParams, "forearmRoll"),
                                            n);
     namedJointPositionMonitors.try_emplace("shoulder_joint",
 
                                            "aux0",
                                            RoboclawChannel::B,
-                                           getEncoderConfigFromParams(encParams, "shoulder"),
+                                           getEncoderConfigFromParams(armParams, "shoulder"),
+                                           getMotorConfigFromParams(armParams, "shoulder"),
                                            n);
     namedJointPositionMonitors.try_emplace("turntable_joint",
 
                                            "aux0",
                                            RoboclawChannel::A,
-                                           getEncoderConfigFromParams(encParams, "turntable"),
+                                           getEncoderConfigFromParams(armParams, "turntable"),
+                                           getMotorConfigFromParams(armParams, "turntable"),
                                            n);
     namedJointPositionMonitors.try_emplace("wristPitch_joint",
 
                                            "aux2",
                                            RoboclawChannel::A,
-                                           getEncoderConfigFromParams(encParams, "wristPitch"),
+                                           getEncoderConfigFromParams(armParams, "wristPitch"),
+                                           getMotorConfigFromParams(armParams, "wristPitch"),
                                            n);
     namedJointPositionMonitors.try_emplace("wristRoll_link",
                                            "aux2",
                                            RoboclawChannel::B,
-                                           getEncoderConfigFromParams(encParams, "wristRoll"),
+                                           getEncoderConfigFromParams(armParams, "wristRoll"),
+                                           getMotorConfigFromParams(armParams, "wristRoll"),
                                            n);
 
     // Initialize the Joint State Data Publisher

src/wr_control_drive_arm/src/SingleEncoderJointPositionMonitor.cpp

@@ -10,10 +10,12 @@ using MathUtil::RADIANS_PER_ROTATION;
 SingleEncoderJointPositionMonitor::SingleEncoderJointPositionMonitor(
     const std::string &controllerName,
     RoboclawChannel channel,
-    EncoderConfiguration config,
+    EncoderConfiguration eConfig,
+    MotorConfiguration mConfig,
     ros::NodeHandle node)
-    : countsPerRotation{config.countsPerRotation},
-      offset{config.offset},
+    : countsPerRotation{eConfig.countsPerRotation},
+      offset{eConfig.offset},
+      gearRatio{mConfig.gearRatio},
       position{0},
       encoderSubscriber{
           std::make_shared<ros::Subscriber>(node.subscribe(
@@ -26,13 +28,15 @@ SingleEncoderJointPositionMonitor::SingleEncoderJointPositionMonitor(
     const SingleEncoderJointPositionMonitor &other)
     : countsPerRotation(other.countsPerRotation),
       offset(other.offset),
+      gearRatio{other.getGearRatio()},
       encoderSubscriber{other.encoderSubscriber},
       position{other.position.load()} {}
 
 SingleEncoderJointPositionMonitor::SingleEncoderJointPositionMonitor(
     SingleEncoderJointPositionMonitor &&other) noexcept
     : countsPerRotation(other.countsPerRotation),
       offset(other.offset),
+      gearRatio(other.getGearRatio()),
       encoderSubscriber{std::move(other.encoderSubscriber)},
       position{other.position.load()} {}
 
@@ -49,3 +53,7 @@ void SingleEncoderJointPositionMonitor::onEncoderReceived(const std_msgs::UInt32
 auto SingleEncoderJointPositionMonitor::getCountsPerRotation() const -> int32_t {
     return this->countsPerRotation;
 }
+
+auto SingleEncoderJointPositionMonitor::getGearRatio() const -> double {
+    return this->gearRatio;
+}