Skip to content

Commit 1eb3459

Browse files
ffasilvaffasilva
authored
This PR adds the class 'DQ_SerialVrepRobot' (#108)
* 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.
1 parent ab09d44 commit 1eb3459

File tree

4 files changed

+302
-97
lines changed

4 files changed

+302
-97
lines changed

‎interfaces/vrep/DQ_SerialVrepRobot.m‎

Lines changed: 240 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,240 @@
1+
% CLASS DQ_SerialVrepRobot - Concrete class to interface with serial robots
2+
% in VREP.
3+
%
4+
% Usage:
5+
% 1) Drag-and-drop a serial robot to a VREP scene. For instance, a
6+
% "my_robot" robot.
7+
% 2) Run
8+
% >> vi = DQ_VrepInterface();
9+
% >> vi.connect('127.0.0.1',19997);
10+
% >> vrep_robot = DQ_SerialVrepRobot("my_robot", 7, "my_robot", vi);
11+
% >> vi.start_simulation();
12+
% >> vrep_robot.get_configuration();
13+
% >> pause(1);
14+
% >> vi.stop_simulation();
15+
% >> vi.disconnect();
16+
% Note that the name of the robot should be EXACTLY the same as in
17+
% VREP. For instance, if you drag-and-drop a second robot, its name
18+
% will become "my_robot#0", a third robot, "my_robot#1", and so on.
19+
%
20+
% DQ_SerialVrepRobot Methods:
21+
% get_joint_names - Gets the joint names of the robot in the CoppeliaSim scene.
22+
% set_configuration - Sets the joint configurations of the robot in the CoppeliaSim scene.
23+
% get_configuration - Gets the joint configurations of the robot in the CoppeliaSim scene.
24+
% set_target_configuration - Sets the joint configurations of the robot in the CoppeliaSim scene as a target configuration for the joint controllers.
25+
% get_configuration_velocities - Gets the joint velocities of the robot in the CoppeliaSim scene.
26+
% set_target_configuration_velocities - Sets the joint velocities of the robot in the CoppeliaSim scene as a target velocity for the joint controllers.
27+
% set_configuration_space_torques - Sets the joint torques of the robot in the CoppeliaSim scene.
28+
% get_configuration_space_torques - Gets the joint torques of the robot in the CoppeliaSim scene.
29+
30+
% (C) Copyright 2011-2024 DQ Robotics Developers
31+
%
32+
% This file is part of DQ Robotics.
33+
%
34+
% DQ Robotics is free software: you can redistribute it and/or modify
35+
% it under the terms of the GNU Lesser General Public License as published by
36+
% the Free Software Foundation, either version 3 of the License, or
37+
% (at your option) any later version.
38+
%
39+
% DQ Robotics is distributed in the hope that it will be useful,
40+
% but WITHOUT ANY WARRANTY; without even the implied warranty of
41+
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
42+
% GNU Lesser General Public License for more details.
43+
%
44+
% You should have received a copy of the GNU Lesser General Public License
45+
% along with DQ Robotics. If not, see <http://www.gnu.org/licenses/>.
46+
%
47+
% DQ Robotics website: dqrobotics.github.io
48+
%
49+
% Contributors to this file:
50+
% 1. Frederico Fernandes Afonso Silva (frederico.silva@ieee.org)
51+
% - Responsible for the original implementation, based on the C++ version:
52+
% - DQ_VrepInterface.h: https://github.com/dqrobotics/cpp-interface-vrep/blob/master/include/dqrobotics/interfaces/vrep/DQ_VrepInterface.h
53+
% - DQ_SerialVrepRobot.cpp: https://github.com/dqrobotics/cpp-interface-vrep/blob/master/src/dqrobotics/interfaces/vrep/DQ_SerialVrepRobot.cpp
54+
55+
classdef DQ_SerialVrepRobot < DQ_VrepRobot
56+
properties
57+
joint_names;
58+
base_frame_name;
59+
end
60+
61+
methods
62+
function obj = DQ_SerialVrepRobot(base_robot_name, robot_dof, robot_name, vrep_interface)
63+
% This method constructs an instance of a DQ_SerialVrepRobot.
64+
% Usage:
65+
% DQ_SerialVrepRobot(base_robot_name, robot_dof, robot_name, vrep_interface);
66+
% base_robot_name: The base name of the robot in the CoppeliaSim scene.
67+
% robot_dof: The number of DoF of the robot in the CoppeliaSim scene.
68+
% robot_name: The name of the robot in the CoppeliaSim scene.
69+
% vrep_interface: The DQ_VrepInterface object connected to the CoppeliaSim scene.
70+
%
71+
% Example:
72+
% vi = DQ_VrepInterface();
73+
% vi.connect('127.0.0.1',19997);
74+
% vrep_robot = DQ_SerialVrepRobot("my_robot", 7, "my_robot#1", vi);
75+
%
76+
% Note that the name of the robot should be EXACTLY the same as in the CoppeliaSim
77+
% scene. For instance, if you drag-and-drop a second robot, its name will become
78+
% "my_robot#0", a third robot, "my_robot#1", and so on.
79+
80+
obj.robot_name = robot_name;
81+
obj.vrep_interface = vrep_interface;
82+
83+
%% The use of 'initialize_joint_names_from_vrep()', as is done in the C++ implementation, is not supported on a constructor in MATLAB
84+
% From the second copy of the robot and onward, VREP appends a
85+
% #number in the robot's name. We check here if the robot is
86+
% called by the correct name and assign an index that will be
87+
% used to correctly infer the robot's joint labels.
88+
splited_name = strsplit(obj.robot_name,'#');
89+
robot_label = splited_name{1};
90+
if ~strcmp(robot_label, base_robot_name)
91+
error('Expected %s', base_robot_name)
92+
end
93+
if length(splited_name) > 1
94+
robot_index = splited_name{2};
95+
else
96+
robot_index = '';
97+
end
98+
99+
% Initialize joint names, link names, and base frame name
100+
obj.joint_names = {};
101+
for i=1:robot_dof
102+
current_joint_name = {robot_label,'_joint',int2str(i),robot_index};
103+
obj.joint_names{i} = strjoin(current_joint_name,'');
104+
end
105+
obj.base_frame_name = obj.joint_names{1};
106+
end
107+
108+
function joint_names = get_joint_names(obj)
109+
% This method gets the joint names of the robot in the CoppeliaSim scene.
110+
% Usage:
111+
% get_joint_names;
112+
%
113+
% Example:
114+
% vi = DQ_VrepInterface();
115+
% vi.connect('127.0.0.1',19997);
116+
% vrep_robot = DQ_SerialVrepRobot("my_robot", 7, "my_robot#1", vi);
117+
% joint_names = vrep_robot.get_joint_names;
118+
119+
joint_names = obj.joint_names;
120+
end
121+
122+
function set_configuration(obj, q)
123+
% This method sets the joint configurations of the robot in the CoppeliaSim scene.
124+
% Usage:
125+
% set_configuration(q);
126+
% q: The joint configurations of the robot in the CoppeliaSim scene.
127+
%
128+
% Example:
129+
% vi = DQ_VrepInterface();
130+
% vi.connect('127.0.0.1',19997);
131+
% vrep_robot = DQ_SerialVrepRobot("my_robot", 7, "my_robot#1", vi);
132+
% q = zeros(7,1);
133+
% vrep_robot.set_configuration(q);
134+
%
135+
% Note that this calls "set_joint_positions" in DQ_VrepInterface, meaning that it
136+
% is only suitable for joints in kinematic mode.
137+
138+
obj.vrep_interface.set_joint_positions(obj.joint_names, q)
139+
end
140+
141+
function q = get_configuration(obj)
142+
% This method gets the joint configurations of the robot in the CoppeliaSim scene.
143+
% Usage:
144+
% get_configuration;
145+
%
146+
% Example:
147+
% vi = DQ_VrepInterface();
148+
% vi.connect('127.0.0.1',19997);
149+
% vrep_robot = DQ_SerialVrepRobot("my_robot", 7, "my_robot#1", vi);
150+
% q = vrep_robot.get_configuration;
151+
152+
q = obj.vrep_interface.get_joint_positions(obj.joint_names);
153+
end
154+
155+
function set_target_configuration(obj, q_target)
156+
% This method sets the joint configurations of the robot in the CoppeliaSim scene as a target configuration for the joint controllers.
157+
% Usage:
158+
% set_target_configuration(q_target);
159+
% q_target: The target joint configurations of the robot in the CoppeliaSim scene.
160+
%
161+
% Example:
162+
% vi = DQ_VrepInterface();
163+
% vi.connect('127.0.0.1',19997);
164+
% vrep_robot = DQ_SerialVrepRobot("my_robot", 7, "my_robot#1", vi);
165+
% q_target = zeros(7,1);
166+
% vrep_robot.set_target_configuration(q_target);
167+
%
168+
% Note that this calls "set_joint_target_positions" in DQ_VrepInterface, meaning that it
169+
% is only suitable for joints in dynamic mode with position control.
170+
171+
obj.vrep_interface.set_joint_target_positions(obj.joint_names, q_target)
172+
end
173+
174+
function qd = get_configuration_velocities(obj)
175+
% This method gets the joint velocities of the robot in the CoppeliaSim scene.
176+
% Usage:
177+
% get_configuration_velocities;
178+
%
179+
% Example:
180+
% vi = DQ_VrepInterface();
181+
% vi.connect('127.0.0.1',19997);
182+
% vrep_robot = DQ_SerialVrepRobot("my_robot", 7, "my_robot#1", vi);
183+
% qd = vrep_robot.get_configuration_velocities;
184+
185+
qd = obj.vrep_interface.get_joint_velocities(obj.joint_names);
186+
end
187+
188+
function set_target_configuration_velocities(obj, v_target)
189+
% This method sets the joint velocities of the robot in the CoppeliaSim scene as a target velocity for the joint controllers.
190+
% Usage:
191+
% set_target_configuration_velocities(v_target);
192+
% v_target: The target joint velocities of the robot in the CoppeliaSim scene.
193+
%
194+
% Example:
195+
% vi = DQ_VrepInterface();
196+
% vi.connect('127.0.0.1',19997);
197+
% vrep_robot = DQ_SerialVrepRobot("my_robot", 7, "my_robot#1", vi);
198+
% v_target = zeros(7,1);
199+
% vrep_robot.set_target_configuration_velocities(v_target);
200+
%
201+
% Note that this calls "set_joint_target_velocities" in DQ_VrepInterface, meaning that it
202+
% is only suitable for joints in dynamic mode with velocity control.
203+
204+
obj.vrep_interface.set_joint_target_velocities(obj.joint_names, v_target);
205+
end
206+
207+
function set_configuration_space_torques(obj,tau)
208+
% This method sets the joint torques of the robot in the CoppeliaSim scene.
209+
% Usage:
210+
% set_configuration_space_torques(tau);
211+
% tau: The joint torques of the robot in the CoppeliaSim scene.
212+
%
213+
% Example:
214+
% vi = DQ_VrepInterface();
215+
% vi.connect('127.0.0.1',19997);
216+
% vrep_robot = DQ_SerialVrepRobot("my_robot", 7, "my_robot#1", vi);
217+
% tau = zeros(7,1);
218+
% vrep_robot.set_configuration_space_torques(tau);
219+
%
220+
% Note that this calls "set_joint_torques" in DQ_VrepInterface, meaning that it
221+
% is only suitable for joints in dynamic mode with force/torque control.
222+
223+
obj.vrep_interface.set_joint_torques(obj.joint_names,tau)
224+
end
225+
226+
function tau = get_configuration_space_torques(obj)
227+
% This method gets the joint torques of the robot in the CoppeliaSim scene.
228+
% Usage:
229+
% get_configuration_space_torques;
230+
%
231+
% Example:
232+
% vi = DQ_VrepInterface();
233+
% vi.connect('127.0.0.1',19997);
234+
% vrep_robot = DQ_SerialVrepRobot("my_robot", 7, "my_robot#1", vi);
235+
% tau = vrep_robot.get_configuration_space_torques;
236+
237+
tau = obj.vrep_interface.get_joint_torques(obj.joint_names);
238+
end
239+
end
240+
end

