diff --git a/interfaces/vrep/DQ_SerialVrepRobot.m b/interfaces/vrep/DQ_SerialVrepRobot.m deleted file mode 100644 index df363a09..00000000 --- a/interfaces/vrep/DQ_SerialVrepRobot.m +++ /dev/null @@ -1,240 +0,0 @@ -% CLASS DQ_SerialVrepRobot - 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 - -classdef DQ_SerialVrepRobot < DQ_VrepRobot - properties - joint_names; - base_frame_name; - end - - 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); - end - end -end \ No newline at end of file diff --git a/interfaces/vrep/DQ_VrepInterface.m b/interfaces/vrep/DQ_VrepInterface.m deleted file mode 100644 index 036d09b3..00000000 --- a/interfaces/vrep/DQ_VrepInterface.m +++ /dev/null @@ -1,1423 +0,0 @@ -% CLASS DQ_VrepInterface - 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_VrepInterface 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 -% get_center_of_mass - Get the object center of mass as a vector -% get_mass- Get the object mass as a real number -% get_inertia_matrix - Get the object inertia tensor as a matrix -% 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_VrepInterface Methods (For advanced users) -% call_script_function - Call a LUA script function in V-REP -% 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 (juanjqo@g.ecc.u-tokyo.ac.jp) -% - 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 -% -% 3. Frederico Fernandes Afonso Silva (frederico.silva@ieee.org) -% - Added the following methods: -% - call_script_function() (see https://github.com/dqrobotics/matlab/pull/109) -% - get_center_of_mass() (see https://github.com/dqrobotics/matlab/pull/109) -% - get_mass() (see https://github.com/dqrobotics/matlab/pull/109) -% - get_inertia_matrix() (see https://github.com/dqrobotics/matlab/pull/109) -% - Added the following properties: -% - DF_LUA_SCRIPT_API (see https://github.com/dqrobotics/matlab/pull/109) -% - ST_CHILD (see https://github.com/dqrobotics/matlab/pull/109) -% - BODY_FRAME (see https://github.com/dqrobotics/matlab/pull/109) -% - ABSOLUTE_FRAME (see https://github.com/dqrobotics/matlab/pull/109) -% - 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_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 DQ Robotic's default LUA script API with CoppeliaSim - DF_LUA_SCRIPT_API = '/DQRoboticsApiCommandServer'; - % 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; - % Constant that denotes the CoppeliaSim's remote API child script type - ST_CHILD = remApi('remoteApi').sim_scripttype_childscript; - % Constant that denotes the shape frame in a CoppeliaSim's object - BODY_FRAME = 'body_frame'; - % Constant that denotes the inertial frame in a CoppeliaSim's scene - ABSOLUTE_FRAME = 'absolute_frame'; - end - - methods (Access = private) - function [return_code, output_ints, output_doubles, output_strings] = call_script_function(obj, function_name, obj_name, input_ints, input_floats, input_strings, script_type, opmode) - % This method calls a LUA script function in CoppeliaSim. - % - % Usage: - % Recommended: - % [return_code, output_ints, output_doubles, output_strings] = call_script_function(function_name, obj_name, input_ints, input_floats, input_strings, script_type) - % - % Advanced: - % [return_code, output_ints, output_doubles, output_strings] = call_script_function(function_name, obj_name, input_ints, input_floats, input_strings, script_type, opmode) - % - % function_name: The name of the script function to call in the specified script. - % obj_name: The name of the object where the script is attached to. - % input_ints: The input integer values. - % input_floats: The input floating-point values. - % input_strings: The input strings. - % script_type: The type of the script. - % - % You can use the following types: - % ST_CHILD - % - % (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#simxCallScriptFunction - % - % Example: - % input_ints = []; - % input_floats = []; - % input_strings = []; - % - % % Recommended: - % [rtn, output_ints, output_doubles, output_strings] = call_script_function('my_function_name', 'DQRoboticsApiCommandServer', input_ints, input_floats, input_strings, ST_CHILD) - % - % % For advanced usage: - % [rtn, output_ints, output_doubles, output_strings] = call_script_function('my_function_name', 'DQRoboticsApiCommandServer', input_ints, input_floats, input_strings, ST_CHILD, OP_BLOCKING) - - % If the user does not specify the opmode, it is chosen as - % OP_BLOCKING as specified by the remote API documentation. - if nargin == 7 - [return_code, output_ints, output_floats, output_strings, ~] = obj.vrep.simxCallScriptFunction(obj.clientID, obj_name, ... - script_type, function_name, input_ints, input_floats , input_strings, [], obj.OP_BLOCKING); - output_doubles = double(output_floats); - else - [return_code, output_ints, output_floats, output_strings, ~] = obj.vrep.simxCallScriptFunction(obj.clientID, obj_name, ... - script_type, function_name, input_ints, input_floats , input_strings, [], opmode); - output_doubles = double(output_floats); - end - end - - 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_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 center_of_mass = get_center_of_mass(obj, objectname, reference_frame, function_name, obj_script_name) - % This method gets the center of mass of an object in the CoppeliaSim scene. - % - % Usage: - % Recommended: - % center_of_mass = get_center_of_mass(objectname); - % - % Advanced: - % center_of_mass = get_center_of_mass(objectname, ref_frame_handle_or_name, function_name, obj_script_name); - % - % objectname: The object's name. - % (optional) reference_frame: Indicates the handle of the relative reference frame in which you want the center of mass. - % If not specified, the shape's reference frame is used. - % (optional) function_name: The name of the script function to call in the specified script. - % (Default: "get_center_of_mass") - % (optional) obj_script_name: The name of the object where the script is attached to. - % (Default: 'DQRoboticsApiCommandServer') - % - % - % Check this link for more details: https://www.coppeliarobotics.com/helpFiles/en/regularApi/simGetShapeInertia.htm - % - % Example: - % % Recommended: - % center_of_mass = get_center_of_mass('/Jaco/Jaco_link2'); - % - % % For advanced usage: - % center_of_mass = get_center_of_mass('/Jaco/Jaco_link2', 'my_reference_frame', 'my_get_center_of_mass', 'my_DQRoboticsApiCommandServer'); - - obj_handle = obj.handle_from_string_or_handle(objectname); - - if nargin == 2 % the call was: center_of_mass = get_center_of_mass(objectname) - [return_code, ~, center_of_mass, ~] = obj.call_script_function('get_center_of_mass', obj.DF_LUA_SCRIPT_API, obj_handle, [], [], obj.ST_CHILD); - elseif nargin == 3 % the call was: center_of_mass = get_center_of_mass(objectname, reference_frame) - if(strcmp(reference_frame, obj.ABSOLUTE_FRAME)) - [return_code, ~, center_of_mass, ~] = obj.call_script_function('get_center_of_mass', obj.DF_LUA_SCRIPT_API, obj_handle, [], obj.ABSOLUTE_FRAME, obj.ST_CHILD); - elseif(strcmp(reference_frame, obj.BODY_FRAME)) - [return_code, ~, center_of_mass, ~] = obj.call_script_function('get_center_of_mass', obj.DF_LUA_SCRIPT_API, [obj_handle, obj_handle], [], [], obj.ST_CHILD); - else - ref_frame_handle = obj.handle_from_string_or_handle(reference_frame); - [return_code, ~, center_of_mass, ~] = obj.call_script_function('get_center_of_mass', obj.DF_LUA_SCRIPT_API, [obj_handle, ref_frame_handle], [], [], obj.ST_CHILD); - end - elseif nargin == 4 % the call was: center_of_mass = get_center_of_mass(objectname, reference_frame, function_name) - if(strcmp(reference_frame, obj.ABSOLUTE_FRAME)) - [return_code, ~, center_of_mass, ~] = obj.call_script_function(function_name, obj.DF_LUA_SCRIPT_API, obj_handle, [], obj.ABSOLUTE_FRAME, obj.ST_CHILD); - elseif(strcmp(reference_frame, obj.BODY_FRAME)) - [return_code, ~, center_of_mass, ~] = obj.call_script_function(function_name, obj.DF_LUA_SCRIPT_API, [obj_handle, obj_handle], [], [], obj.ST_CHILD); - else - ref_frame_handle = obj.handle_from_string_or_handle(reference_frame); - [return_code, ~, center_of_mass, ~] = obj.call_script_function(function_name, obj.DF_LUA_SCRIPT_API, [obj_handle, ref_frame_handle], [], [], obj.ST_CHILD); - end - else % the call was: center_of_mass = get_center_of_mass(objectname, reference_frame, function_name, obj_name) - if(strcmp(reference_frame, obj.ABSOLUTE_FRAME)) - [return_code, ~, center_of_mass, ~] = obj.call_script_function(function_name, obj_script_name, obj_handle, [], obj.ABSOLUTE_FRAME, obj.ST_CHILD); - elseif(strcmp(reference_frame, obj.BODY_FRAME)) - [return_code, ~, center_of_mass, ~] = obj.call_script_function(function_name, obj_script_name, [obj_handle, obj_handle], [], [], obj.ST_CHILD); - else - ref_frame_handle = obj.handle_from_string_or_handle(reference_frame); - [return_code, ~, center_of_mass, ~] = obj.call_script_function(function_name, obj_script_name, [obj_handle, ref_frame_handle], [], [], obj.ST_CHILD); - end - end - - if(return_code ~= 0) - if nargin <= 3 - formatSpec = 'Script function `get_center_of_mass` returned with error code %i.\n'; - fprintf(formatSpec, return_code); - else - formatSpec = 'Script function %s returned with error code %i.\n'; - fprintf(formatSpec, function_name, return_code); - end - error('Failed calling script function!'); - else % ensure cast to double - center_of_mass = double(center_of_mass); - center_of_mass = center_of_mass(1)*DQ.i + center_of_mass(2)*DQ.j + center_of_mass(3)*DQ.k; - end - end - - function mass = get_mass(obj, objectname, function_name, obj_script_name) - % This method gets the mass of an object in the CoppeliaSim scene. - % - % Usage: - % Recommended: - % mass = get_mass(objectname); - % - % Advanced: - % mass = get_mass(objectname, function_name, obj_script_name); - % - % objectname: The object's name. - % (optional) function_name: The name of the script function to call in the specified script. - % (Default: "get_mass") - % (optional) obj_script_name: The name of the object where the script is attached to. - % (Default: 'DQRoboticsApiCommandServer') - % - % - % Check this link for more details: https://www.coppeliarobotics.com/helpFiles/en/regularApi/simGetShapeMass.htm - % - % Example: - % % Recommended: - % mass = get_mass('/Jaco/Jaco_link2'); - % - % % For advanced usage: - % mass = get_mass('/Jaco/Jaco_link2', 'my_get_center_of_mass', 'my_DQRoboticsApiCommandServer'); - - obj_handle = obj.handle_from_string_or_handle(objectname); - - if nargin == 2 % the call was: mass = get_mass(objectname) - [return_code, ~, mass, ~] = obj.call_script_function('get_mass', obj.DF_LUA_SCRIPT_API, obj_handle, [], [], obj.ST_CHILD); - elseif nargin == 3 % the call was: mass = get_mass(objectname, function_name) - [return_code, ~, mass, ~] = obj.call_script_function(function_name, obj.DF_LUA_SCRIPT_API, obj_handle, [], [], obj.ST_CHILD); - else % the call was: mass = get_mass(objectname, function_name, obj_name) - [return_code, ~, mass, ~] = obj.call_script_function(function_name, obj_script_name, obj_handle, [], [], obj.ST_CHILD); - end - - if(return_code ~= 0) - if nargin == 2 - formatSpec = 'Script function `get_mass` returned with error code %i.\n'; - fprintf(formatSpec, return_code); - else - formatSpec = 'Script function %s returned with error code %i.\n'; - fprintf(formatSpec, function_name, return_code); - end - error('Failed calling script function!'); - else % ensure cast to double - mass = double(mass); - end - end - - function inertia_tensor = get_inertia_matrix(obj, objectname, reference_frame, function_name, obj_script_name) - % This method gets the inertia tensor of an object in the CoppeliaSim scene. - % - % Usage: - % Recommended: - % inertia_tensor = get_inertia_matrix(objectname); - % - % Advanced: - % inertia_tensor = get_inertia_matrix(objectname, ref_frame_handle_or_name, function_name, obj_script_name); - % - % objectname: The object's handle or name. - % (optional) reference_frame: Indicates the handle of the relative reference frame in which you want the inertia tensor. - % If not specified, the shape's reference frame is used. - % (optional) function_name: The name of the script function to call in the specified script. - % (Default: "get_inertia") - % (optional) obj_script_name: The name of the object where the script is attached to. - % (Default: 'DQRoboticsApiCommandServer') - % - % - % Check this link for more details: https://www.coppeliarobotics.com/helpFiles/en/regularApi/simGetShapeInertia.htm - % - % Example: - % % Recommended: - % inertia_tensor = get_inertia_matrix('/Jaco/Jaco_link2'); - % - % % For advanced usage: - % inertia_tensor = get_inertia_matrix('/Jaco/Jaco_link2', '/Jaco/Jaco_joint2', 'my_get_inertia_matrix', 'my_DQRoboticsApiCommandServer'); - - obj_handle = obj.handle_from_string_or_handle(objectname); - - if nargin == 2 % the call was: inertia_tensor = get_inertia_matrix(objectname) - [return_code, ~, inertia_tensor, ~] = obj.call_script_function('get_inertia', obj.DF_LUA_SCRIPT_API, obj_handle, [], [], obj.ST_CHILD); - elseif nargin == 3 % the call was: inertia_tensor = get_inertia_matrix(objectname, reference_frame) - if(strcmp(reference_frame, obj.ABSOLUTE_FRAME)) - [return_code, ~, inertia_tensor, ~] = obj.call_script_function('get_inertia', obj.DF_LUA_SCRIPT_API, obj_handle, [], obj.ABSOLUTE_FRAME, obj.ST_CHILD); - elseif(strcmp(reference_frame, obj.BODY_FRAME)) - [return_code, ~, inertia_tensor, ~] = obj.call_script_function('get_inertia', obj.DF_LUA_SCRIPT_API, [obj_handle, obj_handle], [], [], obj.ST_CHILD); - else - ref_frame_handle = obj.handle_from_string_or_handle(reference_frame); - [return_code, ~, inertia_tensor, ~] = obj.call_script_function('get_inertia', obj.DF_LUA_SCRIPT_API, [obj_handle, ref_frame_handle], [], [], obj.ST_CHILD); - end - elseif nargin == 4 % the call was: inertia_tensor = get_inertia_matrix(objectname, reference_frame, function_name) - if(strcmp(reference_frame, obj.ABSOLUTE_FRAME)) - [return_code, ~, inertia_tensor, ~] = obj.call_script_function(function_name, obj.DF_LUA_SCRIPT_API, obj_handle, [], obj.ABSOLUTE_FRAME, obj.ST_CHILD); - elseif(strcmp(reference_frame, obj.BODY_FRAME)) - [return_code, ~, inertia_tensor, ~] = obj.call_script_function(function_name, obj.DF_LUA_SCRIPT_API, [obj_handle, obj_handle], [], [], obj.ST_CHILD); - else - ref_frame_handle = obj.handle_from_string_or_handle(reference_frame); - [return_code, ~, inertia_tensor, ~] = obj.call_script_function(function_name, obj.DF_LUA_SCRIPT_API, [obj_handle, ref_frame_handle], [], [], obj.ST_CHILD); - end - else % the call was: inertia_tensor = get_inertia_matrix(objectname, reference_frame, function_name, obj_name) - if(strcmp(reference_frame, obj.ABSOLUTE_FRAME)) - [return_code, ~, inertia_tensor, ~] = obj.call_script_function(function_name, obj_script_name, obj_handle, [], obj.ABSOLUTE_FRAME, obj.ST_CHILD); - elseif(strcmp(reference_frame, obj.BODY_FRAME)) - [return_code, ~, inertia_tensor, ~] = obj.call_script_function(function_name, obj_script_name, [obj_handle, obj_handle], [], [], obj.ST_CHILD); - else - ref_frame_handle = obj.handle_from_string_or_handle(reference_frame); - [return_code, ~, inertia_tensor, ~] = obj.call_script_function(function_name, obj_script_name, [obj_handle, ref_frame_handle], [], [], obj.ST_CHILD); - end - end - - if(return_code ~= 0) - if nargin <= 3 - formatSpec = 'Script function `get_inertia` returned with error code %i.\n'; - fprintf(formatSpec, return_code); - else - formatSpec = 'Script function %s returned with error code %i.\n'; - fprintf(formatSpec, function_name, return_code); - end - error('Failed calling script function!'); - else % ensure cast to double and reshape the 1x9 vector to a 3x3 matrix - inertia_tensor = double(reshape(inertia_tensor,3,3)); - 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 \ No newline at end of file diff --git a/interfaces/vrep/DQ_VrepInterfaceMapElement.m b/interfaces/vrep/DQ_VrepInterfaceMapElement.m deleted file mode 100644 index 0f04cc07..00000000 --- a/interfaces/vrep/DQ_VrepInterfaceMapElement.m +++ /dev/null @@ -1,47 +0,0 @@ -% (C) Copyright 2019 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: -% Murilo Marques Marinho - murilo@nml.t.u-tokyo.ac.jp - -classdef DQ_VrepInterfaceMapElement - properties - % For VREP's remote API, the first call to any "get" function should be OP_STREAMING and the following calls should be - % OP_BUFFER, so we need to track the states of each of them using - % the following map - set_states_map - handle - end - methods - %% Constructor - function obj = DQ_VrepInterfaceMapElement(handle) - obj.set_states_map = containers.Map; - obj.handle = handle; - end - - function state = state_from_function_signature(obj,function_signature) - if(obj.set_states_map.isKey(function_signature)) - obj.set_states_map(function_signature) = true; - else - obj.set_states_map(function_signature) = false; - end - state = obj.set_states_map(function_signature); - end - end -end diff --git a/interfaces/vrep/DQ_VrepRobot.m b/interfaces/vrep/DQ_VrepRobot.m deleted file mode 100644 index 7c59b4a8..00000000 --- a/interfaces/vrep/DQ_VrepRobot.m +++ /dev/null @@ -1,70 +0,0 @@ -% CLASS DQ_VrepRobot - 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' - -classdef (Abstract) DQ_VrepRobot - - 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/vrep/robots/LBR4pVrepRobot.m b/interfaces/vrep/robots/LBR4pVrepRobot.m deleted file mode 100644 index b020dfad..00000000 --- a/interfaces/vrep/robots/LBR4pVrepRobot.m +++ /dev/null @@ -1,77 +0,0 @@ -% CLASS LBR4pVrepRobot - 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. - -classdef LBR4pVrepRobot < DQ_SerialVrepRobot - methods - function obj = LBR4pVrepRobot(robot_name, vrep_interface) - obj@DQ_SerialVrepRobot("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/vrep/robots/YouBotVrepRobot.m b/interfaces/vrep/robots/YouBotVrepRobot.m deleted file mode 100644 index 7ad10e34..00000000 --- a/interfaces/vrep/robots/YouBotVrepRobot.m +++ /dev/null @@ -1,149 +0,0 @@ -% 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. - -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 - - 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 - end -end -