Skip to content

Commit d2dcb39

Browse files
tobolartobolar
authoredNov 12, 2023
Fix false calculation of w (omega) when resolveInFrame == frame_resolve (#4223)
* Fix false calculation of w (omega) when resolveInFrame == frame_resolve * Add rotation about x-axis to enable a spatial motion
1 parent 6212997 commit d2dcb39

File tree

2 files changed

+82
-66
lines changed

2 files changed

+82
-66
lines changed
 

‎Modelica/Mechanics/MultiBody/Sensors/Internal/BasicAbsoluteAngularVelocity.mo

+1-1
Original file line numberDiff line numberDiff line change
@@ -20,7 +20,7 @@ equation
2020
elseif resolveInFrame == ResolveInFrameA.frame_a then
2121
w = Frames.angularVelocity2(frame_a.R);
2222
elseif resolveInFrame == ResolveInFrameA.frame_resolve then
23-
w = Frames.resolveRelative(Frames.angularVelocity1(frame_a.R), frame_a.R, frame_resolve.R);
23+
w = Frames.resolve2(frame_resolve.R, Frames.angularVelocity1(frame_a.R));
2424
else
2525
assert(false, "Wrong value for parameter resolveInFrame");
2626
w = zeros(3);

‎ModelicaTest/MultiBody.mo

+81-65
Original file line numberDiff line numberDiff line change
@@ -8776,44 +8776,50 @@ This test model was proposed in ticket <a href=\"https://github.com/modelica/Mod
87768776
model AbsoluteSensor "Simple pendulum with one revolute joint and one body"
87778777
extends Modelica.Icons.Example;
87788778
parameter Real tol=1e-3;
8779-
inner MultiBody.World world(gravityType=MultiBody.Types.GravityTypes.UniformGravity)
8780-
annotation (Placement(transformation(extent={{-80,0},{-60,20}})));
8779+
inner MultiBody.World world(
8780+
gravityType=MultiBody.Types.GravityTypes.UniformGravity)
8781+
annotation (Placement(transformation(extent={{-100,-10},{-80,10}})));
87818782
MultiBody.Joints.Revolute rev(
87828783
n={0,0,1},
87838784
useAxisFlange=true,
87848785
phi(fixed=true),
8785-
w(fixed=true)) annotation (Placement(transformation(extent={{-40,0},{-20,
8786-
20}})));
8786+
w(fixed=true)) annotation (Placement(transformation(extent={{-60,-10},{-40,10}})));
8787+
MultiBody.Joints.Revolute rev1(
8788+
n={1,0,0},
8789+
useAxisFlange=true,
8790+
phi(fixed=true),
8791+
w(fixed=true)) annotation (Placement(transformation(extent={{-30,-10},{-10,10}})));
87878792
Modelica.Mechanics.Rotational.Components.Damper damper(d=0.1) annotation (
8788-
Placement(transformation(extent={{-40,40},{-20,60}})));
8789-
MultiBody.Parts.BodyBox box(r={1,0,0}, animation=false)
8790-
annotation (Placement(transformation(extent={{0,0},{20,20}})));
8793+
Placement(transformation(extent={{-60,20},{-40,40}})));
8794+
Modelica.Mechanics.Rotational.Components.Damper damper1(d=0.1) annotation (
8795+
Placement(transformation(extent={{-30,20},{-10,40}})));
8796+
MultiBody.Parts.BodyBox box(r={1,0,1})
8797+
annotation (Placement(transformation(extent={{10,-10},{30,10}})));
87918798
MultiBody.Sensors.AbsoluteSensor sensor_frame_a1(
87928799
get_angles=true,
87938800
resolveInFrame=MultiBody.Types.ResolveInFrameA.frame_a,
87948801
get_r=true,
87958802
get_v=true,
87968803
get_a=true,
87978804
get_w=true,
8798-
get_z=true) annotation (Placement(transformation(extent={{40,40},{60,20}})));
8805+
get_z=true) annotation (Placement(transformation(extent={{50,30},{70,10}})));
87998806

