107 lines
3.8 KiB
C++
107 lines
3.8 KiB
C++
// Copyright 2025 Enactic, Inc.
|
|
//
|
|
// 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 <vector>
|
|
#include <robot_state.hpp>
|
|
|
|
// Represents the state of a single joint
|
|
// struct JointState {
|
|
// double position = 0.0;
|
|
// double velocity = 0.0;
|
|
// double effort = 0.0;
|
|
// };
|
|
|
|
// Represents the state of a single motor (raw values)
|
|
struct MotorState {
|
|
double position = 0.0;
|
|
double velocity = 0.0;
|
|
double effort = 0.0;
|
|
};
|
|
|
|
// Abstract base class for converting between motor and joint states
|
|
class MotorJointConverter {
|
|
public:
|
|
virtual ~MotorJointConverter() = default;
|
|
|
|
// MotorState vector → JointState vector
|
|
virtual std::vector<JointState> motor_to_joint(const std::vector<MotorState>& motor_states) const = 0;
|
|
|
|
// JointState vector → MotorState vector
|
|
virtual std::vector<MotorState> joint_to_motor(const std::vector<JointState>& joint_states) const = 0;
|
|
|
|
virtual size_t get_joint_count() const = 0;
|
|
};
|
|
|
|
// assume motor num equals to joint num
|
|
class OpenArmJointConverter : public MotorJointConverter {
|
|
public:
|
|
explicit OpenArmJointConverter(size_t joint_count) : joint_count_(joint_count) {
|
|
std::cout << "OpenArm joint converter joinit_count is : " << joint_count << std::endl;
|
|
}
|
|
|
|
std::vector<JointState> motor_to_joint(const std::vector<MotorState>& m) const override {
|
|
// std::cout << "joint num conv : " << m.size() << std::endl;
|
|
|
|
std::vector<JointState> j(m.size());
|
|
for (size_t i = 0; i < m.size(); ++i){
|
|
j[i] = {m[i].position, m[i].velocity, m[i].effort};
|
|
|
|
}
|
|
|
|
return j;
|
|
}
|
|
|
|
std::vector<MotorState> joint_to_motor(const std::vector<JointState>& j) const override {
|
|
std::vector<MotorState> m(j.size());
|
|
for (size_t i = 0; i < j.size(); ++i)
|
|
m[i] = {j[i].position, j[i].velocity, j[i].effort};
|
|
return m;
|
|
}
|
|
|
|
size_t get_joint_count() const override { return joint_count_; }
|
|
|
|
private:
|
|
size_t joint_count_;
|
|
};
|
|
|
|
// assume motor num equals to joint num
|
|
class OpenArmJGripperJointConverter : public MotorJointConverter {
|
|
public:
|
|
explicit OpenArmJGripperJointConverter(size_t joint_count) : joint_count_(joint_count) {
|
|
std::cout << "Gripper joint converter joint_count is : " << joint_count << std::endl;
|
|
}
|
|
|
|
std::vector<JointState> motor_to_joint(const std::vector<MotorState>& m) const override {
|
|
std::vector<JointState> j(m.size());
|
|
for (size_t i = 0; i < m.size(); ++i) {
|
|
j[i] = {m[i].position, m[i].velocity, m[i].effort};
|
|
}
|
|
return j;
|
|
}
|
|
|
|
std::vector<MotorState> joint_to_motor(const std::vector<JointState>& j) const override {
|
|
std::vector<MotorState> m(j.size());
|
|
for (size_t i = 0; i < j.size(); ++i) {
|
|
m[i] = {j[i].position, j[i].velocity, j[i].effort};
|
|
}
|
|
return m;
|
|
}
|
|
|
|
size_t get_joint_count() const override { return joint_count_; }
|
|
|
|
private:
|
|
size_t joint_count_;
|
|
}; |