141 lines
3.8 KiB
C++
141 lines
3.8 KiB
C++
#include "sas_robot_driver_franka.h"
|
|
#include "sas_clock/sas_clock.h"
|
|
#include <dqrobotics/utils/DQ_Math.h>
|
|
|
|
namespace sas
|
|
{
|
|
RobotDriverFranka::RobotDriverFranka(const RobotDriverFrankaConfiguration &configuration, std::atomic_bool *break_loops):
|
|
RobotDriver(break_loops),
|
|
configuration_(configuration)
|
|
{
|
|
joint_positions_.resize(7);
|
|
joint_velocities_.resize(7);
|
|
joint_forces_.resize(7);
|
|
end_effector_pose_.resize(7);
|
|
joint_positions_buffer_.resize(8,0);
|
|
end_effector_pose_euler_buffer_.resize(7,0);
|
|
end_effector_pose_homogenous_transformation_buffer_.resize(10,0);
|
|
std::cout<<configuration.ip_address<<std::endl;
|
|
|
|
RobotInterfaceFranka::MODE mode = RobotInterfaceFranka::MODE::None;
|
|
|
|
std::cout<<configuration.mode<<std::endl;
|
|
|
|
if (configuration_.mode == std::string("None"))
|
|
{
|
|
mode = RobotInterfaceFranka::MODE::None;
|
|
}else if (configuration_.mode == std::string("PositionControl"))
|
|
{
|
|
mode = RobotInterfaceFranka::MODE::PositionControl;
|
|
}else if (configuration_.mode == std::string("VelocityControl"))
|
|
{
|
|
mode = RobotInterfaceFranka::MODE::VelocityControl;
|
|
}else
|
|
{
|
|
throw std::runtime_error(std::string("Wrong mode. ") + std::string("You used ") + configuration_.mode
|
|
+ std::string(". However, you must use None, PositionControl or VelocityControl"));
|
|
}
|
|
|
|
|
|
robot_driver_interface_sptr_ = std::make_shared<RobotInterfaceFranka>(configuration.ip_address,
|
|
mode, //None, PositionControl, VelocityControl
|
|
RobotInterfaceFranka::HAND::ON);
|
|
}
|
|
|
|
RobotDriverFranka::~RobotDriverFranka()=default;
|
|
|
|
|
|
/**
|
|
* @brief
|
|
*
|
|
*/
|
|
void RobotDriverFranka::connect()
|
|
{
|
|
robot_driver_interface_sptr_ ->connect();
|
|
}
|
|
|
|
|
|
/**
|
|
* @brief
|
|
*
|
|
*/
|
|
void RobotDriverFranka::initialize()
|
|
{
|
|
robot_driver_interface_sptr_->initialize();
|
|
}
|
|
|
|
|
|
/**
|
|
* @brief
|
|
*
|
|
*/
|
|
void RobotDriverFranka::deinitialize()
|
|
{
|
|
robot_driver_interface_sptr_->deinitialize();
|
|
}
|
|
|
|
|
|
/**
|
|
* @brief
|
|
*
|
|
*/
|
|
void RobotDriverFranka::disconnect()
|
|
{
|
|
robot_driver_interface_sptr_->disconnect();
|
|
}
|
|
|
|
|
|
/**
|
|
* @brief
|
|
*
|
|
* @return VectorXd
|
|
*/
|
|
VectorXd RobotDriverFranka::get_joint_positions()
|
|
{
|
|
return robot_driver_interface_sptr_->get_joint_positions();
|
|
}
|
|
|
|
|
|
/**
|
|
* @brief
|
|
*
|
|
* @param desired_joint_positions_rad
|
|
*/
|
|
void RobotDriverFranka::set_target_joint_positions(const VectorXd& desired_joint_positions_rad)
|
|
{
|
|
robot_driver_interface_sptr_->set_target_joint_positions(desired_joint_positions_rad);
|
|
}
|
|
|
|
/**
|
|
* @brief RobotDriverFranka::get_joint_velocities
|
|
* @return
|
|
*/
|
|
VectorXd RobotDriverFranka::get_joint_velocities()
|
|
{
|
|
joint_velocities_ = robot_driver_interface_sptr_->get_joint_velocities();
|
|
return joint_velocities_;
|
|
}
|
|
|
|
|
|
/**
|
|
* @brief RobotDriverFranka::set_target_joint_velocities
|
|
* @param desired_joint_velocities
|
|
*/
|
|
void RobotDriverFranka::set_target_joint_velocities(const VectorXd &desired_joint_velocities)
|
|
{
|
|
desired_joint_velocities_ = desired_joint_velocities;
|
|
robot_driver_interface_sptr_->set_target_joint_velocities(desired_joint_velocities);
|
|
}
|
|
|
|
/**
|
|
* @brief RobotDriverFranka::get_joint_forces
|
|
* @return
|
|
*/
|
|
VectorXd RobotDriverFranka::get_joint_forces()
|
|
{
|
|
joint_forces_ = robot_driver_interface_sptr_->get_joint_forces();
|
|
return joint_forces_;
|
|
}
|
|
|
|
}
|