1. iorodeo
  2. water_channel_ros

Commits

iorodeo  committed c8edbd1

Added Kalman filter to laser setpt source. Added ability to set gain on contoller.

  • Participants
  • Parent commits 92e0f5f
  • Branches default

Comments (0)

Files changed (16)

File software/distance_118x/manifest.xml

View file
   <depend package="roscpp"/>
   <depend package="opencv2"/>
   <depend package="msg_and_srv"/>
+  <depend package="filters"/>
 
 </package>
 

File software/distance_118x/nodes/distance_node.py

View file
 from msg_and_srv.srv import DistSensorCtl
 from msg_and_srv.srv import DistSensorCtlResponse
 from distance_sensor_118x import DistanceSensor
-import filters
+from filters import kalman_filter
 
 MM2M = 1.0e-3
 
         self.lock =  threading.Lock()
         self.distMsg = DistMsg()
 
-        self.kalman = filters.KalmanFilter()
+        self.kalman = kalman_filter.KalmanFilter()
 
         # Setup distance sensor
         self.fakeit = rospy.get_param('fake_distance_sensor',False)

File software/distance_118x/nodes/filters.py

-import roslib
-roslib.load_manifest('distance_118x')
-import rospy
-import cv
-
-class KalmanFilter(object):
-
-    def __init__(self):
-        self.kal = cv.CreateKalman(2,1,0)
-        cv.SetIdentity(self.kal.transition_matrix, 1.0)
-        cv.SetIdentity(self.kal.measurement_matrix, 1.0)
-        cv.SetIdentity(self.kal.process_noise_cov, 10.0)
-        self.kal.process_noise_cov[0,0] = 1.0
-        self.kal.process_noise_cov[1,1] = 800.0
-        cv.SetIdentity(self.kal.measurement_noise_cov, 1.0)
-        cv.SetIdentity(self.kal.error_cov_post, 1.0)
-        self.measurement = cv.CreateMat(1,1,cv.GetElemType(self.kal.state_pre))
-        self.t_previous = None
-
-    def update(self,z,t):
-        self.t_current = t
-        x, vx = None, None
-        if self.update_dt():
-            cv.KalmanPredict(self.kal)
-            self.measurement[0,0] = z
-            state_post = cv.KalmanCorrect(self.kal,self.measurement)
-            x = state_post[0,0]
-            vx = state_post[1,0]
-        return (x,vx)
-
-    def update_dt(self):
-        status = False
-        if self.t_previous:
-            self.dt = self.t_current - self.t_previous
-            self.kal.transition_matrix[0,1] = self.dt
-            status = True
-        self.t_previous = self.t_current
-        return status

File software/distance_118x/nodes/filters.py.old

View file
+import roslib
+roslib.load_manifest('distance_118x')
+import rospy
+import cv
+
+class KalmanFilter(object):
+
+    def __init__(self):
+        self.kal = cv.CreateKalman(2,1,0)
+        cv.SetIdentity(self.kal.transition_matrix, 1.0)
+        cv.SetIdentity(self.kal.measurement_matrix, 1.0)
+        cv.SetIdentity(self.kal.process_noise_cov, 10.0)
+        self.kal.process_noise_cov[0,0] = 1.0
+        self.kal.process_noise_cov[1,1] = 800.0
+        cv.SetIdentity(self.kal.measurement_noise_cov, 1.0)
+        cv.SetIdentity(self.kal.error_cov_post, 1.0)
+        self.measurement = cv.CreateMat(1,1,cv.GetElemType(self.kal.state_pre))
+        self.t_previous = None
+
+    def update(self,z,t):
+        self.t_current = t
+        x, vx = None, None
+        if self.update_dt():
+            cv.KalmanPredict(self.kal)
+            self.measurement[0,0] = z
+            state_post = cv.KalmanCorrect(self.kal,self.measurement)
+            x = state_post[0,0]
+            vx = state_post[1,0]
+        return (x,vx)
+
+    def update_dt(self):
+        status = False
+        if self.t_previous:
+            self.dt = self.t_current - self.t_previous
+            self.kal.transition_matrix[0,1] = self.dt
+            status = True
+        self.t_previous = self.t_current
+        return status

File software/filters/manifest.xml

View file
   <url>http://ros.org/wiki/filters</url>
   <depend package="rospy"/>
   <depend package="roscpp"/>
