Commits

Anonymous committed 123c9bc

Fixed bug in units conversion for kbais controller.

Comments (0)

Files changed (2)

   float deriv_ctlr_err;
   float max_u;
 
-
   if (config.ctlr_param.type == CTLR_TYPE_KBIAS) {
-    *u = RAD2DEG*setpt - config.ctlr_param.pgain*state_est.vel;
+    *u = DEG2RAD*setpt - config.ctlr_param.pgain*state_est.vel;
   }
   else {
     // Get controller error based on controller type

python/libyawff/yawff_utils.py

         torq_flt = torq[:,0]
         torq_raw = torq[:,1]
 
+        # Reshape t, pos, vel to be (N,) instead of (N,1) 
+        t = scipy.reshape(t,(t.shape[0],))
+        pos = scipy.reshape(pos,(pos.shape[0],))
+        vel = scipy.reshape(vel,(vel.shape[0],))
+
         # Create kinematics dictionary
         kine_dict = {}
         for i in range(0,config['num_motor']):
         pos_last =  pos
     t = scipy.arange(0,yaw_kine.shape[0])*dt
     return t, yaw_kine, pos_array
-    
+
 def resample(x,n):
     """
     Resample data in array x with step size n.
     else:
         return x[0:-1:n,:]
 
+def resample_tuple(data_tuple,n):
+    return tuple([resample(x,n) for x in data_tuple])
+
+def resample_dict(data_dict,n):
+    data_dict_new = {}
+    for k,v in data_dict.iteritems():
+        data_dict_new[k] = resample(v,n)
+    return data_dict_new
+
+
 
 # ------------------------------------------------------------------------------
 # Command line utilties
Tip: Filter by directory path e.g. /media app.js to search for public/media/app.js.
Tip: Use camelCasing e.g. ProjME to search for ProjectModifiedEvent.java.
Tip: Filter by extension type e.g. /repo .js to search for all .js files in the /repo directory.
Tip: Separate your search with spaces e.g. /ssh pom.xml to search for src/ssh/pom.xml.
Tip: Use ↑ and ↓ arrow keys to navigate and return to view the file.
Tip: You can also navigate files with Ctrl+j (next) and Ctrl+k (previous) and view the file with Ctrl+o.
Tip: You can also navigate files with Alt+j (next) and Alt+k (previous) and view the file with Alt+o.