Add motors reflected inertias to the dynamic computations

lib/+wbs/@Contacts/Contacts.m

@@ -34,7 +34,7 @@
         end
 
         function [generalized_total_wrench, wrench_left_foot, wrench_right_foot, base_pose_dot, s_dot] = ...
-                compute_contact(obj, robot, torque, generalized_ext_wrench, base_pose_dot, s_dot)
+                compute_contact(obj, robot, torque, generalized_ext_wrench, motorInertias, base_pose_dot, s_dot)
             % compute_contact Computes the contact forces and the configuration velocity after a (possible) impact
             % INPUTS: - robot: instance of the Robot object
             %         - torque: joint torques
@@ -46,7 +46,7 @@
 
             % collecting robot quantities
             h = robot.get_bias_forces();
-            M = robot.get_mass_matrix();
+            M = robot.get_mass_matrix(motorInertias);
             [J_feet, JDot_nu_feet] = obj.compute_J_and_JDot_nu(robot);
             % compute the vertical distance of every vertex from the ground
             contact_points = obj.compute_contact_points(robot);

lib/+wbs/@Robot/Robot.m

@@ -2,7 +2,7 @@
     %ROBOT The Robot class exploits the iDynTree wrappers to compute Kinematic and Dynamic quantities.
     % Robot Methods:
     %    set_robot_state - Sets the robot state with kinematic information
-    %    get_mass_matrix - Returns the mass matrix
+    %    get_mass_matrix - Returns the mass matrix with or without the motor reflected inertias
     %    get_bias_forces - Returns the bias force
     %    get_feet_jacobians - Returns the Jacobians of the feet
     %    get_feet_JDot_nu - Returns the Jacobian derivative of the feet multiplied by the configuration velocity
@@ -19,6 +19,8 @@
         KinDynModel; % kynDyn robot model
         g; % gravity vector
         M_iDyn; % mass matrix iDynTree
+        useMotorReflectedInertias; % Adds the reflected inetias to the mass matrix
+        T; % motor coupling matrix
         J_LFoot_iDyntree; % Jacobian relative to left foot
         J_RFoot_iDyntree; % Jacobian relative to right foot
         JDot_nu_LFoot_iDyntree; % \dot{J} \nu relative to left foot
@@ -51,6 +53,8 @@
             obj.RFoot_frameID = obj.KinDynModel.kinDynComp.getFrameIndex(config.robotFrames.RIGHT_FOOT);
             obj.h_iDyn = iDynTree.FreeFloatingGeneralizedTorques(obj.KinDynModel.kinDynComp.model);
             obj.M_iDyn = iDynTree.MatrixDynSize();
+            obj.useMotorReflectedInertias = config.SIMULATE_MOTOR_REFLECTED_INERTIA;
+            obj.T = config.MOTOR_REFLECTED_INERTIA_COUPLING;
             obj.NDOF = obj.KinDynModel.NDOF;
             obj.S = [zeros(6, obj.KinDynModel.NDOF); eye(obj.KinDynModel.NDOF)];
         end
@@ -65,15 +69,19 @@ function set_robot_state(obj, w_H_b, s, base_pose_dot, s_dot)
                 base_pose_dot, s_dot, obj.g);
         end
 
-        function M = get_mass_matrix(obj)
+        function M = get_mass_matrix(obj,motorInertias)
             % get_mass_matrix Returns the mass matrix
             % OUTPUT: - M: mass matrix
             if (~obj.KinDynModel.kinDynComp.getFreeFloatingMassMatrix(obj.M_iDyn))
                 error('[Robot: get_mass_matrix] Unable to retrieve the mass matrix');
             end
 
-            M = obj.M_iDyn.toMatlab;
-
+            % Add the reflected inertia if the feature is activated
+            if obj.useMotorReflectedInertias
+                M = obj.M_iDyn.toMatlab + obj.T*diag([zeros(6,1);motorInertias]);
+            else
+                M = obj.M_iDyn.toMatlab;
+            end
         end
 
         function h = get_bias_forces(obj)