+  <depend package="opencv2"/>
 
 </package>
 

File software/filters/src/filters/__init__.py

View file
+import kalman_filter

File software/filters/src/filters/kalman_filter.py

View file
+import roslib
+roslib.load_manifest('filters')
+import rospy
+import cv
+
+
+class KalmanFilter(object):
+
+    def __init__(self,process_noise_cov=(1.0,800.0)):
+        self.kal = cv.CreateKalman(2,1,0)
+        cv.SetIdentity(self.kal.transition_matrix, 1.0)
+        cv.SetIdentity(self.kal.measurement_matrix, 1.0)
+        cv.SetIdentity(self.kal.process_noise_cov, 1.0)
+        self.kal.process_noise_cov[0,0] = process_noise_cov[0] 
+        self.kal.process_noise_cov[1,1] = process_noise_cov[1] 
+        cv.SetIdentity(self.kal.measurement_noise_cov, 1.0)
+        cv.SetIdentity(self.kal.error_cov_post, 1.0)
+        self.measurement = cv.CreateMat(1,1,cv.GetElemType(self.kal.state_pre))
+        self.t_previous = None
+
+    def update(self,z,t):
+        self.t_current = t
+        x, vx = None, None
+        if self.update_dt():
+            cv.KalmanPredict(self.kal)
+            self.measurement[0,0] = z
+            state_post = cv.KalmanCorrect(self.kal,self.measurement)
+            x = state_post[0,0]
+            vx = state_post[1,0]
+        return (x,vx)
+
+    def update_dt(self):
+        status = False
+        if self.t_previous:
+            self.dt = self.t_current - self.t_previous
+            self.kal.transition_matrix[0,1] = self.dt
+            status = True
+        self.t_previous = self.t_current
+        return status
+
+# -----------------------------------------------------------------------------
+if __name__ == '__main__':
+
+    filt = KalmanFilter()
+    for i in (0,1):
+        for j in (0,1):
+            print filt.kal.process_noise_cov[i,j]

File software/motor_cmd_source/nodes/base_controller.py

View file
 # Services
 from msg_and_srv.srv import NodeEnable
 from msg_and_srv.srv import NodeEnableResponse
+from msg_and_srv.srv import GetControllerGains
+from msg_and_srv.srv import GetControllerGainsResponse
+from msg_and_srv.srv import SetControllerGains
+from msg_and_srv.srv import SetControllerGainsResponse
+
 
 class BaseController(object):
 
         self.rate = rospy.Rate(self.update_rate)
         self.enabled = False
 
-        self.controller = pid_controller.PIDController()
-        self.controller.pgain = rospy.get_param('controller_pgain', 0.5)
-        self.controller.igain = rospy.get_param('controller_igain', 0.0)
-        self.controller.dgain = rospy.get_param('controller_dgain', 0.0)
-        self.controller.outputMax = rospy.get_param('controller_max_output', 4000)
-        self.controller.outputMin = rospy.get_param('controller_min_output', -4000)
-        
-        self.feedForward = velocity_feedforward.VelocityFeedForward()
-        self.feedForward.coeff = rospy.get_param('controller_ffcoeff', 5.6)
-        self.controller.ffFunc = self.feedForward.func
-
-        pgain = self.controller.pgain
-        self.pgainScheduler = GainScheduler(0.1*pgain,pgain,0.075) 
+        self.pgain = rospy.get_param('controller_pgain', 0.5)
+        self.pgainSchedMin = rospy.get_param('controller_pgain_sched_min', 0.1)
+        self.pgainSchedWid = rospy.get_param('controller_pgain_sched_wid', 0.075)
+        self.igain = rospy.get_param('controller_igain', 0.0)
+        self.dgain = rospy.get_param('controller_dgain', 0.0)
+        self.outputMax = rospy.get_param('controller_max_output', 4000)
+        self.outputMin = rospy.get_param('controller_min_output', -4000)
+        self.ffcoeff = rospy.get_param('controller_ffcoeff', 5.6)
+        self.createController()
 
         # Setup subscriber setpt topic 
         self.setptSub = rospy.Subscriber('setpt', SetptMsg, self.setptCallback)
         self.PIDMsg = PIDMsg()
         self.PIDPub = rospy.Publisher('pid_terms', PIDMsg)
 
