# robot_arm_flex.mbd
#-----------------------------------------------------------------------------
# [Data Block]
begin: data;
problem: initial value;
end: data;
#-----------------------------------------------------------------------------
# [<Problem> Block]
begin: initial value;
initial time: 0.;
final time: 10.;
time step: 1.e-3;
max iterations: 10;
tolerance: 1.e-6;
end: initial value;
#-----------------------------------------------------------------------------
# [Control Data Block]
begin: control data;
output frequency: 10;
structural nodes: 7;
rigid bodies: 5;
joints: 5
+5;
beams: 1;
gravity;
end: control data;
#-----------------------------------------------------------------------------
# Design Variables
set: real Izz_Stage = 0.1; #[kg m^2] Moment of Inertia of Stage
set: real M_UpperArm = 1.; #[kg] Mass of Upper Arm
set: real M_LowerArm = 1.; #[kg] Mass of Lower Arm
set: real M_Wrist = 0.1; #[kg] Mass of Wrist
set: real M_Hand = 0.2; #[kg] Mass of Hand
set: real H_Stage = 1.; #[m] Height of Stage
set: real L_UpperArm = 1.; #[m] Length of Upper Arm
set: real L_LowerArm = 1.; #[m] Length of Lower Arm
set: real L_Hand = 0.2; #[m] Length of Hand
set: real R_UpperArm = 0.05; #[m] Radius of Upper Arm
set: real R_LowerArm = 0.05; #[m] Radius of Lower Arm
set: real R_Hand = 0.05; #[m] Radius of Hand
set: real EA = 1.5e6; #[N] Beam Stiffness for Pillar
set: real GAy = 0.6e6; #[N] Beam Stiffness for Pillar
set: real GAz = 0.6e6; #[N] Beam Stiffness for Pillar
set: real GJ = 0.4e3; #[Nm^2] Beam Stiffness for Pillar
set: real EJy = 1.e3; #[Nm^2] Beam Stiffness for Pillar
set: real EJz = 1.e3; #[Nm^2] Beam Stiffness for Pillar
#-----------------------------------------------------------------------------
# Scalar Functions
scalar function: "Fun_Motor_JoTotp_Stage",
multilinear,
0.0, 0.0,
1.0, 0.0,
2.0, 0.0,
3.0, 0.0,
4.0, 0.0,
5.0, 0.0,
6.0, -1./2.*pi,
7.0, -1./2.*pi,
8.0, -1./2.*pi,
9.0, -1./2.*pi,
10.0, -1./2.*pi;
scalar function: "Fun_Motor_JoTotj_UpperPillar_UpperArm",
multilinear,
0.0, 0.0,
1.0, -1./6.*pi,
2.0, -2./6.*pi,
3.0, -2./6.*pi,
4.0, -1./6.*pi,
5.0, -1./6.*pi,
6.0, -1./6.*pi,
7.0, -2./6.*pi,
8.0, -2./6.*pi,
9.0, -1./6.*pi,
10.0, -1./6.*pi;
scalar function: "Fun_Motor_JoTotj_UpperArm_LowerArm",
multilinear,
0.0, 0.0,
1.0, -4./6.*pi,
2.0, -1./3.*pi,
3.0, -1./3.*pi,
4.0, -4./6.*pi,
5.0, -4./6.*pi,
6.0, -4./6.*pi,
7.0, -1./3.*pi,
8.0, -1./3.*pi,
9.0, -4./6.*pi,
10.0, -4./6.*pi;
scalar function: "Fun_Motor_JoTotj_LowerArm_Wrist",
multilinear,
0.0, 0.0,
1.0, 1./3.*pi,
2.0, 1./6.*pi,
3.0, 1./6.*pi,
4.0, 1./3.*pi,
5.0, 1./3.*pi,
6.0, 1./3.*pi,
7.0, 1./6.*pi,
8.0, 1./6.*pi,
9.0, 1./3.*pi,
10.0, 1./3.*pi;
scalar function: "Fun_Motor_JoTotj_Wrist_Hand",
multilinear,
0.0, 0.0,
1.0, 0.0,
2.0, 0.0,
3.0, pi,
4.0, pi,
5.0, 0.0,
6.0, 0.0,
7.0, 0.0,
8.0, pi,
9.0, pi,
10.0, 0.0;
#-----------------------------------------------------------------------------
# Reference Labels
set: integer Ref_JoRevh_UpperPillar_UpperArm = 1;
set: integer Ref_JoRevh_UpperArm_LowerArm = 2;
set: integer Ref_JoRevh_LowerArm_Wrist = 3;
set: integer Ref_JoRevh_Wrist_Hand = 4;
# Node Labels
set: integer Node_Stage = 1;
set: integer Node_UpperArm = 2;
set: integer Node_LowerArm = 3;
set: integer Node_Wrist = 4;
set: integer Node_Hand = 5;
set: integer Node_MidPillar = 6;
set: integer Node_UpperPillar = 7;
# Body Labels
set: integer Body_Stage = 1;
set: integer Body_UpperArm = 2;
set: integer Body_LowerArm = 3;
set: integer Body_Wrist = 4;
set: integer Body_Hand = 5;
# Joint Labels
set: integer JoRevp_Stage = 1;
set: integer JoRevh_UpperPillar_UpperArm = 2;
set: integer JoRevh_UpperArm_LowerArm = 3;
set: integer JoRevh_LowerArm_Wrist = 4;
set: integer JoRevh_Wrist_Hand = 5;
set: integer JoTotp_Stage = 6;
set: integer JoTotj_UpperPillar_UpperArm = 7;
set: integer JoTotj_UpperArm_LowerArm = 8;
set: integer JoTotj_LowerArm_Wrist = 9;
set: integer JoTotj_Wrist_Hand = 10;
# Beam Labels
set: integer Beam_Pillar =1;
#-----------------------------------------------------------------------------
# Intermediate Variables
set: real Ixx_UpperArm = 1./12.*M_UpperArm*(L_UpperArm^2+3*R_UpperArm^2);
set: real Iyy_UpperArm = 1./12.*M_UpperArm*(L_UpperArm^2+3*R_UpperArm^2);
set: real Izz_UpperArm = 1./2.*M_UpperArm*R_UpperArm^2;
set: real Ixx_LowerArm = 1./12.*M_LowerArm*(L_LowerArm^2+3*R_LowerArm^2);
set: real Iyy_LowerArm = 1./12.*M_LowerArm*(L_LowerArm^2+3*R_LowerArm^2);
set: real Izz_LowerArm = 1./2.*M_LowerArm*R_LowerArm^2;
set: real Ixx_Hand = 1./12.*M_Hand*(L_Hand^2+3*R_Hand^2);
set: real Iyy_Hand = 1./12.*M_Hand*(L_Hand^2+3*R_Hand^2);
set: real Izz_Hand = 1./2.*M_Hand*R_Hand^2;
#-----------------------------------------------------------------------------
# References
reference: Ref_JoRevh_UpperPillar_UpperArm,
0., 0., H_Stage, # absolute position
euler, pi/2., 0., 0., # absolute orientation
null, # absolute velocity
null; # absolute angular velocity
reference: Ref_JoRevh_UpperArm_LowerArm,
0., 0., H_Stage+L_UpperArm, # absolute position
euler, pi/2., 0., 0., # absolute orientation
null, # absolute velocity
null; # absolute angular velocity
reference: Ref_JoRevh_LowerArm_Wrist,
0., 0., H_Stage+L_UpperArm+L_LowerArm, # absolute position
euler, pi/2., 0., 0., # absolute orientation
null, # absolute velocity
null; # absolute angular velocity
reference: Ref_JoRevh_Wrist_Hand,
0., 0., H_Stage+L_UpperArm+L_LowerArm, # absolute position
eye, # absolute orientation
null, # absolute velocity
null; # absolute angular velocity
#-----------------------------------------------------------------------------
# [Nodes Block]
begin: nodes;
#-----------------------------------------------------------------------------
# Nodes
structural: Node_Stage, dynamic,
null, # absolute position
eye, # absolute orientation
null, # absolute velocity
null; # absolute angular velocity
structural: Node_UpperArm, dynamic,
0., 0., H_Stage+L_UpperArm/2., # absolute position
eye, # absolute orientation
null, # absolute velocity
null; # absolute angular velocity
structural: Node_LowerArm, dynamic,
0., 0., H_Stage+L_UpperArm+L_LowerArm/2., # absolute position
eye, # absolute orientation
null, # absolute velocity
null; # absolute angular velocity
structural: Node_Wrist, dynamic,
0., 0., H_Stage+L_UpperArm+L_LowerArm, # absolute position
eye, # absolute orientation
null, # absolute velocity
null; # absolute angular velocity
structural: Node_Hand, dynamic,
0., 0., H_Stage+L_UpperArm+L_LowerArm, # absolute position
eye, # absolute orientation
null, # absolute velocity
null; # absolute angular velocity
structural: Node_MidPillar, dynamic,
0., 0., H_Stage/2., # absolute position
eye, # absolute orientation
null, # absolute velocity
null; # absolute angular velocity
structural: Node_UpperPillar, dynamic,
0., 0., H_Stage, # absolute position
eye, # absolute orientation
null, # absolute velocity
null; # absolute angular velocity
end: nodes;
#-----------------------------------------------------------------------------
# [Elements Block]
begin: elements;
#-----------------------------------------------------------------------------
# Bodies
body: Body_Stage, Node_Stage,
1., # mass
null, # relative center of mass
diag, 1., 1., Izz_Stage; # inertia matrix
body: Body_UpperArm, Node_UpperArm,
M_UpperArm, # mass
null, # relative center of mass
diag, Ixx_UpperArm, Iyy_UpperArm, Izz_UpperArm; # inertia matrix
body: Body_LowerArm, Node_LowerArm,
M_LowerArm, # mass
null, # relative center of mass
diag, Ixx_LowerArm, Iyy_LowerArm, Izz_LowerArm; # inertia matrix
body: Body_Wrist, Node_Wrist,
M_Wrist, # mass
null, # relative center of mass
diag, 1.e-9, 1.e-9, 1.e-9; # inertia matrix
body: Body_Hand, Node_Hand,
M_Hand, # mass
null, # relative center of mass
diag, Ixx_Hand, Iyy_Hand, Izz_Hand; # inertia matrix
#-----------------------------------------------------------------------------
# Beams
beam3: Beam_Pillar,
Node_Stage,
null, # relative offset
Node_MidPillar,
null, # relative offset
Node_UpperPillar,
null, # relative offset
euler, 0., -pi/2., 0., # orientation matrix section I
linear elastic generic, # constitutive law section I
diag, EA, GAy, GAz, GJ, EJy, EJz,
same, # orientation matrix section II
same; # constitutive law section II
#-----------------------------------------------------------------------------
# Joints
joint: JoRevp_Stage,
revolute pin,
Node_Stage,
null, # relative offset
null; # absolute pin position
joint: JoRevh_UpperPillar_UpperArm,
revolute hinge,
Node_UpperPillar,
reference, Ref_JoRevh_UpperPillar_UpperArm, null, # relative offset
hinge, reference, Ref_JoRevh_UpperPillar_UpperArm, eye, # relative axis orientation
Node_UpperArm,
reference, Ref_JoRevh_UpperPillar_UpperArm, null, # relative offset
hinge, reference, Ref_JoRevh_UpperPillar_UpperArm, eye; # relative axis orientation
joint: JoRevh_UpperArm_LowerArm,
revolute hinge,
Node_UpperArm,
reference, Ref_JoRevh_UpperArm_LowerArm, null, # relative offset
hinge, reference, Ref_JoRevh_UpperArm_LowerArm, eye, # relative axis orientation
Node_LowerArm,
reference, Ref_JoRevh_UpperArm_LowerArm, null, # relative offset
hinge, reference, Ref_JoRevh_UpperArm_LowerArm, eye; # relative axis orientation
joint: JoRevh_LowerArm_Wrist,
revolute hinge,
Node_LowerArm,
reference, Ref_JoRevh_LowerArm_Wrist, null, # relative offset
hinge, reference, Ref_JoRevh_LowerArm_Wrist, eye, # relative axis orientation
Node_Wrist,
reference, Ref_JoRevh_LowerArm_Wrist, null, # relative offset
hinge, reference, Ref_JoRevh_LowerArm_Wrist, eye; # relative axis orientation
joint: JoRevh_Wrist_Hand,
revolute hinge,
Node_Wrist,
reference, Ref_JoRevh_Wrist_Hand, null, # relative offset
hinge, reference, Ref_JoRevh_Wrist_Hand, eye, # relative axis orientation
Node_Hand,
reference, Ref_JoRevh_Wrist_Hand, null, # relative offset
hinge, reference, Ref_JoRevh_Wrist_Hand, eye; # relative axis orientation
joint: JoTotp_Stage,
total pin joint,
Node_Stage,
position, null, # relative offset
position, null, # absolute position
position constraint,
inactive, inactive, inactive,
null,
orientation constraint,
inactive, inactive, active,
single, 0., 0., 1., scalar function, "Fun_Motor_JoTotp_Stage";
joint: JoTotj_UpperPillar_UpperArm,
total joint,
Node_Stage,
position, reference, Ref_JoRevh_UpperPillar_UpperArm, null, # relative offset
rotation orientation, reference, Ref_JoRevh_UpperPillar_UpperArm, eye, # relative rotation orientation
Node_UpperArm,
position, reference, Ref_JoRevh_UpperPillar_UpperArm, null, # relative offset
rotation orientation, reference, Ref_JoRevh_UpperPillar_UpperArm, eye, # relative rotation orientation
position constraint,
inactive, inactive, inactive,
null,
orientation constraint,
inactive, inactive, active,
single, 0., 0., 1., scalar function, "Fun_Motor_JoTotj_UpperPillar_UpperArm";
joint: JoTotj_UpperArm_LowerArm,
total joint,
Node_UpperArm,
position, reference, Ref_JoRevh_UpperArm_LowerArm, null, # relative offset
rotation orientation, reference, Ref_JoRevh_UpperArm_LowerArm, eye, # relative rotation orientation
Node_LowerArm,
position, reference, Ref_JoRevh_UpperArm_LowerArm, null, # relative offset
rotation orientation, reference, Ref_JoRevh_UpperArm_LowerArm, eye, # relative rotation orientation
position constraint,
inactive, inactive, inactive,
null,
orientation constraint,
inactive, inactive, active,
single, 0., 0., 1., scalar function, "Fun_Motor_JoTotj_UpperArm_LowerArm";
joint: JoTotj_LowerArm_Wrist,
total joint,
Node_LowerArm,
position, reference, Ref_JoRevh_LowerArm_Wrist, null, # relative offset
rotation orientation, reference, Ref_JoRevh_LowerArm_Wrist, eye, # relative rotation orientation
Node_Wrist,
position, reference, Ref_JoRevh_LowerArm_Wrist, null, # relative offset
rotation orientation, reference, Ref_JoRevh_LowerArm_Wrist, eye, # relative rotation orientation
position constraint,
inactive, inactive, inactive,
null,
orientation constraint,
inactive, inactive, active,
single, 0., 0., 1., scalar function, "Fun_Motor_JoTotj_LowerArm_Wrist";
joint: JoTotj_Wrist_Hand,
total joint,
Node_Wrist,
position, reference, Ref_JoRevh_Wrist_Hand, null, # relative offset
rotation orientation, reference, Ref_JoRevh_Wrist_Hand, eye, # relative rotation orientation
Node_Hand,
position, reference, Ref_JoRevh_Wrist_Hand, null, # relative offset
rotation orientation, reference, Ref_JoRevh_Wrist_Hand, eye, # relative rotation orientation
position constraint,
inactive, inactive, inactive,
null,
orientation constraint,
inactive, inactive, active,
single, 0., 0., 1., scalar function, "Fun_Motor_JoTotj_Wrist_Hand";
#-----------------------------------------------------------------------------
# Gravity
gravity: 0., 0., -1., const, 9.81;
end: elements;
