Home > リソース > MBDynモデル集 > 24. 受動歩行するからくりロボット
MBDynモデル集

24. 受動歩行するからくりロボット

デザイン

このからくりロボットのデザインはRoberto Lou Ma氏(THE AUTOMATA / AUTOMATON BLOG)によるものです。

実物の動作



アニメーション



入力ファイル

passive_walking_robot.mbd
# passive_walking_robot.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-4;
   max iterations: 500;
   tolerance:      1.e-7;
end: initial value;

#-----------------------------------------------------------------------------
# [Control Data Block]

begin: control data;
   output frequency: 50;
   structural nodes: 6;
   abstract nodes:   2;
   rigid bodies:     10;
   joints:           3;
   forces:           12;
   genels:           2;
   gravity;
end: control data;

#-----------------------------------------------------------------------------
# Design Variables
set: real rho = 500.; #[kg/m^3] Density of the material (wood)

set: real Lx_Head = 6.35e-2; #[m] Width of the Head piece
set: real Ly_Head = 4.46e-2; #[m] Depth of the Head piece
set: real Lz_Head = 2.54e-2; #[m] Height of the Head piece

set: real Lx_BodyBack = 7.62e-2; #[m] Width of the BodyBack piece
set: real Ly_BodyBack = 0.79e-2; #[m] Depth of the BodyBack piece
set: real Lz_BodyBack = 8.89e-2; #[m] Height of the BodyBack piece

set: real Lx_BodyFront = 7.62e-2; #[m] Width of the BodyFront piece
set: real Ly_BodyFront = 0.79e-2; #[m] Depth of the BodyFront piece
set: real Lz_BodyFront = 8.89e-2; #[m] Height of the BodyFront piece

set: real Lx_BodySpacer     = 1.59e-2; #[m] Width of the BodySpacer piece
set: real Ly_BodySpacer     = 3.18e-2; #[m] Depth of the BodySpacer piece
set: real Lz_BodySpacer     = 7.62e-2; #[m] Height of the BodySpacer piece
set: real h_hole_BodySpacer = 1.27e-2; #[m] Location of the axle hole from the top

set: real Lx_Arm     = 1.59e-2; #[m] Width of the Arm piece
set: real Ly_Arm     = 2.54e-2; #[m] Depth of the Arm piece
set: real Lz_Arm     = 7.62e-2; #[m] Height of the Arm piece
set: real h_hole_Arm = 1.59e-2; #[m] Location of the axle hole from the top

set: real Lx_Leg     = 2.22e-2;  #[m] Width of the Leg piece
set: real Ly_Leg     = 1.91e-2;  #[m] Depth of the Leg piece
set: real Lz_Leg     = 10.16e-2; #[m] Height of the Leg piece
set: real h_hole_Leg = 0.95e-2;  #[m] Location of the axle hole from the top

set: real Lx_Foot = 3.49e-2; #[m] Width of the Foot piece
set: real Ly_Foot = 6.99e-2; #[m] Depth of the Foot piece
set: real Lz_Foot = 1.91e-2; #[m] Height of the Foot piece

set: real d_ArmSpacer = 0.95e-2; #[m] Thickness of the ArmSpacer

set: real d_Washer = 0.1e-2; #[m] Thickness of the Washer

set: real R = 17.78e-2; #[m] Radius of the foot curvature

set: real phi_Arm = 10.*deg2rad; #[rad] Arm raise angle

set: real theta    = 3.*deg2rad; #[rad] Surface angle
set: real vx0_Head = -0.2;       #[m/s] Initial lateral velocity of the head

set: real Kc = 100000.; #[N/m]  Contact stiffness
set: real Ec = 1.4;     #[-]    Contact stiffness exponent
set: real Cc = 100.;    #[Ns/m] Contact damping
set: real Dc = 0.001;   #[m]    Contact damping activation depth

set: real Ks_a = 1000.; #[Nm/rad]  Revolute hinge stopper stiffness
set: real Es_a = 1.4;   #[-]       Revolute hinge stopper stiffness exponent
set: real Cs_a = 10.;   #[Nms/rad] Revolute hinge stopper damping
set: real Ds_a = 0.01;  #[rad]     Revolute hinge stopper damping activation depth

set: real mu = 0.3;   #[-]   Friction coefficient
set: real Vt = 0.005; #[m/s] Friction threshold velocity

