Con force i constraints

bicycledataprocessor/database.py

@@ -94,16 +94,30 @@ def __init__(self, **kwargs):
                               'FrameAccelerationY',
                               'FrameAccelerationZ',
                               'PitchRate',
+							  'PitchAcc'
                               'PullForce',
                               'RearWheelRate',
+							  'RearWheelAcc'
+                              'FrontWheelRate'
+							  'FrontWheelAcc'
+                              'FrontWheelYawAngle'
+                              'FrontWheelSteerAngle'
+							  'LongRearConForce_Nonslip'
+							  'LatRearConForce_Nonslip'
+							  'LongFrontConForce_Nonslip'
+							  'LatFrontConForce_Nonslip'
                               'RollAngle',
                               'RollRate',
+							  'RollAcc'
                               'ForwardSpeed',
                               'SteerAngle',
                               'SteerRate',
+							  'SteerAcc'
                               'SteerTorque',
                               'tau', # why tau?
-                              'YawRate']
+							  'YawAngle'
+                              'YawRate'
+							  'YawAcc']
 
     def _task_table_class(self):
         """Creates a class that is used to describe the table containing meta

bicycledataprocessor/main.py

@@ -17,14 +17,16 @@
 
 # dependencies
 import numpy as np
+from numpy import sin, cos
 from scipy import io
 from scipy.integrate import cumtrapz
 from scipy.optimize import curve_fit
 import matplotlib.pyplot as plt
 from tables import NoSuchNodeError
 
 import dtk.process as process
-from dtk.bicycle import front_contact, benchmark_to_moore
+import dtk.bicycle as bi
+
 import bicycleparameters as bp
 
 # local dependencies
@@ -735,6 +737,10 @@ def task_signals(self):
         self.compute_rear_wheel_contact_rates()
         self.compute_rear_wheel_contact_points()
         self.compute_front_wheel_contact_points()
+        self.compute_front_wheel_rate()
+        self.compute_front_wheel_steer_yaw_angle()
+        self.compute_signals_derivatives()
+        self.compute_contact_force_nonslip()
 
         self.topSig = 'task'
 
@@ -762,6 +768,7 @@ def compute_signals(self):
         self.compute_steer_rate()
         self.compute_yaw_roll_pitch_rates()
         self.compute_steer_torque()
+        self.compute_rear_wheel_rate()
 
     def truncate_signals(self):
         """Truncates the calibrated signals based on the time shift."""
@@ -803,6 +810,146 @@ def check_time_shift(self, maxNRMS):
                 'time shift is {}, which is greater '.format(str(nrms)) +
                 'than the maximum allowed: {}'.format(str(maxNRMS))))
 
+    def compute_rear_wheel_rate(self):
+        """
+        Computes computedSignals of the rear wheel rate from truncatedSignals.
+        """
+        try:
+            rearWheelRate = self.truncatedSignals['RearWheelRate']
+        except AttributeError:
+            print('RearWheelRate is not availabe.')
+        else:
+            rearWheelRate = rearWheelRate.convert_units('radian/second')
+
+            rearWheelRate.name = 'RearWheelRate'
+            rearWheelRate.units = 'radian/second'
+            self.computedSignals[rearWheelRate.name] = rearWheelRate
+
+    def compute_front_wheel_rate(self):
+        """Calculates the front wheel rate based on the data of rear_wheel_rate.
+        """
+
+        bp = self.bicycleRiderParameters
+        mp = self.bicycleRiderMooreParameters
+
+        q2 = self.taskSignals['RollAngle']
+        q3 = bp['lam']
+        q4 = self.taskSignals['SteerAngle']
+        u1 = self.taskSignals['YawRate']
+        u1 = self.taskSignals['YawRate']
+        u2 = self.taskSignals['RollRate']
+        u3 = self.taskSignals['PitchRate']
+        u5 = self.taskSignals['RearWheelRate']
+
+        f = np.vectorize(bi.front_wheel_rate)
+
+        u6 = f(q2, q3, q4, u1, u2, u3, u5, mp['rr'], mp['rf'], 
+                mp['d1'], mp['d2'], mp['d3'])
+
+        u6.name = 'FrontWheelRate'
+        u6.units = 'radian/second'
+        self.taskSignals['FrontWheelRate'] = u6
+
+    def compute_front_wheel_steer_yaw_angle(self):
+        """Calculates the yaw angle of the front wheel.
+        """
+
+        bp = self.bicycleRiderParameters
+
+        q1 = self.taskSignals['YawAngle']
+        q2 = self.taskSignals['RollAngle']
+        q3 = bp['lam']
+        q4 = self.taskSignals['SteerAngle']
+
+        f = np.vectorize(bi.front_wheel_steer_yaw_angle)
+
+        frontWheel_SteerAngle, frontWheel_YawAngle = f(q1, q2, q3, q4)
+
+        frontWheel_SteerAngle.name = 'FrontWheelSteerAngle'
+        frontWheel_SteerAngle.units = 'radian'
+        self.taskSignals['FrontWheelSteerAngle'] = frontWheel_SteerAngle
+
+        frontWheel_YawAngle.name = 'FrontWheelYawAngle'
+        frontWheel_YawAngle.unitss = 'radian'
+        self.taskSignals['FrontWheelYawAngle'] = frontWheel_YawAngle
+
+    def compute_signals_derivatives(self):
+        """Calculate the acceleration of some signals by time_derivative.
+        """
+
+        try:
+            yawRate = self.taskSignals['YawRate']
+            rollRate = self.taskSignals['RollRate']
+            pitchRate = self.taskSignals['PitchRate']
+            steerRate = self.taskSignals['SteerRate']
+            rearWheelRate = self.taskSignals['RearWheelRate']
+            frontWheelRate = self.taskSignals['FrontWheelRate']
+        except AttributeError:
+            print('YawRate, RollRate, PitchRate, SteerRate, or/and '\
+             'RearWheelRate, FrontWheelRate are not availabe.')
+        else:
+            yawAcc = yawRate.time_derivative()
+            rollAcc = rollRate.time_derivative()
+            pitchAcc = pitchRate.time_derivative()
+            steerAcc = steerRate.time_derivative()
+            rearWheelAcc = rearWheelRate.time_derivative()
+            frontWheelAcc = frontWheelRate.time_derivative()
+
+            yawAcc.name = 'YawAcc'
+            self.taskSignals[yawAcc.name] = yawAcc
+            rollAcc.name = 'RollAcc'
+            self.taskSignals[rollAcc.name] = rollAcc
+            pitchAcc.name = 'PitchAcc'
+            self.taskSignals[pitchAcc.name] = pitchAcc
+            steerAcc.name = 'SteerAcc'
+            self.taskSignals[steerAcc.name] = steerAcc
+            rearWheelAcc.name = 'RearWheelAcc'
+            self.taskSignals[rearWheelAcc.name] = rearWheelAcc
+            frontWheelAcc.name = 'FrontWheelAcc'
+            self.taskSignals[frontWheelAcc.name] = frontWheelAcc
+
+    def compute_contact_force_nonslip(self):
+        """Calculate the contact force of each wheel under the constraint 
+        condition. Here, the forces are expressed by body-fixed coordinates."""
+
+        bp = self.bicycleRiderParameters
+
+        f0 = np.vectorize(bi.contact_force_rear_longitudinal_N1_nonslip)
+        Fx_r_n = f0(bp['lam'], self.bicycleRiderMooreParameters, self.taskSignals)
+
+        f1 = np.vectorize(bi.contact_force_rear_lateral_N2_nonslip)
+        Fy_r_n = f1(bp['lam'], self.bicycleRiderMooreParameters, self.taskSignals)
+
+        f2 = np.vectorize(bi.contact_force_front_longitudinal_N1_nonslip)
+        Fx_f_n = f2(bp['lam'], self.bicycleRiderMooreParameters, self.taskSignals)
+
+        f3 = np.vectorize(bi.contact_force_front_lateral_N2_nonslip)
+        Fy_f_n = f3(bp['lam'], self.bicycleRiderMooreParameters, self.taskSignals)
+
+        yawAngle = self.taskSignals['YawAngle']
+        frontWheelYawAngle = self.taskSignals['FrontWheelYawAngle']
+
+        Fx_r_ns = cos(yawAngle) * Fx_r_n + sin(yawAngle) * Fy_r_n
+        Fy_r_ns = -sin(yawAngle) * Fx_r_n + cos(yawAngle) * Fy_r_n 
+        Fx_f_ns = cos(frontWheelYawAngle) * Fx_f_n + sin(frontWheelYawAngle) * Fy_f_n
+        Fy_f_ns = -sin(frontWheelYawAngle) * Fx_f_n + cos(frontWheelYawAngle) * Fy_f_n
+
+        Fx_r_ns.name = 'LongRearConForce_Nonslip'
+        Fx_r_ns.units = 'newton'
+        self.taskSignals[Fx_r_ns.name] = Fx_r_ns
+
+        Fy_r_ns.name = 'LatRearConForce_Nonslip'
+        Fy_r_ns.units = 'newton'
+        self.taskSignals[Fy_r_ns.name] = Fy_r_ns
+
+        Fx_f_ns.name = 'LongFrontConForce_Nonslip'
+        Fx_f_ns.units = 'newton'
+        self.taskSignals[Fx_f_ns.name] = Fx_f_ns
+
+        Fy_f_ns.name = 'LatFrontConForce_Nonslip'
+        Fy_f_ns.units = 'newton'
+        self.taskSignals[Fy_f_ns.name] = Fy_f_ns
+
     def compute_rear_wheel_contact_points(self):
         """Computes the location of the wheel contact points in the ground
         plane."""
@@ -839,9 +986,9 @@ def compute_front_wheel_contact_points(self):
         q4 = self.taskSignals['RollAngle']
         q7 = self.taskSignals['SteerAngle']
 
-        p = benchmark_to_moore(self.bicycleRiderParameters)
+        p = self.bicycleRiderMooreParameters
 
-        f = np.vectorize(front_contact)
+        f = np.vectorize(bi.front_contact)
         q9, q10 = f(q1, q2, q3, q4, q7, p['d1'], p['d2'], p['d3'], p['rr'],
             p['rf'])
 
@@ -1214,6 +1361,9 @@ def load_rider(self, pathToParameterData):
         self.bicycleRiderParameters =\
             bp.io.remove_uncertainties(self.bicycle.parameters['Benchmark'])
 
+        self.bicycleRiderMooreParameters =\
+            bi.benchmark_to_moore(self.bicycleRiderParameters)
+
     def plot(self, *args, **kwargs):
         '''
         Returns a plot of the time series of various signals.

defaults.cfg

@@ -1,5 +1,5 @@
 [data]
-base = /media/Data/Documents/School/UC Davis/Bicycle Mechanics
+base = /home/stefenyin/bicycle/bi_pypa
 pathToDatabase = %(base)s/BicycleDataProcessor/InstrumentedBicycleData.h5
 pathToCorruption  = %(base)s/BicycleDataProcessor/data-corruption.csv
 pathToRunMat = %(base)s/BicycleDAQ/data