Skip to content

This PR adds CoppeliaSim interface classes for serial robots #3

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 2 commits into
base: main
Choose a base branch
from
Open

This PR adds CoppeliaSim interface classes for serial robots #3

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
9506d9e
Add CoppeliaSim interface classes for serial robots
ffasilva Sep 10, 2025
c5b0f94
Rename class 'DQ_SerialCoppeliaSimRobotZMQ' to 'DQ_CoppeliaSimRobotZM…
ffasilva Sep 22, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
244 changes: 244 additions & 0 deletions interfaces/coppeliasim/DQ_CoppeliaSimRobotZMQ.m
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,244 @@
% (C) Copyright 2011-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 <http://www.gnu.org/licenses/>.
%
% DQ Robotics website: dqrobotics.github.io
%
% Contributors:
%
% 1. Frederico Fernandes Afonso Silva ([email protected])
% - Responsible for the original implementation

classdef DQ_CoppeliaSimRobotZMQ < DQ_CoppeliaSimRobot
properties (SetAccess = protected)
base_frame_name_;
joint_names_;
end

methods
function obj = DQ_CoppeliaSimRobotZMQ(robot_name, coppeliasim_interface)
% This method constructs an instance of a
% DQ_CoppeliaSimRobotZMQ object.
% Usage:
% DQ_CoppeliaSimRobotZMQ(robot_name, coppeliasim_interface)
% robot_name: The name of the robot in the CoppeliaSim
% scene.
% coppeliasim_interface: The DQ_CoppeliaSimInterfaceZMQ
% object connected to the CoppeliaSim scene.
%
% Example:
% cs = DQ_CoppeliaSimInterfaceZMQ();
% cs.connect;
% robot_coppeliasim = DQ_CoppeliaSimRobotZMQ("/LBR4p", cs);
%
% Note that the name of the robot should be EXACTLY the same
% as in CoppeliaSim. For instance, if you drag-and-drop a
% second robot to the scene, CoppeliaSim will rename the
% first robot to "/LBR4p[0]" and name the second one
% "/LBR4p[1]". A third robot will be named "/LBR4p[2]", and
% so on.

arguments
robot_name (1,:) {mustBeText}
coppeliasim_interface (1,1) DQ_CoppeliaSimInterfaceZMQ
end

obj@DQ_CoppeliaSimRobot;
obj.robot_name_ = robot_name;
obj.coppeliasim_interface_ = coppeliasim_interface;

obj.joint_names_ = ...
obj.coppeliasim_interface_.get_jointnames_from_object( ...
robot_name);
obj.base_frame_name_ = obj.joint_names_{1};
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 for the robot in the
% CoppeliaSim scene.
%
% Example:
% cs = DQ_CoppeliaSimInterfaceZMQ();
% cs.connect;
% robot_coppeliasim = DQ_CoppeliaSimRobotZMQ("/LBR4p", cs);
% q = zeros(7, 1);
% robot_coppeliasim.set_configuration(q);
%
% Note that this calls "set_joint_positions" in
% DQ_CoppeliaSimInterfaceZMQ, meaning that it requires a
% dynamics disabled scene.

arguments
obj (1,1) DQ_CoppeliaSimRobotZMQ
q (1,:) {mustBeNumeric}
end

obj.coppeliasim_interface_.set_joint_positions( ...
obj.joint_names_, q);
end

function set_target_configuration(obj, q)
% This method sets the target joint configurations for the robot
% in the CoppeliaSim scene.
% Usage:
% set_target_configuration(q);
% q: The target joint configurations for the robot in
% the CoppeliaSim scene.
%
% Example:
% cs = DQ_CoppeliaSimInterfaceZMQ();
% cs.connect;
% robot_coppeliasim = DQ_CoppeliaSimRobotZMQ("/LBR4p", cs);
% q = zeros(7, 1);
% robot_coppeliasim.set_target_configuration(q);
%
% Note that this calls "set_joint_target_positions" in
% DQ_CoppeliaSimInterfaceZMQ, meaning that it requires a
% dynamics dynamics-enabled scene and joints in dynamic
% mode with position control mode.

arguments
obj (1,1) DQ_CoppeliaSimRobotZMQ
q (1,:) {mustBeNumeric}
end

