LIBRARY

`PlanarMechanics.examples.dyad_balans.DyadBalans2D`

This example models a wheeled balancing robot with a cascade control system stabilizing the angle in the inner loop and the position in the outer loop.

See also DyadBalans2DFF for a version with a feedforward generator.

Usage

MultibodyComponents.PlanarMechanics.examples.dyad_balans.DyadBalans2D(M=0.1, m=0.05, R=0.04, L=0.08, Ib=2.5e-4, Iw=1e-4, d=0.0001)

Parameters:

Name	Description	Units	Default value
`M`	Body mass	kg	0.1
`m`	Wheel mass	kg	0.05
`R`	Wheel radius	m	0.04
`L`	Distance from wheel axis to body center of mass	m	0.08
`Ib`	Body moment of inertia	kg.m2	0.00025
`Iw`	Wheel + motor moment of inertia	kg.m2	0.0001
`d`	Motor viscous friction	N.m.s/rad	0.0001

Behavior

[\begin{matrix} A n a l y s i s P o i n t (IMU . phi (t), y, [\begin{matrix} angle_controller . u_m (t) \end{matrix}]) \\ A n a l y s i s P o i n t (IMU . x (t), y 2, [\begin{matrix} pos_controller . u_m (t) \end{matrix}]) \\ A n a l y s i s P o i n t (angle_controller . y (t), u, [\begin{matrix} gain . u (t) \end{matrix}]) \\ A n a l y s i s P o i n t (pos_controller . y (t), u 2, [\begin{matrix} gain 1. u (t) \end{matrix}]) \\ connect (s q u a r e_{+} y, f i r s t o r d e r_{+} u) \\ connect (f i r s t o r d e r_{+} y, f i r s t o r d e r 1_{+} u) \\ connect (w h e e l i n e r t i a_{+} f r a m e_{a}, w h e e l J o i n t_{+} f r a m e_{a}) \\ connect (s i m p l e m o t o r_{+} f r a m e_{b}, w h e e l J o i n t_{+} f r a m e_{a}) \\ connect (s i m p l e m o t o r_{+} f r a m e_{a}, I M U_{+} f r a m e_{a}, b o d y_{m a s s_{+} f r a m e_a}) \\ connect (I M U_{+} p h i, a n g l e_{c o n t r o l l e r_{+} u_m}) \\ connect (I M U_{+} x, p o s_{c o n t r o l l e r_{+} u_m}) \\ connect (p o s_{c o n t r o l l e r_{+} y}, g a i n 1_{+} u) \\ connect (g a i n 1_{+} y, a n g l e_{c o n t r o l l e r_{+} u_s}) \\ connect (f i r s t o r d e r 1_{+} y, p o s_{c o n t r o l l e r_{+} u_s}) \\ connect (g a i n_{+} y, s i m p l e m o t o r_{+} t o r q u e i n p u t) \\ connect (a n g l e_{c o n t r o l l e r_{+} y}, g a i n_{+} u) \\ connect (c o n s t a n t 1_{+} y, p o s_{c o n t r o l l e r_{+} u_f f}) \\ connect (c o n s t a n t 2_{+} y, a n g l e_{c o n t r o l l e r_{+} u_f f}) \\ {world . frame}_{b . x} (t) = 0 \\ {world . frame}_{b . y} (t) = 0 \\ {world . frame}_{b . phi} (t) = 0 \\ connect (f r a m e_{b}, f r a m e_{v i s_{+} f r a m e_a}) \\ {world . frame}_{vis . phi} (t) = {world . frame}_{vis . frame_a . phi} (t) \\ {world . frame}_{vis . x_shape . r} (t) = {array}_{l i t e r a l} ([\begin{matrix} 3 \end{matrix}], {world . frame}_{vis . frame_a . x} (t), {world . frame}_{vis . frame_a . y} (t), world . {frame}_{vis . z_position}) \\ {world . frame}_{vis . x_shape . R} (t) = {array}_{l i t e r a l} ([\begin{matrix} 3 \\ 3 \end{matrix}], \cos (- {world . frame}_{vis . phi} (t)), \sin (- {world . frame}_{vis . phi} (t)), 0, - \sin (- {world . frame}_{vis . phi} (t)), \cos (- {world . frame}_{vis . phi} (t)), 0, 0, 0, 1) \\ {world . frame}_{vis . x_shape . r_shape} (t) = [\begin{matrix} 0 \\ 0 \\ 0 \end{matrix}] \\ {world . frame}_{vis . x_shape . length_direction} (t) = [\begin{matrix} 1 \\ 0 \\ 0 \end{matrix}] \\ {world . frame}_{vis . x_shape . width_direction} (t) = [\begin{matrix} 0 \\ 0 \\ 1 \end{matrix}] \\ {world . frame}_{vis . x_shape . length} (t) = world . {frame}_{vis . axis_length} \\ {world . frame}_{vis . x_shape . width} (t) = 2 \cdot world . {frame}_{vis . axis_radius} \\ {world . frame}_{vis . x_shape . height} (t) = 2 \cdot world . {frame}_{vis . axis_radius} \\ {world . frame}_{vis . y_shape . r} (t) = {array}_{l i t e r a l} ([\begin{matrix} 3 \end{matrix}], {world . frame}_{vis . frame_a . x} (t), {world . frame}_{vis . frame_a . y} (t), world . {frame}_{vis . z_position}) \\ {world . frame}_{vis . y_shape . R} (t) = {array}_{l i t e r a l} ([\begin{matrix} 3 \\ 3 \end{matrix}], \cos (- {world . frame}_{vis . phi} (t)), \sin (- {world . frame}_{vis . phi} (t)), 0, - \sin (- {world . frame}_{vis . phi} (t)), \cos (- {world . frame}_{vis . phi} (t)), 0, 0, 0, 1) \\ {world . frame}_{vis . y_shape . r_shape} (t) = [\begin{matrix} 0 \\ 0 \\ 0 \end{matrix}] \\ {world . frame}_{vis . y_shape . length_direction} (t) = [\begin{matrix} 0 \\ 1 \\ 0 \end{matrix}] \\ {world . frame}_{vis . y_shape . width_direction} (t) = [\begin{matrix} 0 \\ 0 \\ 1 \end{matrix}] \\ {world . frame}_{vis . y_shape . length} (t) = world . {frame}_{vis . axis_length} \\ {world . frame}_{vis . y_shape . width} (t) = 2 \cdot world . {frame}_{vis . axis_radius} \\ {world . frame}_{vis . y_shape . height} (t) = 2 \cdot world . {frame}_{vis . axis_radius} \\ {world . frame}_{vis . z_shape . r} (t) = {array}_{l i t e r a l} ([\begin{matrix} 3 \end{matrix}], {world . frame}_{vis . frame_a . x} (t), {world . frame}_{vis . frame_a . y} (t), world . {frame}_{vis . z_position}) \\ {world . frame}_{vis . z_shape . R} (t) = {array}_{l i t e r a l} ([\begin{matrix} 3 \\ 3 \end{matrix}], \cos (- {world . frame}_{vis . phi} (t)), \sin (- {world . frame}_{vis . phi} (t)), 0, - \sin (- {world . frame}_{vis . phi} (t)), \cos (- {world . frame}_{vis . phi} (t)), 0, 0, 0, 1) \\ {world . frame}_{vis . z_shape . r_shape} (t) = [\begin{matrix} 0 \\ 0 \\ 0 \end{matrix}] \\ {world . frame}_{vis . z_shape . length_direction} (t) = [\begin{matrix} 0 \\ 0 \\ 1 \end{matrix}] \\ {world . frame}_{vis . z_shape . width_direction} (t) = [\begin{matrix} 1 \\ 0 \\ 0 \end{matrix}] \\ {world . frame}_{vis . z_shape . length} (t) = world . {frame}_{vis . axis_length} \\ {world . frame}_{vis . z_shape . width} (t) = 2 \cdot world . {frame}_{vis . axis_radius} \\ {world . frame}_{vis . z_shape . height} (t) = 2 \cdot world . {frame}_{vis . axis_radius} \\ \frac{d \cdot firstorder . x (t)}{d t} = \frac{- firstorder . x (t) + firstorder . k \cdot firstorder . u (t)}{firstorder . T} \\ firstorder . y (t) = firstorder . x (t) \\ \frac{d \cdot firstorder 1. x (t)}{d t} = \frac{- firstorder 1. x (t) + firstorder 1. k \cdot firstorder 1. u (t)}{firstorder 1. T} \\ firstorder 1. y (t) = firstorder 1. x (t) \\ {wheelJoint . phi}_{roll} (t) = - {wheelJoint . frame}_{a . phi} (t) \\ {wheelJoint . w}_{roll} (t) = \frac{d \cdot {wheelJoint . phi}_{roll} (t)}{d t} \\ wheelJoint . x (t) = {wheelJoint . frame}_{a . x} (t) \\ wheelJoint . v (t) = \frac{d \cdot wheelJoint . x (t)}{d t} \\ wheelJoint . x (t) = wheelJoint . x 0 + wheelJoint . radius \cdot {wheelJoint . phi}_{roll} (t) \\ {wheelJoint . frame}_{a . tau} (t) = wheelJoint . radius \cdot {wheelJoint . frame}_{a . fx} (t) \\ {wheelJoint . frame}_{a . y} (t) = wheelJoint . radius \\ {wheelJoint . tire}_{shape . r} (t) = {array}_{l i t e r a l} ([\begin{matrix} 3 \end{matrix}], {wheelJoint . frame}_{a . x} (t), {wheelJoint . frame}_{a . y} (t), wheelJoint . z_{position}) \\ {wheelJoint . tire}_{shape . R} (t) = {array}_{l i t e r a l} ([\begin{matrix} 3 \\ 3 \end{matrix}], 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0) \\ {wheelJoint . tire}_{shape . r_shape} (t) = {array}_{l i t e r a l} ([\begin{matrix} 3 \end{matrix}], 0, 0, - \frac{1}{2} \cdot wheelJoint . {wheel}_{width}) \\ {wheelJoint . tire}_{shape . length_direction} (t) = [\begin{matrix} 0 \\ 0 \\ 1 \end{matrix}] \\ {wheelJoint . tire}_{shape . width_direction} (t) = [\begin{matrix} 0 \\ 1 \\ 0 \end{matrix}] \\ {wheelJoint . tire}_{shape . length} (t) = wheelJoint . {wheel}_{width} \\ {wheelJoint . tire}_{shape . width} (t) = 2 \cdot wheelJoint . radius \\ {wheelJoint . tire}_{shape . height} (t) = 2 \cdot wheelJoint . radius \\ {wheelJoint . rim 1}_{shape . r} (t) = {array}_{l i t e r a l} ([\begin{matrix} 3 \end{matrix}], {wheelJoint . frame}_{a . x} (t), {wheelJoint . frame}_{a . y} (t), wheelJoint . z_{position}) \\ {wheelJoint . rim 1}_{shape . R} (t) = {array}_{l i t e r a l} ([\begin{matrix} 3 \\ 3 \end{matrix}], \cos ({wheelJoint . phi}_{roll} (t)), - \sin ({wheelJoint . phi}_{roll} (t)), 0, \sin ({wheelJoint . phi}_{roll} (t)), \cos ({wheelJoint . phi}_{roll} (t)), 0, 0, 0, 1) \\ {wheelJoint . rim 1}_{shape . r_shape} (t) = {array}_{l i t e r a l} ([\begin{matrix} 3 \end{matrix}], - wheelJoint . radius, 0, 0) \\ {wheelJoint . rim 1}_{shape . length_direction} (t) = [\begin{matrix} 1 \\ 0 \\ 0 \end{matrix}] \\ {wheelJoint . rim 1}_{shape . width_direction} (t) = [\begin{matrix} 0 \\ 1 \\ 0 \end{matrix}] \\ {wheelJoint . rim 1}_{shape . length} (t) = 2 \cdot wheelJoint . radius \\ {wheelJoint . rim 1}_{shape . width} (t) = wheelJoint . {rim}_{diameter} \\ {wheelJoint . rim 1}_{shape . height} (t) = wheelJoint . {rim}_{diameter} \\ {wheelJoint . rim 2}_{shape . r} (t) = {array}_{l i t e r a l} ([\begin{matrix} 3 \end{matrix}], {wheelJoint . frame}_{a . x} (t), {wheelJoint . frame}_{a . y} (t), wheelJoint . z_{position}) \\ {wheelJoint . rim 2}_{shape . R} (t) = {array}_{l i t e r a l} ([\begin{matrix} 3 \\ 3 \end{matrix}], \cos (1.570796326794895 + {wheelJoint . phi}_{roll} (t)), - \sin (1.570796326794895 + {wheelJoint . phi}_{roll} (t)), 0, \sin (1.570796326794895 + {wheelJoint . phi}_{roll} (t)), \cos (1.570796326794895 + {wheelJoint . phi}_{roll} (t)), 0, 0, 0, 1) \\ {wheelJoint . rim 2}_{shape . r_shape} (t) = {array}_{l i t e r a l} ([\begin{matrix} 3 \end{matrix}], - wheelJoint . radius, 0, 0) \\ {wheelJoint . rim 2}_{shape . length_direction} (t) = [\begin{matrix} 1 \\ 0 \\ 0 \end{matrix}] \\ {wheelJoint . rim 2}_{shape . width_direction} (t) = [\begin{matrix} 0 \\ 1 \\ 0 \end{matrix}] \\ {wheelJoint . rim 2}_{shape . length} (t) = 2 \cdot wheelJoint . radius \\ {wheelJoint . rim 2}_{shape . width} (t) = wheelJoint . {rim}_{diameter} \\ {wheelJoint . rim 2}_{shape . height} (t) = wheelJoint . {rim}_{diameter} \\ constant 2. y (t) = constant 2. k \\ IMU . x (t) = IMU . pos . x (t) \\ IMU . y (t) = IMU . pos . y (t) \\ IMU . phi (t) = IMU . pos . phi (t) \\ connect (f r a m e_{a}, p o s_{+} f r a m e_{a}) \\ {IMU . pos . frame}_{a . fx} (t) = 0 \\ {IMU . pos . frame}_{a . fy} (t) = 0 \\ {IMU . pos . frame}_{a . tau} (t) = 0 \\ {array}_{l i t e r a l} ([\begin{matrix} 3 \end{matrix}], IMU . pos . x (t), IMU . pos . y (t), IMU . pos . phi (t)) = IMU . pos . r (t) \\ IMU . pos . r (t) = {array}_{l i t e r a l} ([\begin{matrix} 3 \end{matrix}], {IMU . pos . frame}_{a . x} (t), {IMU . pos . frame}_{a . y} (t), {IMU . pos . frame}_{a . phi} (t)) \\ square . y (t) = ifelse (square . {start}_{time} < t, square . offset + square . amplitude \cdot (1 - 2 \cdot ⌊ 2 \cdot square . frequency \cdot (- square . {start}_{time} + t) ⌋ + 4 \cdot ⌊ square . frequency \cdot (- square . {start}_{time} + t) ⌋), square . offset) \\ wheelinertia . r (t) = {array}_{l i t e r a l} ([\begin{matrix} 2 \end{matrix}], {wheelinertia . frame}_{a . x} (t), {wheelinertia . frame}_{a . y} (t)) \\ wheelinertia . v (t) = \frac{d \cdot wheelinertia . r (t)}{d t} \\ wheelinertia . phi (t) = {wheelinertia . frame}_{a . phi} (t) \\ wheelinertia . w (t) = \frac{d \cdot wheelinertia . phi (t)}{d t} \\ wheelinertia . a (t) = \frac{d \cdot wheelinertia . v (t)}{d t} \\ wheelinertia . alpha (t) = \frac{d \cdot wheelinertia . w (t)}{d t} \\ wheelinertia . f (t) = {array}_{l i t e r a l} ([\begin{matrix} 2 \end{matrix}], {wheelinertia . frame}_{a . fx} (t), {wheelinertia . frame}_{a . fy} (t)) \\ wheelinertia . f (t) + {array}_{l i t e r a l} ([\begin{matrix} 2 \end{matrix}], g \cdot {n_{2 d}}_{1} \cdot wheelinertia . m, g \cdot {n_{2 d}}_{2} \cdot wheelinertia . m) = wheelinertia . m \cdot wheelinertia . a (t) \\ wheelinertia . I \cdot wheelinertia . alpha (t) = {wheelinertia . frame}_{a . tau} (t) \\ wheelinertia . shape . r (t) = {array}_{l i t e r a l} ([\begin{matrix} 3 \end{matrix}], {wheelinertia . frame}_{a . x} (t), {wheelinertia . frame}_{a . y} (t), wheelinertia . z_{position}) \\ wheelinertia . shape . R (t) = {array}_{l i t e r a l} ([\begin{matrix} 3 \\ 3 \end{matrix}], \cos (- wheelinertia . phi (t)), \sin (- wheelinertia . phi (t)), 0, - \sin (- wheelinertia . phi (t)), \cos (- wheelinertia . phi (t)), 0, 0, 0, 1) \\ {wheelinertia . shape . r}_{shape} (t) = [\begin{matrix} 0 \\ 0 \\ 0 \end{matrix}] \\ {wheelinertia . shape . length}_{direction} (t) = [\begin{matrix} 0 \\ 0 \\ 1 \end{matrix}] \\ {wheelinertia . shape . width}_{direction} (t) = [\begin{matrix} 1 \\ 0 \\ 0 \end{matrix}] \\ wheelinertia . shape . length (t) = 2 \cdot wheelinertia . radius \\ wheelinertia . shape . width (t) = 2 \cdot wheelinertia . radius \\ wheelinertia . shape . height (t) = 2 \cdot wheelinertia . radius \\ connect (t o r q u e s o u r c e_{+} s p l i n e, m o t o r_{r o t a t i o n_{+} f l a n g e_a}, d a m p e r 1_{+} s p l i n e_{a}) \\ connect (f r a m e_{a}, m o t o r_{r o t a t i o n_{+} f r a m e_a}) \\ connect (m o t o r_{r o t a t i o n_{+} f r a m e_b}, f r a m e_{b}) \\ connect (t o r q u e s o u r c e_{+} s u p p o r t, d a m p e r 1_{+} s p l i n e_{b}, f i x e d_{+} s p l i n e) \\ connect (t o r q u e i n p u t, t o r q u e s o u r c e_{+} t a u) \\ simplemotor . torquesource . support . phi (t) = {simplemotor . torquesource . phi}_{support} (t) \\ simplemotor . torquesource . support . tau (t) = - simplemotor . torquesource . spline . tau (t) \\ simplemotor . torquesource . phi (t) = - {simplemotor . torquesource . phi}_{support} (t) + simplemotor . torquesource . spline . phi (t) \\ simplemotor . torquesource . spline . tau (t) = - simplemotor . torquesource . tau (t) \\ {simplemotor . motor}_{rotation . w} (t) = \frac{d \cdot {simplemotor . motor}_{rotation . phi} (t)}{d t} \\ {simplemotor . motor}_{rotation . alpha} (t) = \frac{d \cdot {simplemotor . motor}_{rotation . w} (t)}{d t} \\ {simplemotor . motor}_{rotation . frame_a . x} (t) = {simplemotor . motor}_{rotation . frame_b . x} (t) \\ {simplemotor . motor}_{rotation . frame_a . y} (t) = {simplemotor . motor}_{rotation . frame_b . y} (t) \\ {simplemotor . motor}_{rotation . phi} (t) + {simplemotor . motor}_{rotation . frame_a . phi} (t) = {simplemotor . motor}_{rotation . frame_b . phi} (t) \\ {simplemotor . motor}_{rotation . frame_a . fx} (t) + {simplemotor . motor}_{rotation . frame_b . fx} (t) = 0 \\ {simplemotor . motor}_{rotation . frame_b . fy} (t) + {simplemotor . motor}_{rotation . frame_a . fy} (t) = 0 \\ {simplemotor . motor}_{rotation . frame_a . tau} (t) + {simplemotor . motor}_{rotation . frame_b . tau} (t) = 0 \\ {simplemotor . motor}_{rotation . frame_a . tau} (t) = {simplemotor . motor}_{rotation . tau} (t) \\ {simplemotor . motor}_{rotation . flange_a . phi} (t) = {simplemotor . motor}_{rotation . phi} (t) \\ {simplemotor . motor}_{rotation . flange_a . tau} (t) = {simplemotor . motor}_{rotation . tau} (t) \\ {simplemotor . motor}_{rotation . shape . r} (t) = {array}_{l i t e r a l} ([\begin{matrix} 3 \end{matrix}], {simplemotor . motor}_{rotation . frame_a . x} (t), {simplemotor . motor}_{rotation . frame_a . y} (t), simplemotor . {motor}_{rotation . z_position}) \\ {simplemotor . motor}_{rotation . shape . R} (t) = {array}_{l i t e r a l} ([\begin{matrix} 3 \\ 3 \end{matrix}], \cos (- {simplemotor . motor}_{rotation . frame_a . phi} (t)), \sin (- {simplemotor . motor}_{rotation . frame_a . phi} (t)), 0, - \sin (- {simplemotor . motor}_{rotation . frame_a . phi} (t)), \cos (- {simplemotor . motor}_{rotation . frame_a . phi} (t)), 0, 0, 0, 1) \\ {simplemotor . motor}_{rotation . shape . r_shape} (t) = {array}_{l i t e r a l} ([\begin{matrix} 3 \end{matrix}], 0, 0, - \frac{1}{2} \cdot simplemotor . {motor}_{rotation . cylinder_length}) \\ {simplemotor . motor}_{rotation . shape . length_direction} (t) = [\begin{matrix} 0 \\ 0 \\ 1 \end{matrix}] \\ {simplemotor . motor}_{rotation . shape . width_direction} (t) = [\begin{matrix} 1 \\ 0 \\ 0 \end{matrix}] \\ {simplemotor . motor}_{rotation . shape . length} (t) = simplemotor . {motor}_{rotation . cylinder_length} \\ {simplemotor . motor}_{rotation . shape . width} (t) = 2 \cdot simplemotor . {motor}_{rotation . radius} \\ {simplemotor . motor}_{rotation . shape . height} (t) = 2 \cdot simplemotor . {motor}_{rotation . radius} \\ {simplemotor . damper 1. phi}_{rel} (t) = {simplemotor . damper 1. spline}_{b . phi} (t) - {simplemotor . damper 1. spline}_{a . phi} (t) \\ {simplemotor . damper 1. spline}_{b . tau} (t) = simplemotor . damper 1. tau (t) \\ {simplemotor . damper 1. spline}_{a . tau} (t) = - simplemotor . damper 1. tau (t) \\ \frac{d \cdot {simplemotor . damper 1. phi}_{rel} (t)}{d t} = {simplemotor . damper 1. w}_{rel} (t) \\ \frac{d \cdot {simplemotor . damper 1. w}_{rel} (t)}{d t} = {simplemotor . damper 1. a}_{rel} (t) \\ simplemotor . damper 1. tau (t) = simplemotor . damper 1. d \cdot {simplemotor . damper 1. w}_{rel} (t) \\ simplemotor . fixed . spline . phi (t) = simplemotor . fixed . phi 0 \\ constant 1. y (t) = constant 1. k \\ {angle}_{controller . control_error} (t) = {angle}_{controller . u_s} (t) - {angle}_{controller . u_m} (t) \\ connect (a d d_{p_{+} y}, p r o p o r t i o n a l_{+} u) \\ connect (a d d_{d_{+} y}, d e r i v a t i v e_{+} u) \\ connect (a d d_{i_{+} y}, i n t e g r a t o r_{+} u) \\ connect (p r o p o r t i o n a l_{+} y, a d d_{p i d_{+} u 1}) \\ connect (d e r i v a t i v e_{+} y, a d d_{p i d_{+} u 2}) \\ connect (i n t e g r a t o r_{+} y, a d d_{p i d_{+} u 3}) \\ connect (a d d_{p i d_{+} y}, g a i n_{p i d_{+} u}) \\ connect (g a i n_{p i d_{+} y}, a d d_{f f_{+} u 1}) \\ connect (a d d_{f f_{+} y}, l i m i t e r_{+} u) \\ connect (l i m i t e r_{+} y, a d d_{s a t_{+} u 1}) \\ connect (a d d_{s a t_{+} y}, g a i n_{t r a c k_{+} u}) \\ connect (g a i n_{t r a c k_{+} y}, a d d_{i_{+} u 3}) \\ connect (l i m i t e r_{+} y, y) \\ connect (a d d_{f f_{+} u 2}, u_{f f}) \\ connect (a d d_{f f_{+} y}, a d d_{s a t_{+} u 2}) \\ connect (a d d_{p_{+} u 1}, u_{s}) \\ connect (a d d_{d_{+} u 1}, u_{s}) \\ connect (a d d_{i_{+} u 1}, u_{s}) \\ connect (u_{m}, a d d_{p_{+} u 2}) \\ connect (a d d_{i_{+} u 2}, u_{m}) \\ connect (a d d_{d_{+} u 2}, u_{m}) \\ {angle}_{controller . add_p . y} (t) = {angle}_{controller . add_p . k 1} \cdot {angle}_{controller . add_p . u 1} (t) + {angle}_{controller . add_p . k 2} \cdot {angle}_{controller . add_p . u 2} (t) \\ {angle}_{controller . add_d . y} (t) = {angle}_{controller . add_d . k 1} \cdot {angle}_{controller . add_d . u 1} (t) + {angle}_{controller . add_d . k 2} \cdot {angle}_{controller . add_d . u 2} (t) \\ {angle}_{controller . add_i . y} (t) = {angle}_{controller . add_i . k 1} \cdot {angle}_{controller . add_i . u 1} (t) + {angle}_{controller . add_i . k 2} \cdot {angle}_{controller . add_i . u 2} (t) + {angle}_{controller . add_i . k 3} \cdot {angle}_{controller . add_i . u 3} (t) \\ {angle}_{controller . proportional . y} (t) = {angle}_{controller . proportional . k} \cdot {angle}_{controller . proportional . u} (t) \\ \frac{d \cdot {angle}_{controller . derivative . x} (t)}{d t} = \frac{- {angle}_{controller . derivative . x} (t) + {angle}_{controller . derivative . u} (t)}{{angle}_{controller . derivative . T}} \\ {angle}_{controller . derivative . y} (t) = \frac{{angle}_{controller . derivative . k} \cdot (- {angle}_{controller . derivative . x} (t) + {angle}_{controller . derivative . u} (t))}{{angle}_{controller . derivative . T}} \\ \frac{d \cdot {angle}_{controller . integrator . x} (t)}{d t} = {angle}_{controller . integrator . k} \cdot {angle}_{controller . integrator . u} (t) \\ {angle}_{controller . integrator . y} (t) = {angle}_{controller . integrator . x} (t) \\ {angle}_{controller . add_pid . y} (t) = {angle}_{controller . add_pid . k 1} \cdot {angle}_{controller . add_pid . u 1} (t) + {angle}_{controller . add_pid . k 2} \cdot {angle}_{controller . add_pid . u 2} (t) + {angle}_{controller . add_pid . k 3} \cdot {angle}_{controller . add_pid . u 3} (t) \\ {angle}_{controller . gain_pid . y} (t) = {angle}_{controller . gain_pid . k} \cdot {angle}_{controller . gain_pid . u} (t) \\ {angle}_{controller . add_ff . y} (t) = {angle}_{controller . add_ff . k 1} \cdot {angle}_{controller . add_ff . u 1} (t) + {angle}_{controller . add_ff . k 2} \cdot {angle}_{controller . add_ff . u 2} (t) \\ {angle}_{controller . limiter . y} (t) = clamp ({angle}_{controller . limiter . u} (t), {angle}_{controller . limiter . y_\min}, {angle}_{controller . limiter . y_\max}) \\ {angle}_{controller . add_sat . y} (t) = {angle}_{controller . add_sat . k 1} \cdot {angle}_{controller . add_sat . u 1} (t) + {angle}_{controller . add_sat . k 2} \cdot {angle}_{controller . add_sat . u 2} (t) \\ {angle}_{controller . gain_track . y} (t) = {angle}_{controller . gain_track . k} \cdot {angle}_{controller . gain_track . u} (t) \\ {pos}_{controller . control_error} (t) = {pos}_{controller . u_s} (t) - {pos}_{controller . u_m} (t) \\ connect (a d d_{p_{+} y}, p r o p o r t i o n a l_{+} u) \\ connect (a d d_{d_{+} y}, d e r i v a t i v e_{+} u) \\ connect (a d d_{i_{+} y}, i n t e g r a t o r_{+} u) \\ connect (p r o p o r t i o n a l_{+} y, a d d_{p i d_{+} u 1}) \\ connect (d e r i v a t i v e_{+} y, a d d_{p i d_{+} u 2}) \\ connect (i n t e g r a t o r_{+} y, a d d_{p i d_{+} u 3}) \\ connect (a d d_{p i d_{+} y}, g a i n_{p i d_{+} u}) \\ connect (g a i n_{p i d_{+} y}, a d d_{f f_{+} u 1}) \\ connect (a d d_{f f_{+} y}, l i m i t e r_{+} u) \\ connect (l i m i t e r_{+} y, a d d_{s a t_{+} u 1}) \\ connect (a d d_{s a t_{+} y}, g a i n_{t r a c k_{+} u}) \\ connect (g a i n_{t r a c k_{+} y}, a d d_{i_{+} u 3}) \\ connect (l i m i t e r_{+} y, y) \\ connect (a d d_{f f_{+} u 2}, u_{f f}) \\ connect (a d d_{f f_{+} y}, a d d_{s a t_{+} u 2}) \\ connect (a d d_{p_{+} u 1}, u_{s}) \\ connect (a d d_{d_{+} u 1}, u_{s}) \\ connect (a d d_{i_{+} u 1}, u_{s}) \\ connect (u_{m}, a d d_{p_{+} u 2}) \\ connect (a d d_{i_{+} u 2}, u_{m}) \\ connect (a d d_{d_{+} u 2}, u_{m}) \\ {pos}_{controller . add_p . y} (t) = {pos}_{controller . add_p . k 1} \cdot {pos}_{controller . add_p . u 1} (t) + {pos}_{controller . add_p . k 2} \cdot {pos}_{controller . add_p . u 2} (t) \\ {pos}_{controller . add_d . y} (t) = {pos}_{controller . add_d . k 1} \cdot {pos}_{controller . add_d . u 1} (t) + {pos}_{controller . add_d . k 2} \cdot {pos}_{controller . add_d . u 2} (t) \\ {pos}_{controller . add_i . y} (t) = {pos}_{controller . add_i . k 1} \cdot {pos}_{controller . add_i . u 1} (t) + {pos}_{controller . add_i . k 2} \cdot {pos}_{controller . add_i . u 2} (t) + {pos}_{controller . add_i . k 3} \cdot {pos}_{controller . add_i . u 3} (t) \\ {pos}_{controller . proportional . y} (t) = {pos}_{controller . proportional . k} \cdot {pos}_{controller . proportional . u} (t) \\ \frac{d \cdot {pos}_{controller . derivative . x} (t)}{d t} = \frac{- {pos}_{controller . derivative . x} (t) + {pos}_{controller . derivative . u} (t)}{{pos}_{controller . derivative . T}} \\ {pos}_{controller . derivative . y} (t) = \frac{{pos}_{controller . derivative . k} \cdot (- {pos}_{controller . derivative . x} (t) + {pos}_{controller . derivative . u} (t))}{{pos}_{controller . derivative . T}} \\ \frac{d \cdot {pos}_{controller . integrator . x} (t)}{d t} = {pos}_{controller . integrator . k} \cdot {pos}_{controller . integrator . u} (t) \\ {pos}_{controller . integrator . y} (t) = {pos}_{controller . integrator . x} (t) \\ {pos}_{controller . add_pid . y} (t) = {pos}_{controller . add_pid . k 1} \cdot {pos}_{controller . add_pid . u 1} (t) + {pos}_{controller . add_pid . k 2} \cdot {pos}_{controller . add_pid . u 2} (t) + {pos}_{controller . add_pid . k 3} \cdot {pos}_{controller . add_pid . u 3} (t) \\ {pos}_{controller . gain_pid . y} (t) = {pos}_{controller . gain_pid . k} \cdot {pos}_{controller . gain_pid . u} (t) \\ {pos}_{controller . add_ff . y} (t) = {pos}_{controller . add_ff . k 1} \cdot {pos}_{controller . add_ff . u 1} (t) + {pos}_{controller . add_ff . k 2} \cdot {pos}_{controller . add_ff . u 2} (t) \\ {pos}_{controller . limiter . y} (t) = clamp ({pos}_{controller . limiter . u} (t), {pos}_{controller . limiter . y_\min}, {pos}_{controller . limiter . y_\max}) \\ {pos}_{controller . add_sat . y} (t) = {pos}_{controller . add_sat . k 1} \cdot {pos}_{controller . add_sat . u 1} (t) + {pos}_{controller . add_sat . k 2} \cdot {pos}_{controller . add_sat . u 2} (t) \\ {pos}_{controller . gain_track . y} (t) = {pos}_{controller . gain_track . k} \cdot {pos}_{controller . gain_track . u} (t) \\ gain . y (t) = gain . k \cdot gain . u (t) \\ gain 1. y (t) = gain 1. k \cdot gain 1. u (t) \\ {body}_{mass . rod_shape . r} (t) = {array}_{l i t e r a l} ([\begin{matrix} 3 \end{matrix}], {body}_{mass . frame_a . x} (t), {body}_{mass . frame_a . y} (t), {body}_{mass . z_position}) \\ {body}_{mass . rod_shape . R} (t) = {array}_{l i t e r a l} ([\begin{matrix} 3 \\ 3 \end{matrix}], 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0) \\ {body}_{mass . rod_shape . r_shape} (t) = [\begin{matrix} 0 \\ 0 \\ 0 \end{matrix}] \\ {body}_{mass . rod_shape . length_direction} (t) = {array}_{l i t e r a l} ([\begin{matrix} 3 \end{matrix}], \frac{{body}_{mass . translation . r 0}_1 (t)}{{body}_{mass . l}}, \frac{{body}_{mass . translation . r 0}_2 (t)}{{body}_{mass . l}}, 0) \\ {body}_{mass . rod_shape . width_direction} (t) = [\begin{matrix} 0 \\ 0 \\ 1 \end{matrix}] \\ {body}_{mass . rod_shape . length} (t) = {body}_{mass . l} \\ {body}_{mass . rod_shape . width} (t) = 2 \cdot {body}_{mass . radius} \\ {body}_{mass . rod_shape . height} (t) = 2 \cdot {body}_{mass . radius} \\ connect (f r a m e_{a}, t r a n s l a t i o n_{+} f r a m e_{a}, t r a n s l a t i o n_{c m_{+} f r a m e_a}) \\ connect (f r a m e_{b}, t r a n s l a t i o n_{+} f r a m e_{b}) \\ connect (t r a n s l a t i o n_{c m_{+} f r a m e_b}, b o d y_{+} f r a m e_{a}) \\ {body}_{mass . translation . phi} (t) = {body}_{mass . translation . frame_a . phi} (t) \\ {body}_{mass . translation . w} (t) = \frac{d \cdot {body}_{mass . translation . phi} (t)}{d t} \\ {body}_{mass . translation . r 0} (t) = {array}_{l i t e r a l} ([\begin{matrix} 2 \\ 2 \end{matrix}], \cos ({body}_{mass . translation . phi} (t)), \sin ({body}_{mass . translation . phi} (t)), - \sin ({body}_{mass . translation . phi} (t)), \cos ({body}_{mass . translation . phi} (t))) \cdot {body}_{mass . translation . r} \\ {body}_{mass . translation . r 0} (t) = {array}_{l i t e r a l} ([\begin{matrix} 2 \end{matrix}], - {body}_{mass . translation . frame_a . x} (t) + {body}_{mass . translation . frame_b . x} (t), {body}_{mass . translation . frame_b . y} (t) - {body}_{mass . translation . frame_a . y} (t)) \\ {body}_{mass . translation . frame_a . phi} (t) = {body}_{mass . translation . frame_b . phi} (t) \\ {body}_{mass . translation . frame_b . fx} (t) + {body}_{mass . translation . frame_a . fx} (t) = 0 \\ {body}_{mass . translation . frame_b . fy} (t) + {body}_{mass . translation . frame_a . fy} (t) = 0 \\ {body}_{mass . translation . frame_b . tau} (t) + {body}_{mass . translation . frame_a . tau} (t) - {body}_{mass . translation . frame_b . fx} (t) \cdot {body}_{mass . translation . r 0}_2 (t) + {body}_{mass . translation . r 0}_1 (t) \cdot {body}_{mass . translation . frame_b . fy} (t) = 0 \\ {body}_{mass . translation . shape . r} (t) = {array}_{l i t e r a l} ([\begin{matrix} 3 \end{matrix}], {body}_{mass . translation . frame_a . x} (t), {body}_{mass . translation . frame_a . y} (t), {body}_{mass . translation . z_position}) \\ {body}_{mass . translation . shape . R} (t) = {array}_{l i t e r a l} ([\begin{matrix} 3 \\ 3 \end{matrix}], 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0) \\ {body}_{mass . translation . shape . r_shape} (t) = [\begin{matrix} 0 \\ 0 \\ 0 \end{matrix}] \\ {body}_{mass . translation . shape . length_direction} (t) = {array}_{l i t e r a l} ([\begin{matrix} 3 \end{matrix}], \frac{{body}_{mass . translation . r 0}_1 (t)}{{body}_{mass . translation . l}}, \frac{{body}_{mass . translation . r 0}_2 (t)}{{body}_{mass . translation . l}}, 0) \\ {body}_{mass . translation . shape . width_direction} (t) = [\begin{matrix} 0 \\ 0 \\ 1 \end{matrix}] \\ {body}_{mass . translation . shape . length} (t) = {body}_{mass . translation . l} \\ {body}_{mass . translation . shape . width} (t) = 2 \cdot {body}_{mass . translation . radius} \\ {body}_{mass . translation . shape . height} (t) = 2 \cdot {body}_{mass . translation . radius} \\ {body}_{mass . translation_cm . phi} (t) = {body}_{mass . translation_cm . frame_a . phi} (t) \\ {body}_{mass . translation_cm . w} (t) = \frac{d \cdot {body}_{mass . translation_cm . phi} (t)}{d t} \\ {body}_{mass . translation_cm . r 0} (t) = {array}_{l i t e r a l} ([\begin{matrix} 2 \\ 2 \end{matrix}], \cos ({body}_{mass . translation_cm . phi} (t)), \sin ({body}_{mass . translation_cm . phi} (t)), - \sin ({body}_{mass . translation_cm . phi} (t)), \cos ({body}_{mass . translation_cm . phi} (t))) \cdot {body}_{mass . translation_cm . r} \\ {body}_{mass . translation_cm . r 0} (t) = {array}_{l i t e r a l} ([\begin{matrix} 2 \end{matrix}], - {body}_{mass . translation_cm . frame_a . x} (t) + {body}_{mass . translation_cm . frame_b . x} (t), {body}_{mass . translation_cm . frame_b . y} (t) - {body}_{mass . translation_cm . frame_a . y} (t)) \\ {body}_{mass . translation_cm . frame_a . phi} (t) = {body}_{mass . translation_cm . frame_b . phi} (t) \\ {body}_{mass . translation_cm . frame_b . fx} (t) + {body}_{mass . translation_cm . frame_a . fx} (t) = 0 \\ {body}_{mass . translation_cm . frame_b . fy} (t) + {body}_{mass . translation_cm . frame_a . fy} (t) = 0 \\ {body}_{mass . translation_cm . frame_a . tau} (t) + {body}_{mass . translation_cm . frame_b . tau} (t) - {body}_{mass . translation_cm . frame_b . fx} (t) \cdot {body}_{mass . translation_cm . r 0}_2 (t) + {body}_{mass . translation_cm . frame_b . fy} (t) \cdot {body}_{mass . translation_cm . r 0}_1 (t) = 0 \\ {body}_{mass . translation_cm . shape . r} (t) = {array}_{l i t e r a l} ([\begin{matrix} 3 \end{matrix}], {body}_{mass . translation_cm . frame_a . x} (t), {body}_{mass . translation_cm . frame_a . y} (t), {body}_{mass . translation_cm . z_position}) \\ {body}_{mass . translation_cm . shape . R} (t) = {array}_{l i t e r a l} ([\begin{matrix} 3 \\ 3 \end{matrix}], 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0) \\ {body}_{mass . translation_cm . shape . r_shape} (t) = [\begin{matrix} 0 \\ 0 \\ 0 \end{matrix}] \\ {body}_{mass . translation_cm . shape . length_direction} (t) = {array}_{l i t e r a l} ([\begin{matrix} 3 \end{matrix}], \frac{{body}_{mass . translation_cm . r 0}_1 (t)}{{body}_{mass . translation_cm . l}}, \frac{{body}_{mass . translation_cm . r 0}_2 (t)}{{body}_{mass . translation_cm . l}}, 0) \\ {body}_{mass . translation_cm . shape . width_direction} (t) = [\begin{matrix} 0 \\ 0 \\ 1 \end{matrix}] \\ {body}_{mass . translation_cm . shape . length} (t) = {body}_{mass . translation_cm . l} \\ {body}_{mass . translation_cm . shape . width} (t) = 2 \cdot {body}_{mass . translation_cm . radius} \\ {body}_{mass . translation_cm . shape . height} (t) = 2 \cdot {body}_{mass . translation_cm . radius} \\ {body}_{mass . body . r} (t) = {array}_{l i t e r a l} ([\begin{matrix} 2 \end{matrix}], {body}_{mass . body . frame_a . x} (t), {body}_{mass . body . frame_a . y} (t)) \\ {body}_{mass . body . v} (t) = \frac{d \cdot {body}_{mass . body . r} (t)}{d t} \\ {body}_{mass . body . phi} (t) = {body}_{mass . body . frame_a . phi} (t) \\ {body}_{mass . body . w} (t) = \frac{d \cdot {body}_{mass . body . phi} (t)}{d t} \\ {body}_{mass . body . a} (t) = \frac{d \cdot {body}_{mass . body . v} (t)}{d t} \\ {body}_{mass . body . alpha} (t) = \frac{d \cdot {body}_{mass . body . w} (t)}{d t} \\ {body}_{mass . body . f} (t) = {array}_{l i t e r a l} ([\begin{matrix} 2 \end{matrix}], {body}_{mass . body . frame_a . fx} (t), {body}_{mass . body . frame_a . fy} (t)) \\ {array}_{l i t e r a l} ([\begin{matrix} 2 \end{matrix}], {body}_{mass . body . m} \cdot g \cdot {n_{2 d}}_{1}, {body}_{mass . body . m} \cdot g \cdot {n_{2 d}}_{2}) + {body}_{mass . body . f} (t) = {body}_{mass . body . m} \cdot {body}_{mass . body . a} (t) \\ {body}_{mass . body . I} \cdot {body}_{mass . body . alpha} (t) = {body}_{mass . body . frame_a . tau} (t) \\ {body}_{mass . body . shape . r} (t) = {array}_{l i t e r a l} ([\begin{matrix} 3 \end{matrix}], {body}_{mass . body . frame_a . x} (t), {body}_{mass . body . frame_a . y} (t), {body}_{mass . body . z_position}) \\ {body}_{mass . body . shape . R} (t) = {array}_{l i t e r a l} ([\begin{matrix} 3 \\ 3 \end{matrix}], \cos (- {body}_{mass . body . phi} (t)), \sin (- {body}_{mass . body . phi} (t)), 0, - \sin (- {body}_{mass . body . phi} (t)), \cos (- {body}_{mass . body . phi} (t)), 0, 0, 0, 1) \\ {body}_{mass . body . shape . r_shape} (t) = [\begin{matrix} 0 \\ 0 \\ 0 \end{matrix}] \\ {body}_{mass . body . shape . length_direction} (t) = [\begin{matrix} 0 \\ 0 \\ 1 \end{matrix}] \\ {body}_{mass . body . shape . width_direction} (t) = [\begin{matrix} 1 \\ 0 \\ 0 \end{matrix}] \\ {body}_{mass . body . shape . length} (t) = 2 \cdot {body}_{mass . body . radius} \\ {body}_{mass . body . shape . width} (t) = 2 \cdot {body}_{mass . body . radius} \\ {body}_{mass . body . shape . height} (t) = 2 \cdot {body}_{mass . body . radius} \end{matrix}]

