78 lines
1.7 KiB
C++
78 lines
1.7 KiB
C++
#pragma once
|
|
|
|
|
|
#include <exception>
|
|
#include <tuple>
|
|
#include <atomic>
|
|
#include <vector>
|
|
#include <memory>
|
|
|
|
#include <dqrobotics/DQ.h>
|
|
|
|
#include <sas_robot_driver/sas_robot_driver.h>
|
|
#include "robot_interface_franka.h"
|
|
|
|
using namespace DQ_robotics;
|
|
using namespace Eigen;
|
|
|
|
namespace sas
|
|
{
|
|
|
|
|
|
struct RobotDriverFrankaConfiguration
|
|
{
|
|
std::string ip_address;
|
|
std::string mode;
|
|
int port;
|
|
double speed;
|
|
};
|
|
|
|
|
|
class RobotDriverFranka: public RobotDriver
|
|
{
|
|
private:
|
|
RobotDriverFrankaConfiguration configuration_;
|
|
|
|
std::shared_ptr<RobotInterfaceFranka> robot_driver_interface_sptr_ = nullptr;
|
|
|
|
//Joint positions
|
|
VectorXd joint_positions_;
|
|
//VectorXd joint_velocities_;
|
|
VectorXd end_effector_pose_;
|
|
std::vector<double> joint_positions_buffer_;
|
|
std::vector<double> end_effector_pose_euler_buffer_;
|
|
std::vector<double> end_effector_pose_homogenous_transformation_buffer_;
|
|
|
|
|
|
|
|
public:
|
|
const static int SLAVE_MODE_JOINT_CONTROL;
|
|
const static int SLAVE_MODE_END_EFFECTOR_CONTROL;
|
|
|
|
RobotDriverFranka(const RobotDriverFranka&)=delete;
|
|
RobotDriverFranka()=delete;
|
|
~RobotDriverFranka();
|
|
|
|
RobotDriverFranka(const RobotDriverFrankaConfiguration& configuration, std::atomic_bool* break_loops);
|
|
|
|
|
|
VectorXd get_joint_positions() override;
|
|
void set_target_joint_positions(const VectorXd& desired_joint_positions_rad) override;
|
|
|
|
VectorXd get_joint_velocities() override;
|
|
void set_target_joint_velocities(const VectorXd& desired_joint_velocities) override;
|
|
|
|
VectorXd get_joint_forces() override;
|
|
|
|
void connect() override;
|
|
void disconnect() override;
|
|
|
|
void initialize() override;
|
|
void deinitialize() override;
|
|
|
|
bool set_end_effector_pose_dq(const DQ& pose);
|
|
DQ get_end_effector_pose_dq();
|
|
|
|
};
|
|
}
|