From 5ea7088e31dc9a2ac059adb09b8ea9b61b5b5afa Mon Sep 17 00:00:00 2001 From: Frederico Afonso Date: Fri, 16 Feb 2024 12:00:53 +0000 Subject: [PATCH 01/10] [DQ_VrepInterface] Added properties 'DF_LUA_SCRIPT_API' and 'ST_CHILD' and methods 'call_script_function()', 'get_center_of_mass()', 'get_mass()', and 'get_inertia_matrix()'. --- interfaces/vrep/DQ_VrepInterface.m | 294 +++++++++++++++++++++++++++++ 1 file changed, 294 insertions(+) diff --git a/interfaces/vrep/DQ_VrepInterface.m b/interfaces/vrep/DQ_VrepInterface.m index c7b8dc78..d7409c88 100644 --- a/interfaces/vrep/DQ_VrepInterface.m +++ b/interfaces/vrep/DQ_VrepInterface.m @@ -44,6 +44,9 @@ % 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 @@ -59,6 +62,7 @@ % get_joint_velocities - Get the joint velocities 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 % @@ -96,6 +100,14 @@ % - 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/XXX) +% - get_center_of_mass (see https://github.com/dqrobotics/matlab/pull/XXX) +% - get_mass() (see https://github.com/dqrobotics/matlab/pull/XXX) +% - get_inertia_matrix() (see https://github.com/dqrobotics/matlab/pull/XXX) +% - Added the following properties: +% - DF_LUA_SCRIPT_API (see https://github.com/dqrobotics/matlab/pull/XXX) +% - ST_CHILD (see https://github.com/dqrobotics/matlab/pull/XXX) % - Altered the following properties from 'private' to 'protected' % (see discussions in https://github.com/dqrobotics/matlab/pull/101 % to further details): @@ -118,6 +130,8 @@ end properties (Constant) + % Constant that denotes DQ Robotic's default LUA script API with V-VREP + 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 @@ -128,6 +142,8 @@ 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 V-VREP's remote API child script type + ST_CHILD = remApi('remoteApi').sim_scripttype_childscript; end methods (Access = private) @@ -162,6 +178,67 @@ disp(['This version of DQ Robotics DQ_VrepInterface is compatible'... ' with VREP 3.5.0']); end + + function [return_code,output_ints,output_doubles,output_strings,retBuffer] = call_script_function(obj,obj_name,script_type,function_name,... + input_ints,input_floats,input_strings,input_buffer,opmode) + % This method calls a LUA script function in V-REP. + % + % Usage: + % Recommended: + % [return_code,output_ints,output_doubles,output_strings,retBuffer] = call_script_function(obj_name, script_type, function_name, input_ints, input_floats, input_strings, input_buffer) + % + % Advanced: + % [return_code,output_ints,output_doubles,output_strings,retBuffer] = call_script_function(obj_name, script_type, function_name, input_ints, input_floats, input_strings, input_buffer, opmode) + % + % obj_name: The name of the object where the script is + % attached to. + % script_type: The type of the script. + % + % You can use the following types: + % ST_CHILD + % + % function_name: The name of the script function to + % call in the specified script. + % input_ints: The input integer values. + % input_floats: The input floating-point values. + % input_strings: The input strings. + % input_buffer: The input buffer. + % (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 = []; + % input_buffer = []; + % + % % Recommended: + % [rtn, output_ints, output_doubles, output_strings, retBuffer] = call_script_function('DQRoboticsApiCommandServer', ST_CHILD, 'my_function_name', input_ints, input_floats, input_strings, input_buffer) + % + % % For advanced usage: + % [rtn, output_ints, output_doubles, output_strings, retBuffer] = call_script_function('DQRoboticsApiCommandServer', ST_CHILD, 'my_function_name', input_ints, input_floats, input_strings, input_buffer, 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 == 8 + [return_code,output_ints,output_floats,output_strings,retBuffer] = obj.vrep.simxCallScriptFunction(obj.clientID, obj_name, ... + script_type, function_name, input_ints, input_floats , input_strings, input_buffer, obj.OP_BLOCKING); + output_doubles = double(output_floats); + else + [return_code,output_ints,output_floats,output_strings,retBuffer] = obj.vrep.simxCallScriptFunction(obj.clientID, obj_name, ... + script_type, function_name, input_ints, input_floats , input_strings, input_buffer, opmode); + output_doubles = double(output_floats); + end + end function connect(obj,ip,port) % This method connects to the remote api server (i.e. CoppeliaSim). @@ -680,6 +757,223 @@ function set_object_pose(obj,objectname,pose,reference_frame,opmode) end end + function center_of_mass = get_center_of_mass(obj, obj_handle_or_name, ref_frame_handle_or_name, 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(obj_handle_or_name); + % + % Advanced: + % center_of_mass = get_center_of_mass(obj_handle_or_name, ref_frame_handle_or_name, function_name, obj_script_name); + % + % obj_handle_or_name: The object's handle or 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', '/Jaco/Jaco_joint2', 'my_get_center_of_mass', 'my_DQRoboticsApiCommandServer'); + + obj_handle = obj.handle_from_string_or_handle(obj_handle_or_name); + + if nargin == 2 % the call was 'center_of_mass = get_center_of_mass(handle)' + [return_code,~,center_of_mass,~,~] = obj.call_script_function(obj.DF_LUA_SCRIPT_API, obj.ST_CHILD, 'get_center_of_mass', obj_handle, [], [], []); + if(return_code ~= 0) + formatSpec = 'Script function `get_center_of_mass` returned with error code %i.\n'; + fprintf(formatSpec, return_code); + error('Failed calling script function!'); + else % ensure cast to double + center_of_mass = double(center_of_mass); + end + elseif nargin == 3 % the call was 'center_of_mass = get_center_of_mass(handle, reference_frame)' + ref_frame_handle = obj.handle_from_string_or_handle(ref_frame_handle_or_name); + + [return_code,~,center_of_mass,~,~] = obj.call_script_function(obj.DF_LUA_SCRIPT_API, obj.ST_CHILD, 'get_center_of_mass', [obj_handle, ref_frame_handle], [], [], []); + if(return_code ~= 0) + formatSpec = 'Script function `get_center_of_mass` returned with error code %i.\n'; + fprintf(formatSpec, return_code); + error('Failed calling script function!'); + else % ensure cast to double + center_of_mass = double(center_of_mass); + end + elseif nargin == 4 % the call was 'center_of_mass = get_center_of_mass(handle, reference_frame, function_name)' + ref_frame_handle = obj.handle_from_string_or_handle(ref_frame_handle_or_name); + + [return_code,~,center_of_mass,~,~] = obj.call_script_function(obj.DF_LUA_SCRIPT_API, obj.ST_CHILD, function_name, [obj_handle, ref_frame_handle], [], [], []); + if(return_code ~= 0) + formatSpec = 'Script function %s returned with error code %i.\n'; + fprintf(formatSpec, function_name, return_code); + error('Failed calling script function!'); + else % ensure cast to double + center_of_mass = double(center_of_mass); + end + else % the call was 'center_of_mass = get_center_of_mass(handle, reference_frame, function_name, obj_name)' + ref_frame_handle = obj.handle_from_string_or_handle(ref_frame_handle_or_name); + + [return_code,~,center_of_mass,~,~] = obj.call_script_function(obj_script_name, obj.ST_CHILD, function_name, [obj_handle, ref_frame_handle], [], [], []); + if(return_code ~= 0) + formatSpec = 'Script function %s returned with error code %i.\n'; + fprintf(formatSpec, function_name, return_code); + error('Failed calling script function!'); + else % ensure cast to double + center_of_mass = double(center_of_mass); + end + end + end + + function mass = get_mass(obj, obj_handle_or_name, function_name, obj_script_name) + % This method gets the mass of an object in the CoppeliaSim scene. + % + % Usage: + % Recommended: + % mass = get_mass(obj_handle_or_name); + % + % Advanced: + % mass = get_mass(obj_handle_or_name, function_name, obj_script_name); + % + % obj_handle_or_name: The object's handle or 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(obj_handle_or_name); + + if nargin == 2 % the call was 'mass = get_mass(handle)' + [return_code,~,mass,~,~] = obj.call_script_function(obj.DF_LUA_SCRIPT_API, obj.ST_CHILD, 'get_mass', obj_handle, [],[],[]); + if(return_code ~= 0) + formatSpec = 'Script function `get_mass` returned with error code %i.\n'; + fprintf(formatSpec, return_code); + error('Failed calling script function!'); + else % ensure cast to double + mass = double(mass); + end + elseif nargin == 3 % the call was 'mass = get_mass(handle, function_name)' + [return_code,~,mass,~,~] = obj.call_script_function(obj.DF_LUA_SCRIPT_API, obj.ST_CHILD, function_name, obj_handle, [],[],[]); + if(return_code ~= 0) + formatSpec = 'Script function %s returned with error code %i.\n'; + fprintf(formatSpec, function_name, return_code); + error('Failed calling script function!'); + else % ensure cast to double + mass = double(mass); + end + else % the call was 'mass = get_mass(handle, function_name, obj_name)' + [return_code,~,mass,~,~] = obj.call_script_function(obj_script_name, obj.ST_CHILD, function_name, obj_handle, [],[],[]); + if(return_code ~= 0) + formatSpec = 'Script function %s returned with error code %i.\n'; + fprintf(formatSpec, function_name, return_code); + error('Failed calling script function!'); + else % ensure cast to double + mass = double(mass); + end + end + end + + function inertia_tensor = get_inertia_matrix(obj, obj_handle_or_name, ref_frame_handle_or_name, 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(obj_handle_or_name); + % + % Advanced: + % inertia_tensor = get_inertia_matrix(obj_handle_or_name, ref_frame_handle_or_name, function_name, obj_script_name); + % + % obj_handle_or_name: 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_center_of_mass', 'my_DQRoboticsApiCommandServer'); + + obj_handle = obj.handle_from_string_or_handle(obj_handle_or_name); + + if nargin == 2 % the call was 'inertia_tensor = get_inertia_matrix(handle)' + [return_code,~,inertia_tensor,~,~] = obj.call_script_function(obj.DF_LUA_SCRIPT_API, obj.ST_CHILD, 'get_inertia', obj_handle, [], [], []); + if(return_code ~= 0) + formatSpec = 'Script function `get_inertia` returned with error code %i.\n'; + fprintf(formatSpec, return_code); + 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 + elseif nargin == 3 % the call was 'inertia_tensor = get_inertia_matrix(handle, reference_frame)' + ref_frame_handle = obj.handle_from_string_or_handle(ref_frame_handle_or_name); + + [return_code,~,inertia_tensor,~,~] = obj.call_script_function(obj.DF_LUA_SCRIPT_API, obj.ST_CHILD, 'get_inertia', [obj_handle, ref_frame_handle], [], [], []); + if(return_code ~= 0) + formatSpec = 'Script function `get_inertia` returned with error code %i.\n'; + fprintf(formatSpec, return_code); + 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 + elseif nargin == 4 % the call was 'inertia_tensor = get_inertia_matrix(handle, reference_frame, function_name)' + ref_frame_handle = obj.handle_from_string_or_handle(ref_frame_handle_or_name); + + [return_code,~,inertia_tensor,~,~] = obj.call_script_function(obj.DF_LUA_SCRIPT_API, obj.ST_CHILD, function_name, [obj_handle, ref_frame_handle], [], [], []); + if(return_code ~= 0) + formatSpec = 'Script function %s returned with error code %i.\n'; + fprintf(formatSpec, function_name, return_code); + 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 + else % the call was 'inertia_tensor = get_inertia_matrix(handle, reference_frame, function_name, obj_name)' + ref_frame_handle = obj.handle_from_string_or_handle(ref_frame_handle_or_name); + + [return_code,~,inertia_tensor,~,~] = obj.call_script_function(obj_script_name, obj.ST_CHILD, function_name, [obj_handle, ref_frame_handle], [], [], []); + if(return_code ~= 0) + formatSpec = 'Script function %s returned with error code %i.\n'; + fprintf(formatSpec, function_name, return_code); + 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 + end function set_joint_positions(obj,jointnames,joint_positions,opmode) % This method sets the joint positions of a robot in the CoppeliaSim scene. From 07538c5ad7254527c27e9b47b2a23ca04d4def28 Mon Sep 17 00:00:00 2001 From: Frederico Afonso Date: Fri, 16 Feb 2024 15:37:25 +0000 Subject: [PATCH 02/10] [DQ_VrepInterface] Modified method 'get_center_of_mass()' to return a DQ. Also simplified how methods 'get_center_of_mass()', 'get_mass()', and 'get_inertia_matrix()' handle their returns from 'call_script_function()'. --- interfaces/vrep/DQ_VrepInterface.m | 106 ++++++++--------------------- 1 file changed, 27 insertions(+), 79 deletions(-) diff --git a/interfaces/vrep/DQ_VrepInterface.m b/interfaces/vrep/DQ_VrepInterface.m index d7409c88..e974d87f 100644 --- a/interfaces/vrep/DQ_VrepInterface.m +++ b/interfaces/vrep/DQ_VrepInterface.m @@ -101,8 +101,8 @@ % % 3. Frederico Fernandes Afonso Silva (frederico.silva@ieee.org) % - Added the following methods: -% - call_script_function (see https://github.com/dqrobotics/matlab/pull/XXX) -% - get_center_of_mass (see https://github.com/dqrobotics/matlab/pull/XXX) +% - call_script_function() (see https://github.com/dqrobotics/matlab/pull/XXX) +% - get_center_of_mass() (see https://github.com/dqrobotics/matlab/pull/XXX) % - get_mass() (see https://github.com/dqrobotics/matlab/pull/XXX) % - get_inertia_matrix() (see https://github.com/dqrobotics/matlab/pull/XXX) % - Added the following properties: @@ -793,46 +793,27 @@ function set_object_pose(obj,objectname,pose,reference_frame,opmode) if nargin == 2 % the call was 'center_of_mass = get_center_of_mass(handle)' [return_code,~,center_of_mass,~,~] = obj.call_script_function(obj.DF_LUA_SCRIPT_API, obj.ST_CHILD, 'get_center_of_mass', obj_handle, [], [], []); - if(return_code ~= 0) - formatSpec = 'Script function `get_center_of_mass` returned with error code %i.\n'; - fprintf(formatSpec, return_code); - error('Failed calling script function!'); - else % ensure cast to double - center_of_mass = double(center_of_mass); - end elseif nargin == 3 % the call was 'center_of_mass = get_center_of_mass(handle, reference_frame)' ref_frame_handle = obj.handle_from_string_or_handle(ref_frame_handle_or_name); [return_code,~,center_of_mass,~,~] = obj.call_script_function(obj.DF_LUA_SCRIPT_API, obj.ST_CHILD, 'get_center_of_mass', [obj_handle, ref_frame_handle], [], [], []); - if(return_code ~= 0) - formatSpec = 'Script function `get_center_of_mass` returned with error code %i.\n'; - fprintf(formatSpec, return_code); - error('Failed calling script function!'); - else % ensure cast to double - center_of_mass = double(center_of_mass); - end elseif nargin == 4 % the call was 'center_of_mass = get_center_of_mass(handle, reference_frame, function_name)' ref_frame_handle = obj.handle_from_string_or_handle(ref_frame_handle_or_name); [return_code,~,center_of_mass,~,~] = obj.call_script_function(obj.DF_LUA_SCRIPT_API, obj.ST_CHILD, function_name, [obj_handle, ref_frame_handle], [], [], []); - if(return_code ~= 0) - formatSpec = 'Script function %s returned with error code %i.\n'; - fprintf(formatSpec, function_name, return_code); - error('Failed calling script function!'); - else % ensure cast to double - center_of_mass = double(center_of_mass); - end else % the call was 'center_of_mass = get_center_of_mass(handle, reference_frame, function_name, obj_name)' ref_frame_handle = obj.handle_from_string_or_handle(ref_frame_handle_or_name); [return_code,~,center_of_mass,~,~] = obj.call_script_function(obj_script_name, obj.ST_CHILD, function_name, [obj_handle, ref_frame_handle], [], [], []); - if(return_code ~= 0) - formatSpec = 'Script function %s returned with error code %i.\n'; - fprintf(formatSpec, function_name, return_code); - error('Failed calling script function!'); - else % ensure cast to double - center_of_mass = double(center_of_mass); - end + end + + if(return_code ~= 0) + formatSpec = 'Script function `get_center_of_mass` returned with error code %i.\n'; + fprintf(formatSpec, return_code); + 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 @@ -868,31 +849,18 @@ function set_object_pose(obj,objectname,pose,reference_frame,opmode) if nargin == 2 % the call was 'mass = get_mass(handle)' [return_code,~,mass,~,~] = obj.call_script_function(obj.DF_LUA_SCRIPT_API, obj.ST_CHILD, 'get_mass', obj_handle, [],[],[]); - if(return_code ~= 0) - formatSpec = 'Script function `get_mass` returned with error code %i.\n'; - fprintf(formatSpec, return_code); - error('Failed calling script function!'); - else % ensure cast to double - mass = double(mass); - end elseif nargin == 3 % the call was 'mass = get_mass(handle, function_name)' [return_code,~,mass,~,~] = obj.call_script_function(obj.DF_LUA_SCRIPT_API, obj.ST_CHILD, function_name, obj_handle, [],[],[]); - if(return_code ~= 0) - formatSpec = 'Script function %s returned with error code %i.\n'; - fprintf(formatSpec, function_name, return_code); - error('Failed calling script function!'); - else % ensure cast to double - mass = double(mass); - end else % the call was 'mass = get_mass(handle, function_name, obj_name)' [return_code,~,mass,~,~] = obj.call_script_function(obj_script_name, obj.ST_CHILD, function_name, obj_handle, [],[],[]); - if(return_code ~= 0) - formatSpec = 'Script function %s returned with error code %i.\n'; - fprintf(formatSpec, function_name, return_code); - error('Failed calling script function!'); - else % ensure cast to double - mass = double(mass); - end + end + + if(return_code ~= 0) + formatSpec = 'Script function %s returned with error code %i.\n'; + fprintf(formatSpec, function_name, return_code); + error('Failed calling script function!'); + else % ensure cast to double + mass = double(mass); end end @@ -932,46 +900,26 @@ function set_object_pose(obj,objectname,pose,reference_frame,opmode) if nargin == 2 % the call was 'inertia_tensor = get_inertia_matrix(handle)' [return_code,~,inertia_tensor,~,~] = obj.call_script_function(obj.DF_LUA_SCRIPT_API, obj.ST_CHILD, 'get_inertia', obj_handle, [], [], []); - if(return_code ~= 0) - formatSpec = 'Script function `get_inertia` returned with error code %i.\n'; - fprintf(formatSpec, return_code); - 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 elseif nargin == 3 % the call was 'inertia_tensor = get_inertia_matrix(handle, reference_frame)' ref_frame_handle = obj.handle_from_string_or_handle(ref_frame_handle_or_name); [return_code,~,inertia_tensor,~,~] = obj.call_script_function(obj.DF_LUA_SCRIPT_API, obj.ST_CHILD, 'get_inertia', [obj_handle, ref_frame_handle], [], [], []); - if(return_code ~= 0) - formatSpec = 'Script function `get_inertia` returned with error code %i.\n'; - fprintf(formatSpec, return_code); - 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 elseif nargin == 4 % the call was 'inertia_tensor = get_inertia_matrix(handle, reference_frame, function_name)' ref_frame_handle = obj.handle_from_string_or_handle(ref_frame_handle_or_name); [return_code,~,inertia_tensor,~,~] = obj.call_script_function(obj.DF_LUA_SCRIPT_API, obj.ST_CHILD, function_name, [obj_handle, ref_frame_handle], [], [], []); - if(return_code ~= 0) - formatSpec = 'Script function %s returned with error code %i.\n'; - fprintf(formatSpec, function_name, return_code); - 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 else % the call was 'inertia_tensor = get_inertia_matrix(handle, reference_frame, function_name, obj_name)' ref_frame_handle = obj.handle_from_string_or_handle(ref_frame_handle_or_name); [return_code,~,inertia_tensor,~,~] = obj.call_script_function(obj_script_name, obj.ST_CHILD, function_name, [obj_handle, ref_frame_handle], [], [], []); - if(return_code ~= 0) - formatSpec = 'Script function %s returned with error code %i.\n'; - fprintf(formatSpec, function_name, return_code); - 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 + + if(return_code ~= 0) + formatSpec = 'Script function %s returned with error code %i.\n'; + fprintf(formatSpec, function_name, return_code); + 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 From a5c8fe35e66ded6421b115f4a944b776c353396c Mon Sep 17 00:00:00 2001 From: Frederico Afonso Date: Fri, 16 Feb 2024 17:35:28 +0000 Subject: [PATCH 03/10] [DQ_VrepInterface] Updated documentation of the modifications. --- interfaces/vrep/DQ_VrepInterface.m | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/interfaces/vrep/DQ_VrepInterface.m b/interfaces/vrep/DQ_VrepInterface.m index e974d87f..785ddb5a 100644 --- a/interfaces/vrep/DQ_VrepInterface.m +++ b/interfaces/vrep/DQ_VrepInterface.m @@ -101,13 +101,13 @@ % % 3. Frederico Fernandes Afonso Silva (frederico.silva@ieee.org) % - Added the following methods: -% - call_script_function() (see https://github.com/dqrobotics/matlab/pull/XXX) -% - get_center_of_mass() (see https://github.com/dqrobotics/matlab/pull/XXX) -% - get_mass() (see https://github.com/dqrobotics/matlab/pull/XXX) -% - get_inertia_matrix() (see https://github.com/dqrobotics/matlab/pull/XXX) +% - 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/XXX) -% - ST_CHILD (see https://github.com/dqrobotics/matlab/pull/XXX) +% - DF_LUA_SCRIPT_API (see https://github.com/dqrobotics/matlab/pull/109) +% - ST_CHILD (see https://github.com/dqrobotics/matlab/pull/109) % - Altered the following properties from 'private' to 'protected' % (see discussions in https://github.com/dqrobotics/matlab/pull/101 % to further details): From 484d95612e5c3a18d44e38c0a3bd69c21c8a864d Mon Sep 17 00:00:00 2001 From: Frederico Afonso Date: Thu, 29 Aug 2024 11:01:08 +0100 Subject: [PATCH 04/10] [DQ_VrepInterface.m] Scrambled 'call_script_function()' inputs and made it private to match the C++ implementation. --- interfaces/vrep/DQ_VrepInterface.m | 98 ++++++++++++++---------------- 1 file changed, 46 insertions(+), 52 deletions(-) diff --git a/interfaces/vrep/DQ_VrepInterface.m b/interfaces/vrep/DQ_VrepInterface.m index 1995834c..15747450 100644 --- a/interfaces/vrep/DQ_VrepInterface.m +++ b/interfaces/vrep/DQ_VrepInterface.m @@ -152,62 +152,26 @@ 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_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 [return_code,output_ints,output_doubles,output_strings,retBuffer] = call_script_function(obj,obj_name,script_type,function_name,... - input_ints,input_floats,input_strings,input_buffer,opmode) - % This method calls a LUA script function in V-REP. + 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,retBuffer] = call_script_function(obj_name, script_type, function_name, input_ints, input_floats, input_strings, input_buffer) + % [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,retBuffer] = call_script_function(obj_name, script_type, function_name, input_ints, input_floats, input_strings, input_buffer, opmode) + % [return_code, output_ints, output_doubles, output_strings] = call_script_function(function_name, obj_name, input_ints, input_floats, input_strings, script_type, opmode) % - % obj_name: The name of the object where the script is - % attached to. + % 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 % - % function_name: The name of the script function to - % call in the specified script. - % input_ints: The input integer values. - % input_floats: The input floating-point values. - % input_strings: The input strings. - % input_buffer: The input buffer. % (optional) opmode: The operation mode. If not % specified, the opmode will be set automatically. % @@ -224,27 +188,57 @@ % input_ints = []; % input_floats = []; % input_strings = []; - % input_buffer = []; % % % Recommended: - % [rtn, output_ints, output_doubles, output_strings, retBuffer] = call_script_function('DQRoboticsApiCommandServer', ST_CHILD, 'my_function_name', input_ints, input_floats, input_strings, input_buffer) + % [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, retBuffer] = call_script_function('DQRoboticsApiCommandServer', ST_CHILD, 'my_function_name', input_ints, input_floats, input_strings, input_buffer, OP_BLOCKING) + % [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 == 8 - [return_code,output_ints,output_floats,output_strings,retBuffer] = obj.vrep.simxCallScriptFunction(obj.clientID, obj_name, ... - script_type, function_name, input_ints, input_floats , input_strings, input_buffer, obj.OP_BLOCKING); + [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,retBuffer] = obj.vrep.simxCallScriptFunction(obj.clientID, obj_name, ... - script_type, function_name, input_ints, input_floats , input_strings, input_buffer, opmode); + [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. From e2b530c33c66ac4a7c38573c46e4c3048e8e0f10 Mon Sep 17 00:00:00 2001 From: Frederico Afonso Date: Thu, 29 Aug 2024 11:56:16 +0100 Subject: [PATCH 05/10] [DQ_VrepInterface.m] Refactored 'get_center_of_mass()' to have the same behaviour of the C++ implementation. Also added the properties 'BODY_FRAME' and 'ABSOLUTE_FRAME' to match the C++ implementation. --- interfaces/vrep/DQ_VrepInterface.m | 72 ++++++++++++++++++------------ 1 file changed, 43 insertions(+), 29 deletions(-) diff --git a/interfaces/vrep/DQ_VrepInterface.m b/interfaces/vrep/DQ_VrepInterface.m index 15747450..1efeca81 100644 --- a/interfaces/vrep/DQ_VrepInterface.m +++ b/interfaces/vrep/DQ_VrepInterface.m @@ -110,6 +110,8 @@ % - 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) @@ -135,7 +137,7 @@ end properties (Constant) - % Constant that denotes DQ Robotic's default LUA script API with V-VREP + % 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; @@ -147,8 +149,12 @@ 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 V-VREP's remote API child script type + % 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) @@ -197,7 +203,7 @@ % If the user does not specify the opmode, it is chosen as % OP_BLOCKING as specified by the remote API documentation. - if nargin == 8 + 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); @@ -741,27 +747,23 @@ function set_object_pose(obj,objectname,pose,reference_frame,opmode) end end - function center_of_mass = get_center_of_mass(obj, obj_handle_or_name, ref_frame_handle_or_name, function_name, obj_script_name) + 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(obj_handle_or_name); + % center_of_mass = get_center_of_mass(objectname); % % Advanced: - % center_of_mass = get_center_of_mass(obj_handle_or_name, ref_frame_handle_or_name, function_name, obj_script_name); + % center_of_mass = get_center_of_mass(objectname, ref_frame_handle_or_name, function_name, obj_script_name); % - % obj_handle_or_name: The object's handle or 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. + % 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') + % (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 @@ -771,24 +773,36 @@ function set_object_pose(obj,objectname,pose,reference_frame,opmode) % center_of_mass = get_center_of_mass('/Jaco/Jaco_link2'); % % % For advanced usage: - % center_of_mass = get_center_of_mass('/Jaco/Jaco_link2', '/Jaco/Jaco_joint2', 'my_get_center_of_mass', 'my_DQRoboticsApiCommandServer'); + % 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(obj_handle_or_name); + obj_handle = obj.handle_from_string_or_handle(objectname); - if nargin == 2 % the call was 'center_of_mass = get_center_of_mass(handle)' - [return_code,~,center_of_mass,~,~] = obj.call_script_function(obj.DF_LUA_SCRIPT_API, obj.ST_CHILD, 'get_center_of_mass', obj_handle, [], [], []); - elseif nargin == 3 % the call was 'center_of_mass = get_center_of_mass(handle, reference_frame)' - ref_frame_handle = obj.handle_from_string_or_handle(ref_frame_handle_or_name); + 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(reference_frame == obj.ABSOLUTE_FRAME) + ref_frame_handle = -1; + else + ref_frame_handle = obj.handle_from_string_or_handle(reference_frame); + end - [return_code,~,center_of_mass,~,~] = obj.call_script_function(obj.DF_LUA_SCRIPT_API, obj.ST_CHILD, 'get_center_of_mass', [obj_handle, ref_frame_handle], [], [], []); - elseif nargin == 4 % the call was 'center_of_mass = get_center_of_mass(handle, reference_frame, function_name)' - ref_frame_handle = obj.handle_from_string_or_handle(ref_frame_handle_or_name); + [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); + elseif nargin == 4 % the call was: center_of_mass = get_center_of_mass(objectname, reference_frame, function_name) + if(reference_frame == obj.ABSOLUTE_FRAME) + ref_frame_handle = -1; + else + ref_frame_handle = obj.handle_from_string_or_handle(reference_frame); + end - [return_code,~,center_of_mass,~,~] = obj.call_script_function(obj.DF_LUA_SCRIPT_API, obj.ST_CHILD, function_name, [obj_handle, ref_frame_handle], [], [], []); - else % the call was 'center_of_mass = get_center_of_mass(handle, reference_frame, function_name, obj_name)' - ref_frame_handle = obj.handle_from_string_or_handle(ref_frame_handle_or_name); + [return_code, ~, center_of_mass, ~] = obj.call_script_function(function_name, obj.DF_LUA_SCRIPT_API, [obj_handle, ref_frame_handle], [], [], obj.ST_CHILD); + else % the call was: center_of_mass = get_center_of_mass(objectname, reference_frame, function_name, obj_name) + if(reference_frame == obj.ABSOLUTE_FRAME) + ref_frame_handle = -1; + else + ref_frame_handle = obj.handle_from_string_or_handle(reference_frame); + end - [return_code,~,center_of_mass,~,~] = obj.call_script_function(obj_script_name, obj.ST_CHILD, function_name, [obj_handle, ref_frame_handle], [], [], []); + [return_code, ~, center_of_mass, ~] = obj.call_script_function(function_name, obj_script_name, [obj_handle, ref_frame_handle], [], [], obj.ST_CHILD); end if(return_code ~= 0) From 0f01ecbe55247f03d5826da471740e68cc129672 Mon Sep 17 00:00:00 2001 From: Frederico Afonso Date: Thu, 29 Aug 2024 12:06:26 +0100 Subject: [PATCH 06/10] [DQ_VrepInterface.m] Refactored 'get_mass()' to have the same behaviour of the C++ implementation. --- interfaces/vrep/DQ_VrepInterface.m | 30 ++++++++++++++---------------- 1 file changed, 14 insertions(+), 16 deletions(-) diff --git a/interfaces/vrep/DQ_VrepInterface.m b/interfaces/vrep/DQ_VrepInterface.m index 1efeca81..b815db37 100644 --- a/interfaces/vrep/DQ_VrepInterface.m +++ b/interfaces/vrep/DQ_VrepInterface.m @@ -815,23 +815,21 @@ function set_object_pose(obj,objectname,pose,reference_frame,opmode) end end - function mass = get_mass(obj, obj_handle_or_name, function_name, obj_script_name) + 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(obj_handle_or_name); + % mass = get_mass(objectname); % % Advanced: - % mass = get_mass(obj_handle_or_name, function_name, obj_script_name); + % mass = get_mass(objectname, function_name, obj_script_name); % - % obj_handle_or_name: The object's handle or name. - % (optional) function_name: The name of the script - % function to call in the specified script. + % 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') + % (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 @@ -843,14 +841,14 @@ function set_object_pose(obj,objectname,pose,reference_frame,opmode) % % For advanced usage: % mass = get_mass('/Jaco/Jaco_link2', 'my_get_center_of_mass', 'my_DQRoboticsApiCommandServer'); - obj_handle = obj.handle_from_string_or_handle(obj_handle_or_name); + obj_handle = obj.handle_from_string_or_handle(objectname); - if nargin == 2 % the call was 'mass = get_mass(handle)' - [return_code,~,mass,~,~] = obj.call_script_function(obj.DF_LUA_SCRIPT_API, obj.ST_CHILD, 'get_mass', obj_handle, [],[],[]); - elseif nargin == 3 % the call was 'mass = get_mass(handle, function_name)' - [return_code,~,mass,~,~] = obj.call_script_function(obj.DF_LUA_SCRIPT_API, obj.ST_CHILD, function_name, obj_handle, [],[],[]); - else % the call was 'mass = get_mass(handle, function_name, obj_name)' - [return_code,~,mass,~,~] = obj.call_script_function(obj_script_name, obj.ST_CHILD, function_name, obj_handle, [],[],[]); + 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) From 71b9f10c9bbda15ca1fe57755ff10cf6bf006cda Mon Sep 17 00:00:00 2001 From: Frederico Afonso Date: Thu, 29 Aug 2024 14:03:46 +0100 Subject: [PATCH 07/10] [DQ_VrepInterface.m] Fixed the way 'get_center_of_mass()' uses 'BODY_FRAME' to match the C++ implementation. --- interfaces/vrep/DQ_VrepInterface.m | 15 ++++++--------- 1 file changed, 6 insertions(+), 9 deletions(-) diff --git a/interfaces/vrep/DQ_VrepInterface.m b/interfaces/vrep/DQ_VrepInterface.m index b815db37..2aad66e8 100644 --- a/interfaces/vrep/DQ_VrepInterface.m +++ b/interfaces/vrep/DQ_VrepInterface.m @@ -781,28 +781,25 @@ function set_object_pose(obj,objectname,pose,reference_frame,opmode) [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(reference_frame == obj.ABSOLUTE_FRAME) - ref_frame_handle = -1; + [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); 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 - - [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); elseif nargin == 4 % the call was: center_of_mass = get_center_of_mass(objectname, reference_frame, function_name) if(reference_frame == obj.ABSOLUTE_FRAME) - ref_frame_handle = -1; + [return_code, ~, center_of_mass, ~] = obj.call_script_function(function_name, obj.DF_LUA_SCRIPT_API, obj_handle, [], obj.ABSOLUTE_FRAME, 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 - - [return_code, ~, center_of_mass, ~] = obj.call_script_function(function_name, obj.DF_LUA_SCRIPT_API, [obj_handle, ref_frame_handle], [], [], obj.ST_CHILD); else % the call was: center_of_mass = get_center_of_mass(objectname, reference_frame, function_name, obj_name) if(reference_frame == obj.ABSOLUTE_FRAME) - ref_frame_handle = -1; + [return_code, ~, center_of_mass, ~] = obj.call_script_function(function_name, obj_script_name, obj_handle, [], obj.ABSOLUTE_FRAME, 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 - - [return_code, ~, center_of_mass, ~] = obj.call_script_function(function_name, obj_script_name, [obj_handle, ref_frame_handle], [], [], obj.ST_CHILD); end if(return_code ~= 0) From 241d3dc80a9d22877ca181ac81b7f2ee990008f1 Mon Sep 17 00:00:00 2001 From: Frederico Afonso Date: Thu, 29 Aug 2024 14:41:49 +0100 Subject: [PATCH 08/10] [DQ_VrepInterface.m] Refactored 'get_center_of_mass()' to have the same behaviour of the C++ implementation. --- interfaces/vrep/DQ_VrepInterface.m | 69 ++++++++++++++++-------------- 1 file changed, 37 insertions(+), 32 deletions(-) diff --git a/interfaces/vrep/DQ_VrepInterface.m b/interfaces/vrep/DQ_VrepInterface.m index 2aad66e8..ef88bd9b 100644 --- a/interfaces/vrep/DQ_VrepInterface.m +++ b/interfaces/vrep/DQ_VrepInterface.m @@ -824,9 +824,9 @@ function set_object_pose(obj,objectname,pose,reference_frame,opmode) % % objectname: The object's name. % (optional) function_name: The name of the script function to call in the specified script. - % (Default: "get_mass") + % (Default: "get_mass") % (optional) obj_script_name: The name of the object where the script is attached to. - % (Default: 'DQRoboticsApiCommandServer') + % (Default: 'DQRoboticsApiCommandServer') % % % Check this link for more details: https://www.coppeliarobotics.com/helpFiles/en/regularApi/simGetShapeMass.htm @@ -857,27 +857,23 @@ function set_object_pose(obj,objectname,pose,reference_frame,opmode) end end - function inertia_tensor = get_inertia_matrix(obj, obj_handle_or_name, ref_frame_handle_or_name, function_name, obj_script_name) + 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(obj_handle_or_name); + % inertia_tensor = get_inertia_matrix(objectname); % % Advanced: - % inertia_tensor = get_inertia_matrix(obj_handle_or_name, ref_frame_handle_or_name, function_name, obj_script_name); + % inertia_tensor = get_inertia_matrix(objectname, ref_frame_handle_or_name, function_name, obj_script_name); % - % obj_handle_or_name: 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') + % 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 @@ -887,24 +883,33 @@ function set_object_pose(obj,objectname,pose,reference_frame,opmode) % inertia_tensor = get_inertia_matrix('/Jaco/Jaco_link2'); % % % For advanced usage: - % inertia_tensor = get_inertia_matrix('/Jaco/Jaco_link2', '/Jaco/Jaco_joint2', 'my_get_center_of_mass', 'my_DQRoboticsApiCommandServer'); - - obj_handle = obj.handle_from_string_or_handle(obj_handle_or_name); + % inertia_tensor = get_inertia_matrix('/Jaco/Jaco_link2', '/Jaco/Jaco_joint2', 'my_get_inertia_matrix', 'my_DQRoboticsApiCommandServer'); - if nargin == 2 % the call was 'inertia_tensor = get_inertia_matrix(handle)' - [return_code,~,inertia_tensor,~,~] = obj.call_script_function(obj.DF_LUA_SCRIPT_API, obj.ST_CHILD, 'get_inertia', obj_handle, [], [], []); - elseif nargin == 3 % the call was 'inertia_tensor = get_inertia_matrix(handle, reference_frame)' - ref_frame_handle = obj.handle_from_string_or_handle(ref_frame_handle_or_name); - - [return_code,~,inertia_tensor,~,~] = obj.call_script_function(obj.DF_LUA_SCRIPT_API, obj.ST_CHILD, 'get_inertia', [obj_handle, ref_frame_handle], [], [], []); - elseif nargin == 4 % the call was 'inertia_tensor = get_inertia_matrix(handle, reference_frame, function_name)' - ref_frame_handle = obj.handle_from_string_or_handle(ref_frame_handle_or_name); + obj_handle = obj.handle_from_string_or_handle(objectname); - [return_code,~,inertia_tensor,~,~] = obj.call_script_function(obj.DF_LUA_SCRIPT_API, obj.ST_CHILD, function_name, [obj_handle, ref_frame_handle], [], [], []); - else % the call was 'inertia_tensor = get_inertia_matrix(handle, reference_frame, function_name, obj_name)' - ref_frame_handle = obj.handle_from_string_or_handle(ref_frame_handle_or_name); - - [return_code,~,inertia_tensor,~,~] = obj.call_script_function(obj_script_name, obj.ST_CHILD, function_name, [obj_handle, ref_frame_handle], [], [], []); + 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(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); + 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(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); + 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(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); + 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) From c95ecf5f70f66024dd655262aadcc1eff71210cb Mon Sep 17 00:00:00 2001 From: Frederico Afonso Date: Thu, 29 Aug 2024 14:58:50 +0100 Subject: [PATCH 09/10] [DQ_VrepInterface.m] Fixed error messages of 'get_center_of_mass()', 'get_mass()', and 'get_inertia_matrix()'. --- interfaces/vrep/DQ_VrepInterface.m | 27 +++++++++++++++++++++------ 1 file changed, 21 insertions(+), 6 deletions(-) diff --git a/interfaces/vrep/DQ_VrepInterface.m b/interfaces/vrep/DQ_VrepInterface.m index ef88bd9b..35ec3892 100644 --- a/interfaces/vrep/DQ_VrepInterface.m +++ b/interfaces/vrep/DQ_VrepInterface.m @@ -803,8 +803,13 @@ function set_object_pose(obj,objectname,pose,reference_frame,opmode) end if(return_code ~= 0) - formatSpec = 'Script function `get_center_of_mass` returned with error code %i.\n'; - fprintf(formatSpec, return_code); + 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); @@ -849,8 +854,13 @@ function set_object_pose(obj,objectname,pose,reference_frame,opmode) end if(return_code ~= 0) - formatSpec = 'Script function %s returned with error code %i.\n'; - fprintf(formatSpec, function_name, return_code); + 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); @@ -913,8 +923,13 @@ function set_object_pose(obj,objectname,pose,reference_frame,opmode) end if(return_code ~= 0) - formatSpec = 'Script function %s returned with error code %i.\n'; - fprintf(formatSpec, function_name, return_code); + 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)); From d29125d7574a9be62231eb737fe5a1bc0b1435ca Mon Sep 17 00:00:00 2001 From: Frederico Afonso Date: Fri, 27 Sep 2024 14:40:52 +0100 Subject: [PATCH 10/10] [DQ_VrepInterface.m] Fixed bug when trying to use 'BODY_FRAME' in 'get_inertia_matrix()' and 'get_center_of_mass()' methods. --- interfaces/vrep/DQ_VrepInterface.m | 24 ++++++++++++++++++------ 1 file changed, 18 insertions(+), 6 deletions(-) diff --git a/interfaces/vrep/DQ_VrepInterface.m b/interfaces/vrep/DQ_VrepInterface.m index 35ec3892..036d09b3 100644 --- a/interfaces/vrep/DQ_VrepInterface.m +++ b/interfaces/vrep/DQ_VrepInterface.m @@ -780,22 +780,28 @@ function set_object_pose(obj,objectname,pose,reference_frame,opmode) 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(reference_frame == obj.ABSOLUTE_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(reference_frame == obj.ABSOLUTE_FRAME) + 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(reference_frame == obj.ABSOLUTE_FRAME) + 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); @@ -900,22 +906,28 @@ function set_object_pose(obj,objectname,pose,reference_frame,opmode) 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(reference_frame == obj.ABSOLUTE_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(reference_frame == obj.ABSOLUTE_FRAME) + 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(reference_frame == obj.ABSOLUTE_FRAME) + 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);