‎interfaces/vrep/DQ_VrepRobot.m‎

Lines changed: 32 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -4,13 +4,11 @@
44
% Usage:
55
% Inherit from this class and implement the abstract methods.
66
%
7-
% DQ_VrepRobot Methods:
8-
% send_q_to_vrep - Sends the joint configurations to VREP
9-
% get_q_from_vrep - Obtains the joint configurations from VREP
10-
% kinematics - Obtains the DQ_Kinematics implementation of this
11-
% robot
7+
% DQ_VrepRobot Methods (Abstract):
8+
% set_configuration - Sends the joint configurations to VREP
9+
% get_configuration - Obtains the joint configurations from VREP
1210

13-
% (C) Copyright 2020 DQ Robotics Developers
11+
% (C) Copyright 2018-2024 DQ Robotics Developers
1412
%
1513
% This file is part of DQ Robotics.
1614
%
@@ -30,7 +28,16 @@
3028
% DQ Robotics website: dqrobotics.sourceforge.net
3129
%
3230
% Contributors to this file:
33-
% Murilo Marques Marinho - murilo@nml.t.u-tokyo.ac.jp
31+
% 1. Murilo Marques Marinho - murilo@nml.t.u-tokyo.ac.jp
32+
% - Responsible for the original implementation.
33+
% 2. Frederico Fernandes Afonso Silva (frederico.silva@ieee.org)
34+
% - Deprecated the following methods to ensure compatibility with the
35+
% C++ version of the class:
36+
% - 'send_q_to_vrep'
37+
% - 'get_q_from_vrep'
38+
% - Removed the following methods to ensure compatibility with the
39+
% C++ version of the class:
40+
% - 'kinematics'
3441

3542
classdef (Abstract) DQ_VrepRobot
3643

@@ -40,9 +47,24 @@
4047
end
4148

4249
methods (Abstract)
43-
send_q_to_vrep(obj,q);
44-
q = get_q_from_vrep(obj);
45-
kin = kinematics(obj);
50+
set_configuration(obj,q);
51+
q = get_configuration(obj);
52+
end
53+
54+
methods
55+
function send_q_to_vrep(obj, q)
56+
% For backwards compatibility only. Do not use this method.
57+
58+
warning('Deprecated. Use set_configuration() instead.')
59+
obj.set_configuration(q)
60+
end
61+
62+
function q = get_q_from_vrep(obj)
63+
% For backwards compatibility only. Do not use this method.
64+
65+
warning('Deprecated. Use get_configuration() instead.')
66+
q = obj.get_configuration();
67+
end
4668
end
4769
end
4870

0 commit comments

Comments
 (0)