Source

dyad

"""
This example models a wheeled balancing robot with a cascade control system stabilizing the angle in the inner loop and the position in the outer loop.

See also DyadBalans2DFF for a version with a feedforward generator.
"""
example component DyadBalans2D
  world = MultibodyComponents.PlanarMechanics.World(g = 9.82) {
    "Dyad": {
      "placement": {
        "diagram": {"iconName": "default", "x1": 110, "y1": 420, "x2": 10, "y2": 320, "rot": 180}
      },
      "tags": []
    }
  }
  firstorder = BlockComponents.Continuous.FirstOrder(T = 0.1) {
    "Dyad": {
      "placement": {
        "diagram": {"iconName": "default", "x1": -570, "y1": 600, "x2": -470, "y2": 700, "rot": 0}
      },
      "tags": []
    }
  }
  firstorder1 = BlockComponents.Continuous.FirstOrder(T = 0.1) {
    "Dyad": {
      "placement": {
        "diagram": {"iconName": "default", "x1": -430, "y1": 600, "x2": -330, "y2": 700, "rot": 0}
      },
      "tags": []
    }
  }
  wheelJoint = MultibodyComponents.PlanarMechanics.OneDOFRollingWheelJoint(radius = R) {
    "Dyad": {
      "placement": {
        "diagram": {"iconName": "default", "x1": 200, "y1": 700, "x2": 460, "y2": 960, "rot": 0}
      },
      "tags": []
    }
  }
  constant2 = BlockComponents.Sources.Constant(k = 0) {
    "Dyad": {
      "placement": {
        "diagram": {"iconName": "default", "x1": 50, "y1": 500, "x2": 110, "y2": 560, "rot": 90}
      },
      "tags": []
    }
  }
  IMU = MultibodyComponents.PlanarMechanics.AbsolutePosition(resolve_in_frame = MultibodyComponents.ResolveInFrame.World()) {
    "Dyad": {
      "placement": {
        "diagram": {"iconName": "default", "x1": 460, "y1": 560, "x2": 560, "y2": 660, "rot": 0}
      },
      "tags": []
    }
  }
  square = BlockComponents.Sources.Square(amplitude = 0.05, frequency = 1 / 10) {
    "Dyad": {
      "placement": {
        "diagram": {"iconName": "default", "x1": -720, "y1": 600, "x2": -620, "y2": 700, "rot": 0}
      },
      "tags": []
    }
  }
  wheelinertia = MultibodyComponents.PlanarMechanics.Body(I = Iw, radius = 0.01, m = m) {
    "Dyad": {
      "placement": {
        "diagram": {"iconName": "default", "x1": 390, "y1": 680, "x2": 530, "y2": 820, "rot": 0}
      },
      "tags": []
    }
  }
  simplemotor = MultibodyComponents.PlanarMechanics.examples.dyad_balans.SimpleMotor(d = d) {
    "Dyad": {
      "placement": {
        "diagram": {"iconName": "default", "x1": 280, "y1": 620, "x2": 380, "y2": 720, "rot": 90}
      },
      "tags": []
    }
  }
  constant1 = BlockComponents.Sources.Constant(k = 0) {
    "Dyad": {
      "placement": {
        "diagram": {"iconName": "default", "x1": -240, "y1": 500, "x2": -180, "y2": 560, "rot": 90}
      },
      "tags": []
    }
  }
  angle_controller = BlockComponents.Continuous.LimPID(k = 0.487401, Ti = 0.0587352, Td = 0.0420526, Nd = 119.368, y_max = 0.1) {
    "Dyad": {
      "placement": {
        "diagram": {"iconName": "default", "x1": 30, "y1": 620, "x2": 130, "y2": 720, "rot": 0}
      },
      "tags": []
    }
  }
  pos_controller = BlockComponents.Continuous.LimPID(k = 0.0666576, Ti = 5.25024, Td = 4.81393, Nd = 4.76616, wd = 1, wp = 1, y_max = deg2rad(25.0)) {
    "Dyad": {
      "placement": {
        "diagram": {"iconName": "default", "x1": -260, "y1": 620, "x2": -160, "y2": 720, "rot": 0}
      },
      "tags": []
    }
  }
  gain = BlockComponents.Math.Gain(k = -1) {
    "Dyad": {
      "placement": {
        "diagram": {"iconName": "default", "x1": 170, "y1": 630, "x2": 250, "y2": 710, "rot": 0}
      },
      "tags": []
    }
  }
  gain1 = BlockComponents.Math.Gain(k = -1) {
    "Dyad": {
      "placement": {
        "diagram": {"iconName": "default", "x1": -120, "y1": 630, "x2": -40, "y2": 710, "rot": 0}
      },
      "tags": []
    }
  }
  body_mass = MultibodyComponents.PlanarMechanics.BodyShape(color = [0.2, 0.2, 0.2, 0.9], r = [0, 2 * L], r_cm = [0, L], m = M, I = Ib, radius = 0.03) {
    "Dyad": {
      "placement": {
        "diagram": {"iconName": "default", "x1": 180, "y1": 250, "x2": 480, "y2": 550, "rot": 270}
      },
      "tags": []
    }
  }
  "Body mass"
  parameter M::Mass = 0.1
  "Wheel mass"
  parameter m::Mass = 0.05
  "Wheel radius"
  parameter R::Length = 0.04
  "Distance from wheel axis to body center of mass"
  parameter L::Length = 0.08
  "Body moment of inertia"
  parameter Ib::Inertia = 2.5e-4
  "Wheel + motor moment of inertia"
  parameter Iw::Inertia = 1e-4
  "Motor viscous friction"
  parameter d::RotationalDampingConstant = 0.0001
