diff --git a/CMakeLists.txt b/CMakeLists.txt index 79910df..0f1787e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -61,13 +61,19 @@ target_link_libraries(QuadraticProgramMotionGenerator dqrobotics ConstraintsManager) +add_library(CustomMotionGeneration src/custom_motion_generation.cpp) +target_link_libraries(CustomMotionGeneration + qpOASES + dqrobotics + ConstraintsManager) add_library(robot_interface_franka src/robot_interface_franka.cpp) target_link_libraries(robot_interface_franka Franka::Franka dqrobotics MotionGenerator - QuadraticProgramMotionGenerator) + QuadraticProgramMotionGenerator + CustomMotionGeneration) diff --git a/include/robot_interface_franka.h b/include/robot_interface_franka.h index ccd9a77..2bfda17 100644 --- a/include/robot_interface_franka.h +++ b/include/robot_interface_franka.h @@ -1,3 +1,35 @@ +/* +# Copyright (c) 2023 Juan Jose Quiroz Omana +# +# This file is part of sas_robot_driver_franka. +# +# sas_robot_driver_franka is free software: you can redistribute it and/or modify +# it under the terms of the GNU Lesser General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# sas_robot_driver_franka is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU Lesser General Public License for more details. +# +# You should have received a copy of the GNU Lesser General Public License +# along with sas_robot_driver. If not, see . +# +# ################################################################ +# +# Author: Juan Jose Quiroz Omana, email: juanjqogm@gmail.com +# +# ################################################################ +# +# Contributors: +# 1. Juan Jose Quiroz Omana (juanjqogm@gmail.com) +# - Original implementation +# +# ################################################################ +*/ + + #pragma once #include @@ -13,6 +45,7 @@ #include "quadratic_program_motion_generator.h" #include #include +#include "custom_motion_generation.h" using namespace DQ_robotics; using namespace Eigen; @@ -63,6 +96,7 @@ private: std::shared_ptr robot_sptr_; std::shared_ptr gripper_sptr_; std::unique_ptr trajectory_generator_sptr_; + std::unique_ptr custom_generator_sptr_; void _setDefaultRobotBehavior(); //bool hand_enabled_ = false; diff --git a/include/sas_robot_driver_franka.h b/include/sas_robot_driver_franka.h index e4662b2..7e9a302 100644 --- a/include/sas_robot_driver_franka.h +++ b/include/sas_robot_driver_franka.h @@ -1,6 +1,37 @@ +/* +# Copyright (c) 2023 Juan Jose Quiroz Omana +# +# This file is part of sas_robot_driver_franka. +# +# sas_robot_driver_franka is free software: you can redistribute it and/or modify +# it under the terms of the GNU Lesser General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# sas_robot_driver_franka is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU Lesser General Public License for more details. +# +# You should have received a copy of the GNU Lesser General Public License +# along with sas_robot_driver. If not, see . +# +# ################################################################ +# +# Author: Juan Jose Quiroz Omana, email: juanjqogm@gmail.com +# +# ################################################################ +# +# Contributors: +# 1. Juan Jose Quiroz Omana (juanjqogm@gmail.com) +# - Adapted from sas_robot_driver_denso.cpp +# (https://github.com/SmartArmStack/sas_robot_driver_denso/blob/master/src/sas_robot_driver_denso.cpp) +# +# ################################################################ +*/ + + #pragma once - - #include #include #include @@ -38,16 +69,16 @@ private: //Joint positions VectorXd joint_positions_; //VectorXd joint_velocities_; - VectorXd end_effector_pose_; - std::vector joint_positions_buffer_; - std::vector end_effector_pose_euler_buffer_; - std::vector end_effector_pose_homogenous_transformation_buffer_; + //VectorXd end_effector_pose_; + //std::vector joint_positions_buffer_; + //std::vector end_effector_pose_euler_buffer_; + //std::vector end_effector_pose_homogenous_transformation_buffer_; public: - const static int SLAVE_MODE_JOINT_CONTROL; - const static int SLAVE_MODE_END_EFFECTOR_CONTROL; + //const static int SLAVE_MODE_JOINT_CONTROL; + //const static int SLAVE_MODE_END_EFFECTOR_CONTROL; RobotDriverFranka(const RobotDriverFranka&)=delete; RobotDriverFranka()=delete; @@ -70,8 +101,8 @@ public: void initialize() override; void deinitialize() override; - bool set_end_effector_pose_dq(const DQ& pose); - DQ get_end_effector_pose_dq(); + //bool set_end_effector_pose_dq(const DQ& pose); + //DQ get_end_effector_pose_dq(); }; } diff --git a/qt/configuration_window/main.cpp b/qt/configuration_window/main.cpp index d49f7d9..fa5346d 100644 --- a/qt/configuration_window/main.cpp +++ b/qt/configuration_window/main.cpp @@ -1,3 +1,34 @@ +/* +# Copyright (c) 2023 Juan Jose Quiroz Omana +# +# This file is part of sas_robot_driver_franka. +# +# sas_robot_driver_franka is free software: you can redistribute it and/or modify +# it under the terms of the GNU Lesser General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# sas_robot_driver_franka is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU Lesser General Public License for more details. +# +# You should have received a copy of the GNU Lesser General Public License +# along with sas_robot_driver. If not, see . +# +# ################################################################ +# +# Author: Juan Jose Quiroz Omana, email: juanjqogm@gmail.com +# +# ################################################################ +# +# Contributors: +# 1. Juan Jose Quiroz Omana (juanjqogm@gmail.com) +# - Original implementation. +# +# ################################################################ +*/ + #include "mainwindow.h" #include diff --git a/qt/configuration_window/mainwindow.cpp b/qt/configuration_window/mainwindow.cpp index d126298..6e1bd2d 100644 --- a/qt/configuration_window/mainwindow.cpp +++ b/qt/configuration_window/mainwindow.cpp @@ -1,3 +1,34 @@ +/* +# Copyright (c) 2023 Juan Jose Quiroz Omana +# +# This file is part of sas_robot_driver_franka. +# +# sas_robot_driver_franka is free software: you can redistribute it and/or modify +# it under the terms of the GNU Lesser General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# sas_robot_driver_franka is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU Lesser General Public License for more details. +# +# You should have received a copy of the GNU Lesser General Public License +# along with sas_robot_driver. If not, see . +# +# ################################################################ +# +# Author: Juan Jose Quiroz Omana, email: juanjqogm@gmail.com +# +# ################################################################ +# +# Contributors: +# 1. Juan Jose Quiroz Omana (juanjqogm@gmail.com) +# - Original implementation. +# +# ################################################################ +*/ + #include "mainwindow.h" #include "./ui_mainwindow.h" #include diff --git a/src/robot_interface_franka.cpp b/src/robot_interface_franka.cpp index 830b4b6..0e285e6 100644 --- a/src/robot_interface_franka.cpp +++ b/src/robot_interface_franka.cpp @@ -1,3 +1,35 @@ +/* +# Copyright (c) 2023 Juan Jose Quiroz Omana +# +# This file is part of sas_robot_driver_franka. +# +# sas_robot_driver_franka is free software: you can redistribute it and/or modify +# it under the terms of the GNU Lesser General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# sas_robot_driver_franka is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU Lesser General Public License for more details. +# +# You should have received a copy of the GNU Lesser General Public License +# along with sas_robot_driver. If not, see . +# +# ################################################################ +# +# Author: Juan Jose Quiroz Omana, email: juanjqogm@gmail.com +# +# ################################################################ +# +# Contributors: +# 1. Juan Jose Quiroz Omana (juanjqogm@gmail.com) +# - Original implementation +# +# ################################################################ +*/ + + #include "robot_interface_franka.h" @@ -364,6 +396,8 @@ void RobotInterfaceFranka::_start_joint_position_control_mode() VectorXd q_dot = VectorXd::Zero(7); _update_status_message("Starting joint position control mode EXPERIMENTAL",verbose_); + + /* trajectory_generator_sptr_ = std::make_unique(1.0, q, q_dot, q); @@ -372,12 +406,13 @@ void RobotInterfaceFranka::_start_joint_position_control_mode() VectorXd K2 = (VectorXd(7)<set_diagonal_gains(K1, K2); + */ + + custom_generator_sptr_ = std::make_unique(0.9, q, q_dot, q); + custom_generator_sptr_->set_proportional_gain(1.0); finish_motion_ = false; - int retry_counter = 0; - int TIMEOUT_IN_MILISECONDS = 5000; - try { robot_sptr_->control( //------------------------------------------------------------ [&initial_position, &time, this](const franka::RobotState& robot_state, @@ -385,7 +420,7 @@ void RobotInterfaceFranka::_start_joint_position_control_mode() time += period.toSec(); double T = period.toSec(); - auto new_q = trajectory_generator_sptr_->compute_new_configuration(desired_joint_positions_, T); + auto new_q = custom_generator_sptr_->compute_new_configuration(desired_joint_positions_, T); if (time == 0.0) { @@ -420,8 +455,8 @@ void RobotInterfaceFranka::_start_joint_position_control_mode() std::cout << e.what() << std::endl; } - std::this_thread::sleep_for(std::chrono::milliseconds(TIMEOUT_IN_MILISECONDS)); - retry_counter++; + + } diff --git a/src/sas_robot_driver_franka.cpp b/src/sas_robot_driver_franka.cpp index 9fb7118..620871a 100644 --- a/src/sas_robot_driver_franka.cpp +++ b/src/sas_robot_driver_franka.cpp @@ -1,3 +1,36 @@ +/* +# Copyright (c) 2023 Juan Jose Quiroz Omana +# +# This file is part of sas_robot_driver_franka. +# +# sas_robot_driver_franka is free software: you can redistribute it and/or modify +# it under the terms of the GNU Lesser General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# sas_robot_driver_franka is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU Lesser General Public License for more details. +# +# You should have received a copy of the GNU Lesser General Public License +# along with sas_robot_driver. If not, see . +# +# ################################################################ +# +# Author: Juan Jose Quiroz Omana, email: juanjqogm@gmail.com +# +# ################################################################ +# +# Contributors: +# 1. Juan Jose Quiroz Omana (juanjqogm@gmail.com) +# - Adapted from sas_robot_driver_denso.cpp +# (https://github.com/SmartArmStack/sas_robot_driver_denso/blob/master/src/sas_robot_driver_denso.cpp) +# +# ################################################################ +*/ + + #include "sas_robot_driver_franka.h" #include "sas_clock/sas_clock.h" #include @@ -11,10 +44,10 @@ namespace sas 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); + //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<. +# +# ################################################################ +# +# Author: Juan Jose Quiroz Omana, email: juanjqogm@gmail.com +# +# ################################################################ +# +# Contributors: +# 1. Juan Jose Quiroz Omana (juanjqogm@gmail.com) +# - Adapted from sas_robot_driver_denso_node.cpp +# (https://github.com/SmartArmStack/sas_robot_driver_denso/blob/master/src/sas_robot_driver_denso_node.cpp) +# +# ################################################################ +*/ + #include