@@ -144,14 +152,14 @@ function set_robot_state(obj, w_H_b, s, base_pose_dot, s_dot)
             JDot_nu = JDot_nu_iDyntree.toMatlab;
         end
 
-        function [base_pose_ddot, s_ddot] = forward_dynamics(obj, torque, generalized_total_wrench)
+        function [base_pose_ddot, s_ddot] = forward_dynamics(obj, torque, generalized_total_wrench,motorInertias)
             % forward_dynamics Compute forward dynamics
             % \dot{v} = inv{M}(S*tau + generalized_external_forces - h)
             % INPUT: - torque: the joints torque
             %        - generalized_total_wrench: the sum of the external wrenches in the configuration space
             % OUTPUT: - base_pose_ddot: the linear and angular acceleration of the base
             %         - s_ddot: the joints acceleration
-            M = obj.get_mass_matrix();
+            M = obj.get_mass_matrix(motorInertias);
             h = obj.get_bias_forces();
             ddot = M \ (obj.S * torque + generalized_total_wrench - h);
             base_pose_ddot = ddot(1:6);

lib/RobotDynamicsWithContacts/+robotDynWC/step_block.m

@@ -27,17 +27,17 @@ function setupImpl(obj)
                 obj.robot_config.initialConditions.base_pose_dot, obj.robot_config.initialConditions.s_dot);
         end
 
-        function [w_H_b, s, base_pose_dot, s_dot, wrench_left_foot, wrench_right_foot, kinDynOut] = stepImpl(obj, generalized_ext_wrench, torque)
+        function [w_H_b, s, base_pose_dot, s_dot, wrench_left_foot, wrench_right_foot, kinDynOut] = stepImpl(obj, generalized_ext_wrench, torque, motorInertias)
             % Implement algorithm. Calculate y as a function of input u and
             % discrete states.
 
             % computes the contact quantites and the velocity after a possible impact
             [generalized_total_wrench, wrench_left_foot, wrench_right_foot, base_pose_dot, s_dot] = ...
-                obj.contacts.compute_contact(obj.robot, torque, generalized_ext_wrench, obj.state.base_pose_dot, obj.state.s_dot);
+                obj.contacts.compute_contact(obj.robot, torque, generalized_ext_wrench, motorInertias, obj.state.base_pose_dot, obj.state.s_dot);
             % sets the velocity in the state
             obj.state.set_velocity(base_pose_dot, s_dot);
             % compute the robot acceleration
-            [base_pose_ddot, s_ddot] = obj.robot.forward_dynamics(torque, generalized_total_wrench);
+            [base_pose_ddot, s_ddot] = obj.robot.forward_dynamics(torque, generalized_total_wrench,motorInertias);
             % integrate the dynamics
             [w_H_b, s, base_pose_dot, s_dot] = obj.state.ode_step(base_pose_ddot, s_ddot);
             % update the robot state
@@ -52,7 +52,7 @@ function setupImpl(obj)
             [kinDynOut.w_H_l_sole    , kinDynOut.w_H_r_sole    ] = obj.robot.get_feet_H();
             [kinDynOut.J_l_sole      , kinDynOut.J_r_sole      ] = obj.robot.get_feet_jacobians();
             [kinDynOut.JDot_l_sole_nu, kinDynOut.JDot_r_sole_nu] = obj.robot.get_feet_JDot_nu();
-            kinDynOut.M = obj.robot.get_mass_matrix();
+            kinDynOut.M = obj.robot.get_mass_matrix(motorInertias);
             kinDynOut.h = obj.robot.get_bias_forces();
             kinDynOut.motorGrpI = zeros(obj.robot_config.N_DOF,1);
             kinDynOut.fc = [wrench_left_foot;wrench_right_foot];