Skip to content

Commit 8bc5d30

Browse files
airballkingairballking
committed
Added first draft of hashed descriptions of PID controllers.
1 parent 5ece691 commit 8bc5d30

File tree

6 files changed

+180
-25
lines changed

6 files changed

+180
-25
lines changed

‎cl_robot_controllers/cl-robot-controllers.asd

Lines changed: 8 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -35,5 +35,12 @@
3535
((:module "src"
3636
:components
3737
((:file "package")
38+
(:file "hash-table-utils" :depends-on ("package"))
3839
(:file "controller-interface" :depends-on ("package"))
39-
(:file "pid" :depends-on ("controller-interface" "package"))))))
40+
(:file "hashed-controller-configuration"
41+
:depends-on ("package" "controller-interface" "hash-table-utils"))
42+
(:file "pid"
43+
:depends-on ("controller-interface"
44+
"package"
45+
"hash-table-utils"
46+
"hashed-controller-configuration"))))))

‎cl_robot_controllers/src/controller-configuration.lisp

Lines changed: 46 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,46 @@
1+
;;; Copyright (c) 2014, Georg Bartels <georg.bartels@cs.uni-bremen.de>
2+
;;; All rights reserved.
3+
;;;
4+
;;; Redistribution and use in source and binary forms, with or without
5+
;;; modification, are permitted provided that the following conditions are met:
6+
;;;
7+
;;; * Redistributions of source code must retain the above copyright
8+
;;; notice, this list of conditions and the following disclaimer.
9+
;;; * Redistributions in binary form must reproduce the above copyright
10+
;;; notice, this list of conditions and the following disclaimer in the
11+
;;; documentation and/or other materials provided with the distribution.
12+
;;; * Neither the name of the Institute for Artificial Intelligence/
13+
;;; Universitaet Bremen nor the names of its contributors may be used to
14+
;;; endorse or promote products derived from this software without specific
15+
;;; prior written permission.
16+
;;;
17+
;;; THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18+
;;; AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19+
;;; IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20+
;;; ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21+
;;; LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22+
;;; CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23+
;;; SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24+
;;; INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25+
;;; CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26+
;;; ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27+
;;; POSSIBILITY OF SUCH DAMAGE.
28+
29+
(in-package :robot-controllers)
30+
31+
32+
33+
(defmethod make-controller ((configuration hashed-controller-configuration))
34+
(multiple-value-bind (type type-p) (gethash :type (content configuration))
35+
(if type-p
36+
(make-controller type configuration)
37+
(error "No hash-key 'type' in hashed-controller-configuration: ~a~%"
38+
configuration)))
39+
40+
(defmethod make-controller ((configuration p-controller-configuration))
41+
"Creates and returns a new P-controller from `configuration'."
42+
(multiple-value-bind (p-gain p-gain-p) (gethash :p-gain (hashed-content configuration))
43+
(if p-gain-p
44+
(make-instance 'p-controller :p-gain p-gain)
45+
(error "Could not find 'p-gain' in configuration: ~a~%" configuration))))
46+

‎cl_robot_controllers/src/controller-interface.lisp

Lines changed: 6 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -31,19 +31,13 @@
3131
(defclass controller () ()
3232
(:documentation "Non-functional default controller."))
3333

34-
(defclass controller-configuration ()
35-
((hashed-content :initform (make-hash-table :test 'equal) :initarg :hashed-content
36-
:accessor hashed-content :type hash-table
37-
:documentation "A hash-table to stuff content for prototyping."))
38-
(:documentation "Empty default controller configuration."))
34+
(defclass controller-configuration () ()
35+
(:documentation "Non-functional default controller configuration."))
3936

40-
(defgeneric make-controller (configuration)
41-
(:documentation "Creates a controller from `configuration'.")
42-
(:method ((configuration controller-configuration))
43-
(warn "Controller configuration of unknown type '~a'. Skipping~%" configuration)))
37+
(defgeneric make-controller (configuration controller-type)
38+
(:documentation "Method to create controllers from their `configuration',
39+
optionally using `controller-type' as a hint for dispatch."))
4440

4541
;;; TODO(Georg): consider turning keys into &rest and using destructuring-bind
4642
(defgeneric compute-command (controller &key &allow-other-keys)
47-
(:documentation "Computes the command of `controller' for input given as key params.")
48-
(:method ((controller controller) &key &allow-other-keys)
49-
(warn "Controller of unknown type: '~a'. Skipping~%" controller)))
43+
(:documentation "Computes and returns the command of `controller' for key params."))

‎cl_robot_controllers/src/hash-table-utils.lisp

Lines changed: 38 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,38 @@
1+
;;; Copyright (c) 2014, Georg Bartels <georg.bartels@cs.uni-bremen.de>
2+
;;; All rights reserved.
3+
;;;
4+
;;; Redistribution and use in source and binary forms, with or without
5+
;;; modification, are permitted provided that the following conditions are met:
6+
;;;
7+
;;; * Redistributions of source code must retain the above copyright
8+
;;; notice, this list of conditions and the following disclaimer.
9+
;;; * Redistributions in binary form must reproduce the above copyright
10+
;;; notice, this list of conditions and the following disclaimer in the
11+
;;; documentation and/or other materials provided with the distribution.
12+
;;; * Neither the name of the Institute for Artificial Intelligence/
13+
;;; Universitaet Bremen nor the names of its contributors may be used to
14+
;;; endorse or promote products derived from this software without specific
15+
;;; prior written permission.
16+
;;;
17+
;;; THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18+
;;; AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19+
;;; IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20+
;;; ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21+
;;; LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22+
;;; CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23+
;;; SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24+
;;; INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25+
;;; CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26+
;;; ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27+
;;; POSSIBILITY OF SUCH DAMAGE.
28+
29+
(in-package :robot-controllers)
30+
31+
(defun read-hash-values (table keys)
32+
(apply #'values (mapcar (lambda (key) (read-hash-value table key)) keys)))
33+
34+
(defun read-hash-value (table key)
35+
(multiple-value-bind (value value-p) (gethash key table)
36+
(if value-p
37+
value
38+
(error "Could not look up key '~a' in table '~a'.~%" key table))))

‎cl_robot_controllers/src/hashed-controller-configuration.lisp

Lines changed: 64 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,64 @@
1+
;;; Copyright (c) 2014, Georg Bartels <georg.bartels@cs.uni-bremen.de>
2+
;;; All rights reserved.
3+
;;;
4+
;;; Redistribution and use in source and binary forms, with or without
5+
;;; modification, are permitted provided that the following conditions are met:
6+
;;;
7+
;;; * Redistributions of source code must retain the above copyright
8+
;;; notice, this list of conditions and the following disclaimer.
9+
;;; * Redistributions in binary form must reproduce the above copyright
10+
;;; notice, this list of conditions and the following disclaimer in the
11+
;;; documentation and/or other materials provided with the distribution.
12+
;;; * Neither the name of the Institute for Artificial Intelligence/
13+
;;; Universitaet Bremen nor the names of its contributors may be used to
14+
;;; endorse or promote products derived from this software without specific
15+
;;; prior written permission.
16+
;;;
17+
;;; THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18+
;;; AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19+
;;; IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20+
;;; ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21+
;;; LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22+
;;; CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23+
;;; SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24+
;;; INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25+
;;; CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26+
;;; ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27+
;;; POSSIBILITY OF SUCH DAMAGE.
28+
29+
(in-package :robot-controllers)
30+
31+
;;;
32+
;;; GENERIC SETUP OF HASHED CONTROLLER CONFIGURATIONS
33+
;;;
34+
;;; The idea is to provide a configuration with a hash-table inside
35+
;;; which fully specifies which controller to create and with which
36+
;;; values to fill it. My intention is to instantiate controllers at
37+
;;; runtime from data without a need to directly depending on the lib.
38+
;;;
39+
;;; EXAMPLE: P-CONTROLLER
40+
;;; Hash-table in slot 'content' is expected to have the following
41+
;;; key-value pairs:
42+
;;; :controller-name --> "P-CONTROLLER"
43+
;;; :package-name --> "CL-ROBOT-CONTROLLERS"
44+
;;; :p-gain --> 2.5
45+
;;;
46+
;;; Calling make-controller with such a configuration `config'
47+
;;; and specifying that the controller-type is :unknown:
48+
;;; CL-USER > (make-controller config :unknown)
49+
;;; CL-USER > #<P-CONTROLLER {...}>
50+
;;;
51+
52+
(defclass hashed-controller-configuration (controller-configuration)
53+
((content :initform (make-hash-table :test 'equal) :initarg :content
54+
:accessor content :type hash-table
55+
:documentation "Hashed content of this configuration."))
56+
(:documentation "Hashed-controller-configuration for prototyping."))
57+
58+
(defmethod make-controller ((configuration hashed-controller-configuration)
59+
(controller-type (eql :UNKNOWN)))
60+
"Default implementation trying to look up controller type from configuration."
61+
(declare (ignore controller-type))
62+
(multiple-value-bind (controller-name package-name)
63+
(read-hash-values (content configuration) (list :controller-name :package-name))
64+
(make-controller configuration (intern controller-name package-name))))

‎cl_robot_controllers/src/pid.lisp

Lines changed: 18 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -36,25 +36,18 @@
3636
((p-gain :initform 0.0 :initarg :p-gain :accessor p-gain :type number))
3737
(:documentation "A simple p-controller."))
3838

39-
(defclass p-controller-configuration (controller-configuration) ()
40-
(:documentation "Configuration for a simple p-controller."))
41-
42-
(defmethod make-controller ((configuration p-controller-configuration))
43-
"Creates and returns a new P-controller from `configuration'."
44-
(multiple-value-bind (p-gain p-gain-p) (gethash :p-gain configuration)
45-
(if p-gain-p
46-
(make-instance 'p-controller :p-gain p-gain)
47-
(error "Could not find 'p-gain' in configuration: ~a~%" configuration))))
48-
49-
;;; TODO(Georg): consider dropping this
5039
(defun make-p-controller (p-gain)
5140
(make-instance 'p-controller :p-gain p-gain))
5241

53-
;;; TODO(Georg): consider dropping this
5442
(defun copy-p-controller (p-controller &key p-gain)
5543
(with-slots ((old-p-gain p-gain)) p-controller
5644
(make-p-controller (or p-gain old-p-gain))))
5745

46+
(defmethod make-controller ((configuration hashed-controller-configuration)
47+
(controller-type (eql 'p-controller)))
48+
(declare (ignore controller-type))
49+
(make-p-controller (read-hash-value (content configuration) :p-gain)))
50+
5851
(defmethod compute-command ((controller p-controller) &key error &allow-other-keys)
5952
(* error (p-gain controller)))
6053

@@ -80,6 +73,13 @@
8073
(make-i-controller (or i-gain old-i-gain) (or i-max old-i-max) (or i-min old-i-min)
8174
(or integrated-error old-integrated-error))))
8275

76+
(defmethod make-controller ((configuration hashed-controller-configuration)
77+
(controller-type (eql 'i-controller)))
78+
(declare (ignore controller-type))
79+
(multiple-value-bind (i-gain i-max i-min)
80+
(read-hash-values (content configuration) (list :i-gain :i-max :i-min))
81+
(make-i-controller i-gain i-max i-min)))
82+
8383
(defmethod compute-command ((controller i-controller) &key error dt &allow-other-keys)
8484
(flet ((limit-value (current-value minimum-value maximum-value)
8585
(max minimum-value (min current-value maximum-value))))
@@ -101,13 +101,19 @@
101101
:type number :documentation "For internal use."))
102102
(:documentation "A simple d-controller."))
103103

104+
104105
(defun make-d-controller (d-gain &optional (last-error 0))
105106
(make-instance 'd-controller :d-gain d-gain :last-error last-error))
106107

107108
(defun copy-d-controller (d-controller &key d-gain last-error)
108109
(with-slots ((old-d-gain d-gain) (old-last-error last-error)) d-controller
109110
(make-d-controller (or d-gain old-d-gain) (or last-error old-last-error))))
110111

112+
(defmethod make-controller ((configuration hashed-controller-configuration)
113+
(controller-type (eql 'd-controller)))
114+
(declare (ignore controller-type))
115+
(make-d-controller (read-hash-value (content configuration) :d-gain)))
116+
111117
(defmethod compute-command ((controller d-controller) &key error dt &allow-other-keys)
112118
(with-slots (d-gain last-error) controller
113119
(when (>= 0.0 dt)

0 commit comments

Comments
 (0)