relations
  initial body_mass.body.phi = 0.1
  initial body_mass.body.w = 0.0
  initial wheelinertia.phi = 0.0
  initial wheelinertia.w = 0.0
  #initial wheelJoint.x = 0.0
  #initial wheelJoint.v = 0.0
  initial firstorder.x = 0.0
  initial firstorder1.x = 0.0
  connect(square.y, firstorder.u) {"Dyad": {"edges": [{"S": 1, "M": [], "E": 2}], "renderStyle": "standard"}}
  connect(firstorder.y, firstorder1.u) {"Dyad": {"edges": [{"S": 1, "M": [], "E": 2}], "renderStyle": "standard"}}
  connect(wheelinertia.frame_a, wheelJoint.frame_a) {
    "Dyad": {
      "edges": [{"S": 1, "M": [{"x": 330, "y": 750}], "E": 2}],
      "renderStyle": "standard"
    }
  }
  connect(simplemotor.frame_b, wheelJoint.frame_a) {"Dyad": {"renderStyle": "standard", "edges": [{"S": 1, "E": 2, "M": []}]}}
  connect(simplemotor.frame_a, IMU.frame_a, body_mass.frame_a) {
    "Dyad": {
      "edges": [
        {"S": 1, "M": [], "E": -1},
        {"S": -1, "M": [{"x": 330, "y": 610}], "E": 2},
        {"S": 3, "M": [], "E": -1}
      ],
      "junctions": [{"x": 330, "y": 610}],
      "renderStyle": "standard"
    }
  }
  y: analysis_point(IMU.phi, angle_controller.u_m)
  y2: analysis_point(IMU.x, pos_controller.u_m)
  u: analysis_point(angle_controller.y, gain.u)
  u2: analysis_point(pos_controller.y, gain1.u)
  connect(IMU.phi, angle_controller.u_m) {
    "Dyad": {
      "edges": [
        {
          "S": 1,
          "M": [
            {"x": 660, "y": 641},
            {"x": 660, "y": 1040},
            {"x": 0, "y": 1040},
            {"x": 0, "y": 693}
          ],
          "E": 2
        }
      ],
      "renderStyle": "standard"
    }
  }
  connect(IMU.x, pos_controller.u_m) {
    "Dyad": {
      "edges": [
        {
          "S": 1,
          "M": [
            {"x": 710, "y": 580},
            {"x": 710, "y": 1090},
            {"x": -300, "y": 1090},
            {"x": -300, "y": 693}
          ],
          "E": 2
        }
      ],
      "renderStyle": "standard"
    }
  }
  connect(pos_controller.y, gain1.u) {
    "Dyad": {
      "edges": [{"S": 1, "M": [{"x": -134.25, "y": 671}, {"x": -134.25, "y": 670}], "E": 2}],
      "renderStyle": "standard"
    }
  }
  connect(gain1.y, angle_controller.u_s) {
    "Dyad": {
      "edges": [{"S": 1, "M": [{"x": -10, "y": 670}, {"x": -10, "y": 647}], "E": 2}],
      "renderStyle": "standard"
    }
  }
  connect(firstorder1.y, pos_controller.u_s) {
    "Dyad": {
      "edges": [{"S": 1, "M": [{"x": -295, "y": 650}, {"x": -295, "y": 647}], "E": 2}],
      "renderStyle": "standard"
    }
  }
  connect(gain.y, simplemotor.torqueinput) {"Dyad": {"edges": [{"S": 1, "M": [], "E": 2}], "renderStyle": "standard"}}
  connect(angle_controller.y, gain.u) {
    "Dyad": {
      "renderStyle": "standard",
      "edges": [{"S": 1, "M": [{"x": 150.5, "y": 671}, {"x": 150.5, "y": 670}], "E": 2}]
    }
  }
  connect(constant1.y, pos_controller.u_ff) {"Dyad": {"edges": [{"S": 1, "M": [], "E": 2}], "renderStyle": "standard"}}
  connect(constant2.y, angle_controller.u_ff) {"Dyad": {"edges": [{"S": 1, "M": [], "E": 2}], "renderStyle": "standard"}}
