From 8b4d833b55ce88070997eb2fe4d3db2713ed13c2 Mon Sep 17 00:00:00 2001 From: qhdwight Date: Wed, 17 Apr 2024 21:37:12 -0400 Subject: [PATCH] Cleanup arm translator a bit --- .../arm_translator_bridge/arm_translator.cpp | 67 ++++--------------- src/util/units/units_eigen.hpp | 31 +++++++++ 2 files changed, 43 insertions(+), 55 deletions(-) create mode 100644 src/util/units/units_eigen.hpp diff --git a/src/esw/arm_translator_bridge/arm_translator.cpp b/src/esw/arm_translator_bridge/arm_translator.cpp index 9dd1ce6a6..b31ec495e 100644 --- a/src/esw/arm_translator_bridge/arm_translator.cpp +++ b/src/esw/arm_translator_bridge/arm_translator.cpp @@ -1,56 +1,19 @@ #include "arm_translator.hpp" -#include #include -#include -#include - -namespace Eigen { - - template< - typename Rep1, typename Conversion1, typename MeterExp1, typename KilogramExp1, typename SecondExp1, typename RadianExp1, typename AmpereExp1, typename KelvinExp1, typename ByteExp1, typename TickExp1, - typename Rep2, typename Conversion2, typename MeterExp2, typename KilogramExp2, typename SecondExp2, typename RadianExp2, typename AmpereExp2, typename KelvinExp2, typename ByteExp2, typename TickExp2> - struct ScalarBinaryOpTraits< - mrover::Unit, - mrover::Unit, - internal::scalar_product_op< - mrover::Unit, - mrover::Unit>> { - using U1 = mrover::Unit; - using U2 = mrover::Unit; - using ReturnType = mrover::multiply; - }; - - template<> - struct ScalarBinaryOpTraits< - mrover::Dimensionless, - mrover::Dimensionless, - internal::scalar_product_op> { - using ReturnType = mrover::Dimensionless; - }; - - // template - // struct NumTraits> : NumTraits { - // using U = mrover::Unit; - // using Real = U; - // using NonInteger = U; - // using Nested = U; - // enum { - // IsComplex = 0, - // IsInteger = 0, - // IsSigned = 1, - // RequireInitialization = 1, - // ReadCost = 1, - // AddCost = 3, - // MulCost = 3, - // }; - // }; - -} // namespace Eigen +#include +#include +#include namespace mrover { + auto static const PITCH_ROLL_TO_0_1 = (Matrix2{} << -1, -1, -1, 1).finished(); + + Dimensionless static constexpr PITCH_ROLL_TO_01_SCALE{40}; + + double static constexpr DE_OFFSET_TIMER_PERIOD = 1; + ArmTranslator::ArmTranslator(ros::NodeHandle& nh) { for (std::string const& hwName: mArmHWNames) { [[maybe_unused]] auto [_, was_inserted] = mAdjustClientsByArmHwNames.try_emplace(hwName, nh.serviceClient(std::format("{}_adjust", hwName))); @@ -67,11 +30,9 @@ namespace mrover { mPositionPub = std::make_unique(nh.advertise("arm_hw_position_cmd", 1)); mJointDataPub = std::make_unique(nh.advertise("arm_joint_data", 1)); - mDeOffsetTimer = nh.createTimer(ros::Duration{1}, &ArmTranslator::updateDeOffsets, this); + mDeOffsetTimer = nh.createTimer(ros::Duration{DE_OFFSET_TIMER_PERIOD}, &ArmTranslator::updateDeOffsets, this); } - auto static const PITCH_ROLL_TO_0_1 = (Matrix2{} << -1, -1, -1, 1).finished(); - auto findJointByName(std::vector const& names, std::string const& name) -> std::optional { auto it = std::ranges::find(names, name); return it == names.end() ? std::nullopt : std::make_optional(std::distance(names.begin(), it)); @@ -103,10 +64,6 @@ namespace mrover { mThrottlePub->publish(throttle); } - // constexpr Dimensionless PITCH_ROLL_TO_01_SCALE = 40; - // Matrix2 static const PITCH_ROLL_TO_01_SCALED = PITCH_ROLL_TO_0_1 * PITCH_ROLL_TO_01_SCALE; - // Note (Isabel) PITCH_ROLL_TO_01_SCALE is unnecessary, moteus config will scale for gear ratio - auto ArmTranslator::processVelocityCmd(Velocity::ConstPtr const& msg) -> void { if (msg->names.size() != msg->velocities.size()) { ROS_ERROR("Name count and value count mismatched!"); @@ -148,7 +105,7 @@ namespace mrover { std::size_t pitchIndex = jointDePitchIndex.value(), rollIndex = jointDeRollIndex.value(); Vector2 pitchRoll{msg->positions.at(pitchIndex), msg->positions.at(rollIndex)}; - Vector2 motorPositions = 40 * PITCH_ROLL_TO_0_1 * pitchRoll; + Vector2 motorPositions = PITCH_ROLL_TO_01_SCALE * PITCH_ROLL_TO_0_1 * pitchRoll; position.names[pitchIndex] = "joint_de_0"; position.names[rollIndex] = "joint_de_1"; @@ -190,7 +147,7 @@ namespace mrover { auto ArmTranslator::updateDeOffsets(ros::TimerEvent const&) -> void { if (!mJointDePitchRoll) return; - Vector2 motorPositions = 40 * PITCH_ROLL_TO_0_1 * mJointDePitchRoll.value(); + Vector2 motorPositions = PITCH_ROLL_TO_01_SCALE * PITCH_ROLL_TO_0_1 * mJointDePitchRoll.value(); { AdjustMotor adjust; adjust.request.name = "joint_de_0"; diff --git a/src/util/units/units_eigen.hpp b/src/util/units/units_eigen.hpp new file mode 100644 index 000000000..5b1faad7a --- /dev/null +++ b/src/util/units/units_eigen.hpp @@ -0,0 +1,31 @@ +#pragma once + +#include "units.hpp" + +#include + +namespace Eigen { + + template< + typename Rep1, typename Conversion1, typename MeterExp1, typename KilogramExp1, typename SecondExp1, typename RadianExp1, typename AmpereExp1, typename KelvinExp1, typename ByteExp1, typename TickExp1, + typename Rep2, typename Conversion2, typename MeterExp2, typename KilogramExp2, typename SecondExp2, typename RadianExp2, typename AmpereExp2, typename KelvinExp2, typename ByteExp2, typename TickExp2> + struct ScalarBinaryOpTraits< + mrover::Unit, + mrover::Unit, + internal::scalar_product_op< + mrover::Unit, + mrover::Unit>> { + using U1 = mrover::Unit; + using U2 = mrover::Unit; + using ReturnType = mrover::multiply; + }; + + template<> + struct ScalarBinaryOpTraits< + mrover::Dimensionless, + mrover::Dimensionless, + internal::scalar_product_op> { + using ReturnType = mrover::Dimensionless; + }; + +} // namespace Eigen