#-----------------------------------------------------------------------------
# Intermediate Variables
set: real m_Head   = rho * Lx_Head * Ly_Head * Lz_Head;
set: real Ixx_Head = 1./12. * m_Head * (Ly_Head^2 + Lz_Head^2);
set: real Iyy_Head = 1./12. * m_Head * (Lz_Head^2 + Lx_Head^2);
set: real Izz_Head = 1./12. * m_Head * (Lx_Head^2 + Ly_Head^2);

set: real m_BodyBack   = rho * Lx_BodyBack * Ly_BodyBack * Lz_BodyBack;
set: real Ixx_BodyBack = 1./12. * m_BodyBack * (Ly_BodyBack^2 + Lz_BodyBack^2);
set: real Iyy_BodyBack = 1./12. * m_BodyBack * (Lz_BodyBack^2 + Lx_BodyBack^2);
set: real Izz_BodyBack = 1./12. * m_BodyBack * (Lx_BodyBack^2 + Ly_BodyBack^2);

set: real m_BodyFront   = rho * Lx_BodyFront * Ly_BodyFront * Lz_BodyFront;
set: real Ixx_BodyFront = 1./12. * m_BodyFront * (Ly_BodyFront^2 + Lz_BodyFront^2);
set: real Iyy_BodyFront = 1./12. * m_BodyFront * (Lz_BodyFront^2 + Lx_BodyFront^2);
set: real Izz_BodyFront = 1./12. * m_BodyFront * (Lx_BodyFront^2 + Ly_BodyFront^2);

set: real m_BodySpacer   = rho * Lx_BodySpacer * Ly_BodySpacer * Lz_BodySpacer;
set: real Ixx_BodySpacer = 1./12. * m_BodySpacer * (Ly_BodySpacer^2 + Lz_BodySpacer^2);
set: real Iyy_BodySpacer = 1./12. * m_BodySpacer * (Lz_BodySpacer^2 + Lx_BodySpacer^2);
set: real Izz_BodySpacer = 1./12. * m_BodySpacer * (Lx_BodySpacer^2 + Ly_BodySpacer^2);

set: real m_Arm   = rho * Lx_Arm * Ly_Arm * Lz_Arm;
set: real Ixx_Arm = 1./12. * m_Arm * (Ly_Arm^2 + Lz_Arm^2);
set: real Iyy_Arm = 1./12. * m_Arm * (Lz_Arm^2 + Lx_Arm^2);
set: real Izz_Arm = 1./12. * m_Arm * (Lx_Arm^2 + Ly_Arm^2);

set: real m_Leg   = rho * Lx_Leg * Ly_Leg * Lz_Leg;
set: real Ixx_Leg = 1./12. * m_Leg * (Ly_Leg^2 + Lz_Leg^2);
set: real Iyy_Leg = 1./12. * m_Leg * (Lz_Leg^2 + Lx_Leg^2);
set: real Izz_Leg = 1./12. * m_Leg * (Lx_Leg^2 + Ly_Leg^2);

set: real m_Foot   = rho * Lx_Foot * Ly_Foot * Lz_Foot;
set: real Ixx_Foot = 1./12. * m_Foot * (Ly_Foot^2 + Lz_Foot^2);
set: real Iyy_Foot = 1./12. * m_Foot * (Lz_Foot^2 + Lx_Foot^2);
set: real Izz_Foot = 1./12. * m_Foot * (Lx_Foot^2 + Ly_Foot^2);

set: real h_Axle = Lz_Foot + Lz_Leg - h_hole_Leg;
set: real h_HeadCG = h_Axle + h_hole_BodySpacer + Lz_Head/2.;

set: real RZ_RevMin = -atan((Ly_BodySpacer-Ly_Leg)/2/(Lz_BodyFront-h_hole_BodySpacer)); 
                                                #[rad] Minimum angle of the revolute hinge
set: real RZ_RevMax = atan((Ly_BodySpacer-Ly_Leg)/2/(Lz_BodyFront-h_hole_BodySpacer));
                                                #[rad] Maximum angle of the revolute hinge