8800-
MultiBody.Sensors.AbsoluteSensor sensor_world1(
8807+
MultiBody.Sensors.AbsoluteSensor sensor_frame_a2(
88018808
get_angles=true,
8802-
resolveInFrame=MultiBody.Types.ResolveInFrameA.world,
8809+
resolveInFrame=MultiBody.Types.ResolveInFrameA.frame_resolve,
88038810
get_r=true,
88048811
get_v=true,
88058812
get_a=true,
88068813
get_w=true,
8807-
get_z=true) annotation (Placement(transformation(extent={{40,-20},{60,0}})));
8808-
8809-
MultiBody.Sensors.AbsoluteSensor sensor_frame_a2(
8814+
get_z=true) annotation (Placement(transformation(extent={{50,50},{70,70}})));
8815+
MultiBody.Sensors.AbsoluteSensor sensor_world1(
88108816
get_angles=true,
8811-
resolveInFrame=MultiBody.Types.ResolveInFrameA.frame_resolve,
8817+
resolveInFrame=MultiBody.Types.ResolveInFrameA.world,
88128818
get_r=true,
88138819
get_v=true,
88148820
get_a=true,
88158821
get_w=true,
8816-
get_z=true) annotation (Placement(transformation(extent={{40,60},{60,80}})));
8822+
get_z=true) annotation (Placement(transformation(extent={{50,-30},{70,-10}})));
88178823

88188824
MultiBody.Sensors.AbsoluteSensor sensor_world2(
88198825
get_angles=true,
@@ -8822,95 +8828,105 @@ This test model was proposed in ticket <a href=\"https://github.com/modelica/Mod
88228828
get_v=true,
88238829
get_a=true,
88248830
get_w=true,
8825-
get_z=true) annotation (Placement(transformation(extent={{40,-40},{60,-60}})));
8831+
get_z=true) annotation (Placement(transformation(extent={{50,-50},{70,-70}})));
88268832

