This PR adds force/torque actuation/reading methods to the class 'DQ_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'.

Author: ffasilva

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
@@ -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):
@@ -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.
@@ -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.
@@ -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. 
             % 
@@ -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.
             %
@@ -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:
@@ -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.
             %
@@ -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:
@@ -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.
             %
@@ -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:
@@ -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.
@@ -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 
@@ -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:
@@ -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:
@@ -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
@@ -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