Debug des nœuds de rate et triangle

This commit is contained in:
Hugo LEVY-FALK 2019-06-05 08:54:37 +02:00
parent 7355556ac8
commit 69cccc4615
23 changed files with 1103 additions and 38 deletions

View file

@ -95,12 +95,13 @@ generate_messages(
generate_dynamic_reconfigure_options(
cfg/DetectTargets.cfg
cfg/triangle_control.cfg
cfg/PIDNode.cfg
cfg/RateNode.cfg
cfg/ProportionalNode.cfg
cfg/IntegralNode.cfg
cfg/DerivativeNode.cfg
cfg/SaturateNode.cfg
cfg/InputNode.cfg
cfg/Triangle.cfg
)
###################################

0
cfg/RateNode.cfg Normal file → Executable file
View file

12
cfg/Triangle.cfg Executable file
View file

@ -0,0 +1,12 @@
#!/usr/bin/env python
PACKAGE = "detect_targets"
from dynamic_reconfigure.parameter_generator_catkin import *
gen = ParameterGenerator()
gen.add("camera_angle", double_t, 0, "The angle corresponding to the image width", 80, 50, 180)
gen.add("target_width", double_t, 0, "the real target width (m)", 1, 0.01, 1.5)
gen.add("target_depth", double_t, 0, "the real target depth (m)", .2, 0.01, 0.5)
exit(gen.generate(PACKAGE, "detect_targets", "Triangle"))

View file

@ -1,6 +1,12 @@
<launch>
<remap from="/reset" to="/bebop/reset"/>
<node name="reconf" pkg="rqt_reconfigure" type="rqt_reconfigure"/>
<node name="Ratex_param" pkg="dynamic_reconfigure" type="dynparam" args="load /ratex $(find detect_targets)/params/settings_test_D_rate.yaml"/>
<node name="Ratey_param" pkg="dynamic_reconfigure" type="dynparam" args="load /ratey $(find detect_targets)/params/settings_test_D_rate.yaml"/>
<node name="Dx_param" pkg="dynamic_reconfigure" type="dynparam" args="load /Dx $(find detect_targets)/params/settings_test_D.yaml"/>
<node name="Dy_param" pkg="dynamic_reconfigure" type="dynparam" args="load /Dy $(find detect_targets)/params/settings_test_D.yaml"/>
<node name="safe_param" pkg="dynamic_reconfigure" type="dynparam" args="load /safe $(find detect_targets)/params/settings_safe.yaml"/>
<node name="targets_param" pkg="dynamic_reconfigure" type="dynparam" args="load /targets $(find detect_targets)/params/settings_blue.yaml"/>
<include file="$(find bebop_driver)/launch/bebop_node.launch" />
<node name="targets" pkg="detect_targets" type="target_publisher.py">
@ -13,25 +19,22 @@
<node name="untwist" pkg="detect_targets" type="untwist.py" output="screen">
<remap from="input" to="/bebop/cmd_vel" />
</node>
<node name="Dx" pkg="detect_targets" type="control_compute.py" args="derivative" output="screen">
<node name="ratex" pkg="detect_targets" type="control_compute.py" args="rate" output="screen">
<remap from="input" to="linear_x" />
<remap from="output" to="derivative_x_input" />
</node>
<node name="Dx" pkg="detect_targets" type="control_compute.py" args="derivative" output="screen">
<remap from="input" to="derivative_x_input" />
<remap from="output" to="measure_x" />
</node>
<node name="Dy" pkg="detect_targets" type="control_compute.py" args="derivative" output="screen">
<node name="ratey" pkg="detect_targets" type="control_compute.py" args="rate" output="screen">
<remap from="input" to="linear_y" />
<remap from="output" to="derivative_y_input" />
</node>
<node name="Dy" pkg="detect_targets" type="control_compute.py" args="derivative" output="screen">
<remap from="input" to="derivative_y_input" />
<remap from="output" to="measure_y" />
</node>
<include file="$(find detect_targets)/launch/control.launch" ns="controller_angular_z">
<arg name="reset" value="/reset" />
<arg name="measure" value="/angular_z" />
</include>
<node name="twister" pkg="detect_targets" type="twist_controls.py">
<remap from="control_linear_z" to="controller_linear_z/output" />
<remap from="control_angular_z" to="controller_angular_z/output" />
<remap from="control_linear_x" to="controller_linear_x/internal_loop/output" />
<remap from="control_linear_y" to="controller_linear_y/internal_loop/output" />
</node>
<node name="safe" pkg="demo_teleop" type="safe_drone_teleop.py" output="screen" launch-prefix="xterm -e">
<remap from="takeoff" to="/bebop/takeoff"/>
@ -41,7 +44,11 @@
</node>
<node name="view" pkg="rqt_image_view" type="rqt_image_view" args="/bebop/image_raw"/>
<node name="view__targets" pkg="rqt_image_view" type="rqt_image_view" args="/img_targets"/>
<node name="view_targets" pkg="rqt_image_view" type="rqt_image_view" args="/img_targets"/>
<node name="plot" pkg="rqt_plot" type="rqt_plot" args="/measure_x /measure_linear_x">
</node>
<node name="graph" pkg="rqt_graph" type="rqt_graph" output="screen"></node>
</launch>

View file

@ -1,7 +1,14 @@
<launch>
<node name="reconf" pkg="rqt_reconfigure" type="rqt_reconfigure"/>
<node name="D_param" pkg="dynamic_reconfigure" type="dynparam" args="load /D $(find detect_targets)/params/settings_test_D.yaml"/>
<node name="Rate_param" pkg="dynamic_reconfigure" type="dynparam" args="load /rate $(find detect_targets)/params/settings_test_D_rate.yaml"/>
<node name="publisher" pkg="detect_targets" type="publish_csv.py" args="$(find detect_targets)/walk.csv" output="screen">
<node name="publisher" pkg="detect_targets" type="publish_csv.py" args="$(find detect_targets)/utils/data/walk.csv" output="screen">
<remap from="output" to="rate_input" />
</node>
<node name="rate" pkg="detect_targets" type="control_compute.py" args="rate" output="screen">
<remap from="input" to="rate_input" />
<remap from="output" to="derivative_input" />
</node>

31
params/settings_blue.yaml Normal file
View file

@ -0,0 +1,31 @@
!!python/object/new:dynamic_reconfigure.encoding.Config
dictitems:
binary: true
blue_max: 255
blue_min: 24
green_max: 203
green_min: 91
groups: !!python/object/new:dynamic_reconfigure.encoding.Config
dictitems:
binary: true
blue_max: 255
blue_min: 24
green_max: 203
green_min: 91
groups: !!python/object/new:dynamic_reconfigure.encoding.Config
state: []
id: 0
name: Default
parameters: !!python/object/new:dynamic_reconfigure.encoding.Config
state: []
parent: 0
red_max: 75
red_min: 36
state: true
targets: true
type: ''
state: []
red_max: 75
red_min: 36
targets: true
state: []

21
params/settings_safe.yaml Normal file
View file

@ -0,0 +1,21 @@
!!python/object/new:dynamic_reconfigure.encoding.Config
dictitems:
angular: 0.5
delay: 2.0
groups: !!python/object/new:dynamic_reconfigure.encoding.Config
dictitems:
angular: 0.5
delay: 2.0
groups: !!python/object/new:dynamic_reconfigure.encoding.Config
state: []
id: 0
linear: 0.05
name: Default
parameters: !!python/object/new:dynamic_reconfigure.encoding.Config
state: []
parent: 0
state: true
type: ''
state: []
linear: 0.05
state: []

View file

@ -0,0 +1,31 @@
!!python/object/new:dynamic_reconfigure.encoding.Config
dictitems:
binary: false
blue_max: 255
blue_min: 136
green_max: 182
green_min: 65
groups: !!python/object/new:dynamic_reconfigure.encoding.Config
dictitems:
binary: false
blue_max: 255
blue_min: 136
green_max: 182
green_min: 65
groups: !!python/object/new:dynamic_reconfigure.encoding.Config
state: []
id: 0
name: Default
parameters: !!python/object/new:dynamic_reconfigure.encoding.Config
state: []
parent: 0
red_max: 127
red_min: 44
state: true
targets: false
type: ''
state: []
red_max: 127
red_min: 44
targets: false
state: []

View file

@ -0,0 +1,25 @@
!!python/object/new:dynamic_reconfigure.encoding.Config
dictitems:
deriv: 1
groups: !!python/object/new:dynamic_reconfigure.encoding.Config
dictitems:
deriv: 1
groups: !!python/object/new:dynamic_reconfigure.encoding.Config
state: []
id: 0
k: 1.0
name: Default
parameters: !!python/object/new:dynamic_reconfigure.encoding.Config
state: []
parent: 0
poly_order: 2
refresh_time: 0.05
size: 19
state: true
type: ''
state: []
k: 1.0
poly_order: 2
refresh_time: 0.05
size: 19
state: []

View file

@ -0,0 +1,19 @@
!!python/object/new:dynamic_reconfigure.encoding.Config
dictitems:
groups: !!python/object/new:dynamic_reconfigure.encoding.Config
dictitems:
groups: !!python/object/new:dynamic_reconfigure.encoding.Config
state: []
id: 0
k: 1.0
name: Default
parameters: !!python/object/new:dynamic_reconfigure.encoding.Config
state: []
parent: 0
refresh_time: 0.05
state: true
type: ''
state: []
k: 1.0
refresh_time: 0.05
state: []

View file

@ -0,0 +1,31 @@
!!python/object/new:dynamic_reconfigure.encoding.Config
dictitems:
binary: false
blue_max: 92
blue_min: 30
green_max: 201
green_min: 109
groups: !!python/object/new:dynamic_reconfigure.encoding.Config
dictitems:
binary: false
blue_max: 92
blue_min: 30
green_max: 201
green_min: 109
groups: !!python/object/new:dynamic_reconfigure.encoding.Config
state: []
id: 0
name: Default
parameters: !!python/object/new:dynamic_reconfigure.encoding.Config
state: []
parent: 0
red_max: 186
red_min: 130
state: true
targets: false
type: ''
state: []
red_max: 186
red_min: 130
targets: false
state: []

View file

@ -11,12 +11,12 @@ from std_msgs.msg import Float64, Empty
import dynamic_reconfigure.server
from detect_targets.cfg import ProportionalNodeConfig, IntegralNodeConfig
from detect_targets.cfg import DerivativeNodeConfig, InputNodeConfig,
from detect_targets.cfg import DerivativeNodeConfig, InputNodeConfig
from detect_targets.cfg import SaturateNodeConfig, RateNodeConfig
class ControlNode(object):
def __init__(self, refresh_time=1.0, config_class=None):
def __init__(self, refresh_time=0.05, config_class=None):
self.last_time = rospy.get_rostime()
self.refresh_time = rospy.Duration.from_sec(refresh_time)
self.output = 0.0
@ -59,31 +59,30 @@ class InputControlNode(ControlNode):
self.input_topics["input"] = rospy.Subscriber("input", Float64, self.on_compute)
class RateNode(InputControlNode):
def __init__(self, *args, **kwargs):
self.rate = rospy.Rate(20)
self.last_input = 0
def __init__(self, refresh_time=0.05):
self.lock = threading.Lock()
super(RateNode, self).__init__(*args, **kwargs)
super(RateNode, self).__init__(refresh_time, RateNodeConfig)
self.rate = rospy.Rate(1.0/self.refresh_time.to_sec())
def on_reconf(self, config, level):
c = super(SaturateNode, self).on_reconf(config, level)
self.rate = rospy.Rate(self.refresh_time)
c = super(RateNode, self).on_reconf(config, level)
self.rate = rospy.Rate(self.refresh_time.to_sec())
return c
def on_compute(self, value):
self.lock.acquire()
self.last_input = value.data
self.output = value.data
self.lock.release()
def on_mainloop(self):
while not rospy.is_shutdown():
self.lock.acquire()
self.output.publish(self.last_input)
self.output_topic.publish(self.output)
self.lock.release()
self.rate.sleep()
class ProportionalNode(InputControlNode):
def __init__(self, k=1.0, refresh_time=1.0):
def __init__(self, k=1.0, refresh_time=0.05):
super(ProportionalNode, self).__init__(refresh_time, ProportionalNodeConfig)
self.k = k
@ -98,7 +97,7 @@ class ProportionalNode(InputControlNode):
super(ProportionalNode, self).on_compute(value)
class SaturateNode(InputControlNode):
def __init__(self, min=None, max=None, refresh_time=1.0, config=SaturateNodeConfig):
def __init__(self, min=None, max=None, refresh_time=0.05, config=SaturateNodeConfig):
super(SaturateNode, self).__init__(refresh_time, config)
self.min = min
self.max = max
@ -116,7 +115,7 @@ class SaturateNode(InputControlNode):
super(SaturateNode, self).on_compute(value)
class IntegralNode(SaturateNode):
def __init__(self, k=1.0, min=None, max=None, refresh_time=1.0):
def __init__(self, k=1.0, min=None, max=None, refresh_time=0.05):
super(IntegralNode, self).__init__(min, max, refresh_time, IntegralNodeConfig)
self.k = k
@ -131,7 +130,7 @@ class IntegralNode(SaturateNode):
return super(IntegralNode, self).on_compute(value)
class DerivativeNode(InputControlNode):
def __init__(self, k=0.05, size=9, polyorder=3, deriv=1, refresh_time=1.0):
def __init__(self, k=0.05, size=9, polyorder=3, deriv=1, refresh_time=0.05):
super(DerivativeNode, self).__init__(refresh_time, DerivativeNodeConfig)
self.k = k
self.filter = savgol_coeffs(size, polyorder, deriv, delta=refresh_time, use='dot')
@ -183,7 +182,7 @@ class DifferenciateNode(InputControlNode):
class InputNode(InputControlNode):
def __init__(self):
super(InputNode, self).__init__(refresh_time=1.0, config_class=InputNodeConfig)
super(InputNode, self).__init__(refresh_time=0.05, config_class=InputNodeConfig)
def on_reconf(self, config, level):
self.output = config["value"]
@ -196,7 +195,7 @@ class InputNode(InputControlNode):
class SumNode(ControlNode):
def __init__(self, nb_topics):
super(SumNode, self).__init__(refresh_time=1.0)
super(SumNode, self).__init__(refresh_time=0.05)
self.nb_topics = int(nb_topics)
self.inputs = dict()
for i in range(self.nb_topics):
@ -230,4 +229,4 @@ if __name__ == '__main__':
else:
node = node_class()
node.on_mailoop()
node.on_mainloop()

View file

@ -11,6 +11,7 @@ if __name__ == '__main__':
rospy.init_node('publish_csv', anonymous=True)
pub = rospy.Publisher('output', Float64, queue_size=1)
file = sys.argv[1]
rospy.sleep(10.)
with open(file) as csvfile:
reader = csv.reader(csvfile)
r = rospy.Rate(20)

View file

@ -94,7 +94,7 @@ class Triangle:
"component_centers", component_centers, self.on_comp, queue_size=1)
self.config_srv = dynamic_reconfigure.server.Server(
TriangleParamConfig, self.on_reconf)
TriangleConfig, self.on_reconf)
self.br = tf.TransformBroadcaster()

