# walking_toy.mbd

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

begin: data;
   problem: initial value;
end: data;

#-----------------------------------------------------------------------------
# [<Problem> Block]

begin: initial value;
   initial time:   0.;
   final time:     7.;
   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: 9;
   abstract nodes:   4;
   rigid bodies:     2;
   joints:           4;
   forces:           11;
   genels:           4;
   gravity;
end: control data;

#-----------------------------------------------------------------------------
# Design Variables
set: real m_Body   = 0.2;   #[kg]     Mass of the body
set: real I_Body   = 0.001; #[kg-m^2] Moment of inertia of the body
set: real h_BodyCG = 0.07;  #[m]      Height of the body CG
set: real d_BodyCG = -0.01; #[m]      Longitudinal offset of the body CG

set: real m_Leg   = 0.05;   #[kg]     Mass of the leg
set: real I_Leg   = 0.0001; #[kg-m^2] Moment of inertia of the leg
set: real h_LegCG = 0.05;   #[m]      Height of the leg CG
set: real d_LegCG = -0.01;  #[m]      Longitudinal offset of the leg CG

set: real R = 0.2;  #[m] Radius of the foot curvature
set: real L = 0.05; #[m] Foot length (not used here)

set: real h_Rev     = 0.1;         #[m]   Height of the revolute hinge
set: real d_Rev     = 0.;          #[m]   Longitudinal offset of the revolute hinge
set: real RZ_RevMax = 15.*pi/180.; #[rad] Maximum angle of the revolute hinge

set: real theta   = 10.*pi/180.; #[rad] Surface angle
set: real v0_head = -0.2;        #[m/s] Initial head velocity

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

#-----------------------------------------------------------------------------
# Reference Labels
set: integer Ref_Surface         = 1;
set: integer Ref_FootCurvCenter  = 2;
set: integer Ref_BodyCG          = 3;
set: integer Ref_LegCG           = 4;
set: integer Ref_JoRevh_Body_Leg = 5;
set: integer Ref_FrontFootEdge   = 6;
set: integer Ref_RearFootEdge    = 7;

# Structural Node Labels
set: integer NoSta_Surface                     = 1;
set: integer NoDyn_Body                        = 2;
set: integer NoDyn_Leg                         = 3;
set: integer NoDum_Body_RelTo_Surface          = 4;
set: integer NoDum_Leg_RelTo_Surface           = 5;
set: integer NoDum_FrontFootEdge               = 6;
set: integer NoDum_RearFootEdge                = 7;
set: integer NoDum_FrontFootEdge_RelTo_Surface = 8;
set: integer NoDum_RearFootEdge_RelTo_Surface  = 9;

# Abstract Node Labels
set: integer NoAbs_FN_Body          = 1;
set: integer NoAbs_FN_Leg           = 2;
set: integer NoAbs_FN_FrontFootEdge = 3;
set: integer NoAbs_FN_RearFootEdge  = 4;

# Body Labels
set: integer Body_Body = 1;
set: integer Body_Leg  = 2;

# Joint Labels
set: integer JoClamp_Surface       = 1;
set: integer JoInp_Surface_Body    = 2;
set: integer JoRevrot_Surface_Body = 3;
set: integer JoRevh_Body_Leg       = 4;

# Force Labels
set: integer FoStrin_Normal_Body_Surface            = 1;
set: integer FoStrin_Friction_Body_Surface          = 2;
set: integer CoStrin_Friction_Body_Surface          = 3;
set: integer FoStrin_Normal_Leg_Surface             = 4;
set: integer FoStrin_Friction_Leg_Surface           = 5;
set: integer CoStrin_Friction_Leg_Surface           = 6;
set: integer FoStrin_Normal_FrontFootEdge_Surface   = 7;
set: integer FoStrin_Normal_RearFootEdge_Surface    = 8;
set: integer FoStrin_Friction_FrontFootEdge_Surface = 9;
set: integer FoStrin_Friction_RearFootEdge_Surface  = 10;
set: integer CoStrin_Stopper_Leg_Body               = 11;

# Genel Labels
set: integer GeClamp_FN_Body          = 1;
set: integer GeClamp_FN_Leg           = 2;
set: integer GeClamp_FN_FrontFootEdge = 3;
set: integer GeClamp_FN_RearFootEdge  = 4;

