128 lines
6.6 KiB
C++
128 lines
6.6 KiB
C++
//
|
|
// Created by xtkuang on 2025/5/8.
|
|
//
|
|
|
|
#ifndef CMVR_ES_ABSTRACT_ROBOT_H
|
|
#define CMVR_ES_ABSTRACT_ROBOT_H
|
|
#pragma once
|
|
|
|
#include "jsoncpp/json/json.h"
|
|
#include "abstract_device.h"
|
|
#include "utils/solver/qp_solver.h"
|
|
#include "cmvr/msgs/geometry.pb.h"
|
|
#include "cmvr/msgs/motor.pb.h"
|
|
|
|
namespace cmvr::device{
|
|
|
|
struct JointPoint{
|
|
std::string joint_name;
|
|
double rad; // rad
|
|
double vel; // rad / s
|
|
|
|
JointPoint(const std::string& name, double r, double v)
|
|
: joint_name(name), rad(r), vel(v) {}
|
|
JointPoint(const std::string& name, double r)
|
|
: joint_name(name), rad(r), vel(0) {}
|
|
JointPoint()
|
|
: joint_name(""), rad(0), vel(0) {}
|
|
} ;
|
|
|
|
typedef struct {
|
|
std::string joint_name;
|
|
double vel; // rad / s
|
|
} JointVelocityCommand;
|
|
|
|
typedef struct {
|
|
std::string joint_name;
|
|
double current; //
|
|
} JointCurrentCommand;
|
|
|
|
class AbstractRobot: public AbstractDevice {
|
|
public:
|
|
explicit AbstractRobot(const XmlNode &config): AbstractDevice(config) {}
|
|
~AbstractRobot() override=default;
|
|
|
|
virtual int getDOF() { throw std::runtime_error("Not implemented"); }
|
|
|
|
virtual std::vector<std::string> getJointNames() { throw std::runtime_error("Not implemented"); }
|
|
|
|
virtual std::unordered_map<std::string,double> getJointQ() const { throw std::runtime_error("Not implemented"); }
|
|
|
|
/**
|
|
*
|
|
* @param joint_qs 函数会根据 joint_qs 的 joint name 去获取值
|
|
*/
|
|
virtual void getJointQ(std::unordered_map<std::string,double> &joint_qs) const {throw std::runtime_error("Not implemented");}
|
|
virtual std::vector<std::string> getLinkNames() { throw std::runtime_error("Not implemented"); }
|
|
|
|
virtual void getJointsState(std::vector<JointState>& states) = 0;
|
|
virtual void getState(RobotState &state) { throw std::runtime_error("Not implemented"); }
|
|
|
|
virtual math::Pose3d getTransform(std::string &bask_link, std::string &target_link) {throw std::runtime_error("Not implemented");}
|
|
|
|
virtual void torqueOn() { throw std::runtime_error("Not implemented"); }
|
|
virtual void torqueOn(const std::string &joint_name) { throw std::runtime_error("Not implemented"); }
|
|
|
|
virtual void torqueOff() { throw std::runtime_error("Not implemented"); }
|
|
virtual void torqueOff(const std::string &joint_name) { throw std::runtime_error("Not implemented"); }
|
|
|
|
virtual void eStop() { throw std::runtime_error("Not implemented"); }
|
|
|
|
virtual void moveJ(std::vector<double> &joints, double vel=0.5, double acc=0.1) { throw std::runtime_error("Not implemented"); }
|
|
|
|
virtual void moveJ(std::vector<JointPoint> &cmd, double vel=0.5, double acc=0.1) { throw std::runtime_error("Not implemented"); }
|
|
virtual void moveJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d pose,double vel = 0.5, double acc = 0.1) { throw std::runtime_error("Not implemented"); }
|
|
virtual void moveDeltaJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d delta_pose,double vel = 0.5, double acc = 0.1) { throw std::runtime_error("Not implemented"); }
|
|
|
|
virtual void moveJ_IK(math::Pose3d &pose, double vel=0.5, double acc=0.1) { throw std::runtime_error("Not implemented"); }
|
|
|
|
virtual void moveJ_IK(const std::string &base_link, const std::vector<cmvr::ctrl::PoseTarget> &targets, double vel=0.5, double acc=0.1) { throw std::runtime_error("Not implemented"); }
|
|
|
|
virtual void moveL(math::Pose3d &pose, double vel=0.5, double acc=0.1) { throw std::runtime_error("Not implemented"); }
|
|
|
|
virtual void moveL(std::string &base_link, std::vector<cmvr::ctrl::PoseTarget> &targets, double vel=0.5, double acc=0.1) { throw std::runtime_error("Not implemented"); }
|
|
|
|
virtual void moveL(const std::string &base_link, const std::string &ee_link,msgs::Pose3d target_pose, double vel, double acc) { throw std::runtime_error("Not implemented"); }
|
|
virtual void moveDeltaL(const std::string &base_link, const std::string &ee_link,msgs::Pose3d delta_pose, double vel, double acc) { throw std::runtime_error("Not implemented"); }
|
|
|
|
virtual void speedJ(std::string &joint_name, RobotJointIndexDirection dir, double vel, double acc=0.5) { throw std::runtime_error("Not implemented"); }
|
|
|
|
virtual void speedL(RobotCartesian cart, RobotJointIndexDirection dir, double vel, double acc=0.5) { throw std::runtime_error("Not implemented"); }
|
|
|
|
virtual void followJointTrajectory(std::vector<std::vector<double>> &traj, double dt) { throw std::runtime_error("Not implemented"); }
|
|
|
|
virtual void followJointTrajectory(std::vector<std::vector<JointPoint>> &traj, double dt) { throw std::runtime_error("Not implemented"); }
|
|
|
|
virtual void followPoseTrajectory(std::vector<math::Pose3d> &traj, double dt) { throw std::runtime_error("Not implemented"); }
|
|
|
|
virtual void followPoseTrajectory(std::string &base_link, std::vector<std::vector<cmvr::ctrl::PoseTarget>> &targets, double dt) { throw std::runtime_error("Not implemented"); }
|
|
|
|
virtual void servoJ(std::vector<double> &joints, double dt) { throw std::runtime_error("Not implemented"); }
|
|
|
|
virtual void servoJ(std::vector<JointPoint> &joints, double dt) { throw std::runtime_error("Not implemented"); }
|
|
virtual void servoJ(std::vector<JointPoint> &joints, double vel, double dt) { throw std::runtime_error("Not implemented"); }
|
|
virtual void servoJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d pose,double vel = 0.1, double acc = 0.1) { throw std::runtime_error("Not implemented"); }
|
|
virtual void servoDeltaJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d delta_pose,double vel = 0.1, double acc = 0.1) { throw std::runtime_error("Not implemented"); }
|
|
|
|
|
|
virtual void servoL(math::Pose3d &pose, double dt) { throw std::runtime_error("Not implemented"); }
|
|
|
|
virtual void servoL(std::string &base_link, std::vector<cmvr::ctrl::PoseTarget> &targets, double dt) { throw std::runtime_error("Not implemented"); }
|
|
|
|
virtual void calibrateZeroQ(const std::string &joint_name) = 0;
|
|
|
|
void setToolFrame(const std::string& toolFrame)
|
|
{
|
|
toolFrame_ = toolFrame;
|
|
}
|
|
|
|
virtual msgs::Pose3d fk(const std::string &base_link, const std::string &ee_link) = 0;
|
|
virtual std::vector<double> ik(const std::string &base_link, const std::string &ee_link,msgs::Pose3d pose) = 0;
|
|
protected:
|
|
int dof_{};
|
|
RobotState state_{};
|
|
std::string toolFrame_;
|
|
};
|
|
}
|
|
|
|
#endif //CMVR_ES_ABSTRACT_ROBOT_H
|