#-----------------------------------------------------------------------------
# Reference Labels
set: integer Ref_Surface                 = 1;
set: integer Ref_Surface_InitialVelocity = 2;
set: integer Ref_LeftFootCurvCenter      = 3;
set: integer Ref_RightFootCurvCenter     = 4;
set: integer Ref_HeadCG                  = 5;
set: integer Ref_BodyBackCG              = 6;
set: integer Ref_BodyFrontCG             = 7;
set: integer Ref_BodySpacerCG            = 8;
set: integer Ref_LeftShoulder            = 9;
set: integer Ref_RightShoulder           = 10;
set: integer Ref_LeftArmCG               = 11;
set: integer Ref_RightArmCG              = 12;
set: integer Ref_LeftLegCG               = 13;
set: integer Ref_RightLegCG              = 14;
set: integer Ref_LeftFootCG              = 15;
set: integer Ref_RightFootCG             = 16;
set: integer Ref_JoRevh_Body_Leg         = 17;

# Structural Node Labels
set: integer NoSta_Surface                = 1;
set: integer NoDyn_Body                   = 2;
set: integer NoDyn_LeftLeg                = 3;
set: integer NoDyn_RightLeg               = 4;
set: integer NoDum_LeftLeg_RelTo_Surface  = 5;
set: integer NoDum_RightLeg_RelTo_Surface = 6;

# Abstract Node Labels
set: integer NoAbs_FN_LeftLeg  = 1;
set: integer NoAbs_FN_RightLeg = 2;

# Body Labels
set: integer Body_Head       = 1;
set: integer Body_BodyBack   = 2;
set: integer Body_BodyFront  = 3;
set: integer Body_BodySpacer = 4;
set: integer Body_LeftArm    = 5;
set: integer Body_RightArm   = 6;
set: integer Body_LeftLeg    = 7;
set: integer Body_RightLeg   = 8;
set: integer Body_LeftFoot   = 9;
set: integer Body_RightFoot  = 10;

# Joint Labels
set: integer JoClamp_Surface      = 1;
set: integer JoRevh_Body_LeftLeg  = 2;
set: integer JoRevh_Body_RightLeg = 3;

# Force Labels
set: integer FoStrin_Normal_LeftLeg_Surface      = 1;
set: integer FoStrin_FrictionFX_LeftLeg_Surface  = 2;
set: integer FoStrin_FrictionFY_LeftLeg_Surface  = 3;
set: integer CoStrin_FrictionTX_LeftLeg_Surface  = 4;
set: integer CoStrin_FrictionTY_LeftLeg_Surface  = 5;
set: integer FoStrin_Normal_RightLeg_Surface     = 6;
set: integer FoStrin_FrictionFX_RightLeg_Surface = 7;
set: integer FoStrin_FrictionFY_RightLeg_Surface = 8;
set: integer CoStrin_FrictionTX_RightLeg_Surface = 9;
set: integer CoStrin_FrictionTY_RightLeg_Surface = 10;
set: integer CoStrin_Stopper_LeftLeg_Body        = 11;
set: integer CoStrin_Stopper_RightLeg_Body       = 12;

# Genel Labels
set: integer GeClamp_FN_LeftLeg  = 1;
set: integer GeClamp_FN_RightLeg = 2;

#-----------------------------------------------------------------------------
# Scalar Functions
scalar function: "cubstep",
   cubicspline, do not extrapolate,
      -0.03, 0.00,
      -0.02, 0.00,
      -0.01, 0.00,
       0.00, 0.00,
       1.00, 1.00,
       1.01, 1.00,
       1.02, 1.00,
       1.03, 1.00;

scalar function: "cubsign",
   cubicspline, do not extrapolate,
      -1.03, -1.00,
      -1.02, -1.00,
      -1.01, -1.00,
      -1.00, -1.00,
       1.00,  1.00,
       1.01,  1.00,
       1.02,  1.00,
       1.03,  1.00;
   
#-----------------------------------------------------------------------------
# References

reference: Ref_Surface,
   null,                 # absolute position
   euler, theta, 0., 0., # absolute orientation
   null,                 # absolute velocity
   null;                 # absolute angular velocity
   
reference: Ref_Surface_InitialVelocity,
   reference, Ref_Surface, null,                      # absolute position
   reference, Ref_Surface, eye,                       # absolute orientation
   reference, Ref_Surface, null,                      # absolute velocity
   reference, Ref_Surface, 0., vx0_Head/h_HeadCG, 0.; # absolute angular velocity

