From 881ed34f75e2373655977fcd32387440fa25c04e Mon Sep 17 00:00:00 2001 From: Juan Date: Thu, 13 Feb 2025 11:18:03 +0000 Subject: [PATCH 1/8] Added the DQ_CoppeliaSimInterface abstract class --- .../coppeliasim/DQ_CoppeliaSimInterface.m | 111 ++++++++++++++++++ 1 file changed, 111 insertions(+) create mode 100644 interfaces/coppeliasim/DQ_CoppeliaSimInterface.m diff --git a/interfaces/coppeliasim/DQ_CoppeliaSimInterface.m b/interfaces/coppeliasim/DQ_CoppeliaSimInterface.m new file mode 100644 index 0000000..34f2b35 --- /dev/null +++ b/interfaces/coppeliasim/DQ_CoppeliaSimInterface.m @@ -0,0 +1,111 @@ + +% (C) Copyright 2025 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: +% +% 1. Juan Jose Quiroz Omana (juanjose.quirozomana@manchester.ac.uk) +% - Responsible for the original implementation, which is based on +% https://github.com/dqrobotics/cpp-interface-coppeliasim/blob/main/include/dqrobotics/interfaces/coppeliasim/DQ_CoppeliaSimInterface.h + +classdef (Abstract) DQ_CoppeliaSimInterface < handle + + + methods(Abstract) + % This method connects to CoppeliaSim. + % Calling this function is required before anything else can happen. + connect(obj, host, port, TIMEOUT_IN_MILISECONDS); + + % This method enables or disables the stepped (synchronous) mode + % for the remote API server service that the client is connected to. + % Example: + % set_stepping_mode(true) % stepping mode enabled + % set_stepping_mode(false) % stepping mode disabled + set_stepping_mode(obj, flag); + + % This method sends trigger signal to the CoppeliaSim scene, + % which performs a simulation step when the stepping mode is used. + trigger_next_simulation_step(obj); + + % This method starts the CoppeliaSim simulation. + start_simulation(obj); + + % This method stops the CoppeliaSim simulation. + stop_simulation(obj); + + % This method gets the handles for a cell array of + % object names in the the CoppeliaSim scene. + handles = get_object_handles(obj,names); + + % This method gets the handle for a given object in the CoppeliaSim scene. + handle = get_object_handle(obj,name); + + % This method gets the translation of an object in the CoppeliaSim scene. + t = get_object_translation(obj,objectname); + + % This method sets the translation of an object in the CoppeliaSim scene. + set_object_translation(obj,objectname,translation); + + % This method gets the rotation of an object in the CoppeliaSim scene. + r = get_object_rotation(obj, objectname); + + % This method sets the rotation of an object in the CoppeliaSim scene. + set_object_rotation(obj,objectname,rotation); + + % This method gets the pose of an object in the CoppeliaSim scene. + x = get_object_pose(obj,objectname); + + % This method sets the pose of an object in the CoppeliaSim scene. + set_object_pose(obj,objectname,pose); + + % This method sets the joint positions of a robot in the CoppeliaSim scene. + set_joint_positions(obj,jointnames,joint_positions); + + % This method sets the joint positions of a robot in the CoppeliaSim scene. + joint_positions = get_joint_positions(obj,jointnames); + + % This method sets the joint target positions of a robot in the CoppeliaSim scene. + % It is required a dynamics enabled scene, and joints in dynamic mode + % with position control mode. + % information about joint modes: + % https://www.coppeliarobotics.com/helpFiles/en/jointModes.htm + set_joint_target_positions(obj,jointnames,joint_target_positions); + + % This method gets the joint velocities of a robot in the CoppeliaSim scene. + joint_velocities = get_joint_velocities(obj,jointnames); + + % This method sets the joint target velocities of a robot in the CoppeliaSim scene. + % It is required a dynamics enabled scene, and joints in dynamic mode + % with velocity control mode. Check this link for more + % information about joint modes: + % https://www.coppeliarobotics.com/helpFiles/en/jointModes.htm + set_joint_target_velocities(obj,jointnames,joint_target_velocities); + + % This method sets the joint torques of a robot in the CoppeliaSim scene. + % It is required a dynamics enabled scene, and joints in dynamic mode + % with velocity or force/torque control mode. Check this link for more + % information about joint modes: + % https://www.coppeliarobotics.com/helpFiles/en/jointModes.htm + set_joint_torques(obj,jointnames,torques); + + % This method gets the joint torques of a robot in the CoppeliaSim scene. + joint_torques = get_joint_torques(obj,jointnames); + end +end + From a96120032c5fe25b259424046826002f29db04fa Mon Sep 17 00:00:00 2001 From: Juan Date: Fri, 14 Feb 2025 15:47:15 +0000 Subject: [PATCH 2/8] [DQ_CoppeliaSimInterface] updated the signature of the connect method. --- interfaces/coppeliasim/DQ_CoppeliaSimInterface.m | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/interfaces/coppeliasim/DQ_CoppeliaSimInterface.m b/interfaces/coppeliasim/DQ_CoppeliaSimInterface.m index 34f2b35..3735193 100644 --- a/interfaces/coppeliasim/DQ_CoppeliaSimInterface.m +++ b/interfaces/coppeliasim/DQ_CoppeliaSimInterface.m @@ -30,7 +30,7 @@ methods(Abstract) % This method connects to CoppeliaSim. % Calling this function is required before anything else can happen. - connect(obj, host, port, TIMEOUT_IN_MILISECONDS); + status = connect(obj, host, port, TIMEOUT_IN_MILISECONDS); % This method enables or disables the stepped (synchronous) mode % for the remote API server service that the client is connected to. From 88bffd2a7d2be78c99cd24c0f92a79958cdb3cd7 Mon Sep 17 00:00:00 2001 From: Juan Date: Mon, 17 Feb 2025 11:01:38 +0000 Subject: [PATCH 3/8] [DQ_CoppeliaSimInterface.m] Updated the name of the argument in the methods get_object_handle and get_object_handles to match the C++ version. --- interfaces/coppeliasim/DQ_CoppeliaSimInterface.m | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/interfaces/coppeliasim/DQ_CoppeliaSimInterface.m b/interfaces/coppeliasim/DQ_CoppeliaSimInterface.m index 3735193..1ada01a 100644 --- a/interfaces/coppeliasim/DQ_CoppeliaSimInterface.m +++ b/interfaces/coppeliasim/DQ_CoppeliaSimInterface.m @@ -51,10 +51,10 @@ % This method gets the handles for a cell array of % object names in the the CoppeliaSim scene. - handles = get_object_handles(obj,names); + handles = get_object_handles(obj,objectnames); % This method gets the handle for a given object in the CoppeliaSim scene. - handle = get_object_handle(obj,name); + handle = get_object_handle(obj,objectname); % This method gets the translation of an object in the CoppeliaSim scene. t = get_object_translation(obj,objectname); From 5dd194ee72df81b7a6ed68b9c83bc1d9b9785d46 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juan=20Jos=C3=A9=20Quiroz=20Oma=C3=B1a?= Date: Thu, 27 Feb 2025 10:39:54 +0000 Subject: [PATCH 4/8] Update README.md --- README.md | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index 1d14a65..0d3ec79 100644 --- a/README.md +++ b/README.md @@ -1 +1,3 @@ -# matlab-interface-coppeliasim \ No newline at end of file +# matlab-interface-coppeliasim + +Contains the common interfaces used to interface DQ Robotics with CoppeliaSim From 95dc1a03917dc905b85fd8a726930c9a0af48091 Mon Sep 17 00:00:00 2001 From: Juancho Date: Mon, 10 Mar 2025 11:01:48 +0000 Subject: [PATCH 5/8] [DQ_CoppeliaSimInterface] Implemented all the style modifications proposed by Frederico. This includes the use of lowercase style for all arguments in the methods, and keep the same spacing between them. --- .../coppeliasim/DQ_CoppeliaSimInterface.m | 63 +++++++++---------- 1 file changed, 29 insertions(+), 34 deletions(-) diff --git a/interfaces/coppeliasim/DQ_CoppeliaSimInterface.m b/interfaces/coppeliasim/DQ_CoppeliaSimInterface.m index 1ada01a..813c243 100644 --- a/interfaces/coppeliasim/DQ_CoppeliaSimInterface.m +++ b/interfaces/coppeliasim/DQ_CoppeliaSimInterface.m @@ -1,5 +1,5 @@ -% (C) Copyright 2025 DQ Robotics Developers +% (C) Copyright 2011-2025 DQ Robotics Developers % % This file is part of DQ Robotics. % @@ -25,15 +25,13 @@ % https://github.com/dqrobotics/cpp-interface-coppeliasim/blob/main/include/dqrobotics/interfaces/coppeliasim/DQ_CoppeliaSimInterface.h classdef (Abstract) DQ_CoppeliaSimInterface < handle - - methods(Abstract) % This method connects to CoppeliaSim. % Calling this function is required before anything else can happen. - status = connect(obj, host, port, TIMEOUT_IN_MILISECONDS); + status = connect(obj, host, port, timeout_in_milliseconds); % This method enables or disables the stepped (synchronous) mode - % for the remote API server service that the client is connected to. + % for the remote API server service to which the client is connected to. % Example: % set_stepping_mode(true) % stepping mode enabled % set_stepping_mode(false) % stepping mode disabled @@ -51,61 +49,58 @@ % This method gets the handles for a cell array of % object names in the the CoppeliaSim scene. - handles = get_object_handles(obj,objectnames); + handles = get_object_handles(obj, objectnames); % This method gets the handle for a given object in the CoppeliaSim scene. - handle = get_object_handle(obj,objectname); + handle = get_object_handle(obj, objectname); % This method gets the translation of an object in the CoppeliaSim scene. - t = get_object_translation(obj,objectname); + t = get_object_translation(obj, objectname); % This method sets the translation of an object in the CoppeliaSim scene. - set_object_translation(obj,objectname,translation); + set_object_translation(obj, objectname, translation); % This method gets the rotation of an object in the CoppeliaSim scene. r = get_object_rotation(obj, objectname); % This method sets the rotation of an object in the CoppeliaSim scene. - set_object_rotation(obj,objectname,rotation); + set_object_rotation(obj, objectname, rotation); % This method gets the pose of an object in the CoppeliaSim scene. - x = get_object_pose(obj,objectname); + x = get_object_pose(obj, objectname); % This method sets the pose of an object in the CoppeliaSim scene. - set_object_pose(obj,objectname,pose); + set_object_pose(obj, objectname, pose); - % This method sets the joint positions of a robot in the CoppeliaSim scene. - set_joint_positions(obj,jointnames,joint_positions); + % This method sets the joint positions in the CoppeliaSim scene. + set_joint_positions(obj, jointnames, joint_positions); - % This method sets the joint positions of a robot in the CoppeliaSim scene. - joint_positions = get_joint_positions(obj,jointnames); + % This method gets the joint positions of a robot in the CoppeliaSim scene. + joint_positions = get_joint_positions(obj, jointnames); - % This method sets the joint target positions of a robot in the CoppeliaSim scene. - % It is required a dynamics enabled scene, and joints in dynamic mode - % with position control mode. - % information about joint modes: + % This method sets the joint target positions in the CoppeliaSim scene. + % It requires a dynamics-enabled scene and joints in dynamic mode with position control mode. + % For more information about joint modes: % https://www.coppeliarobotics.com/helpFiles/en/jointModes.htm - set_joint_target_positions(obj,jointnames,joint_target_positions); + set_joint_target_positions(obj, jointnames, joint_target_positions); - % This method gets the joint velocities of a robot in the CoppeliaSim scene. + % This method gets the joint velocities in the CoppeliaSim scene. joint_velocities = get_joint_velocities(obj,jointnames); - % This method sets the joint target velocities of a robot in the CoppeliaSim scene. - % It is required a dynamics enabled scene, and joints in dynamic mode - % with velocity control mode. Check this link for more - % information about joint modes: + % This method sets the joint target velocities in the CoppeliaSim scene. + % It requires a dynamics-enabled scene and joints in dynamic mode with velocity control mode. + % For more information about joint modes: % https://www.coppeliarobotics.com/helpFiles/en/jointModes.htm - set_joint_target_velocities(obj,jointnames,joint_target_velocities); + set_joint_target_velocities(obj, jointnames, joint_target_velocities); - % This method sets the joint torques of a robot in the CoppeliaSim scene. - % It is required a dynamics enabled scene, and joints in dynamic mode - % with velocity or force/torque control mode. Check this link for more - % information about joint modes: + % This method sets the joint torques in the CoppeliaSim scene. + % It requires a dynamics-enabled scene and joints in dynamic mode with force control mode. + % For more information about joint modes: % https://www.coppeliarobotics.com/helpFiles/en/jointModes.htm - set_joint_torques(obj,jointnames,torques); + set_joint_torques(obj, jointnames, torques); - % This method gets the joint torques of a robot in the CoppeliaSim scene. - joint_torques = get_joint_torques(obj,jointnames); + % This method gets the joint torques in the CoppeliaSim scene. + joint_torques = get_joint_torques(obj, jointnames); end end From a9f671d37ae6d0b96bded72a334a5a6c47e2892c Mon Sep 17 00:00:00 2001 From: Juancho Date: Mon, 10 Mar 2025 11:04:51 +0000 Subject: [PATCH 6/8] [DQ_CoppeliaSimInterface] Updated the documentation of the method get_joint_positions to match the style of the other methods. --- interfaces/coppeliasim/DQ_CoppeliaSimInterface.m | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/interfaces/coppeliasim/DQ_CoppeliaSimInterface.m b/interfaces/coppeliasim/DQ_CoppeliaSimInterface.m index 813c243..a838c9a 100644 --- a/interfaces/coppeliasim/DQ_CoppeliaSimInterface.m +++ b/interfaces/coppeliasim/DQ_CoppeliaSimInterface.m @@ -75,7 +75,7 @@ % This method sets the joint positions in the CoppeliaSim scene. set_joint_positions(obj, jointnames, joint_positions); - % This method gets the joint positions of a robot in the CoppeliaSim scene. + % This method gets the joint positions in the CoppeliaSim scene. joint_positions = get_joint_positions(obj, jointnames); % This method sets the joint target positions in the CoppeliaSim scene. From ca60dfe822563331ef59f90e7bfa08a5c7362457 Mon Sep 17 00:00:00 2001 From: Juancho Date: Mon, 10 Mar 2025 11:12:12 +0000 Subject: [PATCH 7/8] [DQ_CoppeliaSimInterface] Renamed the methods set,get_joint_torques to set_joint_target_forces and get_joint_forces to comply the style of the set/get methods related to joint positions, and velocities. --- interfaces/coppeliasim/DQ_CoppeliaSimInterface.m | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/interfaces/coppeliasim/DQ_CoppeliaSimInterface.m b/interfaces/coppeliasim/DQ_CoppeliaSimInterface.m index a838c9a..49a863b 100644 --- a/interfaces/coppeliasim/DQ_CoppeliaSimInterface.m +++ b/interfaces/coppeliasim/DQ_CoppeliaSimInterface.m @@ -93,14 +93,14 @@ % https://www.coppeliarobotics.com/helpFiles/en/jointModes.htm set_joint_target_velocities(obj, jointnames, joint_target_velocities); - % This method sets the joint torques in the CoppeliaSim scene. + % This method sets the joint target forces in the CoppeliaSim scene. % It requires a dynamics-enabled scene and joints in dynamic mode with force control mode. % For more information about joint modes: % https://www.coppeliarobotics.com/helpFiles/en/jointModes.htm - set_joint_torques(obj, jointnames, torques); + set_joint_target_forces(obj, jointnames, torques); - % This method gets the joint torques in the CoppeliaSim scene. - joint_torques = get_joint_torques(obj, jointnames); + % This method gets the joint forces in the CoppeliaSim scene. + joint_torques = get_joint_forces(obj, jointnames); end end From 3c19375dd663daf30cf547f06dfce3e9bda2d4fa Mon Sep 17 00:00:00 2001 From: Juancho Date: Tue, 11 Mar 2025 12:03:57 +0000 Subject: [PATCH 8/8] [DQ_CoppeliaSimInterface, README] Remove extra spaces and updated the readme file. --- README.md | 2 +- interfaces/coppeliasim/DQ_CoppeliaSimInterface.m | 3 +-- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/README.md b/README.md index 0d3ec79..651bcaa 100644 --- a/README.md +++ b/README.md @@ -1,3 +1,3 @@ # matlab-interface-coppeliasim -Contains the common interfaces used to interface DQ Robotics with CoppeliaSim +Abstract class defining the DQ Robotics interface with CoppeliaSim in MATLAB diff --git a/interfaces/coppeliasim/DQ_CoppeliaSimInterface.m b/interfaces/coppeliasim/DQ_CoppeliaSimInterface.m index 49a863b..dcb08a7 100644 --- a/interfaces/coppeliasim/DQ_CoppeliaSimInterface.m +++ b/interfaces/coppeliasim/DQ_CoppeliaSimInterface.m @@ -1,4 +1,3 @@ - % (C) Copyright 2011-2025 DQ Robotics Developers % % This file is part of DQ Robotics. @@ -85,7 +84,7 @@ set_joint_target_positions(obj, jointnames, joint_target_positions); % This method gets the joint velocities in the CoppeliaSim scene. - joint_velocities = get_joint_velocities(obj,jointnames); + joint_velocities = get_joint_velocities(obj, jointnames); % This method sets the joint target velocities in the CoppeliaSim scene. % It requires a dynamics-enabled scene and joints in dynamic mode with velocity control mode.