Skip to content

This PR adds force/torque actuation/reading methods to the class 'DQ_VrepInterface' #104

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 17 commits into from
Aug 16, 2024
Merged

This PR adds force/torque actuation/reading methods to the class 'DQ_VrepInterface' #104

Changes from all commits
Commits
Show all changes
17 commits
Select commit Hold shift + click to select a range
ea86d9b
Added the class FrankaEmikaPandaRobot().
ffasilva May 15, 2023
faf2f8d
Merge branch 'master' of https://github.com/ffasilva/matlab
ffasilva May 19, 2023
52af08d
Added the method 'get_force_sensor_readings' to DQ_VrepInterface(). …
ffasilva May 27, 2023
f1e52d9
Merge branch 'master' of https://github.com/ffasilva/matlab
ffasilva May 28, 2023
8d00c9a
Merge branch 'master' into dq-vrepinterface-get-force-sensor-readings
ffasilva May 28, 2023
cfaceaa
Added the methods 'get_joint_torques()' and 'set_joint_torques()' to …
ffasilva Jul 8, 2023
43ea9dc
Added the class 'FrankaEmikaPandaVrepRobot'.
ffasilva Jul 10, 2023
a5bb2bf
Removed method 'get_force_sensor_readings()' from class 'DQ_VrepInter…
ffasilva Jul 25, 2023
fb6d048
Removed the class 'FrankaEmikaPandaVrepRobot'. Added the methods 'sen…
ffasilva Jul 25, 2023
1561d25
Improved documentation of the methods 'get_joint_torques' and 'set_jo…
ffasilva Jul 25, 2023
1337a2f
[DQ_VrepInterface] Removed 'return_code' from method 'get_joint_torqu…
ffasilva Nov 15, 2023
937d208
[LBR4pVrepRobot] Removed methods 'send_tau_to_vrep()' and 'get_tau_fr…
ffasilva Jan 17, 2024
85f69ac
[LBR4pVrepRobot.m] Reverted modification on the comments.
ffasilva Aug 15, 2024
0c89441
Merge branch 'dqrobotics:master' into dq-vrepinterface-get-force-sens…
ffasilva Aug 15, 2024
4f1b7c9
[DQ_VrepInterface.m] Changed 'handles' by 'jointnames' in method 'get…
ffasilva Aug 15, 2024
3a6759c
Merge branch 'dq-vrepinterface-get-force-sensor-readings' of https://…
ffasilva Aug 15, 2024
f989f7b
[DQ_VrepInterface.m] Changed 'joint_names' by 'jointnames' in method …
ffasilva Aug 15, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
153 changes: 129 additions & 24 deletions interfaces/vrep/DQ_VrepInterface.m
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -96,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):
Expand Down Expand Up @@ -181,22 +186,19 @@ 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
% the CoppeliaSim scene. This should be the very last method called.
obj.vrep.simxFinish(obj.clientID);
end


function disconnect_all(obj)
% This method ends all running communication threads with the
% CoppeliaSim scene. This should be the very last method called.
obj.vrep.simxFinish(-1);
end


function set_synchronous(obj,flag)
% This method enables or disables the stepped (synchronous) mode
% for the remote API server service that the client is connected to.
Expand All @@ -209,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.
Expand All @@ -259,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.
%
Expand All @@ -275,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.
%
Expand Down Expand Up @@ -353,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:
Expand Down Expand Up @@ -408,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.
%
Expand Down Expand Up @@ -493,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:
Expand Down Expand Up @@ -550,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.
%
Expand Down Expand Up @@ -606,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:
Expand Down Expand Up @@ -678,8 +668,7 @@ function set_object_pose(obj,objectname,pose,reference_frame,opmode)
obj.handle_from_string_or_handle(relative_to_handle),...
opmode);
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.
Expand Down Expand Up @@ -742,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
Expand Down Expand Up @@ -807,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:
Expand Down Expand Up @@ -898,8 +885,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:
Expand Down Expand Up @@ -930,7 +916,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
Expand Down Expand Up @@ -1034,7 +1020,126 @@ function set_joint_target_velocities(obj,jointnames,joint_target_velocities,opmo
opmode);
end
end
end
end