obj.coppeliasim_interface_.set_joint_target_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:
% cs = DQ_CoppeliaSimInterfaceZMQ();
% cs.connect;
% robot_coppeliasim = DQ_CoppeliaSimRobotZMQ("/LBR4p", cs);
% q = robot_coppeliasim.get_configuration;

arguments
obj (1,1) DQ_CoppeliaSimRobotZMQ
end

q = obj.coppeliasim_interface_.get_joint_positions( ...
obj.joint_names_);
end

function set_target_configuration_velocities(obj, dq)
% This method sets the target joint configuration velocities
% for the robot in the CoppeliaSim scene.
% Usage:
% set_target_configuration_velocities(dq);
% dq: The target joint configuration velocities for
% the robot in the CoppeliaSim scene.
%
% Example:
% cs = DQ_CoppeliaSimInterfaceZMQ();
% cs.connect;
% robot_coppeliasim = DQ_CoppeliaSimRobotZMQ("/LBR4p", cs);
% dq = zeros(7, 1);
% robot_coppeliasim.set_target_configuration_velocities(dq);
%
% Note that this calls "set_joint_target_velocities" in
% DQ_CoppeliaSimInterfaceZMQ, meaning that it requires a
% dynamics dynamics-enabled scene and joints in dynamic
% mode with velocity control mode.

arguments
obj (1,1) DQ_CoppeliaSimRobotZMQ
dq (1,:) {mustBeNumeric}
end

obj.coppeliasim_interface_.set_joint_target_velocities( ...
obj.joint_names_, dq);
end

function dq = get_configuration_velocities(obj)
% This method gets the joint configuration velocities of the
% robot in the CoppeliaSim scene.
% Usage:
% get_configuration_velocities();
%
% Example:
% cs = DQ_CoppeliaSimInterfaceZMQ();
% cs.connect;
% robot_coppeliasim = DQ_CoppeliaSimRobotZMQ("/LBR4p", cs);
% dq = robot_coppeliasim.get_configuration_velocities;

arguments
obj (1,1) DQ_CoppeliaSimRobotZMQ
end

dq = obj.coppeliasim_interface_.get_joint_velocities( ...
obj.joint_names_);
end

function set_target_configuration_forces(obj, tau)
% This method sets the target joint configuration forces for
% the robot in the CoppeliaSim scene.
% Usage:
% set_target_configuration_forces(tau);
% tau: The target joint configuration forces for the
% robot in the CoppeliaSim scene.
%
% Example:
% cs = DQ_CoppeliaSimInterfaceZMQ();
% cs.connect;
% robot_coppeliasim = DQ_CoppeliaSimRobotZMQ("/LBR4p", cs);
% tau = zeros(7, 1);
% robot_coppeliasim.set_target_configuration_forces(tau);
%
% Note that this calls "set_joint_target_forces" in
% DQ_CoppeliaSimInterfaceZMQ, meaning that it requires a
% dynamics dynamics-enabled scene and joints in dynamic
% mode with force control mode.

arguments
obj (1,1) DQ_CoppeliaSimRobotZMQ
tau (1,:) {mustBeNumeric}
end

obj.coppeliasim_interface_.set_joint_target_forces( ...
obj.joint_names_, tau);
end

function tau = get_configuration_forces(obj)
% This method gets the joint configuration forces of the
% robot in the CoppeliaSim scene.
% Usage:
% get_configuration_forces();
%
% Example:
% cs = DQ_CoppeliaSimInterfaceZMQ();
% cs.connect;
% robot_coppeliasim = DQ_CoppeliaSimRobotZMQ("/LBR4p", cs);
% tau = robot_coppeliasim.get_configuration_forces;

arguments
obj (1,1) DQ_CoppeliaSimRobotZMQ
end

