From c2acba1d26896cf2b10e8d0f68ef0aed1bfdf8b5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juan=20Jos=C3=A9=20Quiroz=20Oma=C3=B1a?= Date: Tue, 4 Mar 2025 21:29:02 +0000 Subject: [PATCH] Add `set_stepping_mode`, `get_object_handles`, remove extra string from signatures, and remove protected C++ methods. Fix #62, Fix #61, and Fix #60 (#63) * [DQ_CoppeliaSimInterfaceZMQ_py.cpp] Updated the copyright year and added the default connect method. * [DQ_CoppeliaSimInterfaceZMQ_py.cpp] Added the set_stepping_mode. * [DQ_CoppeliaSimInterfaceZMQ_py.cpp] Added the methdos get_object_handles{s}. * [DQ_CoppeliaSimInterfaceZMQ_py.cpp] Removed the bindings of protected C++ methods as set_joint_position, get_joint_position. * [DQ_CoppeliaSimInterfaceZMQ_py.cpp] Fixed the signature of some bindings as set and get object pose. * [DQ_CoppeliaSimInterfaceZMQ_py.cpp] Added default arguments in the connect method --- .../DQ_CoppeliaSimInterfaceZMQ_py.cpp | 50 ++++++++++++++----- 1 file changed, 37 insertions(+), 13 deletions(-) diff --git a/src/interfaces/coppeliasim/DQ_CoppeliaSimInterfaceZMQ_py.cpp b/src/interfaces/coppeliasim/DQ_CoppeliaSimInterfaceZMQ_py.cpp index dacd3a9..70ec91e 100644 --- a/src/interfaces/coppeliasim/DQ_CoppeliaSimInterfaceZMQ_py.cpp +++ b/src/interfaces/coppeliasim/DQ_CoppeliaSimInterfaceZMQ_py.cpp @@ -1,5 +1,5 @@ /** -(C) Copyright 2019-2024 DQ Robotics Developers +(C) Copyright 2019-2025 DQ Robotics Developers This file is part of DQ Robotics. @@ -20,6 +20,11 @@ This file is part of DQ Robotics. 1. Murilo M. Marinho (murilomarinho@ieee.org) - Initial implementation. +2. Juan Jose Quiroz Omana (juanjose.quirozomana@manchester.ac.uk) + - Added bindings for the following methods + set_stepping_mode(), get_object_handle(), get_object_handle() + get_joint_{velocities, torques}(), set_joint_target_velocities(),set_joint_torques() + */ #include "../../dqrobotics_module.h" @@ -38,6 +43,9 @@ void init_DQ_CoppeliaSimInterfaceZMQ_py(py::module& m) > dqcsinterfacezmq_py(m,"DQ_CoppeliaSimInterfaceZMQ"); dqcsinterfacezmq_py.def(py::init<>()); + dqcsinterfacezmq_py.def("connect",(bool (DQ_CoppeliaSimInterfaceZMQ::*) (const std::string&, const int&, const int&))&DQ_CoppeliaSimInterfaceZMQ::connect, + py::arg("host") = "localhost", py::arg("port") = 23000, py::arg("TIMEOUT_IN_MILISECONDS") = 2000, + "establishes a connection between the client (your code) and the host (the computer running the CoppeliaSim scene."); dqcsinterfacezmq_py.def("connect",(bool (DQ_CoppeliaSimInterfaceZMQ::*) (const std::string&, const int&, const int&, const int&))&DQ_CoppeliaSimInterfaceZMQ::connect,"Connects to CoppeliaSim with a given ip."); dqcsinterfacezmq_py.def("disconnect", &DQ_CoppeliaSimInterfaceZMQ::disconnect,"Disconnects from CoppeliaSim."); @@ -46,6 +54,7 @@ void init_DQ_CoppeliaSimInterfaceZMQ_py(py::module& m) dqcsinterfacezmq_py.def("start_simulation",&DQ_CoppeliaSimInterfaceZMQ::start_simulation,"Start simulation"); dqcsinterfacezmq_py.def("stop_simulation", &DQ_CoppeliaSimInterfaceZMQ::stop_simulation,"Stops simulation"); + dqcsinterfacezmq_py.def("set_stepping_mode", (void (DQ_CoppeliaSimInterfaceZMQ::*) (const bool&))&DQ_CoppeliaSimInterfaceZMQ::set_stepping_mode, "enables or disables the stepping mode (formerly known as synchronous mode)."); dqcsinterfacezmq_py.def("set_synchronous", (void (DQ_CoppeliaSimInterfaceZMQ::*) (const bool&))&DQ_CoppeliaSimInterfaceZMQ::set_synchronous, "Sets synchronous mode"); dqcsinterfacezmq_py.def("trigger_next_simulation_step", &DQ_CoppeliaSimInterfaceZMQ::trigger_next_simulation_step, "Sends a synchronization trigger signal to the server."); @@ -57,7 +66,7 @@ void init_DQ_CoppeliaSimInterfaceZMQ_py(py::module& m) "Gets object translation."); dqcsinterfacezmq_py.def("set_object_translation", - (void (DQ_CoppeliaSimInterfaceZMQ::*) (const std::string&, const DQ&, const std::string&))&DQ_CoppeliaSimInterfaceZMQ::set_object_translation, + (void (DQ_CoppeliaSimInterfaceZMQ::*) (const std::string&, const DQ&))&DQ_CoppeliaSimInterfaceZMQ::set_object_translation, "Sets object translation."); dqcsinterfacezmq_py.def("get_object_rotation", @@ -73,20 +82,16 @@ void init_DQ_CoppeliaSimInterfaceZMQ_py(py::module& m) "Gets object pose."); dqcsinterfacezmq_py.def("set_object_pose", - (void (DQ_CoppeliaSimInterfaceZMQ::*) (const std::string&, const DQ&, const std::string&))&DQ_CoppeliaSimInterfaceZMQ::set_object_pose, + (void (DQ_CoppeliaSimInterfaceZMQ::*) (const std::string&, const DQ&))&DQ_CoppeliaSimInterfaceZMQ::set_object_pose, "Sets object pose."); - dqcsinterfacezmq_py.def("set_joint_position", - (void (DQ_CoppeliaSimInterfaceZMQ::*) (const std::string&, const double&))&::DQ_CoppeliaSimInterfaceZMQ::set_joint_positions, - "Set joint position"); - - dqcsinterfacezmq_py.def("set_joint_target_position", - (void (DQ_CoppeliaSimInterfaceZMQ::*) (const std::string&, const double&))&DQ_CoppeliaSimInterfaceZMQ::set_joint_target_positions, - "Set joint target position"); + dqcsinterfacezmq_py.def("get_object_handle", + (int (DQ_CoppeliaSimInterfaceZMQ::*) (const std::string&))&::DQ_CoppeliaSimInterfaceZMQ::get_object_handle, + "gets the object handle from CoppeliaSim."); - dqcsinterfacezmq_py.def("get_joint_position", - (double (DQ_CoppeliaSimInterfaceZMQ::*) (const std::string&))&::DQ_CoppeliaSimInterfaceZMQ::get_joint_positions, - "Get joint position"); + dqcsinterfacezmq_py.def("get_object_handles", + (VectorXd (DQ_CoppeliaSimInterfaceZMQ::*) (const std::vector&))&DQ_CoppeliaSimInterfaceZMQ::get_object_handles, + "returns a vector containing the object handles."); dqcsinterfacezmq_py.def("set_joint_positions", (void (DQ_CoppeliaSimInterfaceZMQ::*) (const std::vector&, const VectorXd&))&DQ_CoppeliaSimInterfaceZMQ::set_joint_positions, @@ -100,4 +105,23 @@ void init_DQ_CoppeliaSimInterfaceZMQ_py(py::module& m) (VectorXd (DQ_CoppeliaSimInterfaceZMQ::*) (const std::vector&))&DQ_CoppeliaSimInterfaceZMQ::get_joint_positions, "Get joint positions"); + dqcsinterfacezmq_py.def("get_joint_velocities", + (VectorXd (DQ_CoppeliaSimInterfaceZMQ::*) (const std::vector&))&DQ_CoppeliaSimInterfaceZMQ::get_joint_velocities, + "gets the joint velocities in the CoppeliaSim scene."); + + dqcsinterfacezmq_py.def("set_joint_target_velocities", + (void (DQ_CoppeliaSimInterfaceZMQ::*) (const std::vector&, const VectorXd&))&DQ_CoppeliaSimInterfaceZMQ::set_joint_target_velocities, + "sets the joint target velocities in the CoppeliaSim scene. " + "This method requires a dynamics enabled scene, and joints in dynamic mode with velocity control mode."); + + dqcsinterfacezmq_py.def("get_joint_torques", + (VectorXd (DQ_CoppeliaSimInterfaceZMQ::*) (const std::vector&))&DQ_CoppeliaSimInterfaceZMQ::get_joint_torques, + "gets the joint torques in the CoppeliaSim scene."); + + dqcsinterfacezmq_py.def("set_joint_torques", + (void (DQ_CoppeliaSimInterfaceZMQ::*) (const std::vector&, const VectorXd&))&DQ_CoppeliaSimInterfaceZMQ::set_joint_torques, + "sets the joint torques in the CoppeliaSim scene. " + "This method requires a dynamics enabled scene, and joints in dynamic mode with force control mode."); + + }