88278833
Modelica.Blocks.Math.Add err1[18](each k2=-1) annotation (Placement(
8828-
transformation(extent={{80,40},{100,60}})));
8834+
transformation(extent={{80,30},{100,50}})));
88298835
Modelica.Blocks.Math.Add err2[18](each k2=-1) annotation (Placement(
8830-
transformation(extent={{80,-40},{100,-20}})));
8836+
transformation(extent={{80,-50},{100,-30}})));
88318837
equation
88328838
for i in 1:18 loop
88338839
assert(err1[i].y < tol and err2[i].y < tol,
8834-
"Errors must be less then tolerance");
8840+
"Errors must be less than tolerance");
88358841
end for;
88368842
connect(world.frame_b, rev.frame_a) annotation (Line(
8837-
points={{-60,10},{-40,10}},
8843+
points={{-80,0},{-60,0}},
8844+
thickness=0.5,
8845+
color={95,95,95}));
8846+
connect(damper.flange_b, rev.axis) annotation (Line(points={{-40,30},{-40,10},{-50,10}}));
8847+
connect(rev.support, damper.flange_a) annotation (Line(points={{-56,10},{-60,10},{-60,30}}));
8848+
connect(rev.frame_b, rev1.frame_a) annotation (Line(
8849+
points={{-40,0},{-30,0}},
8850+
color={95,95,95},
88388851
thickness=0.5));
8839-
connect(damper.flange_b, rev.axis) annotation (Line(points={{-20,50},{-20,
8840-
50},{-20,30},{-30,30},{-30,20}}));
8841-
connect(rev.support, damper.flange_a) annotation (Line(points={{-36,20},{
8842-
-36,30},{-40,30},{-40,50}}));
8843-
connect(rev.frame_b, box.frame_a) annotation (Line(
8844-
points={{-20,10},{0,10}},
8852+
connect(damper1.flange_b, rev1.axis) annotation (Line(points={{-10,30},{-10,10},{-20,10}}));
8853+
connect(rev1.support, damper1.flange_a) annotation (Line(points={{-26,10},{-30,10},{-30,30}}));
8854+
connect(rev1.frame_b, box.frame_a) annotation (Line(
8855+
points={{-10,0},{10,0}},
8856+
color={95,95,95},
88458857
thickness=0.5));
88468858
connect(box.frame_b, sensor_frame_a1.frame_a) annotation (Line(
8847-
points={{20,10},{30,10},{30,30},{40,30}},
8848-
thickness=0.5));
8859+
points={{30,0},{40,0},{40,20},{50,20}},
8860+
thickness=0.5,
8861+
color={95,95,95}));
88498862
connect(box.frame_b, sensor_world1.frame_a) annotation (Line(
8850-
points={{20,10},{30,10},{30,-10},{40,-10}},
8851-
thickness=0.5));
8863+
points={{30,0},{40,0},{40,-20},{50,-20}},
8864+
thickness=0.5,
8865+
color={95,95,95}));
88528866
connect(box.frame_b, sensor_frame_a2.frame_a) annotation (Line(
8853-
points={{20,10},{30,10},{30,70},{40,70}},
8854-
thickness=0.5));
8867+
points={{30,0},{40,0},{40,60},{50,60}},
8868+
thickness=0.5,
8869+
color={95,95,95}));
88558870
connect(box.frame_b, sensor_world2.frame_a) annotation (Line(
8856-
points={{20,10},{30,10},{30,-50},{40,-50}},
8857-
thickness=0.5));
8871+
points={{30,0},{40,0},{40,-60},{50,-60}},
8872+
thickness=0.5,
8873+
color={95,95,95}));
88588874
connect(sensor_world2.frame_resolve, world.frame_b) annotation (Line(
8859-
points={{60,-50},{60,-50},{70,-50},{70,-76},{-50,-76},{-50,10},{-60,
8860-
10}},
8861-
pattern=LinePattern.Dot));
8862-
8863-
connect(sensor_frame_a2.frame_resolve, rev.frame_b) annotation (Line(
8864-
points={{60,70},{60,70},{70,70},{70,90},{-10,90},{-10,10},{-20,10}},
8865-
pattern=LinePattern.Dot));
8875+
points={{70,-60},{70,-80},{-70,-80},{-70,0},{-80,0}},
8876+
color={95,95,95},
8877+
thickness=0.5));
8878+
connect(sensor_frame_a2.frame_resolve, rev1.frame_b) annotation (Line(
8879+
points={{70,60},{70,80},{0,80},{0,0},{-10,0}},
8880+
color={95,95,95},
8881+
thickness=0.5));
88668882
connect(sensor_frame_a2.r[1:3], err1[1:3].u1) annotation (Line(
8867-
points={{40,58.3333},{40,56},{78,56}}, color={0,0,127}));
8883+
points={{50,49.3333},{50,46},{78,46}}, color={0,0,127}));
88688884
connect(sensor_frame_a2.v[1:3], err1[4:6].u1) annotation (Line(
8869-
points={{44,58.3333},{44,56},{78,56}}, color={0,0,127}));
8885+
points={{54,49.3333},{54,46},{78,46}}, color={0,0,127}));
88708886
connect(sensor_frame_a2.a[1:3], err1[7:9].u1) annotation (Line(
8871-
points={{48,58.3333},{48,56},{78,56}}, color={0,0,127}));
8887+
points={{58,49.3333},{58,46},{78,46}}, color={0,0,127}));
88728888
connect(sensor_frame_a2.angles[1:3], err1[10:12].u1) annotation (Line(
8873-
points={{52,58.3333},{52,56},{78,56}}, color={0,0,127}));
8889+
points={{62,49.3333},{62,46},{78,46}}, color={0,0,127}));
88748890
connect(sensor_frame_a2.w[1:3], err1[13:15].u1) annotation (Line(
8875-
points={{56,58.3333},{56,56},{78,56}}, color={0,0,127}));
8891+
points={{66,49.3333},{66,46},{78,46}}, color={0,0,127}));
88768892
connect(sensor_frame_a2.z[1:3], err1[16:18].u1) annotation (Line(
8877-
points={{60,58.3333},{60,56},{78,56}}, color={0,0,127}));
8893+
points={{70,49.3333},{70,46},{78,46}}, color={0,0,127}));
88788894
connect(sensor_frame_a1.r[1:3], err1[1:3].u2) annotation (Line(
8879-
points={{40,41.6667},{40,44},{78,44}}, color={0,0,127}));
8895+
points={{50,30.6667},{50,34},{78,34}}, color={0,0,127}));
88808896
connect(sensor_frame_a1.v[1:3], err1[4:6].u2) annotation (Line(
8881-
points={{44,41.6667},{44,44},{78,44}}, color={0,0,127}));
8897+
points={{54,30.6667},{54,34},{78,34}}, color={0,0,127}));
88828898
connect(sensor_frame_a1.a[1:3], err1[7:9].u2) annotation (Line(
8883-
points={{48,41.6667},{48,44},{78,44}}, color={0,0,127}));
8899+
points={{58,30.6667},{58,34},{78,34}}, color={0,0,127}));
88848900
connect(sensor_frame_a1.angles[1:3], err1[10:12].u2) annotation (Line(
8885-
points={{52,41.6667},{52,44},{78,44}}, color={0,0,127}));
8901+
points={{62,30.6667},{62,34},{78,34}}, color={0,0,127}));
88868902
connect(sensor_frame_a1.w[1:3], err1[13:15].u2) annotation (Line(
8887-
points={{56,41.6667},{56,44},{78,44}}, color={0,0,127}));
8903+
points={{66,30.6667},{66,34},{78,34}}, color={0,0,127}));
88888904
connect(sensor_frame_a1.z[1:3], err1[16:18].u2) annotation (Line(
8889-
points={{60,41.6667},{60,44},{78,44}}, color={0,0,127}));
8905+
points={{70,30.6667},{70,34},{78,34}}, color={0,0,127}));
88908906
connect(sensor_world1.r[1:3], err2[1:3].u1) annotation (Line(
8891-
points={{40,-21.6667},{40,-24},{78,-24}}, color={0,0,127}));
8907+
points={{50,-30.6667},{50,-34},{78,-34}}, color={0,0,127}));
88928908
connect(sensor_world1.v[1:3], err2[4:6].u1) annotation (Line(
8893-
points={{44,-21.6667},{44,-24},{78,-24}}, color={0,0,127}));
8909+
points={{54,-30.6667},{54,-34},{78,-34}}, color={0,0,127}));
88948910
connect(sensor_world1.a[1:3], err2[7:9].u1) annotation (Line(
8895-
points={{48,-21.6667},{48,-24},{78,-24}}, color={0,0,127}));
8911+
points={{58,-30.6667},{58,-34},{78,-34}}, color={0,0,127}));
88968912
connect(sensor_world1.angles[1:3], err2[10:12].u1) annotation (Line(
8897-
points={{52,-21.6667},{52,-24},{78,-24}}, color={0,0,127}));
8913+
points={{62,-30.6667},{62,-34},{78,-34}}, color={0,0,127}));
88988914
connect(sensor_world1.w[1:3], err2[13:15].u1) annotation (Line(
8899-
points={{56,-21.6667},{56,-24},{78,-24}}, color={0,0,127}));
8915+
points={{66,-30.6667},{66,-34},{78,-34}}, color={0,0,127}));
89008916
connect(sensor_world1.z[1:3], err2[16:18].u1) annotation (Line(
8901-
points={{60,-21.6667},{60,-24},{78,-24}}, color={0,0,127}));
8917+
points={{70,-30.6667},{70,-34},{78,-34}}, color={0,0,127}));
89028918
connect(sensor_world2.r[1:3], err2[1:3].u2) annotation (Line(
8903-
points={{40,-38.3333},{40,-36},{78,-36}}, color={0,0,127}));
8919+
points={{50,-49.3333},{50,-46},{78,-46}}, color={0,0,127}));
89048920
connect(sensor_world2.v[1:3], err2[4:6].u2) annotation (Line(
8905-
points={{44,-38.3333},{44,-36},{78,-36}}, color={0,0,127}));
8921+
points={{54,-49.3333},{54,-46},{78,-46}}, color={0,0,127}));
89068922
connect(sensor_world2.a[1:3], err2[7:9].u2) annotation (Line(
8907-
points={{48,-38.3333},{48,-36},{78,-36}}, color={0,0,127}));
8923+
points={{58,-49.3333},{58,-46},{78,-46}}, color={0,0,127}));
89088924
connect(sensor_world2.angles[1:3], err2[10:12].u2) annotation (Line(
8909-
points={{52,-38.3333},{52,-36},{78,-36}}, color={0,0,127}));
8925+
points={{62,-49.3333},{62,-46},{78,-46}}, color={0,0,127}));
89108926
connect(sensor_world2.w[1:3], err2[13:15].u2) annotation (Line(
8911-
points={{56,-38.3333},{56,-36},{78,-36}}, color={0,0,127}));
8927+
points={{66,-49.3333},{66,-46},{78,-46}}, color={0,0,127}));
89128928
connect(sensor_world2.z[1:3], err2[16:18].u2) annotation (Line(
8913-
points={{60,-38.3333},{60,-36},{78,-36}}, color={0,0,127}));
8929+
points={{70,-49.3333},{70,-46},{78,-46}}, color={0,0,127}));
89148930
annotation (experiment(StopTime=5), Documentation(info=""));
89158931
end AbsoluteSensor;
89168932

0 commit comments

Comments
 (0)