function joint_torques = get_joint_torques(obj,jointnames,opmode)
% This method gets the joint torques of a robot in the CoppeliaSim scene.
% Usage:
% Recommended:
% joint_torques = get_joint_torques(jointnames);
%
% Advanced:
% joint_torques = get_joint_torques(jointnames, opmode)
%
% jointnames: The joint names.
% (optional) opmode: The operation mode. If not
% specified, the opmode will be set automatically.
%
% You can use the following modes:
% OP_BLOCKING
% OP_STREAMING
% OP_ONESHOT
% OP_BUFFER;
%
% Check this link for more details: https://www.coppeliarobotics.com/helpFiles/en/remoteApiFunctionsMatlab.htm#simxGetJointForce
%
% Example:
% jointnames={'LBR4p_joint1','LBR4p_joint2','LBR4p_joint3','LBR4p_joint4',...
% 'LBR4p_joint5','LBR4p_joint6','LBR4p_joint7'};
%
% % Recommended:
% joint_torques = get_joint_torques(jointnames);
%
% % Advanced usage:
% joint_torques = get_joint_torques(jointnames, OP_STREAMING);

joint_torques = zeros(length(jointnames),1);

% If the user does not specify the opmode, it is chosen first
% as STREAMING and then as BUFFER, as specified by the remote
% API documentation.
for joint_index=1:length(jointnames)
% First approach to the auto-management using
% DQ_VrepInterfaceMapElements. If the user does not specify the
% opmode, it is chosen first as STREAMING and then as BUFFER,
% as specified by the remote API documentation
if nargin <= 2
if isa(jointnames,'cell')
element = obj.element_from_string(jointnames{joint_index});
else
element = obj.element_from_string(jointnames);
end
if(~element.state_from_function_signature('get_joint_torques'))
[~,tmp] = obj.vrep.simxGetJointForce(obj.clientID, element.handle, obj.OP_STREAMING);
return_code = 1;
while(return_code == 1)
[return_code,tmp] = obj.vrep.simxGetJointForce(obj.clientID, element.handle, obj.OP_BUFFER);
end
else
[~,tmp] = obj.vrep.simxGetJointForce(obj.clientID, element.handle, obj.OP_BUFFER);
end
else
[~,tmp] = obj.vrep.simxGetJointForce(obj.clientID, ...
obj.handle_from_string_or_handle(jointnames{joint_index}), opmode);
end
joint_torques(joint_index) = double(-tmp); % V-REP returns joint torques with an inverse sign
end
end

function set_joint_torques(obj,jointnames,torques,opmode)
% This method sets the joint torques of a robot in the CoppeliaSim scene.
% Usage:
% Recommended:
% set_joint_torques(jointnames, torques);
%
% Advanced:
% set_joint_torques(jointnames, torques, opmode);
%
% jointnames: The joint names.
% torques: The joint torques.
% (optional) opmode: The operation mode. If not
% specified, the opmode will be set automatically.
%
% You can use the following modes:
% OP_BLOCKING
% OP_STREAMING
% OP_ONESHOT
% OP_BUFFER;
%
% Check this link for more details: https://www.coppeliarobotics.com/helpFiles/en/remoteApiFunctionsMatlab.htm#simxSetJointTargetVelocity
%
% Example:
% jointnames={'LBR4p_joint1','LBR4p_joint2','LBR4p_joint3','LBR4p_joint4',...
% 'LBR4p_joint5','LBR4p_joint6','LBR4p_joint7'};
% u = [0.1 0.1 0.1 0.1 0.1 0.1 0.1];
%
% % Recommended:
% set_joint_torques(jointnames, u);
%
% % Advanced usage:
% set_joint_torques(jointnames, u, opmode);

% If the user does not specify the opmode, it is chosen first
% as STREAMING and then as OP_ONESHOT, as specified by the
% remote API documentation.
if nargin == 3
% The recommended mode is OP_ONESHOT
for joint_index=1:length(jointnames)
obj.vrep.simxSetJointTargetVelocity(obj.clientID, obj.handle_from_string_or_handle(jointnames{joint_index}), ...
sign(torques(joint_index))*10e10, obj.OP_ONESHOT);
obj.vrep.simxSetJointForce(obj.clientID, obj.handle_from_string_or_handle(jointnames{joint_index}), ...
abs(torques(joint_index)), obj.OP_ONESHOT);
end
else
for joint_index=1:length(jointnames)
obj.vrep.simxSetJointTargetVelocity(obj.clientID, obj.handle_from_string_or_handle(jointnames{joint_index}),...
sign(torques(joint_index))*10e10, opmode);
obj.vrep.simxSetJointForce(obj.clientID,obj.handle_from_string_or_handle(jointnames{joint_index}),abs(torques(joint_index)),...
opmode);
end
end
end

end

end
Expand Down