reference: Ref_LeftFootCurvCenter,
   reference, Ref_Surface_InitialVelocity, Lx_BodySpacer/2. + d_Washer,
                                           0.,
                                           R,    # absolute position
   reference, Ref_Surface_InitialVelocity, eye,  # absolute orientation
   reference, Ref_Surface_InitialVelocity, null, # absolute velocity
   reference, Ref_Surface_InitialVelocity, null; # absolute angular velocity

reference: Ref_RightFootCurvCenter,
   reference, Ref_Surface_InitialVelocity, -Lx_BodySpacer/2. - d_Washer,
                                           0.,
                                           R,    # absolute position
   reference, Ref_Surface_InitialVelocity, eye,  # absolute orientation
   reference, Ref_Surface_InitialVelocity, null, # absolute velocity
   reference, Ref_Surface_InitialVelocity, null; # absolute angular velocity
   
reference: Ref_HeadCG,
   reference, Ref_Surface, 0., 0., h_HeadCG, # absolute position
   reference, Ref_Surface, eye,              # absolute orientation
   reference, Ref_Surface, null,             # absolute velocity
   reference, Ref_Surface, null;             # absolute angular velocity
   
reference: Ref_BodyBackCG,
   reference, Ref_Surface, 0.,
                           Ly_BodySpacer/2. + Ly_BodyBack/2.,
                           h_Axle + h_hole_BodySpacer - Lz_BodyBack/2., # absolute position
   reference, Ref_Surface, eye,                                         # absolute orientation
   reference, Ref_Surface, null,                                        # absolute velocity
   reference, Ref_Surface, null;                                        # absolute angular velocity

reference: Ref_BodyFrontCG,
   reference, Ref_Surface, 0.,
                           -Ly_BodySpacer/2. - Ly_BodyFront/2.,
                           h_Axle + h_hole_BodySpacer - Lz_BodyFront/2., # absolute position
   reference, Ref_Surface, eye,                                          # absolute orientation
   reference, Ref_Surface, null,                                         # absolute velocity
   reference, Ref_Surface, null;                                         # absolute angular velocity

reference: Ref_BodySpacerCG,
   reference, Ref_Surface, 0.,
                           0.,
                           h_Axle + h_hole_BodySpacer - Lz_BodySpacer/2., # absolute position
   reference, Ref_Surface, eye,                                           # absolute orientation
   reference, Ref_Surface, null,                                          # absolute velocity
   reference, Ref_Surface, null;                                          # absolute angular velocity

reference: Ref_LeftShoulder,
   reference, Ref_Surface, Lx_BodySpacer/2. + d_Washer + Lx_Leg + d_ArmSpacer + Lx_Arm/2.,
                           0.,
                           h_Axle,                  # absolute position
   reference, Ref_Surface, euler, -phi_Arm, 0., 0., # absolute orientation
   reference, Ref_Surface, null,                    # absolute velocity
   reference, Ref_Surface, null;                    # absolute angular velocity
   
reference: Ref_RightShoulder,
   reference, Ref_Surface, -Lx_BodySpacer/2. - d_Washer - Lx_Leg - d_ArmSpacer - Lx_Arm/2.,
                           0.,
                           h_Axle,                  # absolute position
   reference, Ref_Surface, euler, -phi_Arm, 0., 0., # absolute orientation
   reference, Ref_Surface, null,                    # absolute velocity
   reference, Ref_Surface, null;                    # absolute angular velocity
   
reference: Ref_LeftArmCG,
   reference, Ref_LeftShoulder, 0., 0., h_hole_Arm - Lz_Arm/2., # absolute position
   reference, Ref_LeftShoulder, eye,                            # absolute orientation
   reference, Ref_LeftShoulder, null,                           # absolute velocity
   reference, Ref_LeftShoulder, null;                           # absolute angular velocity
   
reference: Ref_RightArmCG,
   reference, Ref_RightShoulder, 0., 0., h_hole_Arm - Lz_Arm/2., # absolute position
   reference, Ref_RightShoulder, eye,                            # absolute orientation
   reference, Ref_RightShoulder, null,                           # absolute velocity
   reference, Ref_RightShoulder, null;                           # absolute angular velocity
   
