[DQ_ClassicQPController] Set the default damping to 1e-3 to match the…

… the Matlab. (See #67 and dqrobotics/python#59)

* [DQ_KinematicController.h] Initialized some variables to match the Matlab implementation

* [DQ_ClassicQPController.cpp] Added a default damping to match the DQ_ClassicQPController.m class

* [DQ_KinematicController.h] Removed from the header the initialization of variables that are initialized in the constructor

* [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.

* Revert "[DQ_KinematicController.h] Initialized some variables to match the Matlab implementation"

This reverts commit 378c3f4.

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 <http://www.gnu.org/licenses/>.
 
 Contributors:
-- Murilo M. Marinho ([email protected])
+
+    1. Murilo M. Marinho ([email protected])
+       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 <dqrobotics/robot_control/DQ_ClassicQPController.h>
@@ -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<DQ_Kinematics> &robot,
                                                const std::shared_ptr<DQ_QuadraticProgrammingSolver> &solver):
     DQ_QuadraticProgrammingController(robot,solver)
 {
-
+    set_damping(1e-3); // Default damping.
 }
 
 MatrixXd DQ_ClassicQPController::compute_objective_function_symmetric_matrix(const MatrixXd &J, const VectorXd&)

src/robot_control/DQ_KinematicController.cpp

@@ -54,15 +54,16 @@ DQ_KinematicController::DQ_KinematicController(const std::shared_ptr<DQ_Kinemati
 DQ_KinematicController::DQ_KinematicController():
     robot_(nullptr),
     control_objective_(ControlObjective::None),
+    attached_primitive_(0.0),
+    target_primitive_(0.0),
     gain_(0.0),
-    system_reached_stable_region_(false),
-    last_error_signal_(VectorXd::Zero(1)),//Todo: change this inialization to use empty vector
-    last_control_signal_(VectorXd::Zero(1)),//Todo: change this inialization to use empty vector
+    damping_(0),//Todo: change this inialization to use empty vector
+    system_reached_stable_region_(false),//Todo: change this inialization to use empty vector
+    last_control_signal_(VectorXd::Zero(1)),
+    last_error_signal_(VectorXd::Zero(1)),
     stability_threshold_(0.0),
     stability_counter_(0.0),
-    stability_counter_max_(10.0),
-    attached_primitive_(0.0),
-    target_primitive_(0.0)
+    stability_counter_max_(10.0)
 {
 
 }