Skip to content

Commit

Permalink
This PR adds force/torque actuation/reading methods to the class 'DQ_…
Browse files Browse the repository at this point in the history 
…VrepInterface' (#104)

* Added the class FrankaEmikaPandaRobot().

* Added the  method 'get_force_sensor_readings' to DQ_VrepInterface(). It allows reading the force/torques of a force sensor in V-REP/CoppeliaSim.

* Added the methods 'get_joint_torques()' and 'set_joint_torques()' to the class DQ_VrepInterface.

* Added the class 'FrankaEmikaPandaVrepRobot'.

* Removed method 'get_force_sensor_readings()' from class 'DQ_VrepInterface'.

* Removed the class 'FrankaEmikaPandaVrepRobot'. Added the methods 'send_tau_to_vrep()' and 'get_tau_from_vrep()' to class 'LBR4pVrepRobot'.

* Improved documentation of the methods 'get_joint_torques' and 'set_joint_torques' in class 'DQ_VrepInterface'.

* [DQ_VrepInterface] Removed 'return_code' from method 'get_joint_torques'.

* [LBR4pVrepRobot] Removed methods 'send_tau_to_vrep()' and 'get_tau_from_vrep()'.

* [LBR4pVrepRobot.m] Reverted modification on the comments.

* [DQ_VrepInterface.m] Changed 'handles' by 'jointnames' in method 'get_joint_torques'.

* [DQ_VrepInterface.m] Changed 'joint_names' by 'jointnames' in method 'set_joint_torques'.
  • Loading branch information
@ffasilva
ffasilva authored Aug 16, 2024
1 parent 1eb3459 commit 71b036b
Showing 1 changed file with 129 additions and 24 deletions.
153 changes: 129 additions & 24 deletions interfaces/vrep/DQ_VrepInterface.m
View file
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 ([email protected])
% - 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

0 comments on commit 71b036b

Please sign in to comment.