115 lines
4.6 KiB
C++
115 lines
4.6 KiB
C++
#ifndef CMVR_ES_AUBO_ARM_H
|
|
#define CMVR_ES_AUBO_ARM_H
|
|
|
|
#include <atomic>
|
|
#include <memory>
|
|
#include <mutex>
|
|
#include <string>
|
|
#include <vector>
|
|
|
|
#include "cmvr/config/arm_config/arm_config.pb.h"
|
|
#include "devices/arm/robot_arm.h"
|
|
|
|
namespace cmvr::device {
|
|
|
|
class AuboArm final : public RobotArm {
|
|
public:
|
|
explicit AuboArm(const config::RobotArmConfig& cfg);
|
|
~AuboArm() override;
|
|
|
|
std::string typeName() const override { return "AuboARM"; }
|
|
bool init() override;
|
|
bool stop() override;
|
|
|
|
RobotModel getRobotModel() const override { return model_; }
|
|
std::size_t getDof() const override { return model_.dof; }
|
|
ArmState getRobotState() const override;
|
|
JointGroupState getJointState() const override;
|
|
CartesianPose getTcpPose(FrameType frame = FrameType::Base) const override;
|
|
RobotMode getRobotMode() const override;
|
|
SafetyMode getSafetyMode() const override { return SafetyMode::Normal; }
|
|
ControlMode getControlMode() const override { return servo_mode_.load() ? ControlMode::Servo : ControlMode::Position; }
|
|
|
|
Result torqueOn() override;
|
|
Result torqueOff() override;
|
|
Result calibrateZeroQ(const std::string& joint_name) override;
|
|
Result emergencyStop() override;
|
|
Result protectiveStop() override { return emergencyStop(); }
|
|
Result setSpeedScaling(double scaling) override;
|
|
double getSpeedScaling() const override { return speed_scaling_; }
|
|
bool isProtectiveStopped() const override { return false; }
|
|
bool isEmergencyStopped() const override { return emergency_stopped_; }
|
|
bool isFault() const override { return false; }
|
|
|
|
Result moveJ(const JointPositionCommand& target, const MotionOptions& options) override;
|
|
Result speedJ(const JointVelocityCommand& velocity, double acceleration, double duration) override;
|
|
Result stopJ(double acceleration) override;
|
|
Result moveL(const CartesianPose& target, const MotionOptions& options, FrameType frame = FrameType::Base) override;
|
|
Result speedL(const CartesianVelocity& velocity, double acceleration, double duration, FrameType frame = FrameType::Base) override;
|
|
Result stopL(double acceleration) override;
|
|
Result stopMotion() override;
|
|
|
|
Result startServoMode(const ServoOptions& options) override;
|
|
Result servoJ(const JointPositionCommand& target) override;
|
|
Result servoL(const CartesianPose& target, FrameType frame = FrameType::Base) override;
|
|
Result servoSpeedJ(const JointVelocityCommand& velocity) override;
|
|
Result servoSpeedL(const CartesianVelocity& velocity, FrameType frame = FrameType::Base) override;
|
|
Result stopServoMode() override;
|
|
|
|
Result connect(const std::string& ip, int port) override;
|
|
Result disconnect() override;
|
|
bool isConnected() const override { return connected_.load(); }
|
|
Result powerOn() override { return torqueOn(); }
|
|
Result powerOff() override { return torqueOff(); }
|
|
Result brakeRelease() override { return torqueOn(); }
|
|
Result shutdown() override;
|
|
Result clearFault() override { return Result::success(); }
|
|
Result unlockProtectiveStop() override { return Result::success(); }
|
|
Result loadProgram(const std::string& program_name) override;
|
|
Result playProgram() override;
|
|
Result pauseProgram() override;
|
|
Result stopProgram() override;
|
|
|
|
std::vector<double> ik(const std::string& base_link,
|
|
const std::string& ee_link,
|
|
const CartesianPose& pose) override;
|
|
std::shared_ptr<cmvr::IKSolver> kinematicsSolver() const override { return nullptr; }
|
|
CartesianPose fk(const std::string& base_link, const std::string& ee_link) override;
|
|
CartesianPose fk(bool is_tcp = true) override;
|
|
CartesianVelocity getSpeedLCommandTwistBase() const override { return {}; }
|
|
bool busy() const override { return busy_.load(); }
|
|
|
|
private:
|
|
Result unsupported_(const std::string& name) const;
|
|
bool validDof_(std::size_t size, std::string& error) const;
|
|
Result ensureConnected_(const std::string& context) const;
|
|
|
|
#if defined(CMVR_HAS_AUBO_SDK)
|
|
struct SdkState;
|
|
#endif
|
|
|
|
private:
|
|
config::RobotArmConfig cfg_;
|
|
config::VendorRobotArmBackendConfig vendor_cfg_;
|
|
RobotModel model_;
|
|
std::string ip_;
|
|
int port_{30004};
|
|
std::string username_;
|
|
std::string password_;
|
|
double speed_scaling_{1.0};
|
|
ServoOptions servo_options_;
|
|
std::atomic<bool> connected_{false};
|
|
std::atomic<bool> busy_{false};
|
|
std::atomic<bool> servo_mode_{false};
|
|
bool emergency_stopped_{false};
|
|
mutable std::mutex mutex_;
|
|
|
|
#if defined(CMVR_HAS_AUBO_SDK)
|
|
std::unique_ptr<SdkState> sdk_;
|
|
#endif
|
|
};
|
|
|
|
} // namespace cmvr::device
|
|
|
|
#endif // CMVR_ES_AUBO_ARM_H
|