d109f32963
Executed the following command and accept suggestions. ```console $ nice pre-commit run --show-diff-on-failure --color=always --all-files ```
111 lines
4.1 KiB
C++
111 lines
4.1 KiB
C++
// Copyright (c) 2025, Reazon Holdings, Inc.
|
|
// Copyright (c) 2025, Stogl Robotics Consulting UG (haftungsbeschränkt)
|
|
//
|
|
// Licensed under the Apache License, Version 2.0 (the "License");
|
|
// you may not use this file except in compliance with the License.
|
|
// You may obtain a copy of the License at
|
|
//
|
|
// http://www.apache.org/licenses/LICENSE-2.0
|
|
//
|
|
// Unless required by applicable law or agreed to in writing, software
|
|
// distributed under the License is distributed on an "AS IS" BASIS,
|
|
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
// See the License for the specific language governing permissions and
|
|
// limitations under the License.
|
|
|
|
#pragma once
|
|
|
|
#include <memory>
|
|
#include <string>
|
|
#include <vector>
|
|
|
|
#include "canbus.hpp"
|
|
#include "hardware_interface/handle.hpp"
|
|
#include "hardware_interface/hardware_info.hpp"
|
|
#include "hardware_interface/system_interface.hpp"
|
|
#include "hardware_interface/types/hardware_interface_return_values.hpp"
|
|
#include "motor.hpp"
|
|
#include "motor_control.hpp"
|
|
#include "openarm_hardware/visibility_control.h"
|
|
#include "rclcpp/macros.hpp"
|
|
#include "rclcpp_lifecycle/state.hpp"
|
|
|
|
namespace openarm_hardware {
|
|
|
|
std::vector<DM_Motor_Type> motor_types{
|
|
DM_Motor_Type::DM4340, DM_Motor_Type::DM4340, DM_Motor_Type::DM4340,
|
|
DM_Motor_Type::DM4340, DM_Motor_Type::DM4310, DM_Motor_Type::DM4310,
|
|
DM_Motor_Type::DM4310};
|
|
std::vector<uint16_t> can_device_ids{0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07};
|
|
std::vector<uint16_t> can_master_ids{0x11, 0x12, 0x13, 0x14, 0x15, 0x16, 0x17};
|
|
static const Control_Type CONTROL_MODE = Control_Type::MIT;
|
|
static const std::size_t ARM_DOF = 7;
|
|
static const std::size_t GRIPPER_DOF = 1;
|
|
static const std::size_t TOTAL_DOF = ARM_DOF + GRIPPER_DOF;
|
|
static const std::array<double, TOTAL_DOF> KP = {80.0, 80.0, 20.0, 55.0,
|
|
5.0, 5.0, 5.0, 0.5};
|
|
static const std::array<double, TOTAL_DOF> KD = {2.75, 2.5, 0.7, 0.4,
|
|
0.7, 0.6, 0.5, 0.1};
|
|
static const double START_POS_TOLERANCE_RAD = 0.1;
|
|
static const double POS_JUMP_TOLERANCE_RAD = 3.1415 / 16.0;
|
|
|
|
static const bool USING_GRIPPER = true;
|
|
static const double GRIPPER_REFERENCE_GEAR_RADIUS_M = 0.00853;
|
|
static const double GRIPPER_GEAR_DIRECTION_MULTIPLIER = -1.0;
|
|
static const int GRIPPER_INDEX = TOTAL_DOF - 1;
|
|
|
|
class OpenArmHW : public hardware_interface::SystemInterface {
|
|
public:
|
|
OpenArmHW();
|
|
|
|
TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC
|
|
hardware_interface::CallbackReturn on_init(
|
|
const hardware_interface::HardwareInfo& info) override;
|
|
|
|
TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC
|
|
hardware_interface::CallbackReturn on_configure(
|
|
const rclcpp_lifecycle::State& previous_state) override;
|
|
|
|
TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC
|
|
std::vector<hardware_interface::StateInterface> export_state_interfaces()
|
|
override;
|
|
|
|
TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC
|
|
std::vector<hardware_interface::CommandInterface> export_command_interfaces()
|
|
override;
|
|
|
|
TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC
|
|
hardware_interface::CallbackReturn on_activate(
|
|
const rclcpp_lifecycle::State& previous_state) override;
|
|
|
|
TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC
|
|
hardware_interface::CallbackReturn on_deactivate(
|
|
const rclcpp_lifecycle::State& previous_state) override;
|
|
|
|
TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC
|
|
hardware_interface::return_type read(const rclcpp::Time& time,
|
|
const rclcpp::Duration& period) override;
|
|
|
|
TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC
|
|
hardware_interface::return_type write(
|
|
const rclcpp::Time& time, const rclcpp::Duration& period) override;
|
|
|
|
std::size_t curr_dof = ARM_DOF; // minus gripper
|
|
private:
|
|
std::string prefix_;
|
|
std::unique_ptr<CANBus> canbus_;
|
|
std::unique_ptr<MotorControl> motor_control_;
|
|
std::vector<double> pos_commands_;
|
|
std::vector<double> pos_states_;
|
|
std::vector<double> vel_commands_;
|
|
std::vector<double> vel_states_;
|
|
std::vector<double> tau_ff_commands_;
|
|
std::vector<double> tau_states_;
|
|
std::vector<std::unique_ptr<Motor>> motors_;
|
|
|
|
void refresh_motors();
|
|
bool disable_torque_;
|
|
};
|
|
|
|
} // namespace openarm_hardware
|