metadata {"Dyad": {"tests": {"case1": {"stop": 50}}}}
end

Flattened Source

dyad

"""
This example models a wheeled balancing robot with a cascade control system stabilizing the angle in the inner loop and the position in the outer loop.

See also DyadBalans2DFF for a version with a feedforward generator.
"""
example component DyadBalans2D
  world = MultibodyComponents.PlanarMechanics.World(g = 9.82) {
    "Dyad": {
      "placement": {
        "diagram": {"iconName": "default", "x1": 110, "y1": 420, "x2": 10, "y2": 320, "rot": 180}
      },
      "tags": []
    }
  }
  firstorder = BlockComponents.Continuous.FirstOrder(T = 0.1) {
    "Dyad": {
      "placement": {
        "diagram": {"iconName": "default", "x1": -570, "y1": 600, "x2": -470, "y2": 700, "rot": 0}
      },
      "tags": []
    }
  }
  firstorder1 = BlockComponents.Continuous.FirstOrder(T = 0.1) {
    "Dyad": {
      "placement": {
        "diagram": {"iconName": "default", "x1": -430, "y1": 600, "x2": -330, "y2": 700, "rot": 0}
      },
      "tags": []
    }
  }
  wheelJoint = MultibodyComponents.PlanarMechanics.OneDOFRollingWheelJoint(radius = R) {
    "Dyad": {
      "placement": {
        "diagram": {"iconName": "default", "x1": 200, "y1": 700, "x2": 460, "y2": 960, "rot": 0}
      },
      "tags": []
    }
  }
  constant2 = BlockComponents.Sources.Constant(k = 0) {
    "Dyad": {
      "placement": {
        "diagram": {"iconName": "default", "x1": 50, "y1": 500, "x2": 110, "y2": 560, "rot": 90}
      },
      "tags": []
    }
  }
  IMU = MultibodyComponents.PlanarMechanics.AbsolutePosition(resolve_in_frame = MultibodyComponents.ResolveInFrame.World()) {
    "Dyad": {
      "placement": {
        "diagram": {"iconName": "default", "x1": 460, "y1": 560, "x2": 560, "y2": 660, "rot": 0}
      },
      "tags": []
    }
  }
  square = BlockComponents.Sources.Square(amplitude = 0.05, frequency = 1 / 10) {
    "Dyad": {
      "placement": {
        "diagram": {"iconName": "default", "x1": -720, "y1": 600, "x2": -620, "y2": 700, "rot": 0}
      },
      "tags": []
    }
  }
  wheelinertia = MultibodyComponents.PlanarMechanics.Body(I = Iw, radius = 0.01, m = m) {
    "Dyad": {
      "placement": {
        "diagram": {"iconName": "default", "x1": 390, "y1": 680, "x2": 530, "y2": 820, "rot": 0}
      },
      "tags": []
    }
  }
  simplemotor = MultibodyComponents.PlanarMechanics.examples.dyad_balans.SimpleMotor(d = d) {
    "Dyad": {
      "placement": {
        "diagram": {"iconName": "default", "x1": 280, "y1": 620, "x2": 380, "y2": 720, "rot": 90}
      },
      "tags": []
    }
  }
  constant1 = BlockComponents.Sources.Constant(k = 0) {
    "Dyad": {
      "placement": {
        "diagram": {"iconName": "default", "x1": -240, "y1": 500, "x2": -180, "y2": 560, "rot": 90}
      },
      "tags": []
    }
  }
  angle_controller = BlockComponents.Continuous.LimPID(k = 0.487401, Ti = 0.0587352, Td = 0.0420526, Nd = 119.368, y_max = 0.1) {
    "Dyad": {
      "placement": {
        "diagram": {"iconName": "default", "x1": 30, "y1": 620, "x2": 130, "y2": 720, "rot": 0}
      },
      "tags": []
    }
  }
  pos_controller = BlockComponents.Continuous.LimPID(k = 0.0666576, Ti = 5.25024, Td = 4.81393, Nd = 4.76616, wd = 1, wp = 1, y_max = deg2rad(25.0)) {
    "Dyad": {
      "placement": {
        "diagram": {"iconName": "default", "x1": -260, "y1": 620, "x2": -160, "y2": 720, "rot": 0}
      },
      "tags": []
    }
  }
  gain = BlockComponents.Math.Gain(k = -1) {
    "Dyad": {
      "placement": {
        "diagram": {"iconName": "default", "x1": 170, "y1": 630, "x2": 250, "y2": 710, "rot": 0}
      },
      "tags": []
    }
  }
  gain1 = BlockComponents.Math.Gain(k = -1) {
    "Dyad": {
      "placement": {
        "diagram": {"iconName": "default", "x1": -120, "y1": 630, "x2": -40, "y2": 710, "rot": 0}
      },
      "tags": []
    }
  }
  body_mass = MultibodyComponents.PlanarMechanics.BodyShape(color = [0.2, 0.2, 0.2, 0.9], r = [0, 2 * L], r_cm = [0, L], m = M, I = Ib, radius = 0.03) {
    "Dyad": {
      "placement": {
        "diagram": {"iconName": "default", "x1": 180, "y1": 250, "x2": 480, "y2": 550, "rot": 270}
      },
      "tags": []
    }
  }
  "Body mass"
  parameter M::Mass = 0.1
  "Wheel mass"
  parameter m::Mass = 0.05
  "Wheel radius"
  parameter R::Length = 0.04
  "Distance from wheel axis to body center of mass"
  parameter L::Length = 0.08
  "Body moment of inertia"
  parameter Ib::Inertia = 2.5e-4
  "Wheel + motor moment of inertia"
  parameter Iw::Inertia = 1e-4
  "Motor viscous friction"
  parameter d::RotationalDampingConstant = 0.0001
