Skip to content

Commit

Permalink
Small edits
Browse files Browse the repository at this point in the history
  • Loading branch information
@BartlomiejK2
BartlomiejK2 committed Jul 24, 2024
1 parent 6f188fa commit eaccd06
Show file tree
Hide file tree
Showing 5 changed files with 57 additions and 51 deletions.
4 changes: 2 additions & 2 deletions src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/MoteusWrapper.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,8 +12,8 @@ class MoteusWrapper: public ActuatorWrapperBase<MoteusWrapper>, protected mjbots
private:

/* Const coefficients for easy radians - rotations transform */
const double rotation_to_radians = 2 * M_PI;
const double radians_to_rotation = 1 / (2 * M_PI); /* Multiplying is faster than dividing */
static const double rotation_to_radians = 2 * M_PI;
static const double radians_to_rotation = 1 / (2 * M_PI); /* Multiplying is faster than dividing */

/* Command structure for moteus object*/
mjbots::moteus::PositionMode::Command position_command_;
Expand Down
4 changes: 4 additions & 0 deletions src/meldog_hardware/pi3hat_hardware_interface/include/imu/IMUTransform.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,11 +4,15 @@
#include <array>
#include "../pi3hat/pi3hat.h"
#include <algorithm>
#include <cmath>

namespace IMU
{
class IMUTransform
{
private:
static const double degrees_to_radians = 2 * M_PI / 360.0;

public:

/* Function for transforming transforming IMU data */
Expand Down
16 changes: 8 additions & 8 deletions ...pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -115,11 +115,11 @@ namespace pi3hat_hardware_interface
std::vector<int> actuator_joint_map_;

/* Actuator states and commands */
std::vector<actuator_wrappers::ActuatorState> hw_actuator_states_;
std::vector<actuator_wrappers::ActuatorCommand> hw_actuator_commands_;
std::vector<actuator_wrappers::ActuatorState> actuator_states_;
std::vector<actuator_wrappers::ActuatorCommand> actuator_commands_;

/* For transmission interface */
std::vector<actuator_wrappers::ActuatorCommand> hw_actuator_transmission_passthrough_;
std::vector<actuator_wrappers::ActuatorCommand> actuator_transmission_passthrough_;

/* Actuator Wrappers (HERE change to your own wrapper) */
std::vector<actuator_wrappers::MoteusWrapper> moteus_wrappers;
Expand All @@ -129,10 +129,10 @@ namespace pi3hat_hardware_interface
using JointCommand = actuator_wrappers::ActuatorCommand;

/* Joint states and commands (for transmissions)*/
std::vector<JointState> hw_joint_states_;
std::vector<JointCommand> hw_joint_commands_;
std::vector<JointState> joint_states_;
std::vector<JointCommand> joint_commands_;
/* For transmission interface */
std::vector<JointCommand> hw_joint_transmission_passthrough_;
std::vector<JointCommand> joint_transmission_passthrough_;

/* Function for choosing wrappers (here u can add your own wrapper)
Remember to change this function in source code */
Expand All @@ -147,7 +147,7 @@ namespace pi3hat_hardware_interface
// TODO: Uporządkuj wcześniej silniki względem id
for(int i = 0; i < number_of_actuators; i++)
{
actuator_wrappers[i].command_to_tx_frame(tx_can_frames_[i], hw_actuator_commands_[i]);
actuator_wrappers[i].command_to_tx_frame(tx_can_frames_[i], actuator_commands_[i]);
}
};

Expand All @@ -157,7 +157,7 @@ namespace pi3hat_hardware_interface
for(int i = 0; i < number_of_actuators; i++)
{
int joint_index = actuator_joint_map_[rx_can_frames_[i].id];
actuator_wrappers[joint_index].rx_frame_to_state(rx_can_frames_[i], hw_actuator_states_[joint_index]);
actuator_wrappers[joint_index].rx_frame_to_state(rx_can_frames_[i], actuator_states_[joint_index]);
}
}