#-----------------------------------------------------------------------------
# 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_FootCurvCenter,
   reference, Ref_Surface, 0., 0., R,        # absolute position
   reference, Ref_Surface, eye,              # absolute orientation
   reference, Ref_Surface, 0., -v0_head, 0., # absolute velocity
   reference, Ref_Surface, null;             # absolute angular velocity
   
reference: Ref_BodyCG,
   reference, Ref_Surface, 0., -d_BodyCG, h_BodyCG, # absolute position
   reference, Ref_Surface, eye,                     # absolute orientation
   reference, Ref_Surface, null,                    # absolute velocity
   reference, Ref_Surface, null;                    # absolute angular velocity
   
reference: Ref_LegCG,
   reference, Ref_Surface, 0., -d_LegCG, h_LegCG, # 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., -d_Rev, h_Rev,    # 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
   
reference: Ref_FrontFootEdge,
   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
   
reference: Ref_RearFootEdge,
   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
      
#-----------------------------------------------------------------------------
# [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_FootCurvCenter, null, # absolute position
      reference, Ref_FootCurvCenter, eye,  # absolute orientation
      reference, Ref_FootCurvCenter, null, # absolute velocity
      reference, Ref_FootCurvCenter, null, # absolute angular velocity
      accelerations, yes;
      
   structural: NoDyn_Leg, dynamic,
      reference, Ref_FootCurvCenter, null, # absolute position
      reference, Ref_FootCurvCenter, eye,  # absolute orientation
      reference, Ref_FootCurvCenter, null, # absolute velocity
      reference, Ref_FootCurvCenter, null, # absolute angular velocity
      accelerations, yes;
      
   structural: NoDum_Body_RelTo_Surface, dummy, NoDyn_Body,
      relative frame, NoSta_Surface;
      
   structural: NoDum_Leg_RelTo_Surface, dummy, NoDyn_Leg,
      relative frame, NoSta_Surface;

   structural: NoDum_FrontFootEdge, dummy, NoDyn_Body,
      offset,
         reference, Ref_FrontFootEdge, null, # relative offset
         reference, Ref_FrontFootEdge, eye;  # relative orientation
         
   structural: NoDum_RearFootEdge, dummy, NoDyn_Leg,
      offset,
         reference, Ref_RearFootEdge, null, # relative offset
         reference, Ref_RearFootEdge, eye;  # relative orientation
         
   structural: NoDum_FrontFootEdge_RelTo_Surface, dummy, NoDum_FrontFootEdge,
      relative frame, NoSta_Surface;
      
   structural: NoDum_RearFootEdge_RelTo_Surface, dummy, NoDum_RearFootEdge,
      relative frame, NoSta_Surface;
      
   abstract: NoAbs_FN_Body, algebraic;
   
   abstract: NoAbs_FN_Leg, algebraic;
   
   abstract: NoAbs_FN_FrontFootEdge, algebraic;
   
   abstract: NoAbs_FN_RearFootEdge, algebraic;
      
end: nodes;

#-----------------------------------------------------------------------------
# Plugin Variables
set: [node, DZ_Body, NoDum_Body_RelTo_Surface, structural, string="X[3]"];
set: [node, VY_Body, NoDum_Body_RelTo_Surface, structural, string="XP[2]"];
set: [node, VZ_Body, NoDum_Body_RelTo_Surface, structural, string="XP[3]"];
set: [node, WX_Body, NoDum_Body_RelTo_Surface, structural, string="omega[1]"];
set: [node, RX_Body, NoDum_Body_RelTo_Surface, structural, string="E[1]"];

set: [node, DZ_Leg, NoDum_Leg_RelTo_Surface, structural, string="X[3]"];
set: [node, VY_Leg, NoDum_Leg_RelTo_Surface, structural, string="XP[2]"];
set: [node, VZ_Leg, NoDum_Leg_RelTo_Surface, structural, string="XP[3]"];
set: [node, WX_Leg, NoDum_Leg_RelTo_Surface, structural, string="omega[1]"];
set: [node, RX_Leg, NoDum_Leg_RelTo_Surface, structural, string="E[1]"];

set: [node, DZ_F, NoDum_FrontFootEdge_RelTo_Surface, structural, string="X[3]"];
set: [node, VY_F, NoDum_FrontFootEdge_RelTo_Surface, structural, string="XP[2]"];
set: [node, VZ_F, NoDum_FrontFootEdge_RelTo_Surface, structural, string="XP[3]"];