relations
  initial body_mass.body.phi = 0.1
  initial body_mass.body.w = 0.0
  initial wheelinertia.phi = 0.0
  initial wheelinertia.w = 0.0
  #initial wheelJoint.x = 0.0
  #initial wheelJoint.v = 0.0
  initial firstorder.x = 0.0
  initial firstorder1.x = 0.0
  connect(square.y, firstorder.u) {"Dyad": {"edges": [{"S": 1, "M": [], "E": 2}], "renderStyle": "standard"}}
  connect(firstorder.y, firstorder1.u) {"Dyad": {"edges": [{"S": 1, "M": [], "E": 2}], "renderStyle": "standard"}}
  connect(wheelinertia.frame_a, wheelJoint.frame_a) {
    "Dyad": {
      "edges": [{"S": 1, "M": [{"x": 330, "y": 750}], "E": 2}],
      "renderStyle": "standard"
    }
  }
  connect(simplemotor.frame_b, wheelJoint.frame_a) {"Dyad": {"renderStyle": "standard", "edges": [{"S": 1, "E": 2, "M": []}]}}
  connect(simplemotor.frame_a, IMU.frame_a, body_mass.frame_a) {
    "Dyad": {
      "edges": [
        {"S": 1, "M": [], "E": -1},
        {"S": -1, "M": [{"x": 330, "y": 610}], "E": 2},
        {"S": 3, "M": [], "E": -1}
      ],
      "junctions": [{"x": 330, "y": 610}],
      "renderStyle": "standard"
    }
  }
  y: analysis_point(IMU.phi, angle_controller.u_m)
  y2: analysis_point(IMU.x, pos_controller.u_m)
  u: analysis_point(angle_controller.y, gain.u)
  u2: analysis_point(pos_controller.y, gain1.u)
  connect(IMU.phi, angle_controller.u_m) {
    "Dyad": {
      "edges": [
        {
          "S": 1,
          "M": [
            {"x": 660, "y": 641},
            {"x": 660, "y": 1040},
            {"x": 0, "y": 1040},
            {"x": 0, "y": 693}
          ],
          "E": 2
        }
      ],
      "renderStyle": "standard"
    }
  }
  connect(IMU.x, pos_controller.u_m) {
    "Dyad": {
      "edges": [
        {
          "S": 1,
          "M": [
            {"x": 710, "y": 580},
            {"x": 710, "y": 1090},
            {"x": -300, "y": 1090},
            {"x": -300, "y": 693}
          ],
          "E": 2
        }
      ],
      "renderStyle": "standard"
    }
  }
  connect(pos_controller.y, gain1.u) {
    "Dyad": {
      "edges": [{"S": 1, "M": [{"x": -134.25, "y": 671}, {"x": -134.25, "y": 670}], "E": 2}],
      "renderStyle": "standard"
    }
  }
  connect(gain1.y, angle_controller.u_s) {
    "Dyad": {
      "edges": [{"S": 1, "M": [{"x": -10, "y": 670}, {"x": -10, "y": 647}], "E": 2}],
      "renderStyle": "standard"
    }
  }
  connect(firstorder1.y, pos_controller.u_s) {
    "Dyad": {
      "edges": [{"S": 1, "M": [{"x": -295, "y": 650}, {"x": -295, "y": 647}], "E": 2}],
      "renderStyle": "standard"
    }
  }
  connect(gain.y, simplemotor.torqueinput) {"Dyad": {"edges": [{"S": 1, "M": [], "E": 2}], "renderStyle": "standard"}}
  connect(angle_controller.y, gain.u) {
    "Dyad": {
      "renderStyle": "standard",
      "edges": [{"S": 1, "M": [{"x": 150.5, "y": 671}, {"x": 150.5, "y": 670}], "E": 2}]
    }
  }
  connect(constant1.y, pos_controller.u_ff) {"Dyad": {"edges": [{"S": 1, "M": [], "E": 2}], "renderStyle": "standard"}}
  connect(constant2.y, angle_controller.u_ff) {"Dyad": {"edges": [{"S": 1, "M": [], "E": 2}], "renderStyle": "standard"}}
metadata {"Dyad": {"tests": {"case1": {"stop": 50}}}}
end

Test Cases

Test Case `case1`

Examples
Experiments
Analyses

Scalar Types

Partial Types

Enumerated Types

Function Types

Partial Types

Enumerated Types

Partial Types

Partial Types

Partial Types

Scalar Types

Partial Types

Partial Types

Partial Types

Enumerated Types

PlanarMechanics.examples.dyad_balans.DyadBalans2D ​

Usage ​

Parameters: ​

Behavior ​

Source ​

Test Cases ​

Test Case case1 ​

Related ​

`PlanarMechanics.examples.dyad_balans.DyadBalans2D`

Usage

Parameters:

Behavior

Source

Test Cases

Test Case `case1`

Related