+        # Set up gain setting service
+        self.setGainsSrv = rospy.Service(
+                'controller_set_gains',
+                SetControllerGains,
+                self.handleSetGains,
+                )
+
+        self.getGainsSrv = rospy.Service(
+                'controller_get_gains',
+                GetControllerGains,
+                self.handleGetGains,
+                )
+
         # Setup enable/disable service 
         self.nodeEnableSrv = rospy.Service(
                 'controller_enable', 
                 NodeEnable, 
-                self.handleNodeEnable
+                self.handleNodeEnable,
                 ) 
 
+    def createController(self):
+        """
+        Creates a new gain scheduling controller based on the current
+        gain settings.
+        """
+        self.feedForward = velocity_feedforward.VelocityFeedForward()
+        self.feedForward.coeff = self.ffcoeff 
+
+        self.controller = pid_controller.PIDController(
+                self.pgain,
+                self.igain,
+                self.dgain,
+                self.outputMin,
+                self.outputMax,
+                self.feedForward.func
+                )
+        
+        self.pgainScheduler = GainScheduler(
+                self.pgainSchedMin*self.pgain,
+                self.pgain,
+                self.pgainSchedWid,
+                ) 
+
+    def handleSetGains(self,req):
+        """
+        Handles reqeusts to set the current controller gains.
+        """
+        flag = True
+        message = ''
+
+        pgain = req.pgain
+        igain = req.igain
+        dgain = req.dgain
+
+        if pgain < 0:
+            flag = False
+            message = 'pgain must be >= 0,'
+
+        if igain < 0:
+            flag = False
+            message = '{0} igain must be >= 0,'
+
+        if dgain < 0:
+            flag = False
+            message = '{0} dgain must be >= 0'
+
+        with self.lock:
+            if flag:
+                self.pgain = pgain
+                self.igain = igain
+                self.dgain = dgain
+                self.createController()
+
+        return SetControllerGainsResponse(flag, message)
+
+    def handleGetGains(self,req):
+        """
+        Handles request for the current PID controller gains.
+        """
+        with self.lock:
+            pgain = self.pgain
+            igain = self.igain
+            dgain = self.dgain
+        return GetControllerGainsResponse(pgain,igain,dgain)
+
     def handleNodeEnable(self,req):
+        """
+        Handles request to enable and disable the controller
+        """
         with self.lock:
             if req.enable:
                 self.enabled = True

File software/motor_cmd_source/nodes/gain_scheduler.py

View file
             gain = self.max_gain
         return gain
 
-#class GainScheduler2(object):
-#    """
-#    Two parameter gain scheduling function which applies a minimum gain
-#    for values near (x,y) = (0,0) and transition to a maximum gain for over 
-#    the transistion widths.
-#    """
-#
-#    def __init__(self, width_x=1.0, width_y=1.0,min_gain=1.0, max_gain=2.0):
-#        self.width_x = width_x
-#        self.width_y = width_y
-#        self.min_gain = min_gain
-#        self.max_gain = max_gain
-#
-#    def gain(self,x,y):
-#        theta = math.atan2(y,x)
-#        return self.x_func(x)*math.cos(theta)**2 + self.y_func(y)*math.sin(theta)**2
-#
-#    def x_func(self,x):
-#        slope = (self.max_gain - self.min_gain)/self.width_x
-#        offset = self.min_gain
-#        value = slope*abs(x) + offset
-#        if value > self.max_gain:
-#            value = self.max_gain
-#        return value
-#
-#    def y_func(self,y):
-#        slope = (self.max_gain - self.min_gain)/self.width_y
-#        offset = self.min_gain
-#        value = slope*abs(y) + offset
-#        if value > self.max_gain:
-#            value = self.max_gain
-#        return value
-#
-
 # -----------------------------------------------------------------------------
 if __name__ == '__main__':
     import matplotlib.pylab as pylab
 
-    if 1:
-        sch = GainScheduler(100, 1000, 0.1)
-        velo = pylab.linspace(-0.2,0.2,100)
-        gain = []
-        for v in velo:
-            gain.append(sch.gain(v))
-        pylab.plot(velo,gain)
-        pylab.show()
+    sch = GainScheduler(100, 1000, 0.1)
+    velo = pylab.linspace(-0.2,0.2,100)
+    gain = []
+    for v in velo:
+        gain.append(sch.gain(v))
+    pylab.plot(velo,gain)
+    pylab.xlabel('velocity (m/s)')
+    pylab.ylabel('gain')
+    pylab.grid('on')
+    pylab.show()
 
