# robot_arm.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: 5; rigid bodies: 5; joints: 5 +5; 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 #----------------------------------------------------------------------------- # 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_Stage_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_Stage_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; # 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_Stage_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_Stage_UpperArm = 7; set: integer JoTotj_UpperArm_LowerArm = 8; set: integer JoTotj_LowerArm_Wrist = 9; set: integer JoTotj_Wrist_Hand = 10; #----------------------------------------------------------------------------- # 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_Stage_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 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 #----------------------------------------------------------------------------- # Joints joint: JoRevp_Stage, revolute pin, Node_Stage, null, # relative offset null; # absolute pin position joint: JoRevh_Stage_UpperArm, revolute hinge, Node_Stage, reference, Ref_JoRevh_Stage_UpperArm, null, # relative offset hinge, reference, Ref_JoRevh_Stage_UpperArm, eye, # relative axis orientation Node_UpperArm, reference, Ref_JoRevh_Stage_UpperArm, null, # relative offset hinge, reference, Ref_JoRevh_Stage_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_Stage_UpperArm, total joint, Node_Stage, position, reference, Ref_JoRevh_Stage_UpperArm, null, # relative offset rotation orientation, reference, Ref_JoRevh_Stage_UpperArm, eye, # relative rotation orientation Node_UpperArm, position, reference, Ref_JoRevh_Stage_UpperArm, null, # relative offset rotation orientation, reference, Ref_JoRevh_Stage_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_Stage_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;