Merge pull request #52 from dqrobotics/issue51

Renames VrepInterface to DQ_VrepInterface and fixes issue #51

interfaces/VrepInterface.m → interfaces/DQ_VrepInterface.m

@@ -1,4 +1,4 @@
-% CLASS VrepInterface - Communicate with V-REP using dual quaternions.
+% CLASS DQ_VrepInterface - Communicate with V-REP using dual quaternions.
 %
 % Installation:
 %   1) Enable V-REP's remote API on the Server Side:
@@ -19,7 +19,7 @@
 %   simulation:
 %       1) Open V-REP with the default scene
 %       2) Run
-%           >> vi = VrepInterface();
+%           >> vi = DQ_VrepInterface();
 %           >> vi.connect('127.0.0.1',19997);
 %           >> vi.start_simulation();
 %           >> pause(1);
@@ -30,7 +30,7 @@
 % helpful to fully understand the documentation.
 % http://www.coppeliarobotics.com/helpFiles/en/legacyRemoteApiOverview.htm
 %
-%   VrepInterface Methods:
+%   DQ_VrepInterface Methods:
 %       connect - Connects to a V-REP Remote API Server
 %       disconnect - Disconnects from currently connected server
 %       disconnect_all - Flushes all Remote API connections
@@ -49,7 +49,7 @@
 %       robot
 %       get_joint_positions - Get the joint positions of a robot
 %
-%   VrepInterface Methods (For advanced users)
+%   DQ_VrepInterface Methods (For advanced users)
 %       get_handle - Get the handle of a V-REP object
 %       get_handles - Get the handles for multiple V-REP objects
 %
@@ -76,14 +76,14 @@
 % Contributors to this file:
 %     Murilo Marques Marinho - [email protected]
 
-classdef VrepInterface < handle
+classdef DQ_VrepInterface < handle
 
     properties (Access = private)
         % the V-REP remote API instance used by this interface
         vrep;
         % the client ID of this remote API connection
         clientID;
-        % a map between V-REP object names and VrepInterfaceMapElements
+        % a map between V-REP object names and DQ_VrepInterfaceMapElements
         handles_map;
     end
 
@@ -107,7 +107,7 @@
                     handle = obj.handles_map(name).handle;
                 else
                     handle = obj.get_handle(name);
-                    obj.handles_map(name) = VrepInterfaceMapElement(handle);
+                    obj.handles_map(name) = DQ_VrepInterfaceMapElement(handle);
                 end
 
             else
@@ -123,11 +123,11 @@
 
     methods
 
-        function obj = VrepInterface()
+        function obj = DQ_VrepInterface()
             obj.vrep=remApi('remoteApi');
             obj.handles_map = containers.Map;
             obj.clientID = -1;
-            disp(['This version of DQ Robotics VrepInterface is compatible'...
+            disp(['This version of DQ Robotics DQ_VrepInterface is compatible'...
                   ' with VREP 3.5.0']);
         end
 
@@ -190,7 +190,7 @@ function stop_simulation(obj)
             %%  >> t = vi.get_object_translation('DefaultCamera');
 
             % First approach to the auto-management using
-            % VrepInterfaceMapElements. If the user does not specify the
+            % 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
@@ -236,7 +236,7 @@ function set_object_translation(obj,handle,t,relative_to_handle,opmode)
 
 
             % First approach to the auto-management using
-            % VrepInterfaceMapElements. If the user does not specify the
+            % 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
@@ -342,7 +342,6 @@ function set_joint_target_positions(obj,handles,thetas,opmode)
             %%  >> vi.set_joint_target_positions(joint_names,[0 pi/2 0 pi/2 0 pi/2 0]);
 
             if nargin == 3
-                obj.vrep.simxPauseCommunication(obj.clientID,1)
                 for joint_index=1:length(handles)
                     if isa(handles,'cell')
                         obj.vrep.simxSetJointTargetPosition(obj.clientID,obj.handle_from_string_or_handle(handles{joint_index}),thetas(joint_index),obj.OP_ONESHOT);
@@ -351,13 +350,10 @@ function set_joint_target_positions(obj,handles,thetas,opmode)
                     end
 
                 end
-                obj.vrep.simxPauseCommunication(obj.clientID,0)
             else
-                obj.vrep.simxPauseCommunication(obj.clientID,1)
                 for joint_index=1:length(handles)
                     obj.vrep.simxSetJointTargetPosition(obj.clientID,obj.handle_from_string_or_handle(handles{joint_index}),thetas(joint_index),opmode);
                 end
-                obj.vrep.simxPauseCommunication(obj.clientID,0)
             end
         end
 
@@ -370,7 +366,7 @@ function set_joint_target_positions(obj,handles,thetas,opmode)
             thetas = zeros(length(handles),1);
             for joint_index=1:length(handles)
                 % First approach to the auto-management using
-                % VrepInterfaceMapElements. If the user does not specify the
+                % 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

interfaces/VrepInterfaceMapElement.m → interfaces/DQ_VrepInterfaceMapElement.m

@@ -20,7 +20,7 @@
 % Contributors to this file:
 %     Murilo Marques Marinho - [email protected]
 
-classdef VrepInterfaceMapElement
+classdef DQ_VrepInterfaceMapElement
     properties
         % For VREP's remote API, the first call to any "get" function should be OP_STREAMING and the following calls should be
         % OP_BUFFER, so we need to track the states of each of them using
@@ -30,7 +30,7 @@
     end
     methods
         %% Constructor
-        function obj = VrepInterfaceMapElement(handle)
+        function obj = DQ_VrepInterfaceMapElement(handle)
             obj.set_states_map = containers.Map;
             obj.handle = handle;
         end

robot_control/DQ_PseudoinverseController.m

@@ -46,12 +46,18 @@
                 task_variable = controller.get_task_variable(q);
                 % get the Jacobian according to the control objective
                 J = controller.get_jacobian(q);
-
+                
                 % calculate the Euclidean error
                 task_error = task_variable - task_reference;
                 % compute the control signal
-                u = pinv(J)*(-controller.gain*task_error + ...
-                    feedforward);
+                if(controller.damping == 0.0)
+                    u = pinv(J)*(-controller.gain*task_error + ...
+                        feedforward);
+                else
+                    u = inv(J'*J+controller.damping^2* ...
+                        eye(length(q)))*J'*...
+                        (-controller.gain*task_error + feedforward);
+                end
 
                 % verify if the closed-loop system has reached a stable
                 % region and update the appropriate flags accordingly.