Add passive joint moments (#32)

* First draft of passive moments in gait2d (not working yet)

* symbols qmin, qmax now added to segment constants

* example_constants.yml: qmin and qmax specified in radians

* example_constants.yml: corrected sign error in qminc

But this is not the reason it failed. qminc would only be used when k2 is not zero, and the simulation fails when k2 is zero and k1 is not zero.

---------

Co-authored-by: Jason K. Moore <moorepants@gmail.com>

Author: tvdbogert

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