reference: Ref_LeftLegCG,
   reference, Ref_Surface, Lx_BodySpacer/2. + d_Washer + Lx_Leg/2.,
                           0.,
                           Lz_Foot + Lz_Leg/2., # absolute position
   reference, Ref_Surface, eye,                 # absolute orientation
   reference, Ref_Surface, null,                # absolute velocity
   reference, Ref_Surface, null;                # absolute angular velocity
   
reference: Ref_RightLegCG,
   reference, Ref_Surface, -Lx_BodySpacer/2. - d_Washer - Lx_Leg/2.,
                           0.,
                           Lz_Foot + Lz_Leg/2., # absolute position
   reference, Ref_Surface, eye,                 # absolute orientation
   reference, Ref_Surface, null,                # absolute velocity
   reference, Ref_Surface, null;                # absolute angular velocity

reference: Ref_LeftFootCG,
   reference, Ref_Surface, Lx_BodySpacer/2. + d_Washer + Lx_Foot/2.,
                           0.,
                           Lz_Foot/2., # absolute position
   reference, Ref_Surface, eye,        # absolute orientation
   reference, Ref_Surface, null,       # absolute velocity
   reference, Ref_Surface, null;       # absolute angular velocity
   
reference: Ref_RightFootCG,
   reference, Ref_Surface, -Lx_BodySpacer/2. - d_Washer - Lx_Foot/2.,
                           0.,
                           Lz_Foot/2., # absolute position
   reference, Ref_Surface, eye,        # absolute orientation
   reference, Ref_Surface, null,       # absolute velocity
   reference, Ref_Surface, null;       # absolute angular velocity
   
reference: Ref_JoRevh_Body_Leg,
   reference, Ref_Surface, 0., 0., h_Axle,       # absolute position
   reference, Ref_Surface, euler, 0., pi/2., 0., # absolute orientation
   reference, Ref_Surface, null,                 # absolute velocity
   reference, Ref_Surface, null;                 # absolute angular velocity
      
#-----------------------------------------------------------------------------
# [Nodes Block]

begin: nodes;

   #-----------------------------------------------------------------------------
   # Nodes
   structural: NoSta_Surface, static,
      reference, Ref_Surface, null, # absolute position
      reference, Ref_Surface, eye,  # absolute orientation
      reference, Ref_Surface, null, # absolute velocity
      reference, Ref_Surface, null; # absolute angular velocity

   structural: NoDyn_Body, dynamic,
      reference, Ref_Surface_InitialVelocity, 0., 0., h_Axle, # absolute position
      reference, Ref_Surface_InitialVelocity, eye,            # absolute orientation
      reference, Ref_Surface_InitialVelocity, null,           # absolute velocity
      reference, Ref_Surface_InitialVelocity, null;           # absolute angular velocity
      
   structural: NoDyn_LeftLeg, dynamic,
      reference, Ref_LeftFootCurvCenter, null, # absolute position
      reference, Ref_LeftFootCurvCenter, eye,  # absolute orientation
      reference, Ref_LeftFootCurvCenter, null, # absolute velocity
      reference, Ref_LeftFootCurvCenter, null, # absolute angular velocity
      accelerations, yes;
      
   structural: NoDyn_RightLeg, dynamic,
      reference, Ref_RightFootCurvCenter, null, # absolute position
      reference, Ref_RightFootCurvCenter, eye,  # absolute orientation
      reference, Ref_RightFootCurvCenter, null, # absolute velocity
      reference, Ref_RightFootCurvCenter, null, # absolute angular velocity
      accelerations, yes;
      
   structural: NoDum_LeftLeg_RelTo_Surface, dummy, NoDyn_LeftLeg,
      relative frame, NoSta_Surface;
      
   structural: NoDum_RightLeg_RelTo_Surface, dummy, NoDyn_RightLeg,
      relative frame, NoSta_Surface;
      
   abstract: NoAbs_FN_LeftLeg, algebraic;
   
   abstract: NoAbs_FN_RightLeg, algebraic;
      
end: nodes;

#-----------------------------------------------------------------------------
# Plugin Variables
set: [node, DZ_L, NoDum_LeftLeg_RelTo_Surface, structural, string="X[3]"];
set: [node, VX_L, NoDum_LeftLeg_RelTo_Surface, structural, string="XP[1]"];
set: [node, VY_L, NoDum_LeftLeg_RelTo_Surface, structural, string="XP[2]"];
set: [node, VZ_L, NoDum_LeftLeg_RelTo_Surface, structural, string="XP[3]"];
set: [node, WX_L, NoDum_LeftLeg_RelTo_Surface, structural, string="Omega[1]"];
set: [node, WY_L, NoDum_LeftLeg_RelTo_Surface, structural, string="Omega[2]"];

