Home > リソース > MBDynモデル集 > 18. ロボットアーム
MBDynモデル集

18. ロボットアーム

アニメーション



入力ファイル

robot_arm.mbd
# 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;