Expand Down
5 changes: 5 additions & 0 deletions src/meldog_hardware/pi3hat_hardware_interface/src/imu/IMUTransform.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,4 +33,9 @@ void IMUTransform::transform_attitude(mjbots::pi3hat::Attitude& attitude)

attitude.attitude.y = -attitude.attitude.z;
attitude.attitude.z = temp_y;

/* degrees/s to radians/s conversion */
attitude.rate_dps.x *= degrees_to_radians;
attitude.rate_dps.y *= degrees_to_radians;
attitude.rate_dps.z *= degrees_to_radians;
}
79 changes: 38 additions & 41 deletions src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,47 +15,44 @@ hardware_interface::CallbackReturn Pi3HatHardwareInterface::on_init(const hardwa
logger_ = std::make_unique<rclcpp::Logger>(
rclcpp::get_logger("Pi3HatHardwareInterface"));

hw_actuator_commands_.resize(info_.joints.size());
hw_actuator_states_.resize(info_.joints.size());
hw_actuator_transmission_passthrough_.resize(info_.joints.size());
actuator_commands_.resize(info_.joints.size());
actuator_states_.resize(info_.joints.size());
actuator_transmission_passthrough_.resize(info_.joints.size());


hw_joint_commands_.resize(info_.joints.size());
hw_joint_states_.resize(info_.joints.size());
hw_joint_transmission_passthrough_.resize(info_.joints.size());
joint_commands_.resize(info_.joints.size());
joint_states_.resize(info_.joints.size());
joint_transmission_passthrough_.resize(info_.joints.size());

actuator_joint_map_.resize(info_.joints.size());

/* Prepare wrappers */
for (const hardware_interface::ComponentInfo &joint : info_.joints)
{
actuator_wrappers::ActuatorParameters params;
params.bus_ = std::stoi(joint.parameters.at("can_bus"));
params.id_ = std::stoi(joint.parameters.at("can_id"));
params.direction_ = std::stoi(joint.parameters.at("direction"));
params.position_offset_ = std::stod(joint.parameters.at("position_offset"));
std::string type_string = joint.parameters.at("motor_type");

WrapperType type = choose_actuator_wrapper(type_string);
params.bus_ = std::stoi(joint.parameters.at("motor_can_bus"));
params.id_ = std::stoi(joint.parameters.at("motor_can_id"));

for(const auto& command_interface: joint.command_interfaces)
{
if(command_interface.name == hardware_interface::HW_IF_POSITION)
{
params.position_max_ = std::stod(command_interface.max);
params.position_min_ = std::stod(command_interface.min);
params.direction_ = std::stoi(joint.parameters.at("motor_direction"));
params.position_offset_ = std::stod(joint.parameters.at("motor_position_offset"));
params.position_max_ = std::stod(joint.parameters.at("motor_position_max"));
params.position_min_ = std::stod(joint.parameters.at("motor_position_min"));
params.velocity_max_ = std::stod(joint.parameters.at("motor_velocity_max"));
params.torque_max_ = std::stod(joint.parameters.at("motor_torque_max"));

}
else if(command_interface.name == hardware_interface::HW_IF_VELOCITY)
{
params.velocity_max_ = std::stod(command_interface.max);
}
else if(command_interface.name == hardware_interface::HW_IF_EFFORT)
{
params.torque_max_ = std::stod(command_interface.max);
}
}
actuator_joint_map_.insert(actuator_joint_map_.begin() + params.id_,
moteus_wrappers.size());

std::string type_string = joint.parameters.at("motor_type");
WrapperType type = choose_actuator_wrapper(type_string);
add_actuator_wrapper(params, type);

}


/* Prepare transmissions */
create_transmission_interface(info_);

/* Standard CAN config */
mjbots::pi3hat::Pi3Hat::CanConfiguration can_config;

Expand All @@ -81,9 +78,9 @@ hardware_interface::CallbackReturn Pi3HatHardwareInterface::on_init(const hardwa
// Set up the CAN configuration
for (size_t i = 0; i < info_.joints.size(); i++) // TUTAJ POMYSL O CO CMN
{
config.can[hw_actuator_can_buses_[i] - 1] = can_config;
pi3hat_input_.tx_can[i].id = hw_actuator_can_ids_[i];
pi3hat_input_.tx_can[i].bus = hw_actuator_can_buses_[i];
config.can[actuator_can_buses_[i] - 1] = can_config;
pi3hat_input_.tx_can[i].id = actuator_can_ids_[i];
pi3hat_input_.tx_can[i].bus = actuator_can_buses_[i];
pi3hat_input_.tx_can[i].expect_reply = true;
}
// Initialize the Pi3Hat
Expand All @@ -93,10 +90,10 @@ hardware_interface::CallbackReturn Pi3HatHardwareInterface::on_init(const hardwa
for(size_t i = 0; i < info_.joints.size(); i++)
{
auto options = mjbots::moteus::Controller::Options();
options.id = hw_actuator_can_ids_[i];
options.bus = hw_actuator_can_buses_[i];
options.id = actuator_can_ids_[i];
options.bus = actuator_can_buses_[i];
auto moteus_wrapper = MoteusWrapper(options, tx_can_frames_[i], rx_can_frames_[i],
hw_actuator_commands_[i], hw_actuator_states_[i], mjbots::moteus::PositionMode::Command());
actuator_commands_[i], actuator_states_[i], mjbots::moteus::PositionMode::Command());
moteus_wrappers.push_back(moteus_wrapper);
}
}
Expand All @@ -110,30 +107,30 @@ hardware_interface::CallbackReturn Pi3HatHardwareInterface::on_init(const hardwa
void Pi3HatHardwareInterface::append_joint_handles(std::vector<transmission_interface::JointHandle>& joint_handles, const std::string joint_name, const int joint_index)
{
transmission_interface::JointHandle joint_handle_position(joint_name, hardware_interface::HW_IF_POSITION,
&hw_joint_transmission_passthrough_[joint_index].position_);
&joint_transmission_passthrough_[joint_index].position_);
joint_handles.push_back(joint_handle_position);

transmission_interface::JointHandle joint_handle_velocity(joint_name, hardware_interface::HW_IF_VELOCITY,
&hw_joint_transmission_passthrough_[joint_index].velocity_);
&joint_transmission_passthrough_[joint_index].velocity_);
joint_handles.push_back(joint_handle_velocity);

transmission_interface::JointHandle joint_handle_torque(joint_name, hardware_interface::HW_IF_EFFORT,
&hw_joint_transmission_passthrough_[joint_index].torque_);
&joint_transmission_passthrough_[joint_index].torque_);
joint_handles.push_back(joint_handle_torque);
}

void Pi3HatHardwareInterface::append_actuator_handles(std::vector<transmission_interface::ActuatorHandle>& actuator_handles, const std::string actuator_name, const int actuator_index)
{
transmission_interface::ActuatorHandle actuator_handle_position(actuator_name, hardware_interface::HW_IF_POSITION,
&hw_actuator_transmission_passthrough_[actuator_index].position_);
&actuator_transmission_passthrough_[actuator_index].position_);
actuator_handles.push_back(actuator_handle_position);

transmission_interface::ActuatorHandle actuator_handle_velocity(actuator_name, hardware_interface::HW_IF_VELOCITY,
&hw_actuator_transmission_passthrough_[actuator_index].velocity_);
&actuator_transmission_passthrough_[actuator_index].velocity_);
actuator_handles.push_back(actuator_handle_velocity);

transmission_interface::ActuatorHandle actuator_handle_torque(actuator_name, hardware_interface::HW_IF_EFFORT,
&hw_actuator_transmission_passthrough_[actuator_index].torque_);
&actuator_transmission_passthrough_[actuator_index].torque_);
actuator_handles.push_back(actuator_handle_torque);
}

Expand Down

0 comments on commit eaccd06

Please sign in to comment.