Added implementation for scaling joint speeds based on gear ratios #41

JackZautner · 2023-03-09T01:30:03Z

Pending hardware testing. Do this one before #34

…cations for arm control system

…are into joint-state-publisher-application

…pplication Joint state publisher application

…ame time.

…spect to inheritance

* Starting to implement new design * Continuing implementation, doing simple parts first to remove big issues * Further implementation * Only Differential stuff and mains left * Up to mains complete * Removing old arm motor files, still in VC for reference * Fixed duplicating copies, need to resolve further * Fixed copying by allowing safer copying. Need to double check path planning * All fixed up. Added test script * Additional changes, some motion planning is possible, need to refine motor/encoder directions * Adding direction multipliers to formalize motor reversal in direct speed converters. Need equivalent for differential speed converters. * Updating constants * First autonomous actions on the arm * Adding legend * Adjusting parameters * Tuning joint limits * 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> * Removed I terms (need synchronization before we can do that) --------- Co-authored-by: Ben Nowotny <bennowotny65535@gmail.com> Co-authored-by: WR-BaseStation <wisconsinrobotics@cae.wisc.edu> Co-authored-by: Ben Nowotny <44074469+bennowotny@users.noreply.github.com>

bennowotny

Additionally, this did not pass hardware testing; the arm was stuttering rather than taking a smooth motion to the target. The cause for this is not apparent, so we may need more testing to find the root cause.

src/wr_control_drive_arm/src/ArmControlActionServer.cpp

…nverters

JackZautner · 2023-03-21T00:17:59Z

I have realized that 1b774df does not explicitly state this but it only contains changes for the direct joint to motor speed converters.

… meant to go there

…nd joint state publisher to pull correct values from arm params

…os to parameter files

bennowotny

Good work; small edits.

bennowotny · 2023-04-20T00:13:26Z

src/wr_control_drive_arm/config/arm_parameters_mock.yaml

In simulation, the gear ratio is always 1.

bennowotny · 2023-04-20T00:15:16Z

src/wr_control_drive_arm/src/ArmControlActionServer.cpp

+            auto currJoint = goal->trajectory.joint_names.at(i);
+
+            // The position monitor whose velocity is currently being scaled
+            auto currPosMtr = namedPositionMonitors.at(currJoint);


Avoid copying the std::shared_ptr:

Suggested change

auto currPosMtr = namedPositionMonitors.at(currJoint);

auto& currPosMtr = *namedPositionMonitors.at(currJoint);

bennowotny · 2023-04-20T00:17:06Z

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 should be unused:

Suggested change

#include "SingleEncoderJointPositionMonitor.hpp"

bennowotny · 2023-04-20T00:18:04Z

src/wr_control_drive_arm/src/JointStatePublisher.cpp

 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"])};
 }


SUGGESTION: Since this is defined in two places the same way, maybe it makes sense to abstract these functions to their own header files and link the implementation in CMake.

Won't do (for now lol)

bennowotny · 2023-04-20T00:18:42Z

src/wr_control_drive_arm/src/JointStatePublisher.cpp

-    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);


SUGGESTION: This may be abstract-able as well.

Won't do (for now lol)

Tzanccc and others added 25 commits October 25, 2022 08:52

Initial commit

9cbb73f

fixin' things

633d124

Undid local testing changes

dc87bfc

Undid local testing changes 2: electric boogaloo

872b85a

more fixin'

6498a69

Fixing PID constants

b11e4e1

Using WROVER_HW to set encoder params

5f0ac8d

Updated moveit using new URDF

ede3d26

started fixing the launch files and packages for the new arm

d4cacd9

Changed the names of the joints in the control layer

1d61fbe

Generated new moveit package

8b48efa

Removing self-dependency

929eddc

cleanup and fixes, works again

853dd98

Separated action server and joint state publisher into separate appli…

03839bf

…cations for arm control system

Deleted old ArmControlSystem file

1fc442f

WIP on joint limits

a78de45

Fixed date in header

9511729

Merge branch 'test/rviz' of github.com:WisconsinRobotics/WRover_Softw…

7701359

…are into joint-state-publisher-application

Merge pull request #28 from WisconsinRobotics/joint-state-publisher-a…

125beae

…pplication Joint state publisher application

Fixed some of the arm code to run the controller and the jsp at the s…

901aa7c

…ame time.

Fixing controller logic, issue in differential joint marked

ad6629c

Adding documentation for new structure

3233dd9

Updates to the structure to be better formed and more dynamic with re…

63de1f4

…spect to inheritance

Added implementation for scaling joint speeds based on gear ratios

b5ba588

JackZautner requested a review from bennowotny March 9, 2023 01:30

bennowotny requested changes Mar 12, 2023

View reviewed changes

src/wr_control_drive_arm/src/ArmControlActionServer.cpp Outdated Show resolved Hide resolved

Base automatically changed from arm-test-jsp-and-controller to test/rviz March 14, 2023 01:03

Base automatically changed from test/rviz to Arm_Control_System March 15, 2023 21:52

Fixed joint target setting code as requested in PR

dcd1a6f

Restructured arm parameter files, added gear ratios to motor speed co…

1b774df

…nverters

JackZautner added 7 commits March 20, 2023 19:29

Removed gear ratios from direct motor speed controllers, they are not…

e895166

… meant to go there

Added gear ratios to joint position monitors, changed action server a…

1114303

…nd joint state publisher to pull correct values from arm params

Updated launch files to reference correct parameters, added gear rati…

3899295

…os to parameter files

Updated roboclaw offsets, cleaned up velocity scaling in arm control

9988616

Added map to store position monitor references

62f6e63

fixed error in constructor

a84b80c

Tuned gear ratios

875b326

JackZautner requested a review from bennowotny April 19, 2023 23:52

bennowotny requested changes Apr 20, 2023

View reviewed changes

Updated code in compliance with #41

ff167a6

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Added implementation for scaling joint speeds based on gear ratios #41

Added implementation for scaling joint speeds based on gear ratios #41

JackZautner commented Mar 9, 2023

bennowotny left a comment

JackZautner commented Mar 21, 2023

bennowotny left a comment

bennowotny Apr 20, 2023

bennowotny Apr 20, 2023

bennowotny Apr 20, 2023

bennowotny Apr 20, 2023

JackZautner Apr 24, 2023

bennowotny Apr 20, 2023

JackZautner Apr 24, 2023

	auto currPosMtr = namedPositionMonitors.at(currJoint);
	auto& currPosMtr = *namedPositionMonitors.at(currJoint);

Added implementation for scaling joint speeds based on gear ratios #41

Are you sure you want to change the base?

Added implementation for scaling joint speeds based on gear ratios #41

Conversation

JackZautner commented Mar 9, 2023

bennowotny left a comment

Choose a reason for hiding this comment

JackZautner commented Mar 21, 2023

bennowotny left a comment

Choose a reason for hiding this comment

bennowotny Apr 20, 2023

Choose a reason for hiding this comment

bennowotny Apr 20, 2023

Choose a reason for hiding this comment

bennowotny Apr 20, 2023

Choose a reason for hiding this comment

bennowotny Apr 20, 2023

Choose a reason for hiding this comment

JackZautner Apr 24, 2023

Choose a reason for hiding this comment

bennowotny Apr 20, 2023

Choose a reason for hiding this comment

JackZautner Apr 24, 2023

Choose a reason for hiding this comment