diff --git a/lib/+mwbs/+RobotDynamicsWithContacts/+robotDynWC/step_block.m b/lib/+mwbs/+RobotDynamicsWithContacts/+robotDynWC/step_block.m index 6b972cc..c193ba6 100644 --- a/lib/+mwbs/+RobotDynamicsWithContacts/+robotDynWC/step_block.m +++ b/lib/+mwbs/+RobotDynamicsWithContacts/+robotDynWC/step_block.m @@ -6,6 +6,7 @@ robot_config; contact_config; physics_config; + motorReflectedInertiaFormat; OutputBusName = 'bus_name'; end @@ -33,11 +34,11 @@ function setupImpl(obj) % 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, motorInertias, 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,obj); % 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,motorInertias); + [base_pose_ddot, s_ddot] = obj.robot.forward_dynamics(torque, generalized_total_wrench,motorInertias,obj); % 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 +53,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(motorInertias); + kinDynOut.M = obj.robot.get_mass_matrix(motorInertias,obj); kinDynOut.h = obj.robot.get_bias_forces(); kinDynOut.motorGrpI = zeros(size(s)); kinDynOut.fc = [wrench_left_foot;wrench_right_foot]; @@ -120,7 +121,19 @@ function resetImpl(obj) 'simFunc_getWorldTransformRFoot', ... 'simFunc_qpOASES'}; end + end + methods (Access = ?mwbs.Robot) + function Mout = getFormattedReflectedInertia(obj,M,motorInertias) + switch obj.motorReflectedInertiaFormat + case 'matrix' + Mout = M + blkdiag(zeros(6),motorInertias); + case 'vector' + Mout = M + diag([zeros(6,1);motorInertias]); + otherwise + Mout = M; + end + end end end diff --git a/lib/+mwbs/@Contacts/Contacts.m b/lib/+mwbs/@Contacts/Contacts.m index 24ed21c..041ec89 100644 --- a/lib/+mwbs/@Contacts/Contacts.m +++ b/lib/+mwbs/@Contacts/Contacts.m @@ -44,7 +44,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, motorInertias, base_pose_dot, s_dot) + compute_contact(obj, robot, torque, generalized_ext_wrench, motorInertias, base_pose_dot, s_dot,obj_step_block) % compute_contact Computes the contact forces and the configuration velocity after a (possible) impact % INPUTS: - robot: instance of the Robot object % - torque: joint torques @@ -56,7 +56,7 @@ % collecting robot quantities h = robot.get_bias_forces(); - M = robot.get_mass_matrix(motorInertias); + M = robot.get_mass_matrix(motorInertias,obj_step_block); [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); diff --git a/lib/+mwbs/@Robot/Robot.m b/lib/+mwbs/@Robot/Robot.m index d597bc6..083cc2e 100644 --- a/lib/+mwbs/@Robot/Robot.m +++ b/lib/+mwbs/@Robot/Robot.m @@ -55,7 +55,7 @@ function set_robot_state(obj, w_H_b, s, base_pose_dot, s_dot) obj.KinDynModel.kinDynComp = obj.KinDynModel.kinDynComp.setRobotState(w_H_b, s, base_pose_dot, s_dot); end - function M = get_mass_matrix(obj,motorInertias) + function M = get_mass_matrix(obj,motorInertias,obj_step_block) % get_mass_matrix Returns the mass matrix % OUTPUT: - M: mass matrix [ack,M] = obj.KinDynModel.kinDynComp.getFreeFloatingMassMatrix(); @@ -65,7 +65,7 @@ function set_robot_state(obj, w_H_b, s, base_pose_dot, s_dot) % Add the reflected inertia if the feature is activated if obj.useMotorReflectedInertias - M = M + diag([zeros(6,1);motorInertias]); + M = obj_step_block.getFormattedReflectedInertia(M,motorInertias); end end @@ -109,14 +109,14 @@ function set_robot_state(obj, w_H_b, s, base_pose_dot, s_dot) H_RFOOT = obj.KinDynModel.kinDynComp.getWorldTransformLRfoot('RFoot'); end - function [base_pose_ddot, s_ddot] = forward_dynamics(obj, torque, generalized_total_wrench,motorInertias) + function [base_pose_ddot, s_ddot] = forward_dynamics(obj, torque, generalized_total_wrench,motorInertias,obj_step_block) % 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(motorInertias); + M = obj.get_mass_matrix(motorInertias,obj_step_block); h = obj.get_bias_forces(); ddot = M \ (obj.S * torque + generalized_total_wrench - h); base_pose_ddot = ddot(1:6); diff --git a/lib/mwbs_robotDynamicsWithContacts_lib.slx b/lib/mwbs_robotDynamicsWithContacts_lib.slx index 07f6343..004c70d 100644 Binary files a/lib/mwbs_robotDynamicsWithContacts_lib.slx and b/lib/mwbs_robotDynamicsWithContacts_lib.slx differ diff --git a/test_matlab_system_2020b.mdl b/test_matlab_system_2020b.mdl index 6424881..99fe7a7 100644 --- a/test_matlab_system_2020b.mdl +++ b/test_matlab_system_2020b.mdl @@ -7,7 +7,7 @@ Model { NumRootInports 0 NumRootOutports 0 ParameterArgumentNames "" - ComputedModelVersion "1.142" + ComputedModelVersion "1.154" NumModelReferences 0 NumTestPointedSignals 0 NumProvidedFunctions 0 @@ -87,7 +87,7 @@ Model { $ObjectID 3 $ClassName "Simulink.WindowInfo" IsActive [1] - Location [43.0, 23.0, 1749.0, 1097.0] + Location [40.0, 23.0, 1752.0, 1097.0] Object { $PropName "ModelBrowserInfo" $ObjectID 4 @@ -114,16 +114,27 @@ Model { IsTabbed [1] ViewObjType "SimulinkTopLevel" LoadSaveID "0" - Extents [1635.0, 868.0] - ZoomFactor [2.0338983050847457] - Offset [-41.691666666666663, -6.8833333333333542] - SceneRectInView [-41.691666666666663, -6.8833333333333542, 803.875, 426.76666666666671] + Extents [1638.0, 868.0] + ZoomFactor [1.9041986989946778] + Offset [-132.35217391304349, -21.417391304347831] + SceneRectInView [-132.35217391304349, -21.417391304347831, 860.204347826087, 455.83478260869566] } Object { $ObjectID 7 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" + LoadSaveID "197" + Extents [1638.0, 868.0] + ZoomFactor [1.1290322580645162] + Offset [115.39687500000002, -192.39999999999998] + SceneRectInView [115.39687500000002, -192.39999999999998, 1450.8, 768.8] + } + Object { + $ObjectID 8 + IsActive [0] + IsTabbed [0] + ViewObjType "SimulinkSubsys" LoadSaveID "238" Extents [1635.0, 868.0] ZoomFactor [2.1756642409883229] @@ -131,7 +142,7 @@ Model { SceneRectInView [-327.97387892812696, -202.97930927193528, 751.49463285625393, 398.95861854387056] } Object { - $ObjectID 8 + $ObjectID 9 IsActive [0] IsTabbed [0] ViewObjType "SimulinkSubsys" @@ -141,17 +152,6 @@ Model { Offset [-247.28050085616439, -126.48458904109589] SceneRectInView [-247.28050085616439, -126.48458904109589, 670.10787671232879, 355.96917808219177] } - Object { - $ObjectID 9 - IsActive [0] - IsTabbed [0] - ViewObjType "SimulinkSubsys" - LoadSaveID "197" - Extents [1637.0, 868.0] - ZoomFactor [1.1290322580645162] - Offset [32.5662946428572, -192.39999999999998] - SceneRectInView [32.5662946428572, -192.39999999999998, 1449.9142857142856, 768.8] - } Object { $ObjectID 10 IsActive [0] @@ -278,7 +278,7 @@ Model { "ABvAG4AZQBuAHQALwBHAEwAVQBFADIAIAB0AHIAZQBlACAAYwBvAG0AcABvAG4AZQBuAHQAAAAAeQAAA6EAAABrAP////sAAABgAFMAaQBtAHUAb" "ABpAG4AawA6AEUAZABpAHQAbwByADoAUgBlAGYAZQByAGUAbgBjAGUAZABGAGkAbABlAHMALwBSAGUAZgBlAHIAZQBuAGMAZQBkACAARgBpAGwAZ" "QBzAAAAAAD/////AAAAjwD///8AAAABAAAAAAAAAAD8AgAAAAH7AAAAVABHAEwAVQBFADIAOgBQAHIAbwBwAGUAcgB0AHkASQBuAHMAcABlAGMAd" - "ABvAHIALwBQAHIAbwBwAGUAcgB0AHkAIABJAG4AcwBwAGUAYwB0AG8AcgAAAAAA/////wAAAawA////AAAGiQAAA6EAAAABAAAAAgAAAAEAAAAC/" + "ABvAHIALwBQAHIAbwBwAGUAcgB0AHkAIABJAG4AcwBwAGUAYwB0AG8AcgAAAAAA/////wAAAawA////AAAGjAAAA6EAAAABAAAAAgAAAAEAAAAC/" "AAAAAMAAAAAAAAAAQAAADYAYwBvAGwAbABhAHAAcwBpAGIAbABlAFAAYQBuAGUAbABUAG8AbwBsAEIAYQByAEwAZQBmAHQDAAAAAP////8AAAAAA" "AAAAAAAAAEAAAABAAAAOABjAG8AbABsAGEAcABzAGkAYgBsAGUAUABhAG4AZQBsAFQAbwBvAGwAQgBhAHIAUgBpAGcAaAB0AwAAAAD/////AAAAA" "AAAAAAAAAADAAAAAQAAADoAYwBvAGwAbABhAHAAcwBpAGIAbABlAFAAYQBuAGUAbABUAG8AbwBsAEIAYQByAEIAbwB0AHQAbwBtAAAAAAD/////A" @@ -301,9 +301,9 @@ Model { ModifiedByFormat "%" LastModifiedBy "nunoguedelha" ModifiedDateFormat "%" - LastModifiedDate "Fri Apr 30 05:51:30 2021" - RTWModifiedTimeStamp 541656573 - ModelVersionFormat "%" + LastModifiedDate "Fri May 07 10:00:40 2021" + RTWModifiedTimeStamp 542282437 + ModelVersionFormat "%" SampleTimeColors off SampleTimeAnnotations off LibraryLinkDisplay "disabled" @@ -1385,7 +1385,7 @@ Model { } System { Name "test_matlab_system_2020b" - Location [43, 23, 1792, 1120] + Location [40, 23, 1792, 1120] SystemRect [0.000000, 0.000000, 0.000000, 0.000000] Open on PortBlocksUseCompactNotation off @@ -1401,7 +1401,7 @@ Model { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "203" + ZoomFactor "190" ReportName "simulink-default.rpt" SIDHighWatermark "238" SimulinkSubDomain "Simulink" @@ -1484,7 +1484,7 @@ Model { BlockType Constant Name "Constant1" SID "140" - Position [0, 275, 30, 305] + Position [-125, 279, 30, 301] ZOrder 1236 Value "zeros(robot_config.N_DOF + 6,1)" } @@ -1492,9 +1492,10 @@ Model { BlockType Constant Name "Constant2" SID "205" - Position [0, 310, 30, 340] + Position [-125, 314, 30, 336] ZOrder 1261 Value "zeros(robot_config.N_DOF,1)" + VectorParams1D off } Block { BlockType Delay @@ -1514,7 +1515,7 @@ Model { Ports [4, 1] Position [435, 297, 655, 413] ZOrder 1281 - LibraryVersion "1.13" + LibraryVersion "1.17" SourceBlock "mwbs_robotSensors_lib/IMUsensor" SourceType "IMUsensor" SourceProductName "Matlab Whole-body Simulator" @@ -1846,7 +1847,7 @@ Model { ZOrder 1252 ForegroundColor "red" BackgroundColor "yellow" - LibraryVersion "3.3" + LibraryVersion "3.20" SourceBlock "mwbs_robotDynamicsWithContacts_lib/RobotDynWithContacts" SourceType "" SourceProductName "Matlab Whole-body Simulator" @@ -1859,6 +1860,7 @@ Model { robot_config "robot_config" contact_config "contact_config" physics_config "physics_config" + motorReflectedInertiaFormat "vector" } Block { BlockType Scope