Binary file not shown.

Binary file not shown.

After

Width:  |  Height:  |  Size: 56 KiB

View file

@ -1,6 +1,6 @@
%!PS-Adobe-3.0 EPSF-3.0
%%Creator: matplotlib version 2.2.4, http://matplotlib.org/
%%CreationDate: Sun Jun 2 18:28:50 2019
%%CreationDate: Sun Jun 2 19:58:19 2019
%%Orientation: portrait
%%BoundingBox: -54 180 666 612
%%EndComments

View file

@ -1,6 +1,6 @@
%!PS-Adobe-3.0 EPSF-3.0
%%Creator: matplotlib version 2.2.4, http://matplotlib.org/
%%CreationDate: Sun Jun 2 18:28:49 2019
%%CreationDate: Sun Jun 2 19:58:18 2019
%%Orientation: portrait
%%BoundingBox: -54 180 666 612
%%EndComments

View file

@ -1,6 +1,6 @@
%!PS-Adobe-3.0 EPSF-3.0
%%Creator: matplotlib version 2.2.4, http://matplotlib.org/
%%CreationDate: Sun Jun 2 18:28:49 2019
%%CreationDate: Sun Jun 2 19:58:18 2019
%%Orientation: portrait
%%BoundingBox: -54 180 666 612
%%EndComments