set: [node, DZ_R, NoDum_RearFootEdge_RelTo_Surface, structural, string="X[3]"];
set: [node, VY_R, NoDum_RearFootEdge_RelTo_Surface, structural, string="XP[2]"];
set: [node, VZ_R, NoDum_RearFootEdge_RelTo_Surface, structural, string="XP[3]"];

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

begin: elements;

   #-----------------------------------------------------------------------------
   # Bodies
   body: Body_Body, NoDyn_Body,
      m_Body,                      # mass
      reference, Ref_BodyCG, null, # relative center of mass
      diag, I_Body, 1., 1.;        # inertia matrix
      
   body: Body_Leg, NoDyn_Leg,
      m_Leg,                      # mass
      reference, Ref_LegCG, null, # relative center of mass
      diag, I_Leg, 1., 1.;        # inertia matrix
   
   #-----------------------------------------------------------------------------
   # Joints
   joint: JoClamp_Surface,
      clamp,
         NoSta_Surface,
            position, node,
            orientation, node;
            
   joint: JoInp_Surface_Body,
      in plane,
         NoSta_Surface,
            null,       # relative plane position
            1., 0., 0., # relative normal direction
         NoDyn_Body;
         
   joint: JoRevrot_Surface_Body,
      revolute rotation,
         NoSta_Surface,
            hinge,
               euler, 0., pi/2., 0., # relative orientation
         NoDyn_Body,
            hinge,
               euler, 0., pi/2., 0.; # relative orientation
               
   joint: JoRevh_Body_Leg,
      revolute hinge,
         NoDyn_Body,
            reference, Ref_JoRevh_Body_Leg, null,       # relative offset
            hinge, reference, Ref_JoRevh_Body_Leg, eye, # relative orientation
         NoDyn_Leg,
            reference, Ref_JoRevh_Body_Leg, null,       # relative offset
            hinge, reference, Ref_JoRevh_Body_Leg, eye; # relative orientation
     
   #-----------------------------------------------------------------------------
   # Genels
   genel: GeClamp_FN_Body,
      clamp,
         NoAbs_FN_Body, abstract,
         string, "max(0,Kc*sign(R-DZ_Body)*abs(R-DZ_Body)^Ec\
                       +Cc*(0-VZ_Body)*model::sf::cubstep((R-DZ_Body)/Dc))\
                 *step(RX_Body)";
               
   genel: GeClamp_FN_Leg,
      clamp,
         NoAbs_FN_Leg, abstract,
         string, "max(0,Kc*sign(R-DZ_Leg)*abs(R-DZ_Leg)^Ec\
                       +Cc*(0-VZ_Leg)*model::sf::cubstep((R-DZ_Leg)/Dc))\
                 *step(-RX_Leg)";
               
   genel: GeClamp_FN_FrontFootEdge,
      clamp,
         NoAbs_FN_FrontFootEdge, abstract,
         string, "max(0,Kc*sign(0-DZ_F)*abs(0-DZ_F)^Ec\
                       +Cc*(0-VZ_F)*model::sf::cubstep((0-DZ_F)/Dc))\
                 *(1-step(RX_Body))";
               
   genel: GeClamp_FN_RearFootEdge,
      clamp,
         NoAbs_FN_RearFootEdge, abstract,
         string, "max(0,Kc*sign(0-DZ_R)*abs(0-DZ_R)^Ec\
                       +Cc*(0-VZ_R)*model::sf::cubstep((0-DZ_R)/Dc))\
                 *(1-step(-RX_Leg))";
         
   #-----------------------------------------------------------------------------
   # Plugin Variables
   set: [element, RZ_Rev, JoRevh_Body_Leg, joint, string="rz"];
   set: [element, WZ_Rev, JoRevh_Body_Leg, joint, string="wz"];
   
   set: [dof, FN_Body, NoAbs_FN_Body, abstract, algebraic];
   set: [dof, FN_Leg, NoAbs_FN_Leg, abstract, algebraic];
   set: [dof, FN_F, NoAbs_FN_FrontFootEdge, abstract, algebraic];
   set: [dof, FN_R, NoAbs_FN_RearFootEdge, abstract, algebraic];
   
   #-----------------------------------------------------------------------------
   # Forces
   force: FoStrin_Normal_Body_Surface,
      absolute internal,
         NoDyn_Body,
            position, null,                                             # relative arm
         NoSta_Surface,
            position, null,                                             # relative arm
         single, reference, Ref_Surface, 0., 0., 1., string, "FN_Body"; # force value
         
   force: FoStrin_Friction_Body_Surface,
      absolute internal,
         NoDyn_Body,
            position, null,                                                   # relative arm
         NoSta_Surface,
            position, null,                                                   # relative arm
         single, reference, Ref_Surface, 0., 1., 0.,
            string, "-model::sf::cubsign((VY_Body+R*WX_Body)/Vt)*mu*FN_Body"; # force value
                           
   couple: CoStrin_Friction_Body_Surface,
      absolute internal,
         NoDyn_Body,
         NoSta_Surface,
         single, reference, Ref_Surface, 1., 0., 0.,
            string, "-R*model::sf::cubsign((VY_Body+R*WX_Body)/Vt)*mu*FN_Body"; # couple value
        
   force: FoStrin_Normal_Leg_Surface,
      absolute internal,
         NoDyn_Leg,
            position, null,                                            # relative arm
         NoSta_Surface,
            position, null,                                            # relative arm
         single, reference, Ref_Surface, 0., 0., 1., string, "FN_Leg"; # force value
         
   force: FoStrin_Friction_Leg_Surface,
      absolute internal,
         NoDyn_Leg,
            position, null,                                                # relative arm
         NoSta_Surface,
            position, null,                                                # relative arm
         single, reference, Ref_Surface, 0., 1., 0.,
            string, "-model::sf::cubsign((VY_Leg+R*WX_Leg)/Vt)*mu*FN_Leg"; # force value
                           
   couple: CoStrin_Friction_Leg_Surface,
      absolute internal,
         NoDyn_Leg,
         NoSta_Surface,
         single, reference, Ref_Surface, 1., 0., 0.,
            string, "-R*model::sf::cubsign((VY_Leg+R*WX_Leg)/Vt)*mu*FN_Leg"; # couple value        
                
   force: FoStrin_Normal_FrontFootEdge_Surface,
      absolute internal,
         NoDyn_Body,
            position, reference, Ref_FrontFootEdge, null,            # relative arm
         NoSta_Surface,
            position, null,                                          # relative arm
         single, reference, Ref_Surface, 0., 0., 1., string, "FN_F"; # force value
         
   force: FoStrin_Normal_RearFootEdge_Surface,
      absolute internal,
         NoDyn_Leg,
            position, reference, Ref_RearFootEdge, null,             # relative arm
         NoSta_Surface,
            position, null,                                          # relative arm
         single, reference, Ref_Surface, 0., 0., 1., string, "FN_R"; # force value
         
   force: FoStrin_Friction_FrontFootEdge_Surface,
      absolute internal,
         NoDyn_Body,
            position, reference, Ref_FrontFootEdge, null,   # relative arm
         NoSta_Surface,
            position, null,                                 # relative arm
         single, reference, Ref_Surface, 0., 1., 0.,
            string, "-model::sf::cubsign(VY_F/Vt)*mu*FN_F"; # force value
            
   force: FoStrin_Friction_RearFootEdge_Surface,
      absolute internal,
         NoDyn_Leg,
            position, reference, Ref_RearFootEdge, null,    # relative arm
         NoSta_Surface,
            position, null,                                 # relative arm
         single, reference, Ref_Surface, 0., 1., 0.,
            string, "-model::sf::cubsign(VY_R/Vt)*mu*FN_R"; # force value

   couple: CoStrin_Stopper_Leg_Body,
      follower internal,
         NoDyn_Leg,
            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(0-RZ_Rev)*abs(0-RZ_Rev)^Es_a\
                          +Cs_a*(0-WZ_Rev)*model::sf::cubstep((0-RZ_Rev)/Ds_a))\
                    -max(0,Ks_a*sign(RZ_Rev-RZ_RevMax)*abs(RZ_Rev-RZ_RevMax)^Es_a\
                          +Cs_a*(WZ_Rev-0)*model::sf::cubstep((RZ_Rev-RZ_RevMax)/Ds_a))"; # couple value
                           
   #-----------------------------------------------------------------------------
   # Gravity 
   gravity: 0., 0., -1., const, 9.81;
   
end: elements;