Updated documentation

Author: Woolfrey

Control/src/SerialKinematicControl.cpp

@@ -2,17 +2,18 @@
  * @file    SerialKinematicControl.cpp
  * @author  Jon Woolfrey
  * @email   [email protected]
- * @date    February 2025
- * @version 1.0
+ * @date    July 2025
+ * @version 1.1
  * @brief   Computes velocity (position) feedback control for a serial link robot arm.
  * 
  * @details This class contains methods for performing velocity control of a serial link robot arm
  *          in both Cartesian and joint space. The fundamental feedforward + feedback control is given by:
  *          control velocity = desired velocity + gain * (desired position - actual position).
  * 
- * @copyright Copyright (c) 2025 Jon Woolfrey
- * 
- * @license GNU General Public License V3
+ * @copyright (c) 2025 Jon Woolfrey
+ *
+ * @license   OSCL - Free for non-commercial open-source use only.
+ *            Commercial use requires a license.
  * 
  * @see https://github.com/Woolfrey/software_robot_library for more information.
  * @see https://github.com/Woolfrey/software_simple_qp for the optimisation algorithm used in the control.
@@ -30,10 +31,14 @@ SerialKinematicControl::track_endpoint_trajectory(const RobotLibrary::Model::Pos
                                                   const Eigen::Vector<double,6>   &desiredVelocity,
                                                   const Eigen::Vector<double,6>   &desiredAcceleration)
 {
-     (void)desiredAcceleration;                                                                     // Not needed in velocity control
-     
+    (void)desiredAcceleration;                                                                      // Not needed in velocity control
+
+    // NOTE: This method saves the magnitude of position and orientation error internally,
+    //       so it can be queried after for analysing performance
+    Eigen::Vector<double,6> poseError = pose_error(desiredPose);
+    
 	return resolve_endpoint_motion(desiredVelocity                                                  // Feedforward term
-	                             + _cartesianStiffness * _endpointPose.error(desiredPose));         // Feedback term
+	                             + _cartesianPoseGain * poseError);                                 // Feedback term
 }
 
   ///////////////////////////////////////////////////////////////////////////////////////////////////
@@ -226,4 +231,4 @@ SerialKinematicControl::compute_control_limits(const unsigned int &jointNumber)
 	return limits;
 }
 
-} }
+} } // namespace