# 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;