diff --git a/interfaces/coppeliasim/DQ_CoppeliaSimInterface.m b/interfaces/coppeliasim/DQ_CoppeliaSimInterface.m new file mode 100644 index 00000000..0c705144 --- /dev/null +++ b/interfaces/coppeliasim/DQ_CoppeliaSimInterface.m @@ -0,0 +1,117 @@ +% (C) Copyright 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. Juan Jose Quiroz Omana (juanjose.quirozomana@manchester.ac.uk) +% - Responsible for the original implementation. + +classdef (Abstract) DQ_CoppeliaSimInterface < handle + + properties (Access = protected) + end + + methods (Access = protected) + % function obj = DQ_CoppeliaSim(inputArg1,inputArg2) + % end + end + + methods(Abstract) + % This method connects to CoppeliaSim. + % Calling this function is required before anything else can happen. + connect(obj, host, port, TIMEOUT_IN_MILISECONDS); + + % This method enables or disables the stepped (synchronous) mode + % for the remote API server service that the client is connected to. + % Example: + % set_stepping_mode(true) % stepping mode enabled + % set_stepping_mode(false) % stepping mode disabled + set_stepping_mode(obj, flag); + + % This method sends trigger signal to the CoppeliaSim scene, + % which performs a simulation step when the stepping mode is used. + trigger_next_simulation_step(obj); + + % This method starts the CoppeliaSim simulation. + start_simulation(obj); + + % This method stops the CoppeliaSim simulation. + stop_simulation(obj); + + % This method gets the handles for a cell array of + % object names in the the CoppeliaSim scene. + handles = get_handles(obj,names); + + % This method gets the handle for a given object in the CoppeliaSim scene. + handle = get_handle(obj,name); + + % This method gets the translation of an object in the CoppeliaSim scene. + t = get_object_translation(obj,objectname); + + % This method sets the translation of an object in the CoppeliaSim scene. + set_object_translation(obj,objectname,translation); + + % This method gets the rotation of an object in the CoppeliaSim scene. + r = get_object_rotation(obj, objectname); + + % This method sets the rotation of an object in the CoppeliaSim scene. + set_object_rotation(obj,objectname,rotation); + + % This method gets the pose of an object in the CoppeliaSim scene. + x = get_object_pose(obj,objectname); + + % This method sets the pose of an object in the CoppeliaSim scene. + set_object_pose(obj,objectname,pose); + + % This method sets the joint positions of a robot in the CoppeliaSim scene. + set_joint_positions(obj,jointnames,joint_positions); + + % This method sets the joint positions of a robot in the CoppeliaSim scene. + joint_positions = get_joint_positions(obj,jointnames); + + % This method sets the joint target positions of a robot in the CoppeliaSim scene. + % It is required a dynamics enabled scene, and joints in dynamic mode + % with position control mode. + % information about joint modes: + % https://www.coppeliarobotics.com/helpFiles/en/jointModes.htm + set_joint_target_positions(obj,jointnames,joint_target_positions); + + % This method gets the joint velocities of a robot in the CoppeliaSim scene. + joint_velocities = get_joint_velocities(obj,jointnames); + + % This method sets the joint target velocities of a robot in the CoppeliaSim scene. + % It is required a dynamics enabled scene, and joints in dynamic mode + % with velocity control mode. Check this link for more + % information about joint modes: + % https://www.coppeliarobotics.com/helpFiles/en/jointModes.htm + set_joint_target_velocities(obj,jointnames,joint_target_velocities); + + % This method sets the joint torques of a robot in the CoppeliaSim scene. + % It is required a dynamics enabled scene, and joints in dynamic mode + % with velocity or force/torque control mode. Check this link for more + % information about joint modes: + % https://www.coppeliarobotics.com/helpFiles/en/jointModes.htm + set_joint_torques(obj,jointnames,torques); + + % This method gets the joint torques of a robot in the CoppeliaSim scene. + joint_torques = get_joint_torques(obj,jointnames); + end +end + + diff --git a/interfaces/coppeliasim/DQ_CoppeliaSimLegacyInterface.m b/interfaces/coppeliasim/DQ_CoppeliaSimLegacyInterface.m new file mode 100644 index 00000000..5ad0383b --- /dev/null +++ b/interfaces/coppeliasim/DQ_CoppeliaSimLegacyInterface.m @@ -0,0 +1,1159 @@ +% CLASS DQ_CoppeliaSimLegacyInterface - Communicate with V-REP using dual quaternions. +% +% Installation: +% 1) Enable V-REP's remote API on the Server Side: +% http://www.coppeliarobotics.com/helpFiles/en/remoteApiServerSide.htm +% - Port 19997 is enabled by default, please refer to the V-REP +% documentation if you need more ports. +% 2) Enable V-REP's remote API on the Client Side: +% http://www.coppeliarobotics.com/helpFiles/en/remoteApiClientSide.htm +% You have to add two folders to your MATLAB path. For example, on +% 64bit Windows: +% - YOUR_VREP_FOLDER\programming\remoteApiBindings\matlab\matlab +% - YOUR_VREP_FOLDER\programming\remoteApiBindings\lib\lib\Windows\64Bit +% For more information refer to the remote API documentation. +% +% Usage: +% If your installation is done correctly, the following minimal example +% will start the V-REP simulation, sleep for one second, and stop the +% simulation: +% 1) Open V-REP with the default scene +% 2) Run +% >> vi = DQ_VrepInterface(); +% >> vi.connect('127.0.0.1',19997); +% >> vi.start_simulation(); +% >> pause(1); +% >> vi.stop_simulation(); +% >> vi.disconnect(); +% +% Familiarizing yourself with V-REP's remote API terminology might be +% helpful to fully understand the documentation. +% http://www.coppeliarobotics.com/helpFiles/en/legacyRemoteApiOverview.htm +% +% DQ_CoppeliaSimLegacyInterface Methods: +% connect - Connects to a V-REP Remote API Server +% disconnect - Disconnects from currently connected server +% disconnect_all - Flushes all Remote API connections +% start_simulation - Start V-REP simulation +% stop_simulation - Stop V-REP simulation +% get_object_translation - Get object translation as a pure +% quaternion +% set_object_translation - Set object translation with a pure +% quaternion +% get_object_rotation - Get object rotation as a unit quaternion +% set_object_rotation - Set object rotation with a unit quaternion +% get_object_pose - Get object pose as a unit dual quaternion +% set_object_pose - Set object pose with a unit dual quaternion +% set_joint_positions - Set the joint positions of a robot +% set_joint_target_positions - Set the joint target positions of a +% robot +% get_joint_positions - Get the joint positions of a robot +% set_synchronous - Set the stepped (synchronous) mode for the remote +% API server service that the client is connected to. +% trigger_next_simulation_step - Send a synchronization trigger +% signal to the CoppeliaSim scene +% wait_for_simulation_step_to_end - Return the time needed for a +% command to be sent to the CoppeliaSim scene, executed, and sent back. +% set_joint_target_velocities - Set the joint target velocities of a +% robot +% get_joint_velocities - Get the joint velocities of a robot +% get_joint_torques - Get the joint torques of a robot +% set_joint_torques - Set the joint torques of a robot +% +% DQ_CoppeliaSimLegacyInterface Methods (For advanced users) +% get_handle - Get the handle of a V-REP object +% get_handles - Get the handles for multiple V-REP objects +% + +% (C) Copyright 2018-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: +% 1. Murilo Marques Marinho (murilo@nml.t.u-tokyo.ac.jp) +% - Responsible for the original implementation. +% +% 2. Juan Jose Quiroz Omana (juanjose.quirozomana@manchester.ac.uk) +% - Added the following methods: +% - set_synchronous() +% - trigger_next_simulation_step() +% - wait_for_simulation_step_to_end() +% - set_joint_target_velocities() +% - get_joint_velocities() +% - Improved the documentation of the class +% - Renamed the class from DQ_VrepInterface to DQ_CoppeliaSimLegacyInterface +% - The class now inherits from DQ_CoppeliaSimInterface +% +% 3. Frederico Fernandes Afonso Silva (frederico.silva@ieee.org) +% - Added the following methods: +% - get_joint_torques() (see https://github.com/dqrobotics/matlab/pull/104) +% - set_joint_torques() (see https://github.com/dqrobotics/matlab/pull/104) +% - Altered the following properties from 'private' to 'protected' +% (see discussions in https://github.com/dqrobotics/matlab/pull/101 +% to further details): +% - vrep +% - clientID + + +classdef DQ_CoppeliaSimLegacyInterface < DQ_CoppeliaSimInterface + + properties (Access = private) + % a map between V-REP object names and DQ_VrepInterfaceMapElements + handles_map; + end + + properties (Access = protected) + % the V-REP remote API instance used by this interface + vrep; + % the client ID of this remote API connection + clientID; + end + + properties (Constant) + % Constant that denotes the V-VREP's remote API blocking operation mode + OP_BLOCKING = remApi('remoteApi').simx_opmode_blocking; + % Constant that denotes the V-VREP's remote API streaming operation mode + OP_STREAMING = remApi('remoteApi').simx_opmode_streaming; + % Constant that denotes the V-VREP's remote API oneshot operation mode + OP_ONESHOT = remApi('remoteApi').simx_opmode_oneshot; + % Constant that denotes the V-VREP's remote API buffer operation mode + OP_BUFFER = remApi('remoteApi').simx_opmode_buffer; + % Constant that denotes the V-VREP's remote API joint velocity ID + JOINT_VELOCITY_PARAMETER_ID = remApi('remoteApi').sim_jointfloatparam_velocity; + end + + methods (Access = private) + + function handle = handle_from_string_or_handle(obj,name_or_handle) + if(ischar(name_or_handle)) + name = name_or_handle; + if(obj.handles_map.isKey(name)) + handle = obj.handles_map(name).handle; + else + handle = obj.get_handle(name); + obj.handles_map(name) = DQ_VrepInterfaceMapElement(handle); + end + + else + handle=name_or_handle; + end + end + + function element = element_from_string(obj,name) + obj.handle_from_string_or_handle(name); %Add new handle if needed + element = obj.handles_map(name); + end + end + + methods + + function obj = DQ_CoppeliaSimLegacyInterface() + obj.vrep=remApi('remoteApi'); + obj.handles_map = containers.Map; + obj.clientID = -1; + disp(['This version of DQ Robotics DQ_VrepInterface is compatible'... + ' with VREP 3.5.0']); + end + + function connect(obj,ip,port) + % This method connects to the remote api server (i.e. CoppeliaSim). + % Calling this function is required before anything else can happen. + % Usage: + % connect(ip, port); + % ip: The ip address where the CoppeliaSim is located. + % port: The port number where to connect. + % + % Example: + % connect('127.0.0.1', 19997); + + obj.clientID = obj.vrep.simxStart(ip,port,true,true,5000,5); + if (obj.clientID>-1) + disp('Connected to the remote API server'); + else + error('Failed to connect to remote API server'); + end + end + + function disconnect(obj) + % This method ends the communication between the client and + % the CoppeliaSim scene. This should be the very last method called. + obj.vrep.simxFinish(obj.clientID); + end + + function disconnect_all(obj) + % This method ends all running communication threads with the + % CoppeliaSim scene. This should be the very last method called. + obj.vrep.simxFinish(-1); + end + + function set_synchronous(obj,flag) + % This method enables or disables the stepped (synchronous) mode + % for the remote API server service that the client is connected to. + % Example: + % set_synchronous(true) % synchronous mode enabled + % set_synchronous(false) % synchronous mode disabled + if (~islogical(flag)) + error('Error in set_synchronous: argument must be logical, not a %s. \nExample:\n set_synchronous(true)', class(flag)); + end + obj.vrep.simxSynchronous(obj.clientID,flag); + end + + % This method enables or disables the stepped (synchronous) mode + % for the remote API server service that the client is connected to. + % Example: + % set_stepping_mode(true) % stepping mode enabled + % set_stepping_mode(false) % stepping mode disabled + function set_stepping_mode(obj, flag) + obj.set_synchronous(flag); + end + + function trigger_next_simulation_step(obj) + % This method sends a synchronization trigger signal to the CoppeliaSim scene, + % which performs a simulation step when the synchronous mode is used. + obj.vrep.simxSynchronousTrigger(obj.clientID); + end + + function ping_time = wait_for_simulation_step_to_end(obj) + % This method returns the time needed for a command to be sent + % to the CoppeliaSim scene, executed, and sent back. + [~, ping_time] = obj.vrep.simxGetPingTime(obj.clientID); + end + + function start_simulation(obj) + % This method starts the CoppeliaSim simulation. + obj.vrep.simxStartSimulation(obj.clientID,obj.vrep.simx_opmode_oneshot); + end + + function stop_simulation(obj) + % This method stops the CoppeliaSim simulation. + obj.vrep.simxStopSimulation(obj.clientID,obj.vrep.simx_opmode_blocking); + end + + function handles = get_handles(obj,names) + % This method gets the handles for a cell array of + % object names in the the CoppeliaSim scene. + % Usage: + % get_handles(names); + % names: The cell array of object names. + % + % Example: + % handle = get_handles({'ReferenceFrame_1', 'ReferenceFrame_2'}); + + handles = []; + if(iscell(names)) + for i=1:length(names) + [~,handles(i)] = obj.vrep.simxGetObjectHandle(... + obj.clientID,... + char(names(i)),... + obj.vrep.simx_opmode_blocking); + end + else + error('Error in get_handles: argument names must be of type cell, e.g. names = [{joint1,joint2}];'); + end + end + + function handle = get_handle(obj,name) + % This method gets the handle for a given object in the CoppeliaSim scene. + % + % Usage: + % get_handles(name); + % names: The object name. + % Example: + % handle = get_handle('ReferenceFrame'); + + [~,handle] = obj.vrep.simxGetObjectHandle(... + obj.clientID,... + name,... + obj.vrep.simx_opmode_blocking); + end + + function t = get_object_translation(obj,objectname,reference_frame,opmode) + % This method gets the translation of an object in the CoppeliaSim scene. + % + % Usage: + % Recommended: + % t = get_object_translation(objectname) + % + % Advanced: + % t = get_object_translation(objectname,reference_frame,opmode); + % + % objectname: The object name + % (optional) reference_frame: Indicates the name of + % the relative reference frame in which you + % want the translation. If not specified, + % the absolute frame is used. + % (optional) opmode: The operation mode. If not specified, + % the opmode will be set automatically. + % + % You can use the following modes: + % OP_BLOCKING + % OP_STREAMING + % OP_ONESHOT + % OP_BUFFER; + % + % Check this link for more details: + % https://www.coppeliarobotics.com/helpFiles/en/remoteApiModusOperandi.htm + % + % Example: + % % Recommended: + % t = get_object_translation('DefaultCamera'); + % + % % For advanced usage: + % t = get_object_translation('DefaultCamera', 'Frame_b', OP_ONESHOT); + + % First approach to the auto-management using + % DQ_VrepInterfaceMapElements. If the user does not specify the + % opmode, it is chosen first as STREAMING and then as BUFFER, + % as specified by the remote API documentation + + handle = objectname; % alias + + if nargin <= 2 + element = obj.element_from_string(handle); + if(~element.state_from_function_signature('get_object_translation')) + [~,object_position] = obj.vrep.simxGetObjectPosition(... + obj.clientID,... + obj.handle_from_string_or_handle(handle),... + -1,... + obj.OP_STREAMING); + % We need to check the buffer until it is not empty, + % TODO add a timeout. + retval = 1; + while retval==1 + [retval,object_position] = obj.vrep.simxGetObjectPosition(... + obj.clientID,... + obj.handle_from_string_or_handle(handle),... + -1,... + obj.OP_BUFFER); + end + else + [~,object_position] = obj.vrep.simxGetObjectPosition(... + obj.clientID,... + obj.handle_from_string_or_handle(handle),... + -1,... + obj.OP_BUFFER); + end + else + relative_to_handle = reference_frame; % alias + [~,object_position] = obj.vrep.simxGetObjectPosition(... + obj.clientID,... + obj.handle_from_string_or_handle(handle),... + obj.handle_from_string_or_handle(relative_to_handle),... + opmode); + end + t = DQ([0,double(object_position)]); + end + + function set_object_translation(obj,objectname,translation,reference_frame,opmode) + % This method sets the translation of an object in the CoppeliaSim scene. + % Usage: + % Recommended: + % set_object_translation(objectname,translation); + % + % Advanced: + % set_object_translation(objectname,translation,reference_frame,opmode); + % + % objectname: The object name. + % translation: The desired translation. + % (optional) reference_frame: Indicates the name of + % the relative reference frame in which + % the desired translation is expressed. + % If not specified, the absolute frame is used. + % (optional) opmode: The operation mode. If not specified, + % the opmode will be set automatically. + % + % You can use the following modes: + % OP_BLOCKING + % OP_STREAMING + % OP_ONESHOT + % OP_BUFFER; + % + % Check this link for more details: + % https://www.coppeliarobotics.com/helpFiles/en/remoteApiModusOperandi.htm + % + % Example: + % % Recommended: + % set_object_translation('DefaultCamera', t); + % + % % For advanced usage: + % set_object_translation('DefaultCamera', t, 'Frame_b', OP_ONESHOT); + + % create some aliases + handle = objectname; + t = translation; + + if nargin == 3 + obj.vrep.simxSetObjectPosition(obj.clientID,... + obj.handle_from_string_or_handle(handle),... + -1,... + t.q(2:4),... + obj.OP_ONESHOT); + else + relative_to_handle = reference_frame; % alias + obj.vrep.simxSetObjectPosition(obj.clientID,... + obj.handle_from_string_or_handle(handle),... + obj.handle_from_string_or_handle(relative_to_handle),... + t.q(2:4),... + opmode); + end + end + + function r = get_object_rotation(obj, objectname, reference_frame, opmode) + % This method gets the rotation of an object in the CoppeliaSim scene. + % + % Usage: + % Recommended: + % t = get_object_rotation(objectname); + % + % Advanced: + % t = get_object_rotation(objectname,reference_frame,opmode); + % + % objectname: The object name + % (optional) reference_frame: Indicates the name of + % the relative reference frame in which you + % want the rotation. If not specified, + % the absolute frame is used. + % (optional) opmode: The operation mode. If not specified, + % the opmode will be set automatically. + % + % You can use the following modes: + % OP_BLOCKING + % OP_STREAMING + % OP_ONESHOT + % OP_BUFFER; + % + % Check this link for more details: + % https://www.coppeliarobotics.com/helpFiles/en/remoteApiModusOperandi.htm + % + % Example: + % % Recommended: + % r = get_object_rotation('DefaultCamera'); + % + % % For advanced usage: + % r = get_object_rotation('DefaultCamera', 'Frame_b', OP_ONESHOT); + + + % create some aliases + handle = objectname; + id = obj.clientID; + handle1 = obj.handle_from_string_or_handle(handle); + + + % First approach to the auto-management using + % DQ_VrepInterfaceMapElements. If the user does not specify the + % opmode, it is chosen first as STREAMING and then as BUFFER, + % as specified by the remote API documentation + if nargin <= 2 + element = obj.element_from_string(handle); + if(~element.state_from_function_signature('get_object_rotation')) + [~,obj_rot] = obj.vrep.simxGetObjectQuaternion(id, ... + handle1,... + -1,... + obj.OP_STREAMING); + % We need to check the buffer until it is not empty, + % TODO add a timeout. + retval = 1; + while retval==1 + [retval,obj_rot] = obj.vrep.simxGetObjectQuaternion(id,... + handle1,... + -1,... + obj.OP_BUFFER); + end + else + [~,obj_rot] = obj.vrep.simxGetObjectQuaternion(id,handle1,... + -1,... + obj.OP_BUFFER); + end + else + relative_to_handle = reference_frame; % alias + handle2 = obj.handle_from_string_or_handle(relative_to_handle); + [~,obj_rot] = obj.vrep.simxGetObjectQuaternion(id,... + handle1,... + handle2,... + opmode); + end + object_rotation_double = double(obj_rot); + + %V-Rep's quaternion representation is [x y z w] so we have to + %take that into account + r = normalize(DQ([object_rotation_double(4),... + object_rotation_double(1),... + object_rotation_double(2),... + object_rotation_double(3)])); + end + + function set_object_rotation(obj,objectname,rotation,reference_frame,opmode) + % This method sets the rotation of an object in the CoppeliaSim scene. + % Usage: + % Recommended: + % set_object_rotation(objectname,rotation); + % + % Advanced: + % set_object_rotation(objectname,rotation,reference_frame,opmode); + % + % objectname: The object name. + % rotation: The desired rotation. + % (optional) reference_frame: Indicates the name of + % the relative reference frame in which + % the desired rotation is expressed. + % If not specified, the absolute frame is used. + % (optional) opmode: The operation mode. If not specified, + % the opmode will be set automatically. + % + % You can use the following modes: + % OP_BLOCKING + % OP_STREAMING + % OP_ONESHOT + % OP_BUFFER; + % + % Check this link for more details: + % https://www.coppeliarobotics.com/helpFiles/en/remoteApiModusOperandi.htm + % + % Example: + % % Recommended: + % set_object_rotation('DefaultCamera', r); + % + % % For advanced usage: + % set_object_rotation('DefaultCamera', r, 'Frame_b', OP_ONESHOT); + + % create some aliases + handle = objectname; + r = rotation; + + if nargin == 3 + obj.vrep.simxSetObjectQuaternion(... + obj.clientID,... + obj.handle_from_string_or_handle(handle),... + -1,... + [r.q(2:4); r.q(1)],... + obj.OP_ONESHOT); %V-Rep's quaternion representation is [x y z w] so we have to take that into account + else + relative_to_handle = reference_frame; % alias + obj.vrep.simxSetObjectQuaternion(... + obj.clientID,... + obj.handle_from_string_or_handle(handle),... + obj.handle_from_string_or_handle(relative_to_handle),... + [r.q(2:4); r.q(1)],... + opmode); %V-Rep's quaternion representation is [x y z w] so we have to take that into account + end + end + + function x = get_object_pose(obj,objectname,reference_frame,opmode) + % This method gets the pose of an object in the CoppeliaSim scene. + % + % Usage: + % Recommended: + % x = get_object_pose(objectname); + % + % Advanced: + % x = get_object_pose(objectname,reference_frame,opmode); + % + % objectname: The object name + % (optional) reference_frame: Indicates the name of + % the relative reference frame in which you + % want the pose. If not specified, + % the absolute frame is used. + % (optional) opmode: The operation mode. If not specified, + % the opmode will be set automatically. + % + % You can use the following modes: + % OP_BLOCKING + % OP_STREAMING + % OP_ONESHOT + % OP_BUFFER; + % + % Check this link for more details: + % https://www.coppeliarobotics.com/helpFiles/en/remoteApiModusOperandi.htm + % + % Example: + % % Recommended: + % x = get_object_pose('DefaultCamera'); + % + % % For advanced usage: + % x = get_object_pose('DefaultCamera', 'Frame_b', OP_ONESHOT); + + + handle = objectname; % alias + + + if nargin <= 2 + t = obj.get_object_translation(handle); + r = obj.get_object_rotation(handle); + else + relative_to_handle = reference_frame; % alias + t = obj.get_object_translation(... + obj.handle_from_string_or_handle(handle),... + obj.handle_from_string_or_handle(relative_to_handle),... + opmode); + r = obj.get_object_rotation(... + obj.handle_from_string_or_handle(handle),... + obj.handle_from_string_or_handle(relative_to_handle),... + opmode); + end + x = r + 0.5*DQ.E*t*r; + end + + function set_object_pose(obj,objectname,pose,reference_frame,opmode) + % This method sets the pose of an object in the CoppeliaSim scene. + % Usage: + % Recommended: + % set_object_pose(objectname,pose); + % + % Advanced: + % set_object_pose(objectname,pose,reference_frame,opmode); + % + % objectname: The object name. + % pose: The desired pose. + % (optional) reference_frame: Indicates the name of + % the relative reference frame in which + % the desired pose is expressed. + % If not specified, the absolute frame is used. + % (optional) opmode: The operation mode. If not specified, + % the opmode will be set automatically. + % + % You can use the following modes: + % OP_BLOCKING + % OP_STREAMING + % OP_ONESHOT + % OP_BUFFER; + % + % Check this link for more details: + % https://www.coppeliarobotics.com/helpFiles/en/remoteApiModusOperandi.htm + % + % Example: + % + % t = DQ.i*0.01; + % r = DQ.i; + % x = r+0.5*DQ.E*t*r; + % + % % Recommended: + % set_object_pose('DefaultCamera', x); + % + % % For advanced usage: + % set_object_pose('DefaultCamera', x, 'Frame_b', OP_ONESHOT); + + % create some aliases + handle = objectname; + x = pose; + + if nargin == 3 + t = translation(x); + r = rotation(x); + obj.set_object_translation(... + obj.handle_from_string_or_handle(handle),... + t,... + -1,... + obj.OP_ONESHOT); + obj.set_object_rotation(... + obj.handle_from_string_or_handle(handle),... + r,... + -1,... + obj.OP_ONESHOT); + else + relative_to_handle = reference_frame; + t = translation(x); + r = rotation(x); + obj.set_object_translation(... + obj.handle_from_string_or_handle(handle),... + t,... + obj.handle_from_string_or_handle(relative_to_handle),... + opmode); + obj.set_object_rotation(... + obj.handle_from_string_or_handle(handle),... + r,... + obj.handle_from_string_or_handle(relative_to_handle),... + opmode); + end + end + + function set_joint_positions(obj,jointnames,joint_positions,opmode) + % This method sets the joint positions of a robot in the CoppeliaSim scene. + % It is required a dynamics disabled scene. + % + % Usage: + % Recommended: + % set_joint_positions(jointnames, joint_positions); + % + % Advanced: + % set_joint_positions(jointnames, joint_positions, opmode); + % + % jointnames: The joint names. + % joint_positions: The joint positions. + % (optional) opmode: The operation mode. If not specified, + % the opmode will be set automatically. + % + % You can use the following modes: + % OP_BLOCKING + % OP_STREAMING + % OP_ONESHOT + % OP_BUFFER; + % + % Check this link for more details: + % https://www.coppeliarobotics.com/helpFiles/en/remoteApiModusOperandi.htm + % + % Example: + % jointnames={'LBR4p_joint1','LBR4p_joint2','LBR4p_joint3','LBR4p_joint4',... + % 'LBR4p_joint5','LBR4p_joint6','LBR4p_joint7'}; + % u = [0.1 0.1 0.1 0.1 0.1 0.1 0.1]; + % + % % Recommended: + % set_joint_positions(jointnames, u); + % + % % Advanced usage: + % set_joint_positions(jointnames, u, OP_ONESHOT); + + % create some aliases + handles = jointnames; + thetas = joint_positions; + + if nargin == 3 + % The recommended mode is OP_ONESHOT + opmode = obj.OP_ONESHOT; + end + + for joint_index=1:length(handles) + if isa(handles,'cell') + obj.vrep.simxSetJointPosition(... + obj.clientID,... + obj.handle_from_string_or_handle(handles{joint_index}),... + thetas(joint_index),... + opmode); + else + obj.vrep.simxSetJointPosition(... + obj.clientID,... + obj.handle_from_string_or_handle(handles),... + thetas(joint_index),... + opmode); + end + end + end + + function set_joint_target_positions(obj,jointnames,joint_target_positions,opmode) + % This method sets the joint target positions of a robot in the CoppeliaSim scene. + % It is required a dynamics enabled scene, and joints in dynamic mode + % with position control mode. + % + % Usage: + % Recommended: + % set_joint_target_positions(jointnames,joint_target_positions); + % + % Advanced: + % set_joint_target_positions(jointnames, joint_target_positions, opmode); + % + % jointnames: The joint names. + % joint_target_positions: The joint target positions. + % (optional) opmode: The operation mode. If not specified, + % the opmode will be set automatically. + % + % You can use the following modes: + % OP_BLOCKING + % OP_STREAMING + % OP_ONESHOT + % OP_BUFFER; + % + % Check this link for more details: + % https://www.coppeliarobotics.com/helpFiles/en/remoteApiModusOperandi.htm + % + % Example: + % jointnames={'LBR4p_joint1','LBR4p_joint2','LBR4p_joint3','LBR4p_joint4',... + % 'LBR4p_joint5','LBR4p_joint6','LBR4p_joint7'}; + % u = [0.1 0.1 0.1 0.1 0.1 0.1 0.1]; + % + % % Recommended: + % set_joint_target_positions(jointnames, u); + % + % % Advanced usage: + % set_joint_target_positions(jointnames, u, OP_ONESHOT); + + % create some aliases + handles = jointnames; + thetas = joint_target_positions; + + if nargin == 3 + % The recommended mode is OP_ONESHOT + opmode = obj.OP_ONESHOT; + end + + for joint_index=1:length(handles) + if isa(handles,'cell') + obj.vrep.simxSetJointTargetPosition(... + obj.clientID,... + obj.handle_from_string_or_handle(handles{joint_index}),... + thetas(joint_index),... + opmode); + else + obj.vrep.simxSetJointTargetPosition(... + obj.clientID,... + obj.handle_from_string_or_handle(handles),... + thetas(joint_index),... + opmode); + end + end + end + + function [joint_positions,retval]=get_joint_positions(obj,jointnames,opmode) + % This method gets the joint positions of a robot in the CoppeliaSim scene. + % Usage: + % Recommended: + % joint_positions = get_joint_positions(jointnames); + % [joint_positions, retval] = get_joint_positions(jointnames); + % + % Advanced: + % joint_positions] = get_joint_positions(jointnames,opmode); + % [joint_positions, retval] = get_joint_positions(jointnames, opmode); + % + % -Parameters: + % + % jointnames: The joint names. + % (optional) opmode: The operation mode. If not specified, + % the opmode will be set automatically. + % + % You can use the following modes: + % OP_BLOCKING + % OP_STREAMING + % OP_ONESHOT + % OP_BUFFER; + % + % Check this link for more details: + % https://www.coppeliarobotics.com/helpFiles/en/remoteApiModusOperandi.htm + % + % - Outputs: + % + % joint_positions: The joints positions + % retval: The return code of the Remote API function, + % which is defined in https://www.coppeliarobotics.com/helpFiles/en/remoteApiConstants.htm#functionErrorCodes + % + % + % Example: + % jointnames={'LBR4p_joint1','LBR4p_joint2','LBR4p_joint3','LBR4p_joint4',... + % 'LBR4p_joint5','LBR4p_joint6','LBR4p_joint7'}; + % + % % Recommended: + % joint_positions = get_joint_positions(jointnames); + % [joint_positions, rtn] = get_joint_positions(jointnames); + % + % % Advanced usage: + % joint_positions = get_joint_positions(jointnames, OP_ONESHOT); + % [joint_positions, rtn] = get_joint_positions(jointnames, OP_ONESHOT); + + + % create some aliases + handles = jointnames; + thetas = zeros(length(handles),1); + + for joint_index=1:length(handles) + % First approach to the auto-management using + % DQ_VrepInterfaceMapElements. If the user does not specify the + % opmode, it is chosen first as STREAMING and then as BUFFER, + % as specified by the remote API documentation + if nargin <= 2 + if isa(handles,'cell') + element = obj.element_from_string(handles{joint_index}); + else + element = obj.element_from_string(handles); + end + if(~element.state_from_function_signature('get_joint_positions')) + [~,tmp] = obj.vrep.simxGetJointPosition(... + obj.clientID,... + element.handle,... + obj.OP_STREAMING); + retval=1; + while retval==1 + [retval,tmp] = obj.vrep.simxGetJointPosition(... + obj.clientID,... + element.handle,... + obj.OP_BUFFER); + end + else + [retval,tmp] = obj.vrep.simxGetJointPosition(... + obj.clientID,... + element.handle,... + obj.OP_BUFFER); + end + else + [retval,tmp] = obj.vrep.simxGetJointPosition(... + obj.clientID,... + obj.handle_from_string_or_handle(handles{joint_index}),... + opmode); + end + thetas(joint_index) = double(tmp); + end + joint_positions = thetas; + end + + function joint_velocities = get_joint_velocities(obj,jointnames,opmode) + % This method gets the joint velocities of a robot in the CoppeliaSim scene. + % Usage: + % Recommended: + % joint_velocities = get_joint_velocities(jointnames); + % + % Advanced: + % joint_velocities = get_joint_velocities(jointnames, opmode) + % + % jointnames: The joint names. + % (optional) opmode: The operation mode. If not specified, + % the opmode will be set automatically. + % + % You can use the following modes: + % OP_BLOCKING + % OP_STREAMING + % OP_ONESHOT + % OP_BUFFER; + % + % Check this link for more details: + % https://www.coppeliarobotics.com/helpFiles/en/remoteApiModusOperandi.htm + % + % Example: + % jointnames={'LBR4p_joint1','LBR4p_joint2','LBR4p_joint3','LBR4p_joint4',... + % 'LBR4p_joint5','LBR4p_joint6','LBR4p_joint7'}; + % + % % Recommended: + % joint_velocities = get_joint_velocities(jointnames); + % + % % Advanced usage: + % joint_velocities = get_joint_velocities(jointnames, OP_ONESHOT); + + joint_velocities = zeros(length(jointnames),1); + for joint_index=1:length(jointnames) + % First approach to the auto-management using + % DQ_VrepInterfaceMapElements. If the user does not specify the + % opmode, it is chosen first as STREAMING and then as BUFFER, + % as specified by the remote API documentation + if nargin <= 2 + if isa(jointnames,'cell') + element = obj.element_from_string(jointnames{joint_index}); + else + element = obj.element_from_string(jointnames); + end + if(~element.state_from_function_signature('get_joint_velocities')) + [~,tmp] = obj.vrep.simxGetObjectFloatParameter(... + obj.clientID,... + element.handle,... + obj.JOINT_VELOCITY_PARAMETER_ID,... + obj.OP_STREAMING); + retval=1; + while retval==1 + [retval,tmp] = obj.vrep.simxGetObjectFloatParameter(... + obj.clientID,... + element.handle,... + obj.JOINT_VELOCITY_PARAMETER_ID,... + obj.OP_BUFFER); + end + else + [~,tmp] = obj.vrep.simxGetObjectFloatParameter(... + obj.clientID,... + element.handle,... + obj.JOINT_VELOCITY_PARAMETER_ID,... + obj.OP_BUFFER); + end + else + [~,tmp] = obj.vrep.simxGetObjectFloatParameter(... + obj.clientID,... + obj.handle_from_string_or_handle(jointnames{joint_index}),... + obj.JOINT_VELOCITY_PARAMETER_ID,... + opmode); + end + joint_velocities(joint_index) = double(tmp); + end + end + + function set_joint_target_velocities(obj,jointnames,joint_target_velocities,opmode) + % This method sets the joint velocities of a robot in the CoppeliaSim scene. + % It is required a dynamics enabled scene, and joints in dynamic mode + % with velocity control mode. Check this link for more + % information about joint modes: + % https://www.coppeliarobotics.com/helpFiles/en/jointModes.htm + % + % Usage: + % Recommended: + % set_joint_target_velocities(jointnames, joint_target_velocities); + % + % Advanced: + % set_joint_target_velocities(jointnames, joint_target_velocities, opmode); + % + % jointnames: The joint names. + % joint_target_velocities: The joint target velocities. + % (optional) opmode: The operation mode. If not specified, + % the opmode will be set automatically. + % + % You can use the following modes: + % OP_BLOCKING + % OP_STREAMING + % OP_ONESHOT + % OP_BUFFER; + % + % Check this link for more details: + % https://www.coppeliarobotics.com/helpFiles/en/remoteApiModusOperandi.htm + % + % Example: + % jointnames={'LBR4p_joint1','LBR4p_joint2','LBR4p_joint3','LBR4p_joint4',... + % 'LBR4p_joint5','LBR4p_joint6','LBR4p_joint7'}; + % u = [0.1 0.1 0.1 0.1 0.1 0.1 0.1]; + % + % % Recommended: + % set_joint_target_velocities(jointnames, u); + % + % % Advanced usage: + % set_joint_target_velocities(jointnames, u, OP_ONESHOT); + + if nargin == 3 + % The recommended mode is OP_ONESHOT + opmode = obj.OP_ONESHOT; + end + + for joint_index=1:length(jointnames) + if isa(jointnames,'cell') + obj.vrep.simxSetJointTargetVelocity(... + obj.clientID,... + obj.handle_from_string_or_handle(jointnames{joint_index}),... + joint_target_velocities(joint_index),... + opmode); + else + obj.vrep.simxSetJointTargetVelocity(... + obj.clientID,... + obj.handle_from_string_or_handle(jointnames),... + joint_target_velocities(joint_index),... + opmode); + end + end + end + + function joint_torques = get_joint_torques(obj,jointnames,opmode) + % This method gets the joint torques of a robot in the CoppeliaSim scene. + % Usage: + % Recommended: + % joint_torques = get_joint_torques(jointnames); + % + % Advanced: + % joint_torques = get_joint_torques(jointnames, opmode) + % + % jointnames: The joint names. + % (optional) opmode: The operation mode. If not + % specified, the opmode will be set automatically. + % + % You can use the following modes: + % OP_BLOCKING + % OP_STREAMING + % OP_ONESHOT + % OP_BUFFER; + % + % Check this link for more details: https://www.coppeliarobotics.com/helpFiles/en/remoteApiFunctionsMatlab.htm#simxGetJointForce + % + % Example: + % jointnames={'LBR4p_joint1','LBR4p_joint2','LBR4p_joint3','LBR4p_joint4',... + % 'LBR4p_joint5','LBR4p_joint6','LBR4p_joint7'}; + % + % % Recommended: + % joint_torques = get_joint_torques(jointnames); + % + % % Advanced usage: + % joint_torques = get_joint_torques(jointnames, OP_STREAMING); + + joint_torques = zeros(length(jointnames),1); + + % If the user does not specify the opmode, it is chosen first + % as STREAMING and then as BUFFER, as specified by the remote + % API documentation. + for joint_index=1:length(jointnames) + % First approach to the auto-management using + % DQ_VrepInterfaceMapElements. If the user does not specify the + % opmode, it is chosen first as STREAMING and then as BUFFER, + % as specified by the remote API documentation + if nargin <= 2 + if isa(jointnames,'cell') + element = obj.element_from_string(jointnames{joint_index}); + else + element = obj.element_from_string(jointnames); + end + if(~element.state_from_function_signature('get_joint_torques')) + [~,tmp] = obj.vrep.simxGetJointForce(obj.clientID, element.handle, obj.OP_STREAMING); + return_code = 1; + while(return_code == 1) + [return_code,tmp] = obj.vrep.simxGetJointForce(obj.clientID, element.handle, obj.OP_BUFFER); + end + else + [~,tmp] = obj.vrep.simxGetJointForce(obj.clientID, element.handle, obj.OP_BUFFER); + end + else + [~,tmp] = obj.vrep.simxGetJointForce(obj.clientID, ... + obj.handle_from_string_or_handle(jointnames{joint_index}), opmode); + end + joint_torques(joint_index) = double(-tmp); % V-REP returns joint torques with an inverse sign + end + end + + function set_joint_torques(obj,jointnames,torques,opmode) + % This method sets the joint torques of a robot in the CoppeliaSim scene. + % Usage: + % Recommended: + % set_joint_torques(jointnames, torques); + % + % Advanced: + % set_joint_torques(jointnames, torques, opmode); + % + % jointnames: The joint names. + % torques: The joint torques. + % (optional) opmode: The operation mode. If not + % specified, the opmode will be set automatically. + % + % You can use the following modes: + % OP_BLOCKING + % OP_STREAMING + % OP_ONESHOT + % OP_BUFFER; + % + % Check this link for more details: https://www.coppeliarobotics.com/helpFiles/en/remoteApiFunctionsMatlab.htm#simxSetJointTargetVelocity + % + % Example: + % jointnames={'LBR4p_joint1','LBR4p_joint2','LBR4p_joint3','LBR4p_joint4',... + % 'LBR4p_joint5','LBR4p_joint6','LBR4p_joint7'}; + % u = [0.1 0.1 0.1 0.1 0.1 0.1 0.1]; + % + % % Recommended: + % set_joint_torques(jointnames, u); + % + % % Advanced usage: + % set_joint_torques(jointnames, u, opmode); + + % If the user does not specify the opmode, it is chosen first + % as STREAMING and then as OP_ONESHOT, as specified by the + % remote API documentation. + if nargin == 3 + % The recommended mode is OP_ONESHOT + for joint_index=1:length(jointnames) + obj.vrep.simxSetJointTargetVelocity(obj.clientID, obj.handle_from_string_or_handle(jointnames{joint_index}), ... + sign(torques(joint_index))*10e10, obj.OP_ONESHOT); + obj.vrep.simxSetJointForce(obj.clientID, obj.handle_from_string_or_handle(jointnames{joint_index}), ... + abs(torques(joint_index)), obj.OP_ONESHOT); + end + else + for joint_index=1:length(jointnames) + obj.vrep.simxSetJointTargetVelocity(obj.clientID, obj.handle_from_string_or_handle(jointnames{joint_index}),... + sign(torques(joint_index))*10e10, opmode); + obj.vrep.simxSetJointForce(obj.clientID,obj.handle_from_string_or_handle(jointnames{joint_index}),abs(torques(joint_index)),... + opmode); + end + end + end + + end + +end + + + diff --git a/interfaces/coppeliasim/DQ_CoppeliaSimRobot.m b/interfaces/coppeliasim/DQ_CoppeliaSimRobot.m new file mode 100644 index 00000000..ab80a798 --- /dev/null +++ b/interfaces/coppeliasim/DQ_CoppeliaSimRobot.m @@ -0,0 +1,72 @@ +% CLASS DQ_CoppeliaSimRobot - Abstract class with methods to send and receive +% robot information to and from VREP. +% +% Usage: +% Inherit from this class and implement the abstract methods. +% +% DQ_VrepRobot Methods (Abstract): +% set_configuration - Sends the joint configurations to VREP +% get_configuration - Obtains the joint configurations from VREP + +% (C) Copyright 2018-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.sourceforge.net +% +% Contributors to this file: +% 1. Murilo Marques Marinho - murilo@nml.t.u-tokyo.ac.jp +% - Responsible for the original implementation. +% 2. Frederico Fernandes Afonso Silva (frederico.silva@ieee.org) +% - Deprecated the following methods to ensure compatibility with the +% C++ version of the class: +% - 'send_q_to_vrep' +% - 'get_q_from_vrep' +% - Removed the following methods to ensure compatibility with the +% C++ version of the class: +% - 'kinematics' +% 3. Juan Jose Quiroz Omana (juanjose.quirozomana@manchester.ac.uk) +% - Renamed the class from DQ_VrepRobot to DQ_CoppeliaSimRobot + +classdef (Abstract) DQ_CoppeliaSimRobot + + properties + robot_name + vrep_interface + end + + methods (Abstract) + set_configuration(obj,q); + q = get_configuration(obj); + end + + methods + function send_q_to_vrep(obj, q) + % For backwards compatibility only. Do not use this method. + + warning('Deprecated. Use set_configuration() instead.') + obj.set_configuration(q) + end + + function q = get_q_from_vrep(obj) + % For backwards compatibility only. Do not use this method. + + warning('Deprecated. Use get_configuration() instead.') + q = obj.get_configuration(); + end + end +end + diff --git a/interfaces/coppeliasim/DQ_SerialCoppeliaSimRobot.m b/interfaces/coppeliasim/DQ_SerialCoppeliaSimRobot.m new file mode 100644 index 00000000..a41d1469 --- /dev/null +++ b/interfaces/coppeliasim/DQ_SerialCoppeliaSimRobot.m @@ -0,0 +1,241 @@ +% CLASS DQ_SerialCoppeliaSimRobot - Concrete class to interface with serial robots +% in VREP. +% +% Usage: +% 1) Drag-and-drop a serial robot to a VREP scene. For instance, a +% "my_robot" robot. +% 2) Run +% >> vi = DQ_VrepInterface(); +% >> vi.connect('127.0.0.1',19997); +% >> vrep_robot = DQ_SerialVrepRobot("my_robot", 7, "my_robot", vi); +% >> vi.start_simulation(); +% >> vrep_robot.get_configuration(); +% >> pause(1); +% >> vi.stop_simulation(); +% >> vi.disconnect(); +% Note that the name of the robot should be EXACTLY the same as in +% VREP. For instance, if you drag-and-drop a second robot, its name +% will become "my_robot#0", a third robot, "my_robot#1", and so on. +% +% DQ_SerialVrepRobot Methods: +% get_joint_names - Gets the joint names of the robot in the CoppeliaSim scene. +% set_configuration - Sets the joint configurations of the robot in the CoppeliaSim scene. +% get_configuration - Gets the joint configurations of the robot in the CoppeliaSim scene. +% set_target_configuration - Sets the joint configurations of the robot in the CoppeliaSim scene as a target configuration for the joint controllers. +% get_configuration_velocities - Gets the joint velocities of the robot in the CoppeliaSim scene. +% set_target_configuration_velocities - Sets the joint velocities of the robot in the CoppeliaSim scene as a target velocity for the joint controllers. +% set_configuration_space_torques - Sets the joint torques of the robot in the CoppeliaSim scene. +% get_configuration_space_torques - Gets the joint torques of the robot in the CoppeliaSim scene. + +% (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, based on the C++ version: +% - DQ_VrepInterface.h: https://github.com/dqrobotics/cpp-interface-vrep/blob/master/include/dqrobotics/interfaces/vrep/DQ_VrepInterface.h +% - DQ_SerialVrepRobot.cpp: https://github.com/dqrobotics/cpp-interface-vrep/blob/master/src/dqrobotics/interfaces/vrep/DQ_SerialVrepRobot.cpp +% 2. Juan Jose Quiroz Omana (juanjose.quirozomana@manchester.ac.uk) +% - Renamed the class from DQ_SerialVrepRobot to DQ_SerialCoppeliaSimRobot +classdef DQ_SerialCoppeliaSimRobot < DQ_CoppeliaSimRobot + properties + joint_names; + base_frame_name; + end + + methods + function obj = DQ_SerialCoppeliaSimRobot(base_robot_name, robot_dof, robot_name, vrep_interface) + % This method constructs an instance of a DQ_SerialVrepRobot. + % Usage: + % DQ_SerialVrepRobot(base_robot_name, robot_dof, robot_name, vrep_interface); + % base_robot_name: The base name of the robot in the CoppeliaSim scene. + % robot_dof: The number of DoF of the robot in the CoppeliaSim scene. + % robot_name: The name of the robot in the CoppeliaSim scene. + % vrep_interface: The DQ_VrepInterface object connected to the CoppeliaSim scene. + % + % Example: + % vi = DQ_VrepInterface(); + % vi.connect('127.0.0.1',19997); + % vrep_robot = DQ_SerialVrepRobot("my_robot", 7, "my_robot#1", vi); + % + % Note that the name of the robot should be EXACTLY the same as in the CoppeliaSim + % scene. For instance, if you drag-and-drop a second robot, its name will become + % "my_robot#0", a third robot, "my_robot#1", and so on. + + obj.robot_name = robot_name; + obj.vrep_interface = vrep_interface; + + %% The use of 'initialize_joint_names_from_vrep()', as is done in the C++ implementation, is not supported on a constructor in MATLAB + % From the second copy of the robot and onward, VREP appends a + % #number in the robot's name. We check here if the robot is + % called by the correct name and assign an index that will be + % used to correctly infer the robot's joint labels. + splited_name = strsplit(obj.robot_name,'#'); + robot_label = splited_name{1}; + if ~strcmp(robot_label, base_robot_name) + error('Expected %s', base_robot_name) + end + if length(splited_name) > 1 + robot_index = splited_name{2}; + else + robot_index = ''; + end + + % Initialize joint names, link names, and base frame name + obj.joint_names = {}; + for i=1:robot_dof + current_joint_name = {robot_label,'_joint',int2str(i),robot_index}; + obj.joint_names{i} = strjoin(current_joint_name,''); + end + obj.base_frame_name = obj.joint_names{1}; + end + + function joint_names = get_joint_names(obj) + % This method gets the joint names of the robot in the CoppeliaSim scene. + % Usage: + % get_joint_names; + % + % Example: + % vi = DQ_VrepInterface(); + % vi.connect('127.0.0.1',19997); + % vrep_robot = DQ_SerialVrepRobot("my_robot", 7, "my_robot#1", vi); + % joint_names = vrep_robot.get_joint_names; + + joint_names = obj.joint_names; + end + + function set_configuration(obj, q) + % This method sets the joint configurations of the robot in the CoppeliaSim scene. + % Usage: + % set_configuration(q); + % q: The joint configurations of the robot in the CoppeliaSim scene. + % + % Example: + % vi = DQ_VrepInterface(); + % vi.connect('127.0.0.1',19997); + % vrep_robot = DQ_SerialVrepRobot("my_robot", 7, "my_robot#1", vi); + % q = zeros(7,1); + % vrep_robot.set_configuration(q); + % + % Note that this calls "set_joint_positions" in DQ_VrepInterface, meaning that it + % is only suitable for joints in kinematic mode. + + obj.vrep_interface.set_joint_positions(obj.joint_names, q) + end + + function q = get_configuration(obj) + % This method gets the joint configurations of the robot in the CoppeliaSim scene. + % Usage: + % get_configuration; + % + % Example: + % vi = DQ_VrepInterface(); + % vi.connect('127.0.0.1',19997); + % vrep_robot = DQ_SerialVrepRobot("my_robot", 7, "my_robot#1", vi); + % q = vrep_robot.get_configuration; + + q = obj.vrep_interface.get_joint_positions(obj.joint_names); + end + + function set_target_configuration(obj, q_target) + % This method sets the joint configurations of the robot in the CoppeliaSim scene as a target configuration for the joint controllers. + % Usage: + % set_target_configuration(q_target); + % q_target: The target joint configurations of the robot in the CoppeliaSim scene. + % + % Example: + % vi = DQ_VrepInterface(); + % vi.connect('127.0.0.1',19997); + % vrep_robot = DQ_SerialVrepRobot("my_robot", 7, "my_robot#1", vi); + % q_target = zeros(7,1); + % vrep_robot.set_target_configuration(q_target); + % + % Note that this calls "set_joint_target_positions" in DQ_VrepInterface, meaning that it + % is only suitable for joints in dynamic mode with position control. + + obj.vrep_interface.set_joint_target_positions(obj.joint_names, q_target) + end + + function qd = get_configuration_velocities(obj) + % This method gets the joint velocities of the robot in the CoppeliaSim scene. + % Usage: + % get_configuration_velocities; + % + % Example: + % vi = DQ_VrepInterface(); + % vi.connect('127.0.0.1',19997); + % vrep_robot = DQ_SerialVrepRobot("my_robot", 7, "my_robot#1", vi); + % qd = vrep_robot.get_configuration_velocities; + + qd = obj.vrep_interface.get_joint_velocities(obj.joint_names); + end + + function set_target_configuration_velocities(obj, v_target) + % This method sets the joint velocities of the robot in the CoppeliaSim scene as a target velocity for the joint controllers. + % Usage: + % set_target_configuration_velocities(v_target); + % v_target: The target joint velocities of the robot in the CoppeliaSim scene. + % + % Example: + % vi = DQ_VrepInterface(); + % vi.connect('127.0.0.1',19997); + % vrep_robot = DQ_SerialVrepRobot("my_robot", 7, "my_robot#1", vi); + % v_target = zeros(7,1); + % vrep_robot.set_target_configuration_velocities(v_target); + % + % Note that this calls "set_joint_target_velocities" in DQ_VrepInterface, meaning that it + % is only suitable for joints in dynamic mode with velocity control. + + obj.vrep_interface.set_joint_target_velocities(obj.joint_names, v_target); + end + + function set_configuration_space_torques(obj,tau) + % This method sets the joint torques of the robot in the CoppeliaSim scene. + % Usage: + % set_configuration_space_torques(tau); + % tau: The joint torques of the robot in the CoppeliaSim scene. + % + % Example: + % vi = DQ_VrepInterface(); + % vi.connect('127.0.0.1',19997); + % vrep_robot = DQ_SerialVrepRobot("my_robot", 7, "my_robot#1", vi); + % tau = zeros(7,1); + % vrep_robot.set_configuration_space_torques(tau); + % + % Note that this calls "set_joint_torques" in DQ_VrepInterface, meaning that it + % is only suitable for joints in dynamic mode with force/torque control. + + obj.vrep_interface.set_joint_torques(obj.joint_names,tau) + end + + function tau = get_configuration_space_torques(obj) + % This method gets the joint torques of the robot in the CoppeliaSim scene. + % Usage: + % get_configuration_space_torques; + % + % Example: + % vi = DQ_VrepInterface(); + % vi.connect('127.0.0.1',19997); + % vrep_robot = DQ_SerialVrepRobot("my_robot", 7, "my_robot#1", vi); + % tau = vrep_robot.get_configuration_space_torques; + + tau = obj.vrep_interface.get_joint_torques(obj.joint_names); + end + end +end \ No newline at end of file diff --git a/interfaces/coppeliasim/robots/LBR4pCoppeliaSimRobot.m b/interfaces/coppeliasim/robots/LBR4pCoppeliaSimRobot.m new file mode 100644 index 00000000..376b21e0 --- /dev/null +++ b/interfaces/coppeliasim/robots/LBR4pCoppeliaSimRobot.m @@ -0,0 +1,80 @@ +% CLASS LBR4pCoppeliaSimRobot - Concrete class to interface with the "KUKA LBR4+" +% robot in VREP. +% +% Usage: +% 1) Drag-and-drop a "KUKA LBR4+" robot to a VREP scene. +% 2) Run +% >> vi = DQ_VrepInterface(); +% >> vi.connect('127.0.0.1',19997); +% >> vrep_robot = LBR4pVrepRobot("LBR4p", vi); +% >> vi.start_simulation(); +% >> robot.get_configuration(); +% >> pause(1); +% >> vi.stop_simulation(); +% >> vi.disconnect(); +% Note that the name of the robot should be EXACTLY the same as in +% VREP. For instance, if you drag-and-drop a second robot, its name +% will become "LBR4p#0", a third robot, "LBR4p#1", and so on. +% +% LBR4pVrepRobot Methods: +% kinematics - Obtains the DQ_Kinematics implementation of this robot + +% (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.sourceforge.net +% +% Contributors to this file: +% 1. Murilo Marques Marinho - murilo@nml.t.u-tokyo.ac.jp +% - Responsible for the original implementation +% 2. Frederico Fernandes Afonso Silva (frederico.silva@ieee.org) +% - Updated for compatibility with the DQ_SerialVrepRobot class. +% 3. Juan Jose Quiroz Omana (juanjose.quirozomana@manchester.ac.uk) +% - Renamed the class from LBR4pVrepRobot to LBR4pCoppeliaSimRobot + + +classdef LBR4pCoppeliaSimRobot < DQ_SerialCoppeliaSimRobot + methods + function obj = LBR4pCoppeliaSimRobot(robot_name, vrep_interface) + obj@DQ_SerialCoppeliaSimRobot("LBR4p", 7, robot_name, vrep_interface); + end + + function kin = kinematics(obj) + %% Obtains the DQ_SerialManipulator instance that represents this LBR4p robot. + % >> vrep_robot = LBR4pVrepRobot("LBR4p", vi); + % >> robot_kinematics = vrep_robot.kinematics(); + + LBR4p_DH_theta=[0, 0, 0, 0, 0, 0, 0]; + LBR4p_DH_d = [0.200, 0, 0.4, 0, 0.39, 0, 0]; + LBR4p_DH_a = [0, 0, 0, 0, 0, 0, 0]; + LBR4p_DH_alpha = [pi/2, -pi/2, pi/2, -pi/2, pi/2, -pi/2, 0]; + LBR4p_DH_type = double(repmat(DQ_JointType.REVOLUTE,1,7)); + LBR4p_DH_matrix = [LBR4p_DH_theta; + LBR4p_DH_d; + LBR4p_DH_a; + LBR4p_DH_alpha + LBR4p_DH_type]; + + kin = DQ_SerialManipulatorDH(LBR4p_DH_matrix); + + kin.set_reference_frame(obj.vrep_interface.get_object_pose(obj.base_frame_name)); + kin.set_base_frame(obj.vrep_interface.get_object_pose(obj.base_frame_name)); + kin.set_effector(1+0.5*DQ.E*DQ.k*0.07); + end + end +end + diff --git a/interfaces/coppeliasim/robots/YouBotCoppeliaSimRobot.m b/interfaces/coppeliasim/robots/YouBotCoppeliaSimRobot.m new file mode 100644 index 00000000..0bf2d5e8 --- /dev/null +++ b/interfaces/coppeliasim/robots/YouBotCoppeliaSimRobot.m @@ -0,0 +1,152 @@ +% CLASS YouBotVrepRobot - Concrete class to interface with the "KUKA YouBot" +% robot in VREP. +% +% Usage: +% 1) Drag-and-drop a "KUKA YouBot" robot to a VREP scene. +% 2) Run +% >> vi = DQ_VrepInterface(); +% >> vi.connect('127.0.0.1',19997); +% >> vrep_robot = YouBotVrepRobot("youBot", vi); +% >> vi.start_simulation(); +% >> robot.get_configuration(); +% >> pause(1); +% >> vi.stop_simulation(); +% >> vi.disconnect(); +% Note that the name of the robot should be EXACTLY the same as in +% VREP. For instance, if you drag-and-drop a second robot, its name +% will become "youBot#0", a third robot, "youBot#1", and so on. +% +% YouBotVrepRobot Methods: +% set_configuration - Sends the joint configurations to VREP +% get_configuration - Obtains the joint configurations from VREP +% kinematics - Obtains the DQ_Kinematics implementation of this robot + +% (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.sourceforge.net +% +% Contributors to this file: +% 1. Murilo Marques Marinho - murilo@nml.t.u-tokyo.ac.jp +% - Responsible for the original implementation +% 2. Frederico Fernandes Afonso Silva (frederico.silva@ieee.org) +% - Updated for compatibility with the DQ_SerialVrepRobot class. +% 3. Juan Jose Quiroz Omana (juanjose.quirozomana@manchester.ac.uk) +% - Renamed the class from YouBotVrepRobot to YouBotCoppeliaSimRobot + + +classdef YouBotCoppeliaSimRobot < DQ_SerialCoppeliaSimRobot + properties (Constant) + adjust = ((cos(pi/2) + DQ.i*sin(pi/2)) * (cos(pi/4) + DQ.j*sin(pi/4)))*(1+0.5*DQ.E*-0.1*DQ.k); + end + + methods + function obj = YouBotCoppeliaSimRobot(robot_name, vrep_interface) + obj@DQ_SerialCoppeliaSimRobot("youBot", 5, robot_name, vrep_interface); + + %% youBot does not follow the standard naming convention in CoppeliaSim. Also, the use of 'set_names()', as is done in the C++ implementation, is not supported on a constructor in MATLAB + % From the second copy of the robot and onward, VREP appends a + % #number in the robot's name. We check here if the robot is + % called by the correct name and assign an index that will be + % used to correctly infer the robot's joint labels. + splited_name = strsplit(robot_name,'#'); + robot_label = splited_name{1}; + if ~strcmp(robot_label,'youBot') + error('Expected youBot') + end + if length(splited_name) > 1 + robot_index = splited_name{2}; + else + robot_index = ''; + end + + % Initialize joint names and base frame + obj.joint_names = {}; + for i=1:5 + current_joint_name = {robot_label,'ArmJoint',int2str(i-1),robot_index}; + obj.joint_names{i} = strjoin(current_joint_name,''); + end + obj.base_frame_name = robot_name; + end + + function set_configuration(obj,q) + %% Sends the joint configurations to VREP + % >> vrep_robot = YouBotVrepRobot("youBot", vi) + % >> q = zeros(8,1); + % >> vrep_robot.set_configuration(q) + x = q(1); + y = q(2); + phi = q(3); + + r = cos(phi/2.0)+DQ.k*sin(phi/2.0); + p = x * DQ.i + y * DQ.j; + pose = (1 + DQ.E*0.5*p)*r; + + obj.vrep_interface.set_joint_positions(obj.joint_names,q(4:8)); + obj.vrep_interface.set_object_pose(obj.base_frame_name, pose * obj.adjust'); + end + + function q = get_configuration(obj) + %% Obtains the joint configurations from VREP + % >> vrep_robot = YouBotVrepRobot("youBot", vi) + % >> q = vrep_robot.get_configuration(q) + base_x = obj.vrep_interface.get_object_pose(obj.base_frame_name) * obj.adjust; + base_t = vec3(translation(base_x)); + base_phi = rotation_angle(rotation(base_x)); + base_arm_q = obj.vrep_interface.get_joint_positions(obj.joint_names); + + q = [base_t(1); base_t(2); base_phi; base_arm_q]; + end + + function kin = kinematics(obj) + %% Obtains the DQ_WholeBody instance that represents this youBot robot. + % >> vrep_robot = YouBotVrepRobot("youBot", vi) + % >> robot_kinematics = vrep_robot.kinematics() + + include_namespace_dq + % The DH parameters and other geometric parameters are based on + % Kuka's documentation: + % http://www.youbot-store.com/wiki/index.php/YouBot_Detailed_Specifications + % https://www.generationrobots.com/img/Kuka-YouBot-Technical-Specs.pdf + pi2 = pi/2; + arm_DH_theta = [ 0, pi2, 0, pi2, 0]; + arm_DH_d = [ 0.147, 0, 0, 0, 0.218]; + arm_DH_a = [ 0.033, 0.155, 0.135, 0, 0]; + arm_DH_alpha = [pi2, 0, 0, pi2, 0]; + arm_DH_type = double(repmat(DQ_JointType.REVOLUTE,1,5)); + arm_DH_matrix = [arm_DH_theta; + arm_DH_d; + arm_DH_a; + arm_DH_alpha + arm_DH_type]; + + arm = DQ_SerialManipulatorDH(arm_DH_matrix); + base = DQ_HolonomicBase(); + + x_bm = 1 + E_*0.5*(0.165*i_ + 0.11*k_); + + base.set_frame_displacement(x_bm); + + kin = DQ_WholeBody(base); + kin.add(arm); + + effector = 1 + E_*0.5*0.3*k_; + kin.set_effector(effector); + end + end +end + diff --git a/interfaces/vrep/DQ_SerialVrepRobot.m b/interfaces/vrep/DQ_SerialVrepRobot.m index df363a09..a9dd9c71 100644 --- a/interfaces/vrep/DQ_SerialVrepRobot.m +++ b/interfaces/vrep/DQ_SerialVrepRobot.m @@ -52,189 +52,11 @@ % - DQ_VrepInterface.h: https://github.com/dqrobotics/cpp-interface-vrep/blob/master/include/dqrobotics/interfaces/vrep/DQ_VrepInterface.h % - DQ_SerialVrepRobot.cpp: https://github.com/dqrobotics/cpp-interface-vrep/blob/master/src/dqrobotics/interfaces/vrep/DQ_SerialVrepRobot.cpp -classdef DQ_SerialVrepRobot < DQ_VrepRobot - properties - joint_names; - base_frame_name; - end - +classdef DQ_SerialVrepRobot < DQ_SerialCoppeliaSimRobot methods function obj = DQ_SerialVrepRobot(base_robot_name, robot_dof, robot_name, vrep_interface) - % This method constructs an instance of a DQ_SerialVrepRobot. - % Usage: - % DQ_SerialVrepRobot(base_robot_name, robot_dof, robot_name, vrep_interface); - % base_robot_name: The base name of the robot in the CoppeliaSim scene. - % robot_dof: The number of DoF of the robot in the CoppeliaSim scene. - % robot_name: The name of the robot in the CoppeliaSim scene. - % vrep_interface: The DQ_VrepInterface object connected to the CoppeliaSim scene. - % - % Example: - % vi = DQ_VrepInterface(); - % vi.connect('127.0.0.1',19997); - % vrep_robot = DQ_SerialVrepRobot("my_robot", 7, "my_robot#1", vi); - % - % Note that the name of the robot should be EXACTLY the same as in the CoppeliaSim - % scene. For instance, if you drag-and-drop a second robot, its name will become - % "my_robot#0", a third robot, "my_robot#1", and so on. - - obj.robot_name = robot_name; - obj.vrep_interface = vrep_interface; - - %% The use of 'initialize_joint_names_from_vrep()', as is done in the C++ implementation, is not supported on a constructor in MATLAB - % From the second copy of the robot and onward, VREP appends a - % #number in the robot's name. We check here if the robot is - % called by the correct name and assign an index that will be - % used to correctly infer the robot's joint labels. - splited_name = strsplit(obj.robot_name,'#'); - robot_label = splited_name{1}; - if ~strcmp(robot_label, base_robot_name) - error('Expected %s', base_robot_name) - end - if length(splited_name) > 1 - robot_index = splited_name{2}; - else - robot_index = ''; - end - - % Initialize joint names, link names, and base frame name - obj.joint_names = {}; - for i=1:robot_dof - current_joint_name = {robot_label,'_joint',int2str(i),robot_index}; - obj.joint_names{i} = strjoin(current_joint_name,''); - end - obj.base_frame_name = obj.joint_names{1}; - end - - function joint_names = get_joint_names(obj) - % This method gets the joint names of the robot in the CoppeliaSim scene. - % Usage: - % get_joint_names; - % - % Example: - % vi = DQ_VrepInterface(); - % vi.connect('127.0.0.1',19997); - % vrep_robot = DQ_SerialVrepRobot("my_robot", 7, "my_robot#1", vi); - % joint_names = vrep_robot.get_joint_names; - - joint_names = obj.joint_names; - end - - function set_configuration(obj, q) - % This method sets the joint configurations of the robot in the CoppeliaSim scene. - % Usage: - % set_configuration(q); - % q: The joint configurations of the robot in the CoppeliaSim scene. - % - % Example: - % vi = DQ_VrepInterface(); - % vi.connect('127.0.0.1',19997); - % vrep_robot = DQ_SerialVrepRobot("my_robot", 7, "my_robot#1", vi); - % q = zeros(7,1); - % vrep_robot.set_configuration(q); - % - % Note that this calls "set_joint_positions" in DQ_VrepInterface, meaning that it - % is only suitable for joints in kinematic mode. - - obj.vrep_interface.set_joint_positions(obj.joint_names, q) - end - - function q = get_configuration(obj) - % This method gets the joint configurations of the robot in the CoppeliaSim scene. - % Usage: - % get_configuration; - % - % Example: - % vi = DQ_VrepInterface(); - % vi.connect('127.0.0.1',19997); - % vrep_robot = DQ_SerialVrepRobot("my_robot", 7, "my_robot#1", vi); - % q = vrep_robot.get_configuration; - - q = obj.vrep_interface.get_joint_positions(obj.joint_names); - end - - function set_target_configuration(obj, q_target) - % This method sets the joint configurations of the robot in the CoppeliaSim scene as a target configuration for the joint controllers. - % Usage: - % set_target_configuration(q_target); - % q_target: The target joint configurations of the robot in the CoppeliaSim scene. - % - % Example: - % vi = DQ_VrepInterface(); - % vi.connect('127.0.0.1',19997); - % vrep_robot = DQ_SerialVrepRobot("my_robot", 7, "my_robot#1", vi); - % q_target = zeros(7,1); - % vrep_robot.set_target_configuration(q_target); - % - % Note that this calls "set_joint_target_positions" in DQ_VrepInterface, meaning that it - % is only suitable for joints in dynamic mode with position control. - - obj.vrep_interface.set_joint_target_positions(obj.joint_names, q_target) - end - - function qd = get_configuration_velocities(obj) - % This method gets the joint velocities of the robot in the CoppeliaSim scene. - % Usage: - % get_configuration_velocities; - % - % Example: - % vi = DQ_VrepInterface(); - % vi.connect('127.0.0.1',19997); - % vrep_robot = DQ_SerialVrepRobot("my_robot", 7, "my_robot#1", vi); - % qd = vrep_robot.get_configuration_velocities; - - qd = obj.vrep_interface.get_joint_velocities(obj.joint_names); - end - - function set_target_configuration_velocities(obj, v_target) - % This method sets the joint velocities of the robot in the CoppeliaSim scene as a target velocity for the joint controllers. - % Usage: - % set_target_configuration_velocities(v_target); - % v_target: The target joint velocities of the robot in the CoppeliaSim scene. - % - % Example: - % vi = DQ_VrepInterface(); - % vi.connect('127.0.0.1',19997); - % vrep_robot = DQ_SerialVrepRobot("my_robot", 7, "my_robot#1", vi); - % v_target = zeros(7,1); - % vrep_robot.set_target_configuration_velocities(v_target); - % - % Note that this calls "set_joint_target_velocities" in DQ_VrepInterface, meaning that it - % is only suitable for joints in dynamic mode with velocity control. - - obj.vrep_interface.set_joint_target_velocities(obj.joint_names, v_target); - end - - function set_configuration_space_torques(obj,tau) - % This method sets the joint torques of the robot in the CoppeliaSim scene. - % Usage: - % set_configuration_space_torques(tau); - % tau: The joint torques of the robot in the CoppeliaSim scene. - % - % Example: - % vi = DQ_VrepInterface(); - % vi.connect('127.0.0.1',19997); - % vrep_robot = DQ_SerialVrepRobot("my_robot", 7, "my_robot#1", vi); - % tau = zeros(7,1); - % vrep_robot.set_configuration_space_torques(tau); - % - % Note that this calls "set_joint_torques" in DQ_VrepInterface, meaning that it - % is only suitable for joints in dynamic mode with force/torque control. - - obj.vrep_interface.set_joint_torques(obj.joint_names,tau) - end - - function tau = get_configuration_space_torques(obj) - % This method gets the joint torques of the robot in the CoppeliaSim scene. - % Usage: - % get_configuration_space_torques; - % - % Example: - % vi = DQ_VrepInterface(); - % vi.connect('127.0.0.1',19997); - % vrep_robot = DQ_SerialVrepRobot("my_robot", 7, "my_robot#1", vi); - % tau = vrep_robot.get_configuration_space_torques; - - tau = obj.vrep_interface.get_joint_torques(obj.joint_names); + warning('Deprecated. Use DQ_SerialCoppeliaSimRobot instead.') + obj@DQ_SerialCoppeliaSimRobot(base_robot_name, robot_dof, robot_name, vrep_interface) end end end \ No newline at end of file diff --git a/interfaces/vrep/DQ_VrepInterface.m b/interfaces/vrep/DQ_VrepInterface.m index a869a347..88dc94d0 100644 --- a/interfaces/vrep/DQ_VrepInterface.m +++ b/interfaces/vrep/DQ_VrepInterface.m @@ -96,6 +96,8 @@ % - set_joint_target_velocities() % - get_joint_velocities() % - Improved the documentation of the class +% - Renamed the class from DQ_VrepInterface to DQ_CoppeliaSimLegacyInterface +% - The class now inherits from DQ_CoppeliaSimLegacyInterface % % 3. Frederico Fernandes Afonso Silva (frederico.silva@ieee.org) % - Added the following methods: @@ -108,1040 +110,13 @@ % - clientID -classdef DQ_VrepInterface < handle - - properties (Access = private) - % a map between V-REP object names and DQ_VrepInterfaceMapElements - handles_map; - end - - properties (Access = protected) - % the V-REP remote API instance used by this interface - vrep; - % the client ID of this remote API connection - clientID; - end - - properties (Constant) - % Constant that denotes the V-VREP's remote API blocking operation mode - OP_BLOCKING = remApi('remoteApi').simx_opmode_blocking; - % Constant that denotes the V-VREP's remote API streaming operation mode - OP_STREAMING = remApi('remoteApi').simx_opmode_streaming; - % Constant that denotes the V-VREP's remote API oneshot operation mode - OP_ONESHOT = remApi('remoteApi').simx_opmode_oneshot; - % Constant that denotes the V-VREP's remote API buffer operation mode - OP_BUFFER = remApi('remoteApi').simx_opmode_buffer; - % Constant that denotes the V-VREP's remote API joint velocity ID - JOINT_VELOCITY_PARAMETER_ID = remApi('remoteApi').sim_jointfloatparam_velocity; - end - - methods (Access = private) - - function handle = handle_from_string_or_handle(obj,name_or_handle) - if(ischar(name_or_handle)) - name = name_or_handle; - if(obj.handles_map.isKey(name)) - handle = obj.handles_map(name).handle; - else - handle = obj.get_handle(name); - obj.handles_map(name) = DQ_VrepInterfaceMapElement(handle); - end - - else - handle=name_or_handle; - end - end - - function element = element_from_string(obj,name) - obj.handle_from_string_or_handle(name); %Add new handle if needed - element = obj.handles_map(name); - end - end - +classdef DQ_VrepInterface < DQ_CoppeliaSimLegacyInterface methods - function obj = DQ_VrepInterface() - obj.vrep=remApi('remoteApi'); - obj.handles_map = containers.Map; - obj.clientID = -1; - disp(['This version of DQ Robotics DQ_VrepInterface is compatible'... - ' with VREP 3.5.0']); - end - - function connect(obj,ip,port) - % This method connects to the remote api server (i.e. CoppeliaSim). - % Calling this function is required before anything else can happen. - % Usage: - % connect(ip, port); - % ip: The ip address where the CoppeliaSim is located. - % port: The port number where to connect. - % - % Example: - % connect('127.0.0.1', 19997); - - obj.clientID = obj.vrep.simxStart(ip,port,true,true,5000,5); - if (obj.clientID>-1) - disp('Connected to the remote API server'); - else - error('Failed to connect to remote API server'); - end - end - - function disconnect(obj) - % This method ends the communication between the client and - % the CoppeliaSim scene. This should be the very last method called. - obj.vrep.simxFinish(obj.clientID); - end - - function disconnect_all(obj) - % This method ends all running communication threads with the - % CoppeliaSim scene. This should be the very last method called. - obj.vrep.simxFinish(-1); - end - - function set_synchronous(obj,flag) - % This method enables or disables the stepped (synchronous) mode - % for the remote API server service that the client is connected to. - % Example: - % set_synchronous(true) % synchronous mode enabled - % set_synchronous(false) % synchronous mode disabled - if (~islogical(flag)) - error('Error in set_synchronous: argument must be logical, not a %s. \nExample:\n set_synchronous(true)', class(flag)); - end - obj.vrep.simxSynchronous(obj.clientID,flag); - end - - function trigger_next_simulation_step(obj) - % This method sends a synchronization trigger signal to the CoppeliaSim scene, - % which performs a simulation step when the synchronous mode is used. - obj.vrep.simxSynchronousTrigger(obj.clientID); - end - - function ping_time = wait_for_simulation_step_to_end(obj) - % This method returns the time needed for a command to be sent - % to the CoppeliaSim scene, executed, and sent back. - [~, ping_time] = obj.vrep.simxGetPingTime(obj.clientID); - end - - function start_simulation(obj) - % This method starts the CoppeliaSim simulation. - obj.vrep.simxStartSimulation(obj.clientID,obj.vrep.simx_opmode_oneshot); - end - - function stop_simulation(obj) - % This method stops the CoppeliaSim simulation. - obj.vrep.simxStopSimulation(obj.clientID,obj.vrep.simx_opmode_blocking); - end - - function handles = get_handles(obj,names) - % This method gets the handles for a cell array of - % object names in the the CoppeliaSim scene. - % Usage: - % get_handles(names); - % names: The cell array of object names. - % - % Example: - % handle = get_handles({'ReferenceFrame_1', 'ReferenceFrame_2'}); - - handles = []; - if(iscell(names)) - for i=1:length(names) - [~,handles(i)] = obj.vrep.simxGetObjectHandle(... - obj.clientID,... - char(names(i)),... - obj.vrep.simx_opmode_blocking); - end - else - error('Error in get_handles: argument names must be of type cell, e.g. names = [{joint1,joint2}];'); - end - end - - function handle = get_handle(obj,name) - % This method gets the handle for a given object in the CoppeliaSim scene. - % - % Usage: - % get_handles(name); - % names: The object name. - % Example: - % handle = get_handle('ReferenceFrame'); - - [~,handle] = obj.vrep.simxGetObjectHandle(... - obj.clientID,... - name,... - obj.vrep.simx_opmode_blocking); - end - - function t = get_object_translation(obj,objectname,reference_frame,opmode) - % This method gets the translation of an object in the CoppeliaSim scene. - % - % Usage: - % Recommended: - % t = get_object_translation(objectname) - % - % Advanced: - % t = get_object_translation(objectname,reference_frame,opmode); - % - % objectname: The object name - % (optional) reference_frame: Indicates the name of - % the relative reference frame in which you - % want the translation. If not specified, - % the absolute frame is used. - % (optional) opmode: The operation mode. If not specified, - % the opmode will be set automatically. - % - % You can use the following modes: - % OP_BLOCKING - % OP_STREAMING - % OP_ONESHOT - % OP_BUFFER; - % - % Check this link for more details: - % https://www.coppeliarobotics.com/helpFiles/en/remoteApiModusOperandi.htm - % - % Example: - % % Recommended: - % t = get_object_translation('DefaultCamera'); - % - % % For advanced usage: - % t = get_object_translation('DefaultCamera', 'Frame_b', OP_ONESHOT); - - % First approach to the auto-management using - % DQ_VrepInterfaceMapElements. If the user does not specify the - % opmode, it is chosen first as STREAMING and then as BUFFER, - % as specified by the remote API documentation - - handle = objectname; % alias - - if nargin <= 2 - element = obj.element_from_string(handle); - if(~element.state_from_function_signature('get_object_translation')) - [~,object_position] = obj.vrep.simxGetObjectPosition(... - obj.clientID,... - obj.handle_from_string_or_handle(handle),... - -1,... - obj.OP_STREAMING); - % We need to check the buffer until it is not empty, - % TODO add a timeout. - retval = 1; - while retval==1 - [retval,object_position] = obj.vrep.simxGetObjectPosition(... - obj.clientID,... - obj.handle_from_string_or_handle(handle),... - -1,... - obj.OP_BUFFER); - end - else - [~,object_position] = obj.vrep.simxGetObjectPosition(... - obj.clientID,... - obj.handle_from_string_or_handle(handle),... - -1,... - obj.OP_BUFFER); - end - else - relative_to_handle = reference_frame; % alias - [~,object_position] = obj.vrep.simxGetObjectPosition(... - obj.clientID,... - obj.handle_from_string_or_handle(handle),... - obj.handle_from_string_or_handle(relative_to_handle),... - opmode); - end - t = DQ([0,double(object_position)]); - end - - function set_object_translation(obj,objectname,translation,reference_frame,opmode) - % This method sets the translation of an object in the CoppeliaSim scene. - % Usage: - % Recommended: - % set_object_translation(objectname,translation); - % - % Advanced: - % set_object_translation(objectname,translation,reference_frame,opmode); - % - % objectname: The object name. - % translation: The desired translation. - % (optional) reference_frame: Indicates the name of - % the relative reference frame in which - % the desired translation is expressed. - % If not specified, the absolute frame is used. - % (optional) opmode: The operation mode. If not specified, - % the opmode will be set automatically. - % - % You can use the following modes: - % OP_BLOCKING - % OP_STREAMING - % OP_ONESHOT - % OP_BUFFER; - % - % Check this link for more details: - % https://www.coppeliarobotics.com/helpFiles/en/remoteApiModusOperandi.htm - % - % Example: - % % Recommended: - % set_object_translation('DefaultCamera', t); - % - % % For advanced usage: - % set_object_translation('DefaultCamera', t, 'Frame_b', OP_ONESHOT); - - % create some aliases - handle = objectname; - t = translation; - - if nargin == 3 - obj.vrep.simxSetObjectPosition(obj.clientID,... - obj.handle_from_string_or_handle(handle),... - -1,... - t.q(2:4),... - obj.OP_ONESHOT); - else - relative_to_handle = reference_frame; % alias - obj.vrep.simxSetObjectPosition(obj.clientID,... - obj.handle_from_string_or_handle(handle),... - obj.handle_from_string_or_handle(relative_to_handle),... - t.q(2:4),... - opmode); - end - end - - function r = get_object_rotation(obj, objectname, reference_frame, opmode) - % This method gets the rotation of an object in the CoppeliaSim scene. - % - % Usage: - % Recommended: - % t = get_object_rotation(objectname); - % - % Advanced: - % t = get_object_rotation(objectname,reference_frame,opmode); - % - % objectname: The object name - % (optional) reference_frame: Indicates the name of - % the relative reference frame in which you - % want the rotation. If not specified, - % the absolute frame is used. - % (optional) opmode: The operation mode. If not specified, - % the opmode will be set automatically. - % - % You can use the following modes: - % OP_BLOCKING - % OP_STREAMING - % OP_ONESHOT - % OP_BUFFER; - % - % Check this link for more details: - % https://www.coppeliarobotics.com/helpFiles/en/remoteApiModusOperandi.htm - % - % Example: - % % Recommended: - % r = get_object_rotation('DefaultCamera'); - % - % % For advanced usage: - % r = get_object_rotation('DefaultCamera', 'Frame_b', OP_ONESHOT); - - - % create some aliases - handle = objectname; - id = obj.clientID; - handle1 = obj.handle_from_string_or_handle(handle); - - - % First approach to the auto-management using - % DQ_VrepInterfaceMapElements. If the user does not specify the - % opmode, it is chosen first as STREAMING and then as BUFFER, - % as specified by the remote API documentation - if nargin <= 2 - element = obj.element_from_string(handle); - if(~element.state_from_function_signature('get_object_rotation')) - [~,obj_rot] = obj.vrep.simxGetObjectQuaternion(id, ... - handle1,... - -1,... - obj.OP_STREAMING); - % We need to check the buffer until it is not empty, - % TODO add a timeout. - retval = 1; - while retval==1 - [retval,obj_rot] = obj.vrep.simxGetObjectQuaternion(id,... - handle1,... - -1,... - obj.OP_BUFFER); - end - else - [~,obj_rot] = obj.vrep.simxGetObjectQuaternion(id,handle1,... - -1,... - obj.OP_BUFFER); - end - else - relative_to_handle = reference_frame; % alias - handle2 = obj.handle_from_string_or_handle(relative_to_handle); - [~,obj_rot] = obj.vrep.simxGetObjectQuaternion(id,... - handle1,... - handle2,... - opmode); - end - object_rotation_double = double(obj_rot); - - %V-Rep's quaternion representation is [x y z w] so we have to - %take that into account - r = normalize(DQ([object_rotation_double(4),... - object_rotation_double(1),... - object_rotation_double(2),... - object_rotation_double(3)])); - end - - function set_object_rotation(obj,objectname,rotation,reference_frame,opmode) - % This method sets the rotation of an object in the CoppeliaSim scene. - % Usage: - % Recommended: - % set_object_rotation(objectname,rotation); - % - % Advanced: - % set_object_rotation(objectname,rotation,reference_frame,opmode); - % - % objectname: The object name. - % rotation: The desired rotation. - % (optional) reference_frame: Indicates the name of - % the relative reference frame in which - % the desired rotation is expressed. - % If not specified, the absolute frame is used. - % (optional) opmode: The operation mode. If not specified, - % the opmode will be set automatically. - % - % You can use the following modes: - % OP_BLOCKING - % OP_STREAMING - % OP_ONESHOT - % OP_BUFFER; - % - % Check this link for more details: - % https://www.coppeliarobotics.com/helpFiles/en/remoteApiModusOperandi.htm - % - % Example: - % % Recommended: - % set_object_rotation('DefaultCamera', r); - % - % % For advanced usage: - % set_object_rotation('DefaultCamera', r, 'Frame_b', OP_ONESHOT); - - % create some aliases - handle = objectname; - r = rotation; - - if nargin == 3 - obj.vrep.simxSetObjectQuaternion(... - obj.clientID,... - obj.handle_from_string_or_handle(handle),... - -1,... - [r.q(2:4); r.q(1)],... - obj.OP_ONESHOT); %V-Rep's quaternion representation is [x y z w] so we have to take that into account - else - relative_to_handle = reference_frame; % alias - obj.vrep.simxSetObjectQuaternion(... - obj.clientID,... - obj.handle_from_string_or_handle(handle),... - obj.handle_from_string_or_handle(relative_to_handle),... - [r.q(2:4); r.q(1)],... - opmode); %V-Rep's quaternion representation is [x y z w] so we have to take that into account - end - end - - function x = get_object_pose(obj,objectname,reference_frame,opmode) - % This method gets the pose of an object in the CoppeliaSim scene. - % - % Usage: - % Recommended: - % x = get_object_pose(objectname); - % - % Advanced: - % x = get_object_pose(objectname,reference_frame,opmode); - % - % objectname: The object name - % (optional) reference_frame: Indicates the name of - % the relative reference frame in which you - % want the pose. If not specified, - % the absolute frame is used. - % (optional) opmode: The operation mode. If not specified, - % the opmode will be set automatically. - % - % You can use the following modes: - % OP_BLOCKING - % OP_STREAMING - % OP_ONESHOT - % OP_BUFFER; - % - % Check this link for more details: - % https://www.coppeliarobotics.com/helpFiles/en/remoteApiModusOperandi.htm - % - % Example: - % % Recommended: - % x = get_object_pose('DefaultCamera'); - % - % % For advanced usage: - % x = get_object_pose('DefaultCamera', 'Frame_b', OP_ONESHOT); - - - handle = objectname; % alias - - - if nargin <= 2 - t = obj.get_object_translation(handle); - r = obj.get_object_rotation(handle); - else - relative_to_handle = reference_frame; % alias - t = obj.get_object_translation(... - obj.handle_from_string_or_handle(handle),... - obj.handle_from_string_or_handle(relative_to_handle),... - opmode); - r = obj.get_object_rotation(... - obj.handle_from_string_or_handle(handle),... - obj.handle_from_string_or_handle(relative_to_handle),... - opmode); - end - x = r + 0.5*DQ.E*t*r; - end - - function set_object_pose(obj,objectname,pose,reference_frame,opmode) - % This method sets the pose of an object in the CoppeliaSim scene. - % Usage: - % Recommended: - % set_object_pose(objectname,pose); - % - % Advanced: - % set_object_pose(objectname,pose,reference_frame,opmode); - % - % objectname: The object name. - % pose: The desired pose. - % (optional) reference_frame: Indicates the name of - % the relative reference frame in which - % the desired pose is expressed. - % If not specified, the absolute frame is used. - % (optional) opmode: The operation mode. If not specified, - % the opmode will be set automatically. - % - % You can use the following modes: - % OP_BLOCKING - % OP_STREAMING - % OP_ONESHOT - % OP_BUFFER; - % - % Check this link for more details: - % https://www.coppeliarobotics.com/helpFiles/en/remoteApiModusOperandi.htm - % - % Example: - % - % t = DQ.i*0.01; - % r = DQ.i; - % x = r+0.5*DQ.E*t*r; - % - % % Recommended: - % set_object_pose('DefaultCamera', x); - % - % % For advanced usage: - % set_object_pose('DefaultCamera', x, 'Frame_b', OP_ONESHOT); - - % create some aliases - handle = objectname; - x = pose; - - if nargin == 3 - t = translation(x); - r = rotation(x); - obj.set_object_translation(... - obj.handle_from_string_or_handle(handle),... - t,... - -1,... - obj.OP_ONESHOT); - obj.set_object_rotation(... - obj.handle_from_string_or_handle(handle),... - r,... - -1,... - obj.OP_ONESHOT); - else - relative_to_handle = reference_frame; - t = translation(x); - r = rotation(x); - obj.set_object_translation(... - obj.handle_from_string_or_handle(handle),... - t,... - obj.handle_from_string_or_handle(relative_to_handle),... - opmode); - obj.set_object_rotation(... - obj.handle_from_string_or_handle(handle),... - r,... - obj.handle_from_string_or_handle(relative_to_handle),... - opmode); - end - end - - function set_joint_positions(obj,jointnames,joint_positions,opmode) - % This method sets the joint positions of a robot in the CoppeliaSim scene. - % It is required a dynamics disabled scene. - % - % Usage: - % Recommended: - % set_joint_positions(jointnames, joint_positions); - % - % Advanced: - % set_joint_positions(jointnames, joint_positions, opmode); - % - % jointnames: The joint names. - % joint_positions: The joint positions. - % (optional) opmode: The operation mode. If not specified, - % the opmode will be set automatically. - % - % You can use the following modes: - % OP_BLOCKING - % OP_STREAMING - % OP_ONESHOT - % OP_BUFFER; - % - % Check this link for more details: - % https://www.coppeliarobotics.com/helpFiles/en/remoteApiModusOperandi.htm - % - % Example: - % jointnames={'LBR4p_joint1','LBR4p_joint2','LBR4p_joint3','LBR4p_joint4',... - % 'LBR4p_joint5','LBR4p_joint6','LBR4p_joint7'}; - % u = [0.1 0.1 0.1 0.1 0.1 0.1 0.1]; - % - % % Recommended: - % set_joint_positions(jointnames, u); - % - % % Advanced usage: - % set_joint_positions(jointnames, u, OP_ONESHOT); - - % create some aliases - handles = jointnames; - thetas = joint_positions; - - if nargin == 3 - % The recommended mode is OP_ONESHOT - opmode = obj.OP_ONESHOT; - end - - for joint_index=1:length(handles) - if isa(handles,'cell') - obj.vrep.simxSetJointPosition(... - obj.clientID,... - obj.handle_from_string_or_handle(handles{joint_index}),... - thetas(joint_index),... - opmode); - else - obj.vrep.simxSetJointPosition(... - obj.clientID,... - obj.handle_from_string_or_handle(handles),... - thetas(joint_index),... - opmode); - end - end - end - - function set_joint_target_positions(obj,jointnames,joint_target_positions,opmode) - % This method sets the joint target positions of a robot in the CoppeliaSim scene. - % It is required a dynamics enabled scene, and joints in dynamic mode - % with position control mode. - % - % Usage: - % Recommended: - % set_joint_target_positions(jointnames,joint_target_positions); - % - % Advanced: - % set_joint_target_positions(jointnames, joint_target_positions, opmode); - % - % jointnames: The joint names. - % joint_target_positions: The joint target positions. - % (optional) opmode: The operation mode. If not specified, - % the opmode will be set automatically. - % - % You can use the following modes: - % OP_BLOCKING - % OP_STREAMING - % OP_ONESHOT - % OP_BUFFER; - % - % Check this link for more details: - % https://www.coppeliarobotics.com/helpFiles/en/remoteApiModusOperandi.htm - % - % Example: - % jointnames={'LBR4p_joint1','LBR4p_joint2','LBR4p_joint3','LBR4p_joint4',... - % 'LBR4p_joint5','LBR4p_joint6','LBR4p_joint7'}; - % u = [0.1 0.1 0.1 0.1 0.1 0.1 0.1]; - % - % % Recommended: - % set_joint_target_positions(jointnames, u); - % - % % Advanced usage: - % set_joint_target_positions(jointnames, u, OP_ONESHOT); - - % create some aliases - handles = jointnames; - thetas = joint_target_positions; - - if nargin == 3 - % The recommended mode is OP_ONESHOT - opmode = obj.OP_ONESHOT; - end - - for joint_index=1:length(handles) - if isa(handles,'cell') - obj.vrep.simxSetJointTargetPosition(... - obj.clientID,... - obj.handle_from_string_or_handle(handles{joint_index}),... - thetas(joint_index),... - opmode); - else - obj.vrep.simxSetJointTargetPosition(... - obj.clientID,... - obj.handle_from_string_or_handle(handles),... - thetas(joint_index),... - opmode); - end - end - end - - function [joint_positions,retval]=get_joint_positions(obj,jointnames,opmode) - % This method gets the joint positions of a robot in the CoppeliaSim scene. - % Usage: - % Recommended: - % joint_positions = get_joint_positions(jointnames); - % [joint_positions, retval] = get_joint_positions(jointnames); - % - % Advanced: - % joint_positions] = get_joint_positions(jointnames,opmode); - % [joint_positions, retval] = get_joint_positions(jointnames, opmode); - % - % -Parameters: - % - % jointnames: The joint names. - % (optional) opmode: The operation mode. If not specified, - % the opmode will be set automatically. - % - % You can use the following modes: - % OP_BLOCKING - % OP_STREAMING - % OP_ONESHOT - % OP_BUFFER; - % - % Check this link for more details: - % https://www.coppeliarobotics.com/helpFiles/en/remoteApiModusOperandi.htm - % - % - Outputs: - % - % joint_positions: The joints positions - % retval: The return code of the Remote API function, - % which is defined in https://www.coppeliarobotics.com/helpFiles/en/remoteApiConstants.htm#functionErrorCodes - % - % - % Example: - % jointnames={'LBR4p_joint1','LBR4p_joint2','LBR4p_joint3','LBR4p_joint4',... - % 'LBR4p_joint5','LBR4p_joint6','LBR4p_joint7'}; - % - % % Recommended: - % joint_positions = get_joint_positions(jointnames); - % [joint_positions, rtn] = get_joint_positions(jointnames); - % - % % Advanced usage: - % joint_positions = get_joint_positions(jointnames, OP_ONESHOT); - % [joint_positions, rtn] = get_joint_positions(jointnames, OP_ONESHOT); - - - % create some aliases - handles = jointnames; - thetas = zeros(length(handles),1); - - for joint_index=1:length(handles) - % First approach to the auto-management using - % DQ_VrepInterfaceMapElements. If the user does not specify the - % opmode, it is chosen first as STREAMING and then as BUFFER, - % as specified by the remote API documentation - if nargin <= 2 - if isa(handles,'cell') - element = obj.element_from_string(handles{joint_index}); - else - element = obj.element_from_string(handles); - end - if(~element.state_from_function_signature('get_joint_positions')) - [~,tmp] = obj.vrep.simxGetJointPosition(... - obj.clientID,... - element.handle,... - obj.OP_STREAMING); - retval=1; - while retval==1 - [retval,tmp] = obj.vrep.simxGetJointPosition(... - obj.clientID,... - element.handle,... - obj.OP_BUFFER); - end - else - [retval,tmp] = obj.vrep.simxGetJointPosition(... - obj.clientID,... - element.handle,... - obj.OP_BUFFER); - end - else - [retval,tmp] = obj.vrep.simxGetJointPosition(... - obj.clientID,... - obj.handle_from_string_or_handle(handles{joint_index}),... - opmode); - end - thetas(joint_index) = double(tmp); - end - joint_positions = thetas; - end - - function joint_velocities = get_joint_velocities(obj,jointnames,opmode) - % This method gets the joint velocities of a robot in the CoppeliaSim scene. - % Usage: - % Recommended: - % joint_velocities = get_joint_velocities(jointnames); - % - % Advanced: - % joint_velocities = get_joint_velocities(jointnames, opmode) - % - % jointnames: The joint names. - % (optional) opmode: The operation mode. If not specified, - % the opmode will be set automatically. - % - % You can use the following modes: - % OP_BLOCKING - % OP_STREAMING - % OP_ONESHOT - % OP_BUFFER; - % - % Check this link for more details: - % https://www.coppeliarobotics.com/helpFiles/en/remoteApiModusOperandi.htm - % - % Example: - % jointnames={'LBR4p_joint1','LBR4p_joint2','LBR4p_joint3','LBR4p_joint4',... - % 'LBR4p_joint5','LBR4p_joint6','LBR4p_joint7'}; - % - % % Recommended: - % joint_velocities = get_joint_velocities(jointnames); - % - % % Advanced usage: - % joint_velocities = get_joint_velocities(jointnames, OP_ONESHOT); - - joint_velocities = zeros(length(jointnames),1); - for joint_index=1:length(jointnames) - % First approach to the auto-management using - % DQ_VrepInterfaceMapElements. If the user does not specify the - % opmode, it is chosen first as STREAMING and then as BUFFER, - % as specified by the remote API documentation - if nargin <= 2 - if isa(jointnames,'cell') - element = obj.element_from_string(jointnames{joint_index}); - else - element = obj.element_from_string(jointnames); - end - if(~element.state_from_function_signature('get_joint_velocities')) - [~,tmp] = obj.vrep.simxGetObjectFloatParameter(... - obj.clientID,... - element.handle,... - obj.JOINT_VELOCITY_PARAMETER_ID,... - obj.OP_STREAMING); - retval=1; - while retval==1 - [retval,tmp] = obj.vrep.simxGetObjectFloatParameter(... - obj.clientID,... - element.handle,... - obj.JOINT_VELOCITY_PARAMETER_ID,... - obj.OP_BUFFER); - end - else - [~,tmp] = obj.vrep.simxGetObjectFloatParameter(... - obj.clientID,... - element.handle,... - obj.JOINT_VELOCITY_PARAMETER_ID,... - obj.OP_BUFFER); - end - else - [~,tmp] = obj.vrep.simxGetObjectFloatParameter(... - obj.clientID,... - obj.handle_from_string_or_handle(jointnames{joint_index}),... - obj.JOINT_VELOCITY_PARAMETER_ID,... - opmode); - end - joint_velocities(joint_index) = double(tmp); - end - end - - function set_joint_target_velocities(obj,jointnames,joint_target_velocities,opmode) - % This method sets the joint velocities of a robot in the CoppeliaSim scene. - % It is required a dynamics enabled scene, and joints in dynamic mode - % with velocity control mode. Check this link for more - % information about joint modes: - % https://www.coppeliarobotics.com/helpFiles/en/jointModes.htm - % - % Usage: - % Recommended: - % set_joint_target_velocities(jointnames, joint_target_velocities); - % - % Advanced: - % set_joint_target_velocities(jointnames, joint_target_velocities, opmode); - % - % jointnames: The joint names. - % joint_target_velocities: The joint target velocities. - % (optional) opmode: The operation mode. If not specified, - % the opmode will be set automatically. - % - % You can use the following modes: - % OP_BLOCKING - % OP_STREAMING - % OP_ONESHOT - % OP_BUFFER; - % - % Check this link for more details: - % https://www.coppeliarobotics.com/helpFiles/en/remoteApiModusOperandi.htm - % - % Example: - % jointnames={'LBR4p_joint1','LBR4p_joint2','LBR4p_joint3','LBR4p_joint4',... - % 'LBR4p_joint5','LBR4p_joint6','LBR4p_joint7'}; - % u = [0.1 0.1 0.1 0.1 0.1 0.1 0.1]; - % - % % Recommended: - % set_joint_target_velocities(jointnames, u); - % - % % Advanced usage: - % set_joint_target_velocities(jointnames, u, OP_ONESHOT); - - if nargin == 3 - % The recommended mode is OP_ONESHOT - opmode = obj.OP_ONESHOT; - end - - for joint_index=1:length(jointnames) - if isa(jointnames,'cell') - obj.vrep.simxSetJointTargetVelocity(... - obj.clientID,... - obj.handle_from_string_or_handle(jointnames{joint_index}),... - joint_target_velocities(joint_index),... - opmode); - else - obj.vrep.simxSetJointTargetVelocity(... - obj.clientID,... - obj.handle_from_string_or_handle(jointnames),... - joint_target_velocities(joint_index),... - opmode); - end - end - end - - function joint_torques = get_joint_torques(obj,jointnames,opmode) - % This method gets the joint torques of a robot in the CoppeliaSim scene. - % Usage: - % Recommended: - % joint_torques = get_joint_torques(jointnames); - % - % Advanced: - % joint_torques = get_joint_torques(jointnames, opmode) - % - % jointnames: The joint names. - % (optional) opmode: The operation mode. If not - % specified, the opmode will be set automatically. - % - % You can use the following modes: - % OP_BLOCKING - % OP_STREAMING - % OP_ONESHOT - % OP_BUFFER; - % - % Check this link for more details: https://www.coppeliarobotics.com/helpFiles/en/remoteApiFunctionsMatlab.htm#simxGetJointForce - % - % Example: - % jointnames={'LBR4p_joint1','LBR4p_joint2','LBR4p_joint3','LBR4p_joint4',... - % 'LBR4p_joint5','LBR4p_joint6','LBR4p_joint7'}; - % - % % Recommended: - % joint_torques = get_joint_torques(jointnames); - % - % % Advanced usage: - % joint_torques = get_joint_torques(jointnames, OP_STREAMING); - - joint_torques = zeros(length(jointnames),1); - - % If the user does not specify the opmode, it is chosen first - % as STREAMING and then as BUFFER, as specified by the remote - % API documentation. - for joint_index=1:length(jointnames) - % First approach to the auto-management using - % DQ_VrepInterfaceMapElements. If the user does not specify the - % opmode, it is chosen first as STREAMING and then as BUFFER, - % as specified by the remote API documentation - if nargin <= 2 - if isa(jointnames,'cell') - element = obj.element_from_string(jointnames{joint_index}); - else - element = obj.element_from_string(jointnames); - end - if(~element.state_from_function_signature('get_joint_torques')) - [~,tmp] = obj.vrep.simxGetJointForce(obj.clientID, element.handle, obj.OP_STREAMING); - return_code = 1; - while(return_code == 1) - [return_code,tmp] = obj.vrep.simxGetJointForce(obj.clientID, element.handle, obj.OP_BUFFER); - end - else - [~,tmp] = obj.vrep.simxGetJointForce(obj.clientID, element.handle, obj.OP_BUFFER); - end - else - [~,tmp] = obj.vrep.simxGetJointForce(obj.clientID, ... - obj.handle_from_string_or_handle(jointnames{joint_index}), opmode); - end - joint_torques(joint_index) = double(-tmp); % V-REP returns joint torques with an inverse sign - end - end - - function set_joint_torques(obj,jointnames,torques,opmode) - % This method sets the joint torques of a robot in the CoppeliaSim scene. - % Usage: - % Recommended: - % set_joint_torques(jointnames, torques); - % - % Advanced: - % set_joint_torques(jointnames, torques, opmode); - % - % jointnames: The joint names. - % torques: The joint torques. - % (optional) opmode: The operation mode. If not - % specified, the opmode will be set automatically. - % - % You can use the following modes: - % OP_BLOCKING - % OP_STREAMING - % OP_ONESHOT - % OP_BUFFER; - % - % Check this link for more details: https://www.coppeliarobotics.com/helpFiles/en/remoteApiFunctionsMatlab.htm#simxSetJointTargetVelocity - % - % Example: - % jointnames={'LBR4p_joint1','LBR4p_joint2','LBR4p_joint3','LBR4p_joint4',... - % 'LBR4p_joint5','LBR4p_joint6','LBR4p_joint7'}; - % u = [0.1 0.1 0.1 0.1 0.1 0.1 0.1]; - % - % % Recommended: - % set_joint_torques(jointnames, u); - % - % % Advanced usage: - % set_joint_torques(jointnames, u, opmode); - - % If the user does not specify the opmode, it is chosen first - % as STREAMING and then as OP_ONESHOT, as specified by the - % remote API documentation. - if nargin == 3 - % The recommended mode is OP_ONESHOT - for joint_index=1:length(jointnames) - obj.vrep.simxSetJointTargetVelocity(obj.clientID, obj.handle_from_string_or_handle(jointnames{joint_index}), ... - sign(torques(joint_index))*10e10, obj.OP_ONESHOT); - obj.vrep.simxSetJointForce(obj.clientID, obj.handle_from_string_or_handle(jointnames{joint_index}), ... - abs(torques(joint_index)), obj.OP_ONESHOT); - end - else - for joint_index=1:length(jointnames) - obj.vrep.simxSetJointTargetVelocity(obj.clientID, obj.handle_from_string_or_handle(jointnames{joint_index}),... - sign(torques(joint_index))*10e10, opmode); - obj.vrep.simxSetJointForce(obj.clientID,obj.handle_from_string_or_handle(jointnames{joint_index}),abs(torques(joint_index)),... - opmode); - end - end + warning('Deprecated. Use DQ_CoppeliaSimLegacyInterface instead.') + obj@DQ_CoppeliaSimLegacyInterface(); end - end - end diff --git a/interfaces/vrep/DQ_VrepRobot.m b/interfaces/vrep/DQ_VrepRobot.m index 7c59b4a8..b715d6e8 100644 --- a/interfaces/vrep/DQ_VrepRobot.m +++ b/interfaces/vrep/DQ_VrepRobot.m @@ -38,32 +38,14 @@ % - Removed the following methods to ensure compatibility with the % C++ version of the class: % - 'kinematics' +% 3. Juan Jose Quiroz Omana (juanjose.quirozomana@manchester.ac.uk) +% - The class now inherits from DQ_CoppeliaSimRobot -classdef (Abstract) DQ_VrepRobot - - properties - robot_name - vrep_interface - end - - methods (Abstract) - set_configuration(obj,q); - q = get_configuration(obj); - end - +classdef (Abstract) DQ_VrepRobot < DQ_CoppeliaSimRobot methods - function send_q_to_vrep(obj, q) - % For backwards compatibility only. Do not use this method. - - warning('Deprecated. Use set_configuration() instead.') - obj.set_configuration(q) - end - - function q = get_q_from_vrep(obj) - % For backwards compatibility only. Do not use this method. - - warning('Deprecated. Use get_configuration() instead.') - q = obj.get_configuration(); + function obj = DQ_VrepRobot() + warning('Deprecated. Use DQ_CoppeliaSimRobot instead.') + obj@DQ_CoppeliaSimRobot(); end end end diff --git a/interfaces/vrep/robots/LBR4pVrepRobot.m b/interfaces/vrep/robots/LBR4pVrepRobot.m index b020dfad..0e1f0963 100644 --- a/interfaces/vrep/robots/LBR4pVrepRobot.m +++ b/interfaces/vrep/robots/LBR4pVrepRobot.m @@ -43,35 +43,16 @@ % - Responsible for the original implementation % 2. Frederico Fernandes Afonso Silva (frederico.silva@ieee.org) % - Updated for compatibility with the DQ_SerialVrepRobot class. +% 3. Juan Jose Quiroz Omana (juanjose.quirozomana@manchester.ac.uk) +% - The class now inherits from LBR4pCoppeliaSimRobot -classdef LBR4pVrepRobot < DQ_SerialVrepRobot + +classdef LBR4pVrepRobot < LBR4pCoppeliaSimRobot methods function obj = LBR4pVrepRobot(robot_name, vrep_interface) - obj@DQ_SerialVrepRobot("LBR4p", 7, robot_name, vrep_interface); + warning('Deprecated. Use LBR4pCoppeliaSimRobot instead.') + obj@LBR4pCoppeliaSimRobot(robot_name, vrep_interface); end - - function kin = kinematics(obj) - %% Obtains the DQ_SerialManipulator instance that represents this LBR4p robot. - % >> vrep_robot = LBR4pVrepRobot("LBR4p", vi); - % >> robot_kinematics = vrep_robot.kinematics(); - - LBR4p_DH_theta=[0, 0, 0, 0, 0, 0, 0]; - LBR4p_DH_d = [0.200, 0, 0.4, 0, 0.39, 0, 0]; - LBR4p_DH_a = [0, 0, 0, 0, 0, 0, 0]; - LBR4p_DH_alpha = [pi/2, -pi/2, pi/2, -pi/2, pi/2, -pi/2, 0]; - LBR4p_DH_type = double(repmat(DQ_JointType.REVOLUTE,1,7)); - LBR4p_DH_matrix = [LBR4p_DH_theta; - LBR4p_DH_d; - LBR4p_DH_a; - LBR4p_DH_alpha - LBR4p_DH_type]; - - kin = DQ_SerialManipulatorDH(LBR4p_DH_matrix); - - kin.set_reference_frame(obj.vrep_interface.get_object_pose(obj.base_frame_name)); - kin.set_base_frame(obj.vrep_interface.get_object_pose(obj.base_frame_name)); - kin.set_effector(1+0.5*DQ.E*DQ.k*0.07); - end end end diff --git a/interfaces/vrep/robots/YouBotVrepRobot.m b/interfaces/vrep/robots/YouBotVrepRobot.m index 7ad10e34..3d00c196 100644 --- a/interfaces/vrep/robots/YouBotVrepRobot.m +++ b/interfaces/vrep/robots/YouBotVrepRobot.m @@ -45,105 +45,16 @@ % - Responsible for the original implementation % 2. Frederico Fernandes Afonso Silva (frederico.silva@ieee.org) % - Updated for compatibility with the DQ_SerialVrepRobot class. +% 3. Juan Jose Quiroz Omana (juanjose.quirozomana@manchester.ac.uk) +% - The class now inherits from YouBotCoppeliaSimRobot -classdef YouBotVrepRobot < DQ_SerialVrepRobot - properties (Constant) - adjust = ((cos(pi/2) + DQ.i*sin(pi/2)) * (cos(pi/4) + DQ.j*sin(pi/4)))*(1+0.5*DQ.E*-0.1*DQ.k); - end - + +classdef YouBotVrepRobot < YouBotCoppeliaSimRobot methods function obj = YouBotVrepRobot(robot_name, vrep_interface) - obj@DQ_SerialVrepRobot("youBot", 5, robot_name, vrep_interface); - - %% youBot does not follow the standard naming convention in CoppeliaSim. Also, the use of 'set_names()', as is done in the C++ implementation, is not supported on a constructor in MATLAB - % From the second copy of the robot and onward, VREP appends a - % #number in the robot's name. We check here if the robot is - % called by the correct name and assign an index that will be - % used to correctly infer the robot's joint labels. - splited_name = strsplit(robot_name,'#'); - robot_label = splited_name{1}; - if ~strcmp(robot_label,'youBot') - error('Expected youBot') - end - if length(splited_name) > 1 - robot_index = splited_name{2}; - else - robot_index = ''; - end - - % Initialize joint names and base frame - obj.joint_names = {}; - for i=1:5 - current_joint_name = {robot_label,'ArmJoint',int2str(i-1),robot_index}; - obj.joint_names{i} = strjoin(current_joint_name,''); - end - obj.base_frame_name = robot_name; - end - - function set_configuration(obj,q) - %% Sends the joint configurations to VREP - % >> vrep_robot = YouBotVrepRobot("youBot", vi) - % >> q = zeros(8,1); - % >> vrep_robot.set_configuration(q) - x = q(1); - y = q(2); - phi = q(3); - - r = cos(phi/2.0)+DQ.k*sin(phi/2.0); - p = x * DQ.i + y * DQ.j; - pose = (1 + DQ.E*0.5*p)*r; - - obj.vrep_interface.set_joint_positions(obj.joint_names,q(4:8)); - obj.vrep_interface.set_object_pose(obj.base_frame_name, pose * obj.adjust'); - end - - function q = get_configuration(obj) - %% Obtains the joint configurations from VREP - % >> vrep_robot = YouBotVrepRobot("youBot", vi) - % >> q = vrep_robot.get_configuration(q) - base_x = obj.vrep_interface.get_object_pose(obj.base_frame_name) * obj.adjust; - base_t = vec3(translation(base_x)); - base_phi = rotation_angle(rotation(base_x)); - base_arm_q = obj.vrep_interface.get_joint_positions(obj.joint_names); - - q = [base_t(1); base_t(2); base_phi; base_arm_q]; - end - - function kin = kinematics(obj) - %% Obtains the DQ_WholeBody instance that represents this youBot robot. - % >> vrep_robot = YouBotVrepRobot("youBot", vi) - % >> robot_kinematics = vrep_robot.kinematics() - - include_namespace_dq - % The DH parameters and other geometric parameters are based on - % Kuka's documentation: - % http://www.youbot-store.com/wiki/index.php/YouBot_Detailed_Specifications - % https://www.generationrobots.com/img/Kuka-YouBot-Technical-Specs.pdf - pi2 = pi/2; - arm_DH_theta = [ 0, pi2, 0, pi2, 0]; - arm_DH_d = [ 0.147, 0, 0, 0, 0.218]; - arm_DH_a = [ 0.033, 0.155, 0.135, 0, 0]; - arm_DH_alpha = [pi2, 0, 0, pi2, 0]; - arm_DH_type = double(repmat(DQ_JointType.REVOLUTE,1,5)); - arm_DH_matrix = [arm_DH_theta; - arm_DH_d; - arm_DH_a; - arm_DH_alpha - arm_DH_type]; - - arm = DQ_SerialManipulatorDH(arm_DH_matrix); - base = DQ_HolonomicBase(); - - x_bm = 1 + E_*0.5*(0.165*i_ + 0.11*k_); - - base.set_frame_displacement(x_bm); - - kin = DQ_WholeBody(base); - kin.add(arm); - - effector = 1 + E_*0.5*0.3*k_; - kin.set_effector(effector); - end + warning('Deprecated. Use YouBotCoppeliaSimRobot instead.') + obj@YouBotCoppeliaSimRobot(robot_name, vrep_interface); + end end end