qlin/sas_robot_driver_franka
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Added support for configuration file mode

Browse Source
This commit is contained in:
Juancho
2023-06-07 13:36:54 +09:00
parent 5a54538279
commit 07112558fa
4 changed files with 24 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
cfg/sas_robot_driver_franka_1.yaml
View File

@@ -1,4 +1,5 @@
"robot_ip_address": "172.16.0.2"
"robot_mode": "VelocityControl"
"robot_port": 5007
"robot_speed": 20.0
"thread_sampling_time_nsec": 4000000

14
include/sas_robot_driver_franka.h
View File

@@ -22,6 +22,7 @@ namespace sas
struct RobotDriverFrankaConfiguration
{
std::string ip_address;
std::string mode;
int port;
double speed;
};
@@ -42,20 +43,7 @@ private:
std::vector<double> end_effector_pose_euler_buffer_;
std::vector<double> end_effector_pose_homogenous_transformation_buffer_;
//DQ _homogenous_vector_to_dq(const VectorXd& homogenousvector) const;
//VectorXd _dq_to_homogenous_vector(const DQ& pose) const;
//VectorXd _get_end_effector_pose_homogenous_transformation();
//double _sign(const double &a) const;
// void _connect(); //Throws std::runtime_error()
// void _motor_on(); //Throws std::runtime_error()
// void _motor_off() noexcept; //No exceptions should be thrown in the path to turn off the robot
// void _set_speed(const float& speed, const float& acceleration, const float& deacceleration); //Throws std::runtime_error()
// void _slave_mode_on(int mode); //Throws std::runtime_error()
//void _slave_mode_off() noexcept; //No exceptions should be thrown in the path to turn off the robot
public:
const static int SLAVE_MODE_JOINT_CONTROL;

22
src/sas_robot_driver_franka.cpp
View File

@@ -17,8 +17,28 @@ namespace sas
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,
RobotInterfaceFranka::MODE::VelocityControl, //None, PositionControl
mode, //None, PositionControl, VelocityControl
RobotInterfaceFranka::HAND::ON);
}

18
src/sas_robot_driver_franka_node.cpp
View File

@@ -41,12 +41,7 @@ int main(int argc, char **argv)
sas::RobotDriverFrankaConfiguration robot_driver_franka_configuration;
sas::get_ros_param(nh,"/robot_ip_address",robot_driver_franka_configuration.ip_address);
// sas::get_ros_param(nh,"/robot_port", robot_driver_franka_configuration.port);
//sas::get_ros_param(nh,"/robot_speed", robot_driver_franka_configuration.speed);
//robot_driver_franka_configuration.ip_address = "172.16.0.2";
//robot_driver_franka_configuration.port = 0;
//robot_driver_franka_configuration.speed = 0.0;
sas::get_ros_param(nh,"/robot_mode", robot_driver_franka_configuration.mode, false);
sas::RobotDriverROSConfiguration robot_driver_ros_configuration;
@@ -54,17 +49,6 @@ int main(int argc, char **argv)
sas::get_ros_param(nh,"/q_min",robot_driver_ros_configuration.q_min);
sas::get_ros_param(nh,"/q_max",robot_driver_ros_configuration.q_max);
//auto qmin = sas::std_vector_double_to_vectorxd(robot_driver_ros_configuration.q_min);
//auto qmax = sas::std_vector_double_to_vectorxd(robot_driver_ros_configuration.q_max);
//std::vector<double> q_min = {-2.3093,-1.5133,-2.4937, -2.7478,-2.4800, 0.8521, -2.6895};
//std::vector<double> q_max = { 2.3093, 1.5133, 2.4937, -0.4461, 2.4800, 4.2094, 2.6895};
//robot_driver_ros_configuration.thread_sampling_time_nsec = 4000000;
//robot_driver_ros_configuration.q_min = q_min;
//robot_driver_ros_configuration.q_max = q_max;
robot_driver_ros_configuration.robot_driver_provider_prefix = ros::this_node::getName();
try