set: [node, DZ_R, NoDum_RightLeg_RelTo_Surface, structural, string="X[3]"];
set: [node, VX_R, NoDum_RightLeg_RelTo_Surface, structural, string="XP[1]"];
set: [node, VY_R, NoDum_RightLeg_RelTo_Surface, structural, string="XP[2]"];
set: [node, VZ_R, NoDum_RightLeg_RelTo_Surface, structural, string="XP[3]"];
set: [node, WX_R, NoDum_RightLeg_RelTo_Surface, structural, string="Omega[1]"];
set: [node, WY_R, NoDum_RightLeg_RelTo_Surface, structural, string="Omega[2]"];

#-----------------------------------------------------------------------------
# [Elements Block]

begin: elements;

   #-----------------------------------------------------------------------------
   # Bodies
   body: Body_Head, NoDyn_Body,
      m_Head,                                  # mass
      reference, Ref_HeadCG, null,             # relative center of mass
      diag, Ixx_Head, Iyy_Head, Izz_Head,      # inertia matrix
         inertial, reference, Ref_HeadCG, eye; # orientation
      
   body: Body_BodyBack, NoDyn_Body,
      m_BodyBack,                                     # mass
      reference, Ref_BodyBackCG, null,                # relative center of mass
      diag, Ixx_BodyBack, Iyy_BodyBack, Izz_BodyBack, # inertia matrix
         inertial, reference, Ref_BodyBackCG, eye;    # orientation
      
   body: Body_BodyFront, NoDyn_Body,
      m_BodyFront,                                       # mass
      reference, Ref_BodyFrontCG, null,                  # relative center of mass
      diag, Ixx_BodyFront, Iyy_BodyFront, Izz_BodyFront, # inertia matrix
         inertial, reference, Ref_BodyFrontCG, eye;      # orientation
      
   body: Body_BodySpacer, NoDyn_Body,
      m_BodySpacer,                                         # mass
      reference, Ref_BodySpacerCG, null,                    # relative center of mass
      diag, Ixx_BodySpacer, Iyy_BodySpacer, Izz_BodySpacer, # inertia matrix
         inertial, reference, Ref_BodySpacerCG, eye;        # orientation
      
   body: Body_LeftArm, NoDyn_Body,
      m_Arm,                                      # mass
      reference, Ref_LeftArmCG, null,             # relative center of mass
      diag, Ixx_Arm, Iyy_Arm, Izz_Arm,            # inertia matrix
         inertial, reference, Ref_LeftArmCG, eye; # orientation
      
   body: Body_RightArm, NoDyn_Body,
      m_Arm,                                       # mass
      reference, Ref_RightArmCG, null,             # relative center of mass
      diag, Ixx_Arm, Iyy_Arm, Izz_Arm,             # inertia matrix
         inertial, reference, Ref_RightArmCG, eye; # orientation
      
   body: Body_LeftLeg, NoDyn_LeftLeg,
      m_Leg,                                      # mass
      reference, Ref_LeftLegCG, null,             # relative center of mass
      diag, Ixx_Leg, Iyy_Leg, Izz_Leg,            # inertia matrix
         inertial, reference, Ref_LeftLegCG, eye; # orientation
      
   body: Body_RightLeg, NoDyn_RightLeg,
      m_Leg,                                       # mass
      reference, Ref_RightLegCG, null,             # relative center of mass
      diag, Ixx_Leg, Iyy_Leg, Izz_Leg,             # inertia matrix
         inertial, reference, Ref_RightLegCG, eye; # orientation
      
   body: Body_LeftFoot, NoDyn_LeftLeg,
      m_Foot,                                      # mass
      reference, Ref_LeftFootCG, null,             # relative center of mass
      diag, Ixx_Foot, Iyy_Foot, Izz_Foot,          # inertia matrix
         inertial, reference, Ref_LeftFootCG, eye; # orientation
      
   body: Body_RightFoot, NoDyn_RightLeg,
      m_Foot,                                       # mass
      reference, Ref_RightFootCG, null,             # relative center of mass
      diag, Ixx_Foot, Iyy_Foot, Izz_Foot,           # inertia matrix
         inertial, reference, Ref_RightFootCG, eye; # orientation
   
   #-----------------------------------------------------------------------------
   # Joints
   joint: JoClamp_Surface,
      clamp,
         NoSta_Surface,
            position, node,
            orientation, node;
               
   joint: JoRevh_Body_LeftLeg,
      revolute hinge,
         NoDyn_Body,
            reference, Ref_JoRevh_Body_Leg, null,       # relative offset
            hinge, reference, Ref_JoRevh_Body_Leg, eye, # relative orientation
         NoDyn_LeftLeg,
            reference, Ref_JoRevh_Body_Leg, null,       # relative offset
            hinge, reference, Ref_JoRevh_Body_Leg, eye; # relative orientation
            
   joint: JoRevh_Body_RightLeg,
      revolute hinge,
         NoDyn_Body,
            reference, Ref_JoRevh_Body_Leg, null,       # relative offset
            hinge, reference, Ref_JoRevh_Body_Leg, eye, # relative orientation
         NoDyn_RightLeg,
            reference, Ref_JoRevh_Body_Leg, null,       # relative offset
            hinge, reference, Ref_JoRevh_Body_Leg, eye; # relative orientation
     
   #-----------------------------------------------------------------------------
   # Genels
   genel: GeClamp_FN_LeftLeg,
      clamp,
         NoAbs_FN_LeftLeg, abstract,
         string, "max(0,Kc*sign(R-DZ_L)*abs(R-DZ_L)^Ec\
                       +Cc*(0-VZ_L)*model::sf::cubstep((R-DZ_L)/Dc))";
               
   genel: GeClamp_FN_RightLeg,
      clamp,
         NoAbs_FN_RightLeg, abstract,
         string, "max(0,Kc*sign(R-DZ_R)*abs(R-DZ_R)^Ec\
                       +Cc*(0-VZ_R)*model::sf::cubstep((R-DZ_R)/Dc))";
             
   #-----------------------------------------------------------------------------
   # Plugin Variables
   set: [element, RZ_Rev_L, JoRevh_Body_LeftLeg, joint, string="rz"];
   set: [element, WZ_Rev_L, JoRevh_Body_LeftLeg, joint, string="wz"];
   
   set: [element, RZ_Rev_R, JoRevh_Body_RightLeg, joint, string="rz"];
   set: [element, WZ_Rev_R, JoRevh_Body_RightLeg, joint, string="wz"];
   
   set: [dof, FN_L, NoAbs_FN_LeftLeg, abstract, algebraic];
   set: [dof, FN_R, NoAbs_FN_RightLeg, abstract, algebraic];
   
   #-----------------------------------------------------------------------------
   # Forces
   force: FoStrin_Normal_LeftLeg_Surface,
      absolute internal,
         NoDyn_LeftLeg,
            position, null,                                          # relative arm
         NoSta_Surface,
            position, null,                                          # relative arm
         single, reference, Ref_Surface, 0., 0., 1., string, "FN_L"; # force value
         
   force: FoStrin_FrictionFX_LeftLeg_Surface,
      absolute internal,
         NoDyn_LeftLeg,
            position, null,                                          # relative arm
         NoSta_Surface,
            position, null,                                          # relative arm
         single, reference, Ref_Surface, 1., 0., 0.,
            string, "-model::sf::cubsign((VX_L-R*WY_L)/Vt)*mu*FN_L"; # force value
            
   force: FoStrin_FrictionFY_LeftLeg_Surface,
      absolute internal,
         NoDyn_LeftLeg,
            position, null,                                          # relative arm
         NoSta_Surface,
            position, null,                                          # relative arm
         single, reference, Ref_Surface, 0., 1., 0.,
            string, "-model::sf::cubsign((VY_L+R*WX_L)/Vt)*mu*FN_L"; # force value
                           
   couple: CoStrin_FrictionTX_LeftLeg_Surface,
      absolute internal,
         NoDyn_LeftLeg,
         NoSta_Surface,
         single, reference, Ref_Surface, 1., 0., 0.,
            string, "-R*model::sf::cubsign((VY_L+R*WX_L)/Vt)*mu*FN_L"; # couple value
            
   couple: CoStrin_FrictionTY_LeftLeg_Surface,
      absolute internal,
         NoDyn_LeftLeg,
         NoSta_Surface,
         single, reference, Ref_Surface, 0., 1., 0.,
            string, "R*model::sf::cubsign((VX_L-R*WY_L)/Vt)*mu*FN_L"; # couple value
            
   force: FoStrin_Normal_RightLeg_Surface,
      absolute internal,
         NoDyn_RightLeg,
            position, null,                                          # relative arm
         NoSta_Surface,
            position, null,                                          # relative arm
         single, reference, Ref_Surface, 0., 0., 1., string, "FN_R"; # force value
         
   force: FoStrin_FrictionFX_RightLeg_Surface,
      absolute internal,
         NoDyn_RightLeg,
            position, null,                                          # relative arm
         NoSta_Surface,
            position, null,                                          # relative arm
         single, reference, Ref_Surface, 1., 0., 0.,
            string, "-model::sf::cubsign((VX_R-R*WY_R)/Vt)*mu*FN_R"; # force value
            
   force: FoStrin_FrictionFY_RightLeg_Surface,
      absolute internal,
         NoDyn_RightLeg,
            position, null,                                          # relative arm
         NoSta_Surface,
            position, null,                                          # relative arm
         single, reference, Ref_Surface, 0., 1., 0.,
            string, "-model::sf::cubsign((VY_R+R*WX_R)/Vt)*mu*FN_R"; # force value
                           
   couple: CoStrin_FrictionTX_RightLeg_Surface,
      absolute internal,
         NoDyn_RightLeg,
         NoSta_Surface,
         single, reference, Ref_Surface, 1., 0., 0.,
            string, "-R*model::sf::cubsign((VY_R+R*WX_R)/Vt)*mu*FN_R"; # couple value
            
   couple: CoStrin_FrictionTY_RightLeg_Surface,
      absolute internal,
         NoDyn_RightLeg,
         NoSta_Surface,
         single, reference, Ref_Surface, 0., 1., 0.,
            string, "R*model::sf::cubsign((VX_R-R*WY_R)/Vt)*mu*FN_R"; # couple value
            
   couple: CoStrin_Stopper_LeftLeg_Body,
      follower internal,
         NoDyn_LeftLeg,
            position, reference, Ref_JoRevh_Body_Leg, null,                                   # relative arm
         NoDyn_Body,
            position, reference, Ref_JoRevh_Body_Leg, null,                                   # relative arm
         single, reference, Ref_JoRevh_Body_Leg, 0., 0., 1.,
            string, "max(0,Ks_a*sign(RZ_RevMin-RZ_Rev_L)*abs(RZ_RevMin-RZ_Rev_L)^Es_a\
                          +Cs_a*(0-WZ_Rev_L)*model::sf::cubstep((RZ_RevMin-RZ_Rev_L)/Ds_a))\
                    -max(0,Ks_a*sign(RZ_Rev_L-RZ_RevMax)*abs(RZ_Rev_L-RZ_RevMax)^Es_a\
                          +Cs_a*(WZ_Rev_L-0)*model::sf::cubstep((RZ_Rev_L-RZ_RevMax)/Ds_a))"; # couple value
                          
   couple: CoStrin_Stopper_RightLeg_Body,
      follower internal,
         NoDyn_RightLeg,
            position, reference, Ref_JoRevh_Body_Leg, null,                                   # relative arm
         NoDyn_Body,
            position, reference, Ref_JoRevh_Body_Leg, null,                                   # relative arm
         single, reference, Ref_JoRevh_Body_Leg, 0., 0., 1.,
            string, "max(0,Ks_a*sign(RZ_RevMin-RZ_Rev_R)*abs(RZ_RevMin-RZ_Rev_R)^Es_a\
                          +Cs_a*(0-WZ_Rev_R)*model::sf::cubstep((RZ_RevMin-RZ_Rev_R)/Ds_a))\
                    -max(0,Ks_a*sign(RZ_Rev_R-RZ_RevMax)*abs(RZ_Rev_R-RZ_RevMax)^Es_a\
                          +Cs_a*(WZ_Rev_R-0)*model::sf::cubstep((RZ_Rev_R-RZ_RevMax)/Ds_a))"; # couple value
                           
   #-----------------------------------------------------------------------------
   # Gravity 
   gravity: 0., 0., -1., const, 9.81;
   
end: elements;