View file

@ -0,0 +1,833 @@
%!PS-Adobe-3.0 EPSF-3.0
%%Creator: matplotlib version 2.2.4, http://matplotlib.org/
%%CreationDate: Tue Jun 4 14:06:46 2019
%%Orientation: portrait
%%BoundingBox: 90 252 522 540
%%EndComments
%%BeginProlog
/mpldict 8 dict def
mpldict begin
/m { moveto } bind def
/l { lineto } bind def
/r { rlineto } bind def
/c { curveto } bind def
/cl { closepath } bind def
/box {
m
1 index 0 r
0 exch r
neg 0 r
cl
} bind def
/clipbox {
box
clip
newpath
} bind def
%!PS-Adobe-3.0 Resource-Font
%%Title: DejaVu Sans
%%Copyright: Copyright (c) 2003 by Bitstream, Inc. All Rights Reserved. Copyright (c) 2006 by Tavmjong Bah. All Rights Reserved. DejaVu changes are in public domain
%%Creator: Converted from TrueType to type 3 by PPR
25 dict begin
/_d{bind def}bind def
/_m{moveto}_d
/_l{lineto}_d
/_cl{closepath eofill}_d
/_c{curveto}_d
/_sc{7 -1 roll{setcachedevice}{pop pop pop pop pop pop}ifelse}_d
/_e{exec}_d
/FontName /DejaVuSans def
/PaintType 0 def
/FontMatrix[.001 0 0 .001 0 0]def
/FontBBox[-1021 -463 1793 1232]def
/FontType 3 def
/Encoding [ /hyphen /period /zero /one /two /five /seven /a /g /l /m /o /p ] def
/FontInfo 10 dict dup begin
/FamilyName (DejaVu Sans) def
/FullName (DejaVu Sans) def
/Notice (Copyright (c) 2003 by Bitstream, Inc. All Rights Reserved. Copyright (c) 2006 by Tavmjong Bah. All Rights Reserved. DejaVu changes are in public domain ) def
/Weight (Book) def
/Version (Version 2.35) def
/ItalicAngle 0.0 def
/isFixedPitch false def
/UnderlinePosition -130 def
/UnderlineThickness 90 def
end readonly def
/CharStrings 14 dict dup begin
/.notdef 0 def
/hyphen{361 0 49 234 312 314 _sc
49 314 _m
312 314 _l
312 234 _l
49 234 _l
49 314 _l
_cl}_d
/period{318 0 107 0 210 124 _sc
107 124 _m
210 124 _l
210 0 _l
107 0 _l
107 124 _l
_cl}_d
/zero{636 0 66 -13 570 742 _sc
318 664 _m
267 664 229 639 203 589 _c
177 539 165 464 165 364 _c
165 264 177 189 203 139 _c
229 89 267 64 318 64 _c
369 64 407 89 433 139 _c
458 189 471 264 471 364 _c
471 464 458 539 433 589 _c
407 639 369 664 318 664 _c
318 742 _m
399 742 461 709 505 645 _c
548 580 570 486 570 364 _c
570 241 548 147 505 83 _c
461 19 399 -13 318 -13 _c
236 -13 173 19 130 83 _c
87 147 66 241 66 364 _c
66 486 87 580 130 645 _c
173 709 236 742 318 742 _c
_cl}_d
/one{636 0 110 0 544 729 _sc
124 83 _m
285 83 _l
285 639 _l
110 604 _l
110 694 _l
284 729 _l
383 729 _l
383 83 _l
544 83 _l
544 0 _l
124 0 _l
124 83 _l
_cl}_d
/two{{636 0 73 0 536 742 _sc
192 83 _m
536 83 _l
536 0 _l
73 0 _l
73 83 _l
110 121 161 173 226 239 _c
290 304 331 346 348 365 _c
380 400 402 430 414 455 _c
426 479 433 504 433 528 _c
433 566 419 598 392 622 _c
365 646 330 659 286 659 _c
255 659 222 653 188 643 _c
154 632 117 616 78 594 _c
78 694 _l
118 710 155 722 189 730 _c
223 738 255 742 284 742 _c
}_e{359 742 419 723 464 685 _c
509 647 532 597 532 534 _c
532 504 526 475 515 449 _c
504 422 484 390 454 354 _c
446 344 420 317 376 272 _c
332 227 271 164 192 83 _c
_cl}_e}_d
/five{{636 0 77 -13 549 729 _sc
108 729 _m
495 729 _l
495 646 _l
198 646 _l
198 467 _l
212 472 227 476 241 478 _c
255 480 270 482 284 482 _c
365 482 429 459 477 415 _c
525 370 549 310 549 234 _c
549 155 524 94 475 51 _c
426 8 357 -13 269 -13 _c
238 -13 207 -10 175 -6 _c
143 -1 111 6 77 17 _c
77 116 _l
106 100 136 88 168 80 _c
199 72 232 69 267 69 _c
}_e{323 69 368 83 401 113 _c
433 143 450 183 450 234 _c
450 284 433 324 401 354 _c
368 384 323 399 267 399 _c
241 399 214 396 188 390 _c
162 384 135 375 108 363 _c
108 729 _l
_cl}_e}_d
/seven{636 0 82 0 551 729 _sc
82 729 _m
551 729 _l
551 687 _l
286 0 _l
183 0 _l
432 646 _l
82 646 _l
82 729 _l
_cl}_d
/a{{613 0 60 -13 522 560 _sc
343 275 _m
270 275 220 266 192 250 _c
164 233 150 205 150 165 _c
150 133 160 107 181 89 _c
202 70 231 61 267 61 _c
317 61 357 78 387 114 _c
417 149 432 196 432 255 _c
432 275 _l
343 275 _l
522 312 _m
522 0 _l
432 0 _l
432 83 _l
411 49 385 25 355 10 _c
325 -5 287 -13 243 -13 _c
187 -13 142 2 109 33 _c
76 64 60 106 60 159 _c
}_e{60 220 80 266 122 298 _c
163 329 224 345 306 345 _c
432 345 _l
432 354 _l
432 395 418 427 391 450 _c
364 472 326 484 277 484 _c
245 484 215 480 185 472 _c
155 464 127 453 100 439 _c
100 522 _l
132 534 164 544 195 550 _c
226 556 256 560 286 560 _c
365 560 424 539 463 498 _c
502 457 522 395 522 312 _c
_cl}_e}_d
/g{{635 0 55 -207 544 560 _sc
454 280 _m
454 344 440 395 414 431 _c
387 467 349 485 301 485 _c
253 485 215 467 188 431 _c
161 395 148 344 148 280 _c
148 215 161 165 188 129 _c
215 93 253 75 301 75 _c
349 75 387 93 414 129 _c
440 165 454 215 454 280 _c
544 68 _m
544 -24 523 -93 482 -139 _c
440 -184 377 -207 292 -207 _c
260 -207 231 -204 203 -200 _c
175 -195 147 -188 121 -178 _c
}_e{121 -91 _l
147 -105 173 -115 199 -122 _c
225 -129 251 -133 278 -133 _c
336 -133 380 -117 410 -87 _c
439 -56 454 -10 454 52 _c
454 96 _l
435 64 411 40 382 24 _c
353 8 319 0 279 0 _c
211 0 157 25 116 76 _c
75 127 55 195 55 280 _c
55 364 75 432 116 483 _c
157 534 211 560 279 560 _c
319 560 353 552 382 536 _c
411 520 435 496 454 464 _c
454 547 _l
544 547 _l
}_e{544 68 _l
_cl}_e}_d
/l{278 0 94 0 184 760 _sc
94 760 _m
184 760 _l
184 0 _l
94 0 _l
94 760 _l
_cl}_d
/m{{974 0 91 0 889 560 _sc
520 442 _m
542 482 569 511 600 531 _c
631 550 668 560 711 560 _c
767 560 811 540 842 500 _c
873 460 889 403 889 330 _c
889 0 _l
799 0 _l
799 327 _l
799 379 789 418 771 444 _c
752 469 724 482 686 482 _c
639 482 602 466 575 435 _c
548 404 535 362 535 309 _c
535 0 _l
445 0 _l
445 327 _l
445 379 435 418 417 444 _c
398 469 369 482 331 482 _c
}_e{285 482 248 466 221 435 _c
194 404 181 362 181 309 _c
181 0 _l
91 0 _l
91 547 _l
181 547 _l
181 462 _l
201 495 226 520 255 536 _c
283 552 317 560 357 560 _c
397 560 430 550 458 530 _c
486 510 506 480 520 442 _c
_cl}_e}_d
/o{612 0 55 -13 557 560 _sc
306 484 _m
258 484 220 465 192 427 _c
164 389 150 338 150 273 _c
150 207 163 156 191 118 _c
219 80 257 62 306 62 _c
354 62 392 80 420 118 _c
448 156 462 207 462 273 _c
462 337 448 389 420 427 _c
392 465 354 484 306 484 _c
306 560 _m
384 560 445 534 490 484 _c
534 433 557 363 557 273 _c
557 183 534 113 490 63 _c
445 12 384 -13 306 -13 _c
227 -13 165 12 121 63 _c
77 113 55 183 55 273 _c
55 363 77 433 121 484 _c
165 534 227 560 306 560 _c
_cl}_d
/p{{635 0 91 -207 580 560 _sc
181 82 _m
181 -207 _l
91 -207 _l
91 547 _l
181 547 _l
181 464 _l
199 496 223 520 252 536 _c
281 552 316 560 356 560 _c
422 560 476 533 518 481 _c
559 428 580 359 580 273 _c
580 187 559 117 518 65 _c
476 13 422 -13 356 -13 _c
316 -13 281 -5 252 10 _c
223 25 199 49 181 82 _c
487 273 _m
487 339 473 390 446 428 _c
418 466 381 485 334 485 _c
}_e{286 485 249 466 222 428 _c
194 390 181 339 181 273 _c
181 207 194 155 222 117 _c
249 79 286 61 334 61 _c
381 61 418 79 446 117 _c
473 155 487 207 487 273 _c
_cl}_e}_d
end readonly def
/BuildGlyph
{exch begin
CharStrings exch
2 copy known not{pop /.notdef}if
true 3 1 roll get exec
end}_d
/BuildChar {
1 index /Encoding get exch get
1 index /BuildGlyph get exec
}_d
FontName currentdict end definefont pop
end
%%EndProlog
mpldict begin
90 252 translate
432 288 0 0 clipbox
gsave
0 0 m
432 0 l
432 288 l
0 288 l
cl
1.000 setgray
fill
grestore
gsave
12.904646 12.094646 m
429.165354 12.094646 l
429.165354 269.805354 l
12.904646 269.805354 l
cl
1.000 setgray
fill
grestore
0.500 setlinewidth
1 setlinejoin
2 setlinecap
[] 0 setdash
0.000 setgray
gsave
416.3 257.7 12.9 12.09 clipbox
20.718955 12.094646 m
20.718955 269.805354 l
stroke
grestore
0.800 setlinewidth
0 setlinecap
gsave
/o {
gsave
newpath
translate
0.8 setlinewidth
1 setlinejoin
0 setlinecap
0 0 m
0 3.5 l
gsave
0.000 setgray
fill
grestore
stroke
grestore
} bind def
20.719 12.0946 o
grestore
/DejaVuSans findfont
8.000 scalefont
setfont
gsave
14.351767 2.516521 translate
0.000000 rotate
0.000000 0.000000 m /zero glyphshow
5.089844 0.000000 m /period glyphshow
7.632812 0.000000 m /zero glyphshow
grestore
0.500 setlinewidth
2 setlinecap
gsave
416.3 257.7 12.9 12.09 clipbox
119.885314 12.094646 m
119.885314 269.805354 l
stroke
grestore
0.800 setlinewidth
0 setlinecap
gsave
/o {
gsave
newpath
translate
0.8 setlinewidth
1 setlinejoin
0 setlinecap
0 0 m
0 3.5 l
gsave
0.000 setgray
fill
grestore
stroke
grestore
} bind def
119.885 12.0946 o
grestore
gsave
113.518126 2.516521 translate
0.000000 rotate
0.000000 0.000000 m /two glyphshow
5.089844 0.000000 m /period glyphshow
7.632812 0.000000 m /five glyphshow
grestore
0.500 setlinewidth
2 setlinecap
gsave
416.3 257.7 12.9 12.09 clipbox
219.051673 12.094646 m
219.051673 269.805354 l
stroke
grestore
0.800 setlinewidth
0 setlinecap
gsave
/o {
gsave
newpath
translate
0.8 setlinewidth
1 setlinejoin
0 setlinecap
0 0 m
0 3.5 l
gsave
0.000 setgray
fill
grestore
stroke
grestore
} bind def
219.052 12.0946 o
grestore
gsave
212.684485 2.516521 translate
0.000000 rotate
0.000000 0.000000 m /five glyphshow
5.089844 0.000000 m /period glyphshow
7.632812 0.000000 m /zero glyphshow
grestore
0.500 setlinewidth
2 setlinecap
gsave
416.3 257.7 12.9 12.09 clipbox
318.218032 12.094646 m
318.218032 269.805354 l
stroke
grestore
0.800 setlinewidth
0 setlinecap
gsave
/o {
gsave
newpath
translate
0.8 setlinewidth
1 setlinejoin
0 setlinecap
0 0 m
0 3.5 l
gsave
0.000 setgray
fill
grestore
stroke
grestore
} bind def
318.218 12.0946 o
grestore
gsave
311.850844 2.516521 translate
0.000000 rotate
0.000000 0.000000 m /seven glyphshow
5.089844 0.000000 m /period glyphshow
7.632812 0.000000 m /five glyphshow
grestore
0.500 setlinewidth
2 setlinecap
gsave
416.3 257.7 12.9 12.09 clipbox
417.384391 12.094646 m
417.384391 269.805354 l
stroke
grestore
0.800 setlinewidth
0 setlinecap
gsave
/o {
gsave
newpath
translate
0.8 setlinewidth
1 setlinejoin
0 setlinecap
0 0 m
0 3.5 l
gsave
0.000 setgray
fill
grestore
stroke
grestore
} bind def
417.384 12.0946 o
grestore
gsave
408.470328 2.516521 translate
0.000000 rotate
0.000000 0.000000 m /one glyphshow
5.089844 0.000000 m /zero glyphshow
10.179688 0.000000 m /period glyphshow
12.722656 0.000000 m /zero glyphshow
grestore
0.500 setlinewidth
2 setlinecap
gsave
416.3 257.7 12.9 12.09 clipbox
12.904646 40.948075 m
429.165354 40.948075 l
stroke
grestore
0.800 setlinewidth
0 setlinecap
gsave
/o {
gsave
newpath
translate
0.8 setlinewidth
1 setlinejoin
0 setlinecap
0 0 m
3.5 0 l
gsave
0.000 setgray
fill
grestore
stroke
grestore
} bind def
12.9046 40.9481 o
grestore
gsave
1.420271 37.909013 translate
0.000000 rotate
0.000000 0.000000 m /hyphen glyphshow
2.886719 0.000000 m /two glyphshow
grestore
0.500 setlinewidth
2 setlinecap
gsave
416.3 257.7 12.9 12.09 clipbox
12.904646 112.156268 m
429.165354 112.156268 l
stroke
grestore
0.800 setlinewidth
0 setlinecap
gsave
/o {
gsave
newpath
translate
0.8 setlinewidth
1 setlinejoin
0 setlinecap
0 0 m
3.5 0 l
gsave
0.000 setgray
fill
grestore
stroke
grestore
} bind def
12.9046 112.156 o
grestore
gsave
1.420271 109.117206 translate
0.000000 rotate
0.000000 0.000000 m /hyphen glyphshow
2.886719 0.000000 m /one glyphshow
grestore
0.500 setlinewidth
2 setlinecap
gsave
416.3 257.7 12.9 12.09 clipbox
12.904646 183.364461 m
429.165354 183.364461 l
stroke
grestore
0.800 setlinewidth
0 setlinecap
gsave
/o {
gsave
newpath
translate
0.8 setlinewidth
1 setlinejoin
0 setlinecap
0 0 m
3.5 0 l
gsave
0.000 setgray
fill
grestore
stroke
grestore
} bind def
12.9046 183.364 o
grestore
gsave
4.310896 180.325399 translate
0.000000 rotate
0.000000 0.000000 m /zero glyphshow
grestore
0.500 setlinewidth
2 setlinecap
gsave
416.3 257.7 12.9 12.09 clipbox
12.904646 254.572654 m
429.165354 254.572654 l
stroke
grestore
0.800 setlinewidth
0 setlinecap
gsave
/o {
gsave
newpath
translate
0.8 setlinewidth
1 setlinejoin
0 setlinecap
0 0 m
3.5 0 l
gsave
0.000 setgray
fill
grestore
stroke
grestore
} bind def
12.9046 254.573 o
grestore
gsave
4.310896 251.533591 translate
0.000000 rotate
0.000000 0.000000 m /one glyphshow
grestore
1.000 setlinewidth
1 setlinecap
0.000 0.606 0.979 setrgbcolor
gsave
416.3 257.7 12.9 12.09 clipbox
24.685609 19.388345 m
25.718458 35.859266 l
26.876096 50.679253 l
28.128322 63.847956 l
29.431256 75.365421 l
30.963311 86.876143 l
32.489218 96.735178 l
34.24241 106.585189 l
35.901302 114.784578 l
37.763687 122.973554 l
39.854527 131.149402 l
42.201845 139.308692 l
44.28538 145.821334 l
46.570988 152.317909 l
49.078268 158.795135 l
51.828719 165.249056 l
54.845924 171.674899 l
58.155756 178.066908 l
60.847137 182.834584 l
63.732005 187.575899 l
66.82427 192.286876 l
70.138842 196.96293 l
73.691702 201.59877 l
77.499983 206.188289 l
81.582046 210.724431 l
85.957574 215.199037 l
90.647665 219.602666 l
95.674932 223.924376 l
101.063616 228.151473 l
106.839699 232.269206 l
110.919316 234.945236 l
115.192189 237.559313 l
119.66747 240.105032 l
124.354751 242.57528 l
129.264071 244.962148 l
134.405951 247.256825 l
139.791406 249.449484 l
145.431975 251.529141 l
151.339743 253.483496 l
157.527367 255.298739 l
164.008104 256.959329 l
170.79584 258.44772 l
177.905116 259.744039 l
185.351166 260.825685 l
193.149942 261.666846 l
201.318152 262.237891 l
209.873299 262.504604 l
218.83371 262.427208 l
228.218584 261.959094 l
238.048027 261.045144 l
243.136006 260.400887 l
248.343101 259.619462 l
253.672101 258.690435 l
259.125861 257.602242 l
264.707301 256.342007 l
270.41941 254.895329 l
276.265248 253.246008 l
282.247945 251.375721 l
288.370705 249.263604 l
294.636808 246.885724 l
301.049609 244.214408 l
307.612543 241.217362 l
314.329124 237.856506 l
321.20295 234.08641 l
328.237701 229.852141 l
335.437146 225.08624 l
342.80514 219.70436 l
350.34563 213.598789 l
358.062652 206.628469 l
365.960341 198.602959 l
374.042926 189.255314 l
382.314736 178.19311 l
390.7802 164.802377 l
399.443852 148.036701 l
408.310333 125.873203 l
417.384391 93.496705 l
417.384391 93.496705 l
stroke
grestore
0 setlinejoin
2 setlinecap
[] 0 setdash
0.000 setgray
gsave
12.904646 12.094646 m
12.904646 269.805354 l
stroke
grestore
gsave
12.904646 12.094646 m
429.165354 12.094646 l
stroke
grestore
/DejaVuSans findfont
14.000 scalefont
setfont
gsave
205.917812 275.805354 translate
0.000000 rotate
0.000000 0.000000 m /p glyphshow
8.886719 0.000000 m /l glyphshow
12.776367 0.000000 m /o glyphshow
21.341797 0.000000 m /p glyphshow
grestore
0 setlinecap
gsave
380.184104 251.655354 m
423.565354 251.655354 l
424.632021 251.655354 425.165354 252.188688 425.165354 253.255354 c
425.165354 264.205354 l
425.165354 265.272021 424.632021 265.805354 423.565354 265.805354 c
380.184104 265.805354 l
379.117438 265.805354 378.584104 265.272021 378.584104 264.205354 c
378.584104 253.255354 l
378.584104 252.188688 379.117438 251.655354 380.184104 251.655354 c
cl
gsave
1.000 setgray
fill
grestore
stroke
grestore
1 setlinejoin
2 setlinecap
[] 0 setdash
0.000 0.606 0.979 setrgbcolor
gsave
381.784104 259.327229 m
397.784104 259.327229 l
stroke
grestore
0.000 setgray
/DejaVuSans findfont
8.000 scalefont
setfont
gsave
404.184104 256.527229 translate
0.000000 rotate
0.000000 0.000000 m /m glyphshow
7.792969 0.000000 m /a glyphshow
12.695312 0.000000 m /g glyphshow
grestore
end
showpage

