From 9c2470582ac287935f1ee723f0f6571095ef48e8 Mon Sep 17 00:00:00 2001 From: Frederico Afonso Date: Mon, 15 May 2023 14:38:29 +0200 Subject: [PATCH 1/3] 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/3] 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 032674e48ac2374e3d0aa0a47d0ed68267de5cca Mon Sep 17 00:00:00 2001 From: Frederico Afonso Date: Fri, 26 Jan 2024 16:36:19 +0000 Subject: [PATCH 3/3] Added example 'lbr4_velocity_control.m'. --- .../lbr4_velocity_control.m | 77 +++++++++++++++++++ 1 file changed, 77 insertions(+) create mode 100644 vrep/lbr4-velocity-control/lbr4_velocity_control.m diff --git a/vrep/lbr4-velocity-control/lbr4_velocity_control.m b/vrep/lbr4-velocity-control/lbr4_velocity_control.m new file mode 100644 index 0000000..2113a75 --- /dev/null +++ b/vrep/lbr4-velocity-control/lbr4_velocity_control.m @@ -0,0 +1,77 @@ +%% Example of velocity actuation and joint position reading +% Frederico Fernandes Afonso Silva - Jan/2024 +% Last modification: Jan/2024 + +% Example of velocity control and reading using a KUKA LBR4 robot. +% +% Usage: +% 1) Open scene "lbr4_velocity_control.ttt" on V-REP. +% Available at: https://osf.io/gzrby +% 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(); + +% Finish any previous V-REP communication +vi.stop_simulation(); +vi.disconnect_all(); + +% Start a new connection +vi.connect('127.0.0.1',19997); +disp('Communication established!') + +% Start simulation +vi.set_synchronous(true); +vi.start_simulation(); +disp('Simulation started!') + +% Define robot interface +robot_vrep = LBR4pVrepRobot('LBR4p', vi); + +%% Definition of the actuation velocity +qd = [-pi/4; -5*pi; 0; pi/2; pi/2; pi/2; pi]; + +%% General variables +iteration = 0; +max_iteration = 10; + +%% Main control loop for the first desired pose +while(iteration <= max_iteration) + % Robot actuation in V-REP + robot_vrep.set_target_configuration_space_velocities(qd); + vi.trigger_next_simulation_step(); + vi.wait_for_simulation_step_to_end(); + + formatSpec = 'Joint velocity sent to V-REP: %f\n'; + fprintf(formatSpec,qd); + disp(' ') + + % Read joint positions from V-REP + q_read = robot_vrep.get_configuration_space_positions; + + formatSpec = 'Joint position read from V-REP: %f\n'; + fprintf(formatSpec,q_read); + 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