Add passive joint moments

.gitignore

@@ -78,3 +78,4 @@ docs/_build/
 *.i*86
 *.x86_64
 *.hex
+

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

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)
 
 # %%

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

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."""