diff --git a/.gitignore b/.gitignore index ba0fbce..75ca02a 100644 --- a/.gitignore +++ b/.gitignore @@ -78,3 +78,4 @@ docs/_build/ *.i*86 *.x86_64 *.hex + diff --git a/examples/example_constants.yml b/examples/example_constants.yml index e914e53..c9a13c8 100644 --- a/examples/example_constants.yml +++ b/examples/example_constants.yml @@ -15,12 +15,16 @@ ib: 0.1522 # moment of inertia about mass center wrt to rthigh reference frame [ xb: 0.0 # local x location of mass center wrt to the hip joint [m] yb: -0.191 # local y location of mass center wrt to the hip joint [m] lb: 0.4410 # joint to joint segment length [m] +qminb: -1.5 # angle (rad) below which a quadratic passive torque is generated +qmaxb: 2.8 # angle (rad) above which a quadratic passive torque is generated # rshank: mc: 3.4875 # mass [kg] ic: 0.0624 # moment of inertia about mass center wrt to rshank reference frame [kg*m^2] xc: 0.0 # x location of mass center wrt to the knee joint [m] yc: -0.1917 # y location of mass center wrt to the knee joint [m] lc: 0.4428 # joint to joint segment length [m] +qminc: -2.8 # angle (rad) below which a quadratic passive torque is generated +qmaxc: 0.0 # angle (rad) above which a quadratic passive torque is generated # rfoot md: 1.0875 # mass [kg] id: 0.0184 # moment of inertia about mass center wrt to rfoot reference frame [kg*m^2] @@ -29,18 +33,24 @@ yd: -0.0351 # local y location of mass center wrt to the ankle joint [m] hxd: -0.06 # local x location of heel wrt to the ankle joint [m] txd: 0.15 # local x location of toe wrt to the ankle joint [m] fyd: -0.07 # local y location of heel and toe relative to ankle joint [m] +qmind: -1.5 # angle (rad) below which a quadratic passive torque is generated +qmaxd: 1.5 # angle (rad) above which a quadratic passive torque is generated # lthigh me: 7.5 # mass [kg] ie: 0.1522 # moment of inertia about mass center wrt to the lthigh reference frame[kg*m^2] xe: 0.0 # local x location of mass center wrt to the hip joint [m] ye: -0.191 # local y location of mass center wrt to the hip joint [m] le: 0.4410 # segment length [m] +qmine: -1.5 # angle (rad) below which a quadratic passive torque is generated +qmaxe: 2.8 # angle (rad) above which a quadratic passive torque is generated # lshank mf: 3.4875 # mass [kg] if: 0.0624 # moment of inertia about mass center wrt to the lshank reference frame [kg*m^2] xf: 0.0 # local x location of mass center wrt to the knee joint [m] yf: -0.1917 # local y location of mass center wrt to the knee joint [m] lf: 0.4428 # segment length [m] +qminf: -2.8 # angle (rad) below which a quadratic passive torque is generated +qmaxf: 0.0 # angle (rad) above which a quadratic passive torque is generated # lfoot mg: 1.0875 # mass [kg] ig: 0.0184 # moment of inertia about mass center wrt to the lfoot reference frame [kg*m^2] @@ -49,6 +59,8 @@ yg: -0.0351 # local y location of mass center wrt to the ankle joint [m] hxg: -0.06 # local x location of heel wrt to the ankle joint [m] txg: 0.15 # local x location of toe wrt to the ankle joint [m] fyg: -0.07 # local y location of heel and toe relative to ankle joint [m] +qming: -1.5 # angle (rad) below which a quadratic passive torque is generated +qmaxg: 1.5 # angle (rad) above which a quadratic passive torque is generated # contact kc: 5.0e+7 # ground contact stiffness, N/m^3 cc: 0.85 # ground contact damping, s/m diff --git a/examples/plot_forward_sim.py b/examples/plot_forward_sim.py index 39cbf22..26dd1c7 100644 --- a/examples/plot_forward_sim.py +++ b/examples/plot_forward_sim.py @@ -6,6 +6,7 @@ that the model falls down on the treadmill. It also compares the evaluation speed of PyDy's and Autolev's models. """ +print("loading modules...") import timeit from algait2de.gait2de import evaluate_autolev_rhs @@ -20,11 +21,13 @@ # %% # Derive the equations of motion, including a constant treadmill motion. -symbolics = derive.derive_equations_of_motion(treadmill=True) +print("deriving equations of motion...") +# symbolics = derive.derive_equations_of_motion(treadmill=True) +symbolics = derive.derive_equations_of_motion(treadmill=True, passive_torques=True) # %% # Load a parameter mapping from pygait2d symbol to numerical value, as well as -# a mappig of the symbol string to numerical value. +# a mapping of the symbol string to numerical value. try: par_map = simulate.load_constants(symbolics.constants, 'example_constants.yml') @@ -58,7 +61,7 @@ # %% # Prepare numerical arrays to be passed to the ODE functions. specifieds_vals = np.zeros(len(symbolics.specifieds)) -specifieds_vals[-1] = 1.0 +specifieds_vals[-1] = 1.0 # treadmill speed args = (specifieds_vals, np.array(list(par_map.values()))) @@ -86,6 +89,7 @@ # Simulate the model for two seconds using the LSODA integrator (switches # between stiff and non-stiff modes). time_vector = np.linspace(0.0, 2.0, num=61) +print("simulating...") trajectories = odeint(rhs, initial_conditions, time_vector, args=args) # %% diff --git a/pygait2d/derive.py b/pygait2d/derive.py index 7c4d9b8..b37d1f1 100644 --- a/pygait2d/derive.py +++ b/pygait2d/derive.py @@ -339,6 +339,7 @@ def derive_equations_of_motion( treadmill=False, hand_of_god=True, stiffness_exp=3, + passive_torques=False, ): """Returns the equations of motion for the planar walking model along with all of the constants, coordinates, speeds, joint torques, visualization @@ -368,6 +369,8 @@ def derive_equations_of_motion( the torso and a torque acting on the torso will be included. stiffness_exp : float, optional Exponent of the contact force stiffness force. + passive_torques : boolean, optional + If true, a nonlinear passive torque function is added. Returns ======= @@ -420,10 +423,12 @@ def derive_equations_of_motion( for label in sorted(segment_descriptions.keys()): segment_class, desc, joint_desc = segment_descriptions[label] - + passive_torque = passive_torques # true or false for this particular segment + if label == 'A': # trunk parent_reference_frame = ground origin_joint = origin + passive_torque = False # no passive torque between ground and trunk elif label == 'E': # left thigh # For the left thigh, set the trunk and hip as the # reference_frame and origin joint. @@ -434,7 +439,7 @@ def derive_equations_of_motion( origin_joint = segments[-1].joint segment = segment_class(label, desc, parent_reference_frame, - origin_joint, joint_desc, ground) + origin_joint, joint_desc, ground, passive_torque) segments.append(segment) # constants, coordinates, speeds, kinematic differential equations diff --git a/pygait2d/segment.py b/pygait2d/segment.py index 99c38ab..aa374dd 100644 --- a/pygait2d/segment.py +++ b/pygait2d/segment.py @@ -57,7 +57,7 @@ class BodySegment(object): viz_cylinder_radius = 0.035 # meters def __init__(self, label, description, parent_reference_frame, - origin_joint, joint_description, inertial_frame): + origin_joint, joint_description, inertial_frame, passive_torque=False): """Initializes a body segment.""" self.label = label @@ -66,6 +66,7 @@ def __init__(self, label, description, parent_reference_frame, self.origin_joint = origin_joint self.joint_description = joint_description self.inertial_frame = inertial_frame + self.passive_torque = passive_torque; self._create_symbols() self._kinematic_differential_equations() @@ -112,6 +113,13 @@ def _create_symbols(self): self.mass_center_x_symbol, self.mass_center_y_symbol] + if self.passive_torque: + # we need these constants to define the passive range of motion + self.qmin_symbol = symbols('qmin{}'.format(subscript), **sym_kwargs) + self.qmax_symbol = symbols('qmax{}'.format(subscript), **sym_kwargs) + self.constants.append(self.qmin_symbol) + self.constants.append(self.qmax_symbol) + # functions of time self.generalized_coordinate_symbol = \ time_varying('q{}'.format(subscript)) @@ -183,8 +191,33 @@ def _create_rigid_body(self): def _joint_torque(self): """Creates the joint torque vector acting on the segment.""" - self.torque = self.joint_torque_symbol * self.reference_frame.z - # TODO : add in passive joint stiffness and damping + torque = self.joint_torque_symbol + + # add in passive joint stiffness and damping + if self.passive_torque: + # hard-coded parameters, these are not critical + k1 = 1.0 # linear stiffness (Nm/rad) + k2 = 5000.0 # quadratic stiffness (Nm/rad^2) applied when outside the qmin to qmax range + b = 1.0 # linear damping (Nms/rad) + + # turn the three terms off individually, for testing + # currently, setting k1 and k2 to zero makes it work. + # k1 = 0.0; + # k2 = 0.0; + # b = 0.0; + + # add a weak linear stiffness and damping + torque += -k1 * self.generalized_coordinate_symbol - b * self.generalized_speed_symbol + + # add quadratic stiffness when angle is outside the range qmin to qmax + qmaxdiff = self.generalized_coordinate_symbol - self.qmax_symbol + qmaxpenetration = (Abs(qmaxdiff) + qmaxdiff) / 2 # this is a positive value + qmindiff = self.qmin_symbol - self.generalized_coordinate_symbol + qminpenetration = (Abs(qmindiff) + qmindiff) / 2 # this is a positive value + torque += -k2 * (qmaxpenetration**2 - qminpenetration**2); + + # apply this torque on the Z axis of the segment + self.torque = torque * self.reference_frame.z def _gravity(self): """Creates the gravitational force vector acting on the segment."""