From a40f232721d5fe52a1ebadf0d295ee9cf5267f70 Mon Sep 17 00:00:00 2001 From: Dong-Jae Park Date: Tue, 22 Dec 2020 18:30:18 -0600 Subject: [PATCH 01/63] Initial commit --- .gitattributes | 2 ++ README.md | 2 ++ 2 files changed, 4 insertions(+) create mode 100644 .gitattributes create mode 100644 README.md diff --git a/.gitattributes b/.gitattributes new file mode 100644 index 0000000..dfe0770 --- /dev/null +++ b/.gitattributes @@ -0,0 +1,2 @@ +# Auto detect text files and perform LF normalization +* text=auto diff --git a/README.md b/README.md new file mode 100644 index 0000000..3eef796 --- /dev/null +++ b/README.md @@ -0,0 +1,2 @@ +# TableUV + Simulation and software for TableUV From 371ef57b17c682a76c51d2b69cf90fed2afca7e9 Mon Sep 17 00:00:00 2001 From: Dong-Jae Park Date: Tue, 22 Dec 2020 18:32:48 -0600 Subject: [PATCH 02/63] Initial Webots simulation. --- .../controllers/first_controller/Initialize.m | 38 ++++ .../controllers/first_controller/Parameters.m | 9 + .../controllers/first_controller/Supervisor.m | 22 +++ .../first_controller/first_controller.m | 62 ++++++ .../controllers/my_controller/my_controller.m | 32 ++++ simulation/worlds/.desk.wbproj | 16 ++ simulation/worlds/desk.wbt | 179 ++++++++++++++++++ 7 files changed, 358 insertions(+) create mode 100644 simulation/controllers/first_controller/Initialize.m create mode 100644 simulation/controllers/first_controller/Parameters.m create mode 100644 simulation/controllers/first_controller/Supervisor.m create mode 100644 simulation/controllers/first_controller/first_controller.m create mode 100644 simulation/controllers/my_controller/my_controller.m create mode 100644 simulation/worlds/.desk.wbproj create mode 100644 simulation/worlds/desk.wbt diff --git a/simulation/controllers/first_controller/Initialize.m b/simulation/controllers/first_controller/Initialize.m new file mode 100644 index 0000000..56ddbb7 --- /dev/null +++ b/simulation/controllers/first_controller/Initialize.m @@ -0,0 +1,38 @@ +wb_console_print('Initializing...', WB_STDOUT); + +% LEFT, RIGHT MOTORS +wb_console_print('Checking Motors', WB_STDOUT); +motor = []; +motor_names = [ "left_motor", "right_motor" ]; +for i = 1:2 + motor(i) = wb_robot_get_device(convertStringsToChars(motor_names(i))); + wb_motor_set_position(motor(i), inf); + wb_motor_set_velocity(motor(i), 0.0); +end +wb_console_print('DONE', WB_STDOUT); + +% LEFT, RIGHT ENCODERS +wb_console_print('Checking Encoders', WB_STDOUT); +ps = []; +ps_names = [ "left_ps", "right_ps" ]; +for i = 1:2 + ps(i) = wb_robot_get_device(convertStringsToChars(ps_names(i))); + wb_position_sensor_enable(ps(i), TIME_STEP); +end +wb_console_print('DONE', WB_STDOUT); + +% GLOBAL GPS +wb_console_print('Checking GPS', WB_STDOUT); +gps_name = "global"; +gps = wb_robot_get_device(convertStringsToChars(gps_name)); +wb_gps_enable(gps, TIME_STEP) +wb_console_print('DONE', WB_STDOUT); + +% RANGE FINDER (TOF) +wb_console_print('Checking TOF', WB_STDOUT); +tof_name = "tof"; +tof = wb_robot_get_device(convertStringsToChars(tof_name)); +wb_range_finder_enable(tof, TIME_STEP); +wb_console_print('DONE', WB_STDOUT); + +wb_console_print('Initialization Complete.', WB_STDOUT); \ No newline at end of file diff --git a/simulation/controllers/first_controller/Parameters.m b/simulation/controllers/first_controller/Parameters.m new file mode 100644 index 0000000..9f175ee --- /dev/null +++ b/simulation/controllers/first_controller/Parameters.m @@ -0,0 +1,9 @@ +TIME_STEP = 64; +MAX_SPEED = 6.28; + +ROBOT_NAME = 'TableUV'; +ROBOT_BASE_RADIUS = 0.03; +WHEEL_RADIUS = 0.017; + +TABLE_WIDTH = 1.22; +TABLE_HEIGHT = 0.76; \ No newline at end of file diff --git a/simulation/controllers/first_controller/Supervisor.m b/simulation/controllers/first_controller/Supervisor.m new file mode 100644 index 0000000..1c2bced --- /dev/null +++ b/simulation/controllers/first_controller/Supervisor.m @@ -0,0 +1,22 @@ +% setting the table +table = wb_supervisor_node_get_from_def('TABLE'); +table_size = wb_supervisor_node_get_field(table, 'size'); +size = [TABLE_WIDTH 0.76 TABLE_HEIGHT]; +wb_supervisor_field_set_sf_vec3f(table_size, size); + +% robot +robot_node = wb_supervisor_node_get_from_def(ROBOT_NAME); +if robot_node == 0 + wb_console_print('No DEF MY_ROBOT node found in the current world file', WB_STDOUT); + quit(1); +end +trans_field = wb_supervisor_node_get_field(robot_node, 'translation'); + +robot_left_wheel = wb_supervisor_node_get_from_def('ROBOT_WHEEL_LEFT'); +left_wheel_rad = wb_supervisor_node_get_field(robot_left_wheel, 'radius'); +wb_supervisor_field_set_sf_float(left_wheel_rad, WHEEL_RADIUS); + +robot_right_wheel = wb_supervisor_node_get_from_def('ROBOT_WHEEL_RIGHT'); +right_wheel_rad = wb_supervisor_node_get_field(robot_right_wheel, 'radius'); +wb_supervisor_field_set_sf_float(right_wheel_rad, WHEEL_RADIUS); + diff --git a/simulation/controllers/first_controller/first_controller.m b/simulation/controllers/first_controller/first_controller.m new file mode 100644 index 0000000..905ca34 --- /dev/null +++ b/simulation/controllers/first_controller/first_controller.m @@ -0,0 +1,62 @@ +% uncomment the next two lines if you want to use +% MATLAB's desktop to interact with the controller: +desktop; +keyboard; + +% PARAMETERS +Parameters; +% DEVICE INITIALIZATION +Initialize; +% SUPERVISOR +Supervisor; + +% MAIN LOOP +wb_console_print('Starting main loop!', WB_STDOUT); + +step = 0; +samples = 0; + +while wb_robot_step(TIME_STEP) ~= -1 + step = step + 1; + + % setting motor speeds + left_speed = 0.5 * MAX_SPEED; + right_speed = 0.5 * MAX_SPEED; + for i=1:2 + if i == 1 + wb_motor_set_velocity(motor(i), left_speed); + end + if i == 2 + wb_motor_set_velocity(motor(i), right_speed); + end + end + + % getting global robot position with GPS + x_y_z_array = wb_gps_get_values(gps); + p(step,:) = x_y_z_array; + % plotting position of robot on a map + plot(p(step,1),-p(step,3),'ro'); + hold on; + axis([-1 1 -0.8 0.8]); + rectangle('Position',[-TABLE_WIDTH/2 -TABLE_HEIGHT/2 TABLE_WIDTH TABLE_HEIGHT]) + hold off; + + % getting tof sensor readings + image = wb_range_finder_get_range_image(tof); + + % getting encoder values + value = wb_position_sensor_get_value(ps(1)); + type = wb_position_sensor_get_type(ps(1)); + + %% SUPERVISOR + % this is done repeatedly + values = wb_supervisor_field_get_sf_vec3f(trans_field); + wb_console_print(sprintf('MY_ROBOT is at position: %g %g %g\n', values(1), values(2), values(3)), WB_STDOUT); + + % if your code plots some graphics, it needs to flushed like this: + drawnow; +end + +%% CLEAN UP +% cleanup code goes here: write data to files, etc. + diff --git a/simulation/controllers/my_controller/my_controller.m b/simulation/controllers/my_controller/my_controller.m new file mode 100644 index 0000000..8bbbf61 --- /dev/null +++ b/simulation/controllers/my_controller/my_controller.m @@ -0,0 +1,32 @@ +% uncomment the next two lines if you want to use +% MATLAB's desktop to interact with the controller: +desktop; +keyboard; + +TIME_STEP = 64; + +% get and enable devices, e.g.: +% camera = wb_robot_get_device('camera'); +% wb_camera_enable(camera, TIME_STEP); +motor = wb_robot_get_device('motor'); + +% main loop: +% perform simulation steps of TIME_STEP milliseconds +% and leave the loop when Webots signals the termination +% +while wb_robot_step(TIME_STEP) ~= -1 + + % read the sensors, e.g.: + % rgb = wb_camera_get_image(camera); + + % Process here sensor data, images, etc. + + % send actuator commands, e.g.: + wb_motor_set_postion(motor, 10.0); + + % if your code plots some graphics, it needs to flushed like this: + drawnow; + +end + +% cleanup code goes here: write data to files, etc. diff --git a/simulation/worlds/.desk.wbproj b/simulation/worlds/.desk.wbproj new file mode 100644 index 0000000..8e35111 --- /dev/null +++ b/simulation/worlds/.desk.wbproj @@ -0,0 +1,16 @@ +Webots Project File version R2020b +perspectives: 000000ff00000000fd000000030000000000000320000003dafc0100000004fc00000000ffffffff0000000000fffffffc0200000001fb00000012005300630065006e0065005400720065006501000000000000039f0000000000000000fb0000001e00480074006d006c0052006f0062006f007400570069006e0064006f00770000000000000003200000000000000000fb0000001e00480074006d006c0052006f0062006f007400570069006e0064006f00770000000000000003200000000000000000fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000006900ffffff00000001000003290000033afc0200000001fb0000001400540065007800740045006400690074006f007200000000150000033a0000008700ffffff000000030000078000000039fc0100000002fb0000000e0043006f006e0073006f006c006501000000000000073f0000000000000000fb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c0100000000000007800000006900ffffff000007800000039100000004000000040000000100000008fc00000000 +simulationViewPerspectives: 000000ff0000000100000002000002730000050b0100000002010000000101 +sceneTreePerspectives: 000000ff0000000100000002000000c0000001120100000002010000000201 +maximizedDockId: -1 +centralWidgetVisible: 1 +projectionMode: PERSPECTIVE +renderingMode: PLAIN +orthographicViewHeight: 1.17306 +textFiles: -1 +globalOptionalRendering: CoordinateSystem::RangeFinderFrustums +consoles: Console:All:All +renderingDevicePerspectives: e-puck:camera;1;1;0;0 +renderingDevicePerspectives: robot(1):e-puck:camera;1;1;0;0 +renderingDevicePerspectives: robot:range-finder;1;3.57813;0;0 +renderingDevicePerspectives: robot:tof;1;3.57813;0;0 diff --git a/simulation/worlds/desk.wbt b/simulation/worlds/desk.wbt new file mode 100644 index 0000000..9dab1f5 --- /dev/null +++ b/simulation/worlds/desk.wbt @@ -0,0 +1,179 @@ +#VRML_SIM R2020b utf8 +WorldInfo { + coordinateSystem "NUE" +} +Viewpoint { + orientation 1 0 0 4.71238898038469 + position 0.5799996859678199 1.3479919680422097 -0.032519647818219095 + follow "robot" +} +TexturedBackground { +} +TexturedBackgroundLight { +} +DEF OBJECTS Group { + children [ + Floor { + name "floor(1)" + } + BeerBottle { + translation 3.68702e-18 0.760422 -0.07 + name "beer bottle(1)" + } + DEF TABLE Table { + size 1.22 0.76 0.76 + frameThickness 0.1 + } + Spoon { + translation -0.14 0.777648 -0.139205 + rotation -0.01851782694315772 0.9996570309244369 0.018517845680389593 -1.571138342843346 + name "spoon(1)" + } + Knife { + translation 0.0385655 0.758308 0.189585 + rotation -0.0029417597513266072 0.9999829154692547 -0.00505121957300935 2.09441 + name "knife(1)" + } + Plate { + translation -0.29 0.759941 0.18 + rotation 0.9989264613518823 0.010627846543856588 -0.04508850949905507 6.926359812943209e-16 + name "plate(1)" + } + Apple { + translation -0.46 0.809765 -0.01 + name "apple(1)" + } + ] +} +DEF TableUV Robot { + translation 0.579997 0.786936 0.0074477 + rotation 0.0016202905968792038 0.9999973683769037 -0.0016241605983048266 -1.5708653071795862 + children [ + RangeFinder { + translation 0 0 0.02 + rotation 0 1 0 3.14159 + name "tof" + fieldOfView 0.471239 + maxRange 4 + } + GPS { + name "global" + } + Solid { + translation 0 -0.017 0.02 + children [ + DEF CASTER_WHEEL Shape { + appearance PBRAppearance { + metalness 0 + } + geometry Sphere { + radius 0.01 + } + } + ] + name "caster_wheel" + boundingObject USE CASTER_WHEEL + physics Physics { + } + } + Solid { + translation 0 -0.017 -0.02 + children [ + DEF CASTER_WHEEL Shape { + appearance PBRAppearance { + metalness 0 + } + geometry Sphere { + radius 0.01 + } + } + ] + name "caster_wheel(1)" + boundingObject USE CASTER_WHEEL + physics Physics { + } + } + HingeJoint { + jointParameters HingeJointParameters { + position 7.414501687456142e-08 + anchor -0.04 -0.01 0 + } + device [ + PositionSensor { + name "left_ps" + resolution 1 + } + RotationalMotor { + name "left_motor" + } + ] + endPoint Solid { + translation -0.04000000040198491 -0.009998878983036004 5.847401393102292e-09 + rotation 1.546138617612343e-08 -1.518241940524984e-08 0.9999999999999998 1.5699999530302227 + children [ + DEF WHEEL Shape { + appearance PBRAppearance { + metalness 0 + } + geometry DEF ROBOT_WHEEL_R Cylinder { + height 0.006 + radius 0.017 + } + } + ] + name "solid(1)" + boundingObject USE WHEEL + physics DEF WHEEL_PHYS Physics { + } + } + } + HingeJoint { + jointParameters HingeJointParameters { + position 3.187095446534079e-08 + anchor 0.04 -0.01 0 + } + device [ + PositionSensor { + name "right_ps" + resolution 1 + } + RotationalMotor { + name "right_motor" + } + ] + endPoint Solid { + translation 0.040000000533529825 -0.009998885232130555 4.164318302510613e-09 + rotation 9.142245450039296e-10 -1.1821754757087804e-09 1 1.57000004697403 + children [ + DEF WHEEL Shape { + appearance PBRAppearance { + metalness 0 + } + geometry DEF ROBOT_WHEEL_LEFT Cylinder { + height 0.006 + radius 0.017 + } + } + ] + boundingObject USE WHEEL + physics DEF WHEEL_PHYS Physics { + } + } + } + DEF BODY Shape { + appearance PBRAppearance { + baseColor 1 0 0 + metalness 0 + } + geometry DEF ROBOT_BASE Cylinder { + height 0.035 + radius 0.03 + } + } + ] + boundingObject USE BODY + physics Physics { + } + controller "first_controller" + supervisor TRUE +} From b159082a91f8fd18813cfbaac497c882d37939fe Mon Sep 17 00:00:00 2001 From: Dong-Jae Park Date: Thu, 24 Dec 2020 16:27:30 -0600 Subject: [PATCH 03/63] Added Autonomous Mode and Manual Mode Controllers --- .../first_controller/Mode_Autonomous.m | 49 +++++++++++++++++++ .../first_controller/Mode_Manual.m | 42 ++++++++++++++++ 2 files changed, 91 insertions(+) create mode 100644 simulation/controllers/first_controller/Mode_Autonomous.m create mode 100644 simulation/controllers/first_controller/Mode_Manual.m diff --git a/simulation/controllers/first_controller/Mode_Autonomous.m b/simulation/controllers/first_controller/Mode_Autonomous.m new file mode 100644 index 0000000..02485d1 --- /dev/null +++ b/simulation/controllers/first_controller/Mode_Autonomous.m @@ -0,0 +1,49 @@ +% MAIN LOOP +wb_console_print('Starting Autonomous Mode!', WB_STDOUT); + +step = 0; +samples = 0; + +while wb_robot_step(TIME_STEP) ~= -1 + step = step + 1; + + % setting motor speeds + left_speed = 0.5 * MAX_SPEED; + right_speed = 0.5 * MAX_SPEED; + for i=1:2 + if i == 1 + wb_motor_set_velocity(motor(i), left_speed); + end + if i == 2 + wb_motor_set_velocity(motor(i), right_speed); + end + end + + % getting global robot position with GPS + x_y_z_array = wb_gps_get_values(gps); + p(step,:) = x_y_z_array; + % plotting position of robot on a map + plot(p(step,1),-p(step,3),'ro'); + hold on; + axis([-1 1 -0.8 0.8]); + rectangle('Position',[-TABLE_WIDTH/2 -TABLE_HEIGHT/2 TABLE_WIDTH TABLE_HEIGHT]) + hold off; + + % getting tof sensor readings + image = wb_range_finder_get_range_image(tof); + + % getting encoder values + value = wb_position_sensor_get_value(ps(1)); + type = wb_position_sensor_get_type(ps(1)); + + %% SUPERVISOR + % this is done repeatedly + values = wb_supervisor_field_get_sf_vec3f(trans_field); + wb_console_print(sprintf('MY_ROBOT is at position: %g %g %g\n', values(1), values(2), values(3)), WB_STDOUT); + + % if your code plots some graphics, it needs to flushed like this: + drawnow; +end + +%% CLEAN UP +% cleanup code goes here: write data to files, etc. \ No newline at end of file diff --git a/simulation/controllers/first_controller/Mode_Manual.m b/simulation/controllers/first_controller/Mode_Manual.m new file mode 100644 index 0000000..3ef2eed --- /dev/null +++ b/simulation/controllers/first_controller/Mode_Manual.m @@ -0,0 +1,42 @@ +% MAIN LOOP +wb_console_print('Starting Manual Mode!', WB_STDOUT); + +step = 0; +samples = 0; + +while wb_robot_step(TIME_STEP) ~= -1 + input = wb_keyboard_get_key(); + + switch input + case 314 + % LEFT + left_speed = 1.0; + right_speed = -1.0; + case 315 + % UP + left_speed = 1.0; + right_speed = 1.0; + case 316 + % RIGHT + left_speed = -1.0; + right_speed = 1.0; + case 317 + % DOWN + left_speed = -1.0; + right_speed = -1.0; + otherwise + % NOTHING + left_speed = 0; + right_speed = 0; + end + + % set the velocity of the motors + wb_motor_set_velocity(motor(1), left_speed); + wb_motor_set_velocity(motor(2), right_speed); + + % if your code plots some graphics, it needs to flushed like this: + drawnow; +end + +%% CLEAN UP +% cleanup code goes here: write data to files, etc. \ No newline at end of file From ca417056edfe5c067c4bac30f63a01d31c2f5874 Mon Sep 17 00:00:00 2001 From: Dong-Jae Park Date: Thu, 24 Dec 2020 16:27:48 -0600 Subject: [PATCH 04/63] Switch statement to choose controller --- .../first_controller/first_controller.m | 66 ++++++------------- 1 file changed, 20 insertions(+), 46 deletions(-) diff --git a/simulation/controllers/first_controller/first_controller.m b/simulation/controllers/first_controller/first_controller.m index 905ca34..1fa72f8 100644 --- a/simulation/controllers/first_controller/first_controller.m +++ b/simulation/controllers/first_controller/first_controller.m @@ -10,53 +10,27 @@ % SUPERVISOR Supervisor; -% MAIN LOOP -wb_console_print('Starting main loop!', WB_STDOUT); - -step = 0; -samples = 0; +% GET INPUT +wb_console_print(sprintf(['Select controller type:\n', ... + '1 - Autonomous\n', ... + '2 - Manual Control\n', ... + '3 - Exit\n' + ]), WB_STDOUT); while wb_robot_step(TIME_STEP) ~= -1 - step = step + 1; - - % setting motor speeds - left_speed = 0.5 * MAX_SPEED; - right_speed = 0.5 * MAX_SPEED; - for i=1:2 - if i == 1 - wb_motor_set_velocity(motor(i), left_speed); - end - if i == 2 - wb_motor_set_velocity(motor(i), right_speed); - end + input = wb_keyboard_get_key(); + switch input + case 49 + Mode_Autonomous + case 50 + Mode_Manual + case 51 + wb_supervisor_simulation_set_mode(WB_SUPERVISOR_SIMULATION_MODE_PAUSE) + wb_console_print('Paused. Press Ctrl+Shift+T to reset.', WB_STDOUT); + return + case -1 + % do nothing + otherwise + wb_console_print('Invalid Input.', WB_STDOUT); end - - % getting global robot position with GPS - x_y_z_array = wb_gps_get_values(gps); - p(step,:) = x_y_z_array; - % plotting position of robot on a map - plot(p(step,1),-p(step,3),'ro'); - hold on; - axis([-1 1 -0.8 0.8]); - rectangle('Position',[-TABLE_WIDTH/2 -TABLE_HEIGHT/2 TABLE_WIDTH TABLE_HEIGHT]) - hold off; - - % getting tof sensor readings - image = wb_range_finder_get_range_image(tof); - - % getting encoder values - value = wb_position_sensor_get_value(ps(1)); - type = wb_position_sensor_get_type(ps(1)); - - %% SUPERVISOR - % this is done repeatedly - values = wb_supervisor_field_get_sf_vec3f(trans_field); - wb_console_print(sprintf('MY_ROBOT is at position: %g %g %g\n', values(1), values(2), values(3)), WB_STDOUT); - - % if your code plots some graphics, it needs to flushed like this: - drawnow; end - -%% CLEAN UP -% cleanup code goes here: write data to files, etc. - From 5bbb1c5aac3d8218c6423aeca3543f1e051e703b Mon Sep 17 00:00:00 2001 From: Dong-Jae Park Date: Thu, 24 Dec 2020 16:28:22 -0600 Subject: [PATCH 05/63] reformat code and added keyboard --- .../first_controller/Initialize.asv | 43 +++++++++++++++++++ .../controllers/first_controller/Initialize.m | 17 +++++--- 2 files changed, 54 insertions(+), 6 deletions(-) create mode 100644 simulation/controllers/first_controller/Initialize.asv diff --git a/simulation/controllers/first_controller/Initialize.asv b/simulation/controllers/first_controller/Initialize.asv new file mode 100644 index 0000000..7cc0c60 --- /dev/null +++ b/simulation/controllers/first_controller/Initialize.asv @@ -0,0 +1,43 @@ +wb_console_print(sprintf('Initializing...\n'), WB_STDOUT); + +% LEFT, RIGHT MOTORS +wb_console_print(sprintf('Checking Motors... '), WB_STDOUT); +motor = []; +motor_names = [ "left_motor", "right_motor" ]; +for i = 1:2 + motor(i) = wb_robot_get_device(convertStringsToChars(motor_names(i))); + wb_motor_set_position(motor(i), inf); + wb_motor_set_velocity(motor(i), 0.0); +end +wb_console_print('DONE', WB_STDOUT); + +% LEFT, RIGHT ENCODERS +wb_console_print(sprintf('Checking Encoders... '), WB_STDOUT); +ps = []; +ps_names = [ "left_ps", "right_ps" ]; +for i = 1:2 + ps(i) = wb_robot_get_device(convertStringsToChars(ps_names(i))); + wb_position_sensor_enable(ps(i), TIME_STEP); +end +wb_console_print('DONE', WB_STDOUT); + +% GLOBAL GPS +wb_console_print(sprintf('Checking GPS... '), WB_STDOUT); +gps_name = "global"; +gps = wb_robot_get_device(convertStringsToChars(gps_name)); +wb_gps_enable(gps, TIME_STEP) +wb_console_print('DONE', WB_STDOUT); + +% RANGE FINDER (TOF) +wb_console_print('Checking TOF', WB_STDOUT); +tof_name = "tof"; +tof = wb_robot_get_device(convertStringsToChars(tof_name)); +wb_range_finder_enable(tof, TIME_STEP); +wb_console_print('DONE', WB_STDOUT); + +% KEYBOARD INPUT +wb_console_print('Checking Keyboard', WB_STDOUT); +wb_keyboard_enable(TIME_STEP) +wb_console_print('DONE', WB_STDOUT); + +wb_console_print('Initialization Complete.\n', WB_STDOUT); \ No newline at end of file diff --git a/simulation/controllers/first_controller/Initialize.m b/simulation/controllers/first_controller/Initialize.m index 56ddbb7..bbdfeb3 100644 --- a/simulation/controllers/first_controller/Initialize.m +++ b/simulation/controllers/first_controller/Initialize.m @@ -1,7 +1,7 @@ -wb_console_print('Initializing...', WB_STDOUT); +wb_console_print(sprintf('Initializing...\n'), WB_STDOUT); % LEFT, RIGHT MOTORS -wb_console_print('Checking Motors', WB_STDOUT); +wb_console_print(sprintf('Checking Motors'), WB_STDOUT); motor = []; motor_names = [ "left_motor", "right_motor" ]; for i = 1:2 @@ -12,7 +12,7 @@ wb_console_print('DONE', WB_STDOUT); % LEFT, RIGHT ENCODERS -wb_console_print('Checking Encoders', WB_STDOUT); +wb_console_print(sprintf('Checking Encoders'), WB_STDOUT); ps = []; ps_names = [ "left_ps", "right_ps" ]; for i = 1:2 @@ -22,17 +22,22 @@ wb_console_print('DONE', WB_STDOUT); % GLOBAL GPS -wb_console_print('Checking GPS', WB_STDOUT); +wb_console_print(sprintf('Checking GPS'), WB_STDOUT); gps_name = "global"; gps = wb_robot_get_device(convertStringsToChars(gps_name)); wb_gps_enable(gps, TIME_STEP) wb_console_print('DONE', WB_STDOUT); % RANGE FINDER (TOF) -wb_console_print('Checking TOF', WB_STDOUT); +wb_console_print(sprintf('Checking TOF'), WB_STDOUT); tof_name = "tof"; tof = wb_robot_get_device(convertStringsToChars(tof_name)); wb_range_finder_enable(tof, TIME_STEP); wb_console_print('DONE', WB_STDOUT); -wb_console_print('Initialization Complete.', WB_STDOUT); \ No newline at end of file +% KEYBOARD INPUT +wb_console_print(sprintf('Checking Keyboard'), WB_STDOUT); +wb_keyboard_enable(TIME_STEP) +wb_console_print('DONE', WB_STDOUT); + +wb_console_print(sprintf('Initialization Complete.\n'), WB_STDOUT); \ No newline at end of file From 70bf158e9ec84f46fa0d5392f6a5c521feb3e466 Mon Sep 17 00:00:00 2001 From: Dong-Jae Park Date: Thu, 24 Dec 2020 16:28:36 -0600 Subject: [PATCH 06/63] World file --- simulation/worlds/.desk.wbproj | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/simulation/worlds/.desk.wbproj b/simulation/worlds/.desk.wbproj index 8e35111..70746c0 100644 --- a/simulation/worlds/.desk.wbproj +++ b/simulation/worlds/.desk.wbproj @@ -1,5 +1,5 @@ Webots Project File version R2020b -perspectives: 000000ff00000000fd000000030000000000000320000003dafc0100000004fc00000000ffffffff0000000000fffffffc0200000001fb00000012005300630065006e0065005400720065006501000000000000039f0000000000000000fb0000001e00480074006d006c0052006f0062006f007400570069006e0064006f00770000000000000003200000000000000000fb0000001e00480074006d006c0052006f0062006f007400570069006e0064006f00770000000000000003200000000000000000fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000006900ffffff00000001000003290000033afc0200000001fb0000001400540065007800740045006400690074006f007200000000150000033a0000008700ffffff000000030000078000000039fc0100000002fb0000000e0043006f006e0073006f006c006501000000000000073f0000000000000000fb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c0100000000000007800000006900ffffff000007800000039100000004000000040000000100000008fc00000000 +perspectives: 000000ff00000000fd000000030000000000000320000003dafc0100000004fc00000000ffffffff0000000000fffffffc0200000001fb00000012005300630065006e0065005400720065006501000000000000039f0000000000000000fb0000001e00480074006d006c0052006f0062006f007400570069006e0064006f00770000000000000003200000000000000000fb0000001e00480074006d006c0052006f0062006f007400570069006e0064006f00770000000000000003200000000000000000fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000006900ffffff00000001000003290000033afc0200000001fb0000001400540065007800740045006400690074006f007200000000150000033a0000004100ffffff000000030000078000000154fc0100000002fb0000000e0043006f006e0073006f006c006501000000000000073f0000000000000000fb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c0100000000000007800000006900ffffff000007800000026c00000004000000040000000100000008fc00000000 simulationViewPerspectives: 000000ff0000000100000002000002730000050b0100000002010000000101 sceneTreePerspectives: 000000ff0000000100000002000000c0000001120100000002010000000201 maximizedDockId: -1 From 54cabc89a5181671b5fa8c42be4509a8c873d6f9 Mon Sep 17 00:00:00 2001 From: Dong-Jae Park Date: Mon, 28 Dec 2020 18:15:13 -0600 Subject: [PATCH 07/63] Updating .gitignore --- .gitignore | 2 ++ 1 file changed, 2 insertions(+) create mode 100644 .gitignore diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..27e6232 --- /dev/null +++ b/.gitignore @@ -0,0 +1,2 @@ + +*.asv From ae36eb25defa16f9c541520d655d7d162d431af2 Mon Sep 17 00:00:00 2001 From: Dong-Jae Park Date: Mon, 28 Dec 2020 18:16:34 -0600 Subject: [PATCH 08/63] Added IMU (Gyroscope) sensor Tested output --- .../controllers/first_controller/Initialize.m | 10 +++++++ .../first_controller/Mode_Autonomous.m | 22 +++++++++++----- .../first_controller/Mode_Manual.m | 26 +++++++++++++++++++ .../controllers/first_controller/Parameters.m | 8 +++++- simulation/worlds/.desk.wbproj | 7 ++--- simulation/worlds/desk.wbt | 14 ++++++---- 6 files changed, 71 insertions(+), 16 deletions(-) diff --git a/simulation/controllers/first_controller/Initialize.m b/simulation/controllers/first_controller/Initialize.m index bbdfeb3..ecef81b 100644 --- a/simulation/controllers/first_controller/Initialize.m +++ b/simulation/controllers/first_controller/Initialize.m @@ -35,6 +35,16 @@ wb_range_finder_enable(tof, TIME_STEP); wb_console_print('DONE', WB_STDOUT); +% IMU, Gyroscope +wb_console_print(sprintf('Checking IMU (Gyro)'), WB_STDOUT); +imu_gyro_name = "imu_gyro"; +imu_gyro = wb_robot_get_device(convertStringsToChars(imu_gyro_name)); +wb_inertial_unit_enable(imu_gyro, TIME_STEP); +wb_console_print('DONE', WB_STDOUT); + +% IMU, Accelerometer + + % KEYBOARD INPUT wb_console_print(sprintf('Checking Keyboard'), WB_STDOUT); wb_keyboard_enable(TIME_STEP) diff --git a/simulation/controllers/first_controller/Mode_Autonomous.m b/simulation/controllers/first_controller/Mode_Autonomous.m index 02485d1..3cffec5 100644 --- a/simulation/controllers/first_controller/Mode_Autonomous.m +++ b/simulation/controllers/first_controller/Mode_Autonomous.m @@ -18,8 +18,12 @@ wb_motor_set_velocity(motor(i), right_speed); end end - - % getting global robot position with GPS + + %% ENCODER + value = wb_position_sensor_get_value(ps(1)); + type = wb_position_sensor_get_type(ps(1)); + + %% GPS x_y_z_array = wb_gps_get_values(gps); p(step,:) = x_y_z_array; % plotting position of robot on a map @@ -29,19 +33,23 @@ rectangle('Position',[-TABLE_WIDTH/2 -TABLE_HEIGHT/2 TABLE_WIDTH TABLE_HEIGHT]) hold off; - % getting tof sensor readings + %% TOF image = wb_range_finder_get_range_image(tof); - % getting encoder values - value = wb_position_sensor_get_value(ps(1)); - type = wb_position_sensor_get_type(ps(1)); + %% IMU + % IMU, Gyro + roll_pitch_yaw_array = wb_inertial_unit_get_roll_pitch_yaw(imu_gyro); + roll_pitch_yaw_array + + % IMU, Acce + %% SUPERVISOR % this is done repeatedly values = wb_supervisor_field_get_sf_vec3f(trans_field); wb_console_print(sprintf('MY_ROBOT is at position: %g %g %g\n', values(1), values(2), values(3)), WB_STDOUT); - % if your code plots some graphics, it needs to flushed like this: + %% if your code plots some graphics, it needs to flushed like this: drawnow; end diff --git a/simulation/controllers/first_controller/Mode_Manual.m b/simulation/controllers/first_controller/Mode_Manual.m index 3ef2eed..36af6fa 100644 --- a/simulation/controllers/first_controller/Mode_Manual.m +++ b/simulation/controllers/first_controller/Mode_Manual.m @@ -5,6 +5,8 @@ samples = 0; while wb_robot_step(TIME_STEP) ~= -1 + step = step + 1; + input = wb_keyboard_get_key(); switch input @@ -33,6 +35,30 @@ % set the velocity of the motors wb_motor_set_velocity(motor(1), left_speed); wb_motor_set_velocity(motor(2), right_speed); + + %% ENCODER + value = wb_position_sensor_get_value(ps(1)); + type = wb_position_sensor_get_type(ps(1)); + + %% GPS + x_y_z_array = wb_gps_get_values(gps); + p(step,:) = x_y_z_array; + % plotting position of robot on a map + plot(p(step,1),-p(step,3),'ro'); + hold on; + axis([-1 1 -0.8 0.8]); + rectangle('Position',[-TABLE_WIDTH/2 -TABLE_HEIGHT/2 TABLE_WIDTH TABLE_HEIGHT]) + hold off; + + %% TOF + image = wb_range_finder_get_range_image(tof); + + %% IMU + % IMU, Gyro + roll_pitch_yaw_array = wb_inertial_unit_get_roll_pitch_yaw(imu_gyro); + roll_pitch_yaw_array + + % IMU, Acce % if your code plots some graphics, it needs to flushed like this: drawnow; diff --git a/simulation/controllers/first_controller/Parameters.m b/simulation/controllers/first_controller/Parameters.m index 9f175ee..40a71a3 100644 --- a/simulation/controllers/first_controller/Parameters.m +++ b/simulation/controllers/first_controller/Parameters.m @@ -1,9 +1,15 @@ TIME_STEP = 64; MAX_SPEED = 6.28; +%% ROBOT PARAMETERS ROBOT_NAME = 'TableUV'; ROBOT_BASE_RADIUS = 0.03; WHEEL_RADIUS = 0.017; TABLE_WIDTH = 1.22; -TABLE_HEIGHT = 0.76; \ No newline at end of file +TABLE_HEIGHT = 0.76; + +%% SENSOR PARAMETERS +% IMU_GYRO +IMU_GYRO_RESOLUTION = 0; +IMU_GYRO_NOISE = 0; \ No newline at end of file diff --git a/simulation/worlds/.desk.wbproj b/simulation/worlds/.desk.wbproj index 70746c0..2c46f3f 100644 --- a/simulation/worlds/.desk.wbproj +++ b/simulation/worlds/.desk.wbproj @@ -1,12 +1,13 @@ Webots Project File version R2020b -perspectives: 000000ff00000000fd000000030000000000000320000003dafc0100000004fc00000000ffffffff0000000000fffffffc0200000001fb00000012005300630065006e0065005400720065006501000000000000039f0000000000000000fb0000001e00480074006d006c0052006f0062006f007400570069006e0064006f00770000000000000003200000000000000000fb0000001e00480074006d006c0052006f0062006f007400570069006e0064006f00770000000000000003200000000000000000fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000006900ffffff00000001000003290000033afc0200000001fb0000001400540065007800740045006400690074006f007200000000150000033a0000004100ffffff000000030000078000000154fc0100000002fb0000000e0043006f006e0073006f006c006501000000000000073f0000000000000000fb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c0100000000000007800000006900ffffff000007800000026c00000004000000040000000100000008fc00000000 -simulationViewPerspectives: 000000ff0000000100000002000002730000050b0100000002010000000101 +perspectives: 000000ff00000000fd000000030000000000000320000003dafc0100000004fc00000000ffffffff0000000000fffffffc0200000001fb00000012005300630065006e0065005400720065006501000000000000039f0000000000000000fb0000001e00480074006d006c0052006f0062006f007400570069006e0064006f00770000000000000003200000000000000000fb0000001e00480074006d006c0052006f0062006f007400570069006e0064006f00770000000000000003200000000000000000fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000006900ffffff00000001000003290000033afc0200000001fb0000001400540065007800740045006400690074006f007200000000150000033a0000004100ffffff000000030000078000000157fc0100000002fb0000000e0043006f006e0073006f006c006501000000000000073f0000000000000000fb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c0100000000000007800000006900ffffff000007800000026900000004000000040000000100000008fc00000000 +simulationViewPerspectives: 000000ff00000001000000020000021f0000055f0100000002010000000101 sceneTreePerspectives: 000000ff0000000100000002000000c0000001120100000002010000000201 +minimizedPerspectives: 000000ff00000000fd000000030000000000000320000003dafc0100000004fc00000000ffffffff0000000000fffffffc0200000001fb00000012005300630065006e0065005400720065006501000000000000039f0000000000000000fb0000001e00480074006d006c0052006f0062006f007400570069006e0064006f00770000000000000003200000000000000000fb0000001e00480074006d006c0052006f0062006f007400570069006e0064006f00770000000000000003200000000000000000fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000006900ffffff00000001000003290000033afc0200000001fb0000001400540065007800740045006400690074006f007200000000150000033a0000004100ffffff000000030000078000000123fc0100000002fb0000000e0043006f006e0073006f006c006501000000000000073f0000000000000000fb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c0100000000000007800000006900ffffff000007800000029d00000004000000040000000100000008fc00000000 maximizedDockId: -1 centralWidgetVisible: 1 projectionMode: PERSPECTIVE renderingMode: PLAIN -orthographicViewHeight: 1.17306 +orthographicViewHeight: 0.974191 textFiles: -1 globalOptionalRendering: CoordinateSystem::RangeFinderFrustums consoles: Console:All:All diff --git a/simulation/worlds/desk.wbt b/simulation/worlds/desk.wbt index 9dab1f5..4364eb2 100644 --- a/simulation/worlds/desk.wbt +++ b/simulation/worlds/desk.wbt @@ -3,8 +3,8 @@ WorldInfo { coordinateSystem "NUE" } Viewpoint { - orientation 1 0 0 4.71238898038469 - position 0.5799996859678199 1.3479919680422097 -0.032519647818219095 + orientation 0.7194490476403281 -0.6523419535133591 -0.23841779198667037 5.343131840159701 + position 0.8140701092654996 1.2056702161982353 0.41895044203045995 follow "robot" } TexturedBackground { @@ -49,6 +49,10 @@ DEF TableUV Robot { translation 0.579997 0.786936 0.0074477 rotation 0.0016202905968792038 0.9999973683769037 -0.0016241605983048266 -1.5708653071795862 children [ + InertialUnit { + rotation 0 1 0 -1.5708 + name "imu_gyro" + } RangeFinder { translation 0 0 0.02 rotation 0 1 0 3.14159 @@ -108,8 +112,8 @@ DEF TableUV Robot { } ] endPoint Solid { - translation -0.04000000040198491 -0.009998878983036004 5.847401393102292e-09 - rotation 1.546138617612343e-08 -1.518241940524984e-08 0.9999999999999998 1.5699999530302227 + translation -0.04000000040198491 -0.009998878983036004 5.847401393102309e-09 + rotation 1.5461386176123583e-08 -1.518241940524984e-08 0.9999999999999998 1.5699999530302227 children [ DEF WHEEL Shape { appearance PBRAppearance { @@ -142,7 +146,7 @@ DEF TableUV Robot { } ] endPoint Solid { - translation 0.040000000533529825 -0.009998885232130555 4.164318302510613e-09 + translation 0.040000000533529825 -0.009998885232130555 4.1643183025106295e-09 rotation 9.142245450039296e-10 -1.1821754757087804e-09 1 1.57000004697403 children [ DEF WHEEL Shape { From c0c75ba226850ac91cd8106b09b297139758443d Mon Sep 17 00:00:00 2001 From: Dong-Jae Park Date: Mon, 28 Dec 2020 18:35:23 -0600 Subject: [PATCH 09/63] Added IMU (Accelerometer) sensor --- .../controllers/first_controller/Initialize.m | 10 +++++++--- .../controllers/first_controller/Mode_Manual.m | 4 ++-- .../controllers/first_controller/Parameters.m | 15 ++++++++++++--- 3 files changed, 21 insertions(+), 8 deletions(-) diff --git a/simulation/controllers/first_controller/Initialize.m b/simulation/controllers/first_controller/Initialize.m index ecef81b..2ccbff7 100644 --- a/simulation/controllers/first_controller/Initialize.m +++ b/simulation/controllers/first_controller/Initialize.m @@ -35,15 +35,19 @@ wb_range_finder_enable(tof, TIME_STEP); wb_console_print('DONE', WB_STDOUT); -% IMU, Gyroscope +% IMU, GYRO wb_console_print(sprintf('Checking IMU (Gyro)'), WB_STDOUT); imu_gyro_name = "imu_gyro"; imu_gyro = wb_robot_get_device(convertStringsToChars(imu_gyro_name)); wb_inertial_unit_enable(imu_gyro, TIME_STEP); wb_console_print('DONE', WB_STDOUT); -% IMU, Accelerometer - +% IMU, ACC +wb_console_print(sprintf('Checking IMU (Acc)'), WB_STDOUT); +imu_acc_name = "imu_acc"; +imu_acc = wb_robot_get_device(convertStringsToChars(imu_acc_name)); +wb_accelerometer_enable(imu_acc, TIME_STEP); +wb_console_print('DONE', WB_STDOUT); % KEYBOARD INPUT wb_console_print(sprintf('Checking Keyboard'), WB_STDOUT); diff --git a/simulation/controllers/first_controller/Mode_Manual.m b/simulation/controllers/first_controller/Mode_Manual.m index 36af6fa..ee11948 100644 --- a/simulation/controllers/first_controller/Mode_Manual.m +++ b/simulation/controllers/first_controller/Mode_Manual.m @@ -55,10 +55,10 @@ %% IMU % IMU, Gyro - roll_pitch_yaw_array = wb_inertial_unit_get_roll_pitch_yaw(imu_gyro); - roll_pitch_yaw_array + roll_pitch_yaw_array = wb_inertial_unit_get_roll_pitch_yaw(imu_gyro) % IMU, Acce + x_y_z_array = wb_accelerometer_get_values(imu_acc) % if your code plots some graphics, it needs to flushed like this: drawnow; diff --git a/simulation/controllers/first_controller/Parameters.m b/simulation/controllers/first_controller/Parameters.m index 40a71a3..1ef6d7a 100644 --- a/simulation/controllers/first_controller/Parameters.m +++ b/simulation/controllers/first_controller/Parameters.m @@ -1,5 +1,5 @@ +%% SIMULATION PARAMETERS TIME_STEP = 64; -MAX_SPEED = 6.28; %% ROBOT PARAMETERS ROBOT_NAME = 'TableUV'; @@ -9,7 +9,16 @@ TABLE_WIDTH = 1.22; TABLE_HEIGHT = 0.76; +MAX_SPEED = 6.28; + %% SENSOR PARAMETERS +% ENCODER +ENC_RESOLUTION = -1; +ENC_NOISE = -1; + % IMU_GYRO -IMU_GYRO_RESOLUTION = 0; -IMU_GYRO_NOISE = 0; \ No newline at end of file +IMU_GYRO_RESOLUTION = -1; +IMU_GYRO_NOISE = -1; + +% IMU_ACC +IMU_ACC_RESOLUTION = -1; \ No newline at end of file From 857f32f47aaf31c7b5fb580a50c8877582754e15 Mon Sep 17 00:00:00 2001 From: Dong-Jae Park Date: Mon, 28 Dec 2020 22:40:56 -0600 Subject: [PATCH 10/63] Reformatting Initializer code --- .../controllers/first_controller/Initialize.m | 14 ++++---- .../controllers/first_controller/Supervisor.m | 35 +++++++++++++++++-- 2 files changed, 39 insertions(+), 10 deletions(-) diff --git a/simulation/controllers/first_controller/Initialize.m b/simulation/controllers/first_controller/Initialize.m index 2ccbff7..af9f9c8 100644 --- a/simulation/controllers/first_controller/Initialize.m +++ b/simulation/controllers/first_controller/Initialize.m @@ -1,6 +1,6 @@ wb_console_print(sprintf('Initializing...\n'), WB_STDOUT); -% LEFT, RIGHT MOTORS +%% LEFT, RIGHT MOTORS wb_console_print(sprintf('Checking Motors'), WB_STDOUT); motor = []; motor_names = [ "left_motor", "right_motor" ]; @@ -11,7 +11,7 @@ end wb_console_print('DONE', WB_STDOUT); -% LEFT, RIGHT ENCODERS +%% LEFT, RIGHT ENCODERS wb_console_print(sprintf('Checking Encoders'), WB_STDOUT); ps = []; ps_names = [ "left_ps", "right_ps" ]; @@ -21,35 +21,35 @@ end wb_console_print('DONE', WB_STDOUT); -% GLOBAL GPS +%% GLOBAL GPS wb_console_print(sprintf('Checking GPS'), WB_STDOUT); gps_name = "global"; gps = wb_robot_get_device(convertStringsToChars(gps_name)); wb_gps_enable(gps, TIME_STEP) wb_console_print('DONE', WB_STDOUT); -% RANGE FINDER (TOF) +%% RANGE FINDER (TOF) wb_console_print(sprintf('Checking TOF'), WB_STDOUT); tof_name = "tof"; tof = wb_robot_get_device(convertStringsToChars(tof_name)); wb_range_finder_enable(tof, TIME_STEP); wb_console_print('DONE', WB_STDOUT); -% IMU, GYRO +%% IMU, GYRO wb_console_print(sprintf('Checking IMU (Gyro)'), WB_STDOUT); imu_gyro_name = "imu_gyro"; imu_gyro = wb_robot_get_device(convertStringsToChars(imu_gyro_name)); wb_inertial_unit_enable(imu_gyro, TIME_STEP); wb_console_print('DONE', WB_STDOUT); -% IMU, ACC +%% IMU, ACC wb_console_print(sprintf('Checking IMU (Acc)'), WB_STDOUT); imu_acc_name = "imu_acc"; imu_acc = wb_robot_get_device(convertStringsToChars(imu_acc_name)); wb_accelerometer_enable(imu_acc, TIME_STEP); wb_console_print('DONE', WB_STDOUT); -% KEYBOARD INPUT +%% KEYBOARD INPUT wb_console_print(sprintf('Checking Keyboard'), WB_STDOUT); wb_keyboard_enable(TIME_STEP) wb_console_print('DONE', WB_STDOUT); diff --git a/simulation/controllers/first_controller/Supervisor.m b/simulation/controllers/first_controller/Supervisor.m index 1c2bced..c2bfae7 100644 --- a/simulation/controllers/first_controller/Supervisor.m +++ b/simulation/controllers/first_controller/Supervisor.m @@ -1,10 +1,11 @@ -% setting the table +%% TABLE SETUP table = wb_supervisor_node_get_from_def('TABLE'); table_size = wb_supervisor_node_get_field(table, 'size'); size = [TABLE_WIDTH 0.76 TABLE_HEIGHT]; wb_supervisor_field_set_sf_vec3f(table_size, size); -% robot +%% ROBOT SETUP +% Getting the robot robot_node = wb_supervisor_node_get_from_def(ROBOT_NAME); if robot_node == 0 wb_console_print('No DEF MY_ROBOT node found in the current world file', WB_STDOUT); @@ -12,11 +13,39 @@ end trans_field = wb_supervisor_node_get_field(robot_node, 'translation'); +% LEFT, RIGHT MOTORS robot_left_wheel = wb_supervisor_node_get_from_def('ROBOT_WHEEL_LEFT'); +if robot_left_wheel == 0 + wb_console_print('No DEF ROBOT_WHEEL_LEFT node found in the current world file', WB_STDOUT); + quit(1); +end left_wheel_rad = wb_supervisor_node_get_field(robot_left_wheel, 'radius'); wb_supervisor_field_set_sf_float(left_wheel_rad, WHEEL_RADIUS); -robot_right_wheel = wb_supervisor_node_get_from_def('ROBOT_WHEEL_RIGHT'); +robot_right_wheel = wb_supervisor_node_get_from_def('ROBOT_WHEEL_RIGH'); +if robot_right_wheel == 0 + wb_console_print('No DEF ROBOT_WHEEL_RIGHT node found in the current world file', WB_STDOUT); + quit(1); +end right_wheel_rad = wb_supervisor_node_get_field(robot_right_wheel, 'radius'); wb_supervisor_field_set_sf_float(right_wheel_rad, WHEEL_RADIUS); +% LEFT, RIGHT ENCODERS +robot_left_encoder = wb_supervisor_node_get_from_def('ROBOT_WHEEL_LEFT'); +if robot_left_encoder == 0 + wb_console_print('No DEF ROBOT_WHEEL_LEFT node found in the current world file', WB_STDOUT); + quit(1); +end + +% GLOBAL GPS + + +% RANGE FINDER (TOF) + + +% IMU, GYRO + + +% IMU, ACC + + From 90d599749c68249f3f4b97efdc486dff2f7906f6 Mon Sep 17 00:00:00 2001 From: Dong-Jae Park Date: Mon, 28 Dec 2020 22:41:06 -0600 Subject: [PATCH 11/63] Changes made to the world. --- simulation/worlds/.desk.wbproj | 4 ++-- simulation/worlds/desk.wbt | 14 +++++++++----- 2 files changed, 11 insertions(+), 7 deletions(-) diff --git a/simulation/worlds/.desk.wbproj b/simulation/worlds/.desk.wbproj index 2c46f3f..cc6db8c 100644 --- a/simulation/worlds/.desk.wbproj +++ b/simulation/worlds/.desk.wbproj @@ -1,7 +1,7 @@ Webots Project File version R2020b -perspectives: 000000ff00000000fd000000030000000000000320000003dafc0100000004fc00000000ffffffff0000000000fffffffc0200000001fb00000012005300630065006e0065005400720065006501000000000000039f0000000000000000fb0000001e00480074006d006c0052006f0062006f007400570069006e0064006f00770000000000000003200000000000000000fb0000001e00480074006d006c0052006f0062006f007400570069006e0064006f00770000000000000003200000000000000000fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000006900ffffff00000001000003290000033afc0200000001fb0000001400540065007800740045006400690074006f007200000000150000033a0000004100ffffff000000030000078000000157fc0100000002fb0000000e0043006f006e0073006f006c006501000000000000073f0000000000000000fb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c0100000000000007800000006900ffffff000007800000026900000004000000040000000100000008fc00000000 +perspectives: 000000ff00000000fd000000030000000000000320000003dafc0100000004fc00000000ffffffff0000000000fffffffc0200000001fb00000012005300630065006e0065005400720065006501000000000000039f0000000000000000fb0000001e00480074006d006c0052006f0062006f007400570069006e0064006f00770000000000000003200000000000000000fb0000001e00480074006d006c0052006f0062006f007400570069006e0064006f00770000000000000003200000000000000000fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000006900ffffff00000001000003290000033afc0200000001fb0000001400540065007800740045006400690074006f007200000000150000033a0000004100ffffff00000003000007800000017ffc0100000002fb0000000e0043006f006e0073006f006c006501000000000000073f0000000000000000fb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c0100000000000007800000006900ffffff000007800000024100000004000000040000000100000008fc00000000 simulationViewPerspectives: 000000ff00000001000000020000021f0000055f0100000002010000000101 -sceneTreePerspectives: 000000ff0000000100000002000000c0000001120100000002010000000201 +sceneTreePerspectives: 000000ff00000001000000020000014e000000fa0100000002010000000201 minimizedPerspectives: 000000ff00000000fd000000030000000000000320000003dafc0100000004fc00000000ffffffff0000000000fffffffc0200000001fb00000012005300630065006e0065005400720065006501000000000000039f0000000000000000fb0000001e00480074006d006c0052006f0062006f007400570069006e0064006f00770000000000000003200000000000000000fb0000001e00480074006d006c0052006f0062006f007400570069006e0064006f00770000000000000003200000000000000000fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000006900ffffff00000001000003290000033afc0200000001fb0000001400540065007800740045006400690074006f007200000000150000033a0000004100ffffff000000030000078000000123fc0100000002fb0000000e0043006f006e0073006f006c006501000000000000073f0000000000000000fb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c0100000000000007800000006900ffffff000007800000029d00000004000000040000000100000008fc00000000 maximizedDockId: -1 centralWidgetVisible: 1 diff --git a/simulation/worlds/desk.wbt b/simulation/worlds/desk.wbt index 4364eb2..493e9a7 100644 --- a/simulation/worlds/desk.wbt +++ b/simulation/worlds/desk.wbt @@ -4,7 +4,7 @@ WorldInfo { } Viewpoint { orientation 0.7194490476403281 -0.6523419535133591 -0.23841779198667037 5.343131840159701 - position 0.8140701092654996 1.2056702161982353 0.41895044203045995 + position 0.9144646583875873 1.34749634753028 0.5537907914172986 follow "robot" } TexturedBackground { @@ -49,6 +49,10 @@ DEF TableUV Robot { translation 0.579997 0.786936 0.0074477 rotation 0.0016202905968792038 0.9999973683769037 -0.0016241605983048266 -1.5708653071795862 children [ + Accelerometer { + rotation 0 1 0 -1.5708 + name "imu_acc" + } InertialUnit { rotation 0 1 0 -1.5708 name "imu_gyro" @@ -112,14 +116,14 @@ DEF TableUV Robot { } ] endPoint Solid { - translation -0.04000000040198491 -0.009998878983036004 5.847401393102309e-09 - rotation 1.5461386176123583e-08 -1.518241940524984e-08 0.9999999999999998 1.5699999530302227 + translation -0.04000000040198491 -0.009998878983036004 5.847401393102315e-09 + rotation 1.5461386176123646e-08 -1.518241940524984e-08 0.9999999999999998 1.5699999530302227 children [ DEF WHEEL Shape { appearance PBRAppearance { metalness 0 } - geometry DEF ROBOT_WHEEL_R Cylinder { + geometry DEF ROBOT_WHEEL_RIGHT Cylinder { height 0.006 radius 0.017 } @@ -146,7 +150,7 @@ DEF TableUV Robot { } ] endPoint Solid { - translation 0.040000000533529825 -0.009998885232130555 4.1643183025106295e-09 + translation 0.040000000533529825 -0.009998885232130555 4.164318302510636e-09 rotation 9.142245450039296e-10 -1.1821754757087804e-09 1 1.57000004697403 children [ DEF WHEEL Shape { From 5bcdc1c50b18e6356730c331d08e5697ceeffe45 Mon Sep 17 00:00:00 2001 From: Dong-Jae Park Date: Tue, 29 Dec 2020 16:02:30 -0600 Subject: [PATCH 12/63] Delete Initialize.asv --- .../first_controller/Initialize.asv | 43 ------------------- 1 file changed, 43 deletions(-) delete mode 100644 simulation/controllers/first_controller/Initialize.asv diff --git a/simulation/controllers/first_controller/Initialize.asv b/simulation/controllers/first_controller/Initialize.asv deleted file mode 100644 index 7cc0c60..0000000 --- a/simulation/controllers/first_controller/Initialize.asv +++ /dev/null @@ -1,43 +0,0 @@ -wb_console_print(sprintf('Initializing...\n'), WB_STDOUT); - -% LEFT, RIGHT MOTORS -wb_console_print(sprintf('Checking Motors... '), WB_STDOUT); -motor = []; -motor_names = [ "left_motor", "right_motor" ]; -for i = 1:2 - motor(i) = wb_robot_get_device(convertStringsToChars(motor_names(i))); - wb_motor_set_position(motor(i), inf); - wb_motor_set_velocity(motor(i), 0.0); -end -wb_console_print('DONE', WB_STDOUT); - -% LEFT, RIGHT ENCODERS -wb_console_print(sprintf('Checking Encoders... '), WB_STDOUT); -ps = []; -ps_names = [ "left_ps", "right_ps" ]; -for i = 1:2 - ps(i) = wb_robot_get_device(convertStringsToChars(ps_names(i))); - wb_position_sensor_enable(ps(i), TIME_STEP); -end -wb_console_print('DONE', WB_STDOUT); - -% GLOBAL GPS -wb_console_print(sprintf('Checking GPS... '), WB_STDOUT); -gps_name = "global"; -gps = wb_robot_get_device(convertStringsToChars(gps_name)); -wb_gps_enable(gps, TIME_STEP) -wb_console_print('DONE', WB_STDOUT); - -% RANGE FINDER (TOF) -wb_console_print('Checking TOF', WB_STDOUT); -tof_name = "tof"; -tof = wb_robot_get_device(convertStringsToChars(tof_name)); -wb_range_finder_enable(tof, TIME_STEP); -wb_console_print('DONE', WB_STDOUT); - -% KEYBOARD INPUT -wb_console_print('Checking Keyboard', WB_STDOUT); -wb_keyboard_enable(TIME_STEP) -wb_console_print('DONE', WB_STDOUT); - -wb_console_print('Initialization Complete.\n', WB_STDOUT); \ No newline at end of file From 5299feffd9af2dc4b4f6610c59672cef49c61479 Mon Sep 17 00:00:00 2001 From: Dong-Jae Park Date: Tue, 29 Dec 2020 16:03:24 -0600 Subject: [PATCH 13/63] Deleted GPS sensor Can use the supervisor to know the true position of the robot. --- .../controllers/first_controller/Initialize.m | 7 ------ .../first_controller/Mode_Autonomous.m | 18 +++++++-------- .../first_controller/Mode_Manual.m | 23 +++++++++++-------- 3 files changed, 21 insertions(+), 27 deletions(-) diff --git a/simulation/controllers/first_controller/Initialize.m b/simulation/controllers/first_controller/Initialize.m index af9f9c8..3f81191 100644 --- a/simulation/controllers/first_controller/Initialize.m +++ b/simulation/controllers/first_controller/Initialize.m @@ -21,13 +21,6 @@ end wb_console_print('DONE', WB_STDOUT); -%% GLOBAL GPS -wb_console_print(sprintf('Checking GPS'), WB_STDOUT); -gps_name = "global"; -gps = wb_robot_get_device(convertStringsToChars(gps_name)); -wb_gps_enable(gps, TIME_STEP) -wb_console_print('DONE', WB_STDOUT); - %% RANGE FINDER (TOF) wb_console_print(sprintf('Checking TOF'), WB_STDOUT); tof_name = "tof"; diff --git a/simulation/controllers/first_controller/Mode_Autonomous.m b/simulation/controllers/first_controller/Mode_Autonomous.m index 3cffec5..dd7af0b 100644 --- a/simulation/controllers/first_controller/Mode_Autonomous.m +++ b/simulation/controllers/first_controller/Mode_Autonomous.m @@ -23,16 +23,6 @@ value = wb_position_sensor_get_value(ps(1)); type = wb_position_sensor_get_type(ps(1)); - %% GPS - x_y_z_array = wb_gps_get_values(gps); - p(step,:) = x_y_z_array; - % plotting position of robot on a map - plot(p(step,1),-p(step,3),'ro'); - hold on; - axis([-1 1 -0.8 0.8]); - rectangle('Position',[-TABLE_WIDTH/2 -TABLE_HEIGHT/2 TABLE_WIDTH TABLE_HEIGHT]) - hold off; - %% TOF image = wb_range_finder_get_range_image(tof); @@ -48,6 +38,14 @@ % this is done repeatedly values = wb_supervisor_field_get_sf_vec3f(trans_field); wb_console_print(sprintf('MY_ROBOT is at position: %g %g %g\n', values(1), values(2), values(3)), WB_STDOUT); + + p(step,:) = values; + % plotting position of robot on a map + plot(p(step,1),-p(step,3),'ro'); + hold on; + axis([-1 1 -0.8 0.8]); + rectangle('Position',[-TABLE_WIDTH/2 -TABLE_HEIGHT/2 TABLE_WIDTH TABLE_HEIGHT]) + hold off; %% if your code plots some graphics, it needs to flushed like this: drawnow; diff --git a/simulation/controllers/first_controller/Mode_Manual.m b/simulation/controllers/first_controller/Mode_Manual.m index ee11948..ce2b45f 100644 --- a/simulation/controllers/first_controller/Mode_Manual.m +++ b/simulation/controllers/first_controller/Mode_Manual.m @@ -40,16 +40,6 @@ value = wb_position_sensor_get_value(ps(1)); type = wb_position_sensor_get_type(ps(1)); - %% GPS - x_y_z_array = wb_gps_get_values(gps); - p(step,:) = x_y_z_array; - % plotting position of robot on a map - plot(p(step,1),-p(step,3),'ro'); - hold on; - axis([-1 1 -0.8 0.8]); - rectangle('Position',[-TABLE_WIDTH/2 -TABLE_HEIGHT/2 TABLE_WIDTH TABLE_HEIGHT]) - hold off; - %% TOF image = wb_range_finder_get_range_image(tof); @@ -59,6 +49,19 @@ % IMU, Acce x_y_z_array = wb_accelerometer_get_values(imu_acc) + + %% SUPERVISOR + % this is done repeatedly + values = wb_supervisor_field_get_sf_vec3f(trans_field); + wb_console_print(sprintf('MY_ROBOT is at position: %g %g %g\n', values(1), values(2), values(3)), WB_STDOUT); + + p(step,:) = values; + % plotting position of robot on a map + plot(p(step,1),-p(step,3),'ro'); + hold on; + axis([-1 1 -0.8 0.8]); + rectangle('Position',[-TABLE_WIDTH/2 -TABLE_HEIGHT/2 TABLE_WIDTH TABLE_HEIGHT]) + hold off; % if your code plots some graphics, it needs to flushed like this: drawnow; From 583c68e7ad21f3c02d583170efedd417d928455c Mon Sep 17 00:00:00 2001 From: Dong-Jae Park Date: Tue, 29 Dec 2020 16:04:42 -0600 Subject: [PATCH 14/63] Sensor specifications can now be customized through the Parameters.m file Support for robot size, table size, encoders, imu and tof --- .../controllers/first_controller/Parameters.m | 26 ++++- .../controllers/first_controller/Supervisor.m | 103 ++++++++++++------ 2 files changed, 90 insertions(+), 39 deletions(-) diff --git a/simulation/controllers/first_controller/Parameters.m b/simulation/controllers/first_controller/Parameters.m index 1ef6d7a..611c864 100644 --- a/simulation/controllers/first_controller/Parameters.m +++ b/simulation/controllers/first_controller/Parameters.m @@ -4,6 +4,7 @@ %% ROBOT PARAMETERS ROBOT_NAME = 'TableUV'; ROBOT_BASE_RADIUS = 0.03; +ROBOT_BASE_HEIGHT = 0.035; WHEEL_RADIUS = 0.017; TABLE_WIDTH = 1.22; @@ -13,12 +14,31 @@ %% SENSOR PARAMETERS % ENCODER -ENC_RESOLUTION = -1; -ENC_NOISE = -1; +ENC_RESOLUTION = 10; +ENC_NOISE = 10; % IMU_GYRO IMU_GYRO_RESOLUTION = -1; IMU_GYRO_NOISE = -1; % IMU_ACC -IMU_ACC_RESOLUTION = -1; \ No newline at end of file +IMU_ACC_RESOLUTION = -1; + +% TOF +TOF_FOV = 0.471; +TOF_WIDTH = 64; +TOF_HEIGHT = 64; +TOF_IS_SPHERICAL = false; +TOF_NEAR = 0.01; +TOF_MIN_RANGE = 0.01; +TOF_MAX_RANGE = 4; +TOF_MOTION_BLUR = 0; +TOF_NOISE = 0; +TOF_RESOLUTION = -1; + + + + + + + diff --git a/simulation/controllers/first_controller/Supervisor.m b/simulation/controllers/first_controller/Supervisor.m index c2bfae7..77e4156 100644 --- a/simulation/controllers/first_controller/Supervisor.m +++ b/simulation/controllers/first_controller/Supervisor.m @@ -1,51 +1,82 @@ -%% TABLE SETUP -table = wb_supervisor_node_get_from_def('TABLE'); -table_size = wb_supervisor_node_get_field(table, 'size'); -size = [TABLE_WIDTH 0.76 TABLE_HEIGHT]; -wb_supervisor_field_set_sf_vec3f(table_size, size); - %% ROBOT SETUP -% Getting the robot -robot_node = wb_supervisor_node_get_from_def(ROBOT_NAME); -if robot_node == 0 - wb_console_print('No DEF MY_ROBOT node found in the current world file', WB_STDOUT); - quit(1); +% Getting the nodes +c = containers.Map; +robot_settings = { ROBOT_NAME, 'ROBOT_WHEEL_LEFT', 'ROBOT_WHEEL_RIGHT', ... + 'ROBOT_BASE','ROBOT_ENC_LEFT', 'ROBOT_ENC_RIGHT', ... + 'ROBOT_TOF'}; +for index = 1:length(robot_settings) + setting = char(robot_settings(index)); + node = wb_supervisor_node_get_from_def(setting); + if isNull(node) + wb_console_print(sprintf('No DEF %s node found in the current world file', char(robot_settings(index))), WB_STDOUT); + quit(1); + end + c(char(robot_settings(index))) = node; end -trans_field = wb_supervisor_node_get_field(robot_node, 'translation'); -% LEFT, RIGHT MOTORS -robot_left_wheel = wb_supervisor_node_get_from_def('ROBOT_WHEEL_LEFT'); -if robot_left_wheel == 0 - wb_console_print('No DEF ROBOT_WHEEL_LEFT node found in the current world file', WB_STDOUT); - quit(1); -end -left_wheel_rad = wb_supervisor_node_get_field(robot_left_wheel, 'radius'); -wb_supervisor_field_set_sf_float(left_wheel_rad, WHEEL_RADIUS); +% Robot True position +trans_field = wb_supervisor_node_get_field(c(ROBOT_NAME), 'translation'); -robot_right_wheel = wb_supervisor_node_get_from_def('ROBOT_WHEEL_RIGH'); -if robot_right_wheel == 0 - wb_console_print('No DEF ROBOT_WHEEL_RIGHT node found in the current world file', WB_STDOUT); - quit(1); -end -right_wheel_rad = wb_supervisor_node_get_field(robot_right_wheel, 'radius'); +% Robot left wheel +left_wheel_rad = wb_supervisor_node_get_field(c('ROBOT_WHEEL_LEFT'), 'radius'); +right_wheel_rad = wb_supervisor_node_get_field(c('ROBOT_WHEEL_RIGHT'), 'radius'); +wb_supervisor_field_set_sf_float(left_wheel_rad, WHEEL_RADIUS); wb_supervisor_field_set_sf_float(right_wheel_rad, WHEEL_RADIUS); -% LEFT, RIGHT ENCODERS -robot_left_encoder = wb_supervisor_node_get_from_def('ROBOT_WHEEL_LEFT'); -if robot_left_encoder == 0 - wb_console_print('No DEF ROBOT_WHEEL_LEFT node found in the current world file', WB_STDOUT); - quit(1); -end +% Robot base +robot_base_rad = wb_supervisor_node_get_field(c('ROBOT_BASE'), 'radius'); +robot_height = wb_supervisor_node_get_field(c('ROBOT_BASE'), 'height'); +wb_supervisor_field_set_sf_float(robot_base_rad, ROBOT_BASE_RADIUS); +wb_supervisor_field_set_sf_float(robot_height, ROBOT_BASE_HEIGHT); + +% Encoders +robot_left_enc_noise = wb_supervisor_node_get_field(c('ROBOT_ENC_LEFT'), 'noise'); +robot_left_enc_res = wb_supervisor_node_get_field(c('ROBOT_ENC_LEFT'), 'resolution'); +robot_right_enc_noise = wb_supervisor_node_get_field(c('ROBOT_ENC_RIGHT'), 'noise'); +robot_right_enc_res = wb_supervisor_node_get_field(c('ROBOT_ENC_RIGHT'), 'resolution'); +wb_supervisor_field_set_sf_float(robot_left_enc_noise, ENC_NOISE); +wb_supervisor_field_set_sf_float(robot_left_enc_res, ENC_RESOLUTION); +wb_supervisor_field_set_sf_float(robot_right_enc_noise, ENC_NOISE); +wb_supervisor_field_set_sf_float(robot_right_enc_res, ENC_RESOLUTION); + +% Range Finder (ToF) +robot_tof_fov = wb_supervisor_node_get_field(c('ROBOT_TOF'), 'fieldOfView'); +robot_tof_width = wb_supervisor_node_get_field(c('ROBOT_TOF'), 'width'); +robot_tof_height = wb_supervisor_node_get_field(c('ROBOT_TOF'), 'height'); +robot_tof_spherical = wb_supervisor_node_get_field(c('ROBOT_TOF'), 'spherical'); +robot_tof_near = wb_supervisor_node_get_field(c('ROBOT_TOF'), 'near'); +robot_tof_minR = wb_supervisor_node_get_field(c('ROBOT_TOF'), 'minRange'); +robot_tof_maxR = wb_supervisor_node_get_field(c('ROBOT_TOF'), 'maxRange'); +robot_tof_blur = wb_supervisor_node_get_field(c('ROBOT_TOF'), 'motionBlur'); +robot_tof_noise = wb_supervisor_node_get_field(c('ROBOT_TOF'), 'noise'); +robot_tof_res = wb_supervisor_node_get_field(c('ROBOT_TOF'), 'resolution'); +wb_supervisor_field_set_sf_float(robot_tof_fov, TOF_FOV); +wb_supervisor_field_set_sf_int32(robot_tof_width, TOF_WIDTH); +wb_supervisor_field_set_sf_int32(robot_tof_height, TOF_HEIGHT); +wb_supervisor_field_set_sf_bool(robot_tof_spherical, TOF_IS_SPHERICAL); +wb_supervisor_field_set_sf_float(robot_tof_near, TOF_NEAR); +wb_supervisor_field_set_sf_float(robot_tof_minR, TOF_MIN_RANGE); +wb_supervisor_field_set_sf_float(robot_tof_maxR, TOF_MAX_RANGE); +wb_supervisor_field_set_sf_float(robot_tof_blur, TOF_MOTION_BLUR); +wb_supervisor_field_set_sf_float(robot_tof_noise, TOF_NOISE); +wb_supervisor_field_set_sf_float(robot_tof_res, TOF_RESOLUTION); -% GLOBAL GPS +% IMU, Gyro +% IMU, Acc -% RANGE FINDER (TOF) -% IMU, GYRO +%% TABLE SETUP +table = wb_supervisor_node_get_from_def('TABLE'); +if isNull(table) + wb_console_print('No DEF TABLE node found in the current world file', WB_STDOUT); + quit(1); +end -% IMU, ACC +table_size = wb_supervisor_node_get_field(table, 'size'); +size = [TABLE_WIDTH 0.76 TABLE_HEIGHT]; +wb_supervisor_field_set_sf_vec3f(table_size, size); From 2104392b490a1277a3c31af691d915b86511d031 Mon Sep 17 00:00:00 2001 From: Dong-Jae Park Date: Tue, 29 Dec 2020 16:52:20 -0600 Subject: [PATCH 15/63] IMU parameter settings --- .../controllers/first_controller/Parameters.m | 6 +++--- .../controllers/first_controller/Supervisor.m | 14 +++++++++----- 2 files changed, 12 insertions(+), 8 deletions(-) diff --git a/simulation/controllers/first_controller/Parameters.m b/simulation/controllers/first_controller/Parameters.m index 611c864..a848067 100644 --- a/simulation/controllers/first_controller/Parameters.m +++ b/simulation/controllers/first_controller/Parameters.m @@ -18,11 +18,11 @@ ENC_NOISE = 10; % IMU_GYRO -IMU_GYRO_RESOLUTION = -1; -IMU_GYRO_NOISE = -1; +IMU_GYRO_RESOLUTION = 10; +IMU_GYRO_NOISE = 10; % IMU_ACC -IMU_ACC_RESOLUTION = -1; +IMU_ACC_RESOLUTION = 10; % TOF TOF_FOV = 0.471; diff --git a/simulation/controllers/first_controller/Supervisor.m b/simulation/controllers/first_controller/Supervisor.m index 77e4156..dd22061 100644 --- a/simulation/controllers/first_controller/Supervisor.m +++ b/simulation/controllers/first_controller/Supervisor.m @@ -1,9 +1,9 @@ %% ROBOT SETUP % Getting the nodes c = containers.Map; -robot_settings = { ROBOT_NAME, 'ROBOT_WHEEL_LEFT', 'ROBOT_WHEEL_RIGHT', ... - 'ROBOT_BASE','ROBOT_ENC_LEFT', 'ROBOT_ENC_RIGHT', ... - 'ROBOT_TOF'}; +robot_settings = { ROBOT_NAME, 'ROBOT_WHEEL_LEFT', 'ROBOT_WHEEL_RIGHT', ... + 'ROBOT_BASE', 'ROBOT_ENC_LEFT', 'ROBOT_ENC_RIGHT', ... + 'ROBOT_TOF' , 'ROBOT_IMU_GYRO', 'ROBOT_IMU_ACC'}; for index = 1:length(robot_settings) setting = char(robot_settings(index)); node = wb_supervisor_node_get_from_def(setting); @@ -62,10 +62,14 @@ wb_supervisor_field_set_sf_float(robot_tof_res, TOF_RESOLUTION); % IMU, Gyro +robot_imu_gyro_noise = wb_supervisor_node_get_field(c('ROBOT_IMU_GYRO'), 'noise'); +robot_imu_gyro_res = wb_supervisor_node_get_field(c('ROBOT_IMU_GYRO'), 'resolution'); +wb_supervisor_field_set_sf_float(robot_imu_gyro_noise, IMU_GYRO_NOISE); +wb_supervisor_field_set_sf_float(robot_imu_gyro_res, IMU_GYRO_RESOLUTION); % IMU, Acc - - +robot_imu_acc_res = wb_supervisor_node_get_field(c('ROBOT_IMU_ACC'), 'resolution'); +wb_supervisor_field_set_sf_float(robot_imu_acc_res, IMU_ACC_RESOLUTION); %% TABLE SETUP From 148ea46d51d9fd7998aa7db5e1982219e10103e4 Mon Sep 17 00:00:00 2001 From: Dong-Jae Park Date: Tue, 29 Dec 2020 16:52:36 -0600 Subject: [PATCH 16/63] Changed to R2021a --- simulation/worlds/.desk.wbproj | 8 ++++---- simulation/worlds/desk.wbt | 33 ++++++++++++++++----------------- 2 files changed, 20 insertions(+), 21 deletions(-) diff --git a/simulation/worlds/.desk.wbproj b/simulation/worlds/.desk.wbproj index cc6db8c..c38d010 100644 --- a/simulation/worlds/.desk.wbproj +++ b/simulation/worlds/.desk.wbproj @@ -1,13 +1,13 @@ -Webots Project File version R2020b -perspectives: 000000ff00000000fd000000030000000000000320000003dafc0100000004fc00000000ffffffff0000000000fffffffc0200000001fb00000012005300630065006e0065005400720065006501000000000000039f0000000000000000fb0000001e00480074006d006c0052006f0062006f007400570069006e0064006f00770000000000000003200000000000000000fb0000001e00480074006d006c0052006f0062006f007400570069006e0064006f00770000000000000003200000000000000000fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000006900ffffff00000001000003290000033afc0200000001fb0000001400540065007800740045006400690074006f007200000000150000033a0000004100ffffff00000003000007800000017ffc0100000002fb0000000e0043006f006e0073006f006c006501000000000000073f0000000000000000fb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c0100000000000007800000006900ffffff000007800000024100000004000000040000000100000008fc00000000 -simulationViewPerspectives: 000000ff00000001000000020000021f0000055f0100000002010000000101 +Webots Project File version R2021a +perspectives: 000000ff00000000fd000000030000000000000320000003dafc0100000004fc00000000ffffffff0000000000fffffffc0200000001fb00000012005300630065006e0065005400720065006501000000000000039f0000000000000000fb0000001e00480074006d006c0052006f0062006f007400570069006e0064006f00770000000000000003200000000000000000fb0000001e00480074006d006c0052006f0062006f007400570069006e0064006f00770000000000000003200000000000000000fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000006900ffffff00000001000003290000033afc0200000001fb0000001400540065007800740045006400690074006f007200000000150000033a0000008700ffffff0000000300000780000000fbfc0100000002fb0000000e0043006f006e0073006f006c006501000000000000073f0000000000000000fb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c0100000000000007800000006900ffffff00000780000002dd00000004000000040000000100000008fc00000000 +simulationViewPerspectives: 000000ff0000000100000002000001b5000005c90100000002010000000101 sceneTreePerspectives: 000000ff00000001000000020000014e000000fa0100000002010000000201 minimizedPerspectives: 000000ff00000000fd000000030000000000000320000003dafc0100000004fc00000000ffffffff0000000000fffffffc0200000001fb00000012005300630065006e0065005400720065006501000000000000039f0000000000000000fb0000001e00480074006d006c0052006f0062006f007400570069006e0064006f00770000000000000003200000000000000000fb0000001e00480074006d006c0052006f0062006f007400570069006e0064006f00770000000000000003200000000000000000fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000006900ffffff00000001000003290000033afc0200000001fb0000001400540065007800740045006400690074006f007200000000150000033a0000004100ffffff000000030000078000000123fc0100000002fb0000000e0043006f006e0073006f006c006501000000000000073f0000000000000000fb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c0100000000000007800000006900ffffff000007800000029d00000004000000040000000100000008fc00000000 maximizedDockId: -1 centralWidgetVisible: 1 projectionMode: PERSPECTIVE renderingMode: PLAIN -orthographicViewHeight: 0.974191 +orthographicViewHeight: 0.601763 textFiles: -1 globalOptionalRendering: CoordinateSystem::RangeFinderFrustums consoles: Console:All:All diff --git a/simulation/worlds/desk.wbt b/simulation/worlds/desk.wbt index 493e9a7..d11d711 100644 --- a/simulation/worlds/desk.wbt +++ b/simulation/worlds/desk.wbt @@ -1,11 +1,11 @@ -#VRML_SIM R2020b utf8 +#VRML_SIM R2021a utf8 WorldInfo { coordinateSystem "NUE" } Viewpoint { orientation 0.7194490476403281 -0.6523419535133591 -0.23841779198667037 5.343131840159701 - position 0.9144646583875873 1.34749634753028 0.5537907914172986 - follow "robot" + position 1.475989507611434 2.064524382416358 1.213311008241961 + follow "table" } TexturedBackground { } @@ -49,24 +49,21 @@ DEF TableUV Robot { translation 0.579997 0.786936 0.0074477 rotation 0.0016202905968792038 0.9999973683769037 -0.0016241605983048266 -1.5708653071795862 children [ - Accelerometer { + DEF ROBOT_IMU_ACC Accelerometer { rotation 0 1 0 -1.5708 name "imu_acc" } - InertialUnit { + DEF ROBOT_IMU_GYRO InertialUnit { rotation 0 1 0 -1.5708 name "imu_gyro" } - RangeFinder { + DEF ROBOT_TOF RangeFinder { translation 0 0 0.02 rotation 0 1 0 3.14159 name "tof" - fieldOfView 0.471239 + fieldOfView 0.471 maxRange 4 } - GPS { - name "global" - } Solid { translation 0 -0.017 0.02 children [ @@ -107,17 +104,18 @@ DEF TableUV Robot { anchor -0.04 -0.01 0 } device [ - PositionSensor { + DEF ROBOT_ENC_LEFT PositionSensor { name "left_ps" - resolution 1 + noise 10 + resolution 10 } RotationalMotor { name "left_motor" } ] endPoint Solid { - translation -0.04000000040198491 -0.009998878983036004 5.847401393102315e-09 - rotation 1.5461386176123646e-08 -1.518241940524984e-08 0.9999999999999998 1.5699999530302227 + translation -0.04000000040198491 -0.009998878983036004 5.847401393102335e-09 + rotation 1.5461386176123815e-08 -1.518241940524984e-08 0.9999999999999998 1.5699999530302227 children [ DEF WHEEL Shape { appearance PBRAppearance { @@ -141,16 +139,17 @@ DEF TableUV Robot { anchor 0.04 -0.01 0 } device [ - PositionSensor { + DEF ROBOT_ENC_RIGHT PositionSensor { name "right_ps" - resolution 1 + noise 10 + resolution 10 } RotationalMotor { name "right_motor" } ] endPoint Solid { - translation 0.040000000533529825 -0.009998885232130555 4.164318302510636e-09 + translation 0.040000000533529825 -0.009998885232130555 4.164318302510656e-09 rotation 9.142245450039296e-10 -1.1821754757087804e-09 1 1.57000004697403 children [ DEF WHEEL Shape { From 420bfb624aa3d9aa963d6a71c6faaead12ac35cf Mon Sep 17 00:00:00 2001 From: Dong-Jae Park Date: Wed, 30 Dec 2020 16:34:43 -0600 Subject: [PATCH 17/63] Encoder ticks to robot pose estimation --- .../controllers/first_controller/enc2pose.m | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) create mode 100644 simulation/controllers/first_controller/enc2pose.m diff --git a/simulation/controllers/first_controller/enc2pose.m b/simulation/controllers/first_controller/enc2pose.m new file mode 100644 index 0000000..6fc4040 --- /dev/null +++ b/simulation/controllers/first_controller/enc2pose.m @@ -0,0 +1,16 @@ +function [x,z,theta] = enc2pose(left_enc, right_enc, prev_x, prev_z, prev_theta, ENC_UNIT, WHEEL_FROM_CENTER) + %enc2pose: Convert encoder values to robot pose + + left_dist = left_enc * ENC_UNIT; + right_dist = right_enc * ENC_UNIT; + center_dist = (left_dist + right_dist) / 2; + + if center_dist < 0.00001 + center_dist = 0 + end + + x = prev_x + center_dist*cos(prev_theta); + z = prev_z + center_dist*sin(prev_theta); + theta = prev_theta + (right_dist-left_dist)/(2*WHEEL_FROM_CENTER); +end + From 67062a0dd8d274fa6ebdee5ec91de72dd7ea3d57 Mon Sep 17 00:00:00 2001 From: Dong-Jae Park Date: Wed, 30 Dec 2020 16:35:17 -0600 Subject: [PATCH 18/63] Checking enc2pose with true robot pose Still has some issues with transformations. --- .../first_controller/Mode_Manual.m | 48 ++++++++++++++----- 1 file changed, 37 insertions(+), 11 deletions(-) diff --git a/simulation/controllers/first_controller/Mode_Manual.m b/simulation/controllers/first_controller/Mode_Manual.m index ce2b45f..39c2f32 100644 --- a/simulation/controllers/first_controller/Mode_Manual.m +++ b/simulation/controllers/first_controller/Mode_Manual.m @@ -1,9 +1,20 @@ % MAIN LOOP wb_console_print('Starting Manual Mode!', WB_STDOUT); -step = 0; +step = 1; samples = 0; +% initial pose of the robot +position = wb_supervisor_field_get_sf_vec3f(trans_field); +orientation = wb_supervisor_field_get_sf_rotation(orien_field); +pose = [position(1) position(3) orientation(4)]'; % [x y theta] +pose_enc(:, step) = pose; +p(:, step) = pose; + +% initializing values for encoder +prev_enc_left = 0; +prev_enc_right = 0; + while wb_robot_step(TIME_STEP) ~= -1 step = step + 1; @@ -33,35 +44,50 @@ end % set the velocity of the motors - wb_motor_set_velocity(motor(1), left_speed); - wb_motor_set_velocity(motor(2), right_speed); + wb_motor_set_velocity(motor(1), left_speed*MAX_SPEED); + wb_motor_set_velocity(motor(2), right_speed*MAX_SPEED); %% ENCODER - value = wb_position_sensor_get_value(ps(1)); - type = wb_position_sensor_get_type(ps(1)); + new_enc_left = wb_position_sensor_get_value(ps(1)); + new_enc_right = wb_position_sensor_get_value(ps(2)); + + diff_enc_left = new_enc_left - prev_enc_left; + diff_enc_right = new_enc_right - prev_enc_right; + + prev_enc_left = new_enc_left; + prev_enc_right = new_enc_right; + + [x, z, theta] = enc2pose(diff_enc_left, diff_enc_right, pose_enc(1, step-1), pose_enc(2, step-1), pose_enc(3, step-1), ENC_UNIT, WHEEL_FROM_CENTER); + pose_enc(:, step) = [x, z, theta]'; + %% TOF image = wb_range_finder_get_range_image(tof); %% IMU % IMU, Gyro - roll_pitch_yaw_array = wb_inertial_unit_get_roll_pitch_yaw(imu_gyro) + roll_pitch_yaw_array = wb_inertial_unit_get_roll_pitch_yaw(imu_gyro); % IMU, Acce - x_y_z_array = wb_accelerometer_get_values(imu_acc) + x_y_z_array = wb_accelerometer_get_values(imu_acc); %% SUPERVISOR % this is done repeatedly values = wb_supervisor_field_get_sf_vec3f(trans_field); - wb_console_print(sprintf('MY_ROBOT is at position: %g %g %g\n', values(1), values(2), values(3)), WB_STDOUT); +% wb_console_print(sprintf('MY_ROBOT is at position: %g %g %g\n', values(1), values(2), values(3)), WB_STDOUT); - p(step,:) = values; + p(:, step) = values'; % plotting position of robot on a map - plot(p(step,1),-p(step,3),'ro'); + plot(p(1, step),-p(3, step),'ro'); hold on; - axis([-1 1 -0.8 0.8]); + plot(pose_enc(1, step), -pose_enc(2, step),'bx'); + hold on; + axis([-0.8 0.8 -0.6 0.6]); rectangle('Position',[-TABLE_WIDTH/2 -TABLE_HEIGHT/2 TABLE_WIDTH TABLE_HEIGHT]) hold off; + + wb_console_print(sprintf('TRUE position: %g %g\n', p(1, step), -p(3, step)), WB_STDOUT); + wb_console_print(sprintf('ENC position: %g %g\n', pose_enc(1, step), pose_enc(2, step)), WB_STDOUT); % if your code plots some graphics, it needs to flushed like this: drawnow; From 20b975a33bcd8b03d26bec57a0e9d83650e83ded Mon Sep 17 00:00:00 2001 From: Dong-Jae Park Date: Wed, 30 Dec 2020 16:35:29 -0600 Subject: [PATCH 19/63] Added units to parameter values --- .../controllers/first_controller/Parameters.m | 37 ++++++++++--------- 1 file changed, 20 insertions(+), 17 deletions(-) diff --git a/simulation/controllers/first_controller/Parameters.m b/simulation/controllers/first_controller/Parameters.m index a848067..3ff8d4f 100644 --- a/simulation/controllers/first_controller/Parameters.m +++ b/simulation/controllers/first_controller/Parameters.m @@ -3,35 +3,38 @@ %% ROBOT PARAMETERS ROBOT_NAME = 'TableUV'; -ROBOT_BASE_RADIUS = 0.03; -ROBOT_BASE_HEIGHT = 0.035; -WHEEL_RADIUS = 0.017; +ROBOT_BASE_RADIUS = 0.03; %[m] +ROBOT_BASE_HEIGHT = 0.035; %[m] +WHEEL_RADIUS = 0.017; %[m] +WHEEL_FROM_CENTER = 0.04; %[m] +WHEEL_CIRCUM = WHEEL_RADIUS * 2 * pi; -TABLE_WIDTH = 1.22; -TABLE_HEIGHT = 0.76; +TABLE_WIDTH = 1.22; %[m] +TABLE_HEIGHT = 0.76; %[m] -MAX_SPEED = 6.28; +MAX_SPEED = 3; %[m/s] %% SENSOR PARAMETERS % ENCODER -ENC_RESOLUTION = 10; -ENC_NOISE = 10; +ENC_RESOLUTION = -1; +ENC_NOISE = 0; +ENC_UNIT = WHEEL_CIRCUM / 6.28; % IMU_GYRO -IMU_GYRO_RESOLUTION = 10; -IMU_GYRO_NOISE = 10; +IMU_GYRO_RESOLUTION = -1; +IMU_GYRO_NOISE = 0; % IMU_ACC -IMU_ACC_RESOLUTION = 10; +IMU_ACC_RESOLUTION = -1; % TOF -TOF_FOV = 0.471; -TOF_WIDTH = 64; -TOF_HEIGHT = 64; +TOF_FOV = 0.471; %[rad] +TOF_WIDTH = 64; %[px] +TOF_HEIGHT = 64; %[px] TOF_IS_SPHERICAL = false; -TOF_NEAR = 0.01; -TOF_MIN_RANGE = 0.01; -TOF_MAX_RANGE = 4; +TOF_NEAR = 0.01; %[m] +TOF_MIN_RANGE = 0.01; %[m] +TOF_MAX_RANGE = 4; %[m] TOF_MOTION_BLUR = 0; TOF_NOISE = 0; TOF_RESOLUTION = -1; From ba59d0a3446da03e1e3b9583aa03bfdb253d4f88 Mon Sep 17 00:00:00 2001 From: Dong-Jae Park Date: Wed, 30 Dec 2020 16:35:52 -0600 Subject: [PATCH 20/63] Tracking the orientation of the robot --- simulation/controllers/first_controller/Supervisor.m | 1 + 1 file changed, 1 insertion(+) diff --git a/simulation/controllers/first_controller/Supervisor.m b/simulation/controllers/first_controller/Supervisor.m index dd22061..2cdf084 100644 --- a/simulation/controllers/first_controller/Supervisor.m +++ b/simulation/controllers/first_controller/Supervisor.m @@ -16,6 +16,7 @@ % Robot True position trans_field = wb_supervisor_node_get_field(c(ROBOT_NAME), 'translation'); +orien_field = wb_supervisor_node_get_field(c(ROBOT_NAME), 'rotation'); % Robot left wheel left_wheel_rad = wb_supervisor_node_get_field(c('ROBOT_WHEEL_LEFT'), 'radius'); From e5da1ee1ca4db4f048dc7c13b55668d69f9ef4c9 Mon Sep 17 00:00:00 2001 From: Dong-Jae Park Date: Wed, 30 Dec 2020 16:35:59 -0600 Subject: [PATCH 21/63] Changes to the world --- simulation/worlds/.desk.wbproj | 4 ++-- simulation/worlds/desk.wbt | 14 +++++--------- 2 files changed, 7 insertions(+), 11 deletions(-) diff --git a/simulation/worlds/.desk.wbproj b/simulation/worlds/.desk.wbproj index c38d010..cef895e 100644 --- a/simulation/worlds/.desk.wbproj +++ b/simulation/worlds/.desk.wbproj @@ -1,5 +1,5 @@ Webots Project File version R2021a -perspectives: 000000ff00000000fd000000030000000000000320000003dafc0100000004fc00000000ffffffff0000000000fffffffc0200000001fb00000012005300630065006e0065005400720065006501000000000000039f0000000000000000fb0000001e00480074006d006c0052006f0062006f007400570069006e0064006f00770000000000000003200000000000000000fb0000001e00480074006d006c0052006f0062006f007400570069006e0064006f00770000000000000003200000000000000000fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000006900ffffff00000001000003290000033afc0200000001fb0000001400540065007800740045006400690074006f007200000000150000033a0000008700ffffff0000000300000780000000fbfc0100000002fb0000000e0043006f006e0073006f006c006501000000000000073f0000000000000000fb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c0100000000000007800000006900ffffff00000780000002dd00000004000000040000000100000008fc00000000 +perspectives: 000000ff00000000fd000000030000000000000320000003dafc0100000004fc00000000ffffffff0000000000fffffffc0200000001fb00000012005300630065006e0065005400720065006501000000000000039f0000000000000000fb0000001e00480074006d006c0052006f0062006f007400570069006e0064006f00770000000000000003200000000000000000fb0000001e00480074006d006c0052006f0062006f007400570069006e0064006f00770000000000000003200000000000000000fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000006900ffffff00000001000003290000033afc0200000001fb0000001400540065007800740045006400690074006f007200000000150000033a0000004100ffffff00000003000007800000003bfc0100000002fb0000000e0043006f006e0073006f006c006501000000000000073f0000000000000000fb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c0100000000000007800000006900ffffff000007800000038500000004000000040000000100000008fc00000000 simulationViewPerspectives: 000000ff0000000100000002000001b5000005c90100000002010000000101 sceneTreePerspectives: 000000ff00000001000000020000014e000000fa0100000002010000000201 minimizedPerspectives: 000000ff00000000fd000000030000000000000320000003dafc0100000004fc00000000ffffffff0000000000fffffffc0200000001fb00000012005300630065006e0065005400720065006501000000000000039f0000000000000000fb0000001e00480074006d006c0052006f0062006f007400570069006e0064006f00770000000000000003200000000000000000fb0000001e00480074006d006c0052006f0062006f007400570069006e0064006f00770000000000000003200000000000000000fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000006900ffffff00000001000003290000033afc0200000001fb0000001400540065007800740045006400690074006f007200000000150000033a0000004100ffffff000000030000078000000123fc0100000002fb0000000e0043006f006e0073006f006c006501000000000000073f0000000000000000fb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c0100000000000007800000006900ffffff000007800000029d00000004000000040000000100000008fc00000000 @@ -7,7 +7,7 @@ maximizedDockId: -1 centralWidgetVisible: 1 projectionMode: PERSPECTIVE renderingMode: PLAIN -orthographicViewHeight: 0.601763 +orthographicViewHeight: 0.808765 textFiles: -1 globalOptionalRendering: CoordinateSystem::RangeFinderFrustums consoles: Console:All:All diff --git a/simulation/worlds/desk.wbt b/simulation/worlds/desk.wbt index d11d711..90bc785 100644 --- a/simulation/worlds/desk.wbt +++ b/simulation/worlds/desk.wbt @@ -3,8 +3,8 @@ WorldInfo { coordinateSystem "NUE" } Viewpoint { - orientation 0.7194490476403281 -0.6523419535133591 -0.23841779198667037 5.343131840159701 - position 1.475989507611434 2.064524382416358 1.213311008241961 + orientation 1 0 0 4.71238898038469 + position -2.3422871881845414e-16 2.985170663630904 -3.162748119241487e-17 follow "table" } TexturedBackground { @@ -106,16 +106,14 @@ DEF TableUV Robot { device [ DEF ROBOT_ENC_LEFT PositionSensor { name "left_ps" - noise 10 - resolution 10 } RotationalMotor { name "left_motor" } ] endPoint Solid { - translation -0.04000000040198491 -0.009998878983036004 5.847401393102335e-09 - rotation 1.5461386176123815e-08 -1.518241940524984e-08 0.9999999999999998 1.5699999530302227 + translation -0.04000000040198491 -0.009998878983036004 5.8474013931023716e-09 + rotation 1.5461386176124146e-08 -1.518241940524984e-08 0.9999999999999998 1.5699999530302227 children [ DEF WHEEL Shape { appearance PBRAppearance { @@ -141,15 +139,13 @@ DEF TableUV Robot { device [ DEF ROBOT_ENC_RIGHT PositionSensor { name "right_ps" - noise 10 - resolution 10 } RotationalMotor { name "right_motor" } ] endPoint Solid { - translation 0.040000000533529825 -0.009998885232130555 4.164318302510656e-09 + translation 0.04 -0.00999889 4.164320000000024e-09 rotation 9.142245450039296e-10 -1.1821754757087804e-09 1 1.57000004697403 children [ DEF WHEEL Shape { From 282dad3989c2172f94c67c74cd7246be2874a767 Mon Sep 17 00:00:00 2001 From: Dong-Jae Park Date: Fri, 1 Jan 2021 11:30:24 -0600 Subject: [PATCH 22/63] Fixed wrong direction turning for the robot --- simulation/controllers/first_controller/Mode_Manual.m | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/simulation/controllers/first_controller/Mode_Manual.m b/simulation/controllers/first_controller/Mode_Manual.m index 39c2f32..92558c0 100644 --- a/simulation/controllers/first_controller/Mode_Manual.m +++ b/simulation/controllers/first_controller/Mode_Manual.m @@ -23,16 +23,16 @@ switch input case 314 % LEFT - left_speed = 1.0; - right_speed = -1.0; + left_speed = -1.0; + right_speed = 1.0; case 315 % UP left_speed = 1.0; right_speed = 1.0; case 316 % RIGHT - left_speed = -1.0; - right_speed = 1.0; + left_speed = 1.0; + right_speed = -1.0; case 317 % DOWN left_speed = -1.0; From 4912f7c8b45daa6449c06ab87b25d2623ce6f87a Mon Sep 17 00:00:00 2001 From: Dong-Jae Park Date: Fri, 1 Jan 2021 11:31:04 -0600 Subject: [PATCH 23/63] Removed objects from table --- simulation/worlds/desk.wbt | 23 ----------------------- 1 file changed, 23 deletions(-) diff --git a/simulation/worlds/desk.wbt b/simulation/worlds/desk.wbt index 90bc785..cbae571 100644 --- a/simulation/worlds/desk.wbt +++ b/simulation/worlds/desk.wbt @@ -16,33 +16,10 @@ DEF OBJECTS Group { Floor { name "floor(1)" } - BeerBottle { - translation 3.68702e-18 0.760422 -0.07 - name "beer bottle(1)" - } DEF TABLE Table { size 1.22 0.76 0.76 frameThickness 0.1 } - Spoon { - translation -0.14 0.777648 -0.139205 - rotation -0.01851782694315772 0.9996570309244369 0.018517845680389593 -1.571138342843346 - name "spoon(1)" - } - Knife { - translation 0.0385655 0.758308 0.189585 - rotation -0.0029417597513266072 0.9999829154692547 -0.00505121957300935 2.09441 - name "knife(1)" - } - Plate { - translation -0.29 0.759941 0.18 - rotation 0.9989264613518823 0.010627846543856588 -0.04508850949905507 6.926359812943209e-16 - name "plate(1)" - } - Apple { - translation -0.46 0.809765 -0.01 - name "apple(1)" - } ] } DEF TableUV Robot { From 3e119b4e3c713f61afd88e967c169587e87616d2 Mon Sep 17 00:00:00 2001 From: Dong-Jae Park Date: Fri, 1 Jan 2021 11:31:29 -0600 Subject: [PATCH 24/63] Fixed wrong naming fot left and right motors --- simulation/worlds/desk.wbt | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/simulation/worlds/desk.wbt b/simulation/worlds/desk.wbt index cbae571..419a736 100644 --- a/simulation/worlds/desk.wbt +++ b/simulation/worlds/desk.wbt @@ -81,11 +81,11 @@ DEF TableUV Robot { anchor -0.04 -0.01 0 } device [ - DEF ROBOT_ENC_LEFT PositionSensor { - name "left_ps" + DEF ROBOT_ENC_RIGHT PositionSensor { + name "right_ps" } RotationalMotor { - name "left_motor" + name "right_motor" } ] endPoint Solid { @@ -114,11 +114,11 @@ DEF TableUV Robot { anchor 0.04 -0.01 0 } device [ - DEF ROBOT_ENC_RIGHT PositionSensor { - name "right_ps" + DEF ROBOT_ENC_LEFT PositionSensor { + name "left_ps" } RotationalMotor { - name "right_motor" + name "left_motor" } ] endPoint Solid { From 535d7b6e6c9f7710a857e4090e7d3af372f8bd49 Mon Sep 17 00:00:00 2001 From: Dong-Jae Park Date: Fri, 1 Jan 2021 11:31:54 -0600 Subject: [PATCH 25/63] Minor fixed and changes to the code --- .../controllers/first_controller/Mode_Autonomous.m | 3 ++- .../controllers/first_controller/Mode_Manual.m | 6 ++++-- simulation/controllers/first_controller/enc2pose.m | 4 ++-- simulation/worlds/.desk.wbproj | 2 +- simulation/worlds/desk.wbt | 13 ++++++------- 5 files changed, 15 insertions(+), 13 deletions(-) diff --git a/simulation/controllers/first_controller/Mode_Autonomous.m b/simulation/controllers/first_controller/Mode_Autonomous.m index dd7af0b..cdb4314 100644 --- a/simulation/controllers/first_controller/Mode_Autonomous.m +++ b/simulation/controllers/first_controller/Mode_Autonomous.m @@ -9,7 +9,8 @@ % setting motor speeds left_speed = 0.5 * MAX_SPEED; - right_speed = 0.5 * MAX_SPEED; + right_speed = 1.5 * MAX_SPEED; + for i=1:2 if i == 1 wb_motor_set_velocity(motor(i), left_speed); diff --git a/simulation/controllers/first_controller/Mode_Manual.m b/simulation/controllers/first_controller/Mode_Manual.m index 92558c0..855bbc5 100644 --- a/simulation/controllers/first_controller/Mode_Manual.m +++ b/simulation/controllers/first_controller/Mode_Manual.m @@ -78,10 +78,12 @@ p(:, step) = values'; % plotting position of robot on a map - plot(p(1, step),-p(3, step),'ro'); + plot(p(1, step), -p(3, step), 'ro'); hold on; - plot(pose_enc(1, step), -pose_enc(2, step),'bx'); + plot(0.1, 0.2, 'bx'); hold on; +% plot(pose_enc(1, step), pose_enc(2, step), 'bx'); +% hold on; axis([-0.8 0.8 -0.6 0.6]); rectangle('Position',[-TABLE_WIDTH/2 -TABLE_HEIGHT/2 TABLE_WIDTH TABLE_HEIGHT]) hold off; diff --git a/simulation/controllers/first_controller/enc2pose.m b/simulation/controllers/first_controller/enc2pose.m index 6fc4040..1c85eed 100644 --- a/simulation/controllers/first_controller/enc2pose.m +++ b/simulation/controllers/first_controller/enc2pose.m @@ -5,8 +5,8 @@ right_dist = right_enc * ENC_UNIT; center_dist = (left_dist + right_dist) / 2; - if center_dist < 0.00001 - center_dist = 0 + if center_dist < 0.0001 + center_dist = 0; end x = prev_x + center_dist*cos(prev_theta); diff --git a/simulation/worlds/.desk.wbproj b/simulation/worlds/.desk.wbproj index cef895e..0ec2794 100644 --- a/simulation/worlds/.desk.wbproj +++ b/simulation/worlds/.desk.wbproj @@ -1,5 +1,5 @@ Webots Project File version R2021a -perspectives: 000000ff00000000fd000000030000000000000320000003dafc0100000004fc00000000ffffffff0000000000fffffffc0200000001fb00000012005300630065006e0065005400720065006501000000000000039f0000000000000000fb0000001e00480074006d006c0052006f0062006f007400570069006e0064006f00770000000000000003200000000000000000fb0000001e00480074006d006c0052006f0062006f007400570069006e0064006f00770000000000000003200000000000000000fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000006900ffffff00000001000003290000033afc0200000001fb0000001400540065007800740045006400690074006f007200000000150000033a0000004100ffffff00000003000007800000003bfc0100000002fb0000000e0043006f006e0073006f006c006501000000000000073f0000000000000000fb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c0100000000000007800000006900ffffff000007800000038500000004000000040000000100000008fc00000000 +perspectives: 000000ff00000000fd000000030000000000000320000003dafc0100000004fc00000000ffffffff0000000000fffffffc0200000001fb00000012005300630065006e0065005400720065006501000000000000039f0000000000000000fb0000001e00480074006d006c0052006f0062006f007400570069006e0064006f00770000000000000003200000000000000000fb0000001e00480074006d006c0052006f0062006f007400570069006e0064006f00770000000000000003200000000000000000fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000006900ffffff00000001000003290000033afc0200000001fb0000001400540065007800740045006400690074006f007200000000150000033a0000004100ffffff0000000300000780000000cdfc0100000002fb0000000e0043006f006e0073006f006c006501000000000000073f0000000000000000fb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c0100000000000007800000006900ffffff000007800000030b00000004000000040000000100000008fc00000000 simulationViewPerspectives: 000000ff0000000100000002000001b5000005c90100000002010000000101 sceneTreePerspectives: 000000ff00000001000000020000014e000000fa0100000002010000000201 minimizedPerspectives: 000000ff00000000fd000000030000000000000320000003dafc0100000004fc00000000ffffffff0000000000fffffffc0200000001fb00000012005300630065006e0065005400720065006501000000000000039f0000000000000000fb0000001e00480074006d006c0052006f0062006f007400570069006e0064006f00770000000000000003200000000000000000fb0000001e00480074006d006c0052006f0062006f007400570069006e0064006f00770000000000000003200000000000000000fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000006900ffffff00000001000003290000033afc0200000001fb0000001400540065007800740045006400690074006f007200000000150000033a0000004100ffffff000000030000078000000123fc0100000002fb0000000e0043006f006e0073006f006c006501000000000000073f0000000000000000fb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c0100000000000007800000006900ffffff000007800000029d00000004000000040000000100000008fc00000000 diff --git a/simulation/worlds/desk.wbt b/simulation/worlds/desk.wbt index 419a736..9f7a30a 100644 --- a/simulation/worlds/desk.wbt +++ b/simulation/worlds/desk.wbt @@ -3,8 +3,8 @@ WorldInfo { coordinateSystem "NUE" } Viewpoint { - orientation 1 0 0 4.71238898038469 - position -2.3422871881845414e-16 2.985170663630904 -3.162748119241487e-17 + orientation 0 1 0 3.1415926535897936 + position -0.00015111794010151723 0.7735005134284224 -0.883401221419889 follow "table" } TexturedBackground { @@ -23,8 +23,7 @@ DEF OBJECTS Group { ] } DEF TableUV Robot { - translation 0.579997 0.786936 0.0074477 - rotation 0.0016202905968792038 0.9999973683769037 -0.0016241605983048266 -1.5708653071795862 + translation 0 0.786936 0 children [ DEF ROBOT_IMU_ACC Accelerometer { rotation 0 1 0 -1.5708 @@ -89,8 +88,8 @@ DEF TableUV Robot { } ] endPoint Solid { - translation -0.04000000040198491 -0.009998878983036004 5.8474013931023716e-09 - rotation 1.5461386176124146e-08 -1.518241940524984e-08 0.9999999999999998 1.5699999530302227 + translation -0.04000000040198491 -0.009998878983036004 5.847401393102382e-09 + rotation 1.546138617612424e-08 -1.518241940524984e-08 0.9999999999999998 1.5699999530302227 children [ DEF WHEEL Shape { appearance PBRAppearance { @@ -122,7 +121,7 @@ DEF TableUV Robot { } ] endPoint Solid { - translation 0.04 -0.00999889 4.164320000000024e-09 + translation 0.04 -0.00999889 4.164320000000035e-09 rotation 9.142245450039296e-10 -1.1821754757087804e-09 1 1.57000004697403 children [ DEF WHEEL Shape { From e8f5109b6da8fddf40c063a99b6262b51e9cf948 Mon Sep 17 00:00:00 2001 From: Dong-Jae Park Date: Fri, 1 Jan 2021 11:53:20 -0600 Subject: [PATCH 26/63] Odometry using encoder values works There is drift and moving backwards is not tracked by the encoders. --- simulation/controllers/first_controller/Mode_Manual.m | 2 +- simulation/controllers/first_controller/enc2pose.m | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/simulation/controllers/first_controller/Mode_Manual.m b/simulation/controllers/first_controller/Mode_Manual.m index 855bbc5..e6b3f99 100644 --- a/simulation/controllers/first_controller/Mode_Manual.m +++ b/simulation/controllers/first_controller/Mode_Manual.m @@ -80,7 +80,7 @@ % plotting position of robot on a map plot(p(1, step), -p(3, step), 'ro'); hold on; - plot(0.1, 0.2, 'bx'); + plot(pose_enc(1, step), -pose_enc(2, step), 'bx'); hold on; % plot(pose_enc(1, step), pose_enc(2, step), 'bx'); % hold on; diff --git a/simulation/controllers/first_controller/enc2pose.m b/simulation/controllers/first_controller/enc2pose.m index 1c85eed..bf137bf 100644 --- a/simulation/controllers/first_controller/enc2pose.m +++ b/simulation/controllers/first_controller/enc2pose.m @@ -9,8 +9,8 @@ center_dist = 0; end - x = prev_x + center_dist*cos(prev_theta); - z = prev_z + center_dist*sin(prev_theta); + z = prev_z + center_dist*cos(prev_theta); + x = prev_x + center_dist*sin(prev_theta); theta = prev_theta + (right_dist-left_dist)/(2*WHEEL_FROM_CENTER); end From 01062fa8f442db42236df1823a7dba935fe59aaa Mon Sep 17 00:00:00 2001 From: Dong-Jae Park Date: Fri, 1 Jan 2021 12:12:19 -0600 Subject: [PATCH 27/63] Fixed odometry not tracking backwards movement --- .../controllers/first_controller/Mode_Manual.m | 17 ++++++++++++----- .../controllers/first_controller/enc2pose.m | 4 ---- 2 files changed, 12 insertions(+), 9 deletions(-) diff --git a/simulation/controllers/first_controller/Mode_Manual.m b/simulation/controllers/first_controller/Mode_Manual.m index e6b3f99..bdd1547 100644 --- a/simulation/controllers/first_controller/Mode_Manual.m +++ b/simulation/controllers/first_controller/Mode_Manual.m @@ -54,6 +54,16 @@ diff_enc_left = new_enc_left - prev_enc_left; diff_enc_right = new_enc_right - prev_enc_right; + if abs(diff_enc_left) < 0.001 + diff_enc_left = 0; + end + + if abs(diff_enc_right) < 0.001 + diff_enc_right = 0; + end + + wb_console_print(sprintf('ENC Values: %g %g\n', new_enc_left, new_enc_right), WB_STDOUT); + prev_enc_left = new_enc_left; prev_enc_right = new_enc_right; @@ -74,7 +84,6 @@ %% SUPERVISOR % this is done repeatedly values = wb_supervisor_field_get_sf_vec3f(trans_field); -% wb_console_print(sprintf('MY_ROBOT is at position: %g %g %g\n', values(1), values(2), values(3)), WB_STDOUT); p(:, step) = values'; % plotting position of robot on a map @@ -82,14 +91,12 @@ hold on; plot(pose_enc(1, step), -pose_enc(2, step), 'bx'); hold on; -% plot(pose_enc(1, step), pose_enc(2, step), 'bx'); -% hold on; axis([-0.8 0.8 -0.6 0.6]); rectangle('Position',[-TABLE_WIDTH/2 -TABLE_HEIGHT/2 TABLE_WIDTH TABLE_HEIGHT]) hold off; - wb_console_print(sprintf('TRUE position: %g %g\n', p(1, step), -p(3, step)), WB_STDOUT); - wb_console_print(sprintf('ENC position: %g %g\n', pose_enc(1, step), pose_enc(2, step)), WB_STDOUT); +% wb_console_print(sprintf('TRUE position: %g %g\n', p(1, step), -p(3, step)), WB_STDOUT); +% wb_console_print(sprintf('ENC position: %g %g\n', pose_enc(1, step), pose_enc(2, step)), WB_STDOUT); % if your code plots some graphics, it needs to flushed like this: drawnow; diff --git a/simulation/controllers/first_controller/enc2pose.m b/simulation/controllers/first_controller/enc2pose.m index bf137bf..d0ee82e 100644 --- a/simulation/controllers/first_controller/enc2pose.m +++ b/simulation/controllers/first_controller/enc2pose.m @@ -5,10 +5,6 @@ right_dist = right_enc * ENC_UNIT; center_dist = (left_dist + right_dist) / 2; - if center_dist < 0.0001 - center_dist = 0; - end - z = prev_z + center_dist*cos(prev_theta); x = prev_x + center_dist*sin(prev_theta); theta = prev_theta + (right_dist-left_dist)/(2*WHEEL_FROM_CENTER); From cb810836494b6cff7cf74eb09055e7c87fb98c15 Mon Sep 17 00:00:00 2001 From: Dong-Jae Park Date: Fri, 1 Jan 2021 13:32:25 -0600 Subject: [PATCH 28/63] Testing noise and resolution values for the encoder --- simulation/controllers/first_controller/Parameters.m | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/simulation/controllers/first_controller/Parameters.m b/simulation/controllers/first_controller/Parameters.m index 3ff8d4f..8f721bc 100644 --- a/simulation/controllers/first_controller/Parameters.m +++ b/simulation/controllers/first_controller/Parameters.m @@ -16,8 +16,8 @@ %% SENSOR PARAMETERS % ENCODER -ENC_RESOLUTION = -1; -ENC_NOISE = 0; +ENC_RESOLUTION = 0.1; %[rad] +ENC_NOISE = 0.05; %[rad] ENC_UNIT = WHEEL_CIRCUM / 6.28; % IMU_GYRO From e8fe8554ea0e4ca92b0f622ece5d6b36416a23db Mon Sep 17 00:00:00 2001 From: Dong-Jae Park Date: Fri, 1 Jan 2021 13:33:18 -0600 Subject: [PATCH 29/63] Changed the data structure for the true robot position to align with pose estimation from encoder --- .../first_controller/Mode_Manual.m | 20 ++++++++++++------- 1 file changed, 13 insertions(+), 7 deletions(-) diff --git a/simulation/controllers/first_controller/Mode_Manual.m b/simulation/controllers/first_controller/Mode_Manual.m index bdd1547..892d693 100644 --- a/simulation/controllers/first_controller/Mode_Manual.m +++ b/simulation/controllers/first_controller/Mode_Manual.m @@ -1,6 +1,7 @@ % MAIN LOOP wb_console_print('Starting Manual Mode!', WB_STDOUT); +%% INITIALIZATION step = 1; samples = 0; @@ -62,8 +63,6 @@ diff_enc_right = 0; end - wb_console_print(sprintf('ENC Values: %g %g\n', new_enc_left, new_enc_right), WB_STDOUT); - prev_enc_left = new_enc_left; prev_enc_right = new_enc_right; @@ -83,11 +82,15 @@ %% SUPERVISOR % this is done repeatedly - values = wb_supervisor_field_get_sf_vec3f(trans_field); + position = wb_supervisor_field_get_sf_vec3f(trans_field); + orientation = wb_supervisor_field_get_sf_rotation(orien_field); + + p(1, step) = position(1); + p(2, step) = position(3); + p(3, step) = orientation(4); - p(:, step) = values'; % plotting position of robot on a map - plot(p(1, step), -p(3, step), 'ro'); + plot(p(1, step), -p(2, step), 'ro'); hold on; plot(pose_enc(1, step), -pose_enc(2, step), 'bx'); hold on; @@ -95,12 +98,15 @@ rectangle('Position',[-TABLE_WIDTH/2 -TABLE_HEIGHT/2 TABLE_WIDTH TABLE_HEIGHT]) hold off; -% wb_console_print(sprintf('TRUE position: %g %g\n', p(1, step), -p(3, step)), WB_STDOUT); -% wb_console_print(sprintf('ENC position: %g %g\n', pose_enc(1, step), pose_enc(2, step)), WB_STDOUT); + wb_console_print(sprintf('TRUE position: %g %g\n', p(1, step), -p(3, step)), WB_STDOUT); + wb_console_print(sprintf('ENC position: %g %g\n', pose_enc(1, step), pose_enc(2, step)), WB_STDOUT); % if your code plots some graphics, it needs to flushed like this: drawnow; end %% CLEAN UP +filename = "manual.mat"; +save(filename) + % cleanup code goes here: write data to files, etc. \ No newline at end of file From c3be3c8a63f0fb1e52e6bb88f6b64e8a63cd3f09 Mon Sep 17 00:00:00 2001 From: Dong-Jae Park Date: Fri, 1 Jan 2021 13:37:31 -0600 Subject: [PATCH 30/63] Updating autonomous mode and manual mode --- .../first_controller/Mode_Autonomous.m | 60 +++++++++++++++---- .../first_controller/Mode_Manual.m | 4 +- 2 files changed, 50 insertions(+), 14 deletions(-) diff --git a/simulation/controllers/first_controller/Mode_Autonomous.m b/simulation/controllers/first_controller/Mode_Autonomous.m index cdb4314..547c7e5 100644 --- a/simulation/controllers/first_controller/Mode_Autonomous.m +++ b/simulation/controllers/first_controller/Mode_Autonomous.m @@ -1,15 +1,27 @@ % MAIN LOOP wb_console_print('Starting Autonomous Mode!', WB_STDOUT); -step = 0; +%% INITIALIZATION +step = 1; samples = 0; +% initial pose of the robot +position = wb_supervisor_field_get_sf_vec3f(trans_field); +orientation = wb_supervisor_field_get_sf_rotation(orien_field); +pose = [position(1) position(3) orientation(4)]'; % [x y theta] +pose_enc(:, step) = pose; +p(:, step) = pose; + +% initializing values for encoder +prev_enc_left = 0; +prev_enc_right = 0; + while wb_robot_step(TIME_STEP) ~= -1 step = step + 1; % setting motor speeds - left_speed = 0.5 * MAX_SPEED; - right_speed = 1.5 * MAX_SPEED; + left_speed = 1.0 * MAX_SPEED; + right_speed = 2.0 * MAX_SPEED; for i=1:2 if i == 1 @@ -21,8 +33,25 @@ end %% ENCODER - value = wb_position_sensor_get_value(ps(1)); - type = wb_position_sensor_get_type(ps(1)); + new_enc_left = wb_position_sensor_get_value(ps(1)); + new_enc_right = wb_position_sensor_get_value(ps(2)); + + diff_enc_left = new_enc_left - prev_enc_left; + diff_enc_right = new_enc_right - prev_enc_right; + + if abs(diff_enc_left) < 0.001 + diff_enc_left = 0; + end + + if abs(diff_enc_right) < 0.001 + diff_enc_right = 0; + end + + prev_enc_left = new_enc_left; + prev_enc_right = new_enc_right; + + [x, z, theta] = enc2pose(diff_enc_left, diff_enc_right, pose_enc(1, step-1), pose_enc(2, step-1), pose_enc(3, step-1), ENC_UNIT, WHEEL_FROM_CENTER); + pose_enc(:, step) = [x, z, theta]'; %% TOF image = wb_range_finder_get_range_image(tof); @@ -30,23 +59,30 @@ %% IMU % IMU, Gyro roll_pitch_yaw_array = wb_inertial_unit_get_roll_pitch_yaw(imu_gyro); - roll_pitch_yaw_array % IMU, Acce - %% SUPERVISOR + %% PLOTTING % this is done repeatedly - values = wb_supervisor_field_get_sf_vec3f(trans_field); - wb_console_print(sprintf('MY_ROBOT is at position: %g %g %g\n', values(1), values(2), values(3)), WB_STDOUT); + position = wb_supervisor_field_get_sf_vec3f(trans_field); + orientation = wb_supervisor_field_get_sf_rotation(orien_field); + + p(1, step) = position(1); + p(2, step) = position(3); + p(3, step) = orientation(4); - p(step,:) = values; % plotting position of robot on a map - plot(p(step,1),-p(step,3),'ro'); + plot(p(1, step), -p(2, step), 'ro'); + hold on; + plot(pose_enc(1, step), -pose_enc(2, step), 'bx'); hold on; - axis([-1 1 -0.8 0.8]); + axis([-0.8 0.8 -0.6 0.6]); rectangle('Position',[-TABLE_WIDTH/2 -TABLE_HEIGHT/2 TABLE_WIDTH TABLE_HEIGHT]) hold off; + + wb_console_print(sprintf('TRUE position: %g %g\n', p(1, step), -p(3, step)), WB_STDOUT); + wb_console_print(sprintf('ENC position: %g %g\n', pose_enc(1, step), pose_enc(2, step)), WB_STDOUT); %% if your code plots some graphics, it needs to flushed like this: drawnow; diff --git a/simulation/controllers/first_controller/Mode_Manual.m b/simulation/controllers/first_controller/Mode_Manual.m index 892d693..e1cb030 100644 --- a/simulation/controllers/first_controller/Mode_Manual.m +++ b/simulation/controllers/first_controller/Mode_Manual.m @@ -80,7 +80,7 @@ % IMU, Acce x_y_z_array = wb_accelerometer_get_values(imu_acc); - %% SUPERVISOR + %% PLOTTING % this is done repeatedly position = wb_supervisor_field_get_sf_vec3f(trans_field); orientation = wb_supervisor_field_get_sf_rotation(orien_field); @@ -101,7 +101,7 @@ wb_console_print(sprintf('TRUE position: %g %g\n', p(1, step), -p(3, step)), WB_STDOUT); wb_console_print(sprintf('ENC position: %g %g\n', pose_enc(1, step), pose_enc(2, step)), WB_STDOUT); - % if your code plots some graphics, it needs to flushed like this: + %% if your code plots some graphics, it needs to flushed like this: drawnow; end From fa8cb0b4dc907435891c4f028fbeb1bcc2189f17 Mon Sep 17 00:00:00 2001 From: Dong-Jae Park Date: Sat, 2 Jan 2021 19:43:02 -0600 Subject: [PATCH 31/63] Using IMU values to estimate the position Will need to implement a double integration on the imu values. There will be signigicant errors due to the integration. --- simulation/controllers/first_controller/Mode_Manual.m | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/simulation/controllers/first_controller/Mode_Manual.m b/simulation/controllers/first_controller/Mode_Manual.m index e1cb030..1919f87 100644 --- a/simulation/controllers/first_controller/Mode_Manual.m +++ b/simulation/controllers/first_controller/Mode_Manual.m @@ -80,6 +80,14 @@ % IMU, Acce x_y_z_array = wb_accelerometer_get_values(imu_acc); + for i=1:3 + if abs(x_y_z_array(i)) < 0.0001 + x_y_z_array(i) = 0; + end + end + + wb_console_print(sprintf('ACC values: %g %g %g\n', x_y_z_array(1), x_y_z_array(2), x_y_z_array(3)), WB_STDOUT); + %% PLOTTING % this is done repeatedly position = wb_supervisor_field_get_sf_vec3f(trans_field); From 0e992333ba653d862a98e7f0708e950ef866d7cd Mon Sep 17 00:00:00 2001 From: Dong-Jae Park Date: Sun, 3 Jan 2021 14:48:55 -0600 Subject: [PATCH 32/63] Disableing matlab pop up --- simulation/controllers/first_controller/first_controller.m | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/simulation/controllers/first_controller/first_controller.m b/simulation/controllers/first_controller/first_controller.m index 1fa72f8..617babb 100644 --- a/simulation/controllers/first_controller/first_controller.m +++ b/simulation/controllers/first_controller/first_controller.m @@ -1,7 +1,7 @@ % uncomment the next two lines if you want to use % MATLAB's desktop to interact with the controller: -desktop; -keyboard; +% desktop; +% keyboard; % PARAMETERS Parameters; From d6d0c8b374fed183e31f20831ab1f175d2a4e7f8 Mon Sep 17 00:00:00 2001 From: Dong-Jae Park Date: Sun, 3 Jan 2021 14:50:34 -0600 Subject: [PATCH 33/63] Getting IMU acceleration values and plotting velocity and distance When in manual mode, the window keeps switching to matlab so no input is taken... --- .../first_controller/Mode_Autonomous.m | 38 ++++++++++++++++++- .../first_controller/Mode_Manual.m | 16 ++++++-- 2 files changed, 49 insertions(+), 5 deletions(-) diff --git a/simulation/controllers/first_controller/Mode_Autonomous.m b/simulation/controllers/first_controller/Mode_Autonomous.m index 547c7e5..15fec47 100644 --- a/simulation/controllers/first_controller/Mode_Autonomous.m +++ b/simulation/controllers/first_controller/Mode_Autonomous.m @@ -16,6 +16,11 @@ prev_enc_left = 0; prev_enc_right = 0; +% initial position and velocity for IMU +raw_imu_acc(:, step) = [0 0 0]'; +imu_vel(1) = 0; +imu_dis(1) = 0; + while wb_robot_step(TIME_STEP) ~= -1 step = step + 1; @@ -61,10 +66,23 @@ roll_pitch_yaw_array = wb_inertial_unit_get_roll_pitch_yaw(imu_gyro); % IMU, Acce + x_y_z_array = wb_accelerometer_get_values(imu_acc); + for i=1:3 + if abs(x_y_z_array(i)) < 0.0001 + x_y_z_array(i) = 0; + end + end + wb_console_print(sprintf('ACC values: %g %g %g\n', x_y_z_array(1), x_y_z_array(2), x_y_z_array(3)), WB_STDOUT); + raw_imu_acc(:, step) = x_y_z_array'; + + for i=2:step + imu_vel(i) = imu_vel(i-1) + (raw_imu_acc(1, i)+raw_imu_acc(1, i-1))/2; + imu_dis(i) = imu_dis(i-1) + imu_vel(i-1)+(imu_vel(i)+imu_vel(i-1))/2; + end %% PLOTTING - % this is done repeatedly + % Getting true position position = wb_supervisor_field_get_sf_vec3f(trans_field); orientation = wb_supervisor_field_get_sf_rotation(orien_field); @@ -72,7 +90,23 @@ p(2, step) = position(3); p(3, step) = orientation(4); - % plotting position of robot on a map + % Plotting IMU values + figure(1) + subplot(3,1,1); + plot(raw_imu_acc(1, 1:step)); + xlabel('Timestep'); + ylabel('Acceleration (m/s^2)'); + subplot(3,1,2); + plot(imu_vel(1:step)); + xlabel('Timestep'); + ylabel('Velocity (??)'); + subplot(3,1,3); + plot(imu_dis(1:step)); + xlabel('Timestep'); + ylabel('Distance (cm)'); + + % Plotting position of robot on a map (true, odometry) + figure(2) plot(p(1, step), -p(2, step), 'ro'); hold on; plot(pose_enc(1, step), -pose_enc(2, step), 'bx'); diff --git a/simulation/controllers/first_controller/Mode_Manual.m b/simulation/controllers/first_controller/Mode_Manual.m index 1919f87..cf37727 100644 --- a/simulation/controllers/first_controller/Mode_Manual.m +++ b/simulation/controllers/first_controller/Mode_Manual.m @@ -16,6 +16,11 @@ prev_enc_left = 0; prev_enc_right = 0; +% initial position and velocity for IMU +raw_imu_acc(:, step) = [0 0 0]'; +imu_vel(1) = 0; +imu_dis(1) = 0; + while wb_robot_step(TIME_STEP) ~= -1 step = step + 1; @@ -87,17 +92,22 @@ end wb_console_print(sprintf('ACC values: %g %g %g\n', x_y_z_array(1), x_y_z_array(2), x_y_z_array(3)), WB_STDOUT); + raw_imu_acc(:, step) = x_y_z_array'; %% PLOTTING - % this is done repeatedly + % Getting true position position = wb_supervisor_field_get_sf_vec3f(trans_field); orientation = wb_supervisor_field_get_sf_rotation(orien_field); - p(1, step) = position(1); p(2, step) = position(3); p(3, step) = orientation(4); - % plotting position of robot on a map + % Plotting IMU values + figure(1) + plot(raw_imu_acc(1, 1:step)); + + % Plotting position of robot on a map (true, odometry) + figure(2) plot(p(1, step), -p(2, step), 'ro'); hold on; plot(pose_enc(1, step), -pose_enc(2, step), 'bx'); From 6837db01bc3dc1f0a4d881ca5a6b17907ac2207a Mon Sep 17 00:00:00 2001 From: Dong-Jae Park Date: Sun, 3 Jan 2021 14:50:48 -0600 Subject: [PATCH 34/63] Changed the time step to be smaller --- simulation/controllers/first_controller/Parameters.m | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/simulation/controllers/first_controller/Parameters.m b/simulation/controllers/first_controller/Parameters.m index 8f721bc..a00e579 100644 --- a/simulation/controllers/first_controller/Parameters.m +++ b/simulation/controllers/first_controller/Parameters.m @@ -1,5 +1,5 @@ %% SIMULATION PARAMETERS -TIME_STEP = 64; +TIME_STEP = 32; %[ms] %% ROBOT PARAMETERS ROBOT_NAME = 'TableUV'; From 9546cfa4ef9f9dab6ffe1c72b6424075e0620964 Mon Sep 17 00:00:00 2001 From: Dong-Jae Park Date: Sun, 3 Jan 2021 22:04:27 -0600 Subject: [PATCH 35/63] Converting IMU data to pose estimation. Might need to roll back and use motion and sensor models for EKF. --- simulation/controllers/first_controller/imu2pose.m | 6 ++++++ 1 file changed, 6 insertions(+) create mode 100644 simulation/controllers/first_controller/imu2pose.m diff --git a/simulation/controllers/first_controller/imu2pose.m b/simulation/controllers/first_controller/imu2pose.m new file mode 100644 index 0000000..20972f4 --- /dev/null +++ b/simulation/controllers/first_controller/imu2pose.m @@ -0,0 +1,6 @@ +function [x, z] = imu2pose(acc_x, acc_z, vel_x, vel_z, prev_x, prev_z) + %imu2pose: Convert IMU values to robot pose + + +end + From 6a30b33b6bc519754ef9b44e38f770a26f62a207 Mon Sep 17 00:00:00 2001 From: Dong-Jae Park Date: Wed, 6 Jan 2021 11:54:04 -0600 Subject: [PATCH 36/63] Changing encoder resolution to be default --- simulation/controllers/first_controller/Parameters.m | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/simulation/controllers/first_controller/Parameters.m b/simulation/controllers/first_controller/Parameters.m index a00e579..4270266 100644 --- a/simulation/controllers/first_controller/Parameters.m +++ b/simulation/controllers/first_controller/Parameters.m @@ -16,8 +16,8 @@ %% SENSOR PARAMETERS % ENCODER -ENC_RESOLUTION = 0.1; %[rad] -ENC_NOISE = 0.05; %[rad] +ENC_RESOLUTION = -1; %[rad] +ENC_NOISE = 0; %[rad] ENC_UNIT = WHEEL_CIRCUM / 6.28; % IMU_GYRO @@ -39,6 +39,12 @@ TOF_NOISE = 0; TOF_RESOLUTION = -1; +% IR + +% COLLISION + + + From 50963779abaab62834fd5a9ad190153a986acce6 Mon Sep 17 00:00:00 2001 From: Dong-Jae Park Date: Wed, 6 Jan 2021 11:55:11 -0600 Subject: [PATCH 37/63] Implementing kinematic model with encoders Using kinematic model with encoders to determine the position of the robot. This has issues. --- .../first_controller/Mode_Autonomous.m | 31 ++++++--- .../first_controller/Mode_Manual.m | 67 ++++++++++++------- .../first_controller/enc2wheelvel.m | 9 +++ .../first_controller/kinematicModel.m | 13 ++++ 4 files changed, 85 insertions(+), 35 deletions(-) create mode 100644 simulation/controllers/first_controller/enc2wheelvel.m create mode 100644 simulation/controllers/first_controller/kinematicModel.m diff --git a/simulation/controllers/first_controller/Mode_Autonomous.m b/simulation/controllers/first_controller/Mode_Autonomous.m index 15fec47..d57178b 100644 --- a/simulation/controllers/first_controller/Mode_Autonomous.m +++ b/simulation/controllers/first_controller/Mode_Autonomous.m @@ -8,7 +8,7 @@ % initial pose of the robot position = wb_supervisor_field_get_sf_vec3f(trans_field); orientation = wb_supervisor_field_get_sf_rotation(orien_field); -pose = [position(1) position(3) orientation(4)]'; % [x y theta] +pose = [position(1) position(3) orientation(4)]'; % [x z theta] pose_enc(:, step) = pose; p(:, step) = pose; @@ -18,8 +18,11 @@ % initial position and velocity for IMU raw_imu_acc(:, step) = [0 0 0]'; -imu_vel(1) = 0; -imu_dis(1) = 0; +pose_imu(:, step) = pose; +imu_vel_x(1) = 0; +imu_dis_x(1) = 0; +imu_vel_z(1) = 0; +imu_dis_z(1) = 0; while wb_robot_step(TIME_STEP) ~= -1 step = step + 1; @@ -77,10 +80,18 @@ raw_imu_acc(:, step) = x_y_z_array'; for i=2:step - imu_vel(i) = imu_vel(i-1) + (raw_imu_acc(1, i)+raw_imu_acc(1, i-1))/2; - imu_dis(i) = imu_dis(i-1) + imu_vel(i-1)+(imu_vel(i)+imu_vel(i-1))/2; + imu_vel_x(i) = imu_vel_x(i-1) + (raw_imu_acc(1, i)+raw_imu_acc(1, i-1))/2; + imu_dis_x(i) = imu_dis_x(i-1) + imu_vel_x(i-1)+(imu_vel_x(i)+imu_vel_x(i-1))/2; + + imu_vel_z(i) = imu_vel_z(i-1) + (raw_imu_acc(1, i)+raw_imu_acc(1, i-1))/2; + imu_dis_z(i) = imu_dis_z(i-1) + imu_vel_z(i-1)+(imu_vel_z(i)+imu_vel_z(i-1))/2; end + pose_imu(1, step) = pose_imu(1, step-1) + imu_dis_x(step); + pose_imu(1, step) = pose_imu(1, step) / 100; + pose_imu(2, step) = pose_imu(2, step-1) + imu_dis_z(step); + pose_imu(2, step) = pose_imu(2, step) / 100; + %% PLOTTING % Getting true position position = wb_supervisor_field_get_sf_vec3f(trans_field); @@ -90,27 +101,29 @@ p(2, step) = position(3); p(3, step) = orientation(4); - % Plotting IMU values + %% Plotting IMU values figure(1) subplot(3,1,1); plot(raw_imu_acc(1, 1:step)); xlabel('Timestep'); ylabel('Acceleration (m/s^2)'); subplot(3,1,2); - plot(imu_vel(1:step)); + plot(imu_vel_x(1:step)); xlabel('Timestep'); ylabel('Velocity (??)'); subplot(3,1,3); - plot(imu_dis(1:step)); + plot(imu_dis_x(1:step)); xlabel('Timestep'); ylabel('Distance (cm)'); - % Plotting position of robot on a map (true, odometry) + %% Plotting position of robot on a map (true, odometry) figure(2) plot(p(1, step), -p(2, step), 'ro'); hold on; plot(pose_enc(1, step), -pose_enc(2, step), 'bx'); hold on; + plot(pose_imu(1, step), -pose_imu(2, step), 'g.'); + hold on; axis([-0.8 0.8 -0.6 0.6]); rectangle('Position',[-TABLE_WIDTH/2 -TABLE_HEIGHT/2 TABLE_WIDTH TABLE_HEIGHT]) hold off; diff --git a/simulation/controllers/first_controller/Mode_Manual.m b/simulation/controllers/first_controller/Mode_Manual.m index cf37727..bb45085 100644 --- a/simulation/controllers/first_controller/Mode_Manual.m +++ b/simulation/controllers/first_controller/Mode_Manual.m @@ -8,11 +8,11 @@ % initial pose of the robot position = wb_supervisor_field_get_sf_vec3f(trans_field); orientation = wb_supervisor_field_get_sf_rotation(orien_field); -pose = [position(1) position(3) orientation(4)]'; % [x y theta] -pose_enc(:, step) = pose; -p(:, step) = pose; +init_pose = [position(1) position(3) orientation(4)]'; % [x z theta] +true_pose(:, step) = init_pose; % initializing values for encoder +pose_enc(:, step) = init_pose; prev_enc_left = 0; prev_enc_right = 0; @@ -54,27 +54,40 @@ wb_motor_set_velocity(motor(2), right_speed*MAX_SPEED); %% ENCODER +% new_enc_left = wb_position_sensor_get_value(ps(1)); +% new_enc_right = wb_position_sensor_get_value(ps(2)); +% +% diff_enc_left = new_enc_left - prev_enc_left; +% diff_enc_right = new_enc_right - prev_enc_right; +% +% if abs(diff_enc_left) < 0.001 +% diff_enc_left = 0; +% end +% +% if abs(diff_enc_right) < 0.001 +% diff_enc_right = 0; +% end +% +% prev_enc_left = new_enc_left; +% prev_enc_right = new_enc_right; +% +% [x, z, theta] = enc2pose(diff_enc_left, diff_enc_right, pose_enc(1, step-1), pose_enc(2, step-1), pose_enc(3, step-1), ENC_UNIT, WHEEL_FROM_CENTER); +% pose_enc(:, step) = [x, z, theta]'; + + %% ENCODER TO POSE USING KINEMATIC MODEL new_enc_left = wb_position_sensor_get_value(ps(1)); new_enc_right = wb_position_sensor_get_value(ps(2)); diff_enc_left = new_enc_left - prev_enc_left; diff_enc_right = new_enc_right - prev_enc_right; - if abs(diff_enc_left) < 0.001 - diff_enc_left = 0; - end - - if abs(diff_enc_right) < 0.001 - diff_enc_right = 0; - end + W = enc2wheelvel(diff_enc_left, diff_enc_right, WHEEL_RADIUS, TIME_STEP); + mu = kinematicModel(pose_enc(:, step-1), W, TIME_STEP, WHEEL_RADIUS, WHEEL_FROM_CENTER); + pose_enc(:, step) = mu; prev_enc_left = new_enc_left; prev_enc_right = new_enc_right; - [x, z, theta] = enc2pose(diff_enc_left, diff_enc_right, pose_enc(1, step-1), pose_enc(2, step-1), pose_enc(3, step-1), ENC_UNIT, WHEEL_FROM_CENTER); - pose_enc(:, step) = [x, z, theta]'; - - %% TOF image = wb_range_finder_get_range_image(tof); @@ -91,33 +104,35 @@ end end - wb_console_print(sprintf('ACC values: %g %g %g\n', x_y_z_array(1), x_y_z_array(2), x_y_z_array(3)), WB_STDOUT); +% wb_console_print(sprintf('ACC values: %g %g %g\n', x_y_z_array(1), x_y_z_array(2), x_y_z_array(3)), WB_STDOUT); raw_imu_acc(:, step) = x_y_z_array'; %% PLOTTING % Getting true position position = wb_supervisor_field_get_sf_vec3f(trans_field); orientation = wb_supervisor_field_get_sf_rotation(orien_field); - p(1, step) = position(1); - p(2, step) = position(3); - p(3, step) = orientation(4); + true_pose(1, step) = position(1); + true_pose(2, step) = position(3); + true_pose(3, step) = orientation(4); + + acc_x = raw_imu_acc(1, 1:step); - % Plotting IMU values - figure(1) - plot(raw_imu_acc(1, 1:step)); + %% Plotting IMU values +% figure(1) +% plot(acc_x); - % Plotting position of robot on a map (true, odometry) + %% Plotting position of robot on a map (true, odometry) figure(2) - plot(p(1, step), -p(2, step), 'ro'); + plot(true_pose(1, step), -true_pose(2, step), 'ro'); hold on; plot(pose_enc(1, step), -pose_enc(2, step), 'bx'); hold on; axis([-0.8 0.8 -0.6 0.6]); rectangle('Position',[-TABLE_WIDTH/2 -TABLE_HEIGHT/2 TABLE_WIDTH TABLE_HEIGHT]) hold off; - - wb_console_print(sprintf('TRUE position: %g %g\n', p(1, step), -p(3, step)), WB_STDOUT); - wb_console_print(sprintf('ENC position: %g %g\n', pose_enc(1, step), pose_enc(2, step)), WB_STDOUT); +% +% wb_console_print(sprintf('TRUE position: %g %g\n', p(1, step), -p(3, step)), WB_STDOUT); +% wb_console_print(sprintf('ENC position: %g %g\n', pose_enc(1, step), pose_enc(2, step)), WB_STDOUT); %% if your code plots some graphics, it needs to flushed like this: drawnow; diff --git a/simulation/controllers/first_controller/enc2wheelvel.m b/simulation/controllers/first_controller/enc2wheelvel.m new file mode 100644 index 0000000..74c72c3 --- /dev/null +++ b/simulation/controllers/first_controller/enc2wheelvel.m @@ -0,0 +1,9 @@ +function W = enc2wheelvel(left_enc,right_enc,WHEEL_RADIUS,TIME_STEP) + %enc2wheelvel: convert encoder values (radians in webots) to wheel vel + left_radPsec = left_enc / (TIME_STEP * 0.001); + right_radPsec = right_enc / (TIME_STEP * 0.001); + + W = [left_radPsec * WHEEL_RADIUS; + right_radPsec * WHEEL_RADIUS]; +end + diff --git a/simulation/controllers/first_controller/kinematicModel.m b/simulation/controllers/first_controller/kinematicModel.m new file mode 100644 index 0000000..7b902d6 --- /dev/null +++ b/simulation/controllers/first_controller/kinematicModel.m @@ -0,0 +1,13 @@ +function X_curr = kinematicModel(X_prev,U,TIME_STEP,WHEEL_RADIUS,WHEEL_FROM_CENTER) + %kinematicModel + %X_prev = [x z theta]' + + r = WHEEL_RADIUS; + l = WHEEL_FROM_CENTER; + dt = TIME_STEP * 0.001; + + X_curr = [ X_prev(1) + (r*(U(1)+U(2)))*0.5*sin(X_prev(3))*dt; + X_prev(2) + (r*(U(1)+U(2)))*0.5*cos(X_prev(3))*dt; + X_prev(3) + (r*(U(2)-U(1)))/(2*l)*dt ]; +end + From 2d571896146931bd72500dc3d51571d3696961b7 Mon Sep 17 00:00:00 2001 From: Dong-Jae Park Date: Wed, 6 Jan 2021 11:55:44 -0600 Subject: [PATCH 38/63] Added spaces --- .../controllers/first_controller/enc2pose.m | 2 +- .../first_controller/first_controller.m | 4 ++-- simulation/worlds/.desk.wbproj | 9 +++------ simulation/worlds/desk.wbt | 16 ++++++++++------ 4 files changed, 16 insertions(+), 15 deletions(-) diff --git a/simulation/controllers/first_controller/enc2pose.m b/simulation/controllers/first_controller/enc2pose.m index d0ee82e..50aa6f0 100644 --- a/simulation/controllers/first_controller/enc2pose.m +++ b/simulation/controllers/first_controller/enc2pose.m @@ -4,7 +4,7 @@ left_dist = left_enc * ENC_UNIT; right_dist = right_enc * ENC_UNIT; center_dist = (left_dist + right_dist) / 2; - + z = prev_z + center_dist*cos(prev_theta); x = prev_x + center_dist*sin(prev_theta); theta = prev_theta + (right_dist-left_dist)/(2*WHEEL_FROM_CENTER); diff --git a/simulation/controllers/first_controller/first_controller.m b/simulation/controllers/first_controller/first_controller.m index 617babb..1fa72f8 100644 --- a/simulation/controllers/first_controller/first_controller.m +++ b/simulation/controllers/first_controller/first_controller.m @@ -1,7 +1,7 @@ % uncomment the next two lines if you want to use % MATLAB's desktop to interact with the controller: -% desktop; -% keyboard; +desktop; +keyboard; % PARAMETERS Parameters; diff --git a/simulation/worlds/.desk.wbproj b/simulation/worlds/.desk.wbproj index 0ec2794..3bcc24b 100644 --- a/simulation/worlds/.desk.wbproj +++ b/simulation/worlds/.desk.wbproj @@ -1,7 +1,7 @@ Webots Project File version R2021a -perspectives: 000000ff00000000fd000000030000000000000320000003dafc0100000004fc00000000ffffffff0000000000fffffffc0200000001fb00000012005300630065006e0065005400720065006501000000000000039f0000000000000000fb0000001e00480074006d006c0052006f0062006f007400570069006e0064006f00770000000000000003200000000000000000fb0000001e00480074006d006c0052006f0062006f007400570069006e0064006f00770000000000000003200000000000000000fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000006900ffffff00000001000003290000033afc0200000001fb0000001400540065007800740045006400690074006f007200000000150000033a0000004100ffffff0000000300000780000000cdfc0100000002fb0000000e0043006f006e0073006f006c006501000000000000073f0000000000000000fb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c0100000000000007800000006900ffffff000007800000030b00000004000000040000000100000008fc00000000 +perspectives: 000000ff00000000fd000000030000000000000320000003dafc0100000004fc00000000ffffffff0000000000fffffffc0200000001fb00000012005300630065006e0065005400720065006501000000000000039f0000000000000000fb0000001e00480074006d006c0052006f0062006f007400570069006e0064006f00770000000000000003200000000000000000fb0000001e00480074006d006c0052006f0062006f007400570069006e0064006f00770000000000000003200000000000000000fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000006900ffffff00000001000003290000033afc0200000001fb0000001400540065007800740045006400690074006f007200000000150000033a0000004100ffffff0000000300000780000000edfc0100000002fb0000000e0043006f006e0073006f006c006501000000000000073f0000000000000000fb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c0100000000000007800000006900ffffff00000780000002eb00000004000000040000000100000008fc00000000 simulationViewPerspectives: 000000ff0000000100000002000001b5000005c90100000002010000000101 -sceneTreePerspectives: 000000ff00000001000000020000014e000000fa0100000002010000000201 +sceneTreePerspectives: 000000ff0000000100000002000000c0000000fa0100000002010000000201 minimizedPerspectives: 000000ff00000000fd000000030000000000000320000003dafc0100000004fc00000000ffffffff0000000000fffffffc0200000001fb00000012005300630065006e0065005400720065006501000000000000039f0000000000000000fb0000001e00480074006d006c0052006f0062006f007400570069006e0064006f00770000000000000003200000000000000000fb0000001e00480074006d006c0052006f0062006f007400570069006e0064006f00770000000000000003200000000000000000fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000006900ffffff00000001000003290000033afc0200000001fb0000001400540065007800740045006400690074006f007200000000150000033a0000004100ffffff000000030000078000000123fc0100000002fb0000000e0043006f006e0073006f006c006501000000000000073f0000000000000000fb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c0100000000000007800000006900ffffff000007800000029d00000004000000040000000100000008fc00000000 maximizedDockId: -1 centralWidgetVisible: 1 @@ -11,7 +11,4 @@ orthographicViewHeight: 0.808765 textFiles: -1 globalOptionalRendering: CoordinateSystem::RangeFinderFrustums consoles: Console:All:All -renderingDevicePerspectives: e-puck:camera;1;1;0;0 -renderingDevicePerspectives: robot(1):e-puck:camera;1;1;0;0 -renderingDevicePerspectives: robot:range-finder;1;3.57813;0;0 -renderingDevicePerspectives: robot:tof;1;3.57813;0;0 +renderingDevicePerspectives: robot:tof;1;3.23438;0;0 diff --git a/simulation/worlds/desk.wbt b/simulation/worlds/desk.wbt index 9f7a30a..7725a91 100644 --- a/simulation/worlds/desk.wbt +++ b/simulation/worlds/desk.wbt @@ -3,8 +3,8 @@ WorldInfo { coordinateSystem "NUE" } Viewpoint { - orientation 0 1 0 3.1415926535897936 - position -0.00015111794010151723 0.7735005134284224 -0.883401221419889 + orientation 1 0 0 4.71238898038469 + position -0.00015111794010612745 2.833319187262137 2.5518379798511636e-09 follow "table" } TexturedBackground { @@ -82,14 +82,16 @@ DEF TableUV Robot { device [ DEF ROBOT_ENC_RIGHT PositionSensor { name "right_ps" + noise 0.05 + resolution 0.1 } RotationalMotor { name "right_motor" } ] endPoint Solid { - translation -0.04000000040198491 -0.009998878983036004 5.847401393102382e-09 - rotation 1.546138617612424e-08 -1.518241940524984e-08 0.9999999999999998 1.5699999530302227 + translation -0.04000000040198491 -0.009998878983036004 5.847401393102429e-09 + rotation 1.5461386176124645e-08 -1.518241940524986e-08 0.9999999999999998 1.569999953030221 children [ DEF WHEEL Shape { appearance PBRAppearance { @@ -115,14 +117,16 @@ DEF TableUV Robot { device [ DEF ROBOT_ENC_LEFT PositionSensor { name "left_ps" + noise 0.05 + resolution 0.1 } RotationalMotor { name "left_motor" } ] endPoint Solid { - translation 0.04 -0.00999889 4.164320000000035e-09 - rotation 9.142245450039296e-10 -1.1821754757087804e-09 1 1.57000004697403 + translation 0.04 -0.00999889 4.164320000000087e-09 + rotation 9.142245450039587e-10 -1.1821754757087817e-09 1 1.5700000469740278 children [ DEF WHEEL Shape { appearance PBRAppearance { From 82dfd59dd4b8a46ba7dfe31d05a7fa828a30afa2 Mon Sep 17 00:00:00 2001 From: Dong-Jae Park Date: Wed, 6 Jan 2021 13:28:44 -0600 Subject: [PATCH 39/63] Fixed enc2wheelvel for odometry pose estimation using kinematic model --- .../first_controller/Mode_Autonomous.m | 63 ++++++++----------- .../first_controller/Mode_Manual.m | 37 +++-------- .../first_controller/enc2wheelvel.m | 6 +- 3 files changed, 40 insertions(+), 66 deletions(-) diff --git a/simulation/controllers/first_controller/Mode_Autonomous.m b/simulation/controllers/first_controller/Mode_Autonomous.m index d57178b..f29ee08 100644 --- a/simulation/controllers/first_controller/Mode_Autonomous.m +++ b/simulation/controllers/first_controller/Mode_Autonomous.m @@ -8,17 +8,17 @@ % initial pose of the robot position = wb_supervisor_field_get_sf_vec3f(trans_field); orientation = wb_supervisor_field_get_sf_rotation(orien_field); -pose = [position(1) position(3) orientation(4)]'; % [x z theta] -pose_enc(:, step) = pose; -p(:, step) = pose; +init_pose = [position(1) position(3) orientation(4)]'; % [x z theta] +true_pose(:, step) = init_pose; % initializing values for encoder +pose_enc(:, step) = init_pose; prev_enc_left = 0; prev_enc_right = 0; % initial position and velocity for IMU raw_imu_acc(:, step) = [0 0 0]'; -pose_imu(:, step) = pose; +pose_imu(:, step) = init_pose; imu_vel_x(1) = 0; imu_dis_x(1) = 0; imu_vel_z(1) = 0; @@ -27,7 +27,7 @@ while wb_robot_step(TIME_STEP) ~= -1 step = step + 1; - % setting motor speeds + % circular movement left_speed = 1.0 * MAX_SPEED; right_speed = 2.0 * MAX_SPEED; @@ -40,27 +40,20 @@ end end - %% ENCODER + %% ENCODER TO POSE USING KINEMATIC MODEL new_enc_left = wb_position_sensor_get_value(ps(1)); new_enc_right = wb_position_sensor_get_value(ps(2)); diff_enc_left = new_enc_left - prev_enc_left; diff_enc_right = new_enc_right - prev_enc_right; - if abs(diff_enc_left) < 0.001 - diff_enc_left = 0; - end - - if abs(diff_enc_right) < 0.001 - diff_enc_right = 0; - end + W = enc2wheelvel(diff_enc_left, diff_enc_right, TIME_STEP); + mu = kinematicModel(pose_enc(:, step-1), W, TIME_STEP, WHEEL_RADIUS, WHEEL_FROM_CENTER); + pose_enc(:, step) = mu; prev_enc_left = new_enc_left; prev_enc_right = new_enc_right; - [x, z, theta] = enc2pose(diff_enc_left, diff_enc_right, pose_enc(1, step-1), pose_enc(2, step-1), pose_enc(3, step-1), ENC_UNIT, WHEEL_FROM_CENTER); - pose_enc(:, step) = [x, z, theta]'; - %% TOF image = wb_range_finder_get_range_image(tof); @@ -97,38 +90,36 @@ position = wb_supervisor_field_get_sf_vec3f(trans_field); orientation = wb_supervisor_field_get_sf_rotation(orien_field); - p(1, step) = position(1); - p(2, step) = position(3); - p(3, step) = orientation(4); + true_pose(1, step) = position(1); + true_pose(2, step) = position(3); + true_pose(3, step) = orientation(4); %% Plotting IMU values - figure(1) - subplot(3,1,1); - plot(raw_imu_acc(1, 1:step)); - xlabel('Timestep'); - ylabel('Acceleration (m/s^2)'); - subplot(3,1,2); - plot(imu_vel_x(1:step)); - xlabel('Timestep'); - ylabel('Velocity (??)'); - subplot(3,1,3); - plot(imu_dis_x(1:step)); - xlabel('Timestep'); - ylabel('Distance (cm)'); +% figure(1) +% subplot(3,1,1); +% plot(raw_imu_acc(1, 1:step)); +% xlabel('Timestep'); +% ylabel('Acceleration (m/s^2)'); +% subplot(3,1,2); +% plot(imu_vel_x(1:step)); +% xlabel('Timestep'); +% ylabel('Velocity (??)'); +% subplot(3,1,3); +% plot(imu_dis_x(1:step)); +% xlabel('Timestep'); +% ylabel('Distance (cm)'); %% Plotting position of robot on a map (true, odometry) figure(2) - plot(p(1, step), -p(2, step), 'ro'); + plot(true_pose(1, step), -true_pose(2, step), 'ro'); hold on; plot(pose_enc(1, step), -pose_enc(2, step), 'bx'); hold on; - plot(pose_imu(1, step), -pose_imu(2, step), 'g.'); - hold on; axis([-0.8 0.8 -0.6 0.6]); rectangle('Position',[-TABLE_WIDTH/2 -TABLE_HEIGHT/2 TABLE_WIDTH TABLE_HEIGHT]) hold off; - wb_console_print(sprintf('TRUE position: %g %g\n', p(1, step), -p(3, step)), WB_STDOUT); + wb_console_print(sprintf('TRUE position: %g %g\n', true_pose(1, step), -true_pose(3, step)), WB_STDOUT); wb_console_print(sprintf('ENC position: %g %g\n', pose_enc(1, step), pose_enc(2, step)), WB_STDOUT); %% if your code plots some graphics, it needs to flushed like this: diff --git a/simulation/controllers/first_controller/Mode_Manual.m b/simulation/controllers/first_controller/Mode_Manual.m index bb45085..dad1a3d 100644 --- a/simulation/controllers/first_controller/Mode_Manual.m +++ b/simulation/controllers/first_controller/Mode_Manual.m @@ -53,27 +53,6 @@ wb_motor_set_velocity(motor(1), left_speed*MAX_SPEED); wb_motor_set_velocity(motor(2), right_speed*MAX_SPEED); - %% ENCODER -% new_enc_left = wb_position_sensor_get_value(ps(1)); -% new_enc_right = wb_position_sensor_get_value(ps(2)); -% -% diff_enc_left = new_enc_left - prev_enc_left; -% diff_enc_right = new_enc_right - prev_enc_right; -% -% if abs(diff_enc_left) < 0.001 -% diff_enc_left = 0; -% end -% -% if abs(diff_enc_right) < 0.001 -% diff_enc_right = 0; -% end -% -% prev_enc_left = new_enc_left; -% prev_enc_right = new_enc_right; -% -% [x, z, theta] = enc2pose(diff_enc_left, diff_enc_right, pose_enc(1, step-1), pose_enc(2, step-1), pose_enc(3, step-1), ENC_UNIT, WHEEL_FROM_CENTER); -% pose_enc(:, step) = [x, z, theta]'; - %% ENCODER TO POSE USING KINEMATIC MODEL new_enc_left = wb_position_sensor_get_value(ps(1)); new_enc_right = wb_position_sensor_get_value(ps(2)); @@ -81,7 +60,15 @@ diff_enc_left = new_enc_left - prev_enc_left; diff_enc_right = new_enc_right - prev_enc_right; - W = enc2wheelvel(diff_enc_left, diff_enc_right, WHEEL_RADIUS, TIME_STEP); + if abs(diff_enc_left) < 0.001 + diff_enc_left = 0; + end + + if abs(diff_enc_right) < 0.001 + diff_enc_right = 0; + end + + W = enc2wheelvel(diff_enc_left, diff_enc_right, TIME_STEP); mu = kinematicModel(pose_enc(:, step-1), W, TIME_STEP, WHEEL_RADIUS, WHEEL_FROM_CENTER); pose_enc(:, step) = mu; @@ -103,8 +90,7 @@ x_y_z_array(i) = 0; end end - -% wb_console_print(sprintf('ACC values: %g %g %g\n', x_y_z_array(1), x_y_z_array(2), x_y_z_array(3)), WB_STDOUT); + raw_imu_acc(:, step) = x_y_z_array'; %% PLOTTING @@ -130,9 +116,6 @@ axis([-0.8 0.8 -0.6 0.6]); rectangle('Position',[-TABLE_WIDTH/2 -TABLE_HEIGHT/2 TABLE_WIDTH TABLE_HEIGHT]) hold off; -% -% wb_console_print(sprintf('TRUE position: %g %g\n', p(1, step), -p(3, step)), WB_STDOUT); -% wb_console_print(sprintf('ENC position: %g %g\n', pose_enc(1, step), pose_enc(2, step)), WB_STDOUT); %% if your code plots some graphics, it needs to flushed like this: drawnow; diff --git a/simulation/controllers/first_controller/enc2wheelvel.m b/simulation/controllers/first_controller/enc2wheelvel.m index 74c72c3..80f4b8b 100644 --- a/simulation/controllers/first_controller/enc2wheelvel.m +++ b/simulation/controllers/first_controller/enc2wheelvel.m @@ -1,9 +1,9 @@ -function W = enc2wheelvel(left_enc,right_enc,WHEEL_RADIUS,TIME_STEP) +function W = enc2wheelvel(left_enc,right_enc,TIME_STEP) %enc2wheelvel: convert encoder values (radians in webots) to wheel vel left_radPsec = left_enc / (TIME_STEP * 0.001); right_radPsec = right_enc / (TIME_STEP * 0.001); - W = [left_radPsec * WHEEL_RADIUS; - right_radPsec * WHEEL_RADIUS]; + W = [left_radPsec; + right_radPsec]; end From 190fa28f77f012f85ca4341363da007c151a14cd Mon Sep 17 00:00:00 2001 From: Dong-Jae Park Date: Wed, 6 Jan 2021 13:33:05 -0600 Subject: [PATCH 40/63] Function name change imu2pose -> imu_acc2vel --- simulation/controllers/first_controller/imu2pose.m | 6 ------ simulation/controllers/first_controller/imu_acc2vel.m | 6 ++++++ 2 files changed, 6 insertions(+), 6 deletions(-) delete mode 100644 simulation/controllers/first_controller/imu2pose.m create mode 100644 simulation/controllers/first_controller/imu_acc2vel.m diff --git a/simulation/controllers/first_controller/imu2pose.m b/simulation/controllers/first_controller/imu2pose.m deleted file mode 100644 index 20972f4..0000000 --- a/simulation/controllers/first_controller/imu2pose.m +++ /dev/null @@ -1,6 +0,0 @@ -function [x, z] = imu2pose(acc_x, acc_z, vel_x, vel_z, prev_x, prev_z) - %imu2pose: Convert IMU values to robot pose - - -end - diff --git a/simulation/controllers/first_controller/imu_acc2vel.m b/simulation/controllers/first_controller/imu_acc2vel.m new file mode 100644 index 0000000..0e268e8 --- /dev/null +++ b/simulation/controllers/first_controller/imu_acc2vel.m @@ -0,0 +1,6 @@ +function [x, z] = imu_acc2vel(acc_x, acc_z) + %imu2pose: Convert IMU values to robot pose + + +end + From 6cb1c08738e0cf1d57d1add726333ea827918e48 Mon Sep 17 00:00:00 2001 From: Dong-Jae Park Date: Wed, 6 Jan 2021 17:29:32 -0600 Subject: [PATCH 41/63] Changed filename to main --- .../controllers/first_controller/{first_controller.m => Main.m} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename simulation/controllers/first_controller/{first_controller.m => Main.m} (100%) diff --git a/simulation/controllers/first_controller/first_controller.m b/simulation/controllers/first_controller/Main.m similarity index 100% rename from simulation/controllers/first_controller/first_controller.m rename to simulation/controllers/first_controller/Main.m From 092ff120a0a33ee7381b335cfa91d5a4ec9d07dd Mon Sep 17 00:00:00 2001 From: Dong-Jae Park Date: Wed, 6 Jan 2021 17:37:32 -0600 Subject: [PATCH 42/63] Changed back to first_controller because of Wevots :anguished: --- .../controllers/first_controller/{Main.m => first_controller} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename simulation/controllers/first_controller/{Main.m => first_controller} (100%) diff --git a/simulation/controllers/first_controller/Main.m b/simulation/controllers/first_controller/first_controller similarity index 100% rename from simulation/controllers/first_controller/Main.m rename to simulation/controllers/first_controller/first_controller From 9789665b320f6ce99f623ff42b7b49501a63aeba Mon Sep 17 00:00:00 2001 From: Dong-Jae Park Date: Wed, 6 Jan 2021 17:38:23 -0600 Subject: [PATCH 43/63] Added the missing .m --- .../first_controller/{first_controller => first_controller.m} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename simulation/controllers/first_controller/{first_controller => first_controller.m} (100%) diff --git a/simulation/controllers/first_controller/first_controller b/simulation/controllers/first_controller/first_controller.m similarity index 100% rename from simulation/controllers/first_controller/first_controller rename to simulation/controllers/first_controller/first_controller.m From 95be4c9cc4ab4e30a291a3c6d9ab3bf7863215df Mon Sep 17 00:00:00 2001 From: Dong-Jae Park Date: Wed, 6 Jan 2021 21:24:52 -0600 Subject: [PATCH 44/63] Fixed velocity calculation from IMU acceleration --- simulation/controllers/first_controller/imu_acc2vel.m | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/simulation/controllers/first_controller/imu_acc2vel.m b/simulation/controllers/first_controller/imu_acc2vel.m index 0e268e8..ad381e2 100644 --- a/simulation/controllers/first_controller/imu_acc2vel.m +++ b/simulation/controllers/first_controller/imu_acc2vel.m @@ -1,6 +1,6 @@ -function [x, z] = imu_acc2vel(acc_x, acc_z) - %imu2pose: Convert IMU values to robot pose - +function vel_z = imu_acc2vel(acc_z, prev_vel_z, TIME_STEP) + dt = TIME_STEP * 0.001; + vel_z = prev_vel_z + acc_z*dt; end From 7813147932593b3828dc96a41577660517ea237c Mon Sep 17 00:00:00 2001 From: Dong-Jae Park Date: Wed, 6 Jan 2021 21:25:25 -0600 Subject: [PATCH 45/63] Kinematic model of differential drive that takes v and w as input --- .../first_controller/kinematicModel_vw.m | 13 +++++++++++++ 1 file changed, 13 insertions(+) create mode 100644 simulation/controllers/first_controller/kinematicModel_vw.m diff --git a/simulation/controllers/first_controller/kinematicModel_vw.m b/simulation/controllers/first_controller/kinematicModel_vw.m new file mode 100644 index 0000000..167e95d --- /dev/null +++ b/simulation/controllers/first_controller/kinematicModel_vw.m @@ -0,0 +1,13 @@ +function X_curr = kinematicModel_vw(X_prev ,U ,TIME_STEP) + % kinematicModel_vw + % X_prev = [x z theta]' + % U = [v_z w_y]' + + dt = TIME_STEP * 0.001; + + X_curr = [ X_prev(1) + U(1)*sin(X_prev(3))*dt; + X_prev(2) + U(1)*cos(X_prev(3))*dt; + X_prev(3) + U(2)*dt ]; + +end + From 13615b23f480fdbbfbaabcd994ef5b858da2ec88 Mon Sep 17 00:00:00 2001 From: Dong-Jae Park Date: Wed, 6 Jan 2021 21:26:06 -0600 Subject: [PATCH 46/63] Replaced inertial unit node to gyroscope node --- simulation/controllers/first_controller/Initialize.m | 2 +- simulation/controllers/first_controller/Supervisor.m | 3 --- 2 files changed, 1 insertion(+), 4 deletions(-) diff --git a/simulation/controllers/first_controller/Initialize.m b/simulation/controllers/first_controller/Initialize.m index 3f81191..5278b94 100644 --- a/simulation/controllers/first_controller/Initialize.m +++ b/simulation/controllers/first_controller/Initialize.m @@ -32,7 +32,7 @@ wb_console_print(sprintf('Checking IMU (Gyro)'), WB_STDOUT); imu_gyro_name = "imu_gyro"; imu_gyro = wb_robot_get_device(convertStringsToChars(imu_gyro_name)); -wb_inertial_unit_enable(imu_gyro, TIME_STEP); +wb_gyro_enable(imu_gyro, TIME_STEP); wb_console_print('DONE', WB_STDOUT); %% IMU, ACC diff --git a/simulation/controllers/first_controller/Supervisor.m b/simulation/controllers/first_controller/Supervisor.m index 2cdf084..fde8be0 100644 --- a/simulation/controllers/first_controller/Supervisor.m +++ b/simulation/controllers/first_controller/Supervisor.m @@ -63,16 +63,13 @@ wb_supervisor_field_set_sf_float(robot_tof_res, TOF_RESOLUTION); % IMU, Gyro -robot_imu_gyro_noise = wb_supervisor_node_get_field(c('ROBOT_IMU_GYRO'), 'noise'); robot_imu_gyro_res = wb_supervisor_node_get_field(c('ROBOT_IMU_GYRO'), 'resolution'); -wb_supervisor_field_set_sf_float(robot_imu_gyro_noise, IMU_GYRO_NOISE); wb_supervisor_field_set_sf_float(robot_imu_gyro_res, IMU_GYRO_RESOLUTION); % IMU, Acc robot_imu_acc_res = wb_supervisor_node_get_field(c('ROBOT_IMU_ACC'), 'resolution'); wb_supervisor_field_set_sf_float(robot_imu_acc_res, IMU_ACC_RESOLUTION); - %% TABLE SETUP table = wb_supervisor_node_get_from_def('TABLE'); if isNull(table) From 5859e1c9c51986a90b296a3ca6f6c73e5f399e06 Mon Sep 17 00:00:00 2001 From: Dong-Jae Park Date: Wed, 6 Jan 2021 21:28:38 -0600 Subject: [PATCH 47/63] Comments describing what the input U looks like --- simulation/controllers/first_controller/kinematicModel.m | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/simulation/controllers/first_controller/kinematicModel.m b/simulation/controllers/first_controller/kinematicModel.m index 7b902d6..0c4c8ff 100644 --- a/simulation/controllers/first_controller/kinematicModel.m +++ b/simulation/controllers/first_controller/kinematicModel.m @@ -1,6 +1,7 @@ function X_curr = kinematicModel(X_prev,U,TIME_STEP,WHEEL_RADIUS,WHEEL_FROM_CENTER) - %kinematicModel - %X_prev = [x z theta]' + % kinematicModel + % X_prev = [x z theta]' + % U = [w_left w_right]' r = WHEEL_RADIUS; l = WHEEL_FROM_CENTER; From 8032547f9a61ac38807fdc99644c55142a19a55d Mon Sep 17 00:00:00 2001 From: Dong-Jae Park Date: Wed, 6 Jan 2021 21:29:19 -0600 Subject: [PATCH 48/63] Manual & Autonomous mode now supports localization using encoders and IMU --- .../first_controller/Mode_Autonomous.m | 70 ++++++++----------- .../first_controller/Mode_Manual.m | 51 +++++++++----- 2 files changed, 62 insertions(+), 59 deletions(-) diff --git a/simulation/controllers/first_controller/Mode_Autonomous.m b/simulation/controllers/first_controller/Mode_Autonomous.m index f29ee08..0b41c4f 100644 --- a/simulation/controllers/first_controller/Mode_Autonomous.m +++ b/simulation/controllers/first_controller/Mode_Autonomous.m @@ -17,12 +17,9 @@ prev_enc_right = 0; % initial position and velocity for IMU -raw_imu_acc(:, step) = [0 0 0]'; pose_imu(:, step) = init_pose; -imu_vel_x(1) = 0; -imu_dis_x(1) = 0; -imu_vel_z(1) = 0; -imu_dis_z(1) = 0; +prev_imu_acc_z = 0; +prev_imu_vel_z = 0; while wb_robot_step(TIME_STEP) ~= -1 step = step + 1; @@ -59,31 +56,35 @@ %% IMU % IMU, Gyro - roll_pitch_yaw_array = wb_inertial_unit_get_roll_pitch_yaw(imu_gyro); + roll_pitch_yaw_array = wb_gyro_get_values(imu_gyro); + new_imu_gyro_y = roll_pitch_yaw_array(2); % IMU, Acce x_y_z_array = wb_accelerometer_get_values(imu_acc); - for i=1:3 - if abs(x_y_z_array(i)) < 0.0001 - x_y_z_array(i) = 0; - end - end + new_imu_acc_z = x_y_z_array(3); + + wb_console_print(sprintf('gyro: %g %g %g\n', roll_pitch_yaw_array(1), roll_pitch_yaw_array(2), roll_pitch_yaw_array(3)), WB_STDOUT); + wb_console_print(sprintf('acce: %g %g %g\n', x_y_z_array(1), x_y_z_array(2), x_y_z_array(3)), WB_STDOUT); - wb_console_print(sprintf('ACC values: %g %g %g\n', x_y_z_array(1), x_y_z_array(2), x_y_z_array(3)), WB_STDOUT); - raw_imu_acc(:, step) = x_y_z_array'; + vel_z = imu_acc2vel(new_imu_acc_z, prev_imu_vel_z, TIME_STEP); - for i=2:step - imu_vel_x(i) = imu_vel_x(i-1) + (raw_imu_acc(1, i)+raw_imu_acc(1, i-1))/2; - imu_dis_x(i) = imu_dis_x(i-1) + imu_vel_x(i-1)+(imu_vel_x(i)+imu_vel_x(i-1))/2; - - imu_vel_z(i) = imu_vel_z(i-1) + (raw_imu_acc(1, i)+raw_imu_acc(1, i-1))/2; - imu_dis_z(i) = imu_dis_z(i-1) + imu_vel_z(i-1)+(imu_vel_z(i)+imu_vel_z(i-1))/2; + if abs(new_imu_gyro_y) < 0.001 + new_imu_gyro_y = 0; end - pose_imu(1, step) = pose_imu(1, step-1) + imu_dis_x(step); - pose_imu(1, step) = pose_imu(1, step) / 100; - pose_imu(2, step) = pose_imu(2, step-1) + imu_dis_z(step); - pose_imu(2, step) = pose_imu(2, step) / 100; + if abs(vel_z) < 0.001 + vel_z = 0; + end + + U = [vel_z new_imu_gyro_y]'; + + wb_console_print(sprintf('U: %g %g\n', U(1), U(2)), WB_STDOUT); + + mu = kinematicModel_vw(pose_imu(:, step-1), U, TIME_STEP); + + pose_imu(:, step) = mu; + prev_imu_acc_z = new_imu_acc_z; + prev_imu_vel_z = vel_z; %% PLOTTING % Getting true position @@ -94,34 +95,19 @@ true_pose(2, step) = position(3); true_pose(3, step) = orientation(4); - %% Plotting IMU values -% figure(1) -% subplot(3,1,1); -% plot(raw_imu_acc(1, 1:step)); -% xlabel('Timestep'); -% ylabel('Acceleration (m/s^2)'); -% subplot(3,1,2); -% plot(imu_vel_x(1:step)); -% xlabel('Timestep'); -% ylabel('Velocity (??)'); -% subplot(3,1,3); -% plot(imu_dis_x(1:step)); -% xlabel('Timestep'); -% ylabel('Distance (cm)'); - %% Plotting position of robot on a map (true, odometry) - figure(2) + figure(1) plot(true_pose(1, step), -true_pose(2, step), 'ro'); hold on; plot(pose_enc(1, step), -pose_enc(2, step), 'bx'); hold on; + plot(pose_imu(1, step), -pose_imu(2, step), 'g.'); + hold on; axis([-0.8 0.8 -0.6 0.6]); rectangle('Position',[-TABLE_WIDTH/2 -TABLE_HEIGHT/2 TABLE_WIDTH TABLE_HEIGHT]) + legend("True Position", "Odometry", "IMU Estimation"); hold off; - wb_console_print(sprintf('TRUE position: %g %g\n', true_pose(1, step), -true_pose(3, step)), WB_STDOUT); - wb_console_print(sprintf('ENC position: %g %g\n', pose_enc(1, step), pose_enc(2, step)), WB_STDOUT); - %% if your code plots some graphics, it needs to flushed like this: drawnow; end diff --git a/simulation/controllers/first_controller/Mode_Manual.m b/simulation/controllers/first_controller/Mode_Manual.m index dad1a3d..cdc9ccf 100644 --- a/simulation/controllers/first_controller/Mode_Manual.m +++ b/simulation/controllers/first_controller/Mode_Manual.m @@ -17,9 +17,9 @@ prev_enc_right = 0; % initial position and velocity for IMU -raw_imu_acc(:, step) = [0 0 0]'; -imu_vel(1) = 0; -imu_dis(1) = 0; +pose_imu(:, step) = init_pose; +prev_imu_acc_z = 0; +prev_imu_vel_z = 0; while wb_robot_step(TIME_STEP) ~= -1 step = step + 1; @@ -80,41 +80,58 @@ %% IMU % IMU, Gyro - roll_pitch_yaw_array = wb_inertial_unit_get_roll_pitch_yaw(imu_gyro); + roll_pitch_yaw_array = wb_gyro_get_values(imu_gyro); + new_imu_gyro_y = roll_pitch_yaw_array(2); % IMU, Acce x_y_z_array = wb_accelerometer_get_values(imu_acc); + new_imu_acc_z = x_y_z_array(3); - for i=1:3 - if abs(x_y_z_array(i)) < 0.0001 - x_y_z_array(i) = 0; - end + wb_console_print(sprintf('gyro: %g %g %g\n', roll_pitch_yaw_array(1), roll_pitch_yaw_array(2), roll_pitch_yaw_array(3)), WB_STDOUT); + wb_console_print(sprintf('acce: %g %g %g\n', x_y_z_array(1), x_y_z_array(2), x_y_z_array(3)), WB_STDOUT); + + vel_z = imu_acc2vel(new_imu_acc_z, prev_imu_vel_z, TIME_STEP); + + if abs(new_imu_gyro_y) < 0.001 + new_imu_gyro_y = 0; + end + + if abs(vel_z) < 0.001 + vel_z = 0; end - - raw_imu_acc(:, step) = x_y_z_array'; + + U = [vel_z new_imu_gyro_y]'; + + wb_console_print(sprintf('U: %g %g\n', U(1), U(2)), WB_STDOUT); + + mu = kinematicModel_vw(pose_imu(:, step-1), U, TIME_STEP); + + pose_imu(:, step) = mu; + prev_imu_acc_z = new_imu_acc_z; + prev_imu_vel_z = vel_z; %% PLOTTING % Getting true position position = wb_supervisor_field_get_sf_vec3f(trans_field); orientation = wb_supervisor_field_get_sf_rotation(orien_field); + true_pose(1, step) = position(1); true_pose(2, step) = position(3); true_pose(3, step) = orientation(4); - acc_x = raw_imu_acc(1, 1:step); - - %% Plotting IMU values -% figure(1) -% plot(acc_x); - %% Plotting position of robot on a map (true, odometry) - figure(2) + figure(1) plot(true_pose(1, step), -true_pose(2, step), 'ro'); hold on; plot(pose_enc(1, step), -pose_enc(2, step), 'bx'); hold on; + plot(pose_imu(1, step), -pose_imu(2, step), 'g.'); + hold on; axis([-0.8 0.8 -0.6 0.6]); rectangle('Position',[-TABLE_WIDTH/2 -TABLE_HEIGHT/2 TABLE_WIDTH TABLE_HEIGHT]) + text(0.35,0.55,'True Position','Color','red'); + text(0.35,0.50,'Odometry','Color','blue'); + text(0.35,0.45,'IMU Estimation','Color','green'); hold off; %% if your code plots some graphics, it needs to flushed like this: From 1a7402d7669cb266abdd43aef778f9e00f3b6134 Mon Sep 17 00:00:00 2001 From: Dong-Jae Park Date: Wed, 6 Jan 2021 21:29:58 -0600 Subject: [PATCH 49/63] Added compass node to complete the full IMU --- simulation/worlds/.desk.wbproj | 8 ++++---- simulation/worlds/desk.wbt | 28 +++++++++++++--------------- 2 files changed, 17 insertions(+), 19 deletions(-) diff --git a/simulation/worlds/.desk.wbproj b/simulation/worlds/.desk.wbproj index 3bcc24b..96bc27f 100644 --- a/simulation/worlds/.desk.wbproj +++ b/simulation/worlds/.desk.wbproj @@ -1,7 +1,7 @@ Webots Project File version R2021a -perspectives: 000000ff00000000fd000000030000000000000320000003dafc0100000004fc00000000ffffffff0000000000fffffffc0200000001fb00000012005300630065006e0065005400720065006501000000000000039f0000000000000000fb0000001e00480074006d006c0052006f0062006f007400570069006e0064006f00770000000000000003200000000000000000fb0000001e00480074006d006c0052006f0062006f007400570069006e0064006f00770000000000000003200000000000000000fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000006900ffffff00000001000003290000033afc0200000001fb0000001400540065007800740045006400690074006f007200000000150000033a0000004100ffffff0000000300000780000000edfc0100000002fb0000000e0043006f006e0073006f006c006501000000000000073f0000000000000000fb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c0100000000000007800000006900ffffff00000780000002eb00000004000000040000000100000008fc00000000 -simulationViewPerspectives: 000000ff0000000100000002000001b5000005c90100000002010000000101 -sceneTreePerspectives: 000000ff0000000100000002000000c0000000fa0100000002010000000201 +perspectives: 000000ff00000000fd000000030000000000000320000003dafc0100000004fc00000000ffffffff0000000000fffffffc0200000001fb00000012005300630065006e0065005400720065006501000000000000039f0000000000000000fb0000001e00480074006d006c0052006f0062006f007400570069006e0064006f00770000000000000003200000000000000000fb0000001e00480074006d006c0052006f0062006f007400570069006e0064006f00770000000000000003200000000000000000fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000006900ffffff0000000100000329000003bdfc0200000001fb0000001400540065007800740045006400690074006f00720000000015000003bd0000004100ffffff000000030000078000000039fc0100000002fb0000000e0043006f006e0073006f006c006501000000000000073f0000000000000000fb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c0100000000000007800000006900ffffff00000780000003bd00000004000000040000000100000008fc00000000 +simulationViewPerspectives: 000000ff000000010000000200000168000006160100000002010000000101 +sceneTreePerspectives: 000000ff0000000100000002000000ea000000fa0100000002010000000201 minimizedPerspectives: 000000ff00000000fd000000030000000000000320000003dafc0100000004fc00000000ffffffff0000000000fffffffc0200000001fb00000012005300630065006e0065005400720065006501000000000000039f0000000000000000fb0000001e00480074006d006c0052006f0062006f007400570069006e0064006f00770000000000000003200000000000000000fb0000001e00480074006d006c0052006f0062006f007400570069006e0064006f00770000000000000003200000000000000000fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000006900ffffff00000001000003290000033afc0200000001fb0000001400540065007800740045006400690074006f007200000000150000033a0000004100ffffff000000030000078000000123fc0100000002fb0000000e0043006f006e0073006f006c006501000000000000073f0000000000000000fb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c0100000000000007800000006900ffffff000007800000029d00000004000000040000000100000008fc00000000 maximizedDockId: -1 centralWidgetVisible: 1 @@ -11,4 +11,4 @@ orthographicViewHeight: 0.808765 textFiles: -1 globalOptionalRendering: CoordinateSystem::RangeFinderFrustums consoles: Console:All:All -renderingDevicePerspectives: robot:tof;1;3.23438;0;0 +renderingDevicePerspectives: robot:tof;1;4.4375;0;0 diff --git a/simulation/worlds/desk.wbt b/simulation/worlds/desk.wbt index 7725a91..6045875 100644 --- a/simulation/worlds/desk.wbt +++ b/simulation/worlds/desk.wbt @@ -4,7 +4,7 @@ WorldInfo { } Viewpoint { orientation 1 0 0 4.71238898038469 - position -0.00015111794010612745 2.833319187262137 2.5518379798511636e-09 + position 0.0012745942137962314 3.412426525803567 2.5518378734743844e-09 follow "table" } TexturedBackground { @@ -23,16 +23,18 @@ DEF OBJECTS Group { ] } DEF TableUV Robot { - translation 0 0.786936 0 + translation -1.1919446714331297e-07 0.7869679433500472 -1.7674686513222782e-11 + rotation -0.0001482848968909727 4.058376648053027e-11 0.9999999890057946 4.417475680697026e-06 children [ DEF ROBOT_IMU_ACC Accelerometer { - rotation 0 1 0 -1.5708 name "imu_acc" } - DEF ROBOT_IMU_GYRO InertialUnit { - rotation 0 1 0 -1.5708 + DEF ROBOT_IMU_GYRO Gyro { name "imu_gyro" } + DEF ROBOT_IMU_COMP Compass { + name "imu_comp" + } DEF ROBOT_TOF RangeFinder { translation 0 0 0.02 rotation 0 1 0 3.14159 @@ -76,22 +78,20 @@ DEF TableUV Robot { } HingeJoint { jointParameters HingeJointParameters { - position 7.414501687456142e-08 + position 7.414499819166283e-08 anchor -0.04 -0.01 0 } device [ DEF ROBOT_ENC_RIGHT PositionSensor { name "right_ps" - noise 0.05 - resolution 0.1 } RotationalMotor { name "right_motor" } ] endPoint Solid { - translation -0.04000000040198491 -0.009998878983036004 5.847401393102429e-09 - rotation 1.5461386176124645e-08 -1.518241940524986e-08 0.9999999999999998 1.569999953030221 + translation -0.04000000040198491 -0.009998878983036004 5.8474013930815415e-09 + rotation 1.5461376827233608e-08 -1.518241006380056e-08 0.9999999999999998 1.569999953030221 children [ DEF WHEEL Shape { appearance PBRAppearance { @@ -111,22 +111,20 @@ DEF TableUV Robot { } HingeJoint { jointParameters HingeJointParameters { - position 3.187095446534079e-08 + position 3.187094133621833e-08 anchor 0.04 -0.01 0 } device [ DEF ROBOT_ENC_LEFT PositionSensor { name "left_ps" - noise 0.05 - resolution 0.1 } RotationalMotor { name "left_motor" } ] endPoint Solid { - translation 0.04 -0.00999889 4.164320000000087e-09 - rotation 9.142245450039587e-10 -1.1821754757087817e-09 1 1.5700000469740278 + translation 0.04 -0.00999889 4.164319999985571e-09 + rotation 9.142179752134203e-10 -1.182168911147554e-09 1 1.5700000469740278 children [ DEF WHEEL Shape { appearance PBRAppearance { From 2e9e59a9812fdbe570d83b67f5baa2ddbd5a6e8d Mon Sep 17 00:00:00 2001 From: Dong-Jae Park Date: Wed, 6 Jan 2021 21:30:09 -0600 Subject: [PATCH 50/63] Deleted file. --- .../controllers/my_controller/my_controller.m | 32 ------------------- 1 file changed, 32 deletions(-) delete mode 100644 simulation/controllers/my_controller/my_controller.m diff --git a/simulation/controllers/my_controller/my_controller.m b/simulation/controllers/my_controller/my_controller.m deleted file mode 100644 index 8bbbf61..0000000 --- a/simulation/controllers/my_controller/my_controller.m +++ /dev/null @@ -1,32 +0,0 @@ -% uncomment the next two lines if you want to use -% MATLAB's desktop to interact with the controller: -desktop; -keyboard; - -TIME_STEP = 64; - -% get and enable devices, e.g.: -% camera = wb_robot_get_device('camera'); -% wb_camera_enable(camera, TIME_STEP); -motor = wb_robot_get_device('motor'); - -% main loop: -% perform simulation steps of TIME_STEP milliseconds -% and leave the loop when Webots signals the termination -% -while wb_robot_step(TIME_STEP) ~= -1 - - % read the sensors, e.g.: - % rgb = wb_camera_get_image(camera); - - % Process here sensor data, images, etc. - - % send actuator commands, e.g.: - wb_motor_set_postion(motor, 10.0); - - % if your code plots some graphics, it needs to flushed like this: - drawnow; - -end - -% cleanup code goes here: write data to files, etc. From 5084a9d824a90c23dbfada00bb1daaf2ffbef5c9 Mon Sep 17 00:00:00 2001 From: Dong-Jae Park Date: Wed, 6 Jan 2021 21:34:54 -0600 Subject: [PATCH 51/63] Updated .gitignore --- .gitignore | 1 + simulation/worlds/.desk.wbproj | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/.gitignore b/.gitignore index 27e6232..de6a6a0 100644 --- a/.gitignore +++ b/.gitignore @@ -1,2 +1,3 @@ *.asv +simulation/worlds/.desk.wbproj diff --git a/simulation/worlds/.desk.wbproj b/simulation/worlds/.desk.wbproj index 96bc27f..69c209b 100644 --- a/simulation/worlds/.desk.wbproj +++ b/simulation/worlds/.desk.wbproj @@ -1,5 +1,5 @@ Webots Project File version R2021a -perspectives: 000000ff00000000fd000000030000000000000320000003dafc0100000004fc00000000ffffffff0000000000fffffffc0200000001fb00000012005300630065006e0065005400720065006501000000000000039f0000000000000000fb0000001e00480074006d006c0052006f0062006f007400570069006e0064006f00770000000000000003200000000000000000fb0000001e00480074006d006c0052006f0062006f007400570069006e0064006f00770000000000000003200000000000000000fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000006900ffffff0000000100000329000003bdfc0200000001fb0000001400540065007800740045006400690074006f00720000000015000003bd0000004100ffffff000000030000078000000039fc0100000002fb0000000e0043006f006e0073006f006c006501000000000000073f0000000000000000fb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c0100000000000007800000006900ffffff00000780000003bd00000004000000040000000100000008fc00000000 +perspectives: 000000ff00000000fd000000030000000000000320000003dafc0100000004fc00000000ffffffff0000000000fffffffc0200000001fb00000012005300630065006e0065005400720065006501000000000000039f0000000000000000fb0000001e00480074006d006c0052006f0062006f007400570069006e0064006f00770000000000000003200000000000000000fb0000001e00480074006d006c0052006f0062006f007400570069006e0064006f00770000000000000003200000000000000000fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000006900ffffff0000000100000329000003bdfc0200000001fb0000001400540065007800740045006400690074006f00720000000015000003bd0000004100ffffff0000000300000780000000a1fc0100000002fb0000000e0043006f006e0073006f006c006501000000000000073f0000000000000000fb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c0100000000000007800000006900ffffff000007800000033700000004000000040000000100000008fc00000000 simulationViewPerspectives: 000000ff000000010000000200000168000006160100000002010000000101 sceneTreePerspectives: 000000ff0000000100000002000000ea000000fa0100000002010000000201 minimizedPerspectives: 000000ff00000000fd000000030000000000000320000003dafc0100000004fc00000000ffffffff0000000000fffffffc0200000001fb00000012005300630065006e0065005400720065006501000000000000039f0000000000000000fb0000001e00480074006d006c0052006f0062006f007400570069006e0064006f00770000000000000003200000000000000000fb0000001e00480074006d006c0052006f0062006f007400570069006e0064006f00770000000000000003200000000000000000fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000006900ffffff00000001000003290000033afc0200000001fb0000001400540065007800740045006400690074006f007200000000150000033a0000004100ffffff000000030000078000000123fc0100000002fb0000000e0043006f006e0073006f006c006501000000000000073f0000000000000000fb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c0100000000000007800000006900ffffff000007800000029d00000004000000040000000100000008fc00000000 From 5a7f8841019cd39e9c1b3f02480f8556e53ae6c5 Mon Sep 17 00:00:00 2001 From: Dong-Jae Park Date: Sat, 9 Jan 2021 21:06:32 -0600 Subject: [PATCH 52/63] Setting the initial position and orientation of the robot --- simulation/controllers/first_controller/Supervisor.m | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/simulation/controllers/first_controller/Supervisor.m b/simulation/controllers/first_controller/Supervisor.m index fde8be0..76c8a94 100644 --- a/simulation/controllers/first_controller/Supervisor.m +++ b/simulation/controllers/first_controller/Supervisor.m @@ -14,9 +14,11 @@ c(char(robot_settings(index))) = node; end -% Robot True position +% Robot Initial Position trans_field = wb_supervisor_node_get_field(c(ROBOT_NAME), 'translation'); orien_field = wb_supervisor_node_get_field(c(ROBOT_NAME), 'rotation'); +wb_supervisor_field_set_sf_vec3f(trans_field, [0 0.787 0]) +wb_supervisor_field_set_sf_rotation(orien_field, [0 1 0 0]) % Robot left wheel left_wheel_rad = wb_supervisor_node_get_field(c('ROBOT_WHEEL_LEFT'), 'radius'); From b688451a2fe9cef061fa2e3fb924230aa7b6e88e Mon Sep 17 00:00:00 2001 From: Dong-Jae Park Date: Sat, 9 Jan 2021 21:07:37 -0600 Subject: [PATCH 53/63] Using the InertialUnit node for the Magnetometer model. --- .../controllers/first_controller/Initialize.m | 12 ++++++++-- .../controllers/first_controller/Parameters.m | 4 +++- .../controllers/first_controller/Supervisor.m | 8 ++++++- simulation/worlds/desk.wbt | 22 +++++++++---------- 4 files changed, 31 insertions(+), 15 deletions(-) diff --git a/simulation/controllers/first_controller/Initialize.m b/simulation/controllers/first_controller/Initialize.m index 5278b94..a4c15bd 100644 --- a/simulation/controllers/first_controller/Initialize.m +++ b/simulation/controllers/first_controller/Initialize.m @@ -28,20 +28,28 @@ wb_range_finder_enable(tof, TIME_STEP); wb_console_print('DONE', WB_STDOUT); -%% IMU, GYRO +%% IMU +% GYRO wb_console_print(sprintf('Checking IMU (Gyro)'), WB_STDOUT); imu_gyro_name = "imu_gyro"; imu_gyro = wb_robot_get_device(convertStringsToChars(imu_gyro_name)); wb_gyro_enable(imu_gyro, TIME_STEP); wb_console_print('DONE', WB_STDOUT); -%% IMU, ACC +% ACC wb_console_print(sprintf('Checking IMU (Acc)'), WB_STDOUT); imu_acc_name = "imu_acc"; imu_acc = wb_robot_get_device(convertStringsToChars(imu_acc_name)); wb_accelerometer_enable(imu_acc, TIME_STEP); wb_console_print('DONE', WB_STDOUT); +% MAG +wb_console_print(sprintf('Checking IMU (Mag)'), WB_STDOUT); +imu_mag_name = "imu_mag"; +imu_mag = wb_robot_get_device(convertStringsToChars(imu_mag_name)); +wb_inertial_unit_enable(imu_mag, TIME_STEP); +wb_console_print('DONE', WB_STDOUT); + %% KEYBOARD INPUT wb_console_print(sprintf('Checking Keyboard'), WB_STDOUT); wb_keyboard_enable(TIME_STEP) diff --git a/simulation/controllers/first_controller/Parameters.m b/simulation/controllers/first_controller/Parameters.m index 4270266..1b0482d 100644 --- a/simulation/controllers/first_controller/Parameters.m +++ b/simulation/controllers/first_controller/Parameters.m @@ -22,11 +22,13 @@ % IMU_GYRO IMU_GYRO_RESOLUTION = -1; -IMU_GYRO_NOISE = 0; % IMU_ACC IMU_ACC_RESOLUTION = -1; +% IMU_MAG +IMU_MAG_RESOLUTION = -1; + % TOF TOF_FOV = 0.471; %[rad] TOF_WIDTH = 64; %[px] diff --git a/simulation/controllers/first_controller/Supervisor.m b/simulation/controllers/first_controller/Supervisor.m index 76c8a94..7c0d492 100644 --- a/simulation/controllers/first_controller/Supervisor.m +++ b/simulation/controllers/first_controller/Supervisor.m @@ -3,7 +3,9 @@ c = containers.Map; robot_settings = { ROBOT_NAME, 'ROBOT_WHEEL_LEFT', 'ROBOT_WHEEL_RIGHT', ... 'ROBOT_BASE', 'ROBOT_ENC_LEFT', 'ROBOT_ENC_RIGHT', ... - 'ROBOT_TOF' , 'ROBOT_IMU_GYRO', 'ROBOT_IMU_ACC'}; + 'ROBOT_TOF' , 'ROBOT_IMU_GYRO', 'ROBOT_IMU_ACC', ... + 'ROBOT_IMU_MAG'}; + for index = 1:length(robot_settings) setting = char(robot_settings(index)); node = wb_supervisor_node_get_from_def(setting); @@ -72,6 +74,10 @@ robot_imu_acc_res = wb_supervisor_node_get_field(c('ROBOT_IMU_ACC'), 'resolution'); wb_supervisor_field_set_sf_float(robot_imu_acc_res, IMU_ACC_RESOLUTION); +% IMU, Mag +robot_imu_mag_res = wb_supervisor_node_get_field(c('ROBOT_IMU_MAG'), 'resolution'); +wb_supervisor_field_set_sf_float(robot_imu_mag_res, IMU_MAG_RESOLUTION); + %% TABLE SETUP table = wb_supervisor_node_get_from_def('TABLE'); if isNull(table) diff --git a/simulation/worlds/desk.wbt b/simulation/worlds/desk.wbt index 6045875..754831f 100644 --- a/simulation/worlds/desk.wbt +++ b/simulation/worlds/desk.wbt @@ -4,7 +4,7 @@ WorldInfo { } Viewpoint { orientation 1 0 0 4.71238898038469 - position 0.0012745942137962314 3.412426525803567 2.5518378734743844e-09 + position -0.00015104981722547305 3.1386728803420745 1.22777122401446e-08 follow "table" } TexturedBackground { @@ -23,8 +23,8 @@ DEF OBJECTS Group { ] } DEF TableUV Robot { - translation -1.1919446714331297e-07 0.7869679433500472 -1.7674686513222782e-11 - rotation -0.0001482848968909727 4.058376648053027e-11 0.9999999890057946 4.417475680697026e-06 + translation -1.1922127978489346e-07 0.7869679750092429 -1.767872493506352e-11 + rotation -0.00014828549525515916 1.630439687735275e-11 0.9999999890057059 4.417476471929301e-06 children [ DEF ROBOT_IMU_ACC Accelerometer { name "imu_acc" @@ -32,8 +32,8 @@ DEF TableUV Robot { DEF ROBOT_IMU_GYRO Gyro { name "imu_gyro" } - DEF ROBOT_IMU_COMP Compass { - name "imu_comp" + DEF ROBOT_IMU_MAG InertialUnit { + name "imu_mag" } DEF ROBOT_TOF RangeFinder { translation 0 0 0.02 @@ -78,7 +78,7 @@ DEF TableUV Robot { } HingeJoint { jointParameters HingeJointParameters { - position 7.414499819166283e-08 + position 7.41449794324498e-08 anchor -0.04 -0.01 0 } device [ @@ -90,8 +90,8 @@ DEF TableUV Robot { } ] endPoint Solid { - translation -0.04000000040198491 -0.009998878983036004 5.8474013930815415e-09 - rotation 1.5461376827233608e-08 -1.518241006380056e-08 0.9999999999999998 1.569999953030221 + translation -0.04000000040198491 -0.009998878983036004 5.847401392695447e-09 + rotation 1.5461368107707866e-08 -1.518240091980168e-08 0.9999999999999998 1.5699999530302196 children [ DEF WHEEL Shape { appearance PBRAppearance { @@ -111,7 +111,7 @@ DEF TableUV Robot { } HingeJoint { jointParameters HingeJointParameters { - position 3.187094133621833e-08 + position 3.187092853206427e-08 anchor 0.04 -0.01 0 } device [ @@ -123,8 +123,8 @@ DEF TableUV Robot { } ] endPoint Solid { - translation 0.04 -0.00999889 4.164319999985571e-09 - rotation 9.142179752134203e-10 -1.182168911147554e-09 1 1.5700000469740278 + translation 0.04 -0.00999889 4.164319999411153e-09 + rotation 9.142114502326543e-10 -1.1821627446781977e-09 1 1.5700000469740278 children [ DEF WHEEL Shape { appearance PBRAppearance { From 93349908d8e52dd51ef7148d983122cb3988b06c Mon Sep 17 00:00:00 2001 From: Dong-Jae Park Date: Sat, 9 Jan 2021 21:09:11 -0600 Subject: [PATCH 54/63] Updating .gitignore and small changes --- .gitignore | 2 ++ simulation/controllers/first_controller/first_controller.m | 2 ++ 2 files changed, 4 insertions(+) diff --git a/.gitignore b/.gitignore index de6a6a0..8448974 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,5 @@ *.asv simulation/worlds/.desk.wbproj +simulation/controllers/first_controller/test_plotting.m +simulation/controllers/first_controller/manual.mat diff --git a/simulation/controllers/first_controller/first_controller.m b/simulation/controllers/first_controller/first_controller.m index 1fa72f8..47a75ac 100644 --- a/simulation/controllers/first_controller/first_controller.m +++ b/simulation/controllers/first_controller/first_controller.m @@ -1,3 +1,5 @@ +clear; clc; + % uncomment the next two lines if you want to use % MATLAB's desktop to interact with the controller: desktop; From 8b71626cec7b2e8a69c13e229e6c75a295a4279e Mon Sep 17 00:00:00 2001 From: Dong-Jae Park Date: Sat, 9 Jan 2021 21:09:29 -0600 Subject: [PATCH 55/63] Implementation of Kalman Filter taken from MTE 544 --- .../first_controller/kalman_filter.m | 44 +++++++++++++++++++ 1 file changed, 44 insertions(+) create mode 100644 simulation/controllers/first_controller/kalman_filter.m diff --git a/simulation/controllers/first_controller/kalman_filter.m b/simulation/controllers/first_controller/kalman_filter.m new file mode 100644 index 0000000..0f17c36 --- /dev/null +++ b/simulation/controllers/first_controller/kalman_filter.m @@ -0,0 +1,44 @@ +function [mu,S,mup,Sp,K] = kalman_filter(ssm,mu,S,u,y) + +% Performs one iteration of Kalman Filtering + +% Inputs: +% ssm : State space model structure +% mu : Mean of current state +% S : Covariance of current state +% u : Control Input of current time step +% y : Measurment of current time step +% example : Example under consideration +% t : Current time step +% freq : Multirate kalman filter frequency +% Outputs: +% mu : Final estimated mean after iteration +% S : Final estimated covariance after iteration +% mup : Predicted mean after motion update +% Sp : Predicted covariance after motion update +% K : Kalman gain for current time step + +% State space model structure +% x[n] = A*x[n-1] + B*u[n] + R (gaussian disturbance) +% y[n] = C*x[n] + D*u[n] + Q (gaussian disturbance) + +% Unwrap state space model +A = ssm.A; +B = ssm.B; +C = ssm.C; +D = ssm.D; +R = ssm.R; +Q = ssm.Q; +n = ssm.n; +m = ssm.m; + +% Prediction Step: ------------------------------------------------ +% Predict new mean based on motion +mup = A*mu + B*u; +% Predict new covariance based on motion +Sp = A*S*A' + R; + +% Measurement Step: ------------------------------------------------ +K = Sp*C'*inv(C*Sp*C'+Q); +mu = mup + K*(y-C*mup); +S = (eye(n)-K*C)*Sp; From 8a7038c2a986f19f06311f8b29611a8e9d7791e0 Mon Sep 17 00:00:00 2001 From: Dong-Jae Park Date: Sat, 9 Jan 2021 21:11:05 -0600 Subject: [PATCH 56/63] Setting up Kalman Filter for orientation estimation using Gyro and Magnetometer --- .../first_controller/Mode_Manual.m | 47 +++++++++++++++---- .../first_controller/measurement_model.m | 7 +++ .../first_controller/motion_model.m | 9 ++++ 3 files changed, 53 insertions(+), 10 deletions(-) create mode 100644 simulation/controllers/first_controller/measurement_model.m create mode 100644 simulation/controllers/first_controller/motion_model.m diff --git a/simulation/controllers/first_controller/Mode_Manual.m b/simulation/controllers/first_controller/Mode_Manual.m index cdc9ccf..8dfd2b2 100644 --- a/simulation/controllers/first_controller/Mode_Manual.m +++ b/simulation/controllers/first_controller/Mode_Manual.m @@ -21,6 +21,21 @@ prev_imu_acc_z = 0; prev_imu_vel_z = 0; +% kf +[A,B,R,n] = motion_model(TIME_STEP); +prev_orient = orientation(4); + +[C,D,Q,m] = measurement_model(); + +ssm.A = A; +ssm.B = B; +ssm.C = C; +ssm.D = D; +ssm.R = R; +ssm.Q = Q; +ssm.n = n; +ssm.m = m; + while wb_robot_step(TIME_STEP) ~= -1 step = step + 1; @@ -78,7 +93,26 @@ %% TOF image = wb_range_finder_get_range_image(tof); - %% IMU + %% Kalman Filter + orientation = wb_supervisor_field_get_sf_rotation(orien_field); + + U = wb_gyro_get_values(imu_gyro); + u = U(2); + pred_of_orient_with_gyro = A*prev_orient + B*u; + + Y = wb_inertial_unit_get_roll_pitch_yaw(imu_mag); + y = Y(3); + meas_of_orient_with_mag = C*y; + + prev_orient = pred_of_orient_with_gyro; + + wb_console_print(sprintf('ORIENT: %g %g %g\n', orientation(4), pred_of_orient_with_gyro, meas_of_orient_with_mag), WB_STDOUT); + + o_1(step) = orientation(4); + o_2(step) = pred_of_orient_with_gyro; + o_3(step) = meas_of_orient_with_mag; + + %% IMU % IMU, Gyro roll_pitch_yaw_array = wb_gyro_get_values(imu_gyro); new_imu_gyro_y = roll_pitch_yaw_array(2); @@ -87,9 +121,6 @@ x_y_z_array = wb_accelerometer_get_values(imu_acc); new_imu_acc_z = x_y_z_array(3); - wb_console_print(sprintf('gyro: %g %g %g\n', roll_pitch_yaw_array(1), roll_pitch_yaw_array(2), roll_pitch_yaw_array(3)), WB_STDOUT); - wb_console_print(sprintf('acce: %g %g %g\n', x_y_z_array(1), x_y_z_array(2), x_y_z_array(3)), WB_STDOUT); - vel_z = imu_acc2vel(new_imu_acc_z, prev_imu_vel_z, TIME_STEP); if abs(new_imu_gyro_y) < 0.001 @@ -101,9 +132,6 @@ end U = [vel_z new_imu_gyro_y]'; - - wb_console_print(sprintf('U: %g %g\n', U(1), U(2)), WB_STDOUT); - mu = kinematicModel_vw(pose_imu(:, step-1), U, TIME_STEP); pose_imu(:, step) = mu; @@ -114,12 +142,11 @@ % Getting true position position = wb_supervisor_field_get_sf_vec3f(trans_field); orientation = wb_supervisor_field_get_sf_rotation(orien_field); - true_pose(1, step) = position(1); true_pose(2, step) = position(3); true_pose(3, step) = orientation(4); - %% Plotting position of robot on a map (true, odometry) + % Plotting position of robot on a map (true, odometry) figure(1) plot(true_pose(1, step), -true_pose(2, step), 'ro'); hold on; @@ -134,7 +161,7 @@ text(0.35,0.45,'IMU Estimation','Color','green'); hold off; - %% if your code plots some graphics, it needs to flushed like this: + % if your code plots some graphics, it needs to flushed like this: drawnow; end diff --git a/simulation/controllers/first_controller/measurement_model.m b/simulation/controllers/first_controller/measurement_model.m new file mode 100644 index 0000000..b35862a --- /dev/null +++ b/simulation/controllers/first_controller/measurement_model.m @@ -0,0 +1,7 @@ +function [C,D,Q,m] = measurement_model() + C = 1; + D = 0; + Q = 1; + m = 1; +end + diff --git a/simulation/controllers/first_controller/motion_model.m b/simulation/controllers/first_controller/motion_model.m new file mode 100644 index 0000000..09e8582 --- /dev/null +++ b/simulation/controllers/first_controller/motion_model.m @@ -0,0 +1,9 @@ +function [A,B,R,n] = motion_model(dt) + dt = dt * 0.001; + + A = 1; + B = dt; + R = 1; + n = 1; +end + From 82944101ce6e64e3fb0b87aba27b00347375c7fc Mon Sep 17 00:00:00 2001 From: Dong-Jae Park Date: Sat, 9 Jan 2021 21:11:13 -0600 Subject: [PATCH 57/63] World file update --- simulation/worlds/.desk.wbproj | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/simulation/worlds/.desk.wbproj b/simulation/worlds/.desk.wbproj index 69c209b..41fbb2a 100644 --- a/simulation/worlds/.desk.wbproj +++ b/simulation/worlds/.desk.wbproj @@ -1,5 +1,5 @@ Webots Project File version R2021a -perspectives: 000000ff00000000fd000000030000000000000320000003dafc0100000004fc00000000ffffffff0000000000fffffffc0200000001fb00000012005300630065006e0065005400720065006501000000000000039f0000000000000000fb0000001e00480074006d006c0052006f0062006f007400570069006e0064006f00770000000000000003200000000000000000fb0000001e00480074006d006c0052006f0062006f007400570069006e0064006f00770000000000000003200000000000000000fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000006900ffffff0000000100000329000003bdfc0200000001fb0000001400540065007800740045006400690074006f00720000000015000003bd0000004100ffffff0000000300000780000000a1fc0100000002fb0000000e0043006f006e0073006f006c006501000000000000073f0000000000000000fb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c0100000000000007800000006900ffffff000007800000033700000004000000040000000100000008fc00000000 +perspectives: 000000ff00000000fd000000030000000000000320000003f8fc0100000004fc00000000ffffffff0000000000fffffffc0200000001fb00000012005300630065006e0065005400720065006501000000000000039f0000000000000000fb0000001e00480074006d006c0052006f0062006f007400570069006e0064006f00770000000000000003200000000000000000fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000006900fffffffb0000001e00480074006d006c0052006f0062006f007400570069006e0064006f00770000000000000003200000006900ffffff0000000100000329000003bdfc0200000001fb0000001400540065007800740045006400690074006f00720000000015000003bd0000004100ffffff000000030000078000000039fc0100000002fb0000000e0043006f006e0073006f006c006501000000000000073f0000000000000000fb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c0100000000000007800000006900ffffff000007800000039f00000004000000040000000100000008fc00000000 simulationViewPerspectives: 000000ff000000010000000200000168000006160100000002010000000101 sceneTreePerspectives: 000000ff0000000100000002000000ea000000fa0100000002010000000201 minimizedPerspectives: 000000ff00000000fd000000030000000000000320000003dafc0100000004fc00000000ffffffff0000000000fffffffc0200000001fb00000012005300630065006e0065005400720065006501000000000000039f0000000000000000fb0000001e00480074006d006c0052006f0062006f007400570069006e0064006f00770000000000000003200000000000000000fb0000001e00480074006d006c0052006f0062006f007400570069006e0064006f00770000000000000003200000000000000000fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000006900ffffff00000001000003290000033afc0200000001fb0000001400540065007800740045006400690074006f007200000000150000033a0000004100ffffff000000030000078000000123fc0100000002fb0000000e0043006f006e0073006f006c006501000000000000073f0000000000000000fb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c0100000000000007800000006900ffffff000007800000029d00000004000000040000000100000008fc00000000 @@ -11,4 +11,4 @@ orthographicViewHeight: 0.808765 textFiles: -1 globalOptionalRendering: CoordinateSystem::RangeFinderFrustums consoles: Console:All:All -renderingDevicePerspectives: robot:tof;1;4.4375;0;0 +renderingDevicePerspectives: robot:tof;1;3.82813;0;0 From bb0b036b277661d80fae66d713cf86a3ebf9df7e Mon Sep 17 00:00:00 2001 From: Dong-Jae Park Date: Sun, 10 Jan 2021 14:57:54 -0600 Subject: [PATCH 58/63] Testing pose estimation with kalman filter for orientation Error in calculating pose using the orientation values from kalman filter. Need to check what values the pose estimator takes in for orientation. --- .../first_controller/Mode_Manual.m | 62 +++++++++++++------ .../first_controller/measurement_model.m | 2 +- .../first_controller/motion_model.m | 2 +- 3 files changed, 45 insertions(+), 21 deletions(-) diff --git a/simulation/controllers/first_controller/Mode_Manual.m b/simulation/controllers/first_controller/Mode_Manual.m index 8dfd2b2..1b307c7 100644 --- a/simulation/controllers/first_controller/Mode_Manual.m +++ b/simulation/controllers/first_controller/Mode_Manual.m @@ -22,11 +22,13 @@ prev_imu_vel_z = 0; % kf +% prior +orient_mu = 0; +orient_S = 0.1; +% motion model and measurement model [A,B,R,n] = motion_model(TIME_STEP); -prev_orient = orientation(4); - [C,D,Q,m] = measurement_model(); - +% store in state space model (ssm) ssm.A = A; ssm.B = B; ssm.C = C; @@ -35,6 +37,12 @@ ssm.Q = Q; ssm.n = n; ssm.m = m; +% kf initializations +orient_x(1) = orient_mu+sqrt(orient_S)*randn(1); +U = wb_gyro_get_values(imu_gyro); +gyro_u(1) = U(2); +pose_imu_kalman(:, step) = [position(1) position(3) orient_x(1)]'; + while wb_robot_step(TIME_STEP) ~= -1 step = step + 1; @@ -90,27 +98,32 @@ prev_enc_left = new_enc_left; prev_enc_right = new_enc_right; - %% TOF - image = wb_range_finder_get_range_image(tof); - %% Kalman Filter - orientation = wb_supervisor_field_get_sf_rotation(orien_field); - + % Select motion disturbance + orient_e = normrnd(0, R); + % Input for motion model U = wb_gyro_get_values(imu_gyro); - u = U(2); - pred_of_orient_with_gyro = A*prev_orient + B*u; - + gyro_u(step) = U(2); + % Update state + orient_x(step) = A*orient_x(step-1)+ B*gyro_u(step-1) + orient_e; +% orient_x = wrapTo2Pi(orient_x); + + % Take measurement + % Select a measurement disturbance + orient_d = normrnd(0, Q); + % Determine measurement Y = wb_inertial_unit_get_roll_pitch_yaw(imu_mag); - y = Y(3); - meas_of_orient_with_mag = C*y; - - prev_orient = pred_of_orient_with_gyro; + orient_z = Y(3); + orient_y(step) = C*orient_z + orient_d; +% orient_y = wrapTo2Pi(orient_y); - wb_console_print(sprintf('ORIENT: %g %g %g\n', orientation(4), pred_of_orient_with_gyro, meas_of_orient_with_mag), WB_STDOUT); + % Kalman Filter Estimation + [orient_mu,orient_S,orient_mup,orient_Sp,K] = kalman_filter(ssm,orient_mu,orient_S,gyro_u(step-1),orient_y(step)); - o_1(step) = orientation(4); - o_2(step) = pred_of_orient_with_gyro; - o_3(step) = meas_of_orient_with_mag; + % Store estimates + orient_mup_S(step) = orient_mup; + orient_mu_S(step) = orient_mu; + orient_kalman(step) = K; %% IMU % IMU, Gyro @@ -131,13 +144,22 @@ vel_z = 0; end + U_Kalman = [vel_z orient_mu_S(step)]'; U = [vel_z new_imu_gyro_y]'; + mu = kinematicModel_vw(pose_imu(:, step-1), U, TIME_STEP); + mu_kalman = kinematicModel_vw(pose_imu_kalman(:, step-1), U_Kalman, TIME_STEP); pose_imu(:, step) = mu; + pose_imu_kalman(:, step) = mu_kalman; prev_imu_acc_z = new_imu_acc_z; prev_imu_vel_z = vel_z; + + + %% TOF + image = wb_range_finder_get_range_image(tof); + %% PLOTTING % Getting true position position = wb_supervisor_field_get_sf_vec3f(trans_field); @@ -154,6 +176,8 @@ hold on; plot(pose_imu(1, step), -pose_imu(2, step), 'g.'); hold on; + plot(pose_imu_kalman(1, step), -pose_imu_kalman(2, step), 'c*'); + hold on axis([-0.8 0.8 -0.6 0.6]); rectangle('Position',[-TABLE_WIDTH/2 -TABLE_HEIGHT/2 TABLE_WIDTH TABLE_HEIGHT]) text(0.35,0.55,'True Position','Color','red'); diff --git a/simulation/controllers/first_controller/measurement_model.m b/simulation/controllers/first_controller/measurement_model.m index b35862a..422c32e 100644 --- a/simulation/controllers/first_controller/measurement_model.m +++ b/simulation/controllers/first_controller/measurement_model.m @@ -1,7 +1,7 @@ function [C,D,Q,m] = measurement_model() C = 1; D = 0; - Q = 1; + Q = 0.065; m = 1; end diff --git a/simulation/controllers/first_controller/motion_model.m b/simulation/controllers/first_controller/motion_model.m index 09e8582..dc1256e 100644 --- a/simulation/controllers/first_controller/motion_model.m +++ b/simulation/controllers/first_controller/motion_model.m @@ -3,7 +3,7 @@ A = 1; B = dt; - R = 1; + R = 0.01; n = 1; end From 151e86f492770613c2d602813e98d2146f2122e6 Mon Sep 17 00:00:00 2001 From: Dong-Jae Park Date: Sun, 10 Jan 2021 14:58:33 -0600 Subject: [PATCH 59/63] World update --- simulation/worlds/desk.wbt | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/simulation/worlds/desk.wbt b/simulation/worlds/desk.wbt index 754831f..77e03a4 100644 --- a/simulation/worlds/desk.wbt +++ b/simulation/worlds/desk.wbt @@ -90,8 +90,8 @@ DEF TableUV Robot { } ] endPoint Solid { - translation -0.04000000040198491 -0.009998878983036004 5.847401392695447e-09 - rotation 1.5461368107707866e-08 -1.518240091980168e-08 0.9999999999999998 1.5699999530302196 + translation -0.04000000040198491 -0.009998878983036004 5.8474013926954475e-09 + rotation 1.546136810770788e-08 -1.518240091980168e-08 0.9999999999999998 1.5699999530302196 children [ DEF WHEEL Shape { appearance PBRAppearance { @@ -123,7 +123,7 @@ DEF TableUV Robot { } ] endPoint Solid { - translation 0.04 -0.00999889 4.164319999411153e-09 + translation 0.04 -0.00999889 4.164319999411154e-09 rotation 9.142114502326543e-10 -1.1821627446781977e-09 1 1.5700000469740278 children [ DEF WHEEL Shape { From f63b48cacaf6b0cc5260d34c2fdb5162100ac4d7 Mon Sep 17 00:00:00 2001 From: Dong-Jae Park Date: Sun, 10 Jan 2021 16:40:25 -0600 Subject: [PATCH 60/63] Added a compass plot to visualize the estimated orientation --- .../first_controller/Mode_Manual.m | 55 +++++++++++++------ 1 file changed, 37 insertions(+), 18 deletions(-) diff --git a/simulation/controllers/first_controller/Mode_Manual.m b/simulation/controllers/first_controller/Mode_Manual.m index 1b307c7..33ff46e 100644 --- a/simulation/controllers/first_controller/Mode_Manual.m +++ b/simulation/controllers/first_controller/Mode_Manual.m @@ -106,7 +106,6 @@ gyro_u(step) = U(2); % Update state orient_x(step) = A*orient_x(step-1)+ B*gyro_u(step-1) + orient_e; -% orient_x = wrapTo2Pi(orient_x); % Take measurement % Select a measurement disturbance @@ -115,7 +114,6 @@ Y = wb_inertial_unit_get_roll_pitch_yaw(imu_mag); orient_z = Y(3); orient_y(step) = C*orient_z + orient_d; -% orient_y = wrapTo2Pi(orient_y); % Kalman Filter Estimation [orient_mu,orient_S,orient_mup,orient_Sp,K] = kalman_filter(ssm,orient_mu,orient_S,gyro_u(step-1),orient_y(step)); @@ -124,7 +122,7 @@ orient_mup_S(step) = orient_mup; orient_mu_S(step) = orient_mu; orient_kalman(step) = K; - + %% IMU % IMU, Gyro roll_pitch_yaw_array = wb_gyro_get_values(imu_gyro); @@ -169,21 +167,42 @@ true_pose(3, step) = orientation(4); % Plotting position of robot on a map (true, odometry) - figure(1) - plot(true_pose(1, step), -true_pose(2, step), 'ro'); - hold on; - plot(pose_enc(1, step), -pose_enc(2, step), 'bx'); - hold on; - plot(pose_imu(1, step), -pose_imu(2, step), 'g.'); - hold on; - plot(pose_imu_kalman(1, step), -pose_imu_kalman(2, step), 'c*'); - hold on - axis([-0.8 0.8 -0.6 0.6]); - rectangle('Position',[-TABLE_WIDTH/2 -TABLE_HEIGHT/2 TABLE_WIDTH TABLE_HEIGHT]) - text(0.35,0.55,'True Position','Color','red'); - text(0.35,0.50,'Odometry','Color','blue'); - text(0.35,0.45,'IMU Estimation','Color','green'); - hold off; +% figure(1) +% plot(true_pose(1, step), -true_pose(2, step), 'ro'); +% hold on; +% plot(pose_enc(1, step), -pose_enc(2, step), 'bx'); +% hold on; +% plot(pose_imu(1, step), -pose_imu(2, step), 'g.'); +% hold on; +% plot(pose_imu_kalman(1, step), -pose_imu_kalman(2, step), 'c*'); +% hold on +% axis([-0.8 0.8 -0.6 0.6]); +% rectangle('Position',[-TABLE_WIDTH/2 -TABLE_HEIGHT/2 TABLE_WIDTH TABLE_HEIGHT]) +% text(0.35,0.55,'True Position','Color','red'); +% text(0.35,0.50,'Odometry','Color','blue'); +% text(0.35,0.45,'IMU Estimation','Color','green'); +% hold off; + + % Plotting compass showing current orientation of the robot + figure(2) + rad_enc = pose_enc(3, step); + rad_enc = rad_enc+pi/2; + u_enc = cos(rad_enc); + v_enc = sin(rad_enc); + + rad_imu = pose_imu(3, step); + rad_imu = rad_imu+pi/2; + u_imu = cos(rad_imu); + v_imu = sin(rad_imu); + + rad_kf = pose_imu_kalman(3, step); + rad_kf = rad_kf+pi/2; + u_kf = cos(rad_kf); + v_kf = sin(rad_kf); + + compass(u_enc,v_enc,'r'); hold on; + compass(u_kf,v_kf,'b'); + compass(u_imu,v_imu,'g'); hold off; % if your code plots some graphics, it needs to flushed like this: drawnow; From 9b4d529e09da5ceb154d6be35b42cb70d35adef7 Mon Sep 17 00:00:00 2001 From: Dong-Jae Park Date: Sun, 10 Jan 2021 19:27:56 -0600 Subject: [PATCH 61/63] Kalman filter for orientation estimation There is discontinuity when going from 0 to 2pi. --- .../controllers/first_controller/Mode_Manual.m | 4 +--- .../first_controller/kinematicModel_kalman.m | 13 +++++++++++++ 2 files changed, 14 insertions(+), 3 deletions(-) create mode 100644 simulation/controllers/first_controller/kinematicModel_kalman.m diff --git a/simulation/controllers/first_controller/Mode_Manual.m b/simulation/controllers/first_controller/Mode_Manual.m index 33ff46e..eba51e9 100644 --- a/simulation/controllers/first_controller/Mode_Manual.m +++ b/simulation/controllers/first_controller/Mode_Manual.m @@ -146,15 +146,13 @@ U = [vel_z new_imu_gyro_y]'; mu = kinematicModel_vw(pose_imu(:, step-1), U, TIME_STEP); - mu_kalman = kinematicModel_vw(pose_imu_kalman(:, step-1), U_Kalman, TIME_STEP); + mu_kalman = kinematicModel_kalman(pose_imu_kalman(:, step-1), U_Kalman, TIME_STEP); pose_imu(:, step) = mu; pose_imu_kalman(:, step) = mu_kalman; prev_imu_acc_z = new_imu_acc_z; prev_imu_vel_z = vel_z; - - %% TOF image = wb_range_finder_get_range_image(tof); diff --git a/simulation/controllers/first_controller/kinematicModel_kalman.m b/simulation/controllers/first_controller/kinematicModel_kalman.m new file mode 100644 index 0000000..9f1cbcd --- /dev/null +++ b/simulation/controllers/first_controller/kinematicModel_kalman.m @@ -0,0 +1,13 @@ +function X_curr = kinematicModel_kalman(X_prev ,U ,TIME_STEP) + % kinematicModel_vw + % X_prev = [x z theta]' + % U = [v_z theta]' + + dt = TIME_STEP * 0.001; + + X_curr = [ X_prev(1) + U(1)*sin(X_prev(3))*dt; + X_prev(2) + U(1)*cos(X_prev(3))*dt; + U(2) ]; + +end + From 6650c022d5fb4aaf6d5ac29856eb98dba2a9b97f Mon Sep 17 00:00:00 2001 From: Dong-Jae Park Date: Tue, 12 Jan 2021 11:56:41 -0600 Subject: [PATCH 62/63] Empty initial commit From 10e9c23558333dcfa0d7eb52de57198a7d512252 Mon Sep 17 00:00:00 2001 From: Dong-Jae Park Date: Wed, 13 Jan 2021 08:25:00 -0600 Subject: [PATCH 63/63] initial commit --- simulation/worlds/.desk.wbproj | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/simulation/worlds/.desk.wbproj b/simulation/worlds/.desk.wbproj index 41fbb2a..e419f7e 100644 --- a/simulation/worlds/.desk.wbproj +++ b/simulation/worlds/.desk.wbproj @@ -1,5 +1,5 @@ Webots Project File version R2021a -perspectives: 000000ff00000000fd000000030000000000000320000003f8fc0100000004fc00000000ffffffff0000000000fffffffc0200000001fb00000012005300630065006e0065005400720065006501000000000000039f0000000000000000fb0000001e00480074006d006c0052006f0062006f007400570069006e0064006f00770000000000000003200000000000000000fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000006900fffffffb0000001e00480074006d006c0052006f0062006f007400570069006e0064006f00770000000000000003200000006900ffffff0000000100000329000003bdfc0200000001fb0000001400540065007800740045006400690074006f00720000000015000003bd0000004100ffffff000000030000078000000039fc0100000002fb0000000e0043006f006e0073006f006c006501000000000000073f0000000000000000fb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c0100000000000007800000006900ffffff000007800000039f00000004000000040000000100000008fc00000000 +perspectives: 000000ff00000000fd000000030000000000000320000003f8fc0100000004fc00000000ffffffff0000000000fffffffc0200000001fb00000012005300630065006e0065005400720065006501000000000000039f0000000000000000fb0000001e00480074006d006c0052006f0062006f007400570069006e0064006f00770000000000000003200000000000000000fb0000001e00480074006d006c0052006f0062006f007400570069006e0064006f00770000000000000003200000000000000000fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000006900ffffff0000000100000329000003bdfc0200000001fb0000001400540065007800740045006400690074006f00720000000015000003bd0000004100ffffff000000030000078000000138fc0100000002fb0000000e0043006f006e0073006f006c006501000000000000073f0000000000000000fb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c0100000000000007800000006900ffffff00000780000002be00000004000000040000000100000008fc00000000 simulationViewPerspectives: 000000ff000000010000000200000168000006160100000002010000000101 sceneTreePerspectives: 000000ff0000000100000002000000ea000000fa0100000002010000000201 minimizedPerspectives: 000000ff00000000fd000000030000000000000320000003dafc0100000004fc00000000ffffffff0000000000fffffffc0200000001fb00000012005300630065006e0065005400720065006501000000000000039f0000000000000000fb0000001e00480074006d006c0052006f0062006f007400570069006e0064006f00770000000000000003200000000000000000fb0000001e00480074006d006c0052006f0062006f007400570069006e0064006f00770000000000000003200000000000000000fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000006900ffffff00000001000003290000033afc0200000001fb0000001400540065007800740045006400690074006f007200000000150000033a0000004100ffffff000000030000078000000123fc0100000002fb0000000e0043006f006e0073006f006c006501000000000000073f0000000000000000fb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c0100000000000007800000006900ffffff000007800000029d00000004000000040000000100000008fc00000000 @@ -11,4 +11,4 @@ orthographicViewHeight: 0.808765 textFiles: -1 globalOptionalRendering: CoordinateSystem::RangeFinderFrustums consoles: Console:All:All -renderingDevicePerspectives: robot:tof;1;3.82813;0;0 +renderingDevicePerspectives: robot:tof;1;3.82813;0.00706033;0.0139969