From 378c3f4ce06caaa1155f0e3d02779127203907b6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juan=20Jos=C3=A9=20Quiroz=20Oma=C3=B1a?= Date: Wed, 31 Jul 2024 13:57:26 +0100 Subject: [PATCH 1/5] [DQ_KinematicController.h] Initialized some variables to match the Matlab implementation --- .../robot_control/DQ_KinematicController.h | 25 +++++++++++++------ 1 file changed, 17 insertions(+), 8 deletions(-) diff --git a/include/dqrobotics/robot_control/DQ_KinematicController.h b/include/dqrobotics/robot_control/DQ_KinematicController.h index da32c42..2373275 100644 --- a/include/dqrobotics/robot_control/DQ_KinematicController.h +++ b/include/dqrobotics/robot_control/DQ_KinematicController.h @@ -1,6 +1,6 @@ #pragma once /** -(C) Copyright 2019-2022 DQ Robotics Developers +(C) Copyright 2019-2024 DQ Robotics Developers This file is part of DQ Robotics. @@ -18,7 +18,16 @@ This file is part of DQ Robotics. along with DQ Robotics. If not, see . Contributors: -- Murilo M. Marinho (murilomarinho@ieee.org) + + 1. Murilo M. Marinho (murilomarinho@ieee.org) + Responsible for the original implementation + + 2. Juan Jose Quiroz Omana + Fixed bug 59 (https://github.com/dqrobotics/python/issues/59) + - Initialized variables that are initialized in the Matlab implementation + of the class DQ_KinematicController.m. + Specifically, I initialized damping, gain, system_reached_stable_region_, + stability_threshold, stability_counter, and stability_counter_max. */ #include @@ -51,16 +60,16 @@ class DQ_KinematicController DQ attached_primitive_; DQ target_primitive_; - double gain_; - double damping_; + double gain_{0}; + double damping_{0}; - bool system_reached_stable_region_; + bool system_reached_stable_region_{false}; VectorXd last_control_signal_; VectorXd last_error_signal_; - double stability_threshold_; - int stability_counter_; - int stability_counter_max_; + double stability_threshold_{0}; + int stability_counter_{0}; + int stability_counter_max_{10}; //For backwards compatibility reasons, to be removed DQ_Kinematics* _get_robot_ptr() const; From ac023e6dd0fb2d252b92f820b7405e914907dfff Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juan=20Jos=C3=A9=20Quiroz=20Oma=C3=B1a?= Date: Wed, 31 Jul 2024 14:07:48 +0100 Subject: [PATCH 2/5] [DQ_ClassicQPController.cpp] Added a default damping to match the DQ_ClassicQPController.m class --- src/robot_control/DQ_ClassicQPController.cpp | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) diff --git a/src/robot_control/DQ_ClassicQPController.cpp b/src/robot_control/DQ_ClassicQPController.cpp index 6243e32..719f6cd 100644 --- a/src/robot_control/DQ_ClassicQPController.cpp +++ b/src/robot_control/DQ_ClassicQPController.cpp @@ -1,5 +1,5 @@ /** -(C) Copyright 2019 DQ Robotics Developers +(C) Copyright 2019-2024 DQ Robotics Developers This file is part of DQ Robotics. @@ -17,7 +17,14 @@ This file is part of DQ Robotics. along with DQ Robotics. If not, see . Contributors: -- Murilo M. Marinho (murilomarinho@ieee.org) + + 1. Murilo M. Marinho (murilomarinho@ieee.org) + Responsible for the original implementation + + 2. Juan Jose Quiroz Omana + Fixed bug 59 (https://github.com/dqrobotics/python/issues/59) + - Initialized a default damping to match the Matlab implementation + of the class DQ_ClassicQPController.m */ #include @@ -28,14 +35,14 @@ namespace DQ_robotics DQ_ClassicQPController::DQ_ClassicQPController(DQ_Kinematics* robot, DQ_QuadraticProgrammingSolver* solver): DQ_QuadraticProgrammingController (robot,solver) { - + set_damping(1e-3); // Default damping. } DQ_ClassicQPController::DQ_ClassicQPController(const std::shared_ptr &robot, const std::shared_ptr &solver): DQ_QuadraticProgrammingController(robot,solver) { - + set_damping(1e-3); // Default damping. } MatrixXd DQ_ClassicQPController::compute_objective_function_symmetric_matrix(const MatrixXd &J, const VectorXd&) From 141cde5d7d2ff828090588056210bdd3700d7b46 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juan=20Jos=C3=A9=20Quiroz=20Oma=C3=B1a?= Date: Fri, 2 Aug 2024 10:55:02 +0100 Subject: [PATCH 3/5] [DQ_KinematicController.h] Removed from the header the initialization of variables that are initialized in the constructor --- .../robot_control/DQ_KinematicController.h | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/include/dqrobotics/robot_control/DQ_KinematicController.h b/include/dqrobotics/robot_control/DQ_KinematicController.h index 2373275..0bc6918 100644 --- a/include/dqrobotics/robot_control/DQ_KinematicController.h +++ b/include/dqrobotics/robot_control/DQ_KinematicController.h @@ -60,16 +60,16 @@ class DQ_KinematicController DQ attached_primitive_; DQ target_primitive_; - double gain_{0}; - double damping_{0}; + double gain_; + double damping_; - bool system_reached_stable_region_{false}; + bool system_reached_stable_region_; VectorXd last_control_signal_; VectorXd last_error_signal_; - double stability_threshold_{0}; - int stability_counter_{0}; - int stability_counter_max_{10}; + double stability_threshold_; + int stability_counter_; + int stability_counter_max_; //For backwards compatibility reasons, to be removed DQ_Kinematics* _get_robot_ptr() const; From cefc6d7937b4ac2c317a856495dee617cc456b6b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juan=20Jos=C3=A9=20Quiroz=20Oma=C3=B1a?= Date: Fri, 2 Aug 2024 10:57:01 +0100 Subject: [PATCH 4/5] [DQ_KinematicController.cpp] Added the initialization of the damping attribute in the constructor. In addition, I fixed the initialization order of the attributes last control and last error signal. --- src/robot_control/DQ_KinematicController.cpp | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/src/robot_control/DQ_KinematicController.cpp b/src/robot_control/DQ_KinematicController.cpp index a44d698..4704aff 100644 --- a/src/robot_control/DQ_KinematicController.cpp +++ b/src/robot_control/DQ_KinematicController.cpp @@ -54,15 +54,16 @@ DQ_KinematicController::DQ_KinematicController(const std::shared_ptr Date: Wed, 18 Sep 2024 10:21:15 +0100 Subject: [PATCH 5/5] Revert "[DQ_KinematicController.h] Initialized some variables to match the Matlab implementation" This reverts commit 378c3f4ce06caaa1155f0e3d02779127203907b6. --- .../robot_control/DQ_KinematicController.h | 13 ++----------- 1 file changed, 2 insertions(+), 11 deletions(-) diff --git a/include/dqrobotics/robot_control/DQ_KinematicController.h b/include/dqrobotics/robot_control/DQ_KinematicController.h index 0bc6918..da32c42 100644 --- a/include/dqrobotics/robot_control/DQ_KinematicController.h +++ b/include/dqrobotics/robot_control/DQ_KinematicController.h @@ -1,6 +1,6 @@ #pragma once /** -(C) Copyright 2019-2024 DQ Robotics Developers +(C) Copyright 2019-2022 DQ Robotics Developers This file is part of DQ Robotics. @@ -18,16 +18,7 @@ This file is part of DQ Robotics. along with DQ Robotics. If not, see . Contributors: - - 1. Murilo M. Marinho (murilomarinho@ieee.org) - Responsible for the original implementation - - 2. Juan Jose Quiroz Omana - Fixed bug 59 (https://github.com/dqrobotics/python/issues/59) - - Initialized variables that are initialized in the Matlab implementation - of the class DQ_KinematicController.m. - Specifically, I initialized damping, gain, system_reached_stable_region_, - stability_threshold, stability_counter, and stability_counter_max. +- Murilo M. Marinho (murilomarinho@ieee.org) */ #include