diff --git a/interfaces/vrep/DQ_SerialVrepRobot.m b/interfaces/vrep/DQ_SerialVrepRobot.m new file mode 100644 index 00000000..df363a09 --- /dev/null +++ b/interfaces/vrep/DQ_SerialVrepRobot.m @@ -0,0 +1,240 @@ +% CLASS DQ_SerialVrepRobot - Concrete class to interface with serial robots +% in VREP. +% +% Usage: +% 1) Drag-and-drop a serial robot to a VREP scene. For instance, a +% "my_robot" robot. +% 2) Run +% >> vi = DQ_VrepInterface(); +% >> vi.connect('127.0.0.1',19997); +% >> vrep_robot = DQ_SerialVrepRobot("my_robot", 7, "my_robot", vi); +% >> vi.start_simulation(); +% >> vrep_robot.get_configuration(); +% >> pause(1); +% >> vi.stop_simulation(); +% >> vi.disconnect(); +% Note that the name of the robot should be EXACTLY the same as in +% VREP. For instance, if you drag-and-drop a second robot, its name +% will become "my_robot#0", a third robot, "my_robot#1", and so on. +% +% DQ_SerialVrepRobot Methods: +% get_joint_names - Gets the joint names of the robot in the CoppeliaSim scene. +% set_configuration - Sets the joint configurations of the robot in the CoppeliaSim scene. +% get_configuration - Gets the joint configurations of the robot in the CoppeliaSim scene. +% set_target_configuration - Sets the joint configurations of the robot in the CoppeliaSim scene as a target configuration for the joint controllers. +% get_configuration_velocities - Gets the joint velocities of the robot in the CoppeliaSim scene. +% set_target_configuration_velocities - Sets the joint velocities of the robot in the CoppeliaSim scene as a target velocity for the joint controllers. +% set_configuration_space_torques - Sets the joint torques of the robot in the CoppeliaSim scene. +% get_configuration_space_torques - Gets the joint torques of the robot in the CoppeliaSim scene. + +% (C) Copyright 2011-2024 DQ Robotics Developers +% +% This file is part of DQ Robotics. +% +% DQ Robotics is free software: you can redistribute it and/or modify +% it under the terms of the GNU Lesser General Public License as published by +% the Free Software Foundation, either version 3 of the License, or +% (at your option) any later version. +% +% DQ Robotics is distributed in the hope that it will be useful, +% but WITHOUT ANY WARRANTY; without even the implied warranty of +% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +% GNU Lesser General Public License for more details. +% +% You should have received a copy of the GNU Lesser General Public License +% along with DQ Robotics. If not, see . +% +% DQ Robotics website: dqrobotics.github.io +% +% Contributors to this file: +% 1. Frederico Fernandes Afonso Silva (frederico.silva@ieee.org) +% - Responsible for the original implementation, based on the C++ version: +% - DQ_VrepInterface.h: https://github.com/dqrobotics/cpp-interface-vrep/blob/master/include/dqrobotics/interfaces/vrep/DQ_VrepInterface.h +% - DQ_SerialVrepRobot.cpp: https://github.com/dqrobotics/cpp-interface-vrep/blob/master/src/dqrobotics/interfaces/vrep/DQ_SerialVrepRobot.cpp + +classdef DQ_SerialVrepRobot < DQ_VrepRobot + properties + joint_names; + base_frame_name; + end + + methods + function obj = DQ_SerialVrepRobot(base_robot_name, robot_dof, robot_name, vrep_interface) + % This method constructs an instance of a DQ_SerialVrepRobot. + % Usage: + % DQ_SerialVrepRobot(base_robot_name, robot_dof, robot_name, vrep_interface); + % base_robot_name: The base name of the robot in the CoppeliaSim scene. + % robot_dof: The number of DoF of the robot in the CoppeliaSim scene. + % robot_name: The name of the robot in the CoppeliaSim scene. + % vrep_interface: The DQ_VrepInterface object connected to the CoppeliaSim scene. + % + % Example: + % vi = DQ_VrepInterface(); + % vi.connect('127.0.0.1',19997); + % vrep_robot = DQ_SerialVrepRobot("my_robot", 7, "my_robot#1", vi); + % + % Note that the name of the robot should be EXACTLY the same as in the CoppeliaSim + % scene. For instance, if you drag-and-drop a second robot, its name will become + % "my_robot#0", a third robot, "my_robot#1", and so on. + + obj.robot_name = robot_name; + obj.vrep_interface = vrep_interface; + + %% The use of 'initialize_joint_names_from_vrep()', as is done in the C++ implementation, is not supported on a constructor in MATLAB + % From the second copy of the robot and onward, VREP appends a + % #number in the robot's name. We check here if the robot is + % called by the correct name and assign an index that will be + % used to correctly infer the robot's joint labels. + splited_name = strsplit(obj.robot_name,'#'); + robot_label = splited_name{1}; + if ~strcmp(robot_label, base_robot_name) + error('Expected %s', base_robot_name) + end + if length(splited_name) > 1 + robot_index = splited_name{2}; + else + robot_index = ''; + end + + % Initialize joint names, link names, and base frame name + obj.joint_names = {}; + for i=1:robot_dof + current_joint_name = {robot_label,'_joint',int2str(i),robot_index}; + obj.joint_names{i} = strjoin(current_joint_name,''); + end + obj.base_frame_name = obj.joint_names{1}; + end + + function joint_names = get_joint_names(obj) + % This method gets the joint names of the robot in the CoppeliaSim scene. + % Usage: + % get_joint_names; + % + % Example: + % vi = DQ_VrepInterface(); + % vi.connect('127.0.0.1',19997); + % vrep_robot = DQ_SerialVrepRobot("my_robot", 7, "my_robot#1", vi); + % joint_names = vrep_robot.get_joint_names; + + joint_names = obj.joint_names; + end + + function set_configuration(obj, q) + % This method sets the joint configurations of the robot in the CoppeliaSim scene. + % Usage: + % set_configuration(q); + % q: The joint configurations of the robot in the CoppeliaSim scene. + % + % Example: + % vi = DQ_VrepInterface(); + % vi.connect('127.0.0.1',19997); + % vrep_robot = DQ_SerialVrepRobot("my_robot", 7, "my_robot#1", vi); + % q = zeros(7,1); + % vrep_robot.set_configuration(q); + % + % Note that this calls "set_joint_positions" in DQ_VrepInterface, meaning that it + % is only suitable for joints in kinematic mode. + + obj.vrep_interface.set_joint_positions(obj.joint_names, q) + end + + function q = get_configuration(obj) + % This method gets the joint configurations of the robot in the CoppeliaSim scene. + % Usage: + % get_configuration; + % + % Example: + % vi = DQ_VrepInterface(); + % vi.connect('127.0.0.1',19997); + % vrep_robot = DQ_SerialVrepRobot("my_robot", 7, "my_robot#1", vi); + % q = vrep_robot.get_configuration; + + q = obj.vrep_interface.get_joint_positions(obj.joint_names); + end + + function set_target_configuration(obj, q_target) + % This method sets the joint configurations of the robot in the CoppeliaSim scene as a target configuration for the joint controllers. + % Usage: + % set_target_configuration(q_target); + % q_target: The target joint configurations of the robot in the CoppeliaSim scene. + % + % Example: + % vi = DQ_VrepInterface(); + % vi.connect('127.0.0.1',19997); + % vrep_robot = DQ_SerialVrepRobot("my_robot", 7, "my_robot#1", vi); + % q_target = zeros(7,1); + % vrep_robot.set_target_configuration(q_target); + % + % Note that this calls "set_joint_target_positions" in DQ_VrepInterface, meaning that it + % is only suitable for joints in dynamic mode with position control. + + obj.vrep_interface.set_joint_target_positions(obj.joint_names, q_target) + end + + function qd = get_configuration_velocities(obj) + % This method gets the joint velocities of the robot in the CoppeliaSim scene. + % Usage: + % get_configuration_velocities; + % + % Example: + % vi = DQ_VrepInterface(); + % vi.connect('127.0.0.1',19997); + % vrep_robot = DQ_SerialVrepRobot("my_robot", 7, "my_robot#1", vi); + % qd = vrep_robot.get_configuration_velocities; + + qd = obj.vrep_interface.get_joint_velocities(obj.joint_names); + end + + function set_target_configuration_velocities(obj, v_target) + % This method sets the joint velocities of the robot in the CoppeliaSim scene as a target velocity for the joint controllers. + % Usage: + % set_target_configuration_velocities(v_target); + % v_target: The target joint velocities of the robot in the CoppeliaSim scene. + % + % Example: + % vi = DQ_VrepInterface(); + % vi.connect('127.0.0.1',19997); + % vrep_robot = DQ_SerialVrepRobot("my_robot", 7, "my_robot#1", vi); + % v_target = zeros(7,1); + % vrep_robot.set_target_configuration_velocities(v_target); + % + % Note that this calls "set_joint_target_velocities" in DQ_VrepInterface, meaning that it + % is only suitable for joints in dynamic mode with velocity control. + + obj.vrep_interface.set_joint_target_velocities(obj.joint_names, v_target); + end + + function set_configuration_space_torques(obj,tau) + % This method sets the joint torques of the robot in the CoppeliaSim scene. + % Usage: + % set_configuration_space_torques(tau); + % tau: The joint torques of the robot in the CoppeliaSim scene. + % + % Example: + % vi = DQ_VrepInterface(); + % vi.connect('127.0.0.1',19997); + % vrep_robot = DQ_SerialVrepRobot("my_robot", 7, "my_robot#1", vi); + % tau = zeros(7,1); + % vrep_robot.set_configuration_space_torques(tau); + % + % Note that this calls "set_joint_torques" in DQ_VrepInterface, meaning that it + % is only suitable for joints in dynamic mode with force/torque control. + + obj.vrep_interface.set_joint_torques(obj.joint_names,tau) + end + + function tau = get_configuration_space_torques(obj) + % This method gets the joint torques of the robot in the CoppeliaSim scene. + % Usage: + % get_configuration_space_torques; + % + % Example: + % vi = DQ_VrepInterface(); + % vi.connect('127.0.0.1',19997); + % vrep_robot = DQ_SerialVrepRobot("my_robot", 7, "my_robot#1", vi); + % tau = vrep_robot.get_configuration_space_torques; + + tau = obj.vrep_interface.get_joint_torques(obj.joint_names); + end + end +end \ No newline at end of file diff --git a/interfaces/vrep/DQ_VrepRobot.m b/interfaces/vrep/DQ_VrepRobot.m index fe6334ed..7c59b4a8 100644 --- a/interfaces/vrep/DQ_VrepRobot.m +++ b/interfaces/vrep/DQ_VrepRobot.m @@ -4,13 +4,11 @@ % Usage: % Inherit from this class and implement the abstract methods. % -% DQ_VrepRobot Methods: -% send_q_to_vrep - Sends the joint configurations to VREP -% get_q_from_vrep - Obtains the joint configurations from VREP -% kinematics - Obtains the DQ_Kinematics implementation of this -% robot +% DQ_VrepRobot Methods (Abstract): +% set_configuration - Sends the joint configurations to VREP +% get_configuration - Obtains the joint configurations from VREP -% (C) Copyright 2020 DQ Robotics Developers +% (C) Copyright 2018-2024 DQ Robotics Developers % % This file is part of DQ Robotics. % @@ -30,7 +28,16 @@ % DQ Robotics website: dqrobotics.sourceforge.net % % Contributors to this file: -% Murilo Marques Marinho - murilo@nml.t.u-tokyo.ac.jp +% 1. Murilo Marques Marinho - murilo@nml.t.u-tokyo.ac.jp +% - Responsible for the original implementation. +% 2. Frederico Fernandes Afonso Silva (frederico.silva@ieee.org) +% - Deprecated the following methods to ensure compatibility with the +% C++ version of the class: +% - 'send_q_to_vrep' +% - 'get_q_from_vrep' +% - Removed the following methods to ensure compatibility with the +% C++ version of the class: +% - 'kinematics' classdef (Abstract) DQ_VrepRobot @@ -40,9 +47,24 @@ end methods (Abstract) - send_q_to_vrep(obj,q); - q = get_q_from_vrep(obj); - kin = kinematics(obj); + set_configuration(obj,q); + q = get_configuration(obj); + end + + methods + function send_q_to_vrep(obj, q) + % For backwards compatibility only. Do not use this method. + + warning('Deprecated. Use set_configuration() instead.') + obj.set_configuration(q) + end + + function q = get_q_from_vrep(obj) + % For backwards compatibility only. Do not use this method. + + warning('Deprecated. Use get_configuration() instead.') + q = obj.get_configuration(); + end end end diff --git a/interfaces/vrep/robots/LBR4pVrepRobot.m b/interfaces/vrep/robots/LBR4pVrepRobot.m index 6e1be7cc..b020dfad 100644 --- a/interfaces/vrep/robots/LBR4pVrepRobot.m +++ b/interfaces/vrep/robots/LBR4pVrepRobot.m @@ -8,7 +8,7 @@ % >> vi.connect('127.0.0.1',19997); % >> vrep_robot = LBR4pVrepRobot("LBR4p", vi); % >> vi.start_simulation(); -% >> robot.get_q_from_vrep(); +% >> robot.get_configuration(); % >> pause(1); % >> vi.stop_simulation(); % >> vi.disconnect(); @@ -17,11 +17,9 @@ % will become "LBR4p#0", a third robot, "LBR4p#1", and so on. % % LBR4pVrepRobot Methods: -% send_q_to_vrep - Sends the joint configurations to VREP -% get_q_from_vrep - Obtains the joint configurations from VREP % kinematics - Obtains the DQ_Kinematics implementation of this robot -% (C) Copyright 2020 DQ Robotics Developers +% (C) Copyright 2011-2024 DQ Robotics Developers % % This file is part of DQ Robotics. % @@ -41,67 +39,21 @@ % DQ Robotics website: dqrobotics.sourceforge.net % % Contributors to this file: -% Murilo Marques Marinho - murilo@nml.t.u-tokyo.ac.jp +% 1. Murilo Marques Marinho - murilo@nml.t.u-tokyo.ac.jp +% - Responsible for the original implementation +% 2. Frederico Fernandes Afonso Silva (frederico.silva@ieee.org) +% - Updated for compatibility with the DQ_SerialVrepRobot class. -classdef LBR4pVrepRobot < DQ_VrepRobot - - properties - joint_names; - base_frame_name; - end - +classdef LBR4pVrepRobot < DQ_SerialVrepRobot methods - function obj = LBR4pVrepRobot(robot_name,vrep_interface) - %% Constructs an instance of a LBR4pVrepRobot - % >> vi = VrepInterface() - % >> vi.connect('127.0.0.1',19997); - % >> robot = LBR4pVrepRobot("LBR4p", vi) - obj.robot_name = robot_name; - obj.vrep_interface = vrep_interface; - - % From the second copy of the robot and onward, VREP appends a - % #number in the robot's name. We check here if the robot is - % called by the correct name and assign an index that will be - % used to correctly infer the robot's joint labels. - splited_name = strsplit(robot_name,'#'); - robot_label = splited_name{1}; - if ~strcmp(robot_label,'LBR4p') - error('Expected LBR4p') - end - if length(splited_name) > 1 - robot_index = splited_name{2}; - else - robot_index = ''; - end - - % Initialize joint names and base frame - obj.joint_names = {}; - for i=1:7 - current_joint_name = {robot_label,'_joint',int2str(i),robot_index}; - obj.joint_names{i} = strjoin(current_joint_name,''); - end - obj.base_frame_name = obj.joint_names{1}; - end - - function send_q_to_vrep(obj,q) - %% Sends the joint configurations to VREP - % >> vrep_robot = LBR4pVrepRobot("LBR4p", vi) - % >> q = zeros(7,1); - % >> vrep_robot.send_q_to_vrep(q) - obj.vrep_interface.set_joint_positions(obj.joint_names,q) + function obj = LBR4pVrepRobot(robot_name, vrep_interface) + obj@DQ_SerialVrepRobot("LBR4p", 7, robot_name, vrep_interface); end - - function q = get_q_from_vrep(obj) - %% Obtains the joint configurations from VREP - % >> vrep_robot = LBR4pVrepRobot("LBR4p", vi) - % >> q = vrep_robot.get_q_from_vrep(q) - q = obj.vrep_interface.get_joint_positions(obj.joint_names); - end - + function kin = kinematics(obj) %% Obtains the DQ_SerialManipulator instance that represents this LBR4p robot. - % >> vrep_robot = LBR4pVrepRobot("LBR4p", vi) - % >> robot_kinematics = vrep_robot.kinematics() + % >> vrep_robot = LBR4pVrepRobot("LBR4p", vi); + % >> robot_kinematics = vrep_robot.kinematics(); LBR4p_DH_theta=[0, 0, 0, 0, 0, 0, 0]; LBR4p_DH_d = [0.200, 0, 0.4, 0, 0.39, 0, 0]; @@ -119,8 +71,7 @@ function send_q_to_vrep(obj,q) kin.set_reference_frame(obj.vrep_interface.get_object_pose(obj.base_frame_name)); kin.set_base_frame(obj.vrep_interface.get_object_pose(obj.base_frame_name)); kin.set_effector(1+0.5*DQ.E*DQ.k*0.07); - end - + end end end diff --git a/interfaces/vrep/robots/YouBotVrepRobot.m b/interfaces/vrep/robots/YouBotVrepRobot.m index c63b408a..7ad10e34 100644 --- a/interfaces/vrep/robots/YouBotVrepRobot.m +++ b/interfaces/vrep/robots/YouBotVrepRobot.m @@ -8,7 +8,7 @@ % >> vi.connect('127.0.0.1',19997); % >> vrep_robot = YouBotVrepRobot("youBot", vi); % >> vi.start_simulation(); -% >> robot.get_q_from_vrep(); +% >> robot.get_configuration(); % >> pause(1); % >> vi.stop_simulation(); % >> vi.disconnect(); @@ -17,11 +17,11 @@ % will become "youBot#0", a third robot, "youBot#1", and so on. % % YouBotVrepRobot Methods: -% send_q_to_vrep - Sends the joint configurations to VREP -% get_q_from_vrep - Obtains the joint configurations from VREP +% set_configuration - Sends the joint configurations to VREP +% get_configuration - Obtains the joint configurations from VREP % kinematics - Obtains the DQ_Kinematics implementation of this robot -% (C) Copyright 2020 DQ Robotics Developers +% (C) Copyright 2011-2024 DQ Robotics Developers % % This file is part of DQ Robotics. % @@ -41,28 +41,21 @@ % DQ Robotics website: dqrobotics.sourceforge.net % % Contributors to this file: -% Murilo Marques Marinho - murilo@nml.t.u-tokyo.ac.jp +% 1. Murilo Marques Marinho - murilo@nml.t.u-tokyo.ac.jp +% - Responsible for the original implementation +% 2. Frederico Fernandes Afonso Silva (frederico.silva@ieee.org) +% - Updated for compatibility with the DQ_SerialVrepRobot class. -classdef YouBotVrepRobot < DQ_VrepRobot - - properties - joint_names; - base_frame_name; - end - +classdef YouBotVrepRobot < DQ_SerialVrepRobot properties (Constant) adjust = ((cos(pi/2) + DQ.i*sin(pi/2)) * (cos(pi/4) + DQ.j*sin(pi/4)))*(1+0.5*DQ.E*-0.1*DQ.k); end methods - function obj = YouBotVrepRobot(robot_name,vrep_interface) - %% Constructs an instance of a YouBotVrepRobot - % >> vi = VrepInterface() - % >> vi.connect('127.0.0.1',19997); - % >> robot = YouBotVrepRobot("youBot", vi) - obj.robot_name = robot_name; - obj.vrep_interface = vrep_interface; + function obj = YouBotVrepRobot(robot_name, vrep_interface) + obj@DQ_SerialVrepRobot("youBot", 5, robot_name, vrep_interface); + %% youBot does not follow the standard naming convention in CoppeliaSim. Also, the use of 'set_names()', as is done in the C++ implementation, is not supported on a constructor in MATLAB % From the second copy of the robot and onward, VREP appends a % #number in the robot's name. We check here if the robot is % called by the correct name and assign an index that will be @@ -87,11 +80,11 @@ obj.base_frame_name = robot_name; end - function send_q_to_vrep(obj,q) + function set_configuration(obj,q) %% Sends the joint configurations to VREP % >> vrep_robot = YouBotVrepRobot("youBot", vi) % >> q = zeros(8,1); - % >> vrep_robot.send_q_to_vrep(q) + % >> vrep_robot.set_configuration(q) x = q(1); y = q(2); phi = q(3); @@ -104,10 +97,10 @@ function send_q_to_vrep(obj,q) obj.vrep_interface.set_object_pose(obj.base_frame_name, pose * obj.adjust'); end - function q = get_q_from_vrep(obj) + function q = get_configuration(obj) %% Obtains the joint configurations from VREP % >> vrep_robot = YouBotVrepRobot("youBot", vi) - % >> q = vrep_robot.get_q_from_vrep(q) + % >> q = vrep_robot.get_configuration(q) base_x = obj.vrep_interface.get_object_pose(obj.base_frame_name) * obj.adjust; base_t = vec3(translation(base_x)); base_phi = rotation_angle(rotation(base_x)); @@ -150,8 +143,7 @@ function send_q_to_vrep(obj,q) effector = 1 + E_*0.5*0.3*k_; kin.set_effector(effector); - end - + end end end