diff --git a/CMakeLists.txt b/CMakeLists.txt index 6623bdf..526dcfc 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -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 ) ################################### diff --git a/cfg/RateNode.cfg b/cfg/RateNode.cfg old mode 100644 new mode 100755 diff --git a/cfg/Triangle.cfg b/cfg/Triangle.cfg new file mode 100755 index 0000000..22e0c8a --- /dev/null +++ b/cfg/Triangle.cfg @@ -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")) diff --git a/launch/internal_tun.launch b/launch/internal_tun.launch index 0a71f6a..7419f96 100644 --- a/launch/internal_tun.launch +++ b/launch/internal_tun.launch @@ -1,6 +1,12 @@ + + + + + + @@ -13,25 +19,22 @@ - - + + + + + - + + + + + - - - - - - - - - - @@ -41,7 +44,11 @@ - + + + + + diff --git a/launch/test_derivative.launch b/launch/test_derivative.launch index 69bc493..ba9975d 100644 --- a/launch/test_derivative.launch +++ b/launch/test_derivative.launch @@ -1,7 +1,14 @@ + + - + + + + + + diff --git a/params/settings_blue.yaml b/params/settings_blue.yaml new file mode 100644 index 0000000..c9335a4 --- /dev/null +++ b/params/settings_blue.yaml @@ -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: [] diff --git a/params/settings_safe.yaml b/params/settings_safe.yaml new file mode 100644 index 0000000..afcfbcd --- /dev/null +++ b/params/settings_safe.yaml @@ -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: [] diff --git a/params/settings_targets.yaml b/params/settings_targets.yaml new file mode 100644 index 0000000..26fb602 --- /dev/null +++ b/params/settings_targets.yaml @@ -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: [] diff --git a/params/settings_test_D.yaml b/params/settings_test_D.yaml new file mode 100644 index 0000000..74af80d --- /dev/null +++ b/params/settings_test_D.yaml @@ -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: [] diff --git a/params/settings_test_D_rate.yaml b/params/settings_test_D_rate.yaml new file mode 100644 index 0000000..f7c767a --- /dev/null +++ b/params/settings_test_D_rate.yaml @@ -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: [] diff --git a/params/settings_yellow.yaml b/params/settings_yellow.yaml new file mode 100644 index 0000000..7444265 --- /dev/null +++ b/params/settings_yellow.yaml @@ -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: [] diff --git a/scripts/control_compute.py b/scripts/control_compute.py index 182774e..e5b4645 100755 --- a/scripts/control_compute.py +++ b/scripts/control_compute.py @@ -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 SaturateNodeConfig, RateNodeConfig +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() diff --git a/scripts/publish_csv.py b/scripts/publish_csv.py index e047e27..f40a701 100755 --- a/scripts/publish_csv.py +++ b/scripts/publish_csv.py @@ -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) diff --git a/scripts/triangle.py b/scripts/triangle.py index 19e11f8..25d5176 100755 --- a/scripts/triangle.py +++ b/scripts/triangle.py @@ -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() diff --git a/utils/data/repind_vitesse.bag b/utils/data/repind_vitesse.bag new file mode 100644 index 0000000..9df5057 Binary files /dev/null and b/utils/data/repind_vitesse.bag differ diff --git a/utils/results/graph_test_d.png b/utils/results/graph_test_d.png new file mode 100644 index 0000000..4dd1906 Binary files /dev/null and b/utils/results/graph_test_d.png differ diff --git a/utils/results/mesure_vitesse_big_quadra.eps b/utils/results/mesure_vitesse_big_quadra.eps index d90b7db..32437ae 100644 --- a/utils/results/mesure_vitesse_big_quadra.eps +++ b/utils/results/mesure_vitesse_big_quadra.eps @@ -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 diff --git a/utils/results/mesure_vitesse_bode_big_quadra_11.eps b/utils/results/mesure_vitesse_bode_big_quadra_11.eps new file mode 100644 index 0000000..e69de29 diff --git a/utils/results/mesure_vitesse_cubique.eps b/utils/results/mesure_vitesse_cubique.eps index fdb5833..c0dbec8 100644 --- a/utils/results/mesure_vitesse_cubique.eps +++ b/utils/results/mesure_vitesse_cubique.eps @@ -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 diff --git a/utils/results/mesure_vitesse_quadratique.eps b/utils/results/mesure_vitesse_quadratique.eps index b0878f6..83ff25f 100644 --- a/utils/results/mesure_vitesse_quadratique.eps +++ b/utils/results/mesure_vitesse_quadratique.eps @@ -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 diff --git a/utils/results/savgol_bode_plop.eps b/utils/results/savgol_bode_plop.eps new file mode 100644 index 0000000..183f602 --- /dev/null +++ b/utils/results/savgol_bode_plop.eps @@ -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 diff --git a/utils/results/savgol_bode_quadratique.eps b/utils/results/savgol_bode_quadratique.eps new file mode 100644 index 0000000..e69de29 diff --git a/utils/test_filter_bode.jl b/utils/test_filter_bode.jl new file mode 100644 index 0000000..302bd0b --- /dev/null +++ b/utils/test_filter_bode.jl @@ -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()