disturbance torque
" + }, + { + "id": 140269547877472, + "text": "velocity feedforward", + "pos_x": -270.0, + "pos_y": -40.0, + "width": 133, + "height": 24, + "fill_color": [ + 255, + 255, + 255, + 255 + ], + "styling": "\n\nvelocity feedforward
" + } + ], + "grouping_boxes": [] +} \ No newline at end of file diff --git a/RVC3/models/ploop_test.py b/RVC3/models/ploop_test.py new file mode 100755 index 0000000..6dd735d --- /dev/null +++ b/RVC3/models/ploop_test.py @@ -0,0 +1,51 @@ +#! /usr/bin/env python + +""" +Creates Fig 9.13 +Robotics, Vision & Control for Python, P. Corke, Springer 2023. +Copyright (c) 2021- Peter Corke +""" + +import site +import numpy as np +import bdsim + +from roboticstoolbox import quintic_func + +site.addsitedir("bdsim") + +from ploop import ploop, G + +# test harness +sim = bdsim.BDSim() +bd = sim.blockdiagram() + +# position = bd.quintic(0) HACK + +quinticfunc = quintic_func(0, 1, 1) +trajectory = bd.FUNCTION(quinticfunc, nout=3, name="quintic") +time = bd.TIME() + +taud = bd.CONSTANT(20 / G) +# speed = bd.WAVEFORM(wave='triangle', freq=2, amplitude=25) +PLOOP = bd.SUBSYSTEM(ploop, name="PLOOP") +theta_scope = bd.SCOPE(nin=2, name=r"$\theta$", labels=["actual", "demand"]) +werr_scope = bd.SCOPE(name=r"$\omega$ error") +tau_scope = bd.SCOPE(name=r"$\tau$") +wff_scope = bd.SCOPE(name=r"$\omega_{ff}$") + +bd.connect(time, trajectory) +bd.connect(trajectory[0], PLOOP[0], theta_scope[1]) +bd.connect(trajectory[1], PLOOP[1], wff_scope) +bd.connect(taud, PLOOP[2]) +bd.connect(PLOOP[0], theta_scope[0]) + +bd.connect(PLOOP[2], werr_scope) +bd.connect(PLOOP[3], tau_scope) + +bd.compile() # check the diagram + + +if __name__ == "__main__": + sim.report(bd) + out = sim.run(bd, 1, dt=1e-3) diff --git a/RVC3/models/vloop_test.bd b/RVC3/models/vloop_test.bd new file mode 100644 index 0000000..1dc5dfa --- /dev/null +++ b/RVC3/models/vloop_test.bd @@ -0,0 +1,625 @@ +{ + "id": 140192552973888, + "created_by": "corkep", + "creation_time": 1632727013, + "scene_width": 7168.000000000001, + "scene_height": 4368.0, + "blocks": [ + { + "id": 140588467244768, + "block_type": "SUBSYSTEM", + "title": "velocity loop", + "pos_x": -160.0, + "pos_y": -120.0, + "width": 200, + "height": 150, + "flipped": false, + "inputsNum": 3, + "outputsNum": 4, + "inputs": [ + { + "id": 140588467291760, + "index": 0, + "multi_wire": true, + "position": 1, + "socket_type": 1 + }, + { + "id": 140588467322208, + "index": 1, + "multi_wire": true, + "position": 1, + "socket_type": 1 + }, + { + "id": 140588467322688, + "index": 2, + "multi_wire": true, + "position": 1, + "socket_type": 1 + } + ], + "outputs": [ + { + "id": 140588467291808, + "index": 0, + "multi_wire": true, + "position": 3, + "socket_type": 2 + }, + { + "id": 140588467322448, + "index": 1, + "multi_wire": true, + "position": 3, + "socket_type": 2 + }, + { + "id": 140588467322640, + "index": 2, + "multi_wire": true, + "position": 3, + "socket_type": 2 + }, + { + "id": 140588473625424, + "index": 3, + "multi_wire": true, + "position": 3, + "socket_type": 2 + } + ], + "parameters": [ + [ + "subsys", + null + ], + [ + "nin", + 3 + ], + [ + "nout", + 4 + ], + [ + "blockargs", + null + ], + [ + "inport labels", + null + ], + [ + "outport labels", + null + ] + ] + }, + { + "id": 140588473655152, + "block_type": "INTERPOLATE", + "title": "velocity profile", + "pos_x": -380.0, + "pos_y": -120.0, + "width": 100, + "height": 100, + "flipped": false, + "inputsNum": 1, + "outputsNum": 1, + "inputs": [ + { + "id": 140588473655248, + "index": 0, + "multi_wire": true, + "position": 1, + "socket_type": 1 + } + ], + "outputs": [ + { + "id": 140588473655200, + "index": 0, + "multi_wire": true, + "position": 3, + "socket_type": 2 + } + ], + "parameters": [ + [ + "x", + null + ], + [ + "y", + null + ], + [ + "xy", + null + ], + [ + "time", + false + ], + [ + "kind", + "linear" + ], + [ + "blockargs", + null + ] + ] + }, + { + "id": 140588473690960, + "block_type": "SCOPE", + "title": "\u03c9", + "pos_x": 160.0, + "pos_y": -240.0, + "width": 100, + "height": 100, + "flipped": false, + "inputsNum": 2, + "outputsNum": 0, + "inputs": [ + { + "id": 140588473691056, + "index": 0, + "multi_wire": true, + "position": 1, + "socket_type": 1 + }, + { + "id": 140588473717424, + "index": 1, + "multi_wire": true, + "position": 1, + "socket_type": 1 + } + ], + "outputs": [], + "parameters": [ + [ + "nin", + 2 + ], + [ + "styles", + "None" + ], + [ + "scale", + "auto" + ], + [ + "labels", + [ + "w*", + "w" + ] + ], + [ + "grid", + "true" + ], + [ + "labels", + null + ], + [ + "grid", + true + ], + [ + "watch", + false + ], + [ + "blockargs", + null + ] + ] + }, + { + "id": 140588473691200, + "block_type": "SCOPE", + "title": "\u03c9 error", + "pos_x": 160.0, + "pos_y": -100.0, + "width": 100, + "height": 100, + "flipped": false, + "inputsNum": 1, + "outputsNum": 0, + "inputs": [ + { + "id": 140588473691488, + "index": 0, + "multi_wire": true, + "position": 1, + "socket_type": 1 + } + ], + "outputs": [], + "parameters": [ + [ + "nin", + 1 + ], + [ + "styles", + "None" + ], + [ + "scale", + "auto" + ], + [ + "labels", + "none" + ], + [ + "grid", + "true" + ], + [ + "labels", + null + ], + [ + "grid", + true + ], + [ + "watch", + false + ], + [ + "blockargs", + null + ] + ] + }, + { + "id": 140588473691632, + "block_type": "SCOPE", + "title": "motor torque", + "pos_x": 160.0, + "pos_y": 40.0, + "width": 100, + "height": 100, + "flipped": false, + "inputsNum": 1, + "outputsNum": 0, + "inputs": [ + { + "id": 140588473691920, + "index": 0, + "multi_wire": true, + "position": 1, + "socket_type": 1 + } + ], + "outputs": [], + "parameters": [ + [ + "nin", + 1 + ], + [ + "styles", + null + ], + [ + "scale", + "auto" + ], + [ + "labels", + null + ], + [ + "grid", + true + ], + [ + "labels", + null + ], + [ + "grid", + true + ], + [ + "watch", + false + ], + [ + "blockargs", + null + ] + ] + }, + { + "id": 140588473692064, + "block_type": "SCOPE", + "title": "integral term", + "pos_x": 160.0, + "pos_y": 180.0, + "width": 100, + "height": 100, + "flipped": false, + "inputsNum": 1, + "outputsNum": 0, + "inputs": [ + { + "id": 140588473688128, + "index": 0, + "multi_wire": true, + "position": 1, + "socket_type": 1 + } + ], + "outputs": [], + "parameters": [ + [ + "nin", + 1 + ], + [ + "styles", + null + ], + [ + "scale", + "auto" + ], + [ + "labels", + null + ], + [ + "grid", + true + ], + [ + "labels", + null + ], + [ + "grid", + true + ], + [ + "watch", + false + ], + [ + "blockargs", + null + ] + ] + }, + { + "id": 140588473720064, + "block_type": "CONSTANT", + "title": "disturbance torque", + "pos_x": -380.0, + "pos_y": 20.0, + "width": 100, + "height": 100, + "flipped": false, + "inputsNum": 0, + "outputsNum": 1, + "inputs": [], + "outputs": [ + { + "id": 140588473719776, + "index": 0, + "multi_wire": true, + "position": 3, + "socket_type": 2 + } + ], + "parameters": [ + [ + "value", + null + ], + [ + "blockargs", + null + ] + ] + }, + { + "id": 140588473719200, + "block_type": "CONSTANT", + "title": "feedforward torque", + "pos_x": -380.0, + "pos_y": 160.0, + "width": 100, + "height": 100, + "flipped": false, + "inputsNum": 0, + "outputsNum": 1, + "inputs": [], + "outputs": [ + { + "id": 140588473719440, + "index": 0, + "multi_wire": true, + "position": 3, + "socket_type": 2 + } + ], + "parameters": [ + [ + "value", + null + ], + [ + "blockargs", + null + ] + ] + }, + { + "id": 140237887245040, + "block_type": "TIME", + "title": "time", + "pos_x": -520.0, + "pos_y": -120.0, + "width": 100, + "height": 100, + "flipped": false, + "inputsNum": 0, + "outputsNum": 1, + "inputs": [], + "outputs": [ + { + "id": 140237887245136, + "index": 0, + "multi_wire": true, + "position": 3, + "socket_type": 2 + } + ], + "parameters": [ + [ + "blockargs", + null + ] + ] + } + ], + "wires": [ + { + "id": 140588473688992, + "start_socket": 140588473655200, + "end_socket": 140588467291760, + "wire_type": 3, + "custom_routing": false, + "wire_coordinates": [] + }, + { + "id": 140588473718960, + "start_socket": 140588467291808, + "end_socket": 140588473717424, + "wire_type": 3, + "custom_routing": false, + "wire_coordinates": [] + }, + { + "id": 140588473719296, + "start_socket": 140588467322448, + "end_socket": 140588473691488, + "wire_type": 3, + "custom_routing": false, + "wire_coordinates": [] + }, + { + "id": 140588473719392, + "start_socket": 140588467322640, + "end_socket": 140588473691920, + "wire_type": 3, + "custom_routing": false, + "wire_coordinates": [] + }, + { + "id": 140588473718624, + "start_socket": 140588473719776, + "end_socket": 140588467322208, + "wire_type": 3, + "custom_routing": false, + "wire_coordinates": [] + }, + { + "id": 140237887272704, + "start_socket": 140237887245136, + "end_socket": 140588473655248, + "wire_type": 3, + "custom_routing": false, + "wire_coordinates": [] + }, + { + "id": 140192616650400, + "start_socket": 140588473655200, + "end_socket": 140588473691056, + "wire_type": 3, + "custom_routing": true, + "wire_coordinates": [ + [ + 160.0, + -200.0 + ], + [ + -220.0, + -200.0 + ], + [ + -220.0, + -80.0 + ], + [ + -280.0, + -80.0 + ] + ] + }, + { + "id": 140192616650544, + "start_socket": 140588473625424, + "end_socket": 140588473688128, + "wire_type": 3, + "custom_routing": true, + "wire_coordinates": [ + [ + 160.0, + 220.0 + ], + [ + 80.0, + 220.0 + ], + [ + 80.0, + -20.0 + ], + [ + 40.0, + -20.0 + ] + ] + }, + { + "id": 140192616650688, + "start_socket": 140588473719440, + "end_socket": 140588467322688, + "wire_type": 3, + "custom_routing": true, + "wire_coordinates": [ + [ + -160.0, + -40.0 + ], + [ + -200.0, + -40.0 + ], + [ + -200.0, + 200.0 + ], + [ + -280.0, + 200.0 + ] + ] + } + ], + "labels": [], + "grouping_boxes": [] +} \ No newline at end of file diff --git a/RVC3/models/vloop_test.py b/RVC3/models/vloop_test.py new file mode 100755 index 0000000..ecd06a7 --- /dev/null +++ b/RVC3/models/vloop_test.py @@ -0,0 +1,58 @@ +#! /usr/bin/env python + +""" +Creates Fig 9.8 +Robotics, Vision & Control for Python, P. Corke, Springer 2023. +Copyright (c) 2021- Peter Corke +""" + +import numpy as np +import bdsim + +import site +import numpy as np +import bdsim + +from vloop import vloop, B + + +# test harness for velocity loop +sim = bdsim.BDSim(graphics=True, progress=False) +bd = sim.blockdiagram() + +# parameters +disturbance = bd.CONSTANT(0, name="disturbance") +feedforward = bd.CONSTANT(0) +# speed = bd.WAVEFORM(wave='triangle', freq=2, amplitude=25) +speed = bd.INTERPOLATE( + x=(0, 0.1, 0.3, 0.4, 0.6, 1), y=(0, 0, 50, 50, 0, 0), time=True, name="demand" +) + +# import the velocity loop +VLOOP = bd.SUBSYSTEM(vloop, name="VLOOP") + +# scopes +w_scope = bd.SCOPE(nin=2, name=r"$\omega$", labels=["actual", "demand"]) +werr_scope = bd.SCOPE(name=r"$\omega_{err}$") +tau_scope = bd.SCOPE(name=r"$\tau$") +integral_scope = bd.SCOPE(name="integral") + +demand_scope = bd.SCOPE(name="demand scope") + +bd.connect(speed, demand_scope) + +bd.connect(VLOOP[0], w_scope[0]) +bd.connect(VLOOP[1], werr_scope) +bd.connect(VLOOP[2], tau_scope) +bd.connect(VLOOP[3], integral_scope) + +bd.connect(disturbance, VLOOP[1]) +bd.connect(speed, VLOOP[0], w_scope[1]) +bd.connect(feedforward, VLOOP[2]) + +bd.compile() # check the diagram + +if __name__ == "__main__": + sim.report(bd) + + out = sim.run(bd, 1, dt=1e-3)