diff --git a/coppeliasim/simulation-ram-paper-updated/coppeliasim_scene_dqcontroller.m b/coppeliasim/simulation-ram-paper-updated/coppeliasim_scene_dqcontroller.m new file mode 100644 index 0000000..f62c790 --- /dev/null +++ b/coppeliasim/simulation-ram-paper-updated/coppeliasim_scene_dqcontroller.m @@ -0,0 +1,308 @@ +% Example used in the paper Adorno and Marinho. 2020. “DQ Robotics: A +% Library for Robot Modeling and Control.” IEEE Robotics & Automation +% Magazine, https://doi.org/10.1109/MRA.2020.2997920. +% +% Usage: Assuming that V-REP is already working with Matlab, +% 1) open the file coppeliasim_scene_felt_pen_official_scene.ttt on +% CoppeliaSim; +% 2) run this file; +% 3) adjust the parameters below if you want to change the +% trajectory parameters, simulation time, etc. + +% (C) Copyright 2011-2025 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: +% Bruno Vihena Adorno - adorno@ieee.org +% - Responsible for the original implementation +% 2. Frederico Fernandes Afonso Silva (frederico.silva@ieee.org) +% - Updated for compatibility with the DQ_CoppeliaSimInterfaceZMQ +% class + +function coppeliasim_scene_dqcontroller(simulation_parameters) + +include_namespace_dq; + +%% Simulation convenience flags +SHOW_FRAMES = simulation_parameters.show_frames; + +%% Initialize CoppeliaSim interface +cs = DQ_CoppeliaSimInterfaceZMQ(); +cs.connect; +cs.start_simulation; + +%% Initialize CoppeliaSim Robots +lwr4_coppeliasim_robot = LBR4pCoppeliaSimRobotZMQ("/LBR4p", cs); +youbot_coppeliasim_robot = YouBotCoppeliaSimRobotZMQ("/youBot", cs); + +%% Get robot joint information from CoppeliaSim +youbot_reference_frames = {'YoubotArm0', 'YoubotArm1', 'YoubotArm2', 'YoubotArm3', 'YoubotArm4'}; + +%% Load DQ Robotics kinematics +lwr4 = lwr4_coppeliasim_robot.kinematics(); +youbot = youbot_coppeliasim_robot.kinematics(); + +%% Initialize controllers +lwr4_controller = DQ_PseudoinverseController(lwr4); +lwr4_controller.set_control_objective(ControlObjective.Pose); +lwr4_controller.set_gain(1); + +solver = DQ_QuadprogSolver; +youbot_controller = DQ_ClassicQPController(youbot,solver); +youbot_controller.set_control_objective(ControlObjective.Pose); +youbot_controller.set_gain(20); +youbot_controller.set_damping(0.01); + +sampling_time = 0.05; % CoppeliaSim's sampling time is 50 ms. +total_time = simulation_parameters.total_time; % Total time, in seconds. + +%% Set the initial robots configurations +lwr4_q = [0; 1.7453e-01; 0; 1.5708e+00; 0; 2.6273e-01; 0]; +lwr4_coppeliasim_robot.set_configuration(lwr4_q); +% Get current end-effector pose with respect to its own base +lwr4_x0 = lwr4.get_reference_frame()'*lwr4.fkm(lwr4_q); +youbot_q = youbot_coppeliasim_robot.get_configuration(); + +%% Initalize arrays to store all signals resulting from the simulation +max_iterations = round(total_time/sampling_time); +youbot_tracking_error_norm = zeros(1,max_iterations); +youbot_control_inputs = zeros(youbot.get_dim_configuration_space(),... + max_iterations); +youbot_q_vector = zeros(youbot.get_dim_configuration_space(), ... + max_iterations); + +lwr4_tracking_error_norm = zeros(1,max_iterations); +lwr4_control_inputs = zeros(lwr4.get_dim_configuration_space(),... + max_iterations); +lwr4_q_vector = zeros(lwr4.get_dim_configuration_space(), ... + max_iterations); + +% index of the auxiliary arrays used to plot data +ii = 1; + +%% Control loop +for t=0:sampling_time:total_time + + %% Get obstacles from CoppeliaSim + plane = get_plane_from_coppeliasim(cs, 'ObstaclePlane', DQ.k); + cylinder1 = get_line_from_coppeliasim(cs, 'ObstacleCylinder1', DQ.k); + cylinder2 = get_line_from_coppeliasim(cs, 'ObstacleCylinder2', DQ.k); + + %% Set reference for the manipulator and the mobile manipulator + [lwr4_xd, lwr4_ff] = compute_lwr4_reference(lwr4, simulation_parameters, ... + lwr4_x0,t); + + [youbot_xd, youbot_ff] = compute_youbot_reference(youbot_controller, ... + lwr4_xd, lwr4_ff); + + + %% Compute control signal for the arm + lwr4_u = lwr4_controller.compute_tracking_control_signal(lwr4_q,... + vec8(lwr4_xd),vec8(lwr4_ff)); + + + %% Compute control signal for the youbot + % first calculate the constraints + [Jconstraint, bconstraint] = compute_constraints(youbot, youbot_q, ... + plane,cylinder1,cylinder2); + youbot_controller.set_inequality_constraint(-Jconstraint,1*bconstraint); + + % compute control signal + youbot_u = youbot_controller.compute_tracking_control_signal(youbot_q,... + vec8(youbot_xd), vec8(youbot_ff)); + + % since we are using CoppeliaSim just for visualization, integrate the + % control signal to update the robots configurations + lwr4_q = lwr4_q + sampling_time*lwr4_u; + youbot_q = youbot_q + sampling_time*youbot_u; + + %% Send desired values + lwr4_coppeliasim_robot.set_configuration(lwr4_q); + youbot_coppeliasim_robot.set_configuration(youbot_q); + + %% Show frames, for testing. This if (and the following else) + % can be removed for release + if SHOW_FRAMES + % Plot current LBR4p end-effector pose on CoppeliaSim + cs.set_object_pose('x1', lwr4.fkm(lwr4_q)); + % Plot desired LBR4p end-effector pose on CoppeliaSim + cs.set_object_pose('xd1', lwr4_xd); + % Plot current youbot end-effector pose on CoppeliaSim + cs.set_object_pose('x2', youbot.fkm(youbot_q)); + % plot desired youbot end-effector pose on CoppeliaSim + cs.set_object_pose('xd2', youbot_xd); + % Show youbot's base frame in CoppeliaSim + cs.set_object_pose('YoubotKinematicBase', ... + youbot.fkm(youbot_q,1)) + % Show youbot's arm frames on CoppeliaSim + for k=1:5 + cs.set_object_pose(youbot_reference_frames{k}, ... + youbot.fkm(youbot_q,2,k)) + end + else + cs.set_object_pose('x1',DQ(1)); + cs.set_object_pose('xd1',DQ(1)); + cs.set_object_pose('x2',DQ(1)); + cs.set_object_pose('xd2',DQ(1)); + cs.set_object_pose('YoubotKinematicBase',DQ(1)); + for k=1:5 + cs.set_object_pose(youbot_reference_frames{k},DQ(1)); + end + end + + %% Get data to plot them later + youbot_tracking_error_norm(:,ii) = norm(youbot_controller.get_last_error_signal()); + youbot_control_inputs(:,ii) = youbot_u; + youbot_q_vector(:,ii) = youbot_q; + lwr4_tracking_error_norm(:,ii) = norm(lwr4_controller.get_last_error_signal()); + lwr4_control_inputs(:,ii) = lwr4_u; + lwr4_q_vector(:,ii) = lwr4_q; + ii = ii + 1; +end + + +%% End CoppeliaSim simulation +cs.stop_simulation; + +end + +function line = get_line_from_coppeliasim(coppeliasim_interface, ... + object_name, primitive) + line_object_pose = coppeliasim_interface.get_object_pose(object_name); + p = translation(line_object_pose); + r = rotation(line_object_pose); + l = Ad(r,primitive); + m = cross(p,l); + line = l + DQ.E*m; +end + +function plane = get_plane_from_coppeliasim(coppeliasim_interface, ... + object_name, primitive) + plane_object_pose = coppeliasim_interface.get_object_pose(object_name); + p = translation(plane_object_pose); + r = rotation(plane_object_pose); + n = Ad(r,primitive); + d = dot(p,n); + plane = n + DQ.E*d; +end + +% compute the time-varying reference for the Kuka LWR 4 end-effector +function [x, xdot] = compute_lwr4_reference(lwr4, simulation_parameters, ... + x0, t) + include_namespace_dq; + + % parameters for the trajectory + dispz = simulation_parameters.dispz; + wd = simulation_parameters.wd; + wn = simulation_parameters.wn; + + % calculate the time-varing plane orientation with respect to the robot + % BASE. + phi = (pi/2)*sin(wn*t); + r = cos(phi/2) + k_ * sin(phi/2); + + z = dispz*cos(wd*t)*k_; + p = 1 + E_ * 0.5 * z; + + x = r*x0*p; + + % calculate its time derivative + phidot = (pi/2)*cos(wn*t)*wn; + rdot = 0.5*(-sin(phi/2) + k_*cos(phi/2))*phidot; + pdot = -E_*0.5*dispz*wd*sin(wd*t)*k_; + + xdot = rdot*x0*p + r*x0*pdot; + + % The trajectory,including the feedforward term, has been calculated + % with respect to the manipulator base. Therefore, we need to calculate + % them with respect to the global reference frame. + x = lwr4.get_reference_frame()*x; + xdot = lwr4.get_reference_frame()*xdot; +end + +function [youbot_xd, youbot_ff] = compute_youbot_reference(controller, ... + lbr4p_xd, lbr4p_ff) + persistent tc; + persistent rcircle; + persistent first_iteration; + + include_namespace_dq; + + circle_radius = 0.1; + tcircle = 1 + E_ * 0.5 * circle_radius * j_; + + %% Youbot trajectory + % Those are the trajectory components to track the whiteboard + youbot_xd = lbr4p_xd * (1 + 0.5*E_*0.015*k_) * j_; + youbot_ff = lbr4p_ff * (1 + 0.5*E_*0.015*k_) * j_; + % Now we modify the trajectory in order to draw a circle, but we do it + % only if the whiteboard pen tip is on the whiteboard surface. + if isempty(first_iteration) + first_iteration = false; + tc = 0; + rcircle = DQ(1); + elseif norm(controller.get_last_error_signal()) < 0.002 + tc = tc + 0.1; % Increment around 0.5 deg. + rcircle = cos(tc/2) + k_ * sin(tc/2); + end + youbot_xd = youbot_xd * rcircle * tcircle; + youbot_ff = youbot_ff * rcircle * tcircle; +end + + +function [Jconstraint, bconstraint] = compute_constraints(youbot, youbot_q, ... + plane, cylinder1, cylinder2) + % We define differential inequalities given by d_dot >= -eta*d, + % where d is the distance from the end-effector to the + % primitive, d_dot is its time derivative and eta determines the + % maximum rate for the approach velocity. + + % This information should be inside the robot itself + robot_radius = 0.35; + radius_cylinder1 = 0.1; + radius_cylinder2 = 0.1; + + include_namespace_dq + + % Get base translation and translation Jacobian + youbot_base = youbot.get_chain(1); + youbot_base_pose = youbot_base.raw_fkm(youbot_q); + Jx = youbot_base.raw_pose_jacobian(youbot_q); + t = translation(youbot_base_pose); + Jt = [youbot.translation_jacobian(Jx,youbot_base_pose),zeros(4,5)]; + + % First we calculate the primitives for the plane + Jdist_plane = youbot.point_to_plane_distance_jacobian(Jt, t, plane); + dist_plane = DQ_Geometry.point_to_plane_distance(t, plane) - ... + robot_radius; + + % Now we compute the primitives for the two cylinders + Jdist_cylinder1 = youbot.point_to_line_distance_jacobian(Jt, t, cylinder1); + dist_cylinder1 = DQ_Geometry.point_to_line_squared_distance(t,cylinder1) - ... + (radius_cylinder1 + robot_radius)^2; + + Jdist_cylinder2 = youbot.point_to_line_distance_jacobian(Jt, t, cylinder2); + dist_cylinder2 = DQ_Geometry.point_to_line_squared_distance(t,cylinder2) - ... + (radius_cylinder2 + robot_radius)^2; + + % Assemble all constraint matrices into a unique matrix + Jconstraint = [Jdist_plane; Jdist_cylinder1; Jdist_cylinder2]; + % And do analagously to the constraint vectors + bconstraint = [dist_plane; dist_cylinder1; dist_cylinder2]; +end \ No newline at end of file diff --git a/coppeliasim/simulation-ram-paper-updated/coppeliasim_scene_felt_pen_official_scene.ttt b/coppeliasim/simulation-ram-paper-updated/coppeliasim_scene_felt_pen_official_scene.ttt new file mode 100644 index 0000000..c706f13 Binary files /dev/null and b/coppeliasim/simulation-ram-paper-updated/coppeliasim_scene_felt_pen_official_scene.ttt differ diff --git a/coppeliasim/simulation-ram-paper-updated/main_simulation_file.m b/coppeliasim/simulation-ram-paper-updated/main_simulation_file.m new file mode 100644 index 0000000..9710fd0 --- /dev/null +++ b/coppeliasim/simulation-ram-paper-updated/main_simulation_file.m @@ -0,0 +1,57 @@ +% Example used in the paper Adorno and Marinho. 2020. “DQ Robotics: A +% Library for Robot Modeling and Control.” IEEE Robotics & Automation +% Magazine, https://doi.org/10.1109/MRA.2020.2997920. +% +% Usage: Assuming that V-REP is already working with Matlab, +% 1) open the file vrep_scene_felt_pen_official_scene.ttt on +% CoppeliaSim; +% 2) run this file; +% 3) adjust the parameters below if you want to change the +% trajectory parameters, simulation time, etc. + +% (C) Copyright 2011-2025 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: +% Bruno Vihena Adorno - adorno@ieee.org +% - Responsible for the original implementation +% 2. Frederico Fernandes Afonso Silva (frederico.silva@ieee.org) +% - Updated for compatibility with the DQ_CoppeliaSimInterfaceZMQ +% class + +clear all; %#ok +close all; +clc; + + +% False if we don' want to show intermediate frames +simulation_parameters.show_frames = false; +% Total simulation time, in seconds. +simulation_parameters.total_time = 200; + +% The maximum radius that the manipulator will perform +simulation_parameters.dispz = 0.1; +% Occilation along the radius +simulation_parameters.wd = 0.5; +% Occilation around the rotation axix +simulation_parameters.wn = 0.1; + +coppeliasim_scene_dqcontroller(simulation_parameters); + + diff --git a/coppeliasim/simulation-ram-paper-updated/models/frame license.txt b/coppeliasim/simulation-ram-paper-updated/models/frame license.txt new file mode 100644 index 0000000..2814bdc --- /dev/null +++ b/coppeliasim/simulation-ram-paper-updated/models/frame license.txt @@ -0,0 +1,2 @@ +Frame is licensed by CC BY +http://www.sweethome3d.com/searchModels.jsp?model=picture \ No newline at end of file diff --git a/coppeliasim/simulation-ram-paper-updated/models/frame.mtl b/coppeliasim/simulation-ram-paper-updated/models/frame.mtl new file mode 100644 index 0000000..b090e05 --- /dev/null +++ b/coppeliasim/simulation-ram-paper-updated/models/frame.mtl @@ -0,0 +1,33 @@ +# frame.mtl +# +# Sweet Home 3D, Copyright (c) 2018 Emmanuel PUYBARET / eTeks +# +# This file is distributed at your choice, under the CC-BY Creative Commons Attribution 3.0 Unported license +# available at http://creativecommons.org/licenses/by/3.0/ or under the terms of the GNU General Public License +# as published by the Free Software Foundation and available at http://www.gnu.org/copyleft/gpl.txt +# + +newmtl Back +Ka 0.5 0.5 0.5 +Kd 0.1837 0.1837 0.1837 +Ks 0.5000 0.5000 0.5000 +illum 1 + +newmtl Frame +Ka 0 0 0 +Kd 0.002 0.002 0.002 +Ks 0.5184 0.5184 0.5184 +illum 2 +Ns 120 + +newmtl Mirror +Ka 0.9551 0.9551 0.9551 +Kd 0.6163 0.6163 0.6163 +Ks 0.3 0.3 0.3 +illum 2 +Ns 250 + +newmtl Picture +Ka 0.3 0.3 0.3 +Kd 0.9 0.9 0.9 +illum 1 diff --git a/coppeliasim/simulation-ram-paper-updated/models/frame.obj b/coppeliasim/simulation-ram-paper-updated/models/frame.obj new file mode 100644 index 0000000..04e40e2 --- /dev/null +++ b/coppeliasim/simulation-ram-paper-updated/models/frame.obj @@ -0,0 +1,115 @@ +# frame.obj Produced by Blender +# +# Sweet Home 3D, Copyright (c) 2018 Emmanuel PUYBARET / eTeks +# +# This file is distributed at your choice, under the CC-BY Creative Commons Attribution 3.0 Unported license +# available at http://creativecommons.org/licenses/by/3.0/ or under the terms of the GNU General Public License +# as published by the Free Software Foundation and available at http://www.gnu.org/copyleft/gpl.txt +# +mtllib frame.mtl +g sweethome3d_window_mirror +v -0.224998 -0.149998 -0.0035 +v -0.224998 0.149998 -0.0035 +v 0.224998 0.149998 -0.0035 +v 0.224998 -0.149998 -0.0035 +v -0.225 0.15 -0.007 +v -0.225 -0.15 -0.007 +v 0.225 -0.15 -0.007 +v 0.225 0.15 -0.007 +vt 1.0 0.0 +vt 1.0 1.0 +vt 0.0 1.0 +vt 0.0 0.0 +usemtl Mirror +s off +f 4/1 3/2 2/3 1/4 +usemtl Back +f 4 7 8 3 +f 3 8 5 2 +f 1 2 5 6 +f 1 6 7 4 +g Picture +v -0.224998 -0.149998 -0.001 +v -0.224998 0.149998 -0.001 +v 0.224998 0.149998 -0.001 +v 0.224998 -0.149998 -0.001 +v -0.225 0.15 -0.007 +v -0.225 -0.15 -0.007 +v 0.225 -0.15 -0.007 +v 0.225 0.15 -0.007 +usemtl Picture +s off +f 12/1 11/2 10/3 9/4 +usemtl Back +f 11 16 13 10 +f 12 15 16 11 +f 9 10 13 14 +f 9 14 15 12 +g Back +v -0.225 0.15 -0.007 +v -0.225 -0.15 -0.007 +v 0.225 -0.15 -0.007 +v 0.225 0.15 -0.007 +usemtl Back +s off +f 18 17 20 19 +g Frame +v -0.2495 -0.1745 0.011 +v -0.2495 0.1745 0.011 +v -0.2495 -0.1745 -0.011 +v -0.2495 0.1745 -0.011 +v 0.2495 -0.1745 0.011 +v 0.2495 0.1745 0.011 +v 0.2495 -0.1745 -0.011 +v 0.2495 0.1745 -0.011 +v -0.224998 -0.149998 -0.001 +v -0.224998 0.149998 -0.001 +v 0.224998 0.149998 -0.001 +v 0.224998 -0.149998 -0.001 +v -0.227305 0.152305 0.003049 +v -0.231621 0.15662 0.006817 +v -0.240536 0.165536 0.009904 +v 0.240536 0.165536 0.009904 +v 0.23162 0.156621 0.006817 +v 0.227305 0.152305 0.003049 +v 0.240536 -0.165536 0.009904 +v 0.231621 -0.156621 0.006817 +v 0.227305 -0.152305 0.003049 +v -0.227305 -0.152305 0.003049 +v -0.231621 -0.156621 0.006817 +v -0.240536 -0.165536 0.009904 +v -0.225 0.15 -0.011 +v -0.225 -0.15 -0.011 +v 0.225 -0.15 -0.011 +v 0.225 0.15 -0.011 +usemtl Frame +s off +f 21 22 24 23 +f 24 28 48 45 +f 27 28 26 25 +f 23 27 25 21 +f 28 24 22 26 +f 23 24 45 46 +f 28 27 47 48 +f 27 23 46 47 +s 1 +f 26 22 35 36 +f 25 26 36 39 +f 22 21 44 35 +f 21 25 39 44 +f 36 35 34 37 +f 37 34 33 38 +f 38 33 30 31 +f 39 36 37 40 +f 40 37 38 41 +f 41 38 31 32 +f 35 44 43 34 +f 34 43 42 33 +f 33 42 29 30 +f 44 39 40 43 +f 43 40 41 42 +f 42 41 32 29 +f 29 46 45 30 +f 31 30 45 48 +f 32 31 48 47 +f 29 32 47 46 diff --git a/coppeliasim/simulation-ram-paper-updated/models/youBot.ttm b/coppeliasim/simulation-ram-paper-updated/models/youBot.ttm new file mode 100644 index 0000000..38a4aef Binary files /dev/null and b/coppeliasim/simulation-ram-paper-updated/models/youBot.ttm differ