-#    if 0:
-#        sch = GainScheduler2(width_x=1.0,width_y=2.0)
-#        x_vals = pylab.linspace(-5,5,1000)
-#        gain_vals = []
-#        for x in x_vals:
-#            gain = sch.gain(0,x)
-#            gain_vals.append(gain)
-#        pylab.plot(x_vals,gain_vals,'.')
-#        pylab.show()
 
 
 

File software/motor_cmd_source/nodes/position_controller.py

View file
     def getMotorCmd(self):
         self.error = self.setptPosition - self.position
         self.ffValue = self.setptVelocity
-
         self.controller.pgain = self.pgainScheduler.gain(self.setptVelocity)
-
-        ## Use continuous gain schedule - to make motion smooth
-        ##----------------------------------------------------------------------
-        #if abs(self.error) < 5:
-        #    self.controller.pgain = 1.0
-        #elif abs(self.error) < 10:
-        #    self.controller.pgain = 2.0
-        #elif abs(self.error) < 15:
-        #    self.controller.pgain = 5.0
-        #elif abs(self.error) < 20:
-        #    self.controller.pgain = 8.0
-        #else:
-        #    self.controller.pgain = 12.0
-        ## ---------------------------------------------------------------------
-        
         self.motorCmd = self.controller.update(self.error,self.ffValue)
 
 # -----------------------------------------------------------------------------

File software/msg_and_srv/srv/GetControllerGains.srv

View file
+---
+float32 pgain
+float32 igain
+float32 dgain

File software/msg_and_srv/srv/SetControllerGains.srv

View file
+float32 pgain
+float32 igain
+float32 dgain
+---
+bool flag
+string message

File software/setpt_source/manifest.xml

View file
   <depend package="joy"/>
   <depend package="msg_and_srv"/>
   <depend package="actions"/>
+  <depend package="filters"/>
 
 </package>
 

File software/setpt_source/nodes/setpt_laser_source.py

View file
 import rospy
 import threading
 import math
+from filters import kalman_filter
 
 # Messages
 from std_msgs.msg import Header
         self.velocity = None
         self.ready = False
         self.enabled = False
+        self.kalman = kalman_filter.KalmanFilter((500.0,1000.0))
         rospy.init_node('laser_setpt_source')
 
         # Setup services
         Handles incoming analog input messages. Converts the analog inputs to a
         set point signal to be published on the setpt topic.
         """
+        t = rospy.get_rostime()
         if not self.ready or self.distance is None:
             return
         ain = data.values[self.ain_num] - self.laser_ain_offset
             enabled = self.enabled
 
         setpt_pos = setpt_pos + distance
+        setpt_pos_kalman, setpt_vel_kalman = self.kalman.update(setpt_pos,t.to_sec())
+
     
         if enabled:
             self.setpt_msg.header.stamp = rospy.get_rostime()
+            #self.setpt_msg.position = setpt_pos_kalman 
             self.setpt_msg.position = setpt_pos 
-            self.setpt_msg.velocity = 0.0 #velocity 
+            self.setpt_msg.velocity = setpt_vel_kalman 
             self.setpt_pub.publish(self.setpt_msg)
 
     def handle_dist_msg(self,data):

File software/utilities/src/utilities/robot_control.py

View file
         sensor.
         """
         if self.mode == 'inertial trajectory':
-            resp = self.laserSetptEnableProxy(True)
+            resp = self.laserSetptEnableProxy(False)
             return resp.status, resp.message
         else:
             return True, ''
         # I don't think this is needed
         self.disableControllerMode()
         self.disableDynamics()
+        self.disableLaserSetpt()
     
 
 # Decorators

File software/wc_config/params/wc_params.xml

View file
     <!-- Position controller parameters -->
     <param name="controller_max_output" type="double" value="4000.0"/>
     <param name="controller_min_output" type="double" value="-4000.0"/>
+    <param name="controller_pgain_sched_min" type="double" value="0.1"/>
 
     <param name="dist_lower_bound_min" type="double" value="0.1" />
     <param name="dist_upper_bound_max" type="double" value="35.0" />