From 9c2470582ac287935f1ee723f0f6571095ef48e8 Mon Sep 17 00:00:00 2001 From: Frederico Afonso Date: Mon, 15 May 2023 14:38:29 +0200 Subject: [PATCH 1/9] Added the example panda_kinematic_control, which uses the DQ_PseudoinverseController() to perform a pose control of a FrankaEmikaPandaRobot(). --- manipulator/panda_kinematic_control.m | 60 +++++++++++++++++++++++++++ 1 file changed, 60 insertions(+) create mode 100644 manipulator/panda_kinematic_control.m diff --git a/manipulator/panda_kinematic_control.m b/manipulator/panda_kinematic_control.m new file mode 100644 index 0000000..52120db --- /dev/null +++ b/manipulator/panda_kinematic_control.m @@ -0,0 +1,60 @@ +function panda_kinematic_control() + + % use the namespace + include_namespace_dq + + % Create a new DQ_kinematics object with the Franka Emika Panda arm + % modified Denavit-Hartenberg parameters + panda = FrankaEmikaPandaRobot.kinematics(); + + % The controller is given by + % u = -pinv(J)*gain*task_error + % where J is the robot Jacobian, gain determines the convergence rate, + % and task_error is the error between the current task variable and + % the desired one. When the task error derivative is below + % stability_threshold, the closed-loop system is said to have reached a + % stable region. + controller = DQ_PseudoinverseController(panda); + controller.set_gain(100); + controller.set_stability_threshold(0.0001); + + % Initial configuration + q = [0; 0; 0; -pi/2; 0; pi/2; 0]; + + % Integration step + T = 0.001; + + % The task variable to be controlled is the end-effector pose + controller.set_control_objective(ControlObjective.Pose); + + % Generate a desired end-effector pose + xd = 1 + 0.5*E_*(i_*0.5 + j_*0.3 + k_*0.4); + task_reference = vec8(xd); + + % Prepare the visualization + figure; + view(70,25); + axis equal; + axis([-0.1, 0.6,-0.4, 0.6, -0.1, 0.8]) + hold on; + plot(panda, q, 'nojoints'); + plot(xd, 'scale', 0.1); + + % This is actually the important part on how to use the controller + while ~controller.system_reached_stable_region() + % Let us calculate the control input + u = controller.compute_setpoint_control_signal(q, task_reference); + + % Do a numerical integration to update the robot in Matlab. In an + % actual robot actuated by means of velocity inputs, this step is + % not necessary + q = q + T*u; + + % Draw the robot in Matlab + plot(panda, q', 'nojoints'); + + pvec = vec3(panda.fkm(q).translation); + plot3(pvec(1),pvec(2),pvec(3), 'ro'); + drawnow; + end +end From 23c1db6557a18a80c151aa5621b536aebcd71faf Mon Sep 17 00:00:00 2001 From: Frederico Afonso Date: Sun, 28 May 2023 10:03:42 +0200 Subject: [PATCH 2/9] Added the missing license header to the file 'panda_kinematic_control'. --- manipulator/panda_kinematic_control.m | 26 +++++++++++++++++++++++++- 1 file changed, 25 insertions(+), 1 deletion(-) diff --git a/manipulator/panda_kinematic_control.m b/manipulator/panda_kinematic_control.m index 52120db..ddb7ee2 100644 --- a/manipulator/panda_kinematic_control.m +++ b/manipulator/panda_kinematic_control.m @@ -1,5 +1,29 @@ -function panda_kinematic_control() +% panda_kinematic_control() performs a pseudoinverse pose control using a +% Franka Emika Panda robot. + +% (C) Copyright 2011-2023 DQ Robotics Developers +% +% This file is part of DQ Robotics. +% +% DQ Robotics is free software: you can redistribute it and/or modify +% it under the terms of the GNU Lesser General Public License as published by +% the Free Software Foundation, either version 3 of the License, or +% (at your option) any later version. +% +% DQ Robotics is distributed in the hope that it will be useful, +% but WITHOUT ANY WARRANTY; without even the implied warranty of +% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +% GNU Lesser General Public License for more details. +% +% You should have received a copy of the GNU Lesser General Public License +% along with DQ Robotics. If not, see . +% +% DQ Robotics website: dqrobotics.github.io +% +% Contributors to this file: +% Frederico Fernandes Afonso Silva - frederico.silva@ieee.org +function panda_kinematic_control() % use the namespace include_namespace_dq From f7d41c30e4f3553e7e76f536e73b7f26840c9cc6 Mon Sep 17 00:00:00 2001 From: Frederico Afonso Date: Mon, 10 Jul 2023 14:37:58 +0200 Subject: [PATCH 3/9] Added example 'vrep/panda-torque-control'. --- .../panda_torque_control.m | 84 +++++++++++++++++++ 1 file changed, 84 insertions(+) create mode 100644 vrep/panda-torque-control/panda_torque_control.m diff --git a/vrep/panda-torque-control/panda_torque_control.m b/vrep/panda-torque-control/panda_torque_control.m new file mode 100644 index 0000000..31671da --- /dev/null +++ b/vrep/panda-torque-control/panda_torque_control.m @@ -0,0 +1,84 @@ +%% Example of torque control and force sensor reading +% Frederico Fernandes Afonso Silva - Jul/2023 +% Last modification: Jul/2023 + +% Example of torque control and force sensor reading using a Franka Emika +% Panda robot. +% +% Usage: +% 1) Open scene "panda_torque_control.ttt" on V-REP. +% Available at: https://osf.io/gmhba/?view_only=e0122282979d43af93aa280cfd390df3 +% 2) Run this file. + +close all +clear class +clear all %#ok +clc + +% use the namespace +include_namespace_dq + +%% V-REP setup +% Create a DQ_VrepInterface object and start communication with V-REP +vi = DQ_VrepInterface(); +vi.connect('127.0.0.1',19997); +disp('Communication established!') + +% Start simulation +vi.set_synchronous(true); +vi.start_simulation(); +disp('Simulation started!') +vi.trigger_next_simulation_step(); % force sensor returns noise values if called before the first trigger +vi.wait_for_simulation_step_to_end(); + +% Define robot interface +robot_vrep = FrankaEmikaPandaVrepRobot('Franka', vi); + +force_sensor_name = '/Franka/connection'; + +%% Definition of the actuation torque +tau = [-0.005; -70; 0; 5; -0.007; 0; 0]; + +%% General variables +iteration = 1; +max_iteration = 10; + +%% Main control loop for the first desired pose +while(iteration <= max_iteration) + % Robot actuation in V-REP + robot_vrep.send_tau_to_vrep(tau); + vi.trigger_next_simulation_step(); + vi.wait_for_simulation_step_to_end(); + + formatSpec = 'Actuation torque sent to V-REP: %f\n'; + fprintf(formatSpec,tau); + disp(' ') + + % Read joint torques from V-REP + tau_read = robot_vrep.get_tau_from_vrep; + + formatSpec = 'Joint torque read from V-REP: %f\n'; + fprintf(formatSpec,tau_read); + disp(' ') + + % Get force sensor readings + [force_vec, torque_vec] = vi.get_force_sensor_readings(force_sensor_name); + wrench_sensor = DQ([force_vec torque_vec]); + + formatSpec = 'Wrench read from V-REP (at sensor`s reference frame): %f\n'; + fprintf(formatSpec,vec6(wrench_sensor)); + disp(' ') + + %% General messages + iteration = iteration + 1; + + formatSpec = 'Iteration: %f\n'; + fprintf(formatSpec,iteration); + disp(' ') +end + +%% Finishes V-REP communication +vi.stop_simulation(); +disp('Simulation finished!') +vi.disconnect_all(); +disp('Communication finished!') \ No newline at end of file From 9af438154d4f02a1368faa667f7e3cd3377d9e09 Mon Sep 17 00:00:00 2001 From: Frederico Afonso Date: Tue, 25 Jul 2023 18:03:13 +0200 Subject: [PATCH 4/9] Replaced Panda torque control example by KUKA LBR4 control example. --- .../lbr4_torque_control.m} | 24 ++++++------------- 1 file changed, 7 insertions(+), 17 deletions(-) rename vrep/{panda-torque-control/panda_torque_control.m => lbr4-torque-control/lbr4_torque_control.m} (71%) diff --git a/vrep/panda-torque-control/panda_torque_control.m b/vrep/lbr4-torque-control/lbr4_torque_control.m similarity index 71% rename from vrep/panda-torque-control/panda_torque_control.m rename to vrep/lbr4-torque-control/lbr4_torque_control.m index 31671da..244cdc3 100644 --- a/vrep/panda-torque-control/panda_torque_control.m +++ b/vrep/lbr4-torque-control/lbr4_torque_control.m @@ -1,12 +1,12 @@ -%% Example of torque control and force sensor reading +%% Example of torque actuation and reading % Frederico Fernandes Afonso Silva - Jul/2023 % Last modification: Jul/2023 -% Example of torque control and force sensor reading using a Franka Emika -% Panda robot. +% Example of torque control and force sensor reading using a KUKA LBR4 +% robot. % % Usage: -% 1) Open scene "panda_torque_control.ttt" on V-REP. +% 1) Open scene "lbr4_torque_control.ttt" on V-REP. % Available at: https://osf.io/gmhba/?view_only=e0122282979d43af93aa280cfd390df3 % 2) Run this file. @@ -32,15 +32,13 @@ vi.wait_for_simulation_step_to_end(); % Define robot interface -robot_vrep = FrankaEmikaPandaVrepRobot('Franka', vi); - -force_sensor_name = '/Franka/connection'; +robot_vrep = LBR4pVrepRobot('LBR4p', vi); %% Definition of the actuation torque -tau = [-0.005; -70; 0; 5; -0.007; 0; 0]; +tau = [-0.005; -25; 0; -30; -0.007; 0; 0]; %% General variables -iteration = 1; +iteration = 0; max_iteration = 10; %% Main control loop for the first desired pose @@ -61,14 +59,6 @@ fprintf(formatSpec,tau_read); disp(' ') - % Get force sensor readings - [force_vec, torque_vec] = vi.get_force_sensor_readings(force_sensor_name); - wrench_sensor = DQ([force_vec torque_vec]); - - formatSpec = 'Wrench read from V-REP (at sensor`s reference frame): %f\n'; - fprintf(formatSpec,vec6(wrench_sensor)); - disp(' ') - %% General messages iteration = iteration + 1; From a50de7e0149a0adb67759f463e04b9aa366a5b15 Mon Sep 17 00:00:00 2001 From: Frederico Fernandes Afonso Silva Date: Thu, 15 Aug 2024 13:53:12 +0100 Subject: [PATCH 5/9] [lbr4_torque_control.m] Removed the use of 'LBR4VrepRobot' class and added direct use of methods 'get_joint_torques()' and 'set_joint_torques()' from the class DQ_VrepInterface. --- vrep/lbr4-torque-control/lbr4_torque_control.m | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/vrep/lbr4-torque-control/lbr4_torque_control.m b/vrep/lbr4-torque-control/lbr4_torque_control.m index 244cdc3..49009bc 100644 --- a/vrep/lbr4-torque-control/lbr4_torque_control.m +++ b/vrep/lbr4-torque-control/lbr4_torque_control.m @@ -32,7 +32,11 @@ vi.wait_for_simulation_step_to_end(); % Define robot interface -robot_vrep = LBR4pVrepRobot('LBR4p', vi); +robot_name = 'LBR4p'; +for i=1:7 + current_joint_name = {robot_name,'_joint',int2str(i)}; + joint_names{i} = strjoin(current_joint_name,''); %#ok +end %% Definition of the actuation torque tau = [-0.005; -25; 0; -30; -0.007; 0; 0]; @@ -44,7 +48,7 @@ %% Main control loop for the first desired pose while(iteration <= max_iteration) % Robot actuation in V-REP - robot_vrep.send_tau_to_vrep(tau); + vi.set_joint_torques(joint_names, tau); vi.trigger_next_simulation_step(); vi.wait_for_simulation_step_to_end(); @@ -53,7 +57,7 @@ disp(' ') % Read joint torques from V-REP - tau_read = robot_vrep.get_tau_from_vrep; + tau_read = vi.get_joint_torques(joint_names); formatSpec = 'Joint torque read from V-REP: %f\n'; fprintf(formatSpec,tau_read); From 15691b173ee6230f333074d302c17808538d327e Mon Sep 17 00:00:00 2001 From: Frederico Fernandes Afonso Silva Date: Thu, 15 Aug 2024 14:57:14 +0100 Subject: [PATCH 6/9] [lbr4_torque_control.m] Updated comments and minor code clean-up. --- vrep/lbr4-torque-control/lbr4_torque_control.m | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/vrep/lbr4-torque-control/lbr4_torque_control.m b/vrep/lbr4-torque-control/lbr4_torque_control.m index 49009bc..fe5ed4f 100644 --- a/vrep/lbr4-torque-control/lbr4_torque_control.m +++ b/vrep/lbr4-torque-control/lbr4_torque_control.m @@ -1,9 +1,8 @@ %% Example of torque actuation and reading % Frederico Fernandes Afonso Silva - Jul/2023 -% Last modification: Jul/2023 +% Last modification: Aug/2024 -% Example of torque control and force sensor reading using a KUKA LBR4 -% robot. +% Example of torque actuation and reading using a KUKA LBR4 robot. % % Usage: % 1) Open scene "lbr4_torque_control.ttt" on V-REP. @@ -28,8 +27,6 @@ vi.set_synchronous(true); vi.start_simulation(); disp('Simulation started!') -vi.trigger_next_simulation_step(); % force sensor returns noise values if called before the first trigger -vi.wait_for_simulation_step_to_end(); % Define robot interface robot_name = 'LBR4p'; From 6efb2ddf995be92226260838540a2b3b5303b43e Mon Sep 17 00:00:00 2001 From: Frederico Fernandes Afonso Silva Date: Thu, 15 Aug 2024 17:07:52 +0100 Subject: [PATCH 7/9] [lbr4_torque_control.m] Updated comments. --- vrep/lbr4-torque-control/lbr4_torque_control.m | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/vrep/lbr4-torque-control/lbr4_torque_control.m b/vrep/lbr4-torque-control/lbr4_torque_control.m index fe5ed4f..b38fc4e 100644 --- a/vrep/lbr4-torque-control/lbr4_torque_control.m +++ b/vrep/lbr4-torque-control/lbr4_torque_control.m @@ -42,7 +42,7 @@ iteration = 0; max_iteration = 10; -%% Main control loop for the first desired pose +%% Main loop while(iteration <= max_iteration) % Robot actuation in V-REP vi.set_joint_torques(joint_names, tau); From 3ead8343be8005a05d281b4012718dd3d1012916 Mon Sep 17 00:00:00 2001 From: Frederico Fernandes Afonso Silva Date: Thu, 15 Aug 2024 17:31:35 +0100 Subject: [PATCH 8/9] [lbr4_torque_control.m] Added copyright. --- .../lbr4-torque-control/lbr4_torque_control.m | 23 +++++++++++++++++++ 1 file changed, 23 insertions(+) diff --git a/vrep/lbr4-torque-control/lbr4_torque_control.m b/vrep/lbr4-torque-control/lbr4_torque_control.m index b38fc4e..cd5a2f3 100644 --- a/vrep/lbr4-torque-control/lbr4_torque_control.m +++ b/vrep/lbr4-torque-control/lbr4_torque_control.m @@ -9,6 +9,29 @@ % Available at: https://osf.io/gmhba/?view_only=e0122282979d43af93aa280cfd390df3 % 2) Run this file. +% (C) Copyright 2011-2024 DQ Robotics Developers +% +% This file is part of DQ Robotics. +% +% DQ Robotics is free software: you can redistribute it and/or modify +% it under the terms of the GNU Lesser General Public License as published by +% the Free Software Foundation, either version 3 of the License, or +% (at your option) any later version. +% +% DQ Robotics is distributed in the hope that it will be useful, +% but WITHOUT ANY WARRANTY; without even the implied warranty of +% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +% GNU Lesser General Public License for more details. +% +% You should have received a copy of the GNU Lesser General Public License +% along with DQ Robotics. If not, see . +% +% DQ Robotics website: dqrobotics.github.io +% +% Contributors to this file: +% 1. Frederico Fernandes Afonso Silva (frederico.silva@ieee.org) +% - Responsible for the original implementation. + close all clear class clear all %#ok From d1dda2862bd0562527a4d118b5e529de5b8c4c6b Mon Sep 17 00:00:00 2001 From: Frederico Fernandes Afonso Silva Date: Thu, 15 Aug 2024 17:37:20 +0100 Subject: [PATCH 9/9] [lbr4_torque_control.m] Updated comments. --- vrep/lbr4-torque-control/lbr4_torque_control.m | 4 ---- 1 file changed, 4 deletions(-) diff --git a/vrep/lbr4-torque-control/lbr4_torque_control.m b/vrep/lbr4-torque-control/lbr4_torque_control.m index cd5a2f3..f875249 100644 --- a/vrep/lbr4-torque-control/lbr4_torque_control.m +++ b/vrep/lbr4-torque-control/lbr4_torque_control.m @@ -1,7 +1,3 @@ -%% Example of torque actuation and reading -% Frederico Fernandes Afonso Silva - Jul/2023 -% Last modification: Aug/2024 - % Example of torque actuation and reading using a KUKA LBR4 robot. % % Usage: