Skip to content

Commit

Permalink
Cleanup arm translator a bit
Browse files Browse the repository at this point in the history
  • Loading branch information
@qhdwight
qhdwight committed Apr 18, 2024
1 parent 4d20d4e commit 4032d59
Show file tree
Hide file tree
Showing 2 changed files with 47 additions and 55 deletions.
71 changes: 16 additions & 55 deletions src/esw/arm_translator_bridge/arm_translator.cpp
View file
Original file line number Diff line number Diff line change
@@ -1,56 +1,23 @@
#include "arm_translator.hpp"

#include <params_utils.hpp>
#include <ros/duration.h>
#include <units/units.hpp>

#include <Eigen/LU>

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<Rep1, Conversion1, MeterExp1, KilogramExp1, SecondExp1, RadianExp1, AmpereExp1, KelvinExp1, ByteExp1, TickExp1>,
mrover::Unit<Rep2, Conversion2, MeterExp2, KilogramExp2, SecondExp2, RadianExp2, AmpereExp2, KelvinExp2, ByteExp2, TickExp2>,
internal::scalar_product_op<
mrover::Unit<Rep1, Conversion1, MeterExp1, KilogramExp1, SecondExp1, RadianExp1, AmpereExp1, KelvinExp1, ByteExp1, TickExp1>,
mrover::Unit<Rep2, Conversion2, MeterExp2, KilogramExp2, SecondExp2, RadianExp2, AmpereExp2, KelvinExp2, ByteExp2, TickExp2>>> {
using U1 = mrover::Unit<Rep1, Conversion1, MeterExp1, KilogramExp1, SecondExp1, RadianExp1, AmpereExp1, KelvinExp1, ByteExp1, TickExp1>;
using U2 = mrover::Unit<Rep2, Conversion2, MeterExp2, KilogramExp2, SecondExp2, RadianExp2, AmpereExp2, KelvinExp2, ByteExp2, TickExp2>;
using ReturnType = mrover::multiply<U1, U2>;
};

template<>
struct ScalarBinaryOpTraits<
mrover::Dimensionless,
mrover::Dimensionless,
internal::scalar_product_op<mrover::Dimensionless>> {
using ReturnType = mrover::Dimensionless;
};

// template<typename Rep1, typename Conversion1, typename MeterExp1, typename KilogramExp1, typename SecondExp1, typename RadianExp1, typename AmpereExp1, typename KelvinExp1, typename ByteExp1, typename TickExp1>
// struct NumTraits<mrover::Unit<Rep1, Conversion1, MeterExp1, KilogramExp1, SecondExp1, RadianExp1, AmpereExp1, KelvinExp1, ByteExp1, TickExp1>> : NumTraits<float> {
// using U = mrover::Unit<Rep1, Conversion1, MeterExp1, KilogramExp1, SecondExp1, RadianExp1, AmpereExp1, KelvinExp1, ByteExp1, TickExp1>;
// 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 <params_utils.hpp>
#include <units/units.hpp>
#include <units/units_eigen.hpp>

namespace mrover {

// Maps pitch and roll values to the DE0 and DE1 motors outputs
// For example when only pitching the motor, both controllers should be moving in the same direction
// When rolling, the controllers should move in opposite directions
auto static const PITCH_ROLL_TO_0_1 = (Matrix2<Dimensionless>{} << -1, -1, -1, 1).finished();
Dimensionless static constexpr PITCH_ROLL_TO_01_SCALE{40};

// How often we send an adjust command to the DE motors
// This updates the quadrature motor source on the Moteus based on the absolute encoder readings
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<AdjustMotor>(std::format("{}_adjust", hwName)));
Expand All @@ -67,11 +34,9 @@ namespace mrover {
mPositionPub = std::make_unique<ros::Publisher>(nh.advertise<Position>("arm_hw_position_cmd", 1));
mJointDataPub = std::make_unique<ros::Publisher>(nh.advertise<sensor_msgs::JointState>("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<Dimensionless>{} << -1, -1, -1, 1).finished();

auto findJointByName(std::vector<std::string> const& names, std::string const& name) -> std::optional<std::size_t> {
auto it = std::ranges::find(names, name);
return it == names.end() ? std::nullopt : std::make_optional(std::distance(names.begin(), it));
Expand Down Expand Up @@ -103,10 +68,6 @@ namespace mrover {
mThrottlePub->publish(throttle);
}

// constexpr Dimensionless PITCH_ROLL_TO_01_SCALE = 40;
// Matrix2<Dimensionless> 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!");
Expand Down Expand Up @@ -148,7 +109,7 @@ namespace mrover {
std::size_t pitchIndex = jointDePitchIndex.value(), rollIndex = jointDeRollIndex.value();

Vector2<RadiansPerSecond> pitchRoll{msg->positions.at(pitchIndex), msg->positions.at(rollIndex)};
Vector2<RadiansPerSecond> motorPositions = 40 * PITCH_ROLL_TO_0_1 * pitchRoll;
Vector2<RadiansPerSecond> motorPositions = PITCH_ROLL_TO_01_SCALE * PITCH_ROLL_TO_0_1 * pitchRoll;

position.names[pitchIndex] = "joint_de_0";
position.names[rollIndex] = "joint_de_1";
Expand Down Expand Up @@ -190,7 +151,7 @@ namespace mrover {
auto ArmTranslator::updateDeOffsets(ros::TimerEvent const&) -> void {
if (!mJointDePitchRoll) return;

Vector2<Radians> motorPositions = 40 * PITCH_ROLL_TO_0_1 * mJointDePitchRoll.value();
Vector2<Radians> motorPositions = PITCH_ROLL_TO_01_SCALE * PITCH_ROLL_TO_0_1 * mJointDePitchRoll.value();
{
AdjustMotor adjust;
adjust.request.name = "joint_de_0";
Expand Down
31 changes: 31 additions & 0 deletions src/util/units/units_eigen.hpp
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,31 @@
#pragma once

#include "units.hpp"

#include <Eigen/Core>

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<Rep1, Conversion1, MeterExp1, KilogramExp1, SecondExp1, RadianExp1, AmpereExp1, KelvinExp1, ByteExp1, TickExp1>,
mrover::Unit<Rep2, Conversion2, MeterExp2, KilogramExp2, SecondExp2, RadianExp2, AmpereExp2, KelvinExp2, ByteExp2, TickExp2>,
internal::scalar_product_op<
mrover::Unit<Rep1, Conversion1, MeterExp1, KilogramExp1, SecondExp1, RadianExp1, AmpereExp1, KelvinExp1, ByteExp1, TickExp1>,
mrover::Unit<Rep2, Conversion2, MeterExp2, KilogramExp2, SecondExp2, RadianExp2, AmpereExp2, KelvinExp2, ByteExp2, TickExp2>>> {
using U1 = mrover::Unit<Rep1, Conversion1, MeterExp1, KilogramExp1, SecondExp1, RadianExp1, AmpereExp1, KelvinExp1, ByteExp1, TickExp1>;
using U2 = mrover::Unit<Rep2, Conversion2, MeterExp2, KilogramExp2, SecondExp2, RadianExp2, AmpereExp2, KelvinExp2, ByteExp2, TickExp2>;
using ReturnType = mrover::multiply<U1, U2>;
};

template<>
struct ScalarBinaryOpTraits<
mrover::Dimensionless,
mrover::Dimensionless,
internal::scalar_product_op<mrover::Dimensionless>> {
using ReturnType = mrover::Dimensionless;
};

} // namespace Eigen

0 comments on commit 4032d59

Please sign in to comment.