Updated the headers and the cmake.

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)
 
 
 

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 <https://www.gnu.org/licenses/>.
+#
+# ################################################################
+#
+#   Author: Juan Jose Quiroz Omana, email: juanjqogm@gmail.com
+#
+# ################################################################
+#
+# Contributors:
+#      1. Juan Jose Quiroz Omana (juanjqogm@gmail.com)
+#         - Original implementation
+#
+# ################################################################
+*/
+
+
 #pragma once
 
 #include <dqrobotics/DQ.h>
@@ -13,6 +45,7 @@
 #include "quadratic_program_motion_generator.h"
 #include <dqrobotics/robots/FrankaEmikaPandaRobot.h>
 #include <atomic>
+#include "custom_motion_generation.h"
 
 using namespace DQ_robotics;
 using namespace Eigen;
@@ -63,6 +96,7 @@ private:
     std::shared_ptr<franka::Robot> robot_sptr_;
     std::shared_ptr<franka::Gripper> gripper_sptr_;
     std::unique_ptr<QuadraticProgramMotionGenerator> trajectory_generator_sptr_;
+    std::unique_ptr<CustomMotionGeneration> custom_generator_sptr_;
 
     void _setDefaultRobotBehavior();
     //bool hand_enabled_ = false;

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 <https://www.gnu.org/licenses/>.
+#
+# ################################################################
+#
+#   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 <exception>
 #include <tuple>
 #include <atomic>
@@ -38,16 +69,16 @@ private:
     //Joint positions
     VectorXd joint_positions_;
     //VectorXd joint_velocities_;
-    VectorXd end_effector_pose_;
+    //VectorXd end_effector_pose_;
-    std::vector<double> joint_positions_buffer_;
+    //std::vector<double> joint_positions_buffer_;
-    std::vector<double> end_effector_pose_euler_buffer_;
+    //std::vector<double> end_effector_pose_euler_buffer_;
-    std::vector<double> end_effector_pose_homogenous_transformation_buffer_;
+    //std::vector<double> end_effector_pose_homogenous_transformation_buffer_;
 
 
 
 public:
-    const static int SLAVE_MODE_JOINT_CONTROL;
+    //const static int SLAVE_MODE_JOINT_CONTROL;
-    const static int SLAVE_MODE_END_EFFECTOR_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);
+    //bool set_end_effector_pose_dq(const DQ& pose);
-    DQ get_end_effector_pose_dq();
+    //DQ get_end_effector_pose_dq();
     
 };
 }

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 <https://www.gnu.org/licenses/>.
+#
+# ################################################################
+#
+#   Author: Juan Jose Quiroz Omana, email: juanjqogm@gmail.com
+#
+# ################################################################
+#
+# Contributors:
+#      1. Juan Jose Quiroz Omana (juanjqogm@gmail.com)
+#         - Original implementation.
+#
+# ################################################################
+*/
+
 #include "mainwindow.h"
 
 #include <QApplication>

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 <https://www.gnu.org/licenses/>.
+#
+# ################################################################
+#
+#   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 <QMessageBox>

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 <https://www.gnu.org/licenses/>.
+#
+# ################################################################
+#
+#   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<QuadraticProgramMotionGenerator>(1.0, q, q_dot, q);
 
@@ -372,12 +406,13 @@ void RobotInterfaceFranka::_start_joint_position_control_mode()
     VectorXd K2 = (VectorXd(7)<<n2, n2, n2, n2, n2, n2, n2).finished();
     VectorXd K1 = (VectorXd(7)<<n1, n1, n1, n1, 3*n1, 3*n1, 3*n1).finished();
     trajectory_generator_sptr_->set_diagonal_gains(K1, K2);
+    */
+
+    custom_generator_sptr_ = std::make_unique<CustomMotionGeneration>(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++;
+
 
 }
 

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 <https://www.gnu.org/licenses/>.
+#
+# ################################################################
+#
+#   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 <dqrobotics/utils/DQ_Math.h>
@@ -11,10 +44,10 @@ namespace sas
         joint_positions_.resize(7);
         joint_velocities_.resize(7);
         joint_forces_.resize(7);
-        end_effector_pose_.resize(7);
+        //end_effector_pose_.resize(7);
-        joint_positions_buffer_.resize(8,0);
+        //joint_positions_buffer_.resize(8,0);
-        end_effector_pose_euler_buffer_.resize(7,0);
+        //end_effector_pose_euler_buffer_.resize(7,0);
-        end_effector_pose_homogenous_transformation_buffer_.resize(10,0);
+        //end_effector_pose_homogenous_transformation_buffer_.resize(10,0);
         std::cout<<configuration.ip_address<<std::endl;
 
         RobotInterfaceFranka::MODE mode = RobotInterfaceFranka::MODE::None;

src/sas_robot_driver_franka_node.cpp

@@ -1,5 +1,35 @@
-//#include "ros/ros.h"
+/*
-//#include "std_msgs/String.h"
+# 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 <https://www.gnu.org/licenses/>.
+#
+# ################################################################
+#
+#   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 <sstream>