How about this model?
model KeepVertical
Modelica.Blocks.Math.Gain gain1(k = -1) annotation(Placement(visible = true, transformation(origin = {50, -10}, extent = {{10, -10}, {-10, 10}}, rotation = 0)));
Modelica.Mechanics.MultiBody.Sensors.RelativeAngles relativeAngles1 annotation(Placement(visible = true, transformation(origin = {80, -40}, extent = {{-10, 10}, {10, -10}}, rotation = 0)));
Modelica.Mechanics.Rotational.Sources.Position position1 annotation(Placement(visible = true, transformation(origin = {14.21, -10}, extent = {{10, -10}, {-10, 10}}, rotation = 0)));
Modelica.Mechanics.MultiBody.Parts.Body body(r_0.start = {0.0, 2, 0.0}) annotation(Placement(visible = true, transformation(origin = {-80, -120}, extent = {{-10, -10}, {10, 10}}, rotation = -90)));
inner Modelica.Mechanics.MultiBody.World world annotation(Placement(visible = true, transformation(origin = {-120, 80}, extent = {{-10, -10}, {10, 10}}, rotation = -360)));
Modelica.Mechanics.MultiBody.Parts.FixedTranslation fixedTranslation1(r = {0.0, -1, 0.0}) annotation(Placement(visible = true, transformation(origin = {-80, 0}, extent = {{-10, -10}, {10, 10}}, rotation = -90)));
Modelica.Mechanics.MultiBody.Parts.FixedTranslation fixedTranslation2(r = {0.0, -1, 0.0}) annotation(Placement(visible = true, transformation(origin = {-80, -80}, extent = {{-10, -10}, {10, 10}}, rotation = -90)));
Modelica.Mechanics.MultiBody.Joints.Revolute revolute2(phi.start = 0.174532925199433, useAxisFlange = false) annotation(Placement(visible = true, transformation(origin = {-80, 40}, extent = {{10, -10}, {-10, 10}}, rotation = 90)));
Modelica.Mechanics.MultiBody.Parts.Body body1(r_0.start = {0.0, 2, 0.0}) annotation(Placement(visible = true, transformation(origin = {-40, 60}, extent = {{-10, -10}, {10, 10}}, rotation = -270)));
Modelica.Mechanics.MultiBody.Joints.Revolute revolute3(useAxisFlange = true) annotation(Placement(visible = true, transformation(origin = {-40, -10}, extent = {{-10, -10}, {10, 10}}, rotation = -90)));
Modelica.Mechanics.MultiBody.Parts.FixedTranslation fixedTranslation3(r = {0.0, -0.5, 0.0}) annotation(Placement(visible = true, transformation(origin = {-40, 20}, extent = {{-10, 10}, {10, -10}}, rotation = 270)));
equation
connect(fixedTranslation2.frame_b, body.frame_a) annotation(Line(visible = true, origin = {-80, -100}, points = {{0, 10}, {0, -10}}, color = {95, 95, 95}, thickness = 0.5));
connect(world.frame_b, revolute2.frame_a) annotation(Line(visible = true, origin = {-90, 70}, points = {{-20, 10}, {10, 10}, {10, -20}}, color = {95, 95, 95}, thickness = 0.5));
connect(revolute2.frame_b, fixedTranslation1.frame_a) annotation(Line(visible = true, origin = {-80, 20}, points = {{0, 10}, {0, -10}}, color = {95, 95, 95}, thickness = 0.5));
connect(fixedTranslation2.frame_a, fixedTranslation1.frame_b) annotation(Line(visible = true, origin = {-80, -40}, points = {{0, -30}, {0, 30}}, color = {95, 95, 95}, thickness = 0.5));
connect(fixedTranslation1.frame_b, revolute3.frame_b) annotation(Line(visible = true, points = {{-20, 2.912}, {-20, -27.088}, {20, -27.088}, {20, -7.088}}, color = {95, 95, 95}, thickness = 0.5, origin = {-60, -12.912}));
connect(fixedTranslation3.frame_a, body1.frame_a) annotation(Line(visible = true, origin = {-40, 40}, points = {{0, -10}, {0, 10}}, color = {95, 95, 95}, thickness = 0.5));
connect(revolute3.frame_a, fixedTranslation3.frame_b) annotation(Line(visible = true, origin = {-40, 5}, points = {{0, -5}, {0, 5}}, color = {95, 95, 95}, thickness = 0.5));
connect(world.frame_b, relativeAngles1.frame_b) annotation(Line(visible = true, origin = {43.879, 40}, points = {{-153.879, 40}, {56.121, 40}, {56.121, -80}, {46.121, -80}}, color = {95, 95, 95}, thickness = 0.5));
connect(gain1.u, relativeAngles1.angles[3]) annotation(Line(visible = true, origin = {74, -16.333}, points = {{-12, 6.333}, {6, 6.333}, {6, -12.667}}, color = {0, 0, 127}));
connect(revolute3.axis, position1.flange) annotation(Line(visible = true, origin = {-12.895, -10}, points = {{-17.105, 0}, {17.105, 0}}));
connect(position1.phi_ref, gain1.y) annotation(Line(visible = true, origin = {32.605, -10}, points = {{-6.395, 0}, {6.395, 0}}, color = {0, 0, 127}));
connect(revolute3.frame_b, relativeAngles1.frame_a) annotation(Line(visible = true, origin = {-3.333, -33.333}, points = {{-36.667, 13.333}, {-36.667, -6.667}, {73.333, -6.667}}, color = {95, 95, 95}, thickness = 0.5));
annotation(uses(Modelica(version = "4.0.0")), Diagram(coordinateSystem(extent = {{-240, -143.033}, {210, 100}}, preserveAspectRatio = true, initialScale = 0.1, grid = {10, 10})));
end KeepVertical;