From ea86d9b6cc2254ffda6f2e9664861467f763bd62 Mon Sep 17 00:00:00 2001 From: Frederico Afonso Date: Mon, 15 May 2023 14:31:51 +0200 Subject: [PATCH 01/12] Added the class FrankaEmikaPandaRobot(). --- robots/FrankaEmikaPandaRobot.m | 55 ++++++++++++++++++++++++++++++++++ 1 file changed, 55 insertions(+) create mode 100644 robots/FrankaEmikaPandaRobot.m diff --git a/robots/FrankaEmikaPandaRobot.m b/robots/FrankaEmikaPandaRobot.m new file mode 100644 index 00000000..a3ae4a92 --- /dev/null +++ b/robots/FrankaEmikaPandaRobot.m @@ -0,0 +1,55 @@ +% panda = FrankaEmikaPandaRobot.kinematics() returns a DQ_kinematics object +% using the modified Denavit-Hartenberg parameters of the Franka Emika +% Panda robot + +% (C) Copyright 2015-2022 DQ Robotics Developers +% +% This file is part of DQ Robotics. +% +% DQ Robotics is free software: you can redistribute it and/or modify +% it under the terms of the GNU Lesser General Public License as published by +% the Free Software Foundation, either version 3 of the License, or +% (at your option) any later version. +% +% DQ Robotics is distributed in the hope that it will be useful, +% but WITHOUT ANY WARRANTY; without even the implied warranty of +% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +% GNU Lesser General Public License for more details. +% +% You should have received a copy of the GNU Lesser General Public License +% along with DQ Robotics. If not, see . +% +% DQ Robotics website: dqrobotics.github.io +% +% Contributors to this file: +% Frederico Fernandes Afonso Silva - frederico.silva@ieee.org + +classdef FrankaEmikaPandaRobot + methods (Static) + function ret = kinematics() + % Modified D-H of Franka Emika Panda + FEp_DH_theta = [0, 0, 0, 0, 0, 0, 0]; + FEp_DH_d = [0.333, 0, 3.16e-1, 0, 3.84e-1, 0, 0]; + FEp_DH_a = [0, 0, 0, 8.25e-2, -8.25e-2, 0, 8.8e-2]; + FEp_DH_alpha = [0, -pi/2, pi/2, pi/2, -pi/2, pi/2, pi/2]; + FEp_DH_type = repmat(DQ_SerialManipulatorDH.JOINT_ROTATIONAL,1,7); + + FEp_DH_matrix = [FEp_DH_theta; + FEp_DH_d; + FEp_DH_a; + FEp_DH_alpha; + FEp_DH_type]; + + ret = DQ_SerialManipulatorMDH(FEp_DH_matrix); + + % Set the base's reference frame + xb = 1 + DQ.E*0.5*DQ([0, 0.0413, 0, 0]); + ret.set_reference_frame(xb); + ret.set_base_frame(xb); + + % Set the end-effector + xe = 1 + DQ.E*0.5*DQ.k*1.07e-1; + ret.set_effector(xe); + end + end +end \ No newline at end of file From 52af08de852b1078a3cbc4708c79d2913a03c501 Mon Sep 17 00:00:00 2001 From: Frederico Afonso Date: Sat, 27 May 2023 10:27:04 +0200 Subject: [PATCH 02/12] Added the method 'get_force_sensor_readings' to DQ_VrepInterface(). It allows reading the force/torques of a force sensor in V-REP/CoppeliaSim. --- interfaces/vrep/DQ_VrepInterface.m | 30 ++++++++++++++++++++++++++++++ 1 file changed, 30 insertions(+) diff --git a/interfaces/vrep/DQ_VrepInterface.m b/interfaces/vrep/DQ_VrepInterface.m index c7b8dc78..8f346c52 100644 --- a/interfaces/vrep/DQ_VrepInterface.m +++ b/interfaces/vrep/DQ_VrepInterface.m @@ -679,6 +679,36 @@ function set_object_pose(obj,objectname,pose,reference_frame,opmode) opmode); end end + + + %% Get Force Sensor Readings + function [force_vec, torque_vec, return_code, state_sensor] = get_force_sensor_readings(obj,handle,opmode) + %% Get the readings of a force sensor in V-REP. For joints that are in 'Torque/force mode' in V-REP + %% >> force_sensor_name = 'Sensor_name_in_VREP'; + %% >> [force_vec, torque_vec] = vi.get_force_sensor_readings(force_sensor_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 + element = obj.element_from_string(handle); + force_sensor_handle = obj.handle_from_string_or_handle(handle); + if nargin <= 2 + if(~element.state_from_function_signature('get_force_sensor_readings')) + [~, ~, tmp_f, tmp_t] = obj.vrep.simxReadForceSensor(obj.clientID,element.handle,obj.OP_STREAMING); + return_code = 1; + while(return_code == 1) + [return_code, state_sensor, tmp_f, tmp_t] = obj.vrep.simxReadForceSensor(obj.clientID,force_sensor_handle,obj.OP_BUFFER); + end + else + [return_code, state_sensor, tmp_f, tmp_t] = obj.vrep.simxReadForceSensor(obj.clientID,force_sensor_handle,obj.OP_BUFFER); + end + else + [return_code, state_sensor, tmp_f, tmp_t] = obj.vrep.simxReadForceSensor(obj.clientID,force_sensor_handle,opmode); + end + force_vec = double(tmp_f); + torque_vec = double(tmp_t); + end function set_joint_positions(obj,jointnames,joint_positions,opmode) From cfaceaa791cf45efced3549696b10dcd10e9790f Mon Sep 17 00:00:00 2001 From: Frederico Afonso Date: Sat, 8 Jul 2023 12:31:51 +0200 Subject: [PATCH 03/12] Added the methods 'get_joint_torques()' and 'set_joint_torques()' to the class DQ_VrepInterface. --- interfaces/vrep/DQ_VrepInterface.m | 131 +++++++++++++++++++++-------- 1 file changed, 95 insertions(+), 36 deletions(-) diff --git a/interfaces/vrep/DQ_VrepInterface.m b/interfaces/vrep/DQ_VrepInterface.m index 8f346c52..61a4a597 100644 --- a/interfaces/vrep/DQ_VrepInterface.m +++ b/interfaces/vrep/DQ_VrepInterface.m @@ -57,6 +57,8 @@ % 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) % get_handle - Get the handle of a V-REP object @@ -678,38 +680,7 @@ function set_object_pose(obj,objectname,pose,reference_frame,opmode) obj.handle_from_string_or_handle(relative_to_handle),... opmode); end - end - - - %% Get Force Sensor Readings - function [force_vec, torque_vec, return_code, state_sensor] = get_force_sensor_readings(obj,handle,opmode) - %% Get the readings of a force sensor in V-REP. For joints that are in 'Torque/force mode' in V-REP - %% >> force_sensor_name = 'Sensor_name_in_VREP'; - %% >> [force_vec, torque_vec] = vi.get_force_sensor_readings(force_sensor_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 - element = obj.element_from_string(handle); - force_sensor_handle = obj.handle_from_string_or_handle(handle); - if nargin <= 2 - if(~element.state_from_function_signature('get_force_sensor_readings')) - [~, ~, tmp_f, tmp_t] = obj.vrep.simxReadForceSensor(obj.clientID,element.handle,obj.OP_STREAMING); - return_code = 1; - while(return_code == 1) - [return_code, state_sensor, tmp_f, tmp_t] = obj.vrep.simxReadForceSensor(obj.clientID,force_sensor_handle,obj.OP_BUFFER); - end - else - [return_code, state_sensor, tmp_f, tmp_t] = obj.vrep.simxReadForceSensor(obj.clientID,force_sensor_handle,obj.OP_BUFFER); - end - else - [return_code, state_sensor, tmp_f, tmp_t] = obj.vrep.simxReadForceSensor(obj.clientID,force_sensor_handle,opmode); - end - force_vec = double(tmp_f); - torque_vec = double(tmp_t); - end - + end function set_joint_positions(obj,jointnames,joint_positions,opmode) % This method sets the joint positions of a robot in the CoppeliaSim scene. @@ -928,8 +899,7 @@ function set_joint_target_positions(obj,jointnames,joint_target_positions,opmode joint_positions = thetas; end - - function joint_velocities = get_joint_velocities(obj,jointnames,opmode) + function joint_velocities = get_joint_velocities(obj,jointnames,opmode) % This method gets the joint velocities of a robot in the CoppeliaSim scene. % Usage: % Recommended: @@ -960,7 +930,7 @@ function set_joint_target_positions(obj,jointnames,joint_target_positions,opmode % % % 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 @@ -1064,7 +1034,96 @@ function set_joint_target_velocities(obj,jointnames,joint_target_velocities,opmo opmode); end end - end + end + + %% Get Joint Torques + function [joint_torques,return_code] = get_joint_torques(obj,handles,opmode) + %% Get the joint torques of a robot in V-REP. For joints that are in 'Torque/force mode' in V-REP + %% >> joint_names = {'redundantRob_joint1','redundantRob_joint2','redundantRob_joint3','redundantRob_joint4','redundantRob_joint5','redundantRob_joint6','redundantRob_joint7'}; + %% >> vi.get_joint_torques(joint_names); + + joint_torques = 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_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 + [return_code,tmp] = obj.vrep.simxGetJointForce(obj.clientID, element.handle, obj.OP_BUFFER); + end + else + [return_code,tmp] = obj.vrep.simxGetJointForce(obj.clientID, ... + obj.handle_from_string_or_handle(handles{joint_index}), opmode); + end + joint_torques(joint_index) = double(-tmp); % V-REP returns joint torques with an inverse sign + end + end + + %% Set Joint Torques + function set_joint_torques(obj,joint_names,torques,opmode) + %% Set the joint torques of a robot in V-REP. For joints that are in 'Torque/force mode' in V-REP + %% >> joint_names = {'redundantRob_joint1','redundantRob_joint2','redundantRob_joint3','redundantRob_joint4','redundantRob_joint5','redundantRob_joint6','redundantRob_joint7'}; + %% >> vi.set_joint_torques(joint_names,[0 pi/2 0 pi/2 0 pi/2 0]); + + if nargin == 3 + % The recommended mode is OP_ONESHOT + for joint_index=1:length(joint_names) + obj.vrep.simxSetJointTargetVelocity(obj.clientID, obj.handle_from_string_or_handle(joint_names{joint_index}), ... + sign(torques(joint_index))*10e10, obj.OP_ONESHOT); + obj.vrep.simxSetJointForce(obj.clientID, obj.handle_from_string_or_handle(joint_names{joint_index}), ... + abs(torques(joint_index)), obj.OP_ONESHOT); + end + else + for joint_index=1:length(joint_names) + obj.vrep.simxSetJointTargetVelocity(obj.clientID, obj.handle_from_string_or_handle(joint_names{joint_index}),... + sign(torques(joint_index))*10e10, opmode); + obj.vrep.simxSetJointForce(obj.clientID,obj.handle_from_string_or_handle(joint_names{joint_index}),abs(torques(joint_index)),... + opmode); + end + end + end + + %% Get Force Sensor Readings + function [force_vec, torque_vec, return_code, state_sensor] = get_force_sensor_readings(obj,handle,opmode) + %% Get the readings of a force sensor in V-REP. For joints that are in 'Torque/force mode' in V-REP + %% >> force_sensor_name = 'Sensor_name_in_VREP'; + %% >> [force_vec, torque_vec] = vi.get_force_sensor_readings(force_sensor_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 + element = obj.element_from_string(handle); + force_sensor_handle = obj.handle_from_string_or_handle(handle); + if nargin <= 2 + if(~element.state_from_function_signature('get_force_sensor_readings')) + [~, ~, tmp_f, tmp_t] = obj.vrep.simxReadForceSensor(obj.clientID,element.handle,obj.OP_STREAMING); + return_code = 1; + while(return_code == 1) + [return_code, state_sensor, tmp_f, tmp_t] = obj.vrep.simxReadForceSensor(obj.clientID,force_sensor_handle,obj.OP_BUFFER); + end + else + [return_code, state_sensor, tmp_f, tmp_t] = obj.vrep.simxReadForceSensor(obj.clientID,force_sensor_handle,obj.OP_BUFFER); + end + else + [return_code, state_sensor, tmp_f, tmp_t] = obj.vrep.simxReadForceSensor(obj.clientID,force_sensor_handle,opmode); + end + force_vec = double(tmp_f); + torque_vec = double(tmp_t); + end + end end From 43ea9dc5beefa1dce0323199011ee8e0a2d33252 Mon Sep 17 00:00:00 2001 From: Frederico Afonso Date: Mon, 10 Jul 2023 11:35:01 +0200 Subject: [PATCH 04/12] Added the class 'FrankaEmikaPandaVrepRobot'. --- .../vrep/robots/FrankaEmikaPandaVrepRobot.m | 158 ++++++++++++++++++ 1 file changed, 158 insertions(+) create mode 100644 interfaces/vrep/robots/FrankaEmikaPandaVrepRobot.m diff --git a/interfaces/vrep/robots/FrankaEmikaPandaVrepRobot.m b/interfaces/vrep/robots/FrankaEmikaPandaVrepRobot.m new file mode 100644 index 00000000..5c4e0b3a --- /dev/null +++ b/interfaces/vrep/robots/FrankaEmikaPandaVrepRobot.m @@ -0,0 +1,158 @@ +% CLASS FrankaEmikaPandaVrepRobot - Concrete class to interface with the +% "FrankaEmikaPanda" robot in VREP. +% +% Usage: +% 1) Drag-and-drop a "FrankaEmikaPanda" robot to a VREP scene. +% 2) Run +% >> vi = DQ_VrepInterface(); +% >> vi.connect('127.0.0.1',19997); +% >> vrep_robot = FrankaEmikaPandaVrepRobot('Franka', vi); +% >> vi.start_simulation(); +% >> robot.get_q_from_vrep(); +% >> 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 "FrankaEmikaPanda#0", a third robot, +% "FrankaEmikaPanda#1", and so on. +% +% LBR4pVrepRobot Methods: +% send_q_to_vrep - Sends the joint configurations to VREP +% get_q_from_vrep - Obtains the joint configurations from VREP +% get_q_dot_from_vrep - Obtains the robot generalized velocities from V-REP. +% send_tau_to_vrep - Sends the joint torques to VREP +% get_tau_from_vrep - Obtains the joint torques from VREP +% kinematics - Obtains the DQ_Kinematics implementation of this robot + +% (C) Copyright 2011-2023 DQ Robotics Developers +% +% This file is part of DQ Robotics. +% +% DQ Robotics is free software: you can redistribute it and/or modify +% it under the terms of the GNU Lesser General Public License as published by +% the Free Software Foundation, either version 3 of the License, or +% (at your option) any later version. +% +% DQ Robotics is distributed in the hope that it will be useful, +% but WITHOUT ANY WARRANTY; without even the implied warranty of +% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +% GNU Lesser General Public License for more details. +% +% You should have received a copy of the GNU Lesser General Public License +% along with DQ Robotics. If not, see . +% +% DQ Robotics website: dqrobotics.github.io +% +% Contributors to this file: +% Frederico Fernandes Afonso Silva - frederico.silva@ieee.org + +classdef FrankaEmikaPandaVrepRobot < DQ_VrepRobot + + properties + joint_names; + base_frame_name; + force_sensor_names; + link_names; + lua_script_name; + end + + methods + function obj = FrankaEmikaPandaVrepRobot(robot_name,vrep_interface) + %% Constructs an instance of a FrankaEmikaPandaVrepRobot + % >> vi = VrepInterface() + % >> vi.connect('127.0.0.1',19997); + % >> robot = FrankaEmikaPandaVrepRobot('Franka', vi) + obj.robot_name = robot_name; + obj.vrep_interface = vrep_interface; + + % 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,'Franka') + error('Expected Franka') + 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:7 + current_joint_name = {robot_label,'_joint',int2str(i),robot_index}; + obj.joint_names{i} = strjoin(current_joint_name,''); + + current_link_name = {robot_label,'_link',int2str(i+1),robot_index}; + obj.link_names{i} = strjoin(current_link_name,''); + end + obj.base_frame_name = obj.joint_names{1}; + end + + function send_q_to_vrep(obj,q) + %% Sends the joint configurations to VREP + % >> vrep_robot = FrankaEmikaPandaVrepRobot('Franka', vi) + % >> q = zeros(7,1); + % >> vrep_robot.send_q_to_vrep(q) + obj.vrep_interface.set_joint_target_positions(obj.joint_names,q); + end + + function q = get_q_from_vrep(obj) + %% Obtains the joint configurations from VREP + % >> vrep_robot = FrankaEmikaPandaVrepRobot('Franka', vi) + % >> q = vrep_robot.get_q_from_vrep() + q = obj.vrep_interface.get_joint_positions(obj.joint_names); + end + + function send_q_dot_to_vrep(obj,q_dot) + %% Sends the joint velocities to VREP + % >> vrep_robot = FrankaEmikaPandaVrepRobot('Franka', vi) + % >> q_dot = zeros(7,1); + % >> vrep_robot.send_q_dot_to_vrep(q_dot) + obj.vrep_interface.set_joint_target_velocities(obj.joint_names,q_dot); + end + + function qd = get_q_dot_from_vrep(obj) + %% Obtains the joint configurations from VREP + % >> vrep_robot = FrankaEmikaPandaVrepRobot('Franka', vi) + % >> qd = vrep_robot.get_q_dot_from_vrep() + qd = obj.vrep_interface.get_joint_velocities(obj.joint_names); + end + + function send_tau_to_vrep(obj,tau) + %% Sends the joint torques to VREP + % >> vrep_robot = FrankaEmikaPandaVrepRobot('Franka', vi) + % >> tau = zeros(7,1); + % >> vrep_robot.send_tau_to_vrep(tau) + obj.vrep_interface.set_joint_torques(obj.joint_names,tau) + end + + function tau = get_tau_from_vrep(obj) + %% Obtains the joint torques from VREP + % >> vrep_robot = FrankaEmikaPandaVrepRobot('Franka', vi) + % >> tau = vrep_robot.get_tau_from_vrep() + tau = obj.vrep_interface.get_joint_torques(obj.joint_names); + end + + function kin = kinematics(obj) + %% Obtains the DQ_SerialManipulator instance that represents this Franka Emika Panda robot. + % >> vrep_robot = FrankaEmikaPandaVrepRobot('Franka', vi) + % >> robot_kinematics = vrep_robot.kinematics() + + % Franka Emika Panda + % Create a DQ_SerialManipulator object + kin = FrankaEmikaPandaRobot.kinematics; + + % Update base and reference frame with V-REP values + 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)); + + % Set the end-effector + kin.set_effector(1+0.5*DQ.E*DQ.k*0.1820); + end + end +end \ No newline at end of file From a5bb2bfba6a7ae6cfe3c2a4ca7aa23e7b7c4047f Mon Sep 17 00:00:00 2001 From: Frederico Afonso Date: Tue, 25 Jul 2023 17:03:51 +0200 Subject: [PATCH 05/12] Removed method 'get_force_sensor_readings()' from class 'DQ_VrepInterface'. --- interfaces/vrep/DQ_VrepInterface.m | 29 ----------------------------- 1 file changed, 29 deletions(-) diff --git a/interfaces/vrep/DQ_VrepInterface.m b/interfaces/vrep/DQ_VrepInterface.m index 61a4a597..6cec6391 100644 --- a/interfaces/vrep/DQ_VrepInterface.m +++ b/interfaces/vrep/DQ_VrepInterface.m @@ -1094,35 +1094,6 @@ function set_joint_torques(obj,joint_names,torques,opmode) end end end - - %% Get Force Sensor Readings - function [force_vec, torque_vec, return_code, state_sensor] = get_force_sensor_readings(obj,handle,opmode) - %% Get the readings of a force sensor in V-REP. For joints that are in 'Torque/force mode' in V-REP - %% >> force_sensor_name = 'Sensor_name_in_VREP'; - %% >> [force_vec, torque_vec] = vi.get_force_sensor_readings(force_sensor_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 - element = obj.element_from_string(handle); - force_sensor_handle = obj.handle_from_string_or_handle(handle); - if nargin <= 2 - if(~element.state_from_function_signature('get_force_sensor_readings')) - [~, ~, tmp_f, tmp_t] = obj.vrep.simxReadForceSensor(obj.clientID,element.handle,obj.OP_STREAMING); - return_code = 1; - while(return_code == 1) - [return_code, state_sensor, tmp_f, tmp_t] = obj.vrep.simxReadForceSensor(obj.clientID,force_sensor_handle,obj.OP_BUFFER); - end - else - [return_code, state_sensor, tmp_f, tmp_t] = obj.vrep.simxReadForceSensor(obj.clientID,force_sensor_handle,obj.OP_BUFFER); - end - else - [return_code, state_sensor, tmp_f, tmp_t] = obj.vrep.simxReadForceSensor(obj.clientID,force_sensor_handle,opmode); - end - force_vec = double(tmp_f); - torque_vec = double(tmp_t); - end end From fb6d048f210d9a46c3c39b59139c5c30a0e09a52 Mon Sep 17 00:00:00 2001 From: Frederico Afonso Date: Tue, 25 Jul 2023 17:20:53 +0200 Subject: [PATCH 06/12] Removed the class 'FrankaEmikaPandaVrepRobot'. Added the methods 'send_tau_to_vrep()' and 'get_tau_from_vrep()' to class 'LBR4pVrepRobot'. --- .../vrep/robots/FrankaEmikaPandaVrepRobot.m | 158 ------------------ interfaces/vrep/robots/LBR4pVrepRobot.m | 23 ++- 2 files changed, 22 insertions(+), 159 deletions(-) delete mode 100644 interfaces/vrep/robots/FrankaEmikaPandaVrepRobot.m diff --git a/interfaces/vrep/robots/FrankaEmikaPandaVrepRobot.m b/interfaces/vrep/robots/FrankaEmikaPandaVrepRobot.m deleted file mode 100644 index 5c4e0b3a..00000000 --- a/interfaces/vrep/robots/FrankaEmikaPandaVrepRobot.m +++ /dev/null @@ -1,158 +0,0 @@ -% CLASS FrankaEmikaPandaVrepRobot - Concrete class to interface with the -% "FrankaEmikaPanda" robot in VREP. -% -% Usage: -% 1) Drag-and-drop a "FrankaEmikaPanda" robot to a VREP scene. -% 2) Run -% >> vi = DQ_VrepInterface(); -% >> vi.connect('127.0.0.1',19997); -% >> vrep_robot = FrankaEmikaPandaVrepRobot('Franka', vi); -% >> vi.start_simulation(); -% >> robot.get_q_from_vrep(); -% >> 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 "FrankaEmikaPanda#0", a third robot, -% "FrankaEmikaPanda#1", and so on. -% -% LBR4pVrepRobot Methods: -% send_q_to_vrep - Sends the joint configurations to VREP -% get_q_from_vrep - Obtains the joint configurations from VREP -% get_q_dot_from_vrep - Obtains the robot generalized velocities from V-REP. -% send_tau_to_vrep - Sends the joint torques to VREP -% get_tau_from_vrep - Obtains the joint torques from VREP -% kinematics - Obtains the DQ_Kinematics implementation of this robot - -% (C) Copyright 2011-2023 DQ Robotics Developers -% -% This file is part of DQ Robotics. -% -% DQ Robotics is free software: you can redistribute it and/or modify -% it under the terms of the GNU Lesser General Public License as published by -% the Free Software Foundation, either version 3 of the License, or -% (at your option) any later version. -% -% DQ Robotics is distributed in the hope that it will be useful, -% but WITHOUT ANY WARRANTY; without even the implied warranty of -% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -% GNU Lesser General Public License for more details. -% -% You should have received a copy of the GNU Lesser General Public License -% along with DQ Robotics. If not, see . -% -% DQ Robotics website: dqrobotics.github.io -% -% Contributors to this file: -% Frederico Fernandes Afonso Silva - frederico.silva@ieee.org - -classdef FrankaEmikaPandaVrepRobot < DQ_VrepRobot - - properties - joint_names; - base_frame_name; - force_sensor_names; - link_names; - lua_script_name; - end - - methods - function obj = FrankaEmikaPandaVrepRobot(robot_name,vrep_interface) - %% Constructs an instance of a FrankaEmikaPandaVrepRobot - % >> vi = VrepInterface() - % >> vi.connect('127.0.0.1',19997); - % >> robot = FrankaEmikaPandaVrepRobot('Franka', vi) - obj.robot_name = robot_name; - obj.vrep_interface = vrep_interface; - - % 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,'Franka') - error('Expected Franka') - 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:7 - current_joint_name = {robot_label,'_joint',int2str(i),robot_index}; - obj.joint_names{i} = strjoin(current_joint_name,''); - - current_link_name = {robot_label,'_link',int2str(i+1),robot_index}; - obj.link_names{i} = strjoin(current_link_name,''); - end - obj.base_frame_name = obj.joint_names{1}; - end - - function send_q_to_vrep(obj,q) - %% Sends the joint configurations to VREP - % >> vrep_robot = FrankaEmikaPandaVrepRobot('Franka', vi) - % >> q = zeros(7,1); - % >> vrep_robot.send_q_to_vrep(q) - obj.vrep_interface.set_joint_target_positions(obj.joint_names,q); - end - - function q = get_q_from_vrep(obj) - %% Obtains the joint configurations from VREP - % >> vrep_robot = FrankaEmikaPandaVrepRobot('Franka', vi) - % >> q = vrep_robot.get_q_from_vrep() - q = obj.vrep_interface.get_joint_positions(obj.joint_names); - end - - function send_q_dot_to_vrep(obj,q_dot) - %% Sends the joint velocities to VREP - % >> vrep_robot = FrankaEmikaPandaVrepRobot('Franka', vi) - % >> q_dot = zeros(7,1); - % >> vrep_robot.send_q_dot_to_vrep(q_dot) - obj.vrep_interface.set_joint_target_velocities(obj.joint_names,q_dot); - end - - function qd = get_q_dot_from_vrep(obj) - %% Obtains the joint configurations from VREP - % >> vrep_robot = FrankaEmikaPandaVrepRobot('Franka', vi) - % >> qd = vrep_robot.get_q_dot_from_vrep() - qd = obj.vrep_interface.get_joint_velocities(obj.joint_names); - end - - function send_tau_to_vrep(obj,tau) - %% Sends the joint torques to VREP - % >> vrep_robot = FrankaEmikaPandaVrepRobot('Franka', vi) - % >> tau = zeros(7,1); - % >> vrep_robot.send_tau_to_vrep(tau) - obj.vrep_interface.set_joint_torques(obj.joint_names,tau) - end - - function tau = get_tau_from_vrep(obj) - %% Obtains the joint torques from VREP - % >> vrep_robot = FrankaEmikaPandaVrepRobot('Franka', vi) - % >> tau = vrep_robot.get_tau_from_vrep() - tau = obj.vrep_interface.get_joint_torques(obj.joint_names); - end - - function kin = kinematics(obj) - %% Obtains the DQ_SerialManipulator instance that represents this Franka Emika Panda robot. - % >> vrep_robot = FrankaEmikaPandaVrepRobot('Franka', vi) - % >> robot_kinematics = vrep_robot.kinematics() - - % Franka Emika Panda - % Create a DQ_SerialManipulator object - kin = FrankaEmikaPandaRobot.kinematics; - - % Update base and reference frame with V-REP values - 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)); - - % Set the end-effector - kin.set_effector(1+0.5*DQ.E*DQ.k*0.1820); - end - end -end \ No newline at end of file diff --git a/interfaces/vrep/robots/LBR4pVrepRobot.m b/interfaces/vrep/robots/LBR4pVrepRobot.m index 6e1be7cc..08298cac 100644 --- a/interfaces/vrep/robots/LBR4pVrepRobot.m +++ b/interfaces/vrep/robots/LBR4pVrepRobot.m @@ -41,7 +41,13 @@ % DQ Robotics website: dqrobotics.sourceforge.net % % Contributors to this file: -% Murilo Marques Marinho - murilo@nml.t.u-tokyo.ac.jp +% 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) +% - Added the following methods: +% - send_tau_to_vrep() +% - get_tau_from_vrep() classdef LBR4pVrepRobot < DQ_VrepRobot @@ -97,6 +103,21 @@ function send_q_to_vrep(obj,q) % >> q = vrep_robot.get_q_from_vrep(q) q = obj.vrep_interface.get_joint_positions(obj.joint_names); end + + function send_tau_to_vrep(obj,tau) + %% Sends the joint torques to VREP + % >> vrep_robot = FrankaEmikaPandaVrepRobot('Franka', vi) + % >> tau = zeros(7,1); + % >> vrep_robot.send_tau_to_vrep(tau) + obj.vrep_interface.set_joint_torques(obj.joint_names,tau) + end + + function tau = get_tau_from_vrep(obj) + %% Obtains the joint torques from VREP + % >> vrep_robot = FrankaEmikaPandaVrepRobot('Franka', vi) + % >> tau = vrep_robot.get_tau_from_vrep() + tau = obj.vrep_interface.get_joint_torques(obj.joint_names); + end function kin = kinematics(obj) %% Obtains the DQ_SerialManipulator instance that represents this LBR4p robot. From 1561d259ab89e7a2a1da0325b79d3e9d3fa926a8 Mon Sep 17 00:00:00 2001 From: Frederico Afonso Date: Tue, 25 Jul 2023 17:36:38 +0200 Subject: [PATCH 07/12] Improved documentation of the methods 'get_joint_torques' and 'set_joint_torques' in class 'DQ_VrepInterface'. --- interfaces/vrep/DQ_VrepInterface.m | 82 +++++++++++++++++++++++++++--- 1 file changed, 74 insertions(+), 8 deletions(-) diff --git a/interfaces/vrep/DQ_VrepInterface.m b/interfaces/vrep/DQ_VrepInterface.m index 6cec6391..eb72b12e 100644 --- a/interfaces/vrep/DQ_VrepInterface.m +++ b/interfaces/vrep/DQ_VrepInterface.m @@ -98,6 +98,9 @@ % - Improved the documentation of the class % % 3. Frederico Fernandes Afonso Silva (frederico.silva@ieee.org) +% - Added the following methods: +% - get_joint_torques() (see https://github.com/dqrobotics/matlab/pull/104) +% - set_joint_torques() (see https://github.com/dqrobotics/matlab/pull/104) % - Altered the following properties from 'private' to 'protected' % (see discussions in https://github.com/dqrobotics/matlab/pull/101 % to further details): @@ -1036,13 +1039,46 @@ function set_joint_target_velocities(obj,jointnames,joint_target_velocities,opmo end end - %% Get Joint Torques function [joint_torques,return_code] = get_joint_torques(obj,handles,opmode) - %% Get the joint torques of a robot in V-REP. For joints that are in 'Torque/force mode' in V-REP - %% >> joint_names = {'redundantRob_joint1','redundantRob_joint2','redundantRob_joint3','redundantRob_joint4','redundantRob_joint5','redundantRob_joint6','redundantRob_joint7'}; - %% >> vi.get_joint_torques(joint_names); + % This method gets the joint torques of a robot in the CoppeliaSim scene. + % Usage: + % Recommended: + % joint_torques = get_joint_torques(jointnames); + % [joint_torques,return_code] = get_joint_torques(jointnames); + % + % Advanced: + % joint_torques = get_joint_torques(jointnames, opmode) + % [joint_torques,return_code] = 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); + % [joint_torques, rtn] = get_joint_torques(jointnames); + % + % % Advanced usage: + % joint_torques = get_joint_torques(jointnames, OP_STREAMING); + % [joint_torques, rtn] = get_joint_torques(jointnames, OP_STREAMING); joint_torques = zeros(length(handles),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(handles) % First approach to the auto-management using % DQ_VrepInterfaceMapElements. If the user does not specify the @@ -1071,12 +1107,42 @@ function set_joint_target_velocities(obj,jointnames,joint_target_velocities,opmo end end - %% Set Joint Torques function set_joint_torques(obj,joint_names,torques,opmode) - %% Set the joint torques of a robot in V-REP. For joints that are in 'Torque/force mode' in V-REP - %% >> joint_names = {'redundantRob_joint1','redundantRob_joint2','redundantRob_joint3','redundantRob_joint4','redundantRob_joint5','redundantRob_joint6','redundantRob_joint7'}; - %% >> vi.set_joint_torques(joint_names,[0 pi/2 0 pi/2 0 pi/2 0]); + % 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(joint_names) From 1337a2f2d0570cf8e088a4c603b7e132dd038830 Mon Sep 17 00:00:00 2001 From: Frederico Afonso Date: Wed, 15 Nov 2023 10:35:10 +0000 Subject: [PATCH 08/12] [DQ_VrepInterface] Removed 'return_code' from method 'get_joint_torques'. --- interfaces/vrep/DQ_VrepInterface.m | 29 ++++------------------------- 1 file changed, 4 insertions(+), 25 deletions(-) diff --git a/interfaces/vrep/DQ_VrepInterface.m b/interfaces/vrep/DQ_VrepInterface.m index eb72b12e..1f7bfd85 100644 --- a/interfaces/vrep/DQ_VrepInterface.m +++ b/interfaces/vrep/DQ_VrepInterface.m @@ -186,7 +186,6 @@ function connect(obj,ip,port) error('Failed to connect to remote API server'); end end - function disconnect(obj) % This method ends the communication between the client and @@ -194,14 +193,12 @@ function disconnect(obj) 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. @@ -214,33 +211,28 @@ function set_synchronous(obj,flag) 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. @@ -264,7 +256,6 @@ function stop_simulation(obj) end end - function handle = get_handle(obj,name) % This method gets the handle for a given object in the CoppeliaSim scene. % @@ -280,7 +271,6 @@ function stop_simulation(obj) 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. % @@ -358,7 +348,6 @@ function stop_simulation(obj) 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: @@ -413,7 +402,6 @@ function set_object_translation(obj,objectname,translation,reference_frame,opmod end end - function r = get_object_rotation(obj, objectname, reference_frame, opmode) % This method gets the rotation of an object in the CoppeliaSim scene. % @@ -498,7 +486,6 @@ function set_object_translation(obj,objectname,translation,reference_frame,opmod 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: @@ -555,7 +542,6 @@ function set_object_rotation(obj,objectname,rotation,reference_frame,opmode) end end - function x = get_object_pose(obj,objectname,reference_frame,opmode) % This method gets the pose of an object in the CoppeliaSim scene. % @@ -611,7 +597,6 @@ function set_object_rotation(obj,objectname,rotation,reference_frame,opmode) 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: @@ -746,8 +731,7 @@ function set_joint_positions(obj,jointnames,joint_positions,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 @@ -811,7 +795,6 @@ function set_joint_target_positions(obj,jointnames,joint_target_positions,opmode 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: @@ -1039,16 +1022,14 @@ function set_joint_target_velocities(obj,jointnames,joint_target_velocities,opmo end end - function [joint_torques,return_code] = get_joint_torques(obj,handles,opmode) + function joint_torques = get_joint_torques(obj,handles,opmode) % This method gets the joint torques of a robot in the CoppeliaSim scene. % Usage: % Recommended: % joint_torques = get_joint_torques(jointnames); - % [joint_torques,return_code] = get_joint_torques(jointnames); % % Advanced: % joint_torques = get_joint_torques(jointnames, opmode) - % [joint_torques,return_code] = get_joint_torques(jointnames, opmode); % % jointnames: The joint names. % (optional) opmode: The operation mode. If not @@ -1068,11 +1049,9 @@ function set_joint_target_velocities(obj,jointnames,joint_target_velocities,opmo % % % Recommended: % joint_torques = get_joint_torques(jointnames); - % [joint_torques, rtn] = get_joint_torques(jointnames); % % % Advanced usage: % joint_torques = get_joint_torques(jointnames, OP_STREAMING); - % [joint_torques, rtn] = get_joint_torques(jointnames, OP_STREAMING); joint_torques = zeros(length(handles),1); @@ -1097,10 +1076,10 @@ function set_joint_target_velocities(obj,jointnames,joint_target_velocities,opmo [return_code,tmp] = obj.vrep.simxGetJointForce(obj.clientID, element.handle, obj.OP_BUFFER); end else - [return_code,tmp] = obj.vrep.simxGetJointForce(obj.clientID, element.handle, obj.OP_BUFFER); + [~,tmp] = obj.vrep.simxGetJointForce(obj.clientID, element.handle, obj.OP_BUFFER); end else - [return_code,tmp] = obj.vrep.simxGetJointForce(obj.clientID, ... + [~,tmp] = obj.vrep.simxGetJointForce(obj.clientID, ... obj.handle_from_string_or_handle(handles{joint_index}), opmode); end joint_torques(joint_index) = double(-tmp); % V-REP returns joint torques with an inverse sign From 937d208250c924adab1a0e4f0b4b6be567f4fc46 Mon Sep 17 00:00:00 2001 From: Frederico Afonso Date: Wed, 17 Jan 2024 15:30:57 +0000 Subject: [PATCH 09/12] [LBR4pVrepRobot] Removed methods 'send_tau_to_vrep()' and 'get_tau_from_vrep()'. --- interfaces/vrep/robots/LBR4pVrepRobot.m | 20 -------------------- 1 file changed, 20 deletions(-) diff --git a/interfaces/vrep/robots/LBR4pVrepRobot.m b/interfaces/vrep/robots/LBR4pVrepRobot.m index 08298cac..a4c7ee1c 100644 --- a/interfaces/vrep/robots/LBR4pVrepRobot.m +++ b/interfaces/vrep/robots/LBR4pVrepRobot.m @@ -43,11 +43,6 @@ % 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) -% - Added the following methods: -% - send_tau_to_vrep() -% - get_tau_from_vrep() classdef LBR4pVrepRobot < DQ_VrepRobot @@ -103,21 +98,6 @@ function send_q_to_vrep(obj,q) % >> q = vrep_robot.get_q_from_vrep(q) q = obj.vrep_interface.get_joint_positions(obj.joint_names); end - - function send_tau_to_vrep(obj,tau) - %% Sends the joint torques to VREP - % >> vrep_robot = FrankaEmikaPandaVrepRobot('Franka', vi) - % >> tau = zeros(7,1); - % >> vrep_robot.send_tau_to_vrep(tau) - obj.vrep_interface.set_joint_torques(obj.joint_names,tau) - end - - function tau = get_tau_from_vrep(obj) - %% Obtains the joint torques from VREP - % >> vrep_robot = FrankaEmikaPandaVrepRobot('Franka', vi) - % >> tau = vrep_robot.get_tau_from_vrep() - tau = obj.vrep_interface.get_joint_torques(obj.joint_names); - end function kin = kinematics(obj) %% Obtains the DQ_SerialManipulator instance that represents this LBR4p robot. From 85f69aca9efd825c51e9274dcbd1d1d52e8972c5 Mon Sep 17 00:00:00 2001 From: Frederico Fernandes Afonso Silva Date: Thu, 15 Aug 2024 12:21:44 +0100 Subject: [PATCH 10/12] [LBR4pVrepRobot.m] Reverted modification on the comments. --- interfaces/vrep/robots/LBR4pVrepRobot.m | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/interfaces/vrep/robots/LBR4pVrepRobot.m b/interfaces/vrep/robots/LBR4pVrepRobot.m index a4c7ee1c..6e1be7cc 100644 --- a/interfaces/vrep/robots/LBR4pVrepRobot.m +++ b/interfaces/vrep/robots/LBR4pVrepRobot.m @@ -41,8 +41,7 @@ % 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. +% Murilo Marques Marinho - murilo@nml.t.u-tokyo.ac.jp classdef LBR4pVrepRobot < DQ_VrepRobot From 4f1b7c9fa018ae6f6f93b41057182400ebffc8bb Mon Sep 17 00:00:00 2001 From: Frederico Fernandes Afonso Silva Date: Thu, 15 Aug 2024 17:12:29 +0100 Subject: [PATCH 11/12] [DQ_VrepInterface.m] Changed 'handles' by 'jointnames' in method 'get_joint_torques'. --- interfaces/vrep/DQ_VrepInterface.m | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/interfaces/vrep/DQ_VrepInterface.m b/interfaces/vrep/DQ_VrepInterface.m index 1f7bfd85..0fe6a1c8 100644 --- a/interfaces/vrep/DQ_VrepInterface.m +++ b/interfaces/vrep/DQ_VrepInterface.m @@ -1022,7 +1022,7 @@ function set_joint_target_velocities(obj,jointnames,joint_target_velocities,opmo end end - function joint_torques = get_joint_torques(obj,handles,opmode) + function joint_torques = get_joint_torques(obj,jointnames,opmode) % This method gets the joint torques of a robot in the CoppeliaSim scene. % Usage: % Recommended: @@ -1053,21 +1053,21 @@ function set_joint_target_velocities(obj,jointnames,joint_target_velocities,opmo % % Advanced usage: % joint_torques = get_joint_torques(jointnames, OP_STREAMING); - joint_torques = zeros(length(handles),1); + 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(handles) + 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(handles,'cell') - element = obj.element_from_string(handles{joint_index}); + if isa(jointnames,'cell') + element = obj.element_from_string(jointnames{joint_index}); else - element = obj.element_from_string(handles); + 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); @@ -1080,7 +1080,7 @@ function set_joint_target_velocities(obj,jointnames,joint_target_velocities,opmo end else [~,tmp] = obj.vrep.simxGetJointForce(obj.clientID, ... - obj.handle_from_string_or_handle(handles{joint_index}), opmode); + 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 From f989f7bff0b27085c877d873e87fb6f35970ad08 Mon Sep 17 00:00:00 2001 From: Frederico Fernandes Afonso Silva Date: Thu, 15 Aug 2024 17:16:21 +0100 Subject: [PATCH 12/12] [DQ_VrepInterface.m] Changed 'joint_names' by 'jointnames' in method 'set_joint_torques'. --- interfaces/vrep/DQ_VrepInterface.m | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/interfaces/vrep/DQ_VrepInterface.m b/interfaces/vrep/DQ_VrepInterface.m index 0fe6a1c8..a869a347 100644 --- a/interfaces/vrep/DQ_VrepInterface.m +++ b/interfaces/vrep/DQ_VrepInterface.m @@ -1086,7 +1086,7 @@ function set_joint_target_velocities(obj,jointnames,joint_target_velocities,opmo end end - function set_joint_torques(obj,joint_names,torques,opmode) + function set_joint_torques(obj,jointnames,torques,opmode) % This method sets the joint torques of a robot in the CoppeliaSim scene. % Usage: % Recommended: @@ -1124,17 +1124,17 @@ function set_joint_torques(obj,joint_names,torques,opmode) % remote API documentation. if nargin == 3 % The recommended mode is OP_ONESHOT - for joint_index=1:length(joint_names) - obj.vrep.simxSetJointTargetVelocity(obj.clientID, obj.handle_from_string_or_handle(joint_names{joint_index}), ... + 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(joint_names{joint_index}), ... + 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(joint_names) - obj.vrep.simxSetJointTargetVelocity(obj.clientID, obj.handle_from_string_or_handle(joint_names{joint_index}),... + 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(joint_names{joint_index}),abs(torques(joint_index)),... + obj.vrep.simxSetJointForce(obj.clientID,obj.handle_from_string_or_handle(jointnames{joint_index}),abs(torques(joint_index)),... opmode); end end