Skip to content

Commit

Permalink
This PR adds the class 'DQ_SerialVrepRobot' (#108)
Browse files Browse the repository at this point in the history 
* Added the class 'DQ_SerialVrepRobot'. Also modified the class 'DQ_VrepRobot' as required by 'DQ_SerialVrepRobot'.

* Updated classes 'LBR4pVrepRobot' and 'YouBotVrepRobot' for compatibility with the class 'DQ_SerialVrepRobot'.

* Fixed a problem with joint naming on the classes 'YouBotVrepRobot' and 'DQ_SerialVrepRobot'.

* [DQ_SerialVrepRobot] Improved documentation of DQ_SerialVrepRobot's methods on the class header.

* [DQ_SerialVrepRobot.m] Removed old comment refering to 'get_q_from_vrep()' method.

* [DQ_SerialVrepRobot.m] Updated copyright's year.

* [DQ_SerialVrepRobot.m] Added reference to C++ version.

* [DQ_SerialVrepRobot.m] Text fixes in the 'set' methods comments.

* [DQ_SerialVrepRobot.m] Updated specifications of the joint modes in the methods's comments.

* [DQ_SerialVrepRobot.m] Fixed copy and paste gore in the methods' usage comments.

* [DQ_VrepRobot.m] Added backwards compatibility for methods 'send_q_to_vrep()' and 'get_q_from_vrep()'.

* [LBR4pVrepRobot.m] Updated usage and methods sections. Also updated copyright's year.

* [LBR4pVrepRobot.m] Updated copyright's year.

* [YouBotVrepRobot.m] Updated usage section and copyright's year.

* [DQ_VrepRobot.m] Updated methods section and copyright's year.

* [YouBotVrepRobot.m] Small text fix in the constructor's comments.

* [YouBotVrepRobot.m] Fixed small wrong number of joints and missing custom 'base_frame_name' in the constructor.

* [DQ_SerialVrepRobot.m] Updated copyright date.

* [DQ_SerialVrepRobot.m] Updated usage comments.

* [DQ_SerialVrepRobot.m] Updated comment describing 'get_configuration' method.

* [DQ_SerialVrepRobot.m] Updated comment describing 'set_configuration' method.

* [DQ_SerialVrepRobot.m] Updated comment describing 'set_target_configuration' method.

* [DQ_SerialVrepRobot.m] Updated comment describing 'get_configuration_velocities' method.

* [DQ_SerialVrepRobot.m] Updated comment describing 'set_target_configuration_velocities' method.

* [DQ_SerialVrepRobot.m] Removed the comment describing a method not implemented in this pull request.

* [DQ_SerialVrepRobot.m] Renamed methods' names as requested by Bruno.

* [DQ_VrepRobot.m] Renamed methods' as requested by Bruno.

* [LBR4pVrepRobot.m] Updated copyright.

* [LBR4pVrepRobot.m] Updated usage comments.

* [YouBotVrepRobot.m] Renamed 'get_configuration_space_positions' to 'get_configuration'.

* [YouBotVrepRobot.m] Renamed 'set_configuration_space_positions' to 'set_configuration'.

* [YouBotVrepRobot.m] Updated copyright.
  • Loading branch information
@ffasilva
ffasilva authored Aug 16, 2024
1 parent ab09d44 commit 1eb3459
Show file tree
Hide file tree
Showing 4 changed files with 302 additions and 97 deletions.
240 changes: 240 additions & 0 deletions interfaces/vrep/DQ_SerialVrepRobot.m
View file
Original file line number Diff line number Diff line change
@@ -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 <http://www.gnu.org/licenses/>.
%
% 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
42 changes: 32 additions & 10 deletions interfaces/vrep/DQ_VrepRobot.m
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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.
%
Expand All @@ -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

Expand All @@ -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

Loading

0 comments on commit 1eb3459

Please sign in to comment.