From 5b983dc854e98d9730478bb455a6569cab59efc1 Mon Sep 17 00:00:00 2001 From: Murilo Marinho Date: Fri, 11 Oct 2019 10:03:11 +0900 Subject: [PATCH 1/2] Renamed VrepInterface to DQ_VrepInterface --- .../{VrepInterface.m => DQ_VrepInterface.m} | 24 +++++++++---------- ...Element.m => DQ_VrepInterfaceMapElement.m} | 4 ++-- robot_control/DQ_PseudoinverseController.m | 12 +++++++--- 3 files changed, 23 insertions(+), 17 deletions(-) rename interfaces/{VrepInterface.m => DQ_VrepInterface.m} (96%) rename interfaces/{VrepInterfaceMapElement.m => DQ_VrepInterfaceMapElement.m} (94%) diff --git a/interfaces/VrepInterface.m b/interfaces/DQ_VrepInterface.m similarity index 96% rename from interfaces/VrepInterface.m rename to interfaces/DQ_VrepInterface.m index 4cffbf79..20a215c7 100644 --- a/interfaces/VrepInterface.m +++ b/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 - murilo@nml.t.u-tokyo.ac.jp -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 @@ -370,7 +370,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 diff --git a/interfaces/VrepInterfaceMapElement.m b/interfaces/DQ_VrepInterfaceMapElement.m similarity index 94% rename from interfaces/VrepInterfaceMapElement.m rename to interfaces/DQ_VrepInterfaceMapElement.m index 951d0fdf..0f04cc07 100644 --- a/interfaces/VrepInterfaceMapElement.m +++ b/interfaces/DQ_VrepInterfaceMapElement.m @@ -20,7 +20,7 @@ % Contributors to this file: % Murilo Marques Marinho - murilo@nml.t.u-tokyo.ac.jp -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 diff --git a/robot_control/DQ_PseudoinverseController.m b/robot_control/DQ_PseudoinverseController.m index 0e211100..ddf5abdd 100644 --- a/robot_control/DQ_PseudoinverseController.m +++ b/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. From 746c64e68c7f84e27a41401e65f0b02c77a3b79c Mon Sep 17 00:00:00 2001 From: Murilo Marinho Date: Fri, 11 Oct 2019 10:05:06 +0900 Subject: [PATCH 2/2] Fixing #51 --- interfaces/DQ_VrepInterface.m | 4 ---- 1 file changed, 4 deletions(-) diff --git a/interfaces/DQ_VrepInterface.m b/interfaces/DQ_VrepInterface.m index 20a215c7..e137b47d 100644 --- a/interfaces/DQ_VrepInterface.m +++ b/interfaces/DQ_VrepInterface.m @@ -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