tau = obj.coppeliasim_interface_.get_joint_forces( ...
obj.joint_names_);
end
end
end
119 changes: 119 additions & 0 deletions interfaces/coppeliasim/robots/LBR4pCoppeliaSimRobotZMQ.m
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,119 @@
% (C) Copyright 2011-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 <http://www.gnu.org/licenses/>.
%
% DQ Robotics website: dqrobotics.github.io
%
% Contributors:
%
% 1. Frederico Fernandes Afonso Silva ([email protected])
% - Responsible for the original implementation
%
%
% LBR4pCoppeliaSimRobotZMQ - Concrete class to interface with the "KUKA
% LBR4+" robot in CoppeliaSim.
%
% Usage:
%
% 1. Drag-and-drop a "KUKA LBR4+" robot to a CoppeliaSim scene.
%
% 2. Run
% >> cs = DQ_CoppeliaSimInterfaceZMQ();
% >> cs.connect;
% >> robot_coppeliasim = LBR4pVrepRobot("/LBR4p", cs);
% >> cs.start_simulation();
% >> robot_coppeliasim.get_configuration();
% >> pause(1);
% >> cs.stop_simulation();
% Note that the name of the robot should be EXACTLY the same as in
% CoppeliaSim. For instance, if you drag-and-drop a second robot to the
% scene, CoppeliaSim will rename the first robot to "/LBR4p[0]" and name
% the second one "/LBR4p[1]". A third robot will be named "/LBR4p[2]",
% and so on.

classdef LBR4pCoppeliaSimRobotZMQ < DQ_CoppeliaSimRobotZMQ
methods
function obj = LBR4pCoppeliaSimRobotZMQ(robot_name, coppeliasim_interface)
% This method constructs an instance of a KUKA LBR4+ robot.
% Usage:
% LBR4pCoppeliaSimRobotZMQ(robot_name, coppeliasim_interface)
% robot_name: The name of the robot in the CoppeliaSim
% scene.
% coppeliasim_interface: The DQ_CoppeliaSimInterfaceZMQ
% object connected to the CoppeliaSim scene.
%
% Example:
% cs = DQ_CoppeliaSimInterfaceZMQ();
% cs.connect;
% robot_coppeliasim = LBR4pCoppeliaSimRobotZMQ("/LBR4p", cs);
%
% Note that the name of the robot should be EXACTLY the same
% as in CoppeliaSim. For instance, if you drag-and-drop a
% second robot to the scene, CoppeliaSim will rename the
% first robot to "/LBR4p[0]" and name the second one
% "/LBR4p[1]". A third robot will be named "/LBR4p[2]", and
% so on.

arguments
robot_name (1,:) {mustBeText}
coppeliasim_interface (1,1) DQ_CoppeliaSimInterfaceZMQ
end

obj@DQ_CoppeliaSimRobotZMQ(robot_name, ...
coppeliasim_interface);
end

function kin = kinematics(obj)
% This method constructs an instance of a DQ_SerialManipulatorDH.
% Usage:
% kinematics();
%
% Example:
% cs = DQ_CoppeliaSimInterfaceZMQ();
% cs.connect;
% robot_coppeliasim = LBR4pCoppeliaSimRobotZMQ("/LBR4p", cs);
% robot_kinematics = robot_coppeliasim.kinematics();

arguments
obj (1,1) LBR4pCoppeliaSimRobotZMQ
end

LBR4p_DH_theta = [0, 0, 0, 0, 0, 0, 0];
LBR4p_DH_d = [0.200, 0, 0.4, 0, 0.39, 0, 0];
LBR4p_DH_a = [0, 0, 0, 0, 0, 0, 0];
LBR4p_DH_alpha = [pi/2, -pi/2, pi/2, -pi/2, pi/2, -pi/2, 0];
LBR4p_DH_type = double(repmat( ...
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@ffasilva the class member DQ_SerialManipulatorDH.JOINT_ROTATIONAL is deprecated. Please use the class DQ_JointType.

DQ_SerialManipulatorDH.JOINT_ROTATIONAL, 1, 7));
LBR4p_DH_matrix = [LBR4p_DH_theta;
LBR4p_DH_d;
LBR4p_DH_a;
LBR4p_DH_alpha
LBR4p_DH_type];

kin = DQ_SerialManipulatorDH(LBR4p_DH_matrix, 'standard');
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Hi @ffasilva, the constructor `DQ_SerialManipulatorDH(DH_matrix, 'standard') is deprecated. Please use the modern constructor of the class.


% Set the reference frames
kin.set_reference_frame( ...
obj.coppeliasim_interface_.get_object_pose( ...
obj.base_frame_name_));
kin.set_base_frame(obj.coppeliasim_interface_.get_object_pose( ...
obj.base_frame_name_));

% Set the end-effector
kin.set_effector(1 + 0.5*DQ.E*DQ.k*0.07);
end
end
end
Loading