Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add optional format alternative to motor reflected inertia #44

Merged
merged 2 commits into from
May 8, 2021
Merged

Add optional format alternative to motor reflected inertia #44

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
6107015
Add optional format alternative to motor reflected inertia
nunoguedelha May 7, 2021
d851c2b
Fixed propagation of 'motorReflectedInertiaFormat' in the mask initia…
nunoguedelha May 7, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
19 changes: 16 additions & 3 deletions lib/+mwbs/+RobotDynamicsWithContacts/+robotDynWC/step_block.m
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,7 @@
robot_config;
contact_config;
physics_config;
motorReflectedInertiaFormat;
OutputBusName = 'bus_name';
end

Expand Down Expand Up @@ -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
Expand All @@ -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];
Expand Down Expand Up @@ -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
4 changes: 2 additions & 2 deletions lib/+mwbs/@Contacts/Contacts.m
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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);
Expand Down
8 changes: 4 additions & 4 deletions lib/+mwbs/@Robot/Robot.m
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Expand All @@ -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

Expand Down Expand Up @@ -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);
Expand Down
Binary file modified lib/mwbs_robotDynamicsWithContacts_lib.slx
View file
Open in desktop
Binary file not shown.
58 changes: 30 additions & 28 deletions test_matlab_system_2020b.mdl
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -7,7 +7,7 @@ Model {
NumRootInports 0
NumRootOutports 0
ParameterArgumentNames ""
ComputedModelVersion "1.142"
ComputedModelVersion "1.154"
NumModelReferences 0
NumTestPointedSignals 0
NumProvidedFunctions 0
Expand Down Expand Up @@ -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
Expand All @@ -114,24 +114,35 @@ 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]
Offset [-327.97387892812696, -202.97930927193528]
SceneRectInView [-327.97387892812696, -202.97930927193528, 751.49463285625393, 398.95861854387056]
}
Object {
$ObjectID 8
$ObjectID 9
IsActive [0]
IsTabbed [0]
ViewObjType "SimulinkSubsys"
Expand All @@ -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]
Expand Down Expand Up @@ -278,7 +278,7 @@ Model {
"ABvAG4AZQBuAHQALwBHAEwAVQBFADIAIAB0AHIAZQBlACAAYwBvAG0AcABvAG4AZQBuAHQAAAAAeQAAA6EAAABrAP////sAAABgAFMAaQBtAHUAb"
"ABpAG4AawA6AEUAZABpAHQAbwByADoAUgBlAGYAZQByAGUAbgBjAGUAZABGAGkAbABlAHMALwBSAGUAZgBlAHIAZQBuAGMAZQBkACAARgBpAGwAZ"
"QBzAAAAAAD/////AAAAjwD///8AAAABAAAAAAAAAAD8AgAAAAH7AAAAVABHAEwAVQBFADIAOgBQAHIAbwBwAGUAcgB0AHkASQBuAHMAcABlAGMAd"
"ABvAHIALwBQAHIAbwBwAGUAcgB0AHkAIABJAG4AcwBwAGUAYwB0AG8AcgAAAAAA/////wAAAawA////AAAGiQAAA6EAAAABAAAAAgAAAAEAAAAC/"
"ABvAHIALwBQAHIAbwBwAGUAcgB0AHkAIABJAG4AcwBwAGUAYwB0AG8AcgAAAAAA/////wAAAawA////AAAGjAAAA6EAAAABAAAAAgAAAAEAAAAC/"
"AAAAAMAAAAAAAAAAQAAADYAYwBvAGwAbABhAHAAcwBpAGIAbABlAFAAYQBuAGUAbABUAG8AbwBsAEIAYQByAEwAZQBmAHQDAAAAAP////8AAAAAA"
"AAAAAAAAAEAAAABAAAAOABjAG8AbABsAGEAcABzAGkAYgBsAGUAUABhAG4AZQBsAFQAbwBvAGwAQgBhAHIAUgBpAGcAaAB0AwAAAAD/////AAAAA"
"AAAAAAAAAADAAAAAQAAADoAYwBvAGwAbABhAHAAcwBpAGIAbABlAFAAYQBuAGUAbABUAG8AbwBsAEIAYQByAEIAbwB0AHQAbwBtAAAAAAD/////A"
Expand All @@ -301,9 +301,9 @@ Model {
ModifiedByFormat "%<Auto>"
LastModifiedBy "nunoguedelha"
ModifiedDateFormat "%<Auto>"
LastModifiedDate "Fri Apr 30 05:51:30 2021"
RTWModifiedTimeStamp 541656573
ModelVersionFormat "%<AutoIncrement:1.142>"
LastModifiedDate "Fri May 07 10:00:40 2021"
RTWModifiedTimeStamp 542282437
ModelVersionFormat "%<AutoIncrement:1.154>"
SampleTimeColors off
SampleTimeAnnotations off
LibraryLinkDisplay "disabled"
Expand Down Expand Up @@ -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
Expand All @@ -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"
Expand Down Expand Up @@ -1484,17 +1484,18 @@ 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)"
}
Block {
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
Expand All @@ -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"
Expand Down Expand Up @@ -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"
Expand All @@ -1859,6 +1860,7 @@ Model {
robot_config "robot_config"
contact_config "contact_config"
physics_config "physics_config"
motorReflectedInertiaFormat "vector"
}
Block {
BlockType Scope
Expand Down