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 8b4d833
Show file tree
Hide file tree
Showing 2 changed files with 43 additions and 55 deletions.
67 changes: 12 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,19 @@
#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 {

auto static const PITCH_ROLL_TO_0_1 = (Matrix2<Dimensionless>{} << -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<AdjustMotor>(std::format("{}_adjust", hwName)));
Expand All @@ -67,11 +30,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 +64,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 +105,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 +147,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 8b4d833

Please sign in to comment.