47
utils/test_filter_bode.jl Normal file
View file

@ -0,0 +1,47 @@
using Plots
using DSP
using CSV
pyplot()
# Parameters
filename = joinpath(@__DIR__, "data", "walk.csv") # input file
h = 1/22 # sample time
orders = [
(title="quadratique", order=2, sizes=5:2:11),
(title="cubique", order=3, sizes=5:2:11),
(title="big_quadra", order=2, sizes=11:10:51)
]
function savgol(size::Int64, poly_order::Int64, deriv::Int64=0, delta::Float64=1.0, conv::Bool=false)
half_size, rem = divrem(size, 2)
if rem == 0
throw(ArgumentError("size must be odd."))
end
M = [-half_size:half_size;] .^ [0:poly_order;]';
y = zeros(poly_order+1)';
y[deriv+1] = factorial(deriv) / delta^deriv;
scal = y*inv(M'*M)*M'
if conv
scal = scal[end:-1:1]
end
scal
end
filter = savgol(19, 2, 1, h, true)
H = tf(filter, [1], h)
mag, phase, w = bode(H)
plot(w,log.(mag[:,1]), label="mag", title="plop")
savefig(joinpath(@__DIR__, "results", string("savgol_bode_", "plop", ".eps")))
show()
# for order in orders
# for size in order.sizes
# filter = savgol(size, order.order, 1, h, true)
# H = tf(filter, [1], 0.05)
# bode(H)
# end
# savefig(joinpath(@__DIR__, "results", string("savgol_bode